在上一篇博文《動手學(xué)無人駕駛（5）：多傳感器數(shù)據(jù)融合》介紹了如何使用Radar和LiDAR數(shù)據(jù)對自行車進行追蹤，這是對汽車外界運動物體進行定位。
對于自動駕駛的汽車來說，有時也需要對自身進行更精確的定位，今天就介紹如何使用IMU和GPS進行自車定位（因為在上一篇文章中對kalman和Extend Kalman原理進行了比較詳細的介紹，本文中理論部分不會再介紹的這么詳細了，有需要的話可以回看上文）。
本文參考了Coursera自動駕駛課程項目，在此表示感謝。

大家可以先看看下面這個視頻，對本項目要介紹的內(nèi)容有個初步了解，視頻鏈接為：https://www.bilibili.com/video/BV1cE411D7Y9?p=18

Coursera 自動駕駛教程：Part2 - State Estimation and Localization for Self-Driving Cars

文章目錄

• • • 1.IMU簡介
    • 2.GPS簡介
    • 3.數(shù)據(jù)融合
    • • 3.1 Extend Kalman Filter
      • 3.2 Motion Model
      • 3.3 Measurement Model
      • 3.4 Sensor Fusion

1.IMU簡介

慣性測量單元（Inertial Measurement Unit）通常由3個加速度計和3個陀螺儀組合而成，加速度計和陀螺儀安裝在互相垂直的測量軸上，這里可以將其輸出看作為三個方向的加速度和角速度，表示為：
$$imu=?\begin{array}{c}{a}_x\\ a_y\\ a_z\\ w_x\\ w_y\\ w_z\end{array}?$$

---

2.GPS簡介

全球定位系統(tǒng)（Global Positioning System）大家應(yīng)該都不陌生，其輸出常見為：經(jīng)度，維度，和高度，表示為：
$$gps=?\begin{array}{c}lng\\ lat\\ alt\end{array}?$$

---

3.數(shù)據(jù)融合

3.1 Extend Kalman Filter

整個流程框架如下圖所示，這里需要注意的是IMU與GPS的輸出信號頻率是不同的，IMU輸出頻率常見為50-500Hz不等，GPS輸出頻率常見為1-10Hz。因此要分為兩部分來討論：

（1）在只有IMU數(shù)據(jù)時（此時GPS還未有輸出產(chǎn)生），IMU數(shù)據(jù)經(jīng)過運動模型，得到預(yù)測狀態(tài)${\check{x}}_k$；然后預(yù)測狀態(tài)傳送回運動模型，繼續(xù)下一步預(yù)測；
（2）當有GPS數(shù)據(jù)產(chǎn)生時，上一時刻產(chǎn)生的預(yù)測狀態(tài)將會和接收到的GPS位置信息進行數(shù)據(jù)融合，得到修正后的狀態(tài)${\hat{x}}_k$。然后再傳回運動模型，進行下一周期的運算。

---

3.2 Motion Model

運動模型如下，狀態(tài)向量為10維狀態(tài)向量，即$x=[p_x,p_y,p_z,v_x,v_y,v_z,q_0,q_1,q_2,q_3{]}^T$，模型可以分為三部分討論：

（1）位置運動模型，假設(shè)載體做勻加速運動，則有：$p_k=\Delta t{v}_{k?1}+\frac{\Delta t^2}{2}(C_{ns}{f}_{k?1}?g)$，其中$f_{k?1}$為imu的測量值，$C_{ns}$為旋轉(zhuǎn)矩陣，用于對imu的測量值進行坐標變換。
（2）速度運動模型，同樣假設(shè)載體做勻加速運動。
（3）方向運動模型，這里$q_{k?1}$表示為四元數(shù)，關(guān)于四元數(shù)的旋轉(zhuǎn)變換可以參考有關(guān)資料，這里不做展開了。

