无人驾驶定位基石：轮速计差速模型与航迹推算实践解析

1. 从轮子开始：为什么说轮速计是无人驾驶的“定盘星”？

很多朋友一聊到无人驾驶，脑子里蹦出来的肯定是激光雷达、摄像头、高精地图这些听起来就“高大上”的玩意儿。确实，它们是让汽车“看得见”的关键。但你想过没有，如果我问你，这辆车现在具体在马路牙子左边第几厘米，它刚刚一秒内往前挪了多远，它是怎么知道的？尤其是在地下车库、隧道里，那些“眼睛”不太好使或者干脆“瞎了”的时候。

这时候，一个你可能都没注意过的“老伙计”就登场了：轮速计。它通常安装在车轮上，像个忠实的会计，一丝不苟地记录着每个轮子转了多少圈。听起来是不是特简单，甚至有点“原始”？但我要告诉你，在定位这个核心问题上，尤其是在低速、信号遮挡的复杂场景里，这套基于轮速计的差速模型和航迹推算方法，往往是整个系统最可靠、最基础的“压舱石”。你可以把它理解成我们人类的“本体感觉”——闭上眼睛，你也能大概知道自己的手移动到了哪里，靠的不是眼睛看，而是肌肉和关节的感觉。轮速计，就是车的“本体感觉”传感器。

我做过不少低速无人车项目，比如园区物流车、港口集装箱转运车。这些地方，GPS信号飘得厉害，激光雷达打到玻璃幕墙或者一排排整齐的集装箱上，也容易犯晕。这时候，我们最信赖的反而是轮速计给出的数据。它可能不“炫酷”，但胜在稳定、高频、不受外界环境干扰。它的核心任务就一个：根据轮子转动的信息，实时推算出车辆“相对”于上一时刻，到底移动了多少距离，转了多少角度。这个“相对”定位，是构建一切精准定位地图的起点。没有它，其他传感器再厉害，也像是没有锚的船，容易在数据海洋里漂移。所以，今天咱们就抛开那些复杂的公式，从工程实践的角度，掰开揉碎了讲讲，这个“定盘星”到底是怎么工作的，我们又该怎么用好它。

2. 双轮差速模型：两个轮子如何“说”出车的姿态？

要理解航迹推算，咱们得先弄明白车是怎么“描述”自己运动的。对于最常见的两轮差分驱动模型（很多机器人、AGV小车都是这个结构），它的运动奥秘全藏在两个轮子的速度差里。

2.1 一个比喻：原地转圈与直线行走

想象一下你和朋友并排推一个很重的柜子。如果你俩用力一样大、速度一样快，柜子就直直地往前走。如果你力气大点，朋友力气小点，柜子就会一边往前走，一边朝朋友那边拐弯。要是你往前推，朋友往后拉，那柜子干脆就在原地打转了。看，控制两个“轮子”（你和朋友）的速度，就能实现前进、转弯、甚至原地旋转。

双轮差速模型就是这个原理的数学表达。我们把车简化一下，就看成左右两个驱动轮，中间连着个轴。车怎么动，完全由左轮速度vl和右轮速度vr决定。这里的速度，通常是指轮子边缘的线速度，单位是米/秒。轮速计干的就是实时测量这两个vl和vr的活儿。

2.2 公式推导：从轮速到车体运动

网上很多文章一上来就甩公式，容易把人看懵。咱们换个方式，用“会计记账”的思路来理解。

第一本账：车跑了多远（线速度 v）这个最好理解。车整体的前进速度，不就是左右轮速度的平均值嘛。左轮跑快点，右轮跑慢点，但车身的中心点（我们通常关心的位置）的移动速度，就是它俩的“中庸之道”。公式很简单：v = (vr + vl) / 2这就是我们得到的第一个关键信息：车身中心点的前进线速度。

第二本账：车转了多少（角速度 ω）这是差速模型的核心。为什么车会转弯？因为两个轮子跑的路程不一样长！假设在很短的一段时间 Δt 里，左轮跑了Sl = vl * Δt的距离，右轮跑了Sr = vr * Δt的距离。如果vr > vl，右轮跑得远，车自然就向左拐弯。

现在，把车在这段时间内的运动轨迹想象成一段圆弧。左轮和右轮走的，就是两个同心圆的圆弧。这两个圆的半径相差多少呢？正好是左右轮之间的轮距L。根据圆弧长度公式（弧长 = 半径 × 转角），我们可以列出等式： 对于右轮：Sr = R * θ对于左轮：Sl = (R - L) * θ这里的θ就是车身在这段时间内转过的角度（以弧度为单位）。

