激光SLAM前端基石：差速轮式里程计运动模型推导、标定与工程实践

1. 从轮子转数到机器人位姿：里程计到底在算什么

很多人第一次接触激光SLAM，注意力全被激光雷达和点云配准吸引走了，觉得里程计就是个"辅助传感器"，随便给个初值就行。我刚开始做移动机器人定位那会儿也是这么想的，结果建出来的地图在长走廊里直接扭成麻花，回环检测怎么调都救不回来。后来才明白，里程计运动模型是整个SLAM前端的地基，地基歪了，后面激光匹配再准也是在错误的基础上修修补补。

里程计的核心任务说起来很简单：根据轮子转了多少圈，推算机器人相对上一时刻移动了多少、转了多少。但"简单"两个字背后藏着一堆工程细节。轮式里程计本质上是一个**航迹推算（Dead Reckoning）**系统，它不依赖外部环境信息，只靠自身运动积分来估计位姿。这意味着两件事：第一，它输出频率高、短时精度好，适合给激光匹配做初值；第二，误差会随时间累积，长时间不修正必然漂移。理解这个"短时可信、长时发散"的特性，是设计整个SLAM系统的前提。

差速驱动是绝大多数室内移动机器人的底盘形式，左右两个驱动轮各带一个编码器。编码器输出的原始数据是脉冲计数，需要经过一系列换算才能变成有物理意义的量。这个换算链条大致是：脉冲数 → 轮子转过的弧度 → 轮子走过的弧长 → 机器人本体的位移和转角。每一步都涉及参数，每个参数都可能标定不准，而标定不准的后果会在积分过程中被放大。

我见过不少团队直接拿厂家给的轮径和轮距参数就用，结果原地转十圈，机器人实际转了九圈半多一点。这种误差在单次动作里看不出来，但SLAM跑个几十米就原形毕露。所以这一篇我想把里程计运动模型从数学推导到代码实现，再到标定实操，完整地捋一遍。不管你是刚入门的学生，还是已经跑通过几个SLAM项目的工程师，这里面的坑和技巧都值得再过一遍。

适合阅读这篇内容的，是已经了解ROS基本概念、跑过至少一个激光SLAM开源方案、但对自己机器人里程计精度心里没底的朋友。如果你连tf树和odom坐标系都还没搞明白，建议先把ROS的坐标变换基础补一补再回来。

2. 差速底盘的运动学推导：从两个轮子到一个位姿

2.1 正运动学：已知轮速求本体速度

差速底盘的运动学模型是整个里程计计算的起点。假设左轮线速度为 $v_L$，右轮线速度为 $v_R$，轮距（左右轮中心之间的距离）为 $L$，那么机器人本体的线速度 $v$ 和角速度 $\omega$ 可以写成：

$$v = \frac{v_L + v_R}{2}$$

$$\omega = \frac{v_R - v_L}{L}$$

这两个公式的物理意义很直观。线速度是左右轮速度的平均值，因为机器人走直线时两轮同速，转弯时两轮速度的平均仍然代表质心的前进速度。角速度则取决于两轮的速度差，速度差越大转得越快，轮距越大同样的速度差产生的角速度越小。

这里有个容易被忽略的点：轮距 $L$ 到底量的是哪里到哪里的距离。严格来说，差速运动学假设的是两个轮子与地面接触点的中心距，而不是轮子外缘的距离，也不是轮毂安装面的距离。实际标定时如果量错了基准，角速度就会系统性偏大或偏小。我一般建议先用卷尺量一个粗略值，然后通过标定实验来修正，因为轮胎的形变、接触点的偏移都会影响有效轮距。

另一个细节是，上面的公式假设轮子纯滚动无侧滑。实际机器人转弯时，尤其是原地旋转时，轮胎与地面之间必然存在侧向滑动。这个滑动量取决于轮胎材质、地面摩擦系数、机器人重量分布。在光滑瓷砖地面上，原地旋转的里程计误差可能比在地毯上大好几倍。这是模型本身的局限，不是标定能完全消除的，只能通过后续的激光匹配来补偿。

2.2 离散积分：把连续运动拆成小步

