4、里程计基础：轮式里程计原理、编码器数据读取、odom话题发布、里程计标定方法

说到机器人导航，里程计是绕不开的基础。说白了，里程计就是机器人对自己“走了多远、转了多少弯”的估计。没有它，激光雷达扫得再准，也不知道自己在地图上的哪个位置。

我刚开始做机器人时，总觉得里程计不就是个轮子转圈嘛，有什么难的？结果第一次让机器人在走廊里走直线，它愣是画了个弧线出来。嗯，从那以后我再也不敢小看里程计了。

4.1 轮式里程计原理

轮式里程计的核心思想很简单：通过测量轮子转动的圈数，推算机器人移动的距离和方向。你想想看，如果知道轮子转了多少圈，又知道轮子的周长，那机器人走了多远不就出来了？

但实际没那么简单。机器人通常有两个驱动轮，左右轮转速不同，机器人就会转弯。这就引出了差速驱动模型。

4.1.1 差速驱动模型

差速驱动是两轮机器人最常用的模型。左右轮独立驱动，通过转速差实现转向。我习惯用下面的公式来推算机器人的位姿变化：

// 左右轮移动距离
Δs_left  = Δencoder_left  * wheel_circumference / encoder_resolution
Δs_right = Δencoder_right * wheel_circumference / encoder_resolution

// 机器人中心移动距离和转角
Δs       = (Δs_left + Δs_right) / 2
Δθ       = (Δs_right - Δs_left) / wheel_base

// 更新位姿（x, y, θ）
x += Δs * cos(θ + Δθ / 2)
y += Δs * sin(θ + Δθ / 2)
θ += Δθ

这里有个关键参数——轮距（wheel_base）。它指的是左右轮中心之间的距离。这个值如果不准，转弯时的角度误差会累积得非常快。

4.1.2 里程计误差来源

里程计误差主要来自三个方面。我整理了一个表格，方便你对照排查：

误差类型	来源	影响
系统误差	轮径不准、轮距不准、编码器分辨率有限	直线走偏、转弯角度不准
非系统误差	地面打滑、轮胎磨损、地面不平	突然的位姿跳变
累积误差	上述误差随时间累积	距离越长，位置越偏

系统误差可以通过标定来修正，非系统误差则很难完全消除。所以里程计通常只用于短距离的局部定位，长距离还是要靠激光雷达或视觉来修正。

4.2 编码器数据读取

编码器是里程计的眼睛。它把轮子的转动转换成电信号，我们通过读取这些信号来计算转速和转角。

4.2.1 编码器类型

常见的编码器有两种：

• 增量式编码器：输出脉冲信号，只能知道相对位置变化。掉电后位置丢失。
• 式编码器：输出角度值，掉电后位置不丢失。但价格贵，接口复杂。

大多数机器人底盘用的是增量式编码器，便宜且够用。我个人习惯在代码里维护一个累计脉冲数，这样即使掉电，重启后也能通过回零操作恢复位置。

4.2.2 在ROS中读取编码器数据

在ROS里，编码器数据通常通过硬件接口读取。我以Arduino为例，展示一个简单的读取逻辑：

// Arduino端：读取编码器脉冲
volatile long left_encoder_count = 0;
volatile long right_encoder_count = 0;

void setup() {
  Serial.begin(115200);
  attachInterrupt(digitalPinToInterrupt(2), left_encoder_isr, RISING);
  attachInterrupt(digitalPinToInterrupt(3), right_encoder_isr, RISING);
}

void loop() {
  // 每10ms发送一次编码器数据
  static unsigned long last_time = 0;
  if (millis() - last_time > 10) {
    Serial.print(left_encoder_count);
    Serial.print(",");
    Serial.println(right_encoder_count);
    last_time = millis();
  }
}

void left_encoder_isr() {
  left_encoder_count++;
}

void right_encoder_isr() {
  right_encoder_count++;
}

在ROS端，我们通过rosserial或自定义串口节点接收这些数据，然后转换成里程计信息。

4.3 odom话题发布

有了编码器数据，接下来就是发布odom话题。ROS里里程计话题的标准格式是nav_msgs/Odometry，包含位置、姿态和速度信息。

4.3.1 构建Odometry消息

我一般这样组织代码：

#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <tf/transform_broadcaster.h>

// 假设已经计算出机器人的位姿 (x, y, theta)
// 以及线速度 vx 和角速度 vth

nav_msgs::Odometry odom_msg;
odom_msg.header.stamp = current_time;
odom_msg.header.frame_id = "odom";

// 位置
odom_msg.pose.pose.position.x = x;
odom_msg.pose.pose.position.y = y;
odom_msg.pose.pose.position.z = 0.0;

// 姿态（四元数）
geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(theta);
odom_msg.pose.pose.orientation = odom_quat;

// 速度
odom_msg.child_frame_id = "base_link";
odom_msg.twist.twist.linear.x = vx;
odom_msg.twist.twist.angular.z = vth;

// 发布
odom_pub.publish(odom_msg);

4.3.2 TF变换

光发布odom话题还不够，还要发布TF变换。ROS的导航栈依赖TF来知道各个坐标系之间的关系。odom到base_link的变换就是里程计的核心输出。

// 发布TF变换
geometry_msgs::TransformStamped odom_trans;
odom_trans.header.stamp = current_time;
odom_trans.header.frame_id = "odom";
odom_trans.child_frame_id = "base_link";

odom_trans.transform.translation.x = x;
odom_trans.transform.translation.y = y;
odom_trans.transform.translation.z = 0.0;
odom_trans.transform.rotation = odom_quat;

tf_broadcaster.sendTransform(odom_trans);

这里要注意：odom是固定坐标系，base_link是机器人本体坐标系。里程计的作用就是不断更新base_link在odom坐标系下的位姿。

4.4 里程计标定方法

里程计标定，说白了就是让机器人的“感觉”和“实际”对齐。我见过太多机器人因为里程计不准，导航时画龙、原地转圈、撞墙。标定是绕不开的一步。

4.4.1 直线标定

直线标定用来修正轮径误差。方法很简单：让机器人走一段已知距离，对比实际距离和里程计计算的距离。

1. 在地面画一条5米长的直线。
2. 让机器人从起点出发，沿直线走到终点。
3. 记录里程计显示的移动距离。
4. 计算修正系数：scale = 实际距离 / 里程计距离。
5. 将修正系数应用到轮径上：wheel_diameter_corrected = wheel_diameter * scale。

我建议你正反方向各走几次，取平均值。因为地面摩擦、电机特性在不同方向上可能有差异。

4.4.2 转弯标定

转弯标定用来修正轮距。方法：让机器人原地旋转一定角度，对比实际转角和里程计计算的转角。

1. 让机器人原地旋转360度（可以用陀螺仪或外部测量工具确认）。
2. 记录里程计计算的总转角。
3. 计算修正系数：scale = 360 / 里程计转角。
4. 将修正系数应用到轮距上：wheel_base_corrected = wheel_base / scale。

4.4.3 使用标定工具

手动标定太麻烦？ROS里有一些现成的标定工具。我推荐robot_pose_ekf和calibration_estimation包。它们能自动采集数据、优化参数。

不过说实话，自动标定工具虽然方便，但前提是你的传感器数据质量要过关。我个人的习惯是：先用手动标定把粗调搞定，再用自动工具做精调。这样效率最高。

4.5 本章知识体系

下面这张图总结了里程计的核心流程和标定方法。你可以把它当作一个快速参考：