4. 轮式里程计：轮式编码器原理、运动学模型（差速/阿克曼）、里程计发布与tf变换

各位同学，欢迎来到第四章。前面我们聊了激光雷达和IMU，但说实话，在机器人定位里，轮式里程计才是那个最朴实、最可靠的“老黄牛”。它不依赖外部环境，只要轮子转，它就算。今天我们就把它彻底吃透。

4.1 轮式编码器原理：从物理到数字

编码器，说白了就是一个把机械旋转变成电脉冲的传感器。我最早接触这东西是在大学做智能车竞赛，那时候用的还是最便宜的光电编码器，经常被灰尘堵住导致丢步，气得我够呛。

目前主流的有两种：

• 增量式编码器：输出A、B两相脉冲，通过相位差判断正反转。每转一圈输出固定的脉冲数（比如1024线）。掉电后位置丢失，上电需要归零。
• 式编码器：直接输出二进制或格雷码表示的角度位置。掉电不丢失，但贵。一般用在协作机器人关节上，轮式机器人用得少。

编码器输出的是脉冲数，怎么变成速度？公式很简单：

// 假设轮子半径 r，编码器线数 N，采样周期 dt
// 脉冲增量 delta_pulse
double wheel_speed = (delta_pulse / N) * 2 * PI * r / dt;

4.2 运动学模型：差速 vs 阿克曼

拿到左右轮速度之后，怎么算机器人的整体运动？这就看你的底盘类型了。我做过两种底盘的项目，踩过的坑不少，一个一个说。

4.2.1 差速模型（两轮独立驱动）

这是最常用的，像Kobuki、TurtleBot都是这种。核心思想：左右轮速度不同，机器人就会转弯。

公式推导其实不复杂，你想想看：

• 线速度 V = (V_left + V_right) / 2
• 角速度 ω = (V_right - V_left) / L （L是轮距）

然后对时间积分，就能得到位姿：

// 假设当前位姿 (x, y, theta)，采样周期 dt
double v = (v_left + v_right) / 2.0;
double w = (v_right - v_left) / wheel_base;

x += v * cos(theta) * dt;
y += v * sin(theta) * dt;
theta += w * dt;

4.2.2 阿克曼模型（前轮转向）

这种模型模拟汽车，前轮转向、后轮驱动。说实话，在ROS里做阿克曼里程计比差速麻烦一点，因为你需要知道转向角。

核心公式：

• 转弯半径 R = L / tan(δ) （L是轴距，δ是前轮转向角）
• 角速度 ω = V / R

我做过一个高尔夫球车的项目，用的就是阿克曼。当时最头疼的是转向角传感器标定——前轮摆正时，编码器读数不一定是0，需要做零点校准。

参数	差速模型	阿克曼模型
驱动方式	左右轮独立	后轮驱动，前轮转向
转弯方式	差速转向	前轮偏转
原地旋转	支持	不支持
里程计精度	中等（滑移影响大）	较高（轮子始终滚动）
ROS支持	直接使用差速控制器	需使用teb_local_planner

4.3 里程计发布与TF变换

算出了位姿，怎么告诉ROS？两个步骤：发布Odometry消息和广播TF变换。

4.3.1 发布Odometry消息

ROS里里程计的标准消息是 nav_msgs/Odometry。我习惯在 odom 坐标系下发布，这样Nav2可以直接订阅。

#include <nav_msgs/Odometry.h>

nav_msgs::Odometry odom_msg;
odom_msg.header.stamp = ros::Time::now();
odom_msg.header.frame_id = "odom";
odom_msg.child_frame_id = "base_link";

// 填入位姿
odom_msg.pose.pose.position.x = x;
odom_msg.pose.pose.position.y = y;
odom_msg.pose.pose.orientation = tf::createQuaternionMsgFromYaw(theta);

// 填入速度（可选，但建议填）
odom_msg.twist.twist.linear.x = v;
odom_msg.twist.twist.angular.z = w;

odom_pub.publish(odom_msg);

4.3.2 广播TF变换

TF是ROS的灵魂。里程计需要广播 odom → base_link 的变换。注意：odom是固定坐标系，base_link是移动坐标系。

#include <tf/transform_broadcaster.h>

tf::TransformBroadcaster tf_broadcaster;
tf::Transform transform;
transform.setOrigin(tf::Vector3(x, y, 0.0));
tf::Quaternion q;
q.setRPY(0, 0, theta);
transform.setRotation(q);

tf_broadcaster.sendTransform(
    tf::StampedTransform(transform, ros::Time::now(), "odom", "base_link"));

4.4 完整代码框架

最后，给一个完整的里程计节点框架。你直接拿去改改轮距和编码器线数就能用。

class WheelOdometry {
public:
    WheelOdometry() : nh("~") {
        // 参数读取
        nh.param("wheel_base", wheel_base, 0.5);
        nh.param("wheel_radius", wheel_radius, 0.1);
        nh.param("encoder_resolution", encoder_res, 1024.0);
        
        // 发布器
        odom_pub = nh.advertise<nav_msgs::Odometry>("odom", 50);
        
        // 订阅编码器数据（假设话题为 /wheel_encoders）
        enc_sub = nh.subscribe("/wheel_encoders", 10, &WheelOdometry::encCallback, this);
    }
    
    void encCallback(const your_msgs::Encoders::ConstPtr& msg) {
        // 1. 计算左右轮速度
        double v_left = calcWheelSpeed(msg->left_pulse, prev_left_pulse);
        double v_right = calcWheelSpeed(msg->right_pulse, prev_right_pulse);
        
        // 2. 运动学解算
        double v = (v_left + v_right) / 2.0;
        double w = (v_right - v_left) / wheel_base;
        
        // 3. 位姿积分（带数值保护）
        double dt = (msg->header.stamp - last_time).toSec();
        if (fabs(w) > 1e-6) {
            x += (v/w) * (sin(theta + w*dt) - sin(theta));
            y += (v/w) * (cos(theta) - cos(theta + w*dt));
        } else {
            x += v * cos(theta) * dt;
            y += v * sin(theta) * dt;
        }
        theta += w * dt;
        
        // 4. 发布
        publishOdometry(msg->header.stamp, v, w);
        publishTF(msg->header.stamp);
        
        // 更新状态
        prev_left_pulse = msg->left_pulse;
        prev_right_pulse = msg->right_pulse;
        last_time = msg->header.stamp;
    }
    
private:
    double calcWheelSpeed(int current_pulse, int& prev_pulse) {
        double delta = current_pulse - prev_pulse;
        return (delta / encoder_res) * 2 * M_PI * wheel_radius;
    }
    
    // ... 发布函数略
};

好了，这一章的内容就是这些。轮式里程计是传感器融合的基石，虽然它会有累积误差，但在短时间内非常可靠。下一章我们会把它和IMU、激光雷达放在一起，看看怎么用卡尔曼滤波把它们融合起来。嗯，到时候你会发现，里程计就像那个老实人，虽然不完美，但不可或缺。