1. 两轮差速底盘的运动本质——为什么机器人总是在绕着一个看不见的圆心转先说一个我这两年反复遇到的现象很多人一上来就翻运动学公式把v (v_l v_r)/2和ω (v_r - v_l)/L背得滚瓜烂熟但一到实际调车就懵了尤其当机器人走的轨迹不是预期圆弧时根本不知道是模型错了还是参数错了。两轮差速机器人其实特别有意思它没有任何转向机构完全靠左右两个驱动轮的速度差来改变方向。所以它的运动学模型解决的不是“轮子转多快”而是“两个轮子的速度差到底能让整个机器人怎么动”这个问题。这个模型用处极广。你做里程计推算需要它你发cmd_vel控制指令需要它你在 ROS 里做 navigation、gmapping、cartographer最后还是绕不开它。甚至你只是想在单片机上写一个循迹小车也必须先把正逆运动学这层纸捅破。1.1 差速底盘能完成的三种基本运动把左右轮速度分别记作v_l和v_r差速底盘其实只做三件事当v_l v_r时机器人沿直线前进或后退这是最容易理解的情况当v_l ≠ v_r时机器人走一段圆弧速度差越大圆弧越“急”当v_l -v_r时机器人原地旋转有点像坦克的“中心转向”。严格说“原地旋转”也只是圆弧运动的一个极限特例。很多新手会把直线、圆弧、原地旋转当成三种割裂的运动其实它们统一在同一个数学模型下面只取决于v和ω两个量的具体取值。这是我觉得最重要的一点差速底盘的运动学本质上是“把两个轮子的速度映射成机器人本体的线速度和角速度”而不是单独讨论某一个轮子。1.2 瞬时旋转中心那个看不见的圆心如果你在地面上俯视一辆差速机器人任意时刻它都在绕着一个看不见的点做圆周运动这个点叫瞬时旋转中心也就是通常说的 ICR。想象一下机器人的两个驱动轮在地面上滚动每个轮子速度方向都是沿轮子前进方向。两个轮子速度方向的延长线交到一点那个交点就是瞬心。机器人在这个瞬时的运动可以等价成绕着这个点转一个微小角度。当左右轮速度相等时瞬心在无穷远处所以机器人看起来是在走直线。当左右轮速度方向相反时瞬心落在两个轮子连线的中点上也就是机器人底盘中心点于是就有了原地旋转。设左右驱动轮之间的中心距为L两轮速度分别为v_l和v_r机器人本体线速度为v、角速度为ω瞬心到机器人中心的距离也就是转弯半径R有关系式v ω × R这个式子虽然简单但它是后面所有推导的根基。你只要记住“任意时刻机器人都在绕瞬心转”这件事推导出正逆运动学公式就是十几分钟的事。1.3 轮距 L 是参数出错率最高的地方我带过不少做毕业设计的学生也看过网上大量开源小车代码发现几乎所有“公式背对了但轨迹不对”的案例都出在L这个参数上。L在运动学公式里指的是左右驱动轮接地中心之间的距离不是底盘宽度更不是轮胎外侧宽度。很多人直接用铝合金底盘上两个电机安装孔的孔距或者拿钢尺量两个轮胎外侧边缘的距离然后把L填进公式。这两个做法都会引入误差。前者忽略了安装公差后者把轮宽也算进去了。比较稳的测量方法有两种一是拆下轮子用游标卡尺量左右驱动轴轴端面之间的中心距二是装好轮子后在桌面上压实轮胎用铅垂线把轮胎接地中心投影到地面再量两个投影点的距离。实际标定中更推荐后面会讲到的“原地旋转标定法”因为理论测量永远代替不了真实运转时的等效参数尤其是充气轮胎在负载下会有形变胎压不同等效轮距也会微变。这个坑我后文会展开。2. 正逆运动学推导从轮速到机器人速度再反推回去有了 “绕瞬心转动” 这个物理图景推导就水到渠成了。这里先把符号约定写清楚r驱动轮半径ω_l、ω_r左右轮的转动角速度单位 rad/sv_l rω_lv_r rω_r左右轮的线速度L左右轮中心距规定机器人逆时针旋转时角速度ω为正顺时针为负。这个方向约定特别重要。你写代码的时候如果左右轮定义反了那cmd_vel里发一个正向角速度机器人会往反方向转查半天查不到原因。2.1 正运动学已知左右轮速求机器人速度和角速度机器人本体的线速度取左右两轮速度的平均值这个结论在物理上很直观因为两轮中点就是底盘几何中心v (v_l v_r) / 2角速度的推导稍微绕一下。假设机器人绕瞬心转动瞬时角速度为ω瞬心到左轮的距离为R - L/2到右轮的距离为R L/2这里设右轮在圆弧外侧。根据圆周运动线速度和角速度的关系v_l ω × (R - L/2) v_r ω × (R L/2)两式相减v_r - v_l ω × L于是ω (v_r - v_l) / L这就是正运动学公式。你只需要编码器算出左右轮的速度就能得到机器人当前本体的线速度和角速度。这里要注意ω的正负完全由v_r - v_l决定。如果右轮比左轮快差值为正机器人逆时针偏转反过来则是顺时针。实际装车时如果电机线序接反公式的符号就会反过来所以调试第一步永远是确认左右轮方向和速度方向定义一致。2.2 逆运动学已知目标 v 和 ω求左右轮速控制机器人的时候我们通常希望它“以线速度v前进同时以角速度ω转弯”这时候要去反解左右轮速。其实就是一个二元一次方程组的事从v (v_l v_r) / 2可得v_l v_r 2v从ω (v_r - v_l) / L可得v_r - v_l ωL两式联立解得v_r v (ωL) / 2 v_l v - (ωL) / 2这个结果非常好理解在一个基础线速度v上给两个轮子叠加一对大小相等、方向相反的速度分量差速越大转向越明显。我把这个公式打印出来贴在工位上。每次写控制代码要发送左右轮速度时我不会直接记公式而是先回忆v和ω的含义然后现场解一遍。因为项目多了之后不同代码里的符号定义会换来换去死记硬背容易串。2.3 几个必须记住的边界情况正逆运动学公式虽然简单但有几个边界情况值得单独拎出来说因为它们直接对应着日常控制里最常见的报错现象。直线运动当ω 0时v_l v_r。如果控制指令里希望机器人走直线但两个轮子有静态差机器人就会慢慢画弧。这是所有差速小车都会遇到的“跑不直”问题。不是运动学模型错了而是两个电机的空载转速、负载响应、轮子实际半径不可能完全一致。解决办法一般是加速度闭环或者做直行标定给某一侧加一个微小比例补偿。原地旋转当v 0时v_l -v_r。此时两个轮子速度相反机器人绕底盘中心原地转。注意原地旋转时左右轮线速度互为相反数如果电机只支持正向 PWM也就是俗称的“单向驱动”这个模型就无法直接执行需要使用 H 桥来实现双方向控制。从 v 和 ω 求转弯半径R v / ω当ω很小时转弯半径很大近似直线当ω很大而v很小时转弯半径很小近似原地转。控制程序里我会做一层保护当ω的绝对值小于某个阈值时直接把它当 0 处理避免因为角速度噪声导致半径计算溢出。还有一个容易忽略的点逆运动学算出来的左右轮速很可能超出电机能提供的最大速度范围。比如你给v一个最大值又给ω一个很大值那某个轮子算出来的速度可能超过电机极限。写控制模块时一定要做饱和处理并且最好保持比例缩放否则机器人实际轨迹会和指令完全不符。3. 把公式变成里程计离散化、航位推算与角度更新顺序前面推导的都是连续运动学公式。但单片机、ROS 控制周期都是离散的所以实际代码里做的其实是“在每个控制周期内把机器人运动近似成一段短时间内的匀速圆弧运动”然后不断累加这就是航位推算。3.1 连续公式不能直接落地如果你尝试直接把x x v·cosθ·Δt这样的式子写成代码会发现一个隐含问题角度θ在这段时间内也在变化到底应该用积分前的角度还是积分后的角度更严谨的做法是认为在一个极短的采样周期Δt内机器人做的是匀速圆弧运动然后对这个圆弧做积分。这样位置增量可以表达为Δs v × Δt Δθ ω × Δt如果采用“中间角度”做近似位置更新会更稳一些x Δs × cos(θ Δθ / 2) y Δs × sin(θ Δθ / 2) θ Δθ这本质上是把每个周期内的轨迹当成一小段圆弧而不是直线精度比直接使用上个周期的角度要高尤其在高角速度情况下改善明显。3.2 增量式航位推算公式在真实代码里我们不一定直接用v和ω而是可以直接用左右轮在采样周期内走过的弧长增量Δs_l和Δs_r来更新里程计。这样做的好处是绕过了速度估计直接用编码器计数。设每个控制周期Δt内左轮编码器增量换算成轮子前进距离为Δs_l右轮为Δs_r则Δs (Δs_l Δs_r) / 2 Δθ (Δs_r - Δs_l) / L然后按照上面的中角度法更新位姿。下面是一个我常用的 Python 伪代码骨架import math class TwoWheelOdometry: def __init__(self, wheel_radius, wheel_track, ticks_per_rev): self.r wheel_radius self.L wheel_track self.ticks_per_rev ticks_per_rev self.x 0.0 self.y 0.0 self.theta 0.0 def tick_to_distance(self, ticks): # 编码器一般有减速比和倍频这里按实际换算 distance ticks / self.ticks_per_rev * 2.0 * math.pi * self.r return distance def update(self, left_ticks, right_ticks): ds_l self.tick_to_distance(left_ticks) ds_r self.tick_to_distance(right_ticks) ds (ds_l ds_r) / 2.0 dtheta (ds_r - ds_l) / self.L 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这段代码的关键点是把编码器换算成距离、把距离增量换算成位姿增量全程没有出现“速度”这个中间量反而更抗噪声。3.3 我踩过的一个坑角度更新顺序有一段时间我做室内回环测试发现机器人绕一圈回来之后x和y的闭合误差非常大但里程计累计的轮子总距离却和实际路径长度差不多。后来逐行排查发现问题出在角度更新顺序上。我当时写了这样的代码self.theta dtheta self.x ds * math.cos(self.theta) self.y ds * math.sin(self.theta)先更新theta再用更新后的角度算x、y。这样会导致位置使用后半段方向而不是整段平均方向。在dtheta比较小的时候这套写法误差不大但在连续快速转向、或者原地旋转时位置误差会被明显放大。改成“先算位置增量最后更新角度”的顺序或者用中间角度theta dtheta/2误差会明显下降。你可以在仿真里对比一下纯直线前进时两者没有差别但在频繁转角时角度顺序会让闭环路径产生肉眼可见的漂移。我的建议是在代码里把位置更新和角度更新分成明确的先后两步并加上注释。这种细节问题不写出单元测试很难一次就发现。4. 标定轮径、轮距和系统延时参数比公式更影响精度公式本身没有争议但为什么同一个底盘别人跑出来的里程计精度很高你跑出来却每次回环偏差半米多我几乎可以断定问题出在参数标定上。4.1 轮径标定跑直线量距离轮径r是一个看似简单、实际上很容易标偏的参数。买来的轮子标注直径 65mm但装上电机轴后轮胎受重力压迫会有形变有效滚动半径往往比标称值略小。最简单的标定方法让机器人以一个固定速度沿直线走一段较长的距离比如 5 米记录期间左右轮编码器的累计脉冲数用激光测距仪或卷尺量真实位移D_real拿编码器累计脉冲数反推实际轮径。假设你初始标称半径为r_nominal编码器累计距离算出来是D_estimate那修正后的半径r_real r_nominal × (D_real / D_estimate)多跑几个来回取平均值可以减少地面摩擦不均带来的随机误差。记住左右轮要分别标定同一轴上的电机减速比、轮胎外径差异都可能导致直行跑偏。4.2 轮距标定原地旋转量角度轮距L对航向角计算的影响比轮径还大。如果L偏大里程计的角度变化就会偏小机器人实际转了 360 度模型可能只算出来 350 度。原地旋转标定法很好操作让机器人原地旋转一定圈数推荐 5 到 10 圈在车头或车尾贴一根激光笔把旋转角度投影到墙上通过左右轮累计弧长差计算实际等效轮距。假设旋转过程中左轮累计走过弧长S_l右轮累计走过弧长S_r实际累计转过的角度为θ_real那么等效轮距L_real (S_r - S_l) / θ_real如果你希望更精确可以在底盘上固定一个陀螺仪或电子罗盘把实际角度测出来。注意要在光滑、轮子不打滑的场地做而且要均匀加速、稳定旋转避免启停瞬间打滑。我做过一次对比实验用游标卡尺量出来的理论轮距是 286mm标定出来的等效轮距是 279mm差了 2.4%。这个差异足以让 10 圈旋转后的航向误差累积到接近 9 度而这 9 度在后面的位置积分里会被放大成几十厘米的横向误差。4.3 编码器分辨率和采样周期除了轮径和轮距编码器分辨率直接决定里程计量的“颗粒度”。如果你用的电机编码器只有 12 线再经过减速比 30那每圈脉冲数大概 360 个听起来还好但每个脉冲对应的轮子弧长可能已经超过 0.5mm。在低速运行时一个控制周期里可能一个脉冲都读不到导致反正运动学解算出来的速度出现跳变。解决办法有两个方向提高硬件分辨率换更高线数编码器或者利用编码器正交解码“四倍频”在软件里做“低速补零”也就是编码器读数为零时不要把速度直接置 0而是沿用最近几次的平均速度或者使用速度平滑滤波。采样周期也不宜过大。常见小车控制周期在 10ms 到 50ms 之间。如果周期太长圆弧近似假设就不成立了尤其在高速旋转时每个周期内角度变化很大中角度法的优势也会随之消失。我个人的底线是一个控制周期内转角不超过 5 度否则就要考虑提高控制频率。在需要快速转向的应用里我会把控制周期压到 10ms 以内。5. 把运动学模型写成控制代码从目标速度到 PWM运动学模型不只是用来算里程计它还有一半工作是逆运动学把上层下发的一个目标速度(v, ω)换算成左右轮速然后交给电机控制器执行。5.1 一个完整的控制代码骨架下面这个 Python 类把正逆运动学、编码器里程计、限速和饱和保护都放在一起。你可以拿它当模板翻译成 C 或单片机上用也很容易。class DifferentialDriveController: def __init__(self, wheel_radius, wheel_track, max_wheel_speed): self.r wheel_radius self.L wheel_track self.max_wheel_speed max_wheel_speed def inverse_kinematics(self, v, omega): 目标线速度和角速度 - 左右轮线速度 v_r v omega * self.L / 2.0 v_l v - omega * self.L / 2.0 v_l, v_r self._saturate(v_l, v_r) return v_l, v_r def forward_kinematics(self, v_l, v_r): 左右轮线速度 - 机器人线速度和角速度 v (v_l v_r) / 2.0 omega (v_r - v_l) / self.L return v, omega def _saturate(self, v_l, v_r): scale max(1.0, abs(v_l) / self.max_wheel_speed, abs(v_r) / self.max_wheel_speed) return v_l / scale, v_r / scale注意这里的_saturate采用等比缩放而不是简单截断。为什么不直接if v_l max: v_l max因为简单截断会破坏v和ω的比例关系导致机器人的实际转弯半径被改变。等比缩放则能保持左右轮速差与总速的比例不变轨迹形状不被破坏只是整体速度变慢。5.2 从目标速度到 PWM 中间的坑算出左右轮线速度之后很多人会直接做线性映射pwm_l v_l * k但这个做法在小车上经常出问题因为电机的 PWM 占空比和转速之间不是严格线性关系尤其在低压启动段有死区也就是 PWM 小到一定程度时电机根本不动接近最大占空比时转速又趋于饱和。比较可靠的做法是加一个简单的速度闭环。比如每个控制周期读取编码器速度把它和目标速度做 PI 控制输出 PWM。差速底盘运动学模型只负责“上层换算”底层电机响应是另一个控制问题不能混为一谈。如果你暂时不想做闭环至少要做“死区补偿”先测出电机启动的最小 PWM然后在这个值之上线性映射。再配合软件里的加速度限制让目标速度缓慢上升而不是阶跃给定能极大减少轮子打滑和底盘顿挫。5.3 在 ROS 里落地cmd_vel 与 odom在 ROS 生态里运动学模型一般承担两个接口订阅/cmd_vel接收geometry_msgs/Twist里的linear.x和angular.z作为逆运动学输入发布/odom里程计消息同时通过tf发布odom - base_footprint的坐标变换。实际部署时我最常看到的问题就是/odom频率和控制频率不匹配。比如底层电机控制是 50Hz里程计发布却是 10Hz。这样上层导航收到的里程计会显得“一跳一跳”局部规划器也容易误判。建议里程计发布时间和底层编码器采样周期保持一致或者整倍数发布不要随意降频。另外cmd_vel里一般还包含linear.y和linear.z、angular.x、angular.y这些对差速底盘而言应该被忽略。但很多人直接在逆运动学里把linear.x当v、angular.z当ω这是没问题的不过要确保上层导航不会往linear.y里塞东西。否则小车轮子没坏代码却以为自己在横移。6. 模型之外的真实世界打滑、负载、融合一个都不能少运动学模型再正确也只是“刚体纯滚动假设”下的一个理想化描述。现实中轮胎会打滑、重心会偏移、地面会不平这些都会让模型失真。这一节讲的不是否定模型而是告诉你什么时候该修正它、什么时候该放弃它。6.1 打滑是纯滚动假设最大的敌人差速底盘运动学的前提是轮子与地面之间只有纯滚动没有滑动。但实际加减速时驱动轮和地面之间的静摩擦力一旦被突破轮子就会打滑。这时候编码器依然在计数但实际位移小于编码器算出来的位移。打滑的典型场景是原地旋转时转速太高轮胎和地面摩擦发出尖锐声突然从静止满 PWM 起步左右轮出现不同程度的空转在地板砖、瓷砖等光滑表面转弯轮子横向滑动。应对打滑不是说让模型变得更复杂而是从源头上抑制打滑控制加速度上限、降低急转弯速度、在驱动轮上方增加配重、选择抓地力更好的轮胎。如果应用环境必然打滑比如户外草地那纯里程计基本不可靠必须引入其他传感器。6.2 重心偏移会影响什么很多人问我把电池放在底盘后侧会不会导致运动学公式失效答案是不会。运动学只关心轮子速度不关心重心位置只要左右轮速度执行准确整体的几何运动关系依然成立。但重心偏移会通过两个间接途径影响模型影响轮胎的接地压力分布进而影响打滑概率和轮胎有效滚动半径影响底盘两侧电机的负载均衡导致同样的 PWM 下左右轮实际速度不同造成直行走偏。所以调车时不要只看电机参数还要注意底盘配重。我习惯把电池这类重物放在底盘对称位置然后在标定之前先确认车体在水平地面上的姿态稳定。6.3 和 IMU 融合别让里程计独自硬扛纯运动学模型算出的里程计短期内很平滑但长期一定漂移。因为编码器是相对测量任何一点微小误差都会在积分里不断累积。地面稍有不平、轮胎有一点磨损半小时后位置误差可能就到了厘米甚至分米级别。工程上最常见的补救方式是融合 IMU。陀螺仪 z 轴积分出的航向角短期稳定性不如编码器差分出的Δθ但长期没有累积漂移如果初始化得当、零偏已补偿可以作为航向参考加速度计和编码器里程计通过扩展卡尔曼滤波或粒子滤波融合可以同时修正位置和姿态。不过我想强调一个顺序问题如果运动学模型参数本身没有标定好融合 IMU 也没用。模型给出了错误的航向变化趋势IMU 也糊里糊涂两个错误信号融合出来还是错误。我自己踩过的坑是先在仿真里把里程计精度调到回环误差很小再引入 IMU不然你根本分不清误差是来自标定还是来自融合算法。另外如果你只是做室内短距离循迹不一定需要上 IMU。认真标定轮距、轮径保持合理速度纯编码器里程计在小范围场景下足够用。IMU 融合更多是长期建图、多楼层定位、或者轮子经常打滑的场景才值得投入。我在实际项目中还有一个体会无论运动学公式写得多么漂亮调试时都要从“最小闭环”开始。先只验证一个轮子的速度闭环再验证左右轮速度跟踪再验证原地旋转角速度最后才验证全场航位推算。每一步单独通过才能定位误差来源。如果一上来就追求“打开发布里程计导航跑一圈”出了问题根本无从下手。差速底盘的运动学模型本身不难难的是把公式背后的物理图景、参数标定、离散化细节和控制闭环全部串在一起。只要把这个模型吃透后面换到三轮全向底盘、四轮麦克纳姆轮你会发现很多思路是相通的只是几何关系换了个样子而已。
