4、纯跟踪算法基础：几何原理、前视距离选择、横向误差计算、转向角指令生成

各位同学，欢迎来到弯道跟踪实战的第四讲。

今天咱们聊一个非常经典、也非常直观的算法——纯跟踪（Pure Pursuit）。说实话，我在刚入行做路径跟踪时，第一个上手的算法就是它。为什么？因为它简单、有效，而且几何意义极其清晰。你想想看，一个算法能用「纯几何」的方式解决控制问题，这本身就很有魅力。

4.1 几何原理：自行车模型与圆弧拟合

纯跟踪的核心思想，说白了就是一句话：让车像追着一个点跑一样。

我们在路径上选一个「前视点」，然后控制车辆转向，让车辆沿着一条圆弧驶向那个点。这个圆弧的半径，就是由车辆当前位置、前视点位置、以及车辆轴距共同决定的。

嗯，这里要用到经典的自行车模型。我们把四轮车简化成两轮，前轮转向角为 δ，轴距为 L。车辆绕着一个瞬时圆心做圆周运动。这个模型的假设是：低速、无侧滑。实际项目中，只要车速不超过 30km/h，这个模型精度完全够用。

我曾经在一个园区物流项目里，直接用这个公式做跟踪，效果出奇的好。当然，前提是前视距离选对了——这个我们马上讲。

4.2 前视距离选择：算法成败的关键

前视距离 ld 怎么选？这是纯跟踪算法里最玄学、也最实际的问题。

我个人习惯把前视距离设计成速度的函数。为什么？因为车速越快，你需要看得越远，否则转向会太急，车会晃。反过来，车速慢时，前视距离短一些，跟踪精度更高。

我曾经在一个项目中，把 k 设成了 1.2，结果车在高速弯道里疯狂画龙。后来改成 0.4，稳如老狗。所以这个参数一定要根据实际车辆动力学去调，别照搬别人的值。

另外，前视距离还和路径曲率有关。曲率大的弯道，前视距离应该适当减小，否则容易切内线。我一般会在曲率大于 0.1 时，把 ld 乘以一个 0.7 的系数。

4.3 横向误差计算：你到底偏了多少？

横向误差，就是车辆当前位置到目标路径的垂直距离。这个值用来评估跟踪效果，也常作为反馈控制的输入。

计算方法很简单：

1. 找到路径上离车辆最近的点（匹配点）
2. 计算车辆位置到该点连线的垂直距离
3. 注意符号：车在路径左侧为正，右侧为负（或者反过来，团队统一就行）

在实际代码中，我通常这样计算：

// 伪代码：计算横向误差
// path_points: 路径点集
// vehicle_pos: 车辆当前位置 (x, y)
// 找到最近点
nearest_idx = find_nearest_point(vehicle_pos, path_points)
// 计算最近点处的切线方向
tangent = compute_tangent(path_points, nearest_idx)
// 计算车辆到最近点的向量
vec = vehicle_pos - path_points[nearest_idx]
// 横向误差 = 向量在法线方向上的投影
cross_error = cross(tangent, vec)  // 二维叉积，带符号

嗯，这里要注意，如果路径点比较稀疏，最近点匹配可能会跳变。我建议做一下平滑处理，或者用插值法找到更精确的匹配位置。

4.4 转向角指令生成：从几何到控制

好，现在我们有前视点、前视距离、车辆轴距，接下来就是生成最终的转向角指令。

公式其实前面已经提到了，我再完整写一遍：

// 输入：
//   ld: 前视距离
//   alpha: 前视点与车辆航向的夹角
//   L: 车辆轴距
// 输出：
//   delta: 前轮转向角

// 计算圆弧半径
R = ld / (2 * sin(alpha))

// 计算转向角（弧度）
delta = atan2(L, R)

// 限幅处理
delta = clamp(delta, -max_steer_angle, max_steer_angle)

这里有个细节：alpha 角怎么算？它是前视点相对于车辆坐标系的方向角，减去车辆当前的航向角。如果前视点在车左边，alpha 为正，转向角为正（左转）。

最后，生成的转向角指令要经过执行器限幅和变化率限制。我一般把最大转向角设为 30°~40°，变化率限制在 0.3~0.5 rad/s。这样既能保证响应速度，又不会让方向盘打得太猛。

好了，纯跟踪的基础内容就这些。几何原理、前视距离、横向误差、转向角生成，这四个环节环环相扣。你只要把前视距离调好了，这个算法在大部分弯道场景下都能跑得不错。