1. 引言

自动驾驶系列博客梳理：

1. 【【自动驾驶】车辆运动学模型】
2. 【路径跟踪控制：Bang-Bang 控制与车辆运动学模型】

参考文献：
3. 【自动驾驶】总目录（持续更新）

随着自动驾驶技术的不断发展，无人车的轨迹跟踪控制成为了研究热点之一。本文将通过介绍无人车的运动学模型和Bang-Bang控制策略，探讨如何实现无人车的轨迹跟踪。我们将使用Python语言编写代码，通过模拟实验展示控制策略的效果。
先看优化后的Bang-Bang追踪sine函数路径跟踪效果啦！！超级OK💖💖💖

2. 环境准备

为了运行本文提供的代码，你需要安装以下Python库：

• numpy: 用于数值计算。
• matplotlib: 用于绘图和动画制作。
• scipy: 提供科学计算工具，本文中用于KDTree查询。

可以通过以下命令安装这些库：

pip install numpy matplotlib scipy

3. 车辆运动学模型

3.1 理论基础

在自动驾驶中，车辆的运动学模型是用于预测车辆未来状态的重要工具。通常情况下，车辆模型被简化为 以后轴中心为车辆中心的单车运动学模型，该模型假设左右前轮合并为一个轮子，后轮同样合并为一个轮子。车辆的运动状态可以通过位置 $x$, $y$、航向角 $\psi$和速度 $v$ 来描述。

对于前轮驱动的车辆，其运动学模型可以表示为：

$$\{\begin{array}{l}\dot{x}=v\cos (\psi )\\ \dot{y}=v\sin (\psi )\\ \dot{\psi }=\frac{v}{L}\tan ({\delta }_f)\end{array}$$
其中：

• $v$ 是车辆的速度；
• $l_f$ 和 $l_r$ 分别是前轴和后轴到车辆重心的距离。
• $L$ 是车辆的前后轴轴距。
• ${\delta }_f$ 是前轮的转向角。

3.2 Python 实现车辆运动学模型建模

以下是车辆运动学模型的Python实现：

import numpy as np
import matplotlib.pyplot as plt
import math
from scipy.spatial import KDTree
from matplotlib.animation import FuncAnimation

# 车辆运动学模型类
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, v, delta_f):
        self.x += v * np.cos(self.psi) * self.dt
        self.y += v * np.sin(self.psi) * self.dt
        self.psi += (v / self.L) * np.tan(delta_f) * self.dt

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

4. Bang-Bang 控制策略

4.1 理论基础

Bang-Bang控制是一种简单的控制策略，它将控制变量限制在两个极端值之间切换。在无人车轨迹跟踪任务中，Bang-Bang控制器可以根据横向跟踪误差$e_y$决定方向盘的角度${\delta }_f$。具体来说，如果误差超过某个阈值 $\text{tolerance}$，则方向盘转向最大角度$\text{max\_delta }$；否则，方向盘保持不动。

4.1.1 误差角度计算与转向角调整

在控制策略中，我们首先计算当前位置与目标位置之间的角度误差$\text{theta\_e }$：

theta_e = alpha - ugv.psi

接下来，我们根据角度误差的大小动态调整最大转向角$\text{max\_delta }$：

if abs(theta_e) > np.pi / 2.0:
    max_delta = np.pi / 12.0
else:
    max_delta = np.pi / 24.0

这里的逻辑是，当角度误差较大时（超过90度），我们允许更大的转向角以便更快地纠正误差；而当角度误差较小时，我们限制转向角以避免过度调整。

4.1.2 Bang-Bang控制实现

有了最大转向角 $\text{max\_delta }$，我们可以实现Bang-Bang控制：

delta_f = np.sign(error_y) * max_delta if abs(error_y) > tolerance else 0
ugv.update_state(2 + l_d/10, delta_f=delta_f)

这里的 $\text{error\_y}$ 是当前位置与目标位置在y轴方向上的误差。我们根据误差的符号决定转向角的方向（左转或右转），并根据 $\text{max\_delta }$ 确定转向角的大小。如果误差在容差范围内，则不进行转向调整。

4.2 完整代码

结合上述分析，以下是完整的Python代码实现：

import numpy as np
import matplotlib.pyplot as plt
import math
from scipy.spatial import KDTree
from matplotlib.animation import FuncAnimation

