从人类驾驶到算法：

纯跟踪的“模仿秀”

人类开车时，从不会死盯着车头，而是会看向前方几十米的目标点，然后自然转动方向盘对准它。纯跟踪算法的核心逻辑，就是把这个过程“数学化”。

先给汽车“瘦身”：从四轮到两轮的简化

真实汽车的转弯原理很复杂（比如阿克曼转向，四个车轮绕同一圆心转动，内侧车轮转角更大），但纯跟踪算法用了个“偷懒”的办法——自行车模型：

图 真实车辆的阿克曼转向几何（上）及其简化的自行车模型（下）

把汽车简化成“两个轮子”（前轴和后轴各一个），只考虑平面运动。这样一来，前轮转角δ和转弯半径R的关系就变得简单：tan(δ)=轴距L/转弯半径R

也就是说，只要算出目标转弯半径R，就能直接得到需要转多少方向盘（δ=arctan(L/R)）。

这个模型在60km/h以下的低速场景非常好用，实测横向偏差能控制在15cm以内（相当于一个拳头的宽度）。

核心操作：

盯准目标点，画个圆弧冲过去

选目标点：以车辆后轴为起点，沿规划路线向前找一个“预瞄点”（距离自己ld米）

算转弯半径：计算从当前位置到目标点，需要画一个多大半径的圆弧

打方向盘：根据圆弧半径，用上面的公式算出前轮该转多少角度

整个过程就像用圆规画圆——后轴是圆心，预瞄距离ld是半径，目标点是圆上的一点。

图 纯跟踪算法的几何关系推导

工程落地：

把公式变成“能跑的代码”

理论好懂，但落地时藏着不少细节。我们按算法运行的步骤，一步步拆解关键操作。

第一步：给目标点“定位

规划好的路线坐标是“全局地图”里的（比如经纬度），而车辆需要知道目标点在“自己的坐标系”里的位置（比如“前方5米，偏右0.3米”）。这就需要做坐标转换：

简单说，就是把“地图上的点”转化成“相对于自己的位置”，这样才能算清楚“该往左转还是右转”。

图 坐标转化

第二步：预瞄距离怎么定？

速度快就看远点预瞄距离ld（就是看前方多远的点）是个关键参数，直接影响控制效果：

太慢的车（比如泊车时<5km/h）：看1-2米就行

园区里跑的车（30-40km/h）：看3-5米

速度再快？抱歉，纯跟踪算法不太擅长（后面会说原因）

工程上常用速度自适应策略：速度v越快，预瞄距离ld越远（比如ld=k×v，k取1.1-1.5）。但要注意：

车速单位要转成m/s（CAN总线常给km/h，别忘除以3.6）

最低预瞄距离留3-5米（防止车速为0时出错）

图 预瞄距离与速度关系

第三步：目标点怎么选？别找错了！

选目标点时，要找到“刚好超过预瞄距离ld”的那个点，步骤很简单：

1.先在规划路线上，找到离自己最近的点（相当于“当前位置”）

2.从这个点往前数，累加路线长度，直到超过ld，这个点就是目标点

最后一步：方向盘转角怎么算？还要做什么处理？

有了目标点，用下面的公式直接算前轮转角：

转向角δ=arctan(2×轴距L×sin(α)/预瞄距离ld)

（α是车辆当前航向与目标点方向的夹角，用坐标转换后的局部坐标就能算）

但工程落地时，这一步还要做两件事：

转成方向盘角度：前轮转角×转向传动比（比如传动比16，就是前轮转1度，方向盘要转16度）

平滑处理：用一阶低通滤波消除抖动（比如滤波系数0.2，新角度=0.8×旧角度+0.2×计算值）

纯跟踪算法：哪些场景“能用”，

哪些场景“头疼”？

简单算法的“生存智慧”

纯跟踪算法就像自动驾驶领域的“老黄牛”——它不算最先进，但胜在简单可靠。在低速封闭场景里，它的表现甚至不输复杂的模型预测控制(MPC)。

理解它的原理，不仅能帮你快速实现一个可用的横向控制模块，更能让你明白：好的算法不是越复杂越好，而是能在特定场景下解决问题。

下次坐园区无人车时，不妨观察一下它过弯的路线——说不定，你就能看出纯跟踪算法的“影子”呢。