04-传感器与感知-里程计与IMU

作者：姚博文 · 2026-09-15

里程计与 IMU — "我走了多远"与"我什么姿态"

定位：从编码器脉冲、陀螺仪/加速度计原始数据，到 ROS2 `/odom` 位姿发布的完整链路——原理、解码、滤波、选型、安装、ROS2 集成 | 适用：智能小车/移动机器人开发

自主导航的第一块砖是回答两个问题：

| 问题 | 回答它的传感器 | 产物 |

| --- | --- | --- |

| 我走了多远、转了多少（相对量） | 轮式编码器 → 里程计（Odometry） | 位姿增量（Δx, Δy, Δθ），累积成相对位姿 |

| 我什么姿态、在怎么转（高频动态量） | IMU（惯性测量单元） | 角速度、加速度、姿态角（roll/pitch/yaw） |

里程计是**相对量**：它告诉你"比上一刻多走了多少"，但没有绝对参考，误差随时间累积（漂移）。IMU 提供 100 Hz 以上的高频运动信息，是短时姿态的"金标准"，但陀螺积分同样会漂。两者互补：

· 里程计：低频可信（10–50 Hz 输出），长期漂移（典型 5%–15% 累计误差率）；

· IMU：高频（100 Hz+），短时极准、秒级漂移。

在链路中的位置（对应 `00-路线图/学习路线图.md` 全链路图的第一级）：

下游消费者：AMCL 用 `/odom` 做粒子滤波的运动模型；Nav2 的控制器、`tf` 树、RViz 显示全都依赖 `odom → base_link` 这条 TF。这篇就是把它做对。

1. 轮式里程计：脉冲 → 线速度 → 位姿递推（含差速运动学公式），误差来源与累积规律；

2. 编码器正交解码：相位关系、方向判别、四倍频，可直接运行的 Python 计数仿真；

3. IMU 三轴原理：单位、噪声模型（白噪声/零偏/漂移）；

4. 姿态解算：互补滤波（完整 Python）、Mahony（完整 Python）、EKF 简介与三者对比；

5. IMU 选型：MPU6050 / ICM-20602 / BMI088 / Xsens 对比表；

6. IMU 安装（位置/减振）与校准流程；

7. ROS2 完整方案：节点结构、TF 树（`map → odom → base_link`）、`robot_localization` 配置与验证方法。

**第一步：脉冲计数 → 轮速。** 电机（或轮毂）上的增量编码器每转一圈输出固定脉冲数。设：

· $N$：编码器每圈**机械**脉冲数（若编码器装在电机轴上且减速比 $i$ 非整数，需按"总线数"折算，见 §5 踩坑）；

· 采用正交四倍频解码（§2.3），每圈实际计数 $C_{rev} = 4N$；

· $r$：轮子**有效滚动半径**（地面接触半径，不是外观半径）；

· $\Delta C$：时间 $\Delta t$ 内的计数变化。

车轮转速（rad/s）与线速度（m/s）：

\omega_{wheel} = \frac{\Delta C}{4N\,\Delta t}\,2\pi, \qquad v_{wheel} = r\,\omega_{wheel} = \frac{2\pi r\,\Delta C}{4N\,\Delta t}

**第二步：轮速 → 车体速度（差速模型）。** 设左右轮速 $v_R, v_L$，轮距（两驱动轮接地中心距离）$L$，车体系（x 向前、y 向左、z 向上，见 REP-103）：

v = \frac{v_R + v_L}{2}, \qquad \omega = \frac{v_R - v_L}{L}

（$v, \omega$ 分别是车体 x 方向平移速度和绕 z 轴角速度；转向轮式/阿克曼底盘公式不同，本文以差速小车为主线。）

**第三步：速度 → 位姿递推。** 设 $k$ 时刻位姿 $(x_k, y_k, \theta_k)$，在 $\Delta t$ 内 $v, \omega$ 近似不变，轨迹是一段圆弧，**精确弧段递推**为：