2. 移动平台运动学与传感器：轮式里程计模型、IMU原理与标定、轮式编码器数据处理
各位同学，大家好。今天我们进入第二章，聊聊移动平台的运动学模型和传感器。说白了，就是搞清楚机器人怎么知道自己走了多远、转了多少弯。这部分内容，我当年刚入行时觉得挺简单，结果第一次做AGV（自动导引车）项目就被现实狠狠教育了一顿。嗯，咱们今天就把这些坑提前填上。
2.1 轮式里程计模型
里程计，英文叫Odometry，是移动机器人最基础的定位手段。它不依赖外部信号，全靠轮子转了多少圈来推算位置。听起来很朴素，但实际工程中，它往往是融合定位的“压舱石”。
2.1.1 差分驱动模型
差分驱动，就是两个独立驱动的轮子，再加一个或两个万向轮。转弯靠左右轮速度差实现。我个人习惯把这种模型叫做“坦克式转向”，因为原理和坦克一模一样。
核心公式其实就三个：
// 假设左右轮编码器读数分别为 delta_encoder_left, delta_encoder_right
// 轮子半径 r，轮距（左右轮间距）b
// 1. 计算左右轮移动距离
delta_s_left = delta_encoder_left * (2 * PI * r) / encoder_resolution
delta_s_right = delta_encoder_right * (2 * PI * r) / encoder_resolution
// 2. 计算机器人中心移动距离和转角
delta_s = (delta_s_left + delta_s_right) / 2.0
delta_theta = (delta_s_right - delta_s_left) / b
// 3. 更新位姿（x, y, theta）
x += delta_s * cos(theta + delta_theta / 2.0)
y += delta_s * sin(theta + delta_theta / 2.0)
theta += delta_theta
这里有个细节：为什么更新位姿时用 theta + delta_theta / 2.0？因为机器人是在转弯过程中移动的，取中间时刻的角度更准确。我在做仓储机器人时，发现如果不这么处理，转圈走矩形路径时，误差会累积得非常快。
避坑指南：我曾经在调试一台重载AGV时，发现里程计在直线上还行，一转弯就偏得离谱。查了两天才发现，左右轮的有效半径不一样——因为负载不均，轮胎被压扁的程度不同。所以，实际工程中，轮子半径不能只取标称值，最好做一次“轮胎半径校准”。
2.1.2 全向驱动模型
全向轮（麦克纳姆轮或全向轮）的好处是：可以横向移动、斜向移动，机动性极强。代价是：模型复杂，而且对地面平整度要求高。
以四轮麦克纳姆轮为例，它的运动学模型是这样的：
// 输入：机器人期望速度 (vx, vy, omega)
// 输出：四个轮子的转速 (w1, w2, w3, w4)
// L: 轮子到中心的纵向距离，W: 横向距离
w1 = (vx - vy - (L + W) * omega) / r
w2 = (vx + vy + (L + W) * omega) / r
w3 = (vx + vy - (L + W) * omega) / r
w4 = (vx - vy + (L + W) * omega) / r
你想想看，这个公式其实是在做“速度分解”。每个轮子既要提供前进力，又要提供侧向力，还要参与旋转。我建议初学者先别急着调参数，先把轮子的安装方向搞清楚——麦克纳姆轮的辊子方向是45度，装反了机器人会原地打转。
我的经验：全向轮里程计最怕打滑。因为四个轮子必须精确配合，只要有一个轮子空转，整个位姿估计就崩了。所以，如果你在瓷砖地面上用全向轮，记得把加速度设小一点，起步不要太猛。
2.2 IMU原理与标定
IMU（惯性测量单元）包含加速度计和陀螺仪。加速度计测线加速度，陀螺仪测角速度。两者结合，理论上可以推算姿态和位置。但现实中，IMU的噪声和漂移是最大的敌人。
2.2.1 加速度计原理
加速度计测量的是“比力”，也就是物体受到的合力减去重力。静止时，它测到的是重力加速度g。所以，我们可以利用加速度计来估计俯仰角和横滚角：
// 从加速度计读数 (ax, ay, az) 计算姿态角
roll = atan2(ay, az)
pitch = atan2(-ax, sqrt(ay*ay + az*az))
注意：加速度计不能用来估计偏航角（yaw），因为绕重力轴旋转时，重力分量不变。说白了，加速度计分不清你是朝北还是朝南。
2.2.2 陀螺仪原理
陀螺仪测量角速度。对角速度积分，就能得到角度变化。但积分会累积误差，这就是漂移的来源。
我做过一个测试：把IMU放在桌面上静止，陀螺仪输出的角速度均值不是0，而是0.01 rad/s左右。积分10秒，角度就偏了0.1 rad。所以，纯陀螺仪积分撑不过几分钟。
小技巧：实际项目中，我们通常用加速度计修正俯仰和横滚，用磁力计修正偏航，再用陀螺仪做短时间的高频积分。这就是“互补滤波”或“卡尔曼滤波”的基本思路。
2.2.3 IMU标定
IMU出厂时都有参数，但温度、安装误差都会导致偏差。我建议每个IMU上机前都做一次标定。
标定主要做三件事：
- 零偏标定：静止采集数据，取均值作为零偏。陀螺仪的零偏会随温度变化，所以最好在恒温环境下做。
- 尺度因子标定：让IMU做已知角速度的旋转，对比输出值。这个通常需要转台，没有转台的话，可以用“六面法”粗略标定加速度计。
- 轴间对准：确保IMU的三个轴与机器人坐标系对齐。我见过一个案例，IMU装歪了2度，结果机器人走直线时，里程计和IMU融合出来的轨迹总是偏的。
// 加速度计六面法标定（简化版）
// 分别将IMU的六个面朝上放置，记录加速度计读数
// 理论上，每个面朝上时，对应轴应输出 ±g
// 通过最小二乘法拟合出零偏和尺度因子
// 示例：X轴朝上时，ax = g + bias_x
// X轴朝下时，ax = -g + bias_x
// 解得：bias_x = (ax_up + ax_down) / 2
// scale_x = (ax_up - ax_down) / (2 * g)
警告：千万不要在IMU通电后立刻标定。我吃过这个亏——刚上电时，IMU内部温度还没稳定，零偏会慢慢漂移。等5分钟再标定，数据才可靠。
2.3 轮式编码器数据处理
编码器是里程计的数据来源。它把轮子的旋转转换成脉冲信号。常见的编码器有增量式和式两种。增量式便宜，但掉电丢位置；式贵，但一上电就知道角度。
2.3.1 脉冲计数与方向判断
增量式编码器通常输出A、B两相脉冲。A相领先B相90度，表示正转；B相领先A相，表示反转。通过检测边沿，可以同时获得位置和方向。
// 伪代码：编码器脉冲处理
// 使用中断或定时器捕获A、B相边沿
int encoder_count = 0;
void encoder_interrupt_handler() {
if (A_rising_edge) {
if (B == HIGH) {
encoder_count--; // 反转
} else {
encoder_count++; // 正转
}
}
// 同理处理B相边沿，实现4倍频
}
这里有个“4倍频”的概念。检测A、B相的上升沿和下降沿，每个周期可以产生4个计数脉冲，分辨率提高4倍。我建议在低速场景下用4倍频，高速时用单倍频——因为中断太频繁会占用CPU。
2.3.2 数据滤波与去抖
编码器信号在电机启停时容易抖动。尤其是PWM调速时，电磁干扰会导致编码器输出毛刺。我常用的处理方法是：
- 硬件滤波：在编码器信号线上加RC低通滤波，截止频率设为电机最大转速对应频率的2倍。
- 软件去抖：连续采样3次，取多数值。或者用“窗口比较器”——只有脉冲宽度超过一定时间才认为是有效信号。
我的教训：有一次，编码器数据突然跳变，导致里程计瞬间“飞”出去几米。查到最后，是编码器线缆和电机动力线绑在一起，电机启动时的大电流耦合了噪声。从此以后，我坚持编码器线单独走线，而且用屏蔽线。
2.3.3 速度计算
从编码器脉冲计算速度，有两种常用方法：
- M法（测频法）：固定时间窗口内计数脉冲数。适合高速场景。
- T法（测周法）：测量两个脉冲之间的时间间隔。适合低速场景。
实际项目中，我通常结合使用：高速用M法，低速用T法，中间有个切换阈值。这样能保证全速度范围内的精度。
// M法计算速度
// 每10ms读取一次编码器计数
int delta_count = current_count - last_count;
float speed = (delta_count / encoder_resolution) * (2 * PI * r) / 0.01; // m/s
// T法计算速度（适用于低速）
// 测量两个脉冲之间的时间间隔 dt
float speed = (2 * PI * r / encoder_resolution) / dt; // m/s
2.4 本章知识体系
为了让大家更直观地理解本章内容的结构，我画了一张图：
这张图把本章的三个核心模块串起来了。运动学模型告诉我们“应该怎么走”，IMU提供高频姿态信息，编码器提供轮子转动的精确数据。三者结合，才能得到可靠的里程计。
最后说一句：学完这章，你可能会觉得“不就是几个公式嘛”。但真正做项目时，你会发现每个参数都有坑。我的建议是：先搭一个简单的测试平台，把里程计数据录下来，画成轨迹图看看。看到轨迹慢慢漂移、扭曲，你才能真正理解这些模型和传感器的脾气。
好了，第二章的内容就到这里。记住，纸上得来终觉浅，绝知此事要躬行。去动手试试吧。
可可精品资料库