纯跟踪算法：只用一个参数跟随路径

没人会教的那一级

规划器给你一条路径。A* 给你一串栅格；一次测绘给你一串航点；一个任务给你地图上的一条线。它们谁都没告诉轮子该做什么。

纯跟踪就住在这个空隙里，而它只是一段几何：

找出路径上在你前方固定距离处的那个点。开出经过它的圆弧。重复。

没有误差信号，没有积分项，没有增益调度。只有一个数——前视距离——而它决定一切。

几何

把目标点放到机器人自己的坐标系里：x 朝前，y 朝左，距离是 L_d。从机器人当前位置经过该点的一段圆弧，其曲率为

κ = 2y / L_d²

这一行值得读两遍，因为一切都从它来。横向偏移 y 在分子上，所以目标点离轴越远，弧就弯得越狠。前视距离在分母上是平方，所以把它翻倍会把曲率变成四分之一。

换成半径，再换成轴距为 b 的车辆的转向角：

R = L_d² / 2y            δ = atan(2 b sin α / L_d)

对差速底盘来说没有转向角——你直接下发旋转速率，ω = v κ。同样的曲率，不同的执行器。

唯一的旋钮，以及它为什么自己跟自己打

**短前视。**目标点很近，所以一点小的横向偏移就产生很大的曲率。机器人跟得很紧——而且对每一次抖动都剧烈反应，包括那些它自己的转向刚刚造成的抖动。

长前视。目标点很远，曲率平缓，转向平滑——而在弯道处，目标点已经绕过了弯，机器人却还没有，于是机器人切弯。

两者朝相反方向拉扯，而算法里没有任何东西去平衡它们。你只能选一个数。

每本教科书推导里的陷阱

把纯跟踪写在纸上，配一台瞬间就能转向的机器人，于是越短总是越好。误差随着前视缩短单调下降。没有最优，上面那条 U 形曲线根本不存在。

真实的机器人有滞后。从下发转向指令到达到那个角度需要时间；到你动作时所依据的那个位姿，在轮子响应之前已经老了几十毫秒。模拟器建模了一个 0.25 s 的转向时间常数和 0.15 s 的感知加计算延迟——总共 0.4 s 的滞后，对一台业余漫游车来说平平无奇。

那份滞后就是最优值存在的全部原因。短前视下发一个修正，修正迟到，机器人已经走过误差所在的位置，下一条指令就朝反方向过度修正。**一个比自己的执行器还快的控制器会振荡。**这和支配 PID 微分增益的是同一个事实，也是停车距离背后的那个：延迟不是细节，它是决定整定的那件事。

那么该怎么选

实测出来的最优值，以及换算成可迁移单位之后的样子：

这张表有两个诚实的读法。

**前视距离随速度增长，但不成比例。**常见的建议是“设成固定几秒的行程”，接近但不准确——在这个范围内时间从 0.90 s 掉到 0.47 s。

**它落在一到两倍“你在一份控制滞后期间走过的距离”上。**这就是那条有用的规则，因为它可测量：找出你的滞后，乘以你的速度，从那里开始。在这台机器人上 v × lag 是 0.4 m 到 1.2 m，而最优值是 0.9 m 到 1.4 m。

实践中，把它做成一条小的线性调度并钳位：

float lookahead = constrain(K_LOOK * speed + L_MIN, L_MIN, L_MAX);

L_MIN 比 K_LOOK 重要得多。静止时调度想要的前视距离是零，而而那就是伪装成别的东西的除以零。

把它实现出来

// Pure pursuit over a waypoint list. Returns the curvature to command.
float pursue(float x, float y, float heading, float lookahead) {
  // 1. Where am I on the path? Search only near the arc length I last reached —
  //    a global search latches onto whichever segment happens to be nearest, and a
  //    path that doubles back will teleport your progress to a corner you passed
  //    twenty seconds ago.
  float s = projectOntoPath(x, y, lastS);
  lastS = max(lastS, s);

  // 2. The goal point, one lookahead further along.
  Point goal = pointAtArcLength(lastS + lookahead);

  // 3. Into the robot frame, then straight into the curvature formula.
  float dx = goal.x - x, dy = goal.y - y;
  float lateral = -sin(heading) * dx + cos(heading) * dy;
  float ld = max(0.3f, hypotf(dx, dy));
  return 2.0f * lateral / (ld * ld);
}

十二行里有三样东西比其余的加起来更值得注意：

• **lastS 是状态，而且只往前走。**没有它，任何会自我交叉的路径都会让控制器搁浅。
• **钳住 ld。**随着目标点接近，曲率趋向无穷。
• **目标点在路径上，而不是最近的航点。**瞄航点会让车在它们之间左右摇摆着走；瞄线段上的一点，才是让运动平滑的原因。

它能做什么，不能做什么

**它不会站着不动、转过来面对路径。**纯跟踪没有“站着不动”这个概念——曲率是一个形状，而形状需要前进运动。让机器人大致对齐着起步，或者先开一小段起始弧。

**它有一个最小转弯半径，而路径未必尊重它。**如果一个弯比 b / tan(δ_max) 更急，机器人无论怎么调都跟不过。要么把路径软化，要么在弯道减速——运动曲线就是干这个的。

**它总是切弯，这是设计使然。**在 2 m/s 的实测中，它比指令路径少开了 0.8%。如果这不能接受，你要的是一个不同的控制器——Stanley 改为按横向误差转向，它会从外侧甩过而不是切内。

**它修不好一个糟糕的位置估计。**上面一切都假设机器人知道自己在哪里。喂它 1 Hz 的 GPS 定位，均方根误差就从 0.09 m 涨到 1.25 m——见GPS 航点导航。

出问题时

在路径跟踪模拟器里让两个控制器跑同一条赛道——拖动前视距离，整趟曲线会重画。然后去做GPS 航点车，在那里位置估计会变成最难的部分。