Sensor fusion

ทำไมต้องรู้

IMU วัดความเร่งได้หลายร้อยครั้งต่อวินาที แต่ถ้าเอาความเร่งมาอินทิเกรตเป็นตำแหน่ง ความคลาดเคลื่อนเล็ก ๆ จะสะสมจนตำแหน่งลอยไปไกล ส่วน GNSS บอกตำแหน่งที่ไม่ลอย แต่ได้เพียงไม่กี่ครั้งต่อวินาที มีสัญญาณรบกวนระดับเมตร และบางครั้งกระโดดผิด ๆ เมื่อสัญญาณสะท้อนอาคาร การหลอมรวมเซนเซอร์ (sensor fusion) เอาจุดแข็งของทั้งสองมารวมกัน คล้ายเราเดินในห้องมืดโดยนับก้าวไปเรื่อย ๆ แล้วแตะผนังเป็นระยะเพื่อแก้ตำแหน่งที่นับผิด

Kalman filter

Kalman filter ประมาณสถานะ (เช่น ตำแหน่งและความเร็ว) พร้อมความไม่แน่นอนของมัน ทำงานสองขั้นวนไปเรื่อย ๆ ตามตำราของ Welch และ Bishop

1. ทำนาย (predict) ใช้แบบจำลองการเคลื่อนที่และค่า IMU เลื่อนสถานะไปข้างหน้า ความไม่แน่นอน $P$ เพิ่มขึ้นทุกครั้ง
2. ปรับ (update) เมื่อมีค่าวัด เช่น GNSS คำนวณ innovation $y$ คือค่าวัดลบค่าที่ทำนาย แล้วขยับสถานะไปทางค่าวัดด้วยน้ำหนัก Kalman gain $K$ ถ้าเชื่อค่าวัดมาก (R เล็ก) $K$ จะใหญ่

ก่อนใช้ค่าวัด ระบบตรวจว่า innovation ใหญ่เกินกว่าที่ความไม่แน่นอนอธิบายได้หรือไม่ ด้วย test ratio $y^2/(g^{2}S)$ เมื่อ $g$ คือ gate ในหน่วยส่วนเบี่ยงเบนมาตรฐาน ถ้าเกิน 1 ค่าวัดนั้นถูกปฏิเสธ EKF2 ของ PX4 ใช้หลักเดียวกัน และ gate ของตำแหน่ง GNSS (EKF2_GPS_P_GATE) ตั้งค่าเริ่มต้นไว้ 5 เท่าของส่วนเบี่ยงเบนมาตรฐาน

import numpy as np

rng = np.random.default_rng(348)
dt = 0.01
t = np.arange(0, 20.0, dt)
true_acc = 0.6 * np.sin(0.5 * t)
true_pos = np.cumsum(np.cumsum(true_acc) * dt) * dt
imu_acc = true_acc + 0.05 + rng.normal(0, 0.2, t.size)
gnss = true_pos + rng.normal(0, 1.5, t.size)
gnss[600] += 25.0                                   # ค่า GNSS ผิดพลาดที่ t = 6 s

x, P = np.zeros(2), np.diag([4.0, 1.0])             # สถานะ [ตำแหน่ง, ความเร็ว]
F, B = np.array([[1, dt], [0, 1]]), np.array([0.5 * dt**2, dt])
Q, R, H, gate = np.diag([1e-4, 4e-4]), 1.5**2, np.array([1.0, 0.0]), 5.0
est, dead, vel, pos = np.zeros(t.size), np.zeros(t.size), 0.0, 0.0
for k in range(t.size):
    x, P = F @ x + B * imu_acc[k], F @ P @ F.T + Q          # ทำนาย
    vel += imu_acc[k] * dt
    pos += vel * dt
    dead[k] = pos                                             # ใช้ IMU อย่างเดียว
    if k % 20 == 0:                                           # มี GNSS ทุก 0.2 s
        y = gnss[k] - H @ x
        S = H @ P @ H + R
        ratio = y**2 / (gate**2 * S)
        if ratio > 1:
            print(f"t = {t[k]:.1f} s: GNSS rejected, test ratio {ratio:.2f}")
        else:
            K = P @ H / S
            x, P = x + K * y, (np.eye(2) - np.outer(K, H)) @ P
    est[k] = x[0]
g = np.arange(0, t.size, 20)
rmse = lambda a, idx=slice(None): np.sqrt(np.mean((a[idx] - true_pos[idx]) ** 2))
print(f"RMSE  IMU only {rmse(dead):.2f} m | GNSS only {rmse(gnss, g):.2f} m | fused {rmse(est):.2f} m")

