参考：ChatGPT、autoware

对应源码文件:
autoware_devel/src/universe/autoware_universe/control/autoware_trajectory_follower_node/src/controller_node.cpp

Pure Pursuit: autoware_devel/src/universe/autoware_universe/control/autoware_pure_pursuit/src/autoware_pure_pursuit/autoware_pure_pursuit_lateral_controller.cpp

从零到工程实践：层层递进理解 Pure Pursuit 横向控制

Pure Pursuit 是自动驾驶和移动机器人中非常经典的路径跟踪算法。它的核心思想很朴素：不要盯着车辆脚下的最近点，而是在路径前方找一个目标点，然后让车辆沿一段圆弧追过去。

1. 横向控制到底在控制什么

自动驾驶车辆通常有两类控制问题：

• 纵向控制：控制速度、加速度、刹车。
• 横向控制：控制方向盘或前轮转角，让车辆沿参考轨迹行驶。

Pure Pursuit 解决的是横向控制问题。

给定：

• 当前车辆位置。
• 当前车辆朝向。
• 一条期望车辆跟踪的参考轨迹。
• 车辆轴距、最大转角等车辆参数。

Pure Pursuit 要输出：

• 一个目标曲率 κ。
• 或者进一步转换成前轮转角 δ。

曲率可以理解为“车辆接下来应该拐多急的弯”。曲率越大，转弯越急；曲率接近 0，车辆接近直行。

2. 从最小场景开始：车只需要追一个点

先不要考虑复杂轨迹。假设车辆当前在平面上，前方有一个目标点。我们希望车辆转向，使它沿一段圆弧到达这个目标点。

这就是 Pure Pursuit 的最小模型：

当前车辆位姿 + 一个前方目标点 -> 一条圆弧 -> 曲率 -> 前轮转角

为什么是圆弧？

因为在低速或中等速度下，车辆在短时间内可以近似看成自行车模型。当前前轮转角固定时，车辆会沿近似圆弧运动。Pure Pursuit 就利用了这个几何事实。

所以算法最关键的问题变成：

已知车辆当前位姿和目标点，如何求出连接它们的圆弧曲率？

3. 建立车辆坐标系

为了让几何关系简单，我们把问题放到车辆坐标系下。

车辆坐标系约定为：

• 车辆当前位置是原点 (0, 0)。
• 车辆当前朝向是 x 轴正方向。
• 车辆左侧是 y 轴正方向。
• 目标点在车辆坐标系下记为 (x, y)。

如果目标点在车辆左前方，则 y > 0。

如果目标点在车辆右前方，则 y < 0。

如果目标点几乎在车辆正前方，则 y ≈ 0。

这个坐标系非常重要，因为 Pure Pursuit 的核心公式全部来自这里。

4. 圆弧假设和半径推导

车辆当前朝向是 x 轴。如果车辆从当前点沿圆弧出发，那么圆弧在当前点的切线方向必须与车辆朝向一致。

圆在某一点的半径方向与切线垂直。既然当前切线是 x 轴，那么圆心一定在 y 轴上。

设圆心为：

O = (0, R)

其中 R 是带符号半径。

设目标点为：

P = (x, y)

目标点在圆上，所以目标点到圆心的距离等于半径：

(x - 0)^2 + (y - R)^2 = R^2

展开：

x^2 + y^2 - 2Ry + R^2 = R^2

两边消去 R^2：

x^2 + y^2 - 2Ry = 0

移项：

x^2 + y^2 = 2Ry

因此：

R = (x^2 + y^2) / (2y)

曲率是半径的倒数：

κ = 1 / R

所以得到 Pure Pursuit 最核心的曲率公式：

κ = 2y / (x^2 + y^2)

这个公式的含义很直观：

• 目标点越靠左，y 越大，曲率越偏向左转。
• 目标点越靠右，y 越小且为负，曲率越偏向右转。
• 目标点越接近正前方，y 越接近 0，曲率越接近 0。
• 目标点越远，x^2 + y^2 越大，曲率通常越小，转向越平缓。

5. 从曲率到方向盘：自行车模型

车辆不能直接执行“曲率”，它通常执行的是前轮转角。

使用简化自行车模型：

κ = tan(δ) / L

其中：

• κ 是路径曲率。
• δ 是前轮转角。
• L 是车辆轴距。

因此：

δ = atan(Lκ)

工程上还要限制最大转角：

δ = clamp(δ, -δmax, δmax)

