无人驾驶学习笔记：GNSS、惯性导航与融合定位

学完高精度地图之后，接着要回答的是：车怎样知道自己在地图中的哪个位置？这是 2018 年 Udacity 无人驾驶先修课的第二篇笔记。

原文的 11 张课程截图暂时无法恢复。本次整理补齐了坐标变换和运动公式，也把原先混在一起的 GPS、RTK、IMU 与地图匹配分开说明。下面是学习记录，没有新增实车测试。

为什么不能只看手机 GPS 的位置

原来这一节叫“为何 GPS 不应用在无人车上”，这个题目不准确。GNSS 本身就可以参与无人驾驶定位。问题在于，普通接收器的定位结果受卫星几何分布、遮挡、多路径和接收设备等因素影响，不能只拿一个位置点就保证车道级定位。

GNSS 是卫星导航系统的统称，GPS 是其中一种。隧道、楼宇附近等环境可能让卫星观测变差，因此还需要其他传感器提供连续的运动信息，或者利用地图中的稳定特征进行匹配。

GPS 可以按空间段、控制段和用户段来理解：卫星广播信号，地面控制设施维护系统，接收器利用收到的信号估计自身位置和时钟偏差。通常需要至少四颗卫星的观测，才能同时求解三维位置和接收机钟差。

RTK 利用载波相位观测和改正信息提高定位精度。它的效果依赖设备、改正数据、解算状态和观测条件，不能把原笔记中的“10 厘米、10 Hz”当作所有 RTK 设备的统一指标。

即使使用 RTK，遮挡和观测质量问题也不会凭空消失。定位精度、输出频率、结果连续性，是需要分别考虑的事情。

IMU 通常包含加速度计和陀螺仪。陀螺仪提供角速度测量；加速度计测的是比力，计算运动加速度时还需要处理姿态和重力影响。

在最简单的一维、恒定加速度模型中：

\begin{gathered} v(t)=v_0+at\\ s(t)=s_0+v_0t+\frac12at^2 \end{gathered}

如果加速度随时间变化，就需要积分：

\begin{gathered} v(t)=v_0+\int_0^t a(\tau)\,\mathrm d\tau\\ s(t)=s_0+\int_0^t v(\tau)\,\mathrm d\tau \end{gathered}

原稿把位移写成了 s = s0 + vt。这个式子适用于恒定速度的情况，不能直接替代匀加速位移公式。

实际车辆在三维空间中运动，姿态估计和传感器误差也会进入计算。惯性测量能提供连续的短时运动信息，但误差会累积，所以通常需要其他观测来约束漂移。

车上的摄像头、激光雷达和 IMU 各自有坐标系，车辆本身和地图也有各自的坐标系。位置与朝向都要对应起来，才能把观测放进地图中。

对一个三维点，刚体变换可以写成：

\mathbf p_{\mathrm{map}}=R\mathbf p_{\mathrm{vehicle}}+\mathbf t

这里的旋转矩阵 R 表示朝向变换，平移向量 t 表示原点偏移。单独的三阶矩阵乘法只描述线性的旋转部分；要把平移也放进一次矩阵乘法，可以使用齐次坐标：

\begin{bmatrix}\mathbf p_{\mathrm{map}}\\1\end{bmatrix} =\begin{bmatrix}R&\mathbf t\\0&1\end{bmatrix} \begin{bmatrix}\mathbf p_{\mathrm{vehicle}}\\1\end{bmatrix}

原笔记列出了激光雷达和视觉定位，却没有写完。这里补充它们在这条思路中的位置：激光雷达可以利用点云中的环境结构，视觉方法可以利用图像中的特征；与已有地图匹配时，都需要处理观测与地图之间的对应关系。

Apollo 3.0 的定位模块说明中，RTK 方案使用 GPS 和 IMU，多传感器融合方案使用 GPS、IMU 与 LiDAR。这与前面的分工一致：绝对位置观测、连续运动估计和环境匹配各自提供约束。

因此，“GPS 没信号就一定定位失败”也过于绝对。能维持多久、误差如何变化，取决于剩余传感器、环境特征、地图和具体算法。缺少这些条件时，不应该直接承诺系统还能正常工作。