t = 6.0 s: GNSS rejected, test ratio 8.06
RMSE  IMU only 4.82 m | GNSS only 2.94 m | fused 0.69 m

EKF ในตัวควบคุมการบิน

ระบบจริงมีสถานะหลายสิบตัว (ตำแหน่ง ความเร็ว ท่าทาง ไบแอสของเซนเซอร์ สนามแม่เหล็ก ลม) และสมการไม่เป็นเชิงเส้น จึงใช้ Extended Kalman Filter (EKF) ที่ทำให้สมการเป็นเชิงเส้นรอบค่าประมาณปัจจุบัน ตำรา Probabilistic Robotics อธิบายหลักนี้ละเอียด

• PX4 EKF2 รวม IMU เข็มทิศ GNSS บารอมิเตอร์ ตัววัดระยะ optical flow external vision และ airspeed แต่ละแหล่งมี test ratio ของตัวเองที่ดูได้ใน log
• ArduPilot EKF3 เป็นตัวประมาณค่าเริ่มต้นของ ArduPilot
• การรับตำแหน่งจากระบบภาพ (เช่น VIO) เข้า EKF2 เปิดด้วย EKF2_EV_CTRL ซึ่งเป็น bitmask เลือกได้ว่ารวมตำแหน่งแนวนอน แนวดิ่ง ความเร็ว หรือมุมหัน ค่าเริ่มต้นคือปิด

VIO (visual-inertial odometry) เช่น VINS-Mono และ OpenVINS ประมาณการเคลื่อนที่จากกล้องกับ IMU ใช้แทน GNSS ในอาคาร แต่ต้องปรับเทียบกล้องกับ IMU ให้ดีก่อน

ปรับเทียบกล้องกับ IMU

การปรับเทียบมีสามส่วน: intrinsics (โมเดลกล้อง เช่น ความยาวโฟกัสและความบิดเบี้ยว) extrinsics (การหมุนและเลื่อนระหว่างกล้องกับ IMU) และ time offset (ความต่างเวลาของสองเซนเซอร์) เครื่องมืออย่าง Kalibr รายงานคุณภาพด้วย reprojection error คือระยะบนภาพระหว่างจุดที่ตรวจพบกับจุดที่ฉายจากแบบจำลอง

residuals = [(3, 4), (-1, 2), (0, -2), (2, 1)]      # (dx, dy) หน่วยพิกเซล
n = len(residuals)
sq = sum(dx * dx + dy * dy for dx, dy in residuals)
print(f"RMSE per point {(sq / n) ** 0.5:.2f} px, per component {(sq / (2 * n)) ** 0.5:.2f} px")

RMSE per point 3.12 px, per component 2.21 px

ค่าเดียวกันให้ตัวเลขต่างกันตามนิยาม OpenCV calibrateCamera หารด้วยจำนวนจุด จึงต้องระบุนิยามทุกครั้ง ค่าเล็กบนชุดที่ใช้ปรับเทียบยังไม่ยืนยันว่าดีกับภาพใหม่ ต้องตรวจกับข้อมูลอีกชุด และบันทึกความละเอียดภาพ เลนส์ โมเดลความบิดเบี้ยว และขนาดเป้าคู่กัน

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

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

สรุป

• Kalman filter ทำนายด้วยแบบจำลองและ IMU แล้วปรับด้วยค่าวัด โดยถ่วงน้ำหนักตามความไม่แน่นอน
• Innovation gate ใช้ test ratio ตัดค่าวัดผิดปกติ ค่าเกิน 1 ถูกปฏิเสธ
• EKF2 ของ PX4 และ EKF3 ของ ArduPilot รวมเซนเซอร์หลายแหล่ง และบันทึก test ratio ไว้ใน log
• การปรับเทียบกล้องกับ IMU มี intrinsics extrinsics และ time offset และต้องรายงาน RMSE พร้อมนิยาม

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

1. ทำไมการอินทิเกรตความเร่งจาก IMU อย่างเดียวจึงให้ตำแหน่งลอยออกไป
2. Innovation $y=6$ m, $S=1$ m², gate $g=5$ test ratio เท่าใด ค่าวัดถูกใช้หรือไม่
3. ถ้าเพิ่ม R (ความแปรปรวนของค่าวัด) Kalman gain จะเพิ่มหรือลด
4. Residual สองจุดคือ (3, 4) และ (0, 0) px reprojection RMSE ต่อจุดเท่าใด
5. พารามิเตอร์ใดของ PX4 เปิดการรวมข้อมูลตำแหน่งจากระบบภาพ