用第一个等式减去第二个等式：Sr - Sl = R * θ - (R - L) * θ = L * θ所以，转角θ = (Sr - Sl) / L = (vr - vl) * Δt / L。

那么，单位时间内转过的角度，也就是角速度ω，就是：ω = θ / Δt = (vr - vl) / L看，这个公式多直观！角速度直接等于左右轮速度之差除以轮距。轮距L是车的固定参数，所以只要知道两个轮子的速度差，瞬间就知道车在怎么转了。如果vr = vl，速度差为零，角速度为零，车直行。如果vr = -vl，即两个轮子速度大小相等、方向相反，代入公式角速度最大，车就原地旋转。

第三本账：转弯半径有多大？有了线速度v和角速度ω，转弯半径R自然就出来了。对于圆周运动，v = ω * R。所以：R = v / ω = [ (vr+vl)/2 ] / [ (vr-vl)/L ] = L * (vr+vl) / (2*(vr-vl))这个公式也挺有意思。当vr和vl非常接近时，分母很小，半径R就会非常大，接近直线运动。这和我们开车时微调方向的感觉是一致的。

把上面三本账总结一下，我们就得到了差速模型的核心方程组，它建立了轮子速度与车体运动状态之间的桥梁：

在实际的代码里，我们通常以很高的频率（比如100Hz）读取轮速计脉冲，换算成vl和vr，然后就用这几个公式瞬间算出车辆当前瞬时的v和ω。有了这两个量，我们就掌握了车在极短时间内的运动“意图”。

3. 航迹推算：像拼图一样，拼出车辆的行驶轨迹

知道了“瞬时”的运动状态，我们怎么知道车从起点开始，总共走到了哪里呢？这就是航迹推算要干的事。它的思想很像我们玩拼图：你有了每一小块拼图（短时间内的位移和转角），只要按顺序把它们一块块拼接起来，最终就能得到完整的画面（从起点开始的完整轨迹）。

3.1 核心思想：积分与叠加

航迹推算，在学术上常被称为“Dead Reckoning”。它的输入就是上一节我们算出来的瞬时线速度v(t)和角速度ω(t)，以及上一时刻的位姿（位置x(t-1),y(t-1)和航向角θ(t-1)）。它的输出，就是当前时刻的位姿x(t),y(t),θ(t)。

计算过程是一个递推的“积分”过程：

1. 更新航向角：θ(t) = θ(t-1) + ω(t) * Δt。角速度乘以时间，得到角度变化量，累加到上一时刻的航向上。
2. 更新位置：这需要用到更新后的航向角。在很短的时间内，我们可以认为车是沿着θ(t)方向以速度v(t)运动的。所以：x(t) = x(t-1) + v(t) * cos(θ(t)) * Δty(t) = y(t-1) + v(t) * sin(θ(t)) * Δt

看，原理就是这么直接。用代码表示，一个简单的航迹推算循环大概是这样的：

import math # 初始位姿 (x, y, theta) x, y, theta = 0.0, 0.0, 0.0 # 轮距 L = 1.5 # 米 # 时间间隔 dt = 0.01 # 秒，对应100Hz def dead_reckoning(vl, vr, x_prev, y_prev, theta_prev, dt, L): """航迹推算一步""" # 1. 计算当前瞬时运动量 v = (vr + vl) / 2.0 omega = (vr - vl) / L # 2. 更新航向角 theta_new = theta_prev + omega * dt # 3. 更新位置 (使用theta_new或theta_prev+omega*dt/2有细微差别，此处用theta_new) x_new = x_prev + v * math.cos(theta_new) * dt y_new = y_prev + v * math.sin(theta_new) * dt return x_new, y_new, theta_new # 模拟循环 for i in range(1000): # 模拟10秒 # 这里应从传感器读取 vl, vr，此处用模拟值 vl = 1.0 # 左轮速度，米/秒 vr = 0.8 # 右轮速度，米/秒 x, y, theta = dead_reckoning(vl, vr, x, y, theta, dt, L) # 此时 (x, y, theta) 就是当前时刻的推算位姿

3.2 实践中的关键细节：时间与模型选择

看起来很简单对吧？但在实际工程里，魔鬼藏在细节中。

第一个细节：时间戳同步与积分间隔 Δt。轮速计的数据、执行计算的CPU时钟，它们的时间未必严格对齐。如果你用固定的dt=0.01s去积分，但实际两次收到轮速数据的时间间隔是0.0101s或0.0099s，长此以往，误差就积累起来了。最佳实践是，每次推算都使用精确的、相邻两次轮速数据到达的时间差作为dt。这需要你的系统有精确的时间戳服务。

