激光SLAM轮式里程计标定：从运动模型到参数实测的工程实践

1. 从轮子说起：为什么里程计是激光SLAM的第一道坎

做激光SLAM的人都有一个共识：建图飘不飘，一半看雷达，一半看里程计。很多人拿到一套移动底盘，第一反应是赶紧把雷达装上去跑Cartographer或者LIO-SAM，结果发现建出来的图要么墙是弯的，要么回环对不上，折腾半天怀疑是雷达精度不够。实际上，问题往往出在最底层——轮式里程计的运动模型和标定没做扎实。

这一篇聚焦的就是这个最基础但最容易被跳过的环节：轮式里程计的传感器数据处理。具体来说，我们要搞清楚两件事——第一，两个轮子的编码器读数怎么变成机器人整体的位姿变化（运动模型）；第二，模型里的那些参数（轮半径、轮距、编码器分辨率）怎么通过实测标定出来，而不是照抄厂家给的datasheet。

适合谁看？如果你正在做差速底盘的激光SLAM，用的是ROS/ROS2，手上有编码器但没有高精度外部真值设备，那这篇就是写给你的。如果你用的是全向轮、阿克曼或者麦轮，运动模型会不一样，但标定的思路是相通的，也可以参考。

我自己的经验是：一套底盘如果里程计标定做好了，跑Cartographer的时候回环数量能少一大半，建图边缘的锯齿感明显减轻。反过来，标定没做，你调多少回环参数都是治标不治本。

2. 差速运动模型：两个轮子如何描述整车的位姿

2.1 从编码器脉冲到轮子位移

编码器输出的原始数据是脉冲数（ticks）。要把它变成有物理意义的位移，中间要过两道换算。

第一道是编码器分辨率。假设编码器线数是N线，电机到轮子之间还有减速比i，那么轮子转一圈对应的脉冲数是：

ticks_per_rev = N × i × 4 （如果是四倍频解码）

这里为什么要乘4？因为很多增量式编码器用的是A/B两相正交信号，四倍频解码后每个周期能数出4个边沿，分辨率直接翻四倍。这一步如果搞错了，后面所有位移都会差一个整数倍，而且这种错误很隐蔽——你标定出来的轮半径会莫名其妙偏大或偏小。

第二道是轮半径。轮子转一圈，理论上走过的距离是2πr。所以单个脉冲对应的位移是：

dist_per_tick = 2πr / ticks_per_rev

左轮位移 ΔL = 左轮脉冲增量 × dist_per_tick，右轮位移 ΔR 同理。

2.2 差速运动学：从两轮位移到整车位姿

有了左右轮各自的位移，接下来就是差速运动学的核心。设轮距（左右轮中心之间的距离）为b，那么在极短时间Δt内：

• 整车前进的线位移：Δs = (ΔL + ΔR) / 2
• 整车转过的角度：Δθ = (ΔR - ΔL) / b

这两个公式是整个里程计的地基。它的物理含义很直观：两个轮子都往前走同样的距离，车就直着走；右轮走得多，车就左转；左轮走得多，车就右转。

然后要把局部位移累积到全局位姿上。假设当前位姿是(x, y, θ)，用中点积分法（mid-point integration）更新：

Δs = (ΔL + ΔR) / 2 Δθ = (ΔR - ΔL) / b x += Δs × cos(θ + Δθ/2) y += Δs × sin(θ + Δθ/2) θ += Δθ

注意这里用的是 θ + Δθ/2，也就是用转向过程中的中间角度来投影位移，而不是用起始角度。这个细节在小转角时差别不大，但在原地快速旋转或者大角度转弯时，用起始角度会累积明显误差。我实测过，一个原地转10圈的动作，用起始角度积分和用中点积分，最终位置偏差能差出十几厘米。

2.3 为什么不用欧拉积分而用中点积分

欧拉积分就是直接用 θ 而不是 θ + Δθ/2：

x += Δs × cos(θ) y += Δs × sin(θ) θ += Δθ

当 Δθ 很小的时候，cos(θ) 和 cos(θ + Δθ/2) 几乎一样，两者没区别。但问题是，如果你的控制周期比较长（比如10Hz），机器人又在快速转弯，单周期 Δθ 可能到0.1rad以上，这时候欧拉积分的误差就不可忽略了。中点积分相当于用梯形法近似积分，精度高一阶，计算量几乎没增加，没有理由不用。

