路径跟踪模拟器：纯跟踪与 Stanley

控件

控制器用来在纯跟踪和 Stanley 之间切换。前视距离是纯跟踪唯一的旋钮， 对 Stanley 则是禁用的，因为它并不使用前视距离——这本身就是重点。

速度与前视距离会相互影响，这是最值得动手玩的部分： 先设成 1 m/s 找到最好的前视距离，再设成 3 m/s 重新找一次。

位置来源可以把完美的里程计换成一个 1 Hz、带米级噪声的定位结果， 并在两次更新之间保持不变。误差始终对照真值评分，所以这个惩罚是真实的， 而不是被藏起来的。

拖动任意航点，或者按 1–8 选中一个再用方向键移动。 拖动时整趟运行都会重新计算。

原理

两个控制器回答的是同一个问题——已知我在哪里、路径在哪里，转向角应该是多少？—— 而它们的回答方式完全不同。

纯跟踪是几何。找出路径上在前视距离 L 之前那个点，然后开过它的圆弧：

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

其中 y 是目标点在机器人坐标系下的横向偏移，b 是轴距。它只有一个参数， 而且是一个距离而不是一个增益——所以它可以在机器人之间迁移。

Stanley是控制。以前轴为参考，加上两项：

δ = θ_e + atan(k · e / v)

航向误差加上横向误差的反正切。除以速度，就是让一个增益能跨越一段 速度范围的原因。k 是一个真正的控制增益，不迁移。

实验台补上了教科书推导里略去的东西：总共 0.4 s 的滞后， 由 0.25 s 的一阶转向响应加上 0.15 s 的感知与计算延迟构成。 没有它，纯跟踪会随着前视距离缩小而单调变好，整个整定问题就消失了。

算法

每一步，对两个控制器都适用：

1. 投影到路径上，在“上次到达的弧长”附近的一个窗口内搜索，而不是整条路径。 全局搜索会匹配到最近的那一段，所以一条折返的赛道会把机器人的进度 瞬移到它一分钟前刚经过的某个拐角。
2. **在顶点处把平局解决到出边那一段。**过了拐角之后，两条相邻线段会投影到 同一个点、且距离相同；保留入边那一段，会递给控制器一个航向误差和 一个横向偏移——两者都接近零——于是它悄无声息地径直开出路径。
3. 计算转向角，按控制器自己的规则来，并把它限制在机械极限内。
4. 施加滞后，然后积分自行车模型。
5. 对照真值评分，永远不要对照估计值。

第 2 步不是一个优化项。它就是 Stanley 能用和 Stanley 每次都恰好 在 78% 的地方自信地开出赛道之间的区别。

源代码

完整的纯跟踪，以它在真实机器人上呈现的形式：

float lastS = 0;   // arc length last reached — state, and it only moves forward

float pursue(float x, float y, float heading, float speed) {
  // The lookahead is scheduled on speed, with a hard floor: at a standstill a
  // proportional schedule asks for zero, which is a division by zero in disguise.
  float lookahead = constrain(0.7f * speed + 0.4f, 0.4f, 3.0f);

  float s = projectOntoPath(x, y, lastS);      // windowed around lastS
  lastS = max(lastS, s);
  Point goal = pointAtArcLength(lastS + lookahead);

  float dx = goal.x - x, dy = goal.y - y;
  float lateral = -sinf(heading) * dx + cosf(heading) * dy;
  float ld = max(0.3f, hypotf(dx, dy));        // clamp, or curvature blows up

  float curvature = 2.0f * lateral / (ld * ld);
  return curvature;                            // omega = v * curvature
}

真正要紧的是三行：调度下限、lastS 的单调性，以及 ld 的钳位。 其余的就是公式。

假设与硬件差异

实验台给了机器人一张完美的路径地图、一个完全已知的轴距、没有轮子打滑， 以及一个干净的一阶滞后模型。真实机器人一样都没有。

轮子打滑会让位置估计以一种 任何控制器都无法察觉的方式出错。减速箱背隙 在每次转向换向时都会加上空行程，它表现得像额外的滞后，会把最优前视距离 往上推。而差速驱动根本没有转向角——它有一个被指令的转速， 所以 ω = v κ 取代了 atan，而轴距只影响它能转得多紧。

在这一切之后仍然存活下来的是那个形状：一个内部最优解、随速度向右移动， 以及在该最优解处的误差无论如何都随速度增长。

电路图

控制器是便宜的部分。它从机器人其余部分需要的东西，才是贵的部分。

**位置。**两个轮子上的正交编码器， 以及在回路速率上对它们做积分的差速里程计。 在室内，这足以应付几十米的距离。

**航向。**短期用陀螺仪，绝对参考用磁力计，或者两者都要。航向误差是两个 控制器都赋予最高权重的项，也正是里程计最先丢失的那一项。

户外加一个绝对修正。GPS 模块要融合进估计里， 而不是直接跟随它——差别是 0.09 m 的均方根误差对 1.25。

完整理论见纯跟踪和 横向误差； 要动手做，GPS 航点车先在室内用编码器起步， 只等跟踪器工作之后才到户外去。