第二个细节：运动模型的选择。上面我们用的模型，假设在dt内，车是沿着更新后的航向角θ(t)做直线运动的。这其实是一种近似。更精确的模型是假设车在dt内做匀速圆周运动。对于圆周运动模型，当角速度ω很小时，计算会复杂一些，但精度更高，尤其是在转弯时。其位置更新公式为：

if |ω| < epsilon: # 近似直线运动 x(t) = x(t-1) + v * cos(θ(t-1)) * dt y(t) = y(t-1) + v * sin(θ(t-1)) * dt else: # 圆周运动 R = v / ω # 转弯半径 # 计算圆心坐标 ICC_x = x(t-1) - R * sin(θ(t-1)) # ICC: Instantaneous Center of Curvature ICC_y = y(t-1) + R * cos(θ(t-1)) # 绕圆心旋转 delta_theta = omega * dt x(t) = math.cos(delta_theta)*(x(t-1)-ICC_x) - math.sin(delta_theta)*(y(t-1)-ICC_y) + ICC_x y(t) = math.sin(delta_theta)*(x(t-1)-ICC_x) + math.cos(delta_theta)*(y(t-1)-ICC_y) + ICC_y

对于大部分低速场景，直线近似模型已经足够好，且计算量小。但对于追求更高精度或角速度较大的情况，采用圆周运动模型是更稳妥的选择。我在项目中通常会做一个开关，根据实际调试效果来选择。

4. 误差从何而来？正视航迹推算的“阿喀琉斯之踵”

航迹推算最大的优点就是自主、高频、短期精度高。但它有一个致命的缺点：误差会随着时间累积，发散得很快。不搞清楚误差来源，定位系统分分钟“跑飞”。这些误差主要来自以下几个方面，我结合踩过的坑来说说。

4.1 传感器误差：轮速计的“谎言”

轮速计本身就不完美。首先是刻度误差：你的代码里认为轮子转一圈走0.5米，但实际轮胎因为气压、磨损、负载，周长可能是0.51米。这个误差是比例因子，跑得越远，绝对误差越大。每次换轮胎或者胎压变化显著时，都必须重新标定。

其次是脉冲量化误差。轮速计输出的是脉冲，我们根据一段时间内的脉冲数算速度。在低速时，脉冲间隔很大，速度计算不准确。比如车刚启动或快要停下时，算出的v和ω可能跳变很厉害。这会导致航迹推算在起停阶段产生“抖动”或“漂移”。实践中，我们需要对低速下的速度测量进行特殊处理，比如设置一个速度阈值，低于它时认为速度为零，或者使用更复杂的滤波算法。

4.2 模型误差：现实不是理想模型

我们的双轮差速模型做了很多理想化假设：车轮纯滚动无滑动、车身是刚体、左右轮距精确已知且不变。现实很骨感。

打滑是最大的敌人。在湿滑路面、急加速或急刹车时，车轮会发生滑动。滑动意味着轮子转的圈数和实际走的距离对不上了。航迹推算对此完全无能为力，还会把它当作真实运动积分进去，误差一下子就上去了。这是系统性偏差，无法通过自身修正。

轮距变化和车身形变。车在负载不均或过坎时，悬架压缩，实际轮距和模型中的L会有微小差异。虽然影响相对较小，但长距离累积也不容忽视。

4.3 积分误差：数学上的必然

即使传感器数据完全正确，模型完全匹配，积分过程本身也会引入误差。我们是用一系列短时间的直线（或圆弧）去逼近一条复杂的曲线。这个逼近过程是有误差的。更重要的是，计算机是离散计算的，存在浮点数精度问题。虽然单次计算误差极小，但经过成千上万次迭代后，这些舍入误差也可能被放大。

如何应对？单独依靠航迹推算做长距离、长时间的定位是绝对不行的。它必须与其他传感器融合。它的核心价值在于提供高频、相对准确的短期运动估计，用来弥补像GPS、激光SLAM这些传感器更新频率低、或在某些时刻失效的不足。在融合滤波器中（比如卡尔曼滤波），航迹推算常常作为“预测”环节的模型，提供一个可靠的先验估计。

5. 低速场景下的王者：轮速计不可替代的价值

既然误差会累积，为什么我们还要如此重视轮速计和航迹推算呢？因为在特定的场景下，它是“矮子里的将军”，甚至是“唯一的选择”。

5.1 场景一：卫星信号拒止环境

地下车库、隧道、高楼林立的城市峡谷、茂密的林荫道。这些地方GPS信号弱、多路径效应严重，定位结果要么没有，要么跳变巨大。视觉和激光雷达在结构单一的长隧道里，也可能因为特征稀少而失效。这时候，轮速计提供的航迹推算，就成了维持定位连续性的“生命线”。虽然它有累积误差，但在几分钟的隧道通行时间内，其推算位置通常还在可接受的范围内，能支撑车辆完成穿越，直到重新接收到可靠的外部信号。