提示：如果你用的是ROS的diff_drive_controller或者自己写的odom节点，先确认一下积分方法。很多开源代码默认是欧拉积分，在低速场景够用，但高速转弯时就是误差来源之一。

3. 标定三件套：轮半径、轮距、编码器分辨率

3.1 为什么不能信datasheet

厂家给的轮半径是名义值，比如标称100mm，实际可能是99.2mm或者101.5mm。轮胎有压缩、磨损，加工有公差，这些都会让实际有效半径偏离名义值。轮距同理，标称可能是340mm，实际装配完可能是338mm或者343mm。

编码器分辨率的问题更隐蔽。有些电机自带编码器，减速比是标称的，但实际减速箱的齿隙和滑差会让有效分辨率偏离理论值。如果你直接套用理论值，标定出来的轮半径和轮距会互相补偿，看起来里程计也能跑，但直线走不准、转弯角度也不对。

所以结论很简单：所有参数必须实测标定，datasheet只能作为初始估计值。

3.2 直线标定：把轮半径和分辨率一起标

直线标定的目标是确定 dist_per_tick，也就是每个脉冲对应的实际位移。方法很朴素：让机器人走一段已知长度的直线，记录左右轮各自的脉冲增量，然后反推。

具体操作：

1. 在地面上量出一段直线距离，建议至少3米，越长越好，减少相对误差。用卷尺量，两端做好标记。
2. 把机器人放在起点，手动对准直线方向。可以用地面瓷砖缝或者拉一根线作为参考。
3. 记录起始时刻左右轮的累计脉冲数 L0, R0。
4. 让机器人以较低速度（比如0.2m/s）直线前进，走到终点标记处停下。
5. 记录终止时刻的累计脉冲数 L1, R1。
6. 计算：

ΔL_ticks = L1 - L0 ΔR_ticks = R1 - R0 dist_per_tick_left = D / ΔL_ticks dist_per_tick_right = D / ΔR_ticks

其中D是实际走过的距离。理想情况下左右轮的 dist_per_tick 应该相等，如果差得比较多（超过5%），说明左右轮半径不一致或者编码器分辨率有差异，需要分别标定。

然后反推轮半径：

r_left = dist_per_tick_left × ticks_per_rev / (2π) r_right = dist_per_tick_right × ticks_per_rev / (2π)

这里 ticks_per_rev 是你认为的理论值。如果理论值本身不准，标出来的轮半径就会偏。所以更稳妥的做法是：先假设轮半径是名义值，反推实际的 ticks_per_rev，然后固定 ticks_per_rev，再反推轮半径。不过实际上，我们最终用的是 dist_per_tick 这个乘积，轮半径和分辨率的具体数值只要乘积对就行。

注意：直线标定至少做3次，取平均。每次走的时候尽量让机器人自己走，不要用手推，因为手推的速度不均匀，编码器采样和实际位移的对应关系会变差。

3.3 旋转标定：轮距的实测方法

轮距b的标定靠旋转。让机器人原地转一个已知角度，记录左右轮的脉冲差，反推b。

操作步骤：

1. 在地面上做一个角度参考。最简单的方法是用一张大纸或者地板砖的直角，画一个十字线，或者用手机上的水平仪App辅助。
2. 在机器人顶部贴一个指针（可以用胶带粘一根牙签），对准起始方向。
3. 记录起始脉冲 L0, R0。
4. 控制机器人原地旋转，建议转360度，转得越多角度误差的相对影响越小。转速不要太快，0.5rad/s左右比较合适。
5. 转到指针回到起始方向，记录终止脉冲 L1, R1。
6. 计算：

ΔL = (L1 - L0) × dist_per_tick_left ΔR = (R1 - R0) × dist_per_tick_right b = (ΔR - ΔL) / Δθ

其中 Δθ 是实际转过的角度，转360度就是 2π。

这里有个容易踩的坑：旋转方向要和公式对应。上面的公式假设右轮往前走、左轮往后走（逆时针转）。如果你转的是顺时针，ΔR - ΔL 会变成负的，取绝对值就行。但一定要确认符号约定，否则标出来的轮距是负的，里程计方向全反。

