Odometry

**里程计（odometry）**是通过对增量运动测量值随时间进行积分来估计机器人位姿的方法——即航位推算（dead reckoning）。从一个已知的初始位姿出发，每一次新的测量都给出一个小的相对运动 $\Delta T_k$，当前位姿就是所有增量的复合：

$$T_k = T_0 \cdot \Delta T_1 \cdot \Delta T_2 \cdots \Delta T_k$$

“odometry”一词最初指的是轮式里程计（wheel odometry）：编码器统计车轮转数，再通过运动学模型（差速驱动、阿克曼转向）将其转换为线速度和角速度。同样的思想可以推广到任何能够测量相对运动的传感器：

• 轮式里程计——在轮式机器人上廉价且始终可用，但会受到车轮打滑的影响，在室外尤为明显。
• 惯性里程计——对 IMU 的加速度和角速度进行积分；由于加速度需要被积分两次，单独使用时会很快产生漂移。
• 视觉里程计（VO）——根据图像间的对应关系估计相机的帧间运动。
• LiDAR 里程计——对齐连续的点云（例如使用 ICP 或基于特征的配准方法）。

里程计的定义性特征是漂移（drift）：每个增量都带有一个小误差，并且由于增量是以乘法方式串联的，误差会无界地累积。每米 0.5% 的平移误差在桌面尺度的运动中不可察觉,但在一公里范围内则是灾难性的。因此,单靠里程计永远无法产生全局一致的地图——这需要回环检测和全局优化,这正是区分里程计与 SLAM 的关键所在。

在现代 SLAM 后端中，里程计以相对位姿约束的形式出现：作为位姿图中连续位姿节点之间的一条边，或作为因子图中的一个运动模型因子，

$$\mathbf{x}_{t+1} = f(\mathbf{x}_t, \mathbf{u}_t) + \mathbf{w}_t, \qquad \mathbf{w}_t \sim \mathcal{N}(\mathbf{0}, Q_t)$$

其中控制/测量量 $\mathbf{u}_t$ 来自编码器、IMU 或视觉里程计前端，而 $Q_t$ 对其不确定性进行建模。

实例演算：差速驱动里程计

对于一台车轮半径为 $r$、编码器测得车轮角速度为 $\omega_l, \omega_r$、轮距为 $L$ 的两轮差速驱动机器人，其机体速度为

$$v = \frac{r(\omega_r + \omega_l)}{2}, \qquad \omega = \frac{r(\omega_r - \omega_l)}{L}.$$

在时间步 $\Delta t$ 上对平面位姿 $(x, y, \theta)$ 进行积分（最简单的欧拉形式）：

$$x_{k+1} = x_k + v\cos\theta_k\,\Delta t, \qquad y_{k+1} = y_k + v\sin\theta_k\,\Delta t, \qquad \theta_{k+1} = \theta_k + \omega\,\Delta t.$$

这个十行代码的积分器就是一个完整的里程计系统——而且它展现出了所有经典的失效模式。注意位置更新依赖于 $\theta_k$：任何朝向误差都会使所有后续的平移旋转到错误的方向上。在以朝向误差 $\delta\theta$ 行驶了距离 $d$ 之后，横向位置误差大致为 $d \cdot \delta\theta$——这正是为什么朝向（陀螺仪质量、轮距标定）主导着里程计精度，也是为什么即便给轮式里程计加上一个廉价的陀螺仪也能大幅提升效果的原因。

漂移的表现规律

• 随机误差以随机游走的方式累积：如果每个增量都携带独立的噪声，那么积分后位姿的方差会随行驶距离的增加而增长——不确定性椭圆会持续膨胀，且在没有外部修正的情况下永不缩小。
• 系统性误差呈线性增长：一个标定不准的车轮半径（比如由于轮胎气压问题）或轮距，会以相同的方式使每个增量产生偏差——一条直走廊在每一次实测中都会略微弯曲。系统性误差是可以也应该被标定消除的误差。
• 朝向误差占主导：正如上面的实例所示，角度误差会按行驶距离转化为位置误差，而纯平移误差只是简单累加。
• 误差的复合在 $SE(3)$ 上是乘法性的：增量是以变换的方式复合的，因此误差并不是简单相加——这正是为什么里程计协方差传播要使用复合运算的雅可比矩阵（或者更简洁地，使用李群工具）。

在实践中，里程计的质量通常用相对误差来衡量——即行驶距离的百分比误差和每米的角度误差——这正是 KITTI 里程计基准和相对位姿误差（RPE）指标所度量的内容。绝对轨迹误差（ATE）并不适合用来衡量里程计系统，因为绝对误差本身在设计上就是无界的。

常见陷阱

• 将里程计输出当作下游的真值——务必传播其协方差；一个忽略里程计不确定性的规划或融合栈会以自信但错误的方式运行。
• 坐标系语义错误（ROS）——里程计应作为一个连续、会漂移的 odom -> base_link 变换发布；而回环闭合式 SLAM 的修正则位于 map -> odom 中。让里程计在修正到来时发生跳变，会破坏所有依赖它的控制器。
• 时间戳处理草率——用错误的 $\Delta t$ 对编码器计数进行积分，或者将速度与错误的图像配对，会引入看起来与传感器噪声完全一样、但实际上纯粹是记账错误所致的误差。
• 忽视打滑和运动学不匹配——差速驱动方程假设车轮纯滚动而不打滑；在地毯、砾石地面或急转弯时，这个假设会悄无声息地失效。应当检测它（例如比较陀螺仪与编码器推算出的偏航角是否一致），而不是简单地对其取平均。

对SLAM的意义

里程计是每个 SLAM 系统的骨架：它提供了高频率、局部精确的运动估计，跟踪、建图和回环检测模块都建立在此之上。理解它的误差特性——局部平滑、全局漂移——正是理解现代 SLAM 架构的关键所在：在短时间尺度上信任里程计，而每当回环检测或绝对测量可用时,就对其进行全局修正。

相关条目

• Proprioceptive sensor
• Visual Odometry
• VO vs SLAM
• Pose graph optimization