4、里程计话题排查：检查/odom话题频率与协方差、里程计跳变问题、轮式里程计与IMU融合问题

4.1 里程计话题频率——建图的第一道防线

做SLAM这么多年，我见过太多建图失败的案例。追根溯源，八成是里程计出了问题。而里程计问题里，频率异常又是最常见的。

你想想看，建图算法就像个裁缝。它需要不断收到新的布料（里程计数据），才能把地图缝起来。如果布料送得太慢，裁缝就会空转；如果送得太快，裁缝又来不及处理。

那怎么检查频率？我习惯用两个命令：

# 方法一：直接看话题频率
rostopic hz /odom

# 方法二：看消息时间戳是否连续
rostopic echo /odom | grep -A 2 "header.stamp"

正常情况，轮式里程计应该在50-100Hz。IMU里程计更高，200-500Hz。低于这个范围，建图就会开始出现漂移。

频率异常排查清单

检查CPU占用率——是不是计算资源不够？
检查发布节点是否被阻塞——有没有死锁？
检查消息队列长度——queue_size设了多少？
检查底层驱动——串口波特率对不对？

4.2 协方差——被忽视的信任度指标

很多人只看里程计的位姿数据，不看协方差。这是个坏习惯。

协方差说白了就是告诉算法：「我这数据有多靠谱」。协方差越小，算法越信任你。协方差突然变大，说明里程计自己都觉得不对劲了。

我记得有一次排查建图抖动问题。折腾了两天，最后发现是轮子打滑时，里程计协方差没更新。算法以为数据很准，结果地图全歪了。

检查协方差的命令：

rostopic echo /odom | grep -A 6 "covariance"

正常情况，协方差矩阵的对角线元素应该稳定在一个小范围内。如果出现数量级跳变，那肯定有问题。

我的经验：协方差不是越小越好。如果协方差一直为零，说明里程计根本没在计算不确定性。这种数据反而不可信。

4.3 里程计跳变——建图的大敌

里程计跳变，就是位置或姿态突然变了。比如上一秒还在(1.0, 2.0)，下一秒就跳到(5.0, 2.0)。

为什么会这样？我遇到过几种情况：

编码器计数溢出——32位整数溢出了
时间戳回跳——系统时间被NTP同步了
坐标系变换错误——tf树断了又重连
数据融合算法bug——EKF发散

怎么检测跳变？写个简单的监听脚本：

#!/usr/bin/env python3
import rospy
from nav_msgs.msg import Odometry

last_pose = None
threshold = 0.5  # 米

def odom_callback(msg):
    global last_pose
    x = msg.pose.pose.position.x
    y = msg.pose.pose.position.y
    
    if last_pose:
        dx = abs(x - last_pose[0])
        dy = abs(y - last_pose[1])
        if dx > threshold or dy > threshold:
            rospy.logwarn(f"里程计跳变！({dx:.2f}, {dy:.2f})")
    
    last_pose = (x, y)

rospy.init_node("odom_jump_detector")
rospy.Subscriber("/odom", Odometry, odom_callback)
rospy.spin()

警告：跳变检测的阈值要根据机器人速度来设。我见过有人设0.1米，结果机器人正常转弯都被报警了。建议先跑一圈正常轨迹，统计出最大位移变化量，再设阈值。

4.4 轮式里程计与IMU融合——1+1>2还是1+1<1？

轮式里程计低频但无漂移，IMU高频但有漂移。融合得好，两者互补。融合不好，互相拖累。

我常用的融合方案有两种：

融合方式	优点	缺点	适用场景
EKF（扩展卡尔曼滤波）	成熟稳定，ROS自带robot_localization包	对非线性系统近似效果一般	大多数室内机器人
ESKF（误差状态卡尔曼滤波）	对IMU处理更好，数值稳定性高	实现复杂，需要自己写	高动态、高速机器人

我个人更推荐ESKF。为什么？因为轮式里程计的误差模型不是高斯分布——打滑时误差很大，正常时误差很小。EKF处理这种非高斯噪声很吃力。

但如果你不想自己写，用robot_localization也够用。关键是要调好参数：

# robot_localization的EKF参数示例
ekf_localization_node:
  frequency: 50
  sensor_timeout: 0.1
  two_d_mode: true
  
  # 轮式里程计的噪声
  odom0: /odom
  odom0_config: [true, true, false, false, false, true, false, false, false, false, false, false, false, false, false]
  odom0_differential: false
  odom0_relative: true
  odom0_queue_size: 10
  
  # IMU的噪声
  imu0: /imu/data
  imu0_config: [false, false, false, false, false, true, false, false, false, false, false, true, false, false, false]
  imu0_differential: false
  imu0_relative: true
  imu0_queue_size: 10

避坑指南：我曾经把odom0_differential设成true，结果里程计数据被做了差分处理。位置信息全丢了，地图直接飞了。这个参数要搞清楚含义再设。

4.5 知识体系总览

下面这张图，是我梳理的里程计排查思路。每次遇到建图异常，我就按这个流程走一遍：

这张图把排查思路串起来了。从建图异常出发，先看频率，再看协方差，最后查跳变。三个都没问题，再考虑融合方案对不对。

嗯，里程计排查这块就讲这么多。记住一个原则：先确认数据质量，再优化融合算法。数据都不对，算法再牛也白搭。

可可精品资料库

📚 课程目录

4、里程计话题排查：检查/odom话题频率与协方差、里程计跳变问题、轮式里程计与IMU融合问题

4.1 里程计话题频率——建图的第一道防线

4.2 协方差——被忽视的信任度指标

4.3 里程计跳变——建图的大敌

4.4 轮式里程计与IMU融合——1+1>2还是1+1<1？

4.5 知识体系总览