提示：转360度的时候，不要只看指针回到原位就停，因为指针本身有宽度，对准误差可能有几度。更好的方法是转多圈，比如转5圈，然后总角度除以5，这样单圈的对准误差被平均掉了。

3.4 标定结果的交叉验证

标定完三个参数后，一定要做交叉验证，否则你永远不知道标出来的参数是不是互相补偿了。

验证方法一：走一个正方形。让机器人走一个1m×1m的正方形，回到起点。如果标定准确，终点应该和起点重合，角度也应该回到初始方向。实测下来，好的标定能让1m正方形的闭合误差在5cm以内，角度误差在3度以内。

验证方法二：走一个已知半径的圆弧。让机器人以固定角速度转一圈，看实际轨迹半径和理论值是否一致。

验证方法三：用激光雷达做真值对比。如果你有雷达，可以把雷达点云匹配出来的位姿变化和里程计位姿变化做对比，看两者的比例关系。这个方法最直接，但需要雷达本身精度够。

我自己的习惯是：直线标定做5次取平均，旋转标定做3次取平均，然后走正方形验证。如果正方形闭合误差大于10cm，就回去检查是不是哪个参数标错了。

4. 实操中的坑：从编码器噪声到时间同步

4.1 编码器噪声和滤波

编码器读数本身有量化噪声，尤其是低线数编码器。比如一个360线的编码器，四倍频后1440 ticks/rev，轮子周长314mm，那么每个tick对应0.218mm。这个量化误差在低速时特别明显——如果控制周期是10ms，机器人速度是0.1m/s，一个周期只走1mm，也就是不到5个tick，量化误差占比就很大了。

解决方法有两个：一是提高编码器线数，二是对速度做低通滤波。我一般会在odom节点里加一个一阶低通滤波：

v_filtered = α × v_raw + (1 - α) × v_filtered_prev

α取0.3到0.5之间。太小了响应慢，太大了滤波效果不明显。这个滤波只用在速度输出上，位姿积分还是用原始位移，否则会引入额外的相位滞后。

4.2 时间同步：里程计和雷达的时间戳对齐

激光SLAM里，里程计和雷达的时间戳必须对齐，否则运动畸变校正会出错。常见的问题是：里程计节点发布频率是50Hz，雷达是10Hz，雷达帧的时间戳和最近的里程计帧差了几毫秒，如果机器人正在快速转弯，这几毫秒就能让点云匹配偏出去。

解决办法：用ROS的message_filters做时间同步，或者用tf2的插值功能。我一般会在雷达回调里，用tf2查雷达时间戳对应的odom→base_link变换，而不是直接用最新的odom。tf2会自动做线性插值，比直接取最近帧准得多。

注意：tf2插值要求odom的发布频率足够高，至少是雷达频率的3倍以上。如果odom只有10Hz，雷达也是10Hz，插值出来的结果可能还不如不插值。这种情况下要么提高odom频率，要么用IMU做辅助。

4.3 轮子打滑：里程计最大的敌人

轮式里程计有一个先天缺陷：它假设轮子不打滑。但实际地面上，急加速、急转弯、地面有沙粒或者水渍，都会导致打滑。打滑时编码器还在转，但车没动，里程计就会累积一个虚假的位移。

识别打滑的方法：对比左右轮的速度差和IMU的角速度。如果左右轮速度差很大但IMU角速度很小，说明有一个轮子在打滑。或者对比里程计速度和IMU加速度积分出来的速度，偏差大就是打滑。

处理打滑没有完美的方案，只能缓解：一是控制加速度，不要让机器人急起急停；二是用IMU做融合，比如EKF里把里程计和IMU按协方差加权；三是在打滑检测到的时候，临时增大里程计的协方差，让滤波器更信任IMU。

4.4 常见问题速查表

5. 从标定到建图：里程计质量如何影响SLAM效果

5.1 里程计精度对建图的影响链路

里程计误差对SLAM的影响不是线性的，而是会通过几个环节放大。

第一层是局部建图。激光SLAM的前端扫描匹配，通常用里程计做初始位姿猜测。如果里程计误差大，扫描匹配的搜索范围就要开大，计算量增加，而且容易匹配到错误的局部极值。我实测过，里程计角度误差从1度增加到5度，Cartographer的前端匹配耗时增加了将近一倍。

