从原理到落地：纯跟踪算法如何让自动驾驶车 “走直线”?

在自动驾驶的 “驾驶技能” 里，横向控制算法就像司机的双手 —— 它决定车辆能否精准沿着规划好的路线行驶。而在众多横向控制算法中，纯跟踪算法(PurePursuit)凭借 “简单好用” 的特质，成为低速场景(比如园区物流车、自动泊车)的首选方案。人类开车时，从不会死盯着车头，而是会看向前方几十米的目标点，然后自然转动方向盘对准它。纯跟踪算法的核心逻辑，就是把这个过程 “数学化”。

一、化繁为简：自行车模型

真实汽车的转弯原理很复杂(比如阿克曼转向，四个车轮绕同一圆心转动，内侧车轮转角更大)，但纯跟踪算法用了个 “偷懒” 的办法 —— 自行车模型。把汽车简化成 “两个轮子”(前轴和后轴各一个)，只考虑平面运动。这样一来，前轮转角 δ 和转弯半径 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” 的那个点，步骤很简单：

先在规划路线上，找到离自己最近的点(相当于 “当前位置”)。

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

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

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

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

应用场景：

计算快：单帧处理 < 5ms，普通单片机都能跑。

自动泊车：10km/h 以内的泊车场景，短预瞄距离能精准入位。

农业机械：农田里直线作业，结合 PID 补偿能提高精度。

纯跟踪算法就像自动驾驶领域的 “老黄牛”—— 它不算最先进，但胜在简单可靠。在低速封闭场景里，它的表现甚至不输复杂的模型预测控制(MPC)。理解它的原理，不仅能帮你快速实现一个可用的横向控制模块，更能让你明白：好的算法不是越复杂越好，而是能在特定场景下解决问题。