IMU และแมกนีโตมิเตอร์

ทำไมต้องรู้

IMU (inertial measurement unit) คือหัวใจของตัวควบคุมการบิน ประกอบด้วย ไจโรสโคป วัดอัตราหมุน และ แอกเซเลอโรมิเตอร์ วัดความเร่ง ตัวควบคุมอ่าน IMU หลายร้อยถึงหลายพันครั้งต่อวินาทีเพื่อรักษาท่าทาง ส่วน แมกนีโตมิเตอร์ วัดสนามแม่เหล็กโลกเพื่อบอกทิศหัว ถ้าเซนเซอร์เหล่านี้คลาดเล็กน้อย โดรนจะเอียงค้าง ลอยไหล หรือบินผิดทิศ

MEMS และแบบจำลองความคลาด

เซนเซอร์ในโดรนเกือบทั้งหมดเป็น MEMS (micro-electro-mechanical systems) โครงสร้างกลไกขนาดไมโครเมตรบนชิปซิลิคอน แอกเซเลอโรมิเตอร์มีมวลเล็ก ๆ แขวนด้วยสปริง เมื่อเร่ง มวลเคลื่อนและค่าความจุไฟฟ้าเปลี่ยน ส่วนไจโร MEMS มีมวลที่สั่นอยู่ตลอด เมื่อหมุน แรงคอริออลิสทำให้มันสั่นในอีกแกนหนึ่ง (Groves, 2013)

แต่ละแกนมีแบบจำลองความคลาดพื้นฐาน $\tilde{a}=sa+b+n$ โดย $s$ คือ scale factor $b$ คือ bias และ $n$ คือ สัญญาณรบกวน ของจริงยังมีแกนเอียงไม่ตั้งฉาก (misalignment) และการเปลี่ยนตามอุณหภูมิ datasheet บอกระดับสัญญาณรบกวน เช่น ICM-42688-P ของ TDK ระบุสัญญาณรบกวนไจโร 2.8 mdps/√Hz และแอกเซเลอโรมิเตอร์ 70 µg/√Hz

สอบเทียบแอกเซเลอโรมิเตอร์หกท่า

เมื่อวางนิ่ง แอกเซเลอโรมิเตอร์วัดแรงโน้มถ่วงได้ขนาด $g$ พอดี วางให้แต่ละแกนชี้ขึ้นและชี้ลงรวมหกท่า จะได้ค่า $+g$ และ $-g$ ของทุกแกน จากสองค่านี้หา bias และ scale factor ได้ทันที คู่มือ ArduPilot ใช้หกท่านี้ (ระดับ ตะแคงขวา ตะแคงซ้าย หัวลง หัวขึ้น และคว่ำ) และมาตรฐาน IEEE 1293-2018 อธิบายการทดสอบแบบพลิก (tumble) ของแอกเซเลอโรมิเตอร์โดยละเอียด

import math

G = 9.80665
readings = {"x": (9.93, -9.69), "y": (9.85, -9.77), "z": (9.62, -10.02)}   # m/s² แกนชี้ขึ้น, ชี้ลง

cal = {}
for axis, (up, down) in readings.items():
    bias = (up + down) / 2
    scale = (up - down) / (2 * G)
    cal[axis] = (bias, scale)
    print(f"{axis}: bias {bias:+.3f} m/s^2, scale factor {scale:.4f}")

raw = (0.12, 0.04, -10.02)            # ลอยนิ่งในแนวระดับจริง แกน z ชี้ลง
fixed = [(r - cal[a][0]) / cal[a][1] for a, r in zip("xyz", raw)]
tilt = lambda v: math.degrees(math.atan2(math.hypot(v[0], v[1]), -v[2]))
print(f"apparent tilt before {tilt(raw):.2f} deg, after {tilt(fixed):.2f} deg")

x: bias +0.120 m/s^2, scale factor 1.0003
y: bias +0.040 m/s^2, scale factor 1.0003
z: bias -0.200 m/s^2, scale factor 1.0014
apparent tilt before 0.72 deg, after 0.00 deg

Allan deviation

การวางนิ่งนาน ๆ แล้วคำนวณ Allan deviation ตามช่วงเวลาเฉลี่ย $\tau$ แยกชนิดสัญญาณรบกวนได้ บันทึกการใช้งาน AN5087 ของ NXP และงานของ El-Sheimy และคณะ (2008) อธิบายการอ่านกราฟลอการิทึมนี้