5.2 场景二：高动态与高频率要求

无人车在复杂交通中行驶，需要极高的控制频率（比如100-200Hz）。激光雷达的扫描频率通常是10Hz，摄像头可能30Hz，GPS只有1-10Hz。它们都无法提供控制所需的实时位姿更新。而轮速计数据可以轻松达到100Hz以上。控制器可以基于航迹推算提供的高频位姿，进行平滑、及时的控制决策，再用低频的绝对定位信息（如激光SLAM）来周期性地校正航迹推算的累积误差。这种“高频推算+低频校正”的模式，是业界的主流做法。

5.3 场景三：低成本与高可靠性方案

对于园区物流车、清扫车、叉车等低速封闭场景的无人车，成本是关键。一套高线数的激光雷达可能比车体还贵。而轮速计是车辆的标配或低成本加装件。构建一个以轮速计航迹推算为核心，辅以低成本IMU（惯性测量单元）和稀疏二维码或UWB（超宽带）锚点进行校正的定位系统，可以极大地降低成本，同时满足运营精度的要求。这种方案的可靠性很高，因为轮速计是物理接触式测量，几乎不受天气、光照、粉尘的影响。

我在一个自动化港口项目里就深有体会。集装箱堆场里，金属集装箱反射干扰激光雷达，GPS信号也被遮挡。我们最初尝试用多激光雷达融合，效果不稳定且成本高昂。后来切换到以高精度编码器（轮速计）为主，在堆场关键路口布设少量UWB锚点的方案。航迹推算负责车辆在箱区内的连续行走定位，每经过一个UWB锚点就做一次“归零”校正，消除累积误差。这套方案最终以不到原先三分之一的成本，实现了稳定可靠的7x24小时运行。

6. 工程实践指南：让你的航迹推算更“稳”

理论说了这么多，最后分享几个让航迹推算在实际项目中更“稳”的工程经验。

第一，精确标定是第一步。上车第一件事，不是写代码，而是标定。找一段平整的直路，让车以固定速度跑一段已知距离（比如50米），记录轮速脉冲总数。反算出轮子的实际周长。重复多次取平均。同样，让车原地旋转360度，记录左右轮的总脉冲差，可以反推出更准确的等效轮距L。这些基础参数差一点，后面怎么滤波都补不回来。

第二，设计合理的数据预处理流水线。原始脉冲信号不能直接拿来用。我的流水线通常是：硬件中断捕获脉冲->定时器中断中计算瞬时转速（防脉冲丢失）->低通滤波平滑（去除高频噪声）->速度合理性检查（剔除异常跳变）->送入推算模型。对于CAN总线传来的轮速信息，也要注意检查报文周期和数据的有效性。

第三，与IMU紧耦合。轮速计怕打滑，IMU（陀螺仪和加速度计）怕漂移。但它俩优势互补。IMU的陀螺仪可以提供独立的角速度测量，与轮速计推导出的角速度进行交叉验证，能有效检测出打滑（比如轮子转但IMU检测到车身没转）。在滤波框架中（如扩展卡尔曼滤波），将轮速计模型与IMU模型一起作为状态预测环节，能获得比单独使用任一传感器更稳健的短期运动估计。

第四，建立误差模型并设置信任度。不要将航迹推算的结果当作“真理”。要根据车辆状态（速度、角速度、路面估计）动态评估本轮推算的“信任度”。例如，在急加减速时，打滑风险高，信任度降低；在低速蠕动时，脉冲量化误差大，信任度也降低。这个信任度可以作为后续多传感器融合时的权重参考。

第五，提供外部校正接口。一定要为你的航迹推算模块设计一个“重置”或“校正”接口。当其他传感器（如激光匹配、视觉重定位、GPS固定解）提供了一个高置信度的绝对位姿时，就用这个位姿去覆盖当前的推算位姿，并将内部的累积误差清零。这是抑制误差发散最直接有效的手段。但要注意校正时的平滑过渡，避免位置跳变引发控制震荡。

说到底，用好轮速计和航迹推算，需要一种“既信任又怀疑”的态度。信任它在短期内的精确和高频，依靠它完成连续的姿态估计；同时怀疑它的长期稳定性，时刻准备用其他信息来纠正它。把这个基础打牢了，再往上叠加激光、视觉、地图这些高级感知层，你的无人驾驶定位系统才能真正做到既“看得远”，又“站得稳”。