定义关键问题
在深入纯跟踪算法核心前，必须澄清一对容易被混淆但至关重要的概念：
| 概念 | 坐标系 | 物理意义 | 计算方式 |
|---|---|---|---|
| 航向角偏差(α_global) | 全局坐标系 | 车辆航向与预瞄点方向的夹角 | 预瞄点方位角 - 车辆航向角 |
| 预瞄角(α_body) | 车身坐标系 | 预瞄点相对于车辆纵轴的夹角 | 车身坐标下的反正切计算 |
关键疑问：为什么文献中对二者的论述常常混用？是否真的 α_global ≡ α_body？
一、核心公式与参数
1. 纯跟踪控制公式
本部分展示了车辆纯跟踪控制中常用的数学公式。我们通过这些公式计算车辆的转向角度，并根据车速、预瞄点的位置等因素来调节转向。以下是核心公式：
δ = arctan ( 2 L ⋅ sin ( α ) l d ) , l d = k v ⋅ v + l o \delta = \arctan\left(\frac{2L \cdot \sin(\alpha)}{l_{\text{d}}}\right), \ l_{\text{d}} = k_v \cdot v + l_{\text{o}} δ=arctan(ld2L⋅sin(α)), ld=kv⋅v+lo
其中， α \alpha α 表示航向角偏差，通过以下公式计算：
α = arctan ( y r − y x r − x ) − ψ \alpha = \arctan\left(\frac{y_{\text{r}}-y}{x_{\text{r}}-x}\right) - \psi α=arctan(xr−xyr−y)−ψ
2. 参数说明表
| 符号 | 含义 | 单位 | 典型值 | 补充说明 |
|---|---|---|---|---|
| δ \delta δ | 前轮转角 | rad | ±0.52 (±30°) | |
| L L L | 车辆轴距 | m | 2.5~3.5 | 轿车约2.7m，SUV约2.9m |
| α \alpha α | 航向角偏差 | rad | - | 即预瞄角 |
| l d l_d ld | 前视距离 | m | 3~20 | 速度相关 |
| k v k_v kv | 速度增益 | s | 0.5~1.0 | 调参重点 |
| l 0 l_0 l0 | 最小视距 | m | 2~3 | 保证低速性能 |
| x , y x,y x,y | 车辆坐标 | m | - | 全局坐标系 |
| x r , y r x_{\text{r}}, y_{\text{r}} xr,yr | 预瞄点坐标 | m | - | |
| ψ \psi ψ | 车辆航向角 | rad | - | 全局坐标系 |
二、等价性证明
在这一部分，我们将讨论全局坐标系中航向角偏差与车身坐标系中预瞄角的计算公式，并证明这两个坐标系中的角度计算是等价的。
1. 全局坐标系计算—航向角偏差
设车辆状态向量为 S t a t e = [ x , y , ψ ] T \mathbf{State} = [x, y, \psi]^T State=[x,y,ψ]T，其中：
- ( x , y ) (x, y) (x,y)为车辆在全局坐标系中的位置
- ψ \psi ψ为车辆航向角（全局坐标系）
给定预瞄点坐标 ( x t , y t ) (x_t, y_t) (xt,yt)，则航向角偏差定义为：
α global = arctan 2 ( y t − y , x t − x ) ⏟ 预瞄点方位角 − ψ \alpha_{\text{global}} = \underbrace{\arctan2(y_t - y, x_t - x)}_{\text{预瞄点方位角}} - \psi αglobal=预瞄点方位角 arctan2(yt−y,xt−x)−ψ
物理意义： α global \alpha_{\text{global}} αglobal表示车辆当前航向与指向预瞄点方向之间的夹角
2. 车身坐标系计算—预瞄角
将预瞄点从全局坐标系转换到车身坐标系的变换过程：
① 坐标平移：以车辆位置为坐标原点
Δ x = x t − x Δ y = y t − y \Delta x = x_t - x \\ \Delta y = y_t - y Δx=xt−xΔy=yt−y
② 坐标旋转：消除航向角影响
[ x body y body ] = R ( ψ ) ⋅ [ Δ x Δ y ] = [ cos ψ sin ψ − sin ψ cos ψ ] [ Δ x Δ y ] \begin{bmatrix} x_{\text{body}} \\ y_{\text{body}} \end{bmatrix} = \mathbf{R}(\psi) \cdot \begin{bmatrix} \Delta x \\ \Delta y \end{bmatrix} = \begin{bmatrix} \cos\psi & \sin\psi \\ -\sin\psi & \cos\psi \end{bmatrix} \begin{bmatrix} \Delta x \\ \Delta y \end{bmatrix} [xbodyybody]=R(ψ)⋅[ΔxΔy]=[cosψ−sinψsinψcosψ][ΔxΔy]
③ 预瞄角计算：
α body = arctan 2 ( y body , x body ) \alpha_{\text{body}} = \arctan2(y_{\text{body}}, x_{\text{body}}) αbody=arctan2(ybody,xbody)
物理意义： α body \alpha_{\text{body}} αbody表示在车辆自身视角下预瞄点与车头正方向的夹角
3. 等价性数学证明
4.等价性结论
α global ≡ α body \alpha_{\text{global}} \equiv \alpha_{\text{body}} αglobal≡αbody
三、Python代码实现：
1. 等价性验证代码
import numpy as npdef calculate_angles(ego_x, ego_y, ego_yaw, target_x, target_y):"""验证航向角偏差与预瞄角的等价性"""# 全局坐标系计算dx = target_x - ego_xdy = target_y - ego_ytarget_yaw = np.arctan2(dy, dx)yaw_error = target_yaw - ego_yawyaw_error = np.arctan2(np.sin(yaw_error), np.cos(yaw_error)) # 归一化# 车身坐标系计算rotation_matrix = np.array([[np.cos(ego_yaw), np.sin(ego_yaw)],[-np.sin(ego_yaw), np.cos(ego_yaw)]])dx_body, dy_body = rotation_matrix @ np.array([dx, dy])preview_angle = np.arctan2(dy_body, dx_body)# 计算差异(应≈0)angle_diff = np.degrees(np.abs(yaw_error - preview_angle))return yaw_error, preview_angle, angle_diff
2. 完整纯跟踪控制器
class PurePursuitController:def __init__(self, wheelbase, k_v=0.6, l0=3.0, max_steer_deg=30.0):"""纯跟踪控制器:param wheelbase: 车辆轴距(m):param k_v: 速度前视增益:param l0: 基础前视距离(m):param max_steer_deg: 最大转向角(度)"""self.L = wheelbaseself.k_v = k_vself.l0 = l0self.max_steer = np.radians(max_steer_deg)def get_steering_angle(self, v, ego_x, ego_y, ego_yaw, target_x, target_y):"""计算转向角:param v: 当前车速(m/s):param ego_x, ego_y: 车辆位置:param ego_yaw: 车辆航向(rad):param target_x, target_y: 预瞄点位置:return: 转向角(rad)"""# 1. 计算动态前视距离lookahead = max(self.l0, self.k_v * v) # 保证最小前视距离# 2. 计算航向角偏差(即预瞄角)dx = target_x - ego_xdy = target_y - ego_ytarget_yaw = np.arctan2(dy, dx)alpha = target_yaw - ego_yawalpha = np.arctan2(np.sin(alpha), np.cos(alpha)) # 归一化到[-π, π]# 3. 纯跟踪公式steering = np.arctan(2 * self.L * np.sin(alpha) / lookahead)# 4. 转向限幅return np.clip(steering, -self.max_steer, self.max_steer)
四、应用场景说明
预瞄距离调参经验：
| 场景 | k v k_v kv | l 0 l_0 l0 | 效果 |
|---|---|---|---|
| 城市道路 | 0.3~0.5 | 3m | 平滑转弯 |
| 高速公路 | 0.8~1.0 | 5m | 提前预判 |
| 泊车场景 | 0.1~0.3 | 2m | 精准控制 |
五、常见问题解答
Q: 为什么不直接使用距离偏差代替角度偏差？
角度偏差具有方向敏感性，当车辆偏离路径时：
- 角度偏差：能立即反映偏离方向（左/右）
- 距离偏差：不能反映偏离方向
Q: 如何防止预瞄点跳变？
采用路径点筛选策略：# 选择最近路径点前方lookahead距离的点 min_idx = np.argmin(np.sqrt((path_x-ego_x)**2 + (path_y-ego_y)**2)) target_idx = min_idx + int(lookahead / point_interval)
六、算法计算逻辑示意图
当然，航向角偏差如果是用预瞄点计算的，那么两者就是等价的，否则根据实际情况判断
七、参考
自动驾驶控制算法——纯跟踪算法（Pure Pursuit）
)
