4. 里程计基础：轮式里程计原理、编码器数据读取与处理、航迹推演（Dead Reckoning）算法实现、里程计误差分析与补偿

各位同学，欢迎来到第四章。这一章我们聊点实在的——里程计。

说实话，我入行那会儿，觉得里程计就是个“轮子上绑个码盘”，没啥技术含量。直到有一次，我负责的AGV在仓库里跑了不到50米，定位就偏了半米多，差点撞上货架。从那以后，我才真正开始认真研究这玩意儿。

里程计是机器人导航的“地基”。地基不稳，上面盖的楼（比如AMCL、Cartographer）再漂亮也得塌。今天我们就把它彻底搞明白。

4.1 轮式里程计原理

说白了，里程计就是利用轮子转了多少圈，来推算机器人走了多远、转了多少角度。你想想看，这其实和我们走路数步数是一个道理。

核心公式其实就两个：

位移 = 轮子周长 × 编码器脉冲数 / 每转脉冲数
转角 = (左轮位移 - 右轮位移) / 轮距

对于差速驱动机器人，我们通常用下面这个模型：

差速模型核心公式：

// 左右轮速度
v_left  = (encoder_left_delta / dt) * (wheel_circumference / ticks_per_rev)
v_right = (encoder_right_delta / dt) * (wheel_circumference / ticks_per_rev)

// 机器人线速度和角速度
v = (v_left + v_right) / 2.0
w = (v_right - v_left) / wheel_base

// 位姿更新（离散时间）
x += v * cos(theta) * dt
y += v * sin(theta) * dt
theta += w * dt

这里有个坑，我当年踩过——wheel_base（轮距）这个参数，你以为量一下轮子中心到中心的距离就行？其实不是。实际有效轮距会因为轮胎变形、地面摩擦而略有变化。我建议你做个标定实验：让机器人原地转10圈，看实际转了多少度，反推有效轮距。

4.2 编码器数据读取与处理

编码器是里程计的“眼睛”。常见的有两种：

类型	原理	精度	成本	我的建议
增量式编码器	输出脉冲信号，需要计数	中	低	大多数场景够用
式编码器	直接输出角度值	高	高	高精度场景推荐

读取编码器数据，ROS里通常用realtime_loop或者中断方式。我个人习惯在驱动层做硬件中断，而不是轮询——轮询容易丢脉冲，尤其是在高速运动时。

小技巧：编码器数据最好做一下“毛刺滤波”。我遇到过电机EMI干扰导致编码器多跳几个脉冲的情况。一个简单的办法：如果当前脉冲和上一帧的差值超过物理极限（比如最大加速度对应的脉冲变化量），直接丢弃这一帧。

代码示例（伪代码，实际使用时请适配你的硬件）：

// 编码器中断服务函数
void encoderISR() {
    static long last_ticks = 0;
    long current_ticks = readEncoderCounter();
    long delta = current_ticks - last_ticks;
    
    // 毛刺滤波
    if (abs(delta) > MAX_DELTA_PER_INTERRUPT) {
        // 丢弃，可能是干扰
        last_ticks = current_ticks;
        return;
    }
    
    // 累加里程
    odom_ticks += delta;
    last_ticks = current_ticks;
}

4.3 航迹推演（Dead Reckoning）算法实现

航迹推演，英文叫Dead Reckoning，名字听着挺吓人，其实就是“猜我在哪”。从上一个已知位置出发，根据编码器数据一步步推算当前位置。

实现起来不复杂，但有几个细节要注意：

坐标系统一：所有计算必须在机器人坐标系（base_link）和世界坐标系（odom）之间正确转换。
时间同步：编码器数据的时间戳要和IMU、激光雷达对齐，否则融合时会出问题。
积分方法：简单用欧拉积分会累积误差，我建议用二阶龙格-库塔（RK2）或者直接使用tf2的transform功能。

警告：千万不要在回调函数里直接做航迹推演！ROS的回调频率不稳定，会导致时间戳错乱。正确的做法是：在单独的定时器里，以固定频率（比如50Hz或100Hz）读取最新的编码器数据，然后做推演。

下面是我在实际项目中用过的航迹推演代码片段：

void OdomNode::updateOdometry(const ros::TimerEvent&) {
    // 读取最新编码器数据
    long left_ticks  = encoder_left_.getTicks();
    long right_ticks = encoder_right_.getTicks();
    
    // 计算增量
    double delta_left  = (left_ticks  - last_left_ticks_)  * distance_per_tick_;
    double delta_right = (right_ticks - last_right_ticks_) * distance_per_tick_;
    
    // 更新位姿（使用RK2积分）
    double delta_s     = (delta_left + delta_right) / 2.0;
    double delta_theta = (delta_right - delta_left) / wheel_base_;
    
    double mid_theta = theta_ + delta_theta / 2.0;
    x_ += delta_s * cos(mid_theta);
    y_ += delta_s * sin(mid_theta);
    theta_ += delta_theta;
    
    // 发布odom消息
    publishOdometry(x_, y_, theta_);
    
    // 更新上一帧数据
    last_left_ticks_  = left_ticks;
    last_right_ticks_ = right_ticks;
}

4.4 里程计误差分析与补偿

里程计最大的问题就是——误差会累积。你跑得越远，位置就越不准。为什么会这样？

误差来源主要有三个：

系统误差：轮径不准、轮距不准、编码器分辨率有限。这些可以通过标定来补偿。
非系统误差：地面打滑、轮胎变形、地面不平。这些很难完全消除，只能通过传感器融合来抑制。
数值误差：积分方法、浮点精度。选好积分方法能缓解。

我记得有一次，机器人在瓷砖地面上跑，编码器显示走了10米，实际只走了9.2米。查了半天，发现是轮胎气压不足导致有效半径变小了。从那以后，我每次做标定都会先检查轮胎气压。

常用的补偿方法：

方法	适用场景	效果	实现难度
UMBmark标定法	差速轮	好（可补偿系统误差）	中
最小二乘标定	任意轮式	较好	高
在线自适应补偿	实时场景	一般（依赖其他传感器）	高

避坑指南：我曾经在UMBmark标定时，只做了直线和旋转，忽略了“弧线运动”的误差。结果标定完直线精度很高，但一跑弧线就偏。后来我加了8字形轨迹的标定数据，效果才真正好起来。

最后，给大家一个建议：里程计不要单独用。它和IMU、激光雷达做融合，才能发挥最大价值。下一章我们会详细讲传感器融合，但在此之前，请确保你的里程计本身是“健康”的——误差可控、参数标定过、数据干净。

📚 课程目录

4. 里程计基础：轮式里程计原理、编码器数据读取与处理、航迹推演（Dead Reckoning）算法实现、里程计误差分析与补偿

4.1 轮式里程计原理

4.2 编码器数据读取与处理

4.3 航迹推演（Dead Reckoning）算法实现

4.4 里程计误差分析与补偿