第二层是回环检测。里程计漂移大，回环检测的搜索窗口就要开大，误匹配率上升。更麻烦的是，如果里程计漂移是非线性的（比如打滑导致的突变），回环检测可能完全失效。

第三层是全局优化。里程计作为优化问题里的约束，如果它的协方差设置不合理（比如实际误差大但协方差设得小），优化结果会被带偏。所以标定完之后，还要根据实际误差调整里程计的协方差参数。

5.2 标定后的效果对比

我拿同一套底盘做过对比实验。标定前，用厂家datasheet参数，走10米直线，终点位置偏差大约30cm，角度偏差约5度。标定后，同样走10米，位置偏差降到5cm以内，角度偏差1度以内。

建图效果上，标定前跑Cartographer，一个20m×15m的室内场景，回环数量大约15个，建图边缘有明显的重影。标定后，同样场景，回环数量降到6个左右，边缘清晰，没有重影。

这个提升不是靠调参调出来的，就是老老实实做标定。所以我的建议是：拿到新底盘，第一件事就是标定里程计，不要急着跑SLAM。

5.3 什么时候需要重新标定

标定不是一劳永逸的。以下几种情况需要重新标定：

• 更换轮胎或者轮胎磨损明显
• 拆装过电机或者编码器
• 底盘受到撞击，轮距可能变化
• 负载变化很大，轮胎压缩量改变
• 发现建图效果突然变差，排除其他原因后

我一般每隔三个月或者每次大拆大装后，都会重新做一次直线和旋转标定，花不了半小时，但能省掉很多调试时间。

6. 代码实现：一个可复现的里程计标定与发布节点

6.1 标定数据采集脚本

标定的时候需要记录脉冲数和实际位移，我写了一个简单的Python脚本，通过ROS订阅轮子编码器话题，手动打标记记录起止点。

import rospy from std_msgs.msg import Int32 import json class OdomCalibrator: def __init__(self): rospy.init_node('odom_calibrator') self.left_ticks = 0 self.right_ticks = 0 self.marks = [] rospy.Subscriber('/left_encoder', Int32, self.left_cb) rospy.Subscriber('/right_encoder', Int32, self.right_cb) def left_cb(self, msg): self.left_ticks = msg.data def right_cb(self, msg): self.right_ticks = msg.data def mark(self, label): self.marks.append({ 'label': label, 'left': self.left_ticks, 'right': self.right_ticks }) rospy.loginfo(f"Marked {label}: L={self.left_ticks}, R={self.right_ticks}") def save(self, filename): with open(filename, 'w') as f: json.dump(self.marks, f, indent=2)

用的时候，在终端里手动调用mark('start')和mark('end')，然后save。这个脚本很简单，但比手抄脉冲数靠谱得多。

6.2 里程计发布节点核心逻辑

标定完参数后，里程计节点的核心就是前面说的中点积分。下面是一个简化版的实现：

import math class DifferentialOdom: def __init__(self, dist_per_tick_l, dist_per_tick_r, wheel_base): self.dpt_l = dist_per_tick_l self.dpt_r = dist_per_tick_r self.b = wheel_base self.x = 0.0 self.y = 0.0 self.theta = 0.0 self.prev_l = None self.prev_r = None def update(self, left_ticks, right_ticks): if self.prev_l is None: self.prev_l = left_ticks self.prev_r = right_ticks return self.x, self.y, self.theta d_l = (left_ticks - self.prev_l) * self.dpt_l d_r = (right_ticks - self.prev_r) * self.dpt_r self.prev_l = left_ticks self.prev_r = right_ticks ds = (d_l + d_r) / 2.0 dtheta = (d_r - d_l) / self.b self.x += ds * math.cos(self.theta + dtheta / 2.0) self.y += ds * math.sin(self.theta + dtheta / 2.0) self.theta += dtheta return self.x, self.y, self.theta

这个类可以直接嵌入到ROS节点里，每次收到编码器消息就调用update，然后发布odom消息和tf变换。

6.3 参数配置文件示例

把标定好的参数写进YAML文件，方便管理和切换：

