动手学无人驾驶（6）：基于IMU和GPS数据融合的自车定位

在上一篇博文《动手学无人驾驶（5）：多传感器数据融合》介绍了如何使用Radar和LiDAR数据对自行车进行追踪，这是对汽车外界运动物体进行定位。
对于自动驾驶的汽车来说，有时也需要对自身进行更精确的定位，今天就介绍如何使用IMU和GPS进行自车定位（因为在上一篇文章中对kalman和Extend Kalman原理进行了比较详细的介绍，本文中理论部分不会再介绍的这么详细了，有需要的话可以回看上文）。
本文参考了Coursera自动驾驶课程项目，在此表示感谢。

大家可以先看看下面这个视频，对本项目要介绍的内容有个初步了解，视频链接为：https://www.bilibili.com/video/BV1cE411D7Y9?p=18

Coursera 自动驾驶教程：Part2 - State Estimation and Localization for Self-Driving Cars

文章目录

1.IMU简介
2.GPS简介
3.数据融合
- 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=[axayazwxwywz]imu=\begin{bmatrix}a_x \\ a_y\\ a_z \\ w_x \\ w_y \\ w_z\end{bmatrix}$

2.GPS简介

全球定位系统（Global Positioning System）大家应该都不陌生，其输出常见为：经度，维度，和高度，表示为：
$gps=[lnglatalt]gps=\begin{bmatrix} lng\\lat\\alt \end{bmatrix}$

3.数据融合

3.1 Extend Kalman Filter

整个流程框架如下图所示，这里需要注意的是IMU与GPS的输出信号频率是不同的，IMU输出频率常见为50-500Hz不等，GPS输出频率常见为1-10Hz。因此要分为两部分来讨论：

（1）在只有IMU数据时（此时GPS还未有输出产生），IMU数据经过运动模型，得到预测状态 $xˇk\check {x}_k$ ；然后预测状态传送回运动模型，继续下一步预测；
（2）当有GPS数据产生时，上一时刻产生的预测状态将会和接收到的GPS位置信息进行数据融合，得到修正后的状态 $x^k\hat {x}_k$ 。然后再传回运动模型，进行下一周期的运算。

3.2 Motion Model

运动模型如下，状态向量为10维状态向量，即 $x=[p_x,p_y,p_z,v_x,v_y,v_z,q_0,q_1,q_2,q_3]^T$ ，模型可以分为三部分讨论：

（1）位置运动模型，假设载体做匀加速运动，则有： $pk=Δtvk−1+Δt22(Cnsfk−1−g)p_k=\Delta{t}v_{k-1}+\frac{\Delta{t}^2}{2}(C_{ns}f_{k-1}-g)$ ，其中 $f_{k-1}$ 为imu的测量值， $C_{ns}$ 为旋转矩阵，用于对imu的测量值进行坐标变换。
（2）速度运动模型，同样假设载体做匀加速运动。
（3）方向运动模型，这里 $q_{k-1}$ 表示为四元数，关于四元数的旋转变换可以参考有关资料，这里不做展开了。

但上面的模型不是线性的，课程中将上面的模型进行线性化处理，结果如下，处理后的误差状态向量为9维的状态向量，这里需要关注的是状态转移矩阵 $F_{k-1}$ ，噪声协方差矩阵 $L_{k-1}$ 。

3.3 Measurement Model

测量模型如下，这里我们需要用到的是GPS的位置数据。

3.4 Sensor Fusion

介绍完理论部分，下面我们开始一步步实现代码部分。
（1）使用IMU数据进行更新，需要注意旋转矩阵的计算。

    # 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 multiplicationq_est[k] = Quaternion(axis_angle = imu_w.data[k-1] * delta_t).quat_mult_right(q_est[k-1])

（2）状态协方差矩阵的更新

   # 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）计算误差状态

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

（5）误差状态修正

    # 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）修正状态协方差矩阵

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

到这一步，就完成了整个处理过程，可以看看最终的结果，途中橙色为轨迹真值位置，蓝色为估计的轨迹位置。

也可以绘制误差分布图，如下图所示，这里使用的 $3σ3\sigma$ 标准。

至此，本文要介绍的内容就结束了。基于IMU和GPS的位置定位，关键点在于IMU的运动模型，特别是四元数更新部分，里面牵涉到的变化比较多，需要留心。

如果想深入了解IMU和GPS融合原理，可以看看这篇文章：重读经典《Quaternion kinematics for the error-state Kalman filter》，这也是Coursera课程关于这一项目的参考文献。