到这里，我们已经得到最小版 Pure Pursuit：

1. 把目标点转换到车辆坐标系，得到 (x, y)。
2. 用 κ = 2y / (x^2 + y^2) 算曲率。
3. 用 δ = atan(Lκ) 算前轮转角。

但是还有一个关键问题没解决：目标点从哪里来？

6. 为什么不能追最近点

直觉上，车辆偏离路径时，好像应该追踪最近的轨迹点。但这样会有两个问题。

第一个问题是目标点太近。车辆如果一直盯着最近点，会产生非常敏感的转向，尤其在高速时容易左右振荡。

第二个问题是最近点可能在车辆旁边甚至身后。车辆追它时可能会打出不自然的方向。

人开车时也不是盯着车头下面的白线，而是看前方一段距离。Pure Pursuit 也是一样：它追踪路径前方的一个点。

这个点叫前瞻目标点。

7. 前瞻距离：Pure Pursuit 的核心参数

前瞻目标点距离车辆多远，由前瞻距离 Ld 决定。

可以把车辆当前位置作为圆心，以 Ld 为半径画一个圆，这个圆叫前瞻圆。参考轨迹与前瞻圆在车辆前方的交点，就是理想的前瞻目标点。

车辆当前位置 P
前瞻距离 Ld
前瞻目标点 T 满足 |PT| = Ld

前瞻距离的大小决定了控制风格。

如果 Ld 较小：

• 车辆看得近。
• 响应更灵敏。
• 低速跟踪精度可能更好。
• 高速时容易振荡。

如果 Ld 较大：

• 车辆看得远。
• 转向更平滑。
• 高速更稳定。
• 弯道中可能切弯或响应迟钝。

所以 Ld 是稳定性和跟踪精度之间的折中。

8. 从连续曲线到离散轨迹

理论上，参考路径是一条连续曲线，前瞻圆和路径可以直接求交。

但工程里，规划模块给控制器的通常是一串离散轨迹点：

P0, P1, P2, ..., Pn

因此，Pure Pursuit 需要把“连续曲线求交”变成“离散点序列上的搜索和插值”。

一个常见流程是：

1. 找到车辆当前对应的最近轨迹点。
2. 从最近点开始沿轨迹向前搜索。
3. 找到第一个距离车辆超过 Ld 的轨迹点。
4. 在这个点和它前一个点之间插值，得到真正的前瞻目标点。

第 4 步非常关键。因为离散点通常不会刚好落在前瞻圆上，如果直接使用第一个超过 Ld 的点，目标点会随着采样点跳变，转向也会跳变。

9. 最近点搜索为什么要看距离和朝向

最近点搜索不能只看欧氏距离。

在复杂道路中，轨迹可能出现掉头、回环、相邻车道靠得很近等情况。某个轨迹点空间距离车辆很近，但方向可能完全相反。

因此工程上通常同时限制：

• 距离不能超过阈值。
• 轨迹点朝向与车辆朝向的差值不能超过阈值。

这样可以避免匹配到“空间上近但语义上不属于当前行驶方向”的轨迹点。

如果找不到满足条件的最近点，说明车辆状态和参考轨迹已经不匹配，当前周期应认为 Pure Pursuit 求解失败。

10. 寻找离散前瞻点

找到最近点后，从该点开始沿轨迹向前搜索。

对于每个候选轨迹点，计算它到车辆当前位置的距离：

d = |Pi - Pvehicle|

找到第一个满足：

d > Ld

的点。

这个点不是最终目标点，而是说明“真正的前瞻圆交点在它和前一个点之间”。

设：

A = 前一个距离小于 Ld 的点
B = 第一个距离大于 Ld 的点

那么真正的目标点应该位于线段 AB 上。

11. 前瞻点插值：线段和圆的交点

现在问题变成一个简单的几何问题：

已知车辆位置 P
已知前瞻半径 Ld
已知轨迹线段 A -> B
求线段 AB 与以 P 为圆心、Ld 为半径的圆的交点

设线段方向向量为：

a = B - A

单位方向向量为：

u = a / |a|

车辆位置 P 到线段 AB 的垂足为 H，横向距离为 h。

如果：

|h| > Ld

说明车辆到线段的最短距离都大于前瞻半径，前瞻圆和线段没有交点，插值失败。

如果：

|h| = Ld

说明圆和线段相切，目标点就是垂足 H。

如果：

|h| < Ld

说明圆和线段有两个交点。沿轨迹前进方向取前方那个交点。