# 车辆运动学模型类
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, v, delta_f):
        self.x += v * np.cos(self.psi) * self.dt
        self.y += v * np.sin(self.psi) * self.dt
        self.psi += (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


def main(interactive=True):
    # 设置参考路径
    refer_path = np.zeros((1000, 2))
    refer_path[:, 0] = np.linspace(0, 100, 1000)
    refer_path[:, 1] = np.zeros(1000)  # 直线轨迹
    # refer_path[:, 1] = np.sin(refer_path[:, 0] / 10) * 2   # sine 轨迹

    refer_tree = KDTree(refer_path)

    # 初始化车辆状态
    ugv = KinematicModel(0, 2, 0, 2, 2, 0.1)
    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")
    target_dot, = ax.plot([], [], 'go', label="Target")
    ax.set_title("Bang-Bang Trajectory Control")
    ax.set_xlabel("x")
    ax.set_ylabel("y")
    ax.set_xlim(-1, 110)
    ax.set_ylim(-3, 3)
    ax.grid(True)
    ax.legend()

    # Bang-Bang 控制器参数
    tolerance = 0.01
    max_delta = np.pi / 12.0

    def init():
        line.set_data([], [])
        target_dot.set_data([], [])
        return line, target_dot

    def update(frame):
        robot_state = np.array([ugv.x, ugv.y])
        distance, ind = refer_tree.query(robot_state)

        # 控制器逻辑
        dx, dy = refer_path[ind] - robot_state
        alpha = math.atan2(dy, dx)
        l_d = np.linalg.norm(refer_path[ind] - robot_state)
        theta_e = alpha - ugv.psi
        error_y = l_d * math.sin(theta_e)
        if abs(theta_e) > np.pi / 2.0:
            max_delta = np.pi / 12.0
        else:
            max_delta = np.pi / 24.0

        # Bang-Bang 控制
        delta_f = np.sign(error_y) * max_delta if abs(error_y) > tolerance else 0
        ugv.update_state(2 + l_d/10 , delta_f=delta_f)

        # 更新轨迹
        trajectory_x.append(ugv.x)
        trajectory_y.append(ugv.y)
        line.set_data(trajectory_x, trajectory_y)

        # 更新目标点 (确保传入的是列表或数组)
        target_dot.set_data([refer_path[ind, 0]], [refer_path[ind, 1]])
        return line, target_dot

    # 创建动画
    anim = FuncAnimation(fig, update, frames=500, init_func=init, blit=True, interval=50, repeat=False)

    if interactive:
        plt.show()
    else:
        anim.save('Control/Controllers_python/Bang_Bang/ugv_animation.gif', writer='pillow', fps=20)
        print("GIF 已保存")

# 运行主程序
if __name__ == '__main__':
    interactive_mode = True  # 设置为 True 以在交互式模式下运行，False 以保存 GIF
    if not interactive_mode:
        import matplotlib
        matplotlib.use('Agg')

    main(interactive=interactive_mode)

4.3 控制策略解释

1. 初始化：

   • 设置参考路径为一条直线。
   • 初始化车辆状态，包括位置、航向角和速度。
   • 创建绘图窗口，绘制参考路径。

2. 控制器逻辑：

   • 使用KDTree查询当前车辆位置到最近路径点的距离和索引。
   • 计算当前车辆位置与最近路径点之间的角度差${\theta }_e$和横向跟踪误差$\text{error\_y}$。
   • 根据 ${\theta }_e$ 的大小调整最大转向角 $\text{max\_delta}$。
   • 如果横向跟踪误差$\text{error\_y}$超过阈值 $\text{tolerance}$，则执行Bang-Bang控制，即转向最大角度$\text{max\_delta}$；否则，方向盘保持不动。

3. 更新状态：

   • 使用计算出的转向角${\delta }_f$更新车辆状态。
   • 将新的位置添加到轨迹列表中。
   • 更新绘图窗口中的轨迹和目标点。
   • interactive_mode = True # 设置为 True 以在交互式模式下运行，False 以保存 GIF
     效果如下
     
     整体跟踪效果一般，有明显的震荡，尝试收敛中

如果换成sine跟踪，只需调整跟踪轨迹代码如下

refer_path[:, 1] = np.sin(refer_path[:, 0] / 10) * 2   # sine 轨迹

勉强可以跟踪啦！🤣🤣🤣

5. 改进BangBang控制

在上文中，原始的Bang-Bang控制算法主要依赖于车辆当前位置与参考路径之间的横向误差来决定转向角。当横向误差超过设定的阈值时，车辆会以最大转向角转向；当误差小于阈值时，车辆停止转向。这种控制方式简单有效，但在复杂路径跟踪时可能表现不佳，例如在曲率变化较大的路径上。改进后的Bang-Bang控制算法引入了对路径点的动态管理机制，以适应更长的路径跟踪需求。此外，还优化了速度控制策略，使得车辆能够更加平滑地跟随路径。

5.2 动态管理路径点

在改进的版本中，我们引入了一个全局变量 iadd 来追踪当前车辆应跟踪的路径点索引。当 iadd 超出路径点数组的长度时，动态增加路径点，确保路径点数组足够长以支持整个动画过程。

5.3 优化速度控制策略

改进后的速度控制策略不再固定为常数，而是根据当前车辆与目标路径点之间的距离 l_d 来动态调整。这有助于车辆在接近目标点时减速，从而减少过冲现象，提高路径跟踪精度。

Python 实现

以下是改进后的Bang-Bang控制算法的Python实现：

import numpy as np
import matplotlib.pyplot as plt
import math
from scipy.spatial import KDTree
from matplotlib.animation import FuncAnimation

# 初始化参考路径
refer_path = np.zeros((1000, 2))

# 车辆运动学模型类
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, v, delta_f):
        self.x += v * np.cos(self.psi) * self.dt
        self.y += v * np.sin(self.psi) * self.dt
        self.psi += (v / self.L) * np.tan(delta_f) * self.dt

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


