定義關鍵問題

在深入純跟蹤算法核心前，必須澄清一對容易被混淆但至關重要的概念：

概念	坐標系	物理意義	計算方式
航向角偏差(α_global)	全局坐標系	車輛航向與預瞄點方向的夾角	預瞄點方位角 - 車輛航向角
預瞄角(α_body)	車身坐標系	預瞄點相對于車輛縱軸的夾角	車身坐標下的反正切計算

關鍵疑問：為什么文獻中對二者的論述常常混用？是否真的 α_global ≡ α_body？

一、核心公式與參數

1. 純跟蹤控制公式

本部分展示了車輛純跟蹤控制中常用的數學公式。我們通過這些公式計算車輛的轉向角度，并根據車速、預瞄點的位置等因素來調節轉向。以下是核心公式：
$$\delta =\arctan \left(\frac{2L?\sin (\alpha )}{l_{\text{d}}}\right),l_{\text{d}}=k_v?v+l_{\text{o}}$$
其中，$\alpha$ 表示航向角偏差，通過以下公式計算：
$$\alpha =\arctan \left(\frac{y_{\text{r}}?y}{x_{\text{r}}?x}\right)?\psi$$

2. 參數說明表

符號	含義	單位	典型值	補充說明
$\delta$	前輪轉角	rad	±0.52 (±30°)
$L$	車輛軸距	m	2.5~3.5	轎車約2.7m，SUV約2.9m
$\alpha$	航向角偏差	rad	-	即預瞄角
$l_d$	前視距離	m	3~20	速度相關
$k_v$	速度增益	s	0.5~1.0	調參重點
$l_0$	最小視距	m	2~3	保證低速性能
$x,y$	車輛坐標	m	-	全局坐標系
$x_{\text{r}},y_{\text{r}}$	預瞄點坐標	m	-
$\psi$	車輛航向角	rad	-	全局坐標系

二、等價性證明

在這一部分，我們將討論全局坐標系中航向角偏差與車身坐標系中預瞄角的計算公式，并證明這兩個坐標系中的角度計算是等價的。

1. 全局坐標系計算—航向角偏差

設車輛狀態向量為$\mathbf{State}=[x,y,\psi {]}^T$，其中：

• $(x,y)$為車輛在全局坐標系中的位置
• $\psi$為車輛航向角（全局坐標系）

給定預瞄點坐標$(x_t,y_t)$，則航向角偏差定義為：
$${\alpha }_{\text{global}}=\underset{\text{預瞄點方位角}}{\underbrace{\arctan 2(y_t?y,x_t?x)}}?\psi$$

物理意義：${\alpha }_{\text{global}}$表示車輛當前航向與指向預瞄點方向之間的夾角

2. 車身坐標系計算—預瞄角

將預瞄點從全局坐標系轉換到車身坐標系的變換過程：
① 坐標平移：以車輛位置為坐標原點
$$\begin{gathered} \Delta x=x_t?x \\ \Delta y=y_t?y \end{gathered}$$
② 坐標旋轉：消除航向角影響
$$\left[\begin{array}{c}{x}_{\text{body}}\\ y_{\text{body}}\end{array}\right]=\mathbf{R}(\psi )?\left[\begin{array}{c}\Delta x\\ \Delta y\end{array}\right]=\left[\begin{array}{cc}\cos \psi & \sin \psi \\ ?\sin \psi & \cos \psi \end{array}\right]\left[\begin{array}{c}\Delta x\\ \Delta y\end{array}\right]$$

③ 預瞄角計算：
$${\alpha }_{\text{body}}=\arctan 2(y_{\text{body}},x_{\text{body}})$$

物理意義：${\alpha }_{\text{body}}$表示在車輛自身視角下預瞄點與車頭正方向的夾角

3. 等價性數學證明

4.等價性結論

$${\alpha }_{\text{global}}\equiv {\alpha }_{\text{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$	$l_0$	效果
城市道路	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）