【路径跟踪控制：Pure Pursuit控制与车辆运动学模型】

1. 引言

建议系统学习以下博客：
【【自动驾驶】车辆运动学模型】
【路径跟踪控制：Bang-Bang 控制与车辆运动学模型】
【路径跟踪控制：PID控制与车辆运动学模型】
【路径跟踪控制：Pure Pursuit控制与车辆运动学模型】

论文：Implementation of the Pure Pursuit Path Tracking Algorithm
路径跟踪控制是自动驾驶领域的一个重要组成部分，其目的是使车辆沿着预定的路径行驶。本文将介绍一种常用的路径跟踪控制方法——Pure Pursuit 控制，并结合车辆运动学模型进行实现。通过 Python 代码示例，我们将展示如何使用 Pure Pursuit 控制器使车辆沿给定路径行驶。

2. 车辆运动学模型

车辆运动学模型描述了车辆的位置、速度和方向随时间的变化。对于两轮模型（参考【【自动驾驶】车辆运动学模型】），车辆的状态可以用以下四个参数表示：

• $x$：车辆在 x 轴上的位置
• $y$：车辆在 y 轴上的位置
• $\psi$：车辆的航向角（即车头指向的方向）
• $v$：车辆的速度
  

车辆的运动可以通过以下方程组描述：
$$\begin{array}{rl}\dot{x}& =v\cos (\psi )\\ \dot{y}& =v\sin (\psi )\\ \dot{\psi }& =\frac{v}{L}\tan ({\delta }_f)\\ \dot{v}& =a\end{array}\text{\hspace{1em}(1)}$$
其中：

• $L$ 是车辆的轴距
• ${\delta }_f$ 是前轮的转向角
• $a$ 是车辆的加速度

3. Pure Pursuit 控制原理

Pure Pursuit 控制是一种基于几何的路径跟踪方法。其基本思想是选择路径上的一个前视目标点（lookahead point），并通过调整车辆的转向角使其朝向前视目标点行驶。前视目标点的选择通常基于当前车辆位置和前视距离 $l_d$。

在三角形中，运用正弦定理：

