ROS教程15精读：robot_localization——从轮式里程计与 IMU 到机器人状态估计

为什么 Driver 已经发了 /odom，你还需要 robot_localization？

这是「wdfk-prog 的嵌入式日记」ROS 教程系列第 15 篇的核心问题。前几章已经搭好三层基础：第 12 章的 Driver 数据契约（/cmd_vel → wheel → /odom、/imu/data_raw、/joint_states）、第 13 章的 TF/tf2（map → odom → base_link → sensor frames）、第 14 章的 URDF/Xacro。Driver 能发 /odom，IMU 能发 /imu/data_raw——那为什么还要状态估计？

答案一句话：/odom 只是某个里程计来源给出的估计，并不等于"系统最终应该使用的机器人状态"。真实 AGV 中，轮编码器、IMU、视觉里程计、GPS 各自只擅长观测一部分状态，每个来源都有误差、漂移、延迟和不确定性。robot_localization 的职责不是替代 Driver，也不是替代 SLAM，而是维护一份连续的机器人状态估计：

Wheel Odometry ─┐
                ├──> robot_localization ──> /odometry/filtered
IMU ────────────┘                         └──> odom -> base_link

本章不纠缠卡尔曼滤波矩阵证明，而是回答五个工程上更直接的问题：哪些字段应该融合？frame 怎么配？covariance 到底有什么用？时间戳不一致会怎样？谁该发布 odom → base_link？

一、先划清边界：它是什么，它不是什么

robot_localization 是 ROS 中用于实时非线性状态估计的一组 Node，Noetic 中最常用的是 ekf_localization_node（EKF）和 ukf_localization_node（UKF）。它接收 nav_msgs/Odometry、sensor_msgs/Imu、PoseWithCovarianceStamped、TwistWithCovarianceStamped；它不是电机 Driver、轮速解码器、IMU Driver、SLAM、AMCL，也不是路径规划器——它位于这些模块之间。这一层真正关心的是：

上游给出的观测值是什么、在哪个坐标系、对应哪个时间、可信度多大；然后维护一份连续状态估计并对外发布。

二、TF Ownership：把 /odom 改名 /odom/raw

教程做了一个关键重构：Driver 输出从 /odom 改为 /odom/raw（原始轮式里程计），EKF 输出 /odometry/filtered 并接管 odom → base_link 的发布。此时运行链的 ownership 清晰划分：

数据 / 变换	当前 owner
/odom/raw、/imu/data_raw、/joint_states	ros1_driver_lab
/odometry/filtered、odom → base_link	ekf_localization_node
base_link → imu/laser/wheel links	robot_state_publisher
map → odom	尚未进入，本章无人发布

launch 中有意删除了此前教学用的 odom_tf_broadcaster——因为 publish_tf=true, world_frame=odom 之后，这条动态变换已归 EKF 所有。同一条 parent/child 变换必须有唯一运行时 owner，否则就是 TF ownership 冲突。这一设计原则贯穿全书：TF tree 中每条边都要能回答"谁在发布"。

三、15 维状态与 two_d_mode：别把消息字段全开成 true

滤波器维护 15 个状态变量：[X, Y, Z, roll, pitch, yaw, Vx, Vy, Vz, Vroll, Vpitch, Vyaw, Ax, Ay, Az]。但 15 维不代表要有 15 个传感器，更不代表每个传感器都要提供全部维度。二维差速 AGV 真正关心的通常只有：X, Y, yaw, Vx, Vyaw。这就是 two_d_mode: true 的作用——不是"把输入消息变成二维"，而是把平面外运动相关的状态约束在二维机器人模型中。

四、EKF vs UKF：predict + correct 的两种非线性打开方式

卡尔曼滤波的核心不是"多传感器求平均"，而是持续维护状态估计 x 和状态不确定性 P，循环执行 predict（受过程噪声 Q 影响）与 correct（结合测量值和测量噪声 R）。两者差别在于如何处理非线性——差速机器人的平面运动包含 dx = Vx·cos(yaw)·dt 这样的姿态-速度耦合，状态转移天然非线性。

