在上一篇博文《動(dòng)手學(xué)無(wú)人駕駛（5）：多傳感器數(shù)據(jù)融合》介紹了如何使用Radar和LiDAR數(shù)據(jù)對(duì)自行車進(jìn)行追蹤，這是對(duì)汽車外界運(yùn)動(dòng)物體進(jìn)行定位。
對(duì)于自動(dòng)駕駛的汽車來(lái)說(shuō)，有時(shí)也需要對(duì)自身進(jìn)行更精確的定位，今天就介紹如何使用IMU和GPS進(jìn)行自車定位（因?yàn)樵谏弦黄恼轮袑?duì)kalman和Extend Kalman原理進(jìn)行了比較詳細(xì)的介紹，本文中理論部分不會(huì)再介紹的這么詳細(xì)了，有需要的話可以回看上文）。
本文參考了Coursera自動(dòng)駕駛課程項(xiàng)目，在此表示感謝。

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

Coursera 自動(dòng)駕駛教程：Part2 - State Estimation and Localization for Self-Driving Cars

文章目錄

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

1.IMU簡(jiǎn)介

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

---

2.GPS簡(jiǎn)介

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

---

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

3.1 Extend Kalman Filter

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

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

---

3.2 Motion Model

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

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

---

3.3 Measurement Model

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

3.4 Sensor Fusion

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

# 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）計(jì)算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）計(jì)算誤差狀態(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)

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

也可以繪制誤差分布圖，如下圖所示，這里使用的$3\sigma$標(biāo)準(zhǔn)。

---

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

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

總結(jié)

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

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