但上面的模型不是線性的，課程中將上面的模型進行線性化處理，結(jié)果如下，處理后的誤差狀態(tài)向量為9維的狀態(tài)向量，這里需要關(guān)注的是狀態(tài)轉(zhuǎn)移矩陣$F_{k?1}$，噪聲協(xié)方差矩陣$L_{k?1}$。

---

3.3 Measurement Model

測量模型如下，這里我們需要用到的是GPS的位置數(shù)據(jù)。

3.4 Sensor Fusion

介紹完理論部分，下面我們開始一步步實現(xiàn)代碼部分。
（1）使用IMU數(shù)據(jù)進行更新，需要注意旋轉(zhuǎn)矩陣的計算。

# 1. Update state with IMU inputsC_ns = Quaternion(*q_est[k-1]).to_mat() #rotational matrixC_ns_dot_f_km = np.dot(C_ns, imu_f.data[k-1])# 1.1 Linearize the motion model and compute Jacobiansp_est[k] = p_est[k-1] + delta_t * v_est[k-1] + (delta_t**2)/2.0 * (C_ns.dot(imu_f.data[k-1]) + g)v_est[k] = v_est[k-1] + delta_t*(C_ns.dot(imu_f.data[k-1]) + g)# Instead of using Omega, we use quaternion multiplication q_est[k] = Quaternion(axis_angle = imu_w.data[k-1] * delta_t).quat_mult_right(q_est[k-1])

---

（2）狀態(tài)協(xié)方差矩陣的更新

# 2. Propagate uncertainty# Global orientation error, over local orientation error# See Sola technical reportF = np.identity(9)F[:3, 3:6] = delta_t * np.identity(3)#F[3:6, 6:] = -(C_ns.dot(skew_symmetric(imu_f.data[k-1].reshape((3,1)))))F[3:6,6:9] = -skew_symmetric(C_ns_dot_f_km) *delta_tQ = np.identity(6)Q[:, :3] *= delta_t**2 * var_imu_fQ[:, -3:] *= delta_t**2 * var_imu_wp_cov[k] = F.dot(p_cov[k-1]).dot(F.T) + l_jac.dot(Q).dot(l_jac.T) #uncertainty

---

（3）計算kalman增益

# 3.1 Compute Kalman GainK_k = p_cov_check.dot(h_jac.T).dot(np.linalg.inv(h_jac.dot(p_cov_check).dot(h_jac.T)+np.identity(3)*sensor_var))

---

（4）計算誤差狀態(tài)

# 3.2 Compute error stateerrorState = K_k.dot(y_k - p_check)

---

（5）誤差狀態(tài)修正

# 3.3 Correct predicted statep_hat = p_check + errorState[:3]v_hat = v_check + errorState[3:6]q_hat = Quaternion(euler=errorState[6:]).quat_mult_left(\q_check) # left or right

---

（6）修正狀態(tài)協(xié)方差矩陣

# 3.4 Compute corrected covariancep_cov_hat = (np.identity(9) - K_k.dot(h_jac)).dot(p_cov_check)

到這一步，就完成了整個處理過程，可以看看最終的結(jié)果，途中橙色為軌跡真值位置，藍色為估計的軌跡位置。

也可以繪制誤差分布圖，如下圖所示，這里使用的$3\sigma$標準。

---

至此，本文要介紹的內(nèi)容就結(jié)束了。基于IMU和GPS的位置定位，關(guān)鍵點在于IMU的運動模型，特別是四元數(shù)更新部分，里面牽涉到的變化比較多，需要留心。

如果想深入了解IMU和GPS融合原理，可以看看這篇文章： 重讀經(jīng)典《Quaternion kinematics for the error-state Kalman filter》，這也是Coursera課程關(guān)于這一項目的參考文獻。

總結(jié)

以上是生活随笔為你收集整理的动手学无人驾驶（6）：基于IMU和GPS数据融合的自车定位的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯，歡迎將生活随笔推薦給好友。