实际系统里我们拿到的不是连续的轮速，而是离散时间间隔内的编码器增量。假设在时间 $\Delta t$ 内，左轮走了 $\Delta s_L$，右轮走了 $\Delta s_R$，那么：

$$\Delta s = \frac{\Delta s_L + \Delta s_R}{2}$$

$$\Delta \theta = \frac{\Delta s_R - \Delta s_L}{L}$$

这里的 $\Delta s$ 是机器人沿自身前进方向的位移，$\Delta \theta$ 是绕自身中心的转角。接下来要把这个局部位移累加到全局位姿上，就有讲究了。常见的积分方法有三种：

欧拉积分最简单，就是把 $\Delta s$ 直接投影到上一时刻的朝向方向上。当 $\Delta \theta$ 很小时误差可以忽略，但如果机器人快速旋转，比如一秒转90度，欧拉积分会产生明显的轨迹畸变。中点积分用 $\theta + \Delta\theta/2$ 作为投影方向，相当于用平均朝向来近似，精度提升明显，计算量增加很少，是我最常用的方案。

精确圆弧积分则把机器人的运动看作一段圆弧，当 $\Delta\theta$ 不为零时，位移的解析解是：

$$\Delta x = \frac{\Delta s}{\Delta \theta} \left[ \sin(\theta + \Delta\theta) - \sin\theta \right]$$

$$\Delta y = \frac{\Delta s}{\Delta \theta} \left[ -\cos(\theta + \Delta\theta) + \cos\theta \right]$$

当 $\Delta\theta$ 趋近于零时，这个公式会出现除零问题，需要退化为直线运动处理。实际代码里通常加一个判断：如果 $|\Delta\theta|$ 小于某个阈值（比如 $10^{-4}$），就用直线公式，否则用圆弧公式。

2.3 为什么积分频率不能太低

有人会问，既然有精确圆弧积分，是不是积分频率低一点也没关系？答案是否定的。积分频率低意味着每次积分的 $\Delta\theta$ 变大，虽然圆弧积分在数学上更精确，但它假设的是机器人在这个时间段内做匀速圆周运动。实际机器人的运动是任意的，可能先加速再减速，可能中途改变转向。时间段越长，这个"匀速圆周"假设就越不成立。

我实测过，在同样的数据下，用10Hz积分和用50Hz积分，跑完一条50米的复杂路径，终点位置差能有十几厘米。所以只要编码器数据能拿到，积分频率越高越好。一般轮式里程计的原始数据在50到100Hz，我建议至少保持50Hz的积分频率，不要为了省算力去降频。

还有一个实践中的坑：时间戳对齐。编码器数据、激光数据、IMU数据各有各的时间戳，如果积分时用的时间间隔和实际数据间隔对不上，速度估计就会出错。ROS里通常用ros::Time来记录，但要注意仿真时间和真实时间的区别，以及不同传感器驱动的时间同步问题。我习惯在里程计节点里维护一个上一次积分的时间戳，每次收到新数据先算实际时间差，而不是假设固定的周期。

3. 编码器数据到轮子位移：那些标定手册不会告诉你的换算细节

3.1 脉冲计数、减速比与轮径的三角关系

编码器给出的原始数据是脉冲数，要变成轮子走过的距离，需要经过三层换算。第一层是编码器线数，也就是轮子转一圈编码器输出多少个脉冲。第二层是减速比，电机经过减速器后转速降低、扭矩放大，编码器通常安装在电机端，所以轮子转一圈对应的编码器脉冲数是线数乘以减速比。第三层是轮径，轮子转一圈走过的距离是 $2\pi r$。

所以单个脉冲对应的轮子位移是：

$$\Delta s_{wheel} = \frac{2\pi r}{N \cdot i}$$

其中 $r$ 是轮子半径，$N$ 是编码器线数，$i$ 是减速比。这个公式看起来简单，但每个参数都有坑。

编码器线数一般厂家会标，但要注意是单相计数还是四倍频计数。增量式编码器通常有A、B两相，如果只对A相计数，一圈的脉冲数就是线数；如果对A、B相的上升沿和下降沿都计数，就是四倍频，脉冲数变成线数的四倍。很多驱动器默认输出的是四倍频后的计数值，如果你按线数去算，位移就会差四倍。这个错误我在早期项目中犯过，机器人走一米，里程计显示走了四米，排查了半天才发现是倍频的问题。