• EKF（扩展卡尔曼滤波）：在当前状态附近对非线性函数做局部线性化——predict 阶段计算 Jacobian F = ∂f/∂x，用 P- = F·P·Fᵀ + Q 传播协方差；correct 阶段用 Kalman Gain 决定"这一维该更信预测还是更信观测"。计算量低、行为好分析、在 robot_localization 中使用成熟，是工程默认路径。
• UKF（无迹卡尔曼滤波）：不做 Jacobian，而是在当前概率分布周围挑一组 sigma points（15 维状态即 2n+1 = 31 个点），让它们真正穿过非线性模型，再从结果恢复新的均值与协方差。强非线性下通常更能保留分布特征，代价是计算量更高、多出 alpha/kappa/beta 三个参数（官方建议不熟悉就保持默认 0.001/0.0/2.0）。

教程的对比实验设计值得学习：launch 参数 filter_node 让 EKF/UKF 在同一批输入、同一套 frame/time/covariance 契约、同一套输出 Topic/TF 下只替换滤波核心——这样比较才有意义。同时作者诚实标注：当前 Demo IMU 仍由轮速推导，实验只能验证配置可切换、数据链一致、TF owner 唯一，不能证明哪种滤波器在真实 AGV 上精度更高。

维度	EKF	UKF
非线性处理	当前状态附近 Jacobian 线性化	sigma points 通过真实非线性模型
计算量	较低	较高（31 个 sigma points）
工程调试	相对直接	多出 alpha/kappa/beta 行为
本系列定位	默认路径	对照实验

更重要的一条工程判断：不能得出"UKF 一定比 EKF 准"。滤波效果首先受传感器可信度、frame 正确性、时间戳、covariance 合理性、状态维度选择、是否重复融合相关数据限制——输入契约本身错误，换 UKF 不会自动修复系统。

五、covariance 不是装饰字段：P、Q、R 各管什么

• P（Estimate Error Covariance）：当前状态估计自身有多不确定；
• Q（Process Noise Covariance）：运动模型本身有多不可靠；
• R（Measurement Covariance）：这一次传感器测量有多不可靠。

ROS 消息里的 Odometry.twist.covariance、Imu.angular_velocity_covariance 等主要对应测量侧的 R。covariance 会直接影响滤波器如何对待这份测量——方差越小声明越确定，越大越不可信。但不能靠"随便把 covariance 调得很小"让结果看起来稳定：真实产品中的 covariance 应来自传感器规格、实验统计、标定或可解释的误差模型。教程还专门设计了 launch 参数实验（imu_angular_velocity_variance、odom_twist_linear_variance）证明：covariance 属于传感器数据契约，会随观测进入滤波器，不是无意义字段。

六、_config 的 15 个 true/false：先问"这是谁测到的"

每个输入需要一个 15 位 _config 数组。本章默认 profile 异常克制：

odom0_config: [..., Vx=true, ...]   # wheel odometry 只贡献车体前向速度
imu0_config:  [..., Vyaw=true, ...] # IMU 只贡献绕 Z 轴角速度

为什么默认不融合 /odom/raw 的 X、Y、yaw？因为教学 Driver 中这些位姿本来就是 Vx/Vyaw 经差速运动学积分出来的——把它们全设 true 会制造"五份独立证据"的认知错觉，实际来源仍是同一套轮编码器。先问这个状态是谁真正测到的，再决定是否作为观测输入，而不是按消息字段数量配置滤波器。

IMU 只用 angular_velocity.z 的原因同样明确：Driver 消息里 orientation_covariance[0] = -1.0 表示设备没有可用姿态解算，orientation.w = 1.0 只是字段合法的占位。而 IMU 测量在 imu_link 坐标系表达，EKF 能正确解释它，靠的正是第 13、14 章建立的 TF 链——三个章节由此串成一个整体：Driver 给 frame_id，TF 提供 frame 间变换，robot_localization 才能正确解释观测方向。

七、differential 与 relative：两个最容易混的参数

这两个参数都涉及"做差"，但改变的是完全不同的观测语义。设一个 pose 传感器输出 t0: (10.0m, 30°), t1: (10.5m, 32°), t2: (11.2m, 35°)：