• ช่วงที่เส้นลดลงด้วยความชัน −1/2 คือสัญญาณรบกวนขาว ค่าบนเส้นนี้ที่ $\tau =1$ s คือ angle random walk (ARW)
• บริเวณที่เส้นแบนก่อนเพิ่มขึ้นคือ bias instability บอกว่า bias ลอยได้น้อยที่สุดเท่าใด AN5087 อ่านค่าต่ำสุดโดยตรง ส่วนมาตรฐาน IEEE สำหรับไจโรหารค่าต่ำสุดด้วย 0.664 เวลารายงานต้องบอกว่าใช้วิธีใด

ค่าเหล่านี้ใช้ตั้งพารามิเตอร์สัญญาณรบกวนของ EKF และเทียบเซนเซอร์สองรุ่นก่อนซื้อ

แมกนีโตมิเตอร์และ hard iron

แมกนีโตมิเตอร์วัดสนามแม่เหล็กโลกซึ่งอ่อนมาก ชิ้นส่วนเหล็กและกระแสไฟในโดรนจึงรบกวนได้ง่าย hard iron คือสนามคงที่จากแม่เหล็กถาวรหรือชิ้นส่วนที่มีอำนาจแม่เหล็ก ทำให้ข้อมูลเมื่อหมุนรอบตัวเป็นวงกลมที่ เลื่อนศูนย์กลาง ส่วน soft iron จากวัสดุที่นำสนามแม่เหล็กทำให้วงกลมกลายเป็นวงรี การสอบเทียบเข็มทิศใน ArduPilot หรือ PX4 จึงให้หมุนโดรนหลายทิศเพื่อเก็บข้อมูลให้ครบ และ ArduPilot มีขั้นตอน CompassMot สำหรับแก้ผลของกระแสมอเตอร์

import math

OFF, H = (12.0, -8.0), 35.0          # µT ออฟเซ็ต hard iron และสนามแนวราบสมมติ
samples = [(d, H * math.cos(math.radians(d)) + OFF[0], H * math.sin(math.radians(d)) + OFF[1])
           for d in range(0, 360, 10)]

def worst_error(off):
    errs = []
    for d, mx, my in samples:
        head = math.degrees(math.atan2(my - off[1], mx - off[0]))
        errs.append(abs((head - d + 180) % 360 - 180))
    return max(errs)

est = ((max(s[1] for s in samples) + min(s[1] for s in samples)) / 2,
       (max(s[2] for s in samples) + min(s[2] for s in samples)) / 2)
print(f"worst heading error without calibration: {worst_error((0, 0)):.1f} deg")
print(f"estimated offset ({est[0]:.1f}, {est[1]:.1f}) uT")
print(f"worst heading error after hard-iron correction: {worst_error(est):.1f} deg")

worst heading error without calibration: 24.3 deg
estimated offset (12.0, -8.0) uT
worst heading error after hard-iron correction: 0.0 deg

ทิศหัวที่ได้จากเข็มทิศเป็นทิศเหนือแม่เหล็ก ต่างจากทิศเหนือจริงเท่ากับ มุมเดคลิเนชัน (declination) ซึ่งเปลี่ยนตามสถานที่และเวลา ArduPilot มีแบบจำลองสนามแม่เหล็กโลกในตัวจึงตั้งค่าให้อัตโนมัติได้ ถ้าต้องการตรวจค่า ให้ใช้เครื่องคำนวณ World Magnetic Model ฉบับ WMM2025 ของ NOAA

ปฏิบัติการประจำโมดูล

ข้อผิดพลาดที่พบบ่อย

สรุป

• IMU แบบ MEMS มีความคลาด bias, scale factor และสัญญาณรบกวน
• การสอบเทียบหกท่าหา bias และ scale factor จากค่า ±g ของทุกแกน
• Allan deviation แยก angle random walk และ bias instability ได้จากข้อมูลวางนิ่ง
• hard iron เลื่อนศูนย์กลางวงกลมข้อมูลเข็มทิศ ทำให้ทิศหัวผิดมาก ต้องสอบเทียบและระวังแหล่งรบกวน

แบบฝึกตรวจความเข้าใจ

1. แกนหนึ่งอ่าน +9.90 และ −9.70 m/s² bias เท่าใด
2. จากข้อ 1 scale factor เท่าใด (g = 9.80665)
3. ช่วงความชัน −1/2 ในกราฟ Allan deviation แทนสัญญาณรบกวนชนิดใด
4. mx อ่านได้ระหว่าง −20 ถึง 40 µT ออฟเซ็ต hard iron แกนนี้เท่าใด
5. hard iron กับ soft iron ทำให้ข้อมูลเข็มทิศเปลี่ยนรูปต่างกันอย่างไร