def main(interactive=True):
    global refer_path, iadd  # 确保这些变量被声明为全局变量
    
    # 初始化车辆状态
    ugv = KinematicModel(0, 2, 0, 2, 2, 0.01)
    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")
    target_dot, = ax.plot([], [], 'go', label="Target")
    ax.set_title("Improved Bang-Bang Trajectory Control")
    ax.set_xlabel("x")
    ax.set_ylabel("y")
    ax.set_xlim(-1, 110)
    ax.set_ylim(-3, 3)
    ax.grid(True)
    ax.legend()

    # Bang-Bang 控制器参数
    tolerance = 0.01
    max_delta = np.pi / 12.0
    
    def init():
        line.set_data([], [])
        target_dot.set_data([], [])
        return line, target_dot
        
    def update(frame):
        global iadd, refer_path  # 确保使用全局变量
        robot_state = np.array([ugv.x, ugv.y])

        # 判断 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 坐标

        dx, dy = refer_path[iadd] - robot_state
        alpha = math.atan2(dy, dx)
        l_d = np.linalg.norm(refer_path[iadd] - robot_state)
        theta_e = alpha - ugv.psi
        error_y = l_d * math.sin(theta_e)

        # 根据误差调整最大转向角
        if abs(theta_e) > np.pi / 2.0:
            max_delta = np.pi / 12.0
        else:
            max_delta = np.pi / 24.0

        # Bang-Bang 控制
        delta_f = np.sign(error_y) * max_delta if abs(error_y) > tolerance else 0
        ugv.update_state(l_d * 4, delta_f=delta_f)

        # 更新轨迹
        trajectory_x.append(ugv.x)
        trajectory_y.append(ugv.y)
        line.set_data(trajectory_x, trajectory_y)

        # 更新目标点
        target_dot.set_data([refer_path[iadd, 0]], [refer_path[iadd, 1]])
        iadd += 1  # 增加 iadd
        return line, target_dot

    # 创建动画
    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/Bang_Bang/ugv_animation.gif', writer='pillow', fps=20)
        print("GIF 已保存")

# 运行主程序
if __name__ == '__main__':
    iadd = 0
    refer_path[:, 0] = np.linspace(0, 100, 1000)
    refer_path[:, 1] = np.sin(refer_path[:, 0] / 10) * 2  # sine 轨迹
    interactive_mode = False  # 设置为 True 以在交互式模式下运行，False 以保存 GIF
    
    if not interactive_mode:
        import matplotlib
        matplotlib.use('Agg')

    main(interactive=interactive_mode)

• 动态管理路径点
  原始算法：使用固定的路径点数组，可能导致路径点不足的问题。
  改进算法：动态增加路径点，确保路径点数组足够长，适用于更长的路径跟踪。
• 速度控制策略
  原始算法：速度固定为常数，可能导致过冲现象。
  改进算法：根据距离动态调整速度，减少过冲现象，提高路径跟踪精度。
• 控制效果
  原始算法：在直线路段表现良好，但在曲率变化较大的路径上可能表现不佳。
  改进算法：通过动态调整速度和路径点管理，提高了在复杂路径上的跟踪精度和平滑性。

6. 结论

通过上述改进，Bang-Bang控制算法在复杂路径跟踪中的性能得到了显著提升。动态管理路径点和优化速度控制策略使得车辆能够更加平滑地跟随路径，减少了过冲现象，提高了路径跟踪精度。通过Python代码实现和模拟实验，我们可以观察到控制策略的效果。后期会有更多的路径跟踪（PID，LQR，MPC等）算法分享，希望本文对你理解和实现无人车轨迹跟踪有所帮助。

如果你有任何问题或建议，欢迎加QQ群留言交流！😘😘😘