航位推算入门——整合轮子、惯性测量单元和全球导航卫星系统以跟踪机器人的运动

Contents — find the section you need

机器人最原始的定位方法是计算自身“向右移动 1 米，向前移动 2 米”的位移。它会随着时间的推移，将车轮转数、电机转速和惯性测量单元 (IMU) 的角速度进行积分，从而估算出当前位置。这种方法称为航位推算。即使 GPS 或摄像头暂时无法使用，这种方法仍然有效，但微小的车轮打滑或传感器偏差会随着时间的推移而累积成位置误差。

绝对 GNSS 观测

低成本 IMU 示例

图片：Surveyor 使用 RTK 解决方案的 GNSS 接收机 (SweetCanadianMullet，CC BY 4.0) / GY-521 MPU-6050 模块 (Nevit Dilmen， CC BY-SA 3.0），维基共享资源。所示传感器仅供参考，并非强制性硬件组合。

30 秒概要

航位推算通过“先前状态加上测量位移”来预测当前状态。即使没有观测数据，它也能以很高的速率更新。
在差速驱动机器人中，左右车轮位移的差值决定了转弯角度，它们的平均值决定了前进距离。车轮半径和轮距的误差表现为一条系统性的曲线。
里程计误差通过积分累积。获取绝对位置需要使用 GNSS、激光雷达、摄像头或反射标记等观测数据进行校正。
IMU 在短期姿态变化方面表现出色，但当加速度进行双重积分时，位置漂移会迅速增加。将车轮和 IMU 通过扩展卡尔曼滤波器 (EKF) 或因子图融合是常用的方法。
在实际应用中，时间戳、坐标系、左右方向符号约定、车轮半径、滑移检测和协方差都必须从一开始就正确定义——一旦出错，即使是昂贵的传感器也无法弥补。

1. 差速驱动运动学

设左右车轮半径为 $r$ ，车轮间距（轮距）为 $b$ ，左右角速度为 $\omega_L,\omega_R$ 。车轮的线速度分别为 $v_L=r\omega_L$ 和 $v_R=r\omega_R$ ，因此车辆中心的平移速度 $v$ 和偏航角速度 $\dot{\theta}$ 分别为：

v=\frac{v_R+v_L}{2},\qquad \dot{\theta}=\frac{v_R-v_L}{b}

设时刻 $k$ 的姿态为 $(x_k,y_k,\theta_k)$ ，采样周期为 $\Delta t$ ，简单的欧拉积分可得：

x_{k+1}=x_k+v\cos\theta_k\Delta t,

y_{k+1}=y_k+v\sin\theta_k\Delta t, \qquad \theta_{k+1}=\theta_k+\dot{\theta}\Delta t

当以恒定速度沿弧线行驶时，即使在低速和高转弯率的情况下，使用精确积分（车身坐标系中的指数映射）也能保持较小的误差。

图 1 — 在差速驱动中，左右速度的差异会产生偏航率。停止一个车轮可以让机器人绕其旋转。

2. 为什么里程计会漂移

如果车轮半径存在 1% 的误差，并且您在行驶过程中假设左右方向相同，那么即使您认为自己在直线行驶，实际上也会逐渐发生偏转。左右编码器分辨率、齿轮间隙、地面不平整、轮胎磨损以及负载引起的变形都会产生系统误差。如果车轮在湿滑的地面或碎石上打滑，则测得的旋转角度和实际位移将不再匹配。

姿态误差 $\delta\theta$ 会产生横向位置误差。对于前向距离 $s$ ，短时近似值如下：

\delta y\approx s\,\delta\theta

以 0.5 度的角度误差行驶 1 公里，横向偏移量约为 8.7 米。简单地将位置误差表示为“每秒多少厘米”无法捕捉直线行驶、转弯和打滑时不同的误差结构。

车轮里程计的协方差将左右位移的不确定性传递到位姿中，并据此进行更新。在实际应用中，转弯或检测到打滑时会增大协方差，以防止下游传感器融合过度依赖里程计数据。

3. 添加IMU

对IMU陀螺仪的角速率 $\omega_z$ 进行积分，得到

\theta(t)=\theta_0+\int_0^t(\omega_z(\tau)-b_g-n_g)\,d\tau

其中 $b_g$ 为陀螺仪偏差， $n_g$ 为噪声。即使在车轮打滑的瞬间，仍然可以测量角速率的短期变化，从而获得比单独使用车轮更平滑的姿态估计。当对加速度进行两次积分以获得速度和位置时，偏差 $b_a$ 的误差会随着以下因素增大：

\delta p(t)\simeq\frac{1}{2}b_a t^2

因此，单独使用IMU无法作为长时间位置传感器。

结合车轮和IMU：车轮处理低频位移，IMU处理高频角速率和加速度。横滚/俯仰角根据重力方向初始化，当有磁力计或GNSS可用时，还会添加绝对偏航角参考。

4. 使用扩展卡尔曼滤波器 (EKF) 或因子图进行校正

状态向量示例：

\mathbf{x}=[x,y,\theta,v,b_g,b_a]^T

在预测步骤中，根据车轮和惯性测量单元 (IMU) 计算非线性运动模型 $f$ ；当观测值到达时，使用残差进行校正。

\mathbf{x}_{k+1}=f(\mathbf{x}_k,\mathbf{u}_k)+\mathbf{w}_k,

\mathbf{z}_k=h(\mathbf{x}_k)+\mathbf{v}_k

扩展卡尔曼滤波器 (EKF) 通过雅可比矩阵对非线性模型进行局部线性化。全球导航卫星系统 (GNSS) 位置、激光雷达 (LiDAR) 扫描匹配、相机特征点和已知标记位姿均可作为观测值输入 $\mathbf{z}$ 。对于较大的环路或重复访问，将过去的位姿和观测值保存在因子图中，并在之后重新优化整个模型。

对于 ROS 2（假设此处采用 Jazzy 标准消息定义），车轮的 nav_msgs/msg/Odometry、IMU 的 sensor_msgs/msg/Imu 以及 TF 的 odom → base_link 变换应使用相同的时间基准。这些消息类型对应于 ROS 1 中的 nav_msgs/Odometry 和 sensor_msgs/Imu，但 ROS 2 的实现细节、QoS 和分发设置必须单独检查。robot_localization 的 EKF 是一个合理的实现起点，但将协方差置零或重复输入相同的信息会导致估计结果过于僵硬。

5. 当前机器人中的应用

室内自主移动机器人 (AMR) 会持续使用车轮编码器和 IMU，并通过 LiDAR 或摄像头地图匹配来重置漂移。在自动驾驶中，车轮速度、转向角、惯性测量单元 (IMU)、全球导航卫星系统 (GNSS) 和激光雷达 (LiDAR) 数据会被融合，即使在隧道或高架结构下方 GNSS 信号中断的情况下，估计值也能保持数秒至数十秒的稳定。在农业机器人中，泥泞、车辙和坡度会增加车轮打滑，因此需要结合实时定位 GNSS (RTK-GNSS)、IMU 和农具力传感器。

水下机器人和航天器在长时间没有 GNSS 信号的情况下也会运行。此时，会添加多普勒速度记录仪 (DVL)、星敏感器或地形匹配等低频绝对观测数据，以控制航位推算的协方差。关键在于将估计值连同“我们对这个估计值的置信度”一起传递给规划和控制系统。

6. 实施检查清单

将传感器时间戳转换为通用时钟并测量延迟。
绘制 base_link、odom 和 map 坐标系及其右手符号约定。
通过实际驾驶测试确定车轮半径、轮距和编码器分辨率。
分别评估直线行驶、匀速转弯、8 字形行驶、颠簸路面和低摩擦路面的误差。
检测到打滑或丢包时，增大协方差；获得绝对观测值时，进行修正。
回放日志，确认相同的输入产生相同的估计值。

在测试中，不仅要记录终点位置误差，还要记录姿态误差、最大误差、第 95 百分位数以及丢包期间的漂移率。通常准确但偶尔偏差较大的估计值不能用于安全路径规划。

总结

航位推算是一种以尽可能短的延迟估计机器人运动的基础技术。它整合了车轮位移和惯性测量单元 (IMU) 角速率，并利用来自全球导航卫星系统 (GNSS)、激光雷达 (LiDAR) 或摄像头的绝对观测值来抑制漂移。在添加更多传感器之前，确保运动模型、时间戳、坐标系和协方差的正确性是决定精度和重复性的关键。

检查你的理解

精确的车轮旋转能否消除位置误差？

仍然存在滑移以及车轮或安装几何误差。

传感器精度和运动模型有效性是两个不同的问题。

参考资料

Try it in the browser

Lab Change the conditions in a browser experiment读取静止IMU日志：偏差、散布和艾伦偏差 Lab Change the conditions in a browser experimentGNSS工作原理及主要产品——u-blox、Trimble、Fixposition