【移动机器人】从编码器到轨迹：轮式里程计的运动学推导与实践

1. 轮式里程计：移动机器人的"计步器"

刚接触移动机器人开发时，我最困惑的就是：这玩意儿怎么知道自己走到哪了？后来发现，轮式里程计就像机器人的"计步器"。想象你蒙着眼睛走路，通过数步数和感受转弯角度，也能大概知道自己位置——这就是里程计的核心思路。

在实际项目中，我常用编码器作为"步数传感器"。比如某次用STM32开发时，通过AB相编码器每转产生390个脉冲，配合74HC245芯片做信号调理。这里有个实用细节：正交编码器能通过AB相90度相位差判断旋转方向，这对区分左右轮前进/后退至关重要。

轮式里程计的优势很明显：低成本（两个编码器几十元）、高更新频率（轻松达到100Hz）、无需外部依赖（不像激光SLAM需要环境特征）。但缺点也很突出——轮子打滑时误差会累积。有次测试时机器人卡在门槛上，轮子空转导致定位漂移了2米多，这个坑我至今记忆犹新。

2. 从脉冲到速度：硬件数据的软着陆

2.1 编码器信号处理实战

拿到编码器脉冲只是第一步，我通常这样处理数据（以STM32为例）：

c复制// 定时器编码器模式配置
TIM_Encoder_InitTypeDef encoder_config = {
    .EncoderMode = TIM_ENCODERMODE_TI12,
    .IC1Polarity = TIM_ICPOLARITY_RISING,
    .IC1Selection = TIM_ICSELECTION_DIRECTTI,
    .IC1Prescaler = TIM_ICPSC_DIV1,
    .IC1Filter = 6,  // 适当滤波防抖动
    // 类似配置IC2...
};
HAL_TIM_Encoder_Init(&htim3, &encoder_config);

关键点在于速度计算策略。我对比过三种方法：

• 脉冲计数法：固定周期(如10ms)读取计数值，简单但低速时分辨率差
• 周期测量法：测量脉冲间隔时间，低速精准但高速会溢出
• 混合法：根据速度自动切换模式，效果最好但实现复杂

实测下来，对于轮径0.1m的机器人，在1m/s速度时：

2.2 轮速计算的工程细节

得到脉冲数后，真正的坑才开始。有次调试时发现机器人总是走弧线，后来发现是左右轮径存在0.3mm差异。这里分享我的校准方法：

1. 让机器人直线行走5米
2. 测量实际偏移量Δx
3. 计算修正系数：右轮径 *= (5.0)/(5.0 + Δx)
4. 重复3次直到Δx<1cm

线速度计算时容易忽略轮子变形的影响。橡胶轮在负重时接触面会变形成矩形，等效半径减小。我的经验公式：

code复制有效半径 = 标称半径 × (1 - 0.12×负载重量/kg)

3. 运动学模型：机器人如何"思考"自己的运动

3.1 差分底盘的运动学本质

差分机器人的运动学就像人在冰上行走：两脚（轮子）分开推冰产生转向。推导时我常用"瞬时旋转中心(ICR)"的概念辅助理解：

1. 画一条连接两轮轴的直线
2. 根据速度比确定ICR位置
3. 整个机器人可视为绕ICR做瞬时圆周运动

这个视角下，角速度公式ω = (vr - vl)/L就非常直观了。但要注意这个模型的两个前提：

• 纯滚动假设：轮子不能打滑
• 刚性车身：轮距L必须恒定

3.2 航向角处理的坑

航向角θ的积分是最容易出问题的地方。有次机器人转圈时θ突然跳变，原来是没处理角度周期性。正确的做法是：

python复制def angle_wrap(theta):
    while theta > np.pi:
        theta -= 2*np.pi
    while theta < -np.pi:
        theta += 2*np.pi
    return theta

另一个常见错误是角速度积分顺序。应该采用：

code复制θ_new = θ_old + ω*Δt + 0.5*(ω_new - ω_old)*Δt

这个二阶近似能显著减小高速转弯时的误差。

4. 从理论到实践：轨迹推算的完整实现

4.1 时间同步的艺术

实际开发中最头疼的是传感器数据不同步。我的解决方案是：

• 为编码器数据添加硬件时间戳
• 采用双缓冲机制：

  c复制typedef struct {
      uint32_t timestamp;
      int32_t left_ticks;
      int32_t right_ticks;
  } EncoderBuffer;
  
  EncoderBuffer buf[2];
  volatile uint8_t active_buf = 0;
  

运动学更新时采用线性预测：

code复制v_pred = v_prev + (t_now - t_prev)*a_est

4.2 完整的轨迹推算流程

这是我优化过的实现步骤：

1. 数据准备：

   • 获取左右轮脉冲数ΔT_L, ΔT_R
   • 查询时间间隔Δt（建议用硬件定时器）

2. 速度计算：

   python复制# 轮子圆周运动模型
   wheel_circumference = 2 * np.pi * wheel_radius
   v_l = (delta_ticks_l / ticks_per_rev) * wheel_circumference / delta_t
   v_r = (delta_ticks_r / ticks_per_rev) * wheel_circumference / delta_t
   

3. 机体速度转换：

   python复制v = (v_r + v_l) / 2
   omega = (v_r - v_l) / wheel_base
   

4. 位姿更新（改进欧拉法）：

   python复制delta_theta = omega * delta_t
   avg_theta = theta + delta_theta / 2
   x += v * np.cos(avg_theta) * delta_t
   y += v * np.sin(avg_theta) * delta_t
   theta += delta_theta
   

5. 误差补偿：

   • 应用预先标定的系统误差矩阵
   • 限幅处理异常值

4.3 可视化调试技巧

我习惯用Python的matplotlib实时绘制轨迹：

python复制def update_plot():
    plt.clf()
    plt.quiver(x, y, np.cos(theta), np.sin(theta), color='r') 
    plt.plot(path_x, path_y, 'b-')
    plt.axis("equal")
    plt.pause(0.01)

关键指标监控：

• 航向角波动：应<0.5度/秒（静止时）
• 速度一致性：左右轮速差应<2%
• 闭合误差：往返同一位置时的偏差应<移动距离的3%