$$\frac{sin(\frac{\pi }{2}-\alpha )}{R}=\frac{sin(2\alpha )}{l_d}\text{\hspace{1em}(2)}$$
化简：
$$\frac{cos(\alpha )}{R}=\frac{2sin(\alpha )cos(\alpha )}{l_d}\text{\hspace{1em}(3)}$$
$$R=\frac{l_d}{2sin(\alpha )}\text{\hspace{1em}(4)}$$
以后轴中心为车辆中心的单车模型特点：
$$tan(\delta )=\frac{L}{R}\text{\hspace{1em}(5)}$$
联合公式(4)(5)，消去$R$,转向角 $\delta$ (和${\delta }_f$一样）的计算公式为：
$$\delta =arctan\left(\frac{2Lsin(\alpha )}{l_d}\right)\text{\hspace{1em}(6)}$$
其中：

• $\alpha$ 是前视目标点与车辆当前位置之间的相对航向角
  前视距离 $l_d$ 可以根据车辆的速度动态调整：
  $$l_d=\lambda v+c\text{\hspace{1em}(7)}$$
• $\lambda$ 是前视距离系数
• $c$ 是前视距离常数，一般取车长长度

4. 实现代码

以下是使用 Python 实现 Pure Pursuit 控制的完整代码：

from scipy.spatial import KDTree
from matplotlib.animation import FuncAnimation
import numpy as np
import matplotlib.pyplot as plt
import math# 定义车辆参数
L = 2  # 车辆轴距，单位：m
v = 2  # 初始速度
x_0 = 0  # 初始x
y_0 = -3  # 初始y
psi_0 = 0  # 初始航向角
dt = 0.1  # 时间间隔，单位：s
lam = 0.1  # 前视距离系数
c = 2  # 前视距离常数# 车辆运动学模型类
class KinematicModel_3:def __init__(self, x, y, psi, v, L, dt):self.x = xself.y = yself.psi = psiself.v = vself.L = Lself.dt = dtdef update_state(self, a, delta_f):self.x += self.v * math.cos(self.psi) * self.dtself.y += self.v * math.sin(self.psi) * self.dtself.psi += (self.v / self.L) * math.tan(delta_f) * self.dtself.v += a * self.dtdef get_state(self):return self.x, self.y, self.psi, self.v# 计算前视目标点
def cal_target_index(robot_state, refer_path, l_d):tree = KDTree(refer_path)_, min_index = tree.query(robot_state)while l_d > np.linalg.norm(refer_path[min_index] - robot_state) and (min_index + 1) < len(refer_path):min_index += 1return min_index# Pure Pursuit 控制器
def pure_pursuit_control(robot_state, current_ref_point, l_d, psi):alpha = math.atan2(current_ref_point[1] - robot_state[1], current_ref_point[0] - robot_state[0]) - psidelta = math.atan2(2 * L * math.sin(alpha), l_d)return delta# 主函数
def main(interactive=True):# 设置参考轨迹refer_path = np.zeros((1000, 2))refer_path[:, 0] = np.linspace(0, 100, 1000)refer_path[:, 1] = 2 * np.sin(refer_path[:, 0] / 3.0) + 2.5 * np.cos(refer_path[:, 0] / 2.0)# 初始化车辆状态ugv = KinematicModel_3(x_0, y_0, psi_0, v, L, dt)trajectory_x, trajectory_y = [], []# 创建绘图窗口fig, ax = plt.subplots()ax.plot(refer_path[:, 0], refer_path[:, 1], '-.b', linewidth=1.0, label="Reference Path")line, = ax.plot([], [], '-r', label="UGV Trajectory")current_dot, = ax.plot([], [], 'bo', label="Current")target_dot, = ax.plot([], [], 'go', label="Target")ax.set_title("Pure Pursuit Control")ax.set_xlabel("x")ax.set_ylabel("y")ax.set_xlim(-1, 101)ax.set_ylim(-6, 6)ax.grid(True)ax.legend()def init():line.set_data([], [])current_dot.set_data([], [])target_dot.set_data([], [])return line, current_dot, target_dotdef update(frame):nonlocal ugvrobot_state = np.array([ugv.x, ugv.y])l_d = lam * ugv.v + cind = cal_target_index(robot_state, refer_path, l_d)delta = pure_pursuit_control(robot_state, refer_path[ind], l_d, ugv.psi)ugv.update_state(0, delta)  # 加速度设为0，恒速trajectory_x.append(ugv.x)trajectory_y.append(ugv.y)line.set_data(trajectory_x, trajectory_y)current_dot.set_data([ugv.x], [ugv.y])target_dot.set_data([refer_path[ind, 0]], [refer_path[ind, 1]])return line, current_dot, target_dot# 创建动画anim = FuncAnimation(fig, update, frames=700, init_func=init, blit=True, interval=20, repeat=False)if interactive:plt.show()else:import matplotlibmatplotlib.use('Agg')  # 非交互模式下，使用非GUI后端anim.save('Control/Controllers_python/Pure_pursuit/pure_pursuit_trajectory.gif', writer='pillow', fps=20)print("GIF 已保存")if __name__ == '__main__':interactive_mode = False  # 设置为 True 以在交互式模式下运行，False 以保存 GIFmain(interactive=interactive_mode)

5. 结果与分析

运行上述代码后，将生成一个动画，展示车辆沿参考路径行驶的过程。动画中可以看到：

• 蓝色虚线表示参考路径
• 红色实线表示车辆的实际轨迹
• 蓝色圆点表示车辆的当前位置
• 绿色圆点表示前视目标点
  

通过观察动画，可以验证 Pure Pursuit 控制器的有效性。车辆能够沿着预定的路径平稳行驶，且前视目标点的选择合理，确保了路径跟踪的精度。

6. 串联学习

PID、Pure Pursuit 和 Bang-Bang 控制

import numpy as np
import matplotlib.pyplot as plt
import math
from matplotlib.animation import FuncAnimation# 初始化参考路径
refer_path = np.zeros((1000, 2))
# 车辆初始状态
x0, y0, psi0, v0 = 0, 2, 0, 1
L = 2  # 车轮间距
dt = 0.1  # 时间间隔
delta_limit = np.deg2rad(30)  # 最大转向角度限制# 车辆运动学模型类
class KinematicModel:def __init__(self, x, y, psi, v, L, dt):self.x = xself.y = yself.psi = psiself.v = vself.L = Lself.dt = dtdef update_state(self, a, delta_f):self.x += self.v * np.cos(self.psi) * self.dtself.y += self.v * np.sin(self.psi) * self.dtself.psi += (self.v / self.L) * np.tan(delta_f) * self.dtself.v += a * self.dtdef get_state(self):return self.x, self.y, self.psi, self.v# PID 控制器类
class PIDController:def __init__(self, Kp, Ki, Kd, dt):self.Kp = Kpself.Ki = Kiself.Kd = Kdself.dt = dtself.prev_error = 0self.integral = 0def control(self, robot_state, current_ref_point, psi):error = math.atan2(current_ref_point[1] - robot_state[1], current_ref_point[0] - robot_state[0]) - psiself.integral += error * self.dtderivative = (error - self.prev_error) / self.dtoutput = self.Kp * error + self.Ki * self.integral + self.Kd * derivativeself.prev_error = errorreturn output# Pure Pursuit 控制器类
class PurePursuitController:def __init__(self, L, lam, c):self.L = Lself.lam = lamself.c = cdef control(self, robot_state, current_ref_point, psi):alpha = math.atan2(current_ref_point[1] - robot_state[1], current_ref_point[0] - robot_state[0]) - psil_d = self.lam * robot_state[2] + self.cdelta = math.atan2(2 * self.L * math.sin(alpha), l_d)return delta# Bang-Bang 控制器类
class BangBangController:def __init__(self, tolerance, max_delta):self.tolerance = toleranceself.max_delta = max_deltadef control(self, robot_state, current_ref_point, psi):dx, dy = current_ref_point - robot_state[:2]alpha = math.atan2(dy, dx)l_d = np.linalg.norm(current_ref_point - robot_state[:2])theta_e = alpha - psierror_y = l_d * math.sin(theta_e)# 根据误差调整最大转向角max_delta = self.max_delta if abs(theta_e) > np.pi / 2.0 else self.max_delta / 2.0# Bang-Bang 控制delta_f = np.sign(error_y) * max_delta if abs(error_y) > self.tolerance else 0return delta_f# 公共更新函数
def update_common(ugv, controller, refer_path, iadd, delta_limit, dt, trajectory_x, trajectory_y, line, current_dot, delta_f_line, target_dot, reached_end):robot_state = np.array([ugv.x, ugv.y, ugv.v])current_ref_point = refer_path[iadd]# 计算控制输入delta_f = controller.control(robot_state, current_ref_point, ugv.psi)delta_f = np.clip(delta_f, -delta_limit, delta_limit)l_d = np.linalg.norm(current_ref_point - robot_state[:2])# 固定速度a = l_d * np.sin(delta_f)/50  # 可以根据需要调整速度控制# 更新车辆状态ugv.update_state(a, delta_f)# 判断是否到达终点if iadd == refer_path.shape[0] - 1 and abs(l_d) < 0.2:  # 到达终点且距离误差小于 0.2ugv.v = 0a = 0reached_end = Trueprint(f"{controller.__class__.__name__} 控制到达终点")else:# 更新轨迹trajectory_x.append(ugv.x)trajectory_y.append(ugv.y)line.set_data(trajectory_x, trajectory_y)# 更新当前点current_dot.set_data([ugv.x], [ugv.y])# 更新转向角曲线delta_f_line.set_data([ugv.x, ugv.x + 1 * np.cos(ugv.psi + delta_f)], [ugv.y, ugv.y + 1 * np.sin(ugv.psi + delta_f)])# 更新目标点target_dot.set_data([current_ref_point[0]], [current_ref_point[1]])return line, current_dot, delta_f_line, target_dot, reached_end# 主函数
def main(interactive=True):global refer_path, iadd  # 确保这些变量被声明为全局变量# 初始化车辆状态ugv_pid = KinematicModel(x0, y0, psi0, v0, L,dt)ugv_pp = KinematicModel(x0, y0, psi0, v0, L, dt)ugv_bb = KinematicModel(x0, y0, psi0, v0, L, dt)trajectory_x_pid, trajectory_y_pid = [], []trajectory_x_pp, trajectory_y_pp = [], []trajectory_x_bb, trajectory_y_bb = [], []# 创建绘图窗口fig, ax = plt.subplots()ax.plot(refer_path[:, 0], refer_path[:, 1], '-.b', linewidth=1.0, label="Reference Path")line_pid, = ax.plot([], [], '-r', label="PID Trajectory")line_pp, = ax.plot([], [], '-g', label="PP Trajectory")line_bb, = ax.plot([], [], '-m', label="BB Trajectory")current_dot_pid, = ax.plot([], [], 'bo', label="PID Current")current_dot_pp, = ax.plot([], [], 'go', label="PP Current")current_dot_bb, = ax.plot([], [], 'mo', label="BB Current")delta_f_line_pid, = ax.plot([], [], '-y', label=r"PID $\delta_f$")delta_f_line_pp, = ax.plot([], [], '-c', label=r"PP $\delta_f$")delta_f_line_bb, = ax.plot([], [], '-k', label=r"BB $\delta_f$")target_dot_pid, = ax.plot([], [], 'go', label="Target")ax.set_title("PID vs Pure Pursuit vs Bang-Bang Trajectory Control")ax.set_xlabel("x")ax.set_ylabel("y")ax.set_xlim(-1, 110)ax.set_ylim(-5, 6)ax.grid(True)ax.legend()# 控制器参数Kp = 20  # 航向角PID控制器比例增益Ki = 0.05  # 航向角PID控制器积分增益Kd = 0.05  # 航向角PID控制器微分增益lam = 0.5  # 前视距离系数c = 0.5  # 车头到参考路径的距离tolerance = 0.01max_delta = np.deg2rad(30)pid_controller = PIDController(Kp, Ki, Kd, dt)pp_controller = PurePursuitController(L, lam, c)bb_controller = BangBangController(tolerance, max_delta)def init():line_pid.set_data([], [])line_pp.set_data([], [])line_bb.set_data([], [])current_dot_pid.set_data([], [])current_dot_pp.set_data([], [])current_dot_bb.set_data([], [])delta_f_line_pid.set_data([], [])delta_f_line_pp.set_data([], [])delta_f_line_bb.set_data([], [])target_dot_pid.set_data([], [])return line_pid, line_pp, line_bb, current_dot_pid, current_dot_pp, current_dot_bb, delta_f_line_pid, delta_f_line_pp, delta_f_line_bb, target_dot_piddef update(frame):global iadd, refer_path  # 确保使用全局变量# 判断 iadd 是否超出 refer_path 的长度，适当添加路径点if iadd + 1 > refer_path.shape[0]:refer_path = np.resize(refer_path, (refer_path.shape[0] + 1, 2))  # 增加一行refer_path[iadd, 0] = refer_path[iadd - 1, 0]  # 增加新路径点refer_path[iadd, 1] = refer_path[iadd - 1, 1]  # 增加新路径点的 y 坐标# PID 控制_, _, _, _, reached_pid_end = update_common(ugv_pid, pid_controller, refer_path, iadd, delta_limit, dt,trajectory_x_pid, trajectory_y_pid, line_pid, current_dot_pid, delta_f_line_pid, target_dot_pid, False)# Pure Pursuit 控制_, _, _, _, reached_pp_end = update_common(ugv_pp, pp_controller, refer_path, iadd, delta_limit, dt,trajectory_x_pp, trajectory_y_pp, line_pp, current_dot_pp, delta_f_line_pp, target_dot_pid, False)# Bang-Bang 控制_, _, _, _, reached_bb_end = update_common(ugv_bb, bb_controller, refer_path, iadd, delta_limit, dt,trajectory_x_bb, trajectory_y_bb, line_bb, current_dot_bb, delta_f_line_bb, target_dot_pid, False)iadd += 1return line_pid, line_pp, line_bb, current_dot_pid, current_dot_pp, current_dot_bb, delta_f_line_pid, delta_f_line_pp, delta_f_line_bb, target_dot_pid# 创建动画anim = FuncAnimation(fig, update, frames=1050, init_func=init, blit=True, interval=20, repeat=False)if interactive:plt.show()else:anim.save('Control/Controllers_python/Pure_pursuit/PID_vs_PP_vs_BB_trajectory.gif', writer='pillow', fps=20)print("GIF 已保存")# 运行主程序
if __name__ == '__main__':iadd = 0refer_path[:, 0] = np.linspace(0, 100, 1000)refer_path[:, 1] = 2 * np.sin(refer_path[:, 0] / 7.0) + 2.5 * np.cos(refer_path[:, 0] / 5.0)  # 新的参考路径interactive_mode = True  # 设置为 True 以在交互式模式下运行，False 以保存 GIFif not interactive_mode:import matplotlibmatplotlib.use('Agg')main(interactive=interactive_mode)

效果如下：

6.1. PID 控制器 (PID Controller)

【路径跟踪控制：PID控制与车辆运动学模型】
优点：

• 鲁棒性强：PID 控制器通过比例、积分和微分项的组合，能够有效地应对系统中的扰动和不确定性。
• 易于调参：通过调整 Kp、Ki 和 Kd 参数，可以灵活地控制系统的响应特性。
• 广泛适用：适用于多种类型的控制系统，特别是在线性和非线性系统中都有较好的表现。

缺点：

• 参数敏感：PID 控制器的性能高度依赖于参数的选择，不当的参数可能导致系统振荡或响应过慢。
• 复杂性：对于复杂的非线性系统，PID 控制器可能无法达到最优控制效果。
• 积分饱和：积分项可能导致积分饱和问题，需要额外的措施来防止。

6.2. Pure Pursuit 控制器 (Pure Pursuit Controller)

优点：

• 简单直观：Pure Pursuit 控制器基于几何原理，易于理解和实现。
• 实时性好：计算量小，适合实时控制应用。
• 鲁棒性好：对路径的形状和曲率变化有较好的适应能力。

缺点：

• 路径跟踪精度有限：对于复杂路径，特别是急转弯处，Pure Pursuit 控制器可能无法精确跟踪路径。
• 参数选择：前视距离 ( l_d ) 的选择对控制效果影响较大，需要根据具体应用场景进行调整。
• 速度依赖：控制效果受车辆速度的影响较大，高速时可能难以保持稳定。

6.3. Bang-Bang 控制器 (Bang-Bang Controller)

【路径跟踪控制：Bang-Bang 控制与车辆运动学模型】
优点：

• 响应快：Bang-Bang 控制器通过快速切换控制信号，能够在短时间内使系统接近目标。
• 简单易实现：控制逻辑简单，容易实现。

缺点：

• 过度震荡：由于控制信号的突然变化，系统可能会出现过度震荡，导致不稳定。
• 精度差：Bang-Bang 控制器通常只能将系统控制到目标的近似位置，难以实现高精度控制。
• 对噪声敏感：外部噪声和测量误差可能导致控制信号频繁切换，影响系统性能。

7. 总结

• PID 控制器适用于需要高精度和鲁棒性的应用场景，但需要仔细调参。
• Pure Pursuit 控制器适用于实时性要求高、路径跟踪精度要求适中的场景，特别适合自动驾驶等应用。
• Bang-Bang 控制器适用于对响应速度要求高、但对精度要求不高的场景，如简单的开关控制。

本文介绍了 Pure Pursuit 控制的基本原理及其在车辆路径跟踪中的应用。通过 Python 代码实现了车辆运动学模型和 Pure Pursuit 控制器，并展示了其在实际路径跟踪任务中的表现。Pure Pursuit 控制方法简单有效，适用于多种路径跟踪场景。未来的工作可以进一步优化控制器参数，提高路径跟踪的精度和稳定性。
参考文献：【自动驾驶】PurePursuit实现轨迹跟踪