odom_calibration: dist_per_tick_left: 0.000218 dist_per_tick_right: 0.000219 wheel_base: 0.338 ticks_per_rev: 1440 left_radius: 0.0499 right_radius: 0.0501

注意 dist_per_tick 和 radius 是冗余的，实际用的时候只用 dist_per_tick 就行，radius 只是记录一下方便追溯。

提示：如果你用的是ROS2，参数配置方式类似，但节点声明参数的方式不同，用 declare_parameter 和 get_parameter。逻辑是一样的。

7. 进阶话题：从轮式里程计到多传感器融合

7.1 轮式里程计的局限性

轮式里程计有三个绕不过去的局限：一是打滑，二是地面不平导致的轮子悬空，三是长期漂移。这三个问题靠轮式里程计本身解决不了，必须引入其他传感器。

IMU可以提供角速度和加速度，在短时间尺度上比里程计准，尤其是角速度。但IMU有零偏，长时间积分会漂。所以里程计和IMU是互补的：里程计长期稳定但短期噪声大，IMU短期准但长期漂。

激光雷达可以提供环境观测，通过扫描匹配得到位姿变化，这个位姿变化不受轮子打滑影响。但激光匹配在长廊或者空旷环境会退化。

7.2 EKF融合的基本思路

最常用的融合方法是扩展卡尔曼滤波（EKF）。状态量取位姿和速度，观测量取里程计位移和IMU角速度。预测步用运动模型，更新步用观测模型。

关键点是协方差的设置。里程计的协方差要根据标定后的实际误差来设，不能拍脑袋。我一般会做一组实验：让机器人走已知轨迹，记录里程计输出和真值的偏差，然后统计偏差的方差，作为里程计协方差的参考。

IMU的协方差则要根据零偏稳定性来设。静态下采集几分钟IMU数据，看角速度的标准差，这个标准差就是角速度观测噪声的参考值。

7.3 什么时候该上融合，什么时候纯里程计够用

不是所有场景都需要融合。如果你的机器人跑得很慢（小于0.3m/s），地面平整，轮子抓地力好，纯里程计加激光SLAM就够用了。我做过一个室内巡检项目，速度0.2m/s，纯里程计跑Cartographer，建图效果完全满足需求。

但如果你的机器人速度快、地面有坡度、或者经常急转弯，那就必须上融合。尤其是室外场景，地面不平导致轮子偶尔悬空，纯里程计基本没法用。

判断标准很简单：如果你发现建图时经常需要手动干预回环，或者建出来的图在某些区域明显变形，那就是里程计不够用了，该考虑融合了。

8. 我踩过的几个坑和最后的建议

第一个坑是编码器方向搞反。有一次换了一个新电机，编码器A/B相接线反了，结果里程计往前走的时候读数是负的。这个错误很隐蔽，因为如果你只看绝对值，位移是对的，但方向反了。建图的时候表现为机器人往前走，地图往后跑。排查方法很简单：手动转一下轮子，看编码器读数是增还是减，和实际方向对比。

第二个坑是轮距标定的时候转错了方向。前面说过，公式假设逆时针转，结果我顺时针转的，标出来的轮距是负的。当时没注意，跑SLAM的时候机器人原地转圈，地图也跟着转，但方向完全反了。后来在代码里加了一个符号检查，如果轮距是负的就报错。

第三个坑是忽略了轮胎压缩。标定的时候机器人是空载，跑起来的时候装了电池和雷达，重量增加了，轮胎压缩量变大，有效半径变小。结果就是空载标定的参数，满载跑的时候里程计偏大。后来我标定的时候都会把实际负载装上，或者至少用等重的配重模拟。

最后一个建议：标定参数要版本管理。每次标定完，把参数文件、标定日期、标定时的负载情况、标定结果都记录下来。我见过太多人标定完就把参数一改，过两个月发现建图效果变差了，想回退都不知道回退到哪个版本。用Git管理参数文件，每次标定提交一次，备注写清楚，这是最省事的做法。

这个内容后续还可以这样扩展：如果你用的是麦轮或者全向轮，运动模型会变成三个轮子的组合，标定方法也要相应调整，但核心思路——用已知运动反推参数——是一样的。另外，如果你有高精度的外部真值设备（比如动作捕捉系统），标定可以做得更精细，甚至能标定出轮子的滑移率模型。