根据直角三角形：

s^2 + h^2 = Ld^2

因此：

s = sqrt(Ld^2 - h^2)

目标点为：

T = H + s * u

这一步把离散轨迹点变成了连续目标点。目标点不再受轨迹采样间隔直接限制，控制输出会更平滑。

12. 横向误差的几何意义

上面用到的 h 就是车辆到轨迹线段的横向误差。

设轨迹线段为 A -> B，车辆位置为 P：

a = B - A
b = P - A

二维叉积为：

a × b = ax * by - ay * bx

横向误差为：

h = (a × b) / |a|

它的绝对值表示车辆到线段的垂直距离，符号表示车辆在线段左侧还是右侧。

横向误差在 Pure Pursuit 里有两个作用：

• 在前瞻点插值中判断前瞻圆和轨迹线段是否相交。
• 在动态前瞻距离中帮助车辆平顺地回到轨迹。

13. 到这里，基础 Pure Pursuit 已经完整了

把前面的部分串起来，一个基础 Pure Pursuit 控制周期是：

1. 接收当前车辆位姿和参考轨迹。
2. 找到最近轨迹点。
3. 从最近点向前搜索离散前瞻点。
4. 在线段上插值得到真正前瞻目标点。
5. 把目标点转换到车辆坐标系。
6. 用 κ = 2y / (x^2 + y^2) 算目标曲率。
7. 用 δ = atan(Lκ) 算前轮转角。
8. 限制最大转角并输出。

这就是 Pure Pursuit 的主干。

接下来讨论工程里为什么还需要更多处理。

14. 动态前瞻距离：让算法适应速度和道路形状

如果 Ld 固定，车辆在低速、高速、直道、急弯、偏离轨迹较大时都会使用同一个前瞻距离。这通常不是最优的。

工程中常使用动态前瞻距离：

Ld = clamp(Lv + Lc + Le, Lmin, Lmax)

其中：

• Lv 是速度项。
• Lc 是曲率项。
• Le 是横向误差项。
• Lmin 是最小前瞻距离。
• Lmax 是最大前瞻距离。

14.1 速度项

速度越高，前瞻距离应该越大：

Lv = kv |v|

高速度下看得太近，车辆会频繁修正方向，容易振荡。看得远一些，控制会更平顺。

这里通常用速度绝对值，因为前进和倒车都需要考虑速度大小。

14.2 曲率项

道路越弯，前瞻距离应该适当缩短：

Lc = -kc |κref|

急弯时如果看得太远，目标点会落在弯道更后方，车辆可能倾向于切弯。缩短前瞻距离后，目标点更贴近局部弯道，车辆会更积极地转入弯内。

14.3 横向误差项

车辆偏离轨迹较大时，前瞻距离可以适当增大：

Le = ke |e|

这听起来有点反直觉。偏离大时不是应该更快切回去吗？

如果车辆以很大转角快速切回轨迹，容易过冲和振荡。增大前瞻距离可以让车辆选择更平滑的回归路径。

通常只有当横向误差超过阈值时才启用这一项：

if |e| >= eth:
    Le = ke |e|
else:
    Le = 0

14.4 前瞻距离的上下限

最终结果需要限制：

Ld = clamp(Ld, Lmin, Lmax)

最小值防止车辆看得太近导致抖动。

最大值防止车辆看得太远导致转向迟钝。

倒车时通常还会使用单独的最小前瞻距离，因为倒车控制比前进更敏感。

15. 参考轨迹曲率如何影响前瞻距离

动态前瞻距离中的 κref 是参考轨迹局部曲率，不是 Pure Pursuit 最终输出的目标曲率。

一种常见估计方式是：在最近轨迹点前后各取一个点，使用三个点估计局部圆弧曲率。

设：

Pprev, Pcurr, Pnext

这三个点越接近同一圆弧，估计出的曲率越可靠。

如果车辆接近轨迹起点或终点，前后点不够，曲率估计会变得不可靠。工程上可以选择返回 0，或者使用其他边界处理方式。

这个曲率主要用于调节前瞻距离，而最终控制曲率仍然来自前瞻目标点。

16. 轨迹为什么要重采样

规划模块输出的轨迹点间距可能不均匀。

有些地方点很密，有些地方点很稀。如果直接使用，会带来问题：

• 最近点搜索不稳定。
• 曲率估计受点间距影响。
• 前瞻点搜索容易跳变。
• 预测轨迹步长和参考轨迹尺度不一致。