减速比也有讲究。标称的减速比可能是精确值，比如1:30，也可能是近似值，比如1:29.7。如果是行星减速器，标称值通常比较准；如果是蜗轮蜗杆，实际减速比可能和标称有偏差。最可靠的办法是看电机手册里的精确参数，或者自己做实验测。

轮径是最容易出问题的。标称的轮径是空载、标准气压下的直径，但实际使用时轮胎会被压扁，有效半径变小。充气轮胎的形变量可能达到几毫米，对于半径50mm的小轮子来说，这就是百分之几的误差。橡胶轮在负载下也会变形。所以轮径必须通过实际标定来确定，不能直接用厂家给的数值。

3.2 轮距标定：原地旋转法

轮距的标定比轮径更麻烦，因为它不能通过直线运动来测。我常用的是原地旋转法：让机器人原地旋转整整数圈，比如十圈，然后看里程计报告的转角是多少。如果里程计报告转了十圈，说明轮距标定准确；如果报告转了九圈半，说明轮距设大了。

具体操作步骤是这样的。首先把机器人放在一个平整的地面上，在机器人外壳上贴一个标记，正对前方。然后通过程序控制机器人原地旋转，旋转过程中记录里程计累积的转角。旋转十圈后，机器人应该回到初始朝向，用标记确认。如果里程计报告的转角是 $\theta_{odom}$，实际转了十圈即 $20\pi$ 弧度，那么修正后的轮距是：

$$L_{corrected} = L_{original} \cdot \frac{\theta_{odom}}{20\pi}$$

这个方法的原理是：角速度 $\omega = (v_R - v_L)/L$，在轮速差固定的情况下，$L$ 越大角速度越小，累积转角越小。所以如果里程计转角偏小，说明 $L$ 设大了，需要按比例缩小。

实际操作时有几个注意点。第一，旋转速度不要太快，避免轮胎打滑，我一般用0.3到0.5 rad/s。第二，旋转圈数要够多，十圈起步，圈数越多相对误差越小。第三，正转和反转各做一次，取平均值，可以抵消一些系统偏差。第四，地面要干净平整，瓷砖地面比地毯好，因为地毯的侧向阻力大，打滑更严重。

3.3 轮径标定：直线行走法

轮径标定相对直接：让机器人直线走一段已知距离，比较里程计报告的位移和实际位移。我通常在地面上量出十米的直线，让机器人沿着直线走，记录里程计累积的位移。如果里程计报告走了9.8米，实际走了10米，说明轮径设小了，需要按比例放大：

$$r_{corrected} = r_{original} \cdot \frac{10}{9.8}$$

直线行走法的关键是保证机器人真的走直线。如果左右轮的实际直径不一致，机器人会跑偏，这时候需要分别标定左右轮的半径。方法是让机器人走一段直线，测量起点和终点的横向偏移，然后根据偏移量反推左右轮的半径差。

左右轮径不一致是常见问题，尤其是自己组装底盘的时候。两个轮子的橡胶磨损程度不同、充气压力不同、加工公差不同，都会导致有效半径有差异。这个差异在直线行走时表现为跑偏，在旋转时表现为旋转中心偏移。如果差异不大，可以通过标定左右轮各自的半径系数来补偿；如果差异很大，建议直接换轮子。

3.4 系统误差与随机误差的区分

标定能解决的是系统误差，也就是每次都在同一方向上偏的误差。但里程计还有随机误差，比如轮胎打滑、编码器丢脉冲、地面不平导致的颠簸。这些误差没有固定规律，标定消除不了。

区分两者的方法是做重复性实验。让机器人重复走同一条路径十次，记录每次的终点位姿。如果十次的终点都偏向同一侧，说明有系统误差，可以通过标定改善；如果十次的终点随机散布在真实值周围，说明主要是随机误差，标定帮不上忙，只能靠激光匹配或融合其他传感器来抑制。

我一般建议先做系统误差标定，把确定性偏差压到最小，然后再评估随机误差的量级。如果随机误差太大，比如走十米就偏半米，那说明底盘机械结构或者编码器质量有问题，得从硬件上找原因，而不是继续调参数。

