Pure Pursuit 纯跟踪算法#

1. 算法思想#

纯跟踪（Pure Pursuit）是一种基于几何的路径跟踪控制方法。其核心思想是：在车辆前方一定距离处选取一个预瞄点（lookahead point），然后控制车辆朝向该点行驶。通过调节预瞄距离，可以平衡响应速度与轨迹平滑度。

预瞄距离 $L$ 定义为车辆当前位置到预瞄点的直线距离。

当前点 $P$ 到路径上任意点 $P_i$ 的距离： $$d = \sqrt{(x_i - x)^2 + (y_i - y)^2}$$

固定预瞄距离：适用于低速或简单场景。

动态预瞄距离：常用经验公式 $$L = k \cdot v + L_{d0}$$

其中： - $k$：速度前馈系数（通常在 0.5 ~ 1.5 之间） - $v$：车速 (m/s) - $L_{d0}$：截距/最小预瞄距离（通常取 0.3 ~ 0.5 m）

高速时需要更大的预瞄距离以保证稳定性。

设当前车辆位置为 $P(x, y)$，朝向角为 $\theta$，预瞄点为 $L(x_L, y_L)$。

计算当前点到预瞄点的向量在车体坐标系下的表示： $$\alpha = \text{atan2}(y_L - y, x_L - x) - \theta$$
由几何关系（弦长公式），转向角满足： $$\sin(\alpha) = \frac{L \cdot \sin(\alpha)}{L} = \frac{L_d}{L}$$

其中 $L_d$ 是当前点到预瞄点的距离（近似为 $L$）。

\[\boxed{\omega = \frac{2 v \sin(\alpha)}{L}}\]

参数	增大时的效果
$L$（预瞄距离）	响应变慢，轨迹更平滑，稳态误差增大
$v$（速度）	响应加快，可能导致振荡或失稳

见 01.pure_pursuit_sim.ipynb

参数	增大时的效果
\(L\)（预瞄距离）	响应变慢，轨迹更平滑，稳态误差增大
\(v\)（速度）	响应加快，可能导致振荡或失稳