因此工程中常按固定弧长间隔重采样轨迹。

假设轨迹总弧长为 S，采样间隔为 ds，新的采样位置为：

0, ds, 2ds, 3ds, ..., S

重采样后，轨迹点空间分布更均匀，Pure Pursuit 的搜索和曲率估计都会更稳定。

17. 轨迹为什么要平滑

轨迹可能存在局部抖动，比如位置锯齿、速度突变、朝向不连续。这些小抖动会被控制器放大成方向盘抖动。

一个简单有效的平滑方式是移动平均。

对第 i 个点，取它前后各 M 个点：

i - M, ..., i, ..., i + M

然后对位置、速度、加速度等字段求平均：

p'i = mean(p(i-M), ..., p(i), ..., p(i+M))

靠近轨迹起点或终点时，窗口一侧没有足够点，需要缩小窗口。

平滑不是为了改变轨迹整体形状，而是降低局部噪声。

朝向平滑要更小心。四元数不能直接逐分量平均。常见做法是先转换成 yaw，再平均 yaw，最后转回四元数。

如果 yaw 接近 +π 和 -π 边界，普通平均会出错。更稳健的角度平均方式是：

yaw_avg = atan2(mean(sin(yaw)), mean(cos(yaw)))

18. 前进和倒车需要分开处理

车辆前进和倒车时，目标点应该位于不同方向。

前进时，目标点应该在车辆前方：

x > 0

倒车时，车辆沿后方运动，目标点应该在车辆后方：

x < 0

如果不区分方向，倒车时可能会追踪到车辆前方点，导致控制方向异常。

倒车还常使用不同的最小前瞻距离，因为倒车时车辆姿态变化更敏感。

19. 求解失败时怎么办

Pure Pursuit 不是每一帧都一定能成功。

失败原因可能包括：

• 轨迹点太少。
• 找不到最近轨迹点。
• 找不到合适的前瞻点。
• 前瞻圆和轨迹线段没有交点。
• 车辆偏离轨迹过远。

控制器不能因为某一帧失败就输出随机方向。常见降级策略是：

1. 当前周期成功时，输出新转角并缓存。
2. 当前周期失败但有上一帧有效指令时，沿用上一帧。
3. 刚启动且没有历史有效指令时，输出 0 曲率，尽量直行。

这样可以避免方向盘突然跳变。

20. 转向收敛判断

控制器输出目标转角，但车辆实际前轮转角不会瞬间达到目标。

因此可以判断：

|δcmd - δactual| < δthreshold

如果满足，就认为转向已经收敛。

这个标志可以提供给纵向控制或行为状态机。例如车辆可能希望横向姿态稳定后再加速。

21. 预测轨迹为什么存在

Pure Pursuit 除了输出当前转角，还常生成一条预测轨迹。

预测轨迹回答的是：

如果车辆继续按照当前控制器逻辑行驶，未来一小段路径大概是什么样？

它常用于：

• 可视化控制器行为。
• 调试前瞻距离和曲率。
• 检查短期运动趋势。
• 给其他模块提供参考。

22. 预测轨迹怎么生成

预测轨迹可以通过滚动外推生成。

设每一步前进距离为 ds，预测长度为 Spred：

N = ceil(Spred / ds)

预测流程：

1. 第一个预测点使用当前真实车辆位姿。
2. 在当前预测点上运行 Pure Pursuit，得到曲率。
3. 把曲率转换成前轮转角。
4. 用运动学模型向前推进 ds。
5. 得到下一个预测点。
6. 在下一个预测点上重复以上过程。

这是一种闭环滚动预测。每一步都会重新找前瞻目标点、重新计算曲率，所以预测轨迹能跟随参考轨迹的弯曲变化。

预测长度通常不能超过：

• 参考轨迹剩余长度。
• 配置的最大预测长度。

因此：

Spred = min(Sremaining, Smax)

23. 单步运动学外推

给定当前预测点、前轮转角 δ、轴距 L、前进距离 ds，可以估计下一点。

自行车模型给出：

κ = tan(δ) / L

这一小段距离内的航向变化为：

Δψ = κ ds

如果当前位姿为：

(x, y, ψ)

则短距离近似为：

xnext = x + ds cos(ψ)
ynext = y + ds sin(ψ)
ψnext = ψ + Δψ

也可以从坐标变换角度理解：

下一点全局位姿 = 当前点全局位姿 * 车辆局部坐标系下的一步运动增量