4. 标定实操：从数据采集到参数验证的完整链路

4.1 数据采集：录包与回放

标定过程中最忌讳的就是边跑边调。正确的做法是先录包，把编码器原始数据、里程计输出、激光数据都录下来，然后离线回放分析。这样每次调整参数都可以用同一份数据验证，排除了实时运行中的不确定性。

录包时要注意几点。第一，确保录包时机器人运动充分，包含直线、转弯、原地旋转等多种运动模式，这样标定出来的参数才具有普适性。第二，录包时间要够长，我一般至少录五分钟的有效运动数据。第三，如果可能的话，同时录一段高精度的外部参考数据，比如动作捕捉系统的输出，或者全站仪的测量结果，用来做真值对比。没有外部真值的话，就用卷尺量的距离和量角器量的角度作为近似真值。

回放分析时，我习惯写一个Python脚本，读取bag文件里的编码器数据和里程计数据，重新计算里程计输出，然后和录包时的里程计输出对比。这样可以快速验证参数修改的效果，不用每次都重新跑机器人。

4.2 参数辨识：最小二乘的思路

如果不想手动试凑参数，可以用最小二乘来辨识。基本思路是：把轮径和轮距作为待辨识参数，用实际观测到的机器人位姿变化作为真值，构建优化问题。

假设我们有一组观测数据，每个时刻 $k$ 有编码器增量 $\Delta s_{L,k}$、$\Delta s_{R,k}$，以及对应的机器人真实位移 $\Delta x_k$、$\Delta y_k$、$\Delta\theta_k$。根据运动学模型，里程计预测的位移是参数的函数。最小二乘就是找一组参数，使得预测位移和真实位移的误差平方和最小。

实际操作中，真值往往不好获取。一个替代方案是用激光匹配的结果作为真值。激光SLAM在短距离内的匹配精度通常比里程计高，可以用它来标定里程计。但要注意，激光匹配本身也可能有偏差，所以这个方法适合在环境特征丰富的场景下使用。

另一个思路是只用旋转数据标定轮距，只用直线数据标定轮径，把问题解耦。这样每个子问题都是单参数优化，简单可靠。我通常先用这个方法得到一个不错的初值，然后再用最小二乘做联合优化。

4.3 验证：闭环测试与长距离测试

标定完参数后必须验证。我常用的验证方法是闭环测试：让机器人走一个闭合路径，比如矩形或者8字形，回到起点。如果里程计报告的终点和起点重合，说明标定基本准确；如果终点偏离起点，偏离量就是累积误差的体现。

闭环测试要注意，机器人实际回到起点和里程计报告回到起点是两回事。我们关心的是里程计报告的终点和实际终点的偏差。实际操作时，在地面上标记起点位置和朝向，机器人走完闭环后，测量实际终点和标记起点的偏差，同时记录里程计报告的终点位姿，两者之差就是里程计的累积误差。

长距离测试则是让机器人走几十米甚至上百米，观察误差增长的趋势。如果误差随距离线性增长，说明主要是尺度误差，标定还有优化空间；如果误差随距离平方增长，说明主要是角度误差累积导致的，需要重点检查轮距和陀螺仪的融合。

我一般要求标定后的里程计在走十米直线后，横向偏差不超过十厘米，角度偏差不超过五度。这个指标对于室内SLAM来说基本够用，剩下的误差交给激光匹配来修正。

4.4 标定频率与维护

标定不是一劳永逸的。轮胎会磨损，气压会变化，机械结构会松动，这些都会导致标定参数漂移。我建议每次重要实验前都快速验证一下里程计精度，如果发现明显变差就重新标定。

对于长期运行的机器人，可以设计一个自动标定流程：机器人定期走一段预设的标定路径，系统自动计算当前参数和标定参数的偏差，如果超过阈值就报警或者自动更新参数。这个思路在工业AGV里比较常见，因为AGV的运行路径固定，标定数据容易获取。

还有一个容易被忽略的点：不同负载下里程计参数可能不同。机器人装上货物后，轮胎形变增大，有效轮径变小。如果负载变化大，可能需要针对不同负载分别标定，或者用称重传感器来自动补偿。

5. 里程计与激光SLAM的配合：初值、融合与失效处理

5.1 里程计作为激光匹配的初值

