【路径跟踪控制：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 = x
        self.y = y
        self.psi = psi
        self.v = v
        self.L = L
        self.dt = dt

    def update_state(self, a, delta_f):
        self.x += self.v * math.cos(self.psi) * self.dt
        self.y += self.v * math.sin(self.psi) * self.dt
        self.psi += (self.v / self.L) * math.tan(delta_f) * self.dt
        self.v += a * self.dt

    def 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 += 1
    return 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]) - psi
    delta = 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_dot

    def update(frame):
        nonlocal ugv
        robot_state = np.array([ugv.x, ugv.y])
        l_d = lam * ugv.v + c
        ind = 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 matplotlib
        matplotlib.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 以保存 GIF
    main(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 = x
        self.y = y
        self.psi = psi
        self.v = v
        self.L = L
        self.dt = dt

    def update_state(self, a, delta_f):
        self.x += self.v * np.cos(self.psi) * self.dt
        self.y += self.v * np.sin(self.psi) * self.dt
        self.psi += (self.v / self.L) * np.tan(delta_f) * self.dt
        self.v += a * self.dt

    def get_state(self):
        return self.x, self.y, self.psi, self.v

# PID 控制器类
class PIDController:
    def __init__(self, Kp, Ki, Kd, dt):
        self.Kp = Kp
        self.Ki = Ki
        self.Kd = Kd
        self.dt = dt
        self.prev_error = 0
        self.integral = 0

    def 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]) - psi
        self.integral += error * self.dt
        derivative = (error - self.prev_error) / self.dt
        output = self.Kp * error + self.Ki * self.integral + self.Kd * derivative
        self.prev_error = error
        return output

# Pure Pursuit 控制器类
class PurePursuitController:
    def __init__(self, L, lam, c):
        self.L = L
        self.lam = lam
        self.c = c

    def 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]) - psi
        l_d = self.lam * robot_state[2] + self.c
        delta = math.atan2(2 * self.L * math.sin(alpha), l_d)
        return delta

# Bang-Bang 控制器类
class BangBangController:
    def __init__(self, tolerance, max_delta):
        self.tolerance = tolerance
        self.max_delta = max_delta

    def 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 - psi
        error_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 0
        return 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.2
        ugv.v = 0
        a = 0
        reached_end = True
        print(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.01
    max_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_pid

    def 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 += 1

        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_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 = 0
    refer_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 以保存 GIF
    
    if not interactive_mode:
        import matplotlib
        matplotlib.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实现轨迹跟踪