• differential=true：相邻两帧做差 再除以 dt，把绝对 pose 转成 velocity 观测。典型场景：两个绝对 yaw 来源（轮式里程计与 IMU orientation）长期漂移后互相拉扯滤波器——保留最可信来源的 absolute pose，把另一个 differential 化成变化率。代价：失去该输入的"绝对朝向锚点"，若所有 orientation 来源都被 differential 化，yaw 绝对误差缺少观测拉回、covariance 会持续增长。它是改变观测模型，不是无代价的"防抖开关"；融合 GPS 时官方明确要求保持 false。
• relative=true：始终减去首帧，但仍然是 pose 语义——不除以 dt、不转速度。适合传感器初始绝对值（如第一帧在 (10.0, 2.0, 30°)）不是系统想保留的全局锚点、只关心启动后运动的场景。局限：只能去掉初始常量偏置，解决不了后续各自漂移。

维度	differential	relative
参考对象	上一帧 t-1	第一帧 t0
是否用 dt	是	否
输出语义	velocity	pose
典型场景	多绝对 pose 来源冲突且缺直接速度观测	初始 pose 非零，只关心相对位姿

教程为此专门建了隔离实验：新增 /demo_pose 输入与两个独立 profile，launch 故意不启动 Driver，用固定发布三帧非零初始位姿的脚本分别观察——变化只来自参数本身，不被其它观测污染。这种"控制变量到极致"的实验设计，比结论本身更值得借鉴。

八、world_frame 与时间：谁管哪条 TF，滤波器不是收到消息才回调

world_frame: odom 决定本章 EKF 只发布 odom → base_link；故意没有 map → odom——odom 追求短期连续、允许长期漂移，map 追求全局一致、允许校正带来的离散变化，下一章 SLAM/AMCL 才接管全局层。不能为"TF tree 看起来完整"就随手用 static transform 固定 map → odom。

时间维度上，滤波器有自己的输出频率（30Hz）并需处理输入时间戳、到达顺序、丢帧与 TF 查询时间。sensor_timeout: 0.20 的含义不是"没消息就退出"，而是：传感器暂时沉默时，滤波器依据内部模型继续 prediction，而不是进行 measurement correction——这正是 predict 与 correct 必须区分的原因。transform_timeout: 0.0 则表示不为等待 TF 长时间阻塞滤波循环。

结语：一条链的成立，靠的是每个环节的契约

这一章把前几章的三层第一次真正合在一起：/cmd_vel → Driver → /odom/raw + /imu/data_raw → EKF → /odometry/filtered，而 odom → base_link 的发布权从教学 broadcaster 平稳移交给了 EKF。它给出的与其说是 robot_localization 配置教程，不如说是一套状态估计的数据契约思维：每个观测要能回答"是谁测的、在哪个坐标系、哪个时间、可信度多大"；每条 TF 要有唯一 owner；每次参数改动要有隔离实验验证。下一章进入 SLAM/AMCL，map → odom 这条全局边才会上线——从"连续局部状态估计"走向"机器人在全局地图中的位置"。

---

参考资料（引自原文文末）

本章关于 robot_localization 的 15 维状态、EKF/UKF、two_d_mode、sensor_timeout、world_frame、publish_tf、*_config、*_differential 和 *_relative 的行为，依据 ROS robot_localization 官方文档与维护仓库说明：

• ROS Noetic API 文档入口：https://docs.ros.org/en/noetic/api/robot_localization/html/index.html
• robot_localization 状态估计 Node 文档：https://github.com/cra-ros-pkg/robot_localization/blob/noetic-devel/doc/state_estimation_nodes.rst
• 配置说明：https://github.com/cra-ros-pkg/robot_localization/blob/noetic-devel/doc/configuring_robot_localization.rst
• Noetic FilterBase API：https://docs.ros.org/en/noetic/api/robot_localization/html/api/classRobotLocalization_1_1FilterBase.html
• Noetic UKF 源码：https://docs.ros.org/en/noetic/api/robot_localization/html/api/ukf_8cpp_source.html

本工程固定在 ROS1 Noetic；仓库链接也绑定 noetic-devel，避免把 ROS2 分支上的参数或行为误写进本章。

原文：ROS教程15：robot_localization——从轮式里程计与 IMU 到机器人状态估计（wdfk-prog 的嵌入式日记，微信公众号）。本博客为经授权整理的精读版。