局部运动增量是：

平移: (ds, 0)
旋转: Δψ

当 ds 足够小时，这种近似适合生成短期预测轨迹。

24. 控制和预测中的速度

速度在 Pure Pursuit 中可能有两个作用：

• 用于计算前瞻距离。
• 作为轨迹点或预测点的速度属性。

实际控制时，前瞻距离通常应考虑当前车辆实际速度，因为车辆此刻的物理状态决定了转向响应。

预测轨迹时，初始点通常使用当前实际速度；后续点可以根据设计使用目标速度或当前速度。关键是要区分“速度影响前瞻距离”和“速度作为轨迹点属性”这两个概念。

有些实现会把预测轨迹最后一个点速度设为 0。这通常只是表示预测到此结束，不代表车辆真的要在该点停车。

25. 完整 Pure Pursuit 工程流程

现在把所有内容按真实控制周期串起来：

1. 接收车辆当前位姿、速度和参考轨迹。
2. 对参考轨迹进行重采样，使点间距更均匀。
3. 可选地对轨迹做平滑，减少局部抖动。
4. 在轨迹上寻找与车辆匹配的最近点。
5. 计算横向误差和参考轨迹局部曲率。
6. 根据速度、曲率、横向误差计算动态前瞻距离。
7. 从最近点沿轨迹向前搜索离散前瞻点。
8. 在线段上插值得到真正前瞻目标点。
9. 把目标点转换到车辆坐标系。
10. 用 κ = 2y / (x^2 + y^2) 计算目标曲率。
11. 用 δ = atan(Lκ) 转换成前轮转角。
12. 限制最大转角。
13. 如果求解成功，输出控制指令并缓存。
14. 如果求解失败，使用降级策略。
15. 可选地生成预测轨迹和调试信息。

这条链路就是 Pure Pursuit 从理论走向工程的完整形态。

26. 参数调节顺序

调 Pure Pursuit 参数时，建议按层次来，不要一开始同时改所有参数。

26.1 先确认轨迹质量

如果轨迹本身抖动、不连续、点间距混乱，控制器参数很难调好。

先检查：

• 轨迹点是否连续。
• 点间距是否合理。
• 是否需要重采样。
• 是否需要平滑。

26.2 再调基础前瞻距离

先用固定或近似固定的前瞻距离观察车辆行为。

低速振荡明显，可以增大最小前瞻距离。

低速弯道跟踪不足，可以减小最小前瞻距离。

26.3 再调速度项

如果高速抖动，增大速度比例，让高速看得更远。

如果高速转向太迟钝，减小速度比例或最大前瞻距离。

26.4 再调曲率项

如果急弯切弯，增大曲率项影响，让急弯前瞻距离更短。

如果急弯转向太激进，减小曲率项影响。

26.5 最后调横向误差项

如果车辆偏离轨迹后回归太猛，增大横向误差项。

如果车辆回归太慢，减小横向误差项或提高触发阈值。

27. Pure Pursuit 的优点

Pure Pursuit 的优点是：

• 几何意义清晰。
• 数学推导简单。
• 计算量小。
• 实时性好。
• 参数相对少。
• 对平滑轨迹和中高速道路表现稳定。

这也是它在自动驾驶入门和工程系统中长期流行的原因。

28. Pure Pursuit 的局限

Pure Pursuit 也有明显局限：

• 不直接优化控制量。
• 不显式考虑轮胎侧偏和车辆动力学。
• 不直接约束目标点处的车辆朝向。
• 对前瞻距离较敏感。
• 低速大曲率、泊车、狭窄空间中可能表现不足。
• 轨迹噪声会直接影响控制输出。

因此，在复杂低速场景或强约束场景中，可能需要 Stanley、MPC 或其他更复杂的控制方法。

29. 一句话总结

Pure Pursuit 的学习路径可以概括为：

追一个前方目标点
-> 用圆弧连接车辆和目标点
-> 推导曲率 κ = 2y / (x^2 + y^2)
-> 用自行车模型转成前轮转角
-> 在离散轨迹上搜索和插值得到目标点
-> 用动态前瞻距离提升稳定性和适应性
-> 加入重采样、平滑、降级和预测轨迹形成工程控制器

真正理解 Pure Pursuit，不是记住某个公式，而是理解它为什么要“看向前方”、为什么用圆弧、为什么前瞻距离决定控制风格，以及真实轨迹离散化后为什么必须做搜索和插值。