激光SLAM的前端通常用扫描匹配来估计相邻帧之间的位姿变化。扫描匹配是一个非线性优化问题，需要一个初始猜测。如果初值离真实值太远，优化可能陷入局部最优，导致匹配失败。

里程计的作用就是提供这个初值。由于里程计在短时间内的精度不错，用它作为初值可以大大提高扫描匹配的成功率和速度。我实测过，在同样的激光数据下，用里程计做初值的匹配耗时只有不用初值的三分之一左右，而且匹配失败率大幅降低。

但要注意，里程计的初值质量直接影响匹配结果。如果里程计标定不准，初值偏差大，扫描匹配可能被带偏。所以里程计标定是激光SLAM能跑好的前提条件之一。

5.2 松耦合与紧耦合的选择

里程计和激光的融合方式分松耦合和紧耦合。松耦合是把里程计的输出作为一个独立的位姿估计，和激光匹配的结果做加权平均或者用滤波器融合。紧耦合则是把里程计的测量直接放进扫描匹配的优化目标里，一起优化。

松耦合实现简单，模块化好，里程计和激光算法可以独立开发和调试。紧耦合精度更高，但实现复杂，需要修改激光匹配的优化框架。对于大多数室内SLAM应用，松耦合已经够用。我一般先用松耦合跑通，如果精度不够再考虑紧耦合。

松耦合的融合策略也有讲究。常见的是用扩展卡尔曼滤波，把里程计作为预测，激光匹配作为观测。但EKF的线性化误差在转角大时比较明显。另一种策略是用位姿图优化，把里程计约束和激光匹配约束都作为边，一起优化。位姿图的方法在开源SLAM里更常见，比如Cartographer和LIO-SAM都是基于图优化的。

5.3 里程计失效的检测与处理

里程计不是永远可靠的。轮胎打滑、编码器故障、底盘被抬起，都会导致里程计输出异常。如果SLAM系统盲目信任里程计，可能会在里程计失效时崩溃。

检测里程计失效的方法有几种。一是检查编码器数据的合理性，比如轮速是否超过物理极限，左右轮速差是否异常大。二是对比里程计预测的位姿变化和激光匹配估计的位姿变化，如果两者差异持续超过阈值，说明至少有一个出了问题。三是用IMU做交叉验证，IMU的角速度和里程计的角速度应该大致一致。

检测到失效后，处理策略取决于失效的严重程度。如果是短暂打滑，可以降低里程计的权重，等恢复正常后再恢复权重。如果是持续失效，比如编码器坏了，就需要切换到纯激光或者激光加IMU的模式，同时报警提示维护。

我在实际项目中遇到过一种情况：机器人在光滑地面上急转弯时轮胎打滑，里程计报告的转角比实际小很多，导致激光匹配的初值偏差大，匹配失败。后来加了一个打滑检测，当轮速差变化率超过阈值时，临时降低里程计权重，问题就解决了。

5.4 时间同步与坐标变换

里程计和激光的数据必须时间对齐才能融合。如果时间戳有偏差，融合结果就会出错。ROS里通常用message_filters来做时间同步，但要注意不同传感器的时间戳来源可能不同，有的是驱动打的时间戳，有的是ROS收到消息的时间。

坐标变换也是容易出错的地方。里程计输出的是odom坐标系下的位姿，激光数据在laser坐标系下，需要经过base_link到laser的外参变换。如果外参标定不准，激光匹配的位姿和里程计的位姿就不在同一个坐标系下，融合会出问题。

我习惯在系统启动时检查tf树是否完整，odom到base_link的变换是否由里程计节点发布，base_link到laser的变换是否由静态tf发布。这些检查看起来简单，但能避免很多低级错误。

6. 那些年我在里程计上踩过的坑

6.1 编码器计数溢出与方向判断

增量式编码器的计数值是有限位数的，比如16位编码器计数范围是0到65535，超过就溢出回零。如果机器人持续朝一个方向运动，计数值会溢出，如果不处理，里程计就会突然跳变。

处理溢出的方法是记录上一次的计数值，每次计算增量时判断是否发生了溢出。如果当前值比上一次小很多，说明发生了正向溢出，增量应该是当前值加上最大值再减去上一次的值。反之则是反向溢出。

方向判断也有坑。编码器的A、B两相相位差90度，通过判断哪一相先变化来确定方向。如果接线接反了，方向就会反。这个错误在调试时表现为机器人往前走，里程计显示往后退。排查方法是手动转动轮子，看计数值是增加还是减少，和预期方向对比。

6.2 轮子悬空与打滑的识别

调试时经常需要把机器人架起来让轮子空转，这时候里程计会报告机器人移动了，但实际上机器人原地没动。如果这时候跑SLAM，地图就会乱掉。

识别轮子悬空的方法是比较里程计的速度和IMU的加速度。如果里程计报告有速度，但IMU没有检测到相应的加速度，说明轮子在空转。另一个方法是用电流检测，空转时电机电流通常比负载时小。

打滑的识别更复杂一些。直线打滑时，轮速突然增大但机器人实际速度没变，可以通过轮速变化率来检测。转弯打滑时，左右轮速差和IMU的角速度不匹配，可以用这个来检测。

6.3 多传感器时间戳不一致的排查

时间戳问题是最难排查的，因为它的表现是"偶尔出错"，没有固定规律。我遇到过一次，里程计和激光融合后，机器人在某些时刻位姿突然跳变，但单独看里程计和激光的数据都正常。后来发现是激光驱动的时间戳用的是ROS收到消息的时间，而里程计用的是编码器采集的时间，两者在系统负载高时会差几十毫秒。

解决方法是统一时间戳来源。要么都用驱动采集的时间，要么都用ROS收到的时间，不能混用。如果驱动不支持硬件时间戳，至少要在驱动层做一次时间对齐，把采集时间传递上来。

6.4 参数单位与量纲的隐形错误

单位错误是新手最容易犯的。轮径用毫米还是米，角度用弧度还是度，速度用米每秒还是毫米每秒，这些如果不统一，结果会差几个数量级。

我建议在代码里统一用国际单位制：长度用米，角度用弧度，时间用秒。所有输入数据在进入计算前先转换成国际单位，输出时再按需要转换。这样虽然多几步转换，但能避免很多单位混乱的问题。

还有一个隐形的坑是角度归一化。角度累积后可能超过 $2\pi$ 或者小于 $-2\pi$，如果不归一化，在计算三角函数时虽然结果正确，但在比较角度差或者做插值时就会出错。我习惯在每次更新位姿后把角度归一化到 $[-\pi, \pi]$ 区间。

7. 从标定台到真实场景：参数迁移的经验

标定台上标定好的参数，放到真实场景里可能就不准了。标定台通常是平整的硬地面，真实场景可能是地毯、斜坡、门槛。地面材质不同，轮胎的形变和打滑特性就不同，有效轮径和轮距都会变化。

我的经验是，标定要尽量在真实场景的代表性地面上做。如果机器人要在多种地面上运行，就在每种地面上分别标定，然后取一个折中值，或者根据场景切换参数。有些高端系统会用IMU和视觉来在线估计地面特性，自动调整里程计参数，但这超出了大多数项目的需求。

温度也会影响参数。橡胶轮胎在低温下变硬，有效轮径会略微增大；电机在高温下效率下降，同样的PWM占空比对应的转速会降低。如果机器人要在宽温度范围内工作，这些因素需要考虑。室内机器人一般温度变化不大，可以忽略。

负载变化的影响前面提过，这里再强调一下。如果机器人负载变化大，比如搬运机器人空载和满载，建议做两组标定参数，根据负载传感器切换。如果没有负载传感器，可以用电机电流来估计负载，进而调整参数。

最后说一个心态问题。里程计标定是一个"够用就好"的事情，不要追求完美。室内SLAM里，里程计的主要作用是给激光匹配提供初值，只要初值偏差在激光匹配的收敛范围内就行。花太多时间把里程计标到极致，不如把精力放在激光匹配和回环检测上。我一般把里程计误差控制在百分之几以内就收手，剩下的交给后端优化。

实际跑系统时，我习惯在启动后先让机器人走一小段，观察里程计和激光匹配的偏差，如果偏差在合理范围内就直接开始任务，如果偏差大就停下来检查。这个习惯帮我避免了很多次"带病运行"导致的建图失败。