第4章：里程计传感器接入

各位同学，今天我们来聊聊轮式里程计。说实话，里程计在导航系统里就像人的眼睛——虽然不完美，但没了它，机器人基本就是个瞎子。我做了这么多年导航，踩过最多的坑，就是里程计不准导致的定位漂移。

4.1 轮式里程计原理

轮式里程计，说白了就是通过轮子转了多少圈，来推算机器人走了多远。听起来简单，但实际做起来门道不少。

核心公式其实就一个：

位移 = 轮子周长 × 编码器脉冲数 / 每圈脉冲数

嗯，这里要注意——轮子周长不是一成不变的。轮胎气压、负载大小都会影响实际滚动半径。我在项目里遇到过，空载时里程计挺准，一装上负载就偏了10%以上。

对于差速驱动机器人，我们通常用这样的模型：

左轮速度 vL = (2πR × ΔN_L) / (Δt × PPR)
右轮速度 vR = (2πR × ΔN_R) / (Δt × PPR)

机器人线速度 v = (vL + vR) / 2
机器人角速度 ω = (vR - vL) / L

其中L是轮距，PPR是编码器每圈脉冲数。这个模型虽然简单，但够用。我见过有人用更复杂的模型，结果反而因为参数太多调不准。

关键点：里程计本质上是积分过程，误差会累积。所以它只适合短距离定位，长距离必须配合其他传感器。

4.2 编码器驱动实现

编码器驱动，我建议直接在ROS2的驱动层实现。为什么？因为这样能保证时间戳的准确性。时间戳不准，里程计就没法用。

下面是我常用的编码器驱动框架：

class WheelEncoderNode : public rclcpp::Node
{
public:
  WheelEncoderNode() : Node("wheel_encoder_node")
  {
    // 初始化编码器接口
    encoder_left_ = std::make_unique<Encoder>(LEFT_WHEEL_PIN);
    encoder_right_ = std::make_unique<Encoder>(RIGHT_WHEEL_PIN);
    
    // 创建里程计发布者
    odom_pub_ = this->create_publisher<nav_msgs::msg::Odometry>("odom", 10);
    
    // 定时器，10ms周期读取编码器
    timer_ = this->create_wall_timer(
      10ms, std::bind(&WheelEncoderNode::update_odometry, this));
  }

private:
  void update_odometry()
  {
    // 读取编码器脉冲数
    int32_t left_pulses = encoder_left_->read();
    int32_t right_pulses = encoder_right_->read();
    
    // 计算位移和角度变化
    double delta_left = (left_pulses / PPR_) * WHEEL_CIRCUMFERENCE_;
    double delta_right = (right_pulses / PPR_) * WHEEL_CIRCUMFERENCE_;
    
    double delta_s = (delta_left + delta_right) / 2.0;
    double delta_theta = (delta_right - delta_left) / WHEEL_BASE_;
    
    // 更新位姿
    x_ += delta_s * cos(theta_ + delta_theta / 2.0);
    y_ += delta_s * sin(theta_ + delta_theta / 2.0);
    theta_ += delta_theta;
    
    // 发布里程计消息
    auto odom_msg = nav_msgs::msg::Odometry();
    odom_msg.header.stamp = this->now();
    odom_msg.header.frame_id = "odom";
    odom_msg.child_frame_id = "base_footprint";
    
    odom_msg.pose.pose.position.x = x_;
    odom_msg.pose.pose.position.y = y_;
    odom_msg.pose.pose.orientation = createQuaternionFromYaw(theta_);
    
    // 设置协方差
    setCovariance(odom_msg);
    
    odom_pub_->publish(odom_msg);
  }
};

这段代码我用了很多年，基本没大改过。唯一要注意的是，编码器读取要用硬件中断，别用轮询。我曾经在树莓派上用轮询，结果CPU占用率直接飙到80%。

个人经验：编码器驱动里，一定要加防抖处理。机械振动会导致编码器产生毛刺脉冲，不加防抖的话，里程计会莫名其妙地漂移。

4.3 里程计协方差配置

协方差配置，这是很多人忽略的地方。说白了，协方差就是告诉滤波器：「我这个里程计有多可信」。

在ROS2的Odometry消息里，协方差是一个6×6的矩阵：

# 协方差矩阵排列顺序
# [x, y, z, roll, pitch, yaw]
# 对于轮式机器人，我们主要关注：
# x方向位置误差、y方向位置误差、yaw角度误差

odom_msg.pose.covariance = {
  // x方向协方差
  x_cov, 0.0, 0.0, 0.0, 0.0, 0.0,
  // y方向协方差
  0.0, y_cov, 0.0, 0.0, 0.0, 0.0,
  // z方向（固定为0）
  0.0, 0.0, 1e-9, 0.0, 0.0, 0.0,
  // roll（固定为0）
  0.0, 0.0, 0.0, 1e-9, 0.0, 0.0,
  // pitch（固定为0）
  0.0, 0.0, 0.0, 0.0, 1e-9, 0.0,
  // yaw角度协方差
  0.0, 0.0, 0.0, 0.0, 0.0, yaw_cov
};

协方差值怎么设？我一般这样估算：

误差来源	典型协方差值	说明
编码器量化误差	0.01 ~ 0.05	取决于编码器分辨率
轮子打滑	0.1 ~ 0.5	地面越滑，值越大
轮距误差	0.05 ~ 0.2	安装误差导致
轮胎变形	0.02 ~ 0.1	负载变化引起

避坑指南：我曾经把协方差设得太小，结果EKF完全信任里程计，IMU的数据基本被忽略。机器人一打滑，定位就飞了。后来我把协方差调大了3倍，效果好了很多。

实际调试时，我建议这样做：

先让机器人走直线，看x方向误差
再让机器人原地转圈，看yaw方向误差
根据实测数据调整协方差
最后在真实场景里跑一遍，微调参数

嗯，这里有个小技巧——协方差不是一成不变的。速度越快，误差越大。我习惯根据当前速度动态调整协方差：

double speed = sqrt(vx * vx + vy * vy);
double base_cov = 0.05;
double speed_factor = 1.0 + speed * 0.1;  // 速度越快，协方差越大

x_cov = base_cov * speed_factor;
y_cov = base_cov * speed_factor;
yaw_cov = 0.1 * speed_factor;

这样处理之后，机器人在低速时定位更稳，高速时也不会因为过度信任里程计而出问题。

总结一下：里程计接入看似简单，但要做好不容易。原理要懂，驱动要稳，协方差要调。这三样都做到位了，你的导航系统就成功了一半。

好了，里程计这块就讲这么多。记住，理论再漂亮，不如实际跑一圈。回去把代码写出来，在真实机器人上试试，你会发现很多书上没写的问题。

📚 课程目录

第4章：里程计传感器接入

4.1 轮式里程计原理

4.2 编码器驱动实现

4.3 里程计协方差配置