基于模糊控制的纯跟踪横向控制在倒车中的应用及实现

文章目录

- 1. 引言
- 2. Pure Pursuit在倒车场景的推导
- 3. 模糊控制器的设计
- - 3.1 基础知识
  - 3.2 预瞄距离系数k的模糊控制器设计
- 4. 算法和仿真实现

1. 引言

Pure Pursuit是一种几何跟踪控制算法，也被称为纯跟踪控制算法。他的思想就是基于当前车辆的后轮中心的位置，在参考路径上寻找一个预瞄点，假设车辆可按照一定转弯半径下行驶到该目标点，然后根据车辆当前位置到预瞄点距离、转弯半径和预瞄点与车头朝向夹角的几何关系来计算车辆的前轮转角，进而得到车辆的横向输入 $\delta_f$ 。其中，预瞄点的选取对跟踪效果至关重要，选择合适的预瞄点可以提高车辆的行驶稳定性和路径跟踪精度。

模糊控制作为一种智能控制方法，具有处理非线性、不确定性问题的能力。它不需要建立精确的数学模型，而是通过模糊集合和模糊逻辑来模拟人类的思维过程，实现对复杂系统的有效控制。因此，将模糊控制引入纯跟踪预瞄点的计算，有望提高车辆横向控制的精度和鲁棒性。

2. Pure Pursuit在倒车场景的推导

在这里插入图片描述

其中

$P$ ：当前车辆的预瞄点
$l_d$ ：车辆后轴中心点 $A$ 到 $F$ 的距离，即预瞄距离
$\theta$ ： $l_d$ 与车轴的夹角
$\varphi$ ：车辆的航向角
$e_y$ ：预瞄点与车辆横向偏差
$L$ ：轴距
$R$ ：纯跟踪的目标转弯半径
$\delta_f$ ：前轮转角

由图中的三角形几何关系可得
$\frac{sin(2\theta)}{l_d} = \frac{sin(\frac{\pi}{2} - \theta)}{R} \tag{1}$
化简后为
$R=\frac{l_d}{2sin\theta} \tag{2}$
由简化的车辆运动学模型和上图中的几何关系可得
$tan\delta_f=\frac{L}{R} \tag{3}$
由(2)和(3)可得
$\delta_f = arctan\frac{L}{R}=arctan\frac{2Lsin\theta}{l_d} \tag{4}$
$\theta$ 为 $l_d$ 与车轴的夹角，结合图中角度关系可得。
$\begin{align*} \theta = \pi-\angle PAB &= \pi-(\angle PAX- \angle BAX)\\ &= \pi-(arctan\frac{P_y-A_y}{P_x-A_x}- \varphi)\\ \end{align*} \tag{5}$
所以有
$sin\theta=sin(\pi-(arctan\frac{P_y-A_y}{P_x-A_x}- \varphi))=sin(arctan\frac{P_y-A_y}{P_x-A_x}- \varphi) \tag{6}$
将(6)式代入(4)即可以得到前轮转角 $\delta_f$ 。

注：这里 $\theta$ 和 $sin\theta$ 的计算方式与纯跟踪横向控制和算法仿真实现不太一样，本文这里是算法实现过程中常用的方法，而前面的文章是为了归纳 $l_d$ 对 $\delta_f$ 的影响，且其代码实现也是用的公式(6)的方法进行计算的。向前行驶和倒车中唯一的区别就是(5)式少了个 $\pi-$ （感兴趣可以根据前面的文章的原理图推导一下），但由于sin的特性，使得最终两种场景，对应的统一套代码，不需要区分具体场景。

结合前面的纯跟踪横向控制和算法仿真实现，我们可以知道，通常情况下我们会使用车辆的速度来衡量 $l_d$ 的大小，其对应关系如下
$l_d=kv+l_fc \tag{7}$
其中 $k$ 为预瞄距离系数， $l_fc$ 为预设距离。

对于公式(7)，仅考虑了车速对预瞄距离的影响，忽略了横向误差和航向误差对于瞄距离的影响，假如车速不变，当横向误差或者航向误差越大的时候，需要增加预瞄距离，降低车辆超调和震荡发生的周期，使得切入更平滑。由于横向误差和航向误差与预瞄距离，没有很明确线性的几何关系，这里我们引入模糊控制器对公式(7)中的预瞄距离系数 $k$ 进行处理。

3. 模糊控制器的设计

3.1 基础知识

模糊控制是一种基于模糊逻辑的智能控制方法，它模仿人的模糊推理和决策过程。模糊控制不依赖于被控对象的精确数学模型，而是利用控制规则来描述系统变量间的关系，使用语言式的模糊变量来描述系统，便于操作人员使用自然语言进行人机对话。模糊控制特别适用于非线性、时变、滞后、模型不完全系统的控制，具有鲁棒性、适应性和容错性。

在模糊控制系统中，通常包含以下几个关键部分：

模糊化接口：将真实确定的输入量转换为模糊矢量。
知识库：由数据库和规则库构成，数据库存放输入输出变量的隶属度矢量值，规则库基于专家知识或操作人员的经验。
推理与解模糊接口：完成模糊推理，将推理得到的模糊矢量转换为清晰的控制量输出。

模糊控制的设计缺乏系统性，获取模糊规则及隶属函数主要凭经验进行。信息简单的模糊处理可能导致控制精度降低和动态品质变差。为提高精度，可能需要增加量化级数，这会增加规则搜索范围，降低决策速度。

Python中实现模糊控制可以使用scikit-fuzzy模块，它提供了一系列工具来构建和模拟模糊控制系统。以下是使用Python和scikit-fuzzy模块实现模糊控制的基本步骤：

定义输入输出变量的范围，并创建模糊集合：使用numpy数组定义输入和输出变量的可能值范围，定义输入和输出的模糊集合。
定义隶属函数：为模糊集合定义其隶属度函数。
建立模糊控制规则：根据专家经验或操作人员的经验，建立控制规则。
解模糊：确定输出的解模糊方法，如质心法等。
运行模拟：通过输入数据运行模糊控制系统并观察输出。

下面我们以预瞄距离系数 $k$ 的模糊控制设计为例，结合Python的scikit-fuzzy模块，介绍并实现该模糊控制器。

3.2 预瞄距离系数k的模糊控制器设计

结合我们第二节的分析，该模糊控制器的输入为车辆的横向误差 $e_y$ 和航向误差 $e_{yaw}$ ，输出为预描距离 $k$ ，它们的关系就是 $e_y$ 和 $e_{yaw}$ 越大，预瞄距离越大，则 $k$ 越大，反之，亦然。制定模糊表规则如下：

在这里插入图片描述

定义输入输出变量的范围，并创建模糊集合

from skfuzzy import control as ctrl 

u_lat = ctrl.Antecedent(np.arange(0, 13, 1), 'u_lat')  # 横向误差范围为[0，12]
u_yaw = ctrl.Antecedent(np.arange(0, 1.3, 0.1), 'u_yaw')  # 航向误差范围为[0，1.2]
x_k = ctrl.Consequent(np.arange(0, 2.5, 0.1), 'x_k')  # 预描距离系数k范围为[0，2.4]

这里使用Antecedent创建横向误差、航向误差和预瞄距离系数 $k$ 三个模糊集合，并用u_lat、u_yaw和x_k来表示。

定义隶属函数

# 设置目标的模糊规则
u_lat['lowest'] = fuzz.trimf(u_lat.universe, [0, 0, 2.0])
u_lat['lower'] = fuzz.trimf(u_lat.universe, [0, 2, 4.0])
u_lat['low'] = fuzz.trimf(u_lat.universe, [2.0, 4.0, 6.0])
u_lat['middle'] = fuzz.trimf(u_lat.universe, [4.0, 6.0, 8.0])
u_lat['high'] = fuzz.trimf(u_lat.universe, [6.0, 8.0, 10.0])
u_lat['higher'] = fuzz.trimf(u_lat.universe, [8.0, 10.0, 12.0])
u_lat['highest'] = fuzz.trimf(u_lat.universe, [10.0, 12.0, 12.0])

u_yaw['lowest'] = fuzz.trimf(u_yaw.universe, [0, 0, 0.2])
u_yaw['lower'] = fuzz.trimf(u_yaw.universe, [0, 0.2, 0.4])
u_yaw['low'] = fuzz.trimf(u_yaw.universe, [0.2, 0.4, 0.6])
u_yaw['middle'] = fuzz.trimf(u_yaw.universe, [0.4, 0.6, 0.8])
u_yaw['high'] = fuzz.trimf(u_yaw.universe, [0.6, 0.8, 1.0])
u_yaw['higher'] = fuzz.trimf(u_yaw.universe, [0.8, 1.0, 1.2])
u_yaw['highest'] = fuzz.trimf(u_yaw.universe, [1.0, 1.2, 1.2])

x_k['lowest'] = fuzz.trimf(x_k.universe, [0, 0, 0.2])
x_k['very_low'] = fuzz.trimf(x_k.universe, [0, 0.2, 0.4])
x_k['extremely_low'] = fuzz.trimf(x_k.universe, [0.2, 0.4, 0.6])
x_k['much_lower'] = fuzz.trimf(x_k.universe, [0.4, 0.6, 0.8])
x_k['lower'] = fuzz.trimf(x_k.universe, [0.6, 0.8, 1.0])
x_k['below_average'] = fuzz.trimf(x_k.universe, [0.8, 1.0, 1.2])
x_k['average'] = fuzz.trimf(x_k.universe, [1.0, 1.2, 1.4])
x_k['above_average'] = fuzz.trimf(x_k.universe, [1.2, 1.4, 1.6])
x_k['higher'] = fuzz.trimf(x_k.universe, [1.4, 1.6, 1.8])
x_k['much_high'] = fuzz.trimf(x_k.universe, [1.6, 1.8, 2.0])
x_k['very_high'] = fuzz.trimf(x_k.universe, [1.8, 2.0, 2.2])
x_k['extremely_high'] = fuzz.trimf(x_k.universe, [2.0, 2.2, 2.4])
x_k['highest'] = fuzz.trimf(x_k.universe, [2.2, 2.4, 2.4])

# 绘制隶属函数图
u_lat.view()
u_yaw.view()
x_k.view()

每个集合对应的隶属函数图如下所示：

在这里插入图片描述

根据规则表创建规则

# 根据规则表创建规则
rule1 = ctrl.Rule(u_lat['lowest'] & u_yaw['lowest'], x_k['lowest'])
rule2 = ctrl.Rule((u_lat['lower'] & u_yaw['lowest']) | (u_lat['lowest'] & u_yaw['lower']), x_k['very_low'])
rule3 = ctrl.Rule((u_lat['lowest'] & u_yaw['low']) | (u_lat['lower'] & u_yaw['lower'])
                  |(u_lat['low'] & u_yaw['lowest']), x_k['extremely_low'])
rule4 = ctrl.Rule((u_lat['lowest'] & u_yaw['middle']) | (u_lat['lower'] & u_yaw['low'])
                  |(u_lat['low'] & u_yaw['lower']) | (u_lat['middle'] & u_yaw['lowest']), x_k['much_lower'])
rule5 = ctrl.Rule((u_lat['lowest'] & u_yaw['high']) | (u_lat['lower'] & u_yaw['middle'])
                  |(u_lat['low'] & u_yaw['low']) | (u_lat['middle'] & u_yaw['lower'])
                  |(u_lat['high'] & u_yaw['lowest']), x_k['lower'])
rule6 = ctrl.Rule((u_lat['lowest'] & u_yaw['higher']) | (u_lat['lower'] & u_yaw['high'])
                  |(u_lat['low'] & u_yaw['middle']) | (u_lat['middle'] & u_yaw['low'])
                  |(u_lat['high'] & u_yaw['lower']) |(u_lat['higher'] & u_yaw['lowest']), x_k['below_average'])
rule7 = ctrl.Rule((u_lat['lowest'] & u_yaw['highest']) | (u_lat['lower'] & u_yaw['higher'])
                  |(u_lat['low'] & u_yaw['high']) | (u_lat['middle'] & u_yaw['middle'])
                  |(u_lat['high'] & u_yaw['low']) |(u_lat['higher'] & u_yaw['lower'])
                  |(u_lat['highest'] & u_yaw['lowest']), x_k['average'])
rule8 = ctrl.Rule((u_lat['lower'] & u_yaw['highest']) | (u_lat['low'] & u_yaw['higher'])
                  |(u_lat['middle'] & u_yaw['high']) | (u_lat['high'] & u_yaw['middle'])
                  |(u_lat['higher'] & u_yaw['low']) |(u_lat['highest'] & u_yaw['lower']), x_k['above_average'])
rule9 = ctrl.Rule((u_lat['low'] & u_yaw['highest']) | (u_lat['middle'] & u_yaw['higher'])
                  |(u_lat['high'] & u_yaw['high']) | (u_lat['higher'] & u_yaw['middle'])
                  |(u_lat['highest'] & u_yaw['low']), x_k['higher'])
rule10 = ctrl.Rule((u_lat['highest'] & u_yaw['middle']) | (u_lat['higher'] & u_yaw['high'])
                  |(u_lat['high'] & u_yaw['higher']) | (u_lat['middle'] & u_yaw['highest']), x_k['much_high'])
rule11 = ctrl.Rule((u_lat['highest'] & u_yaw['high']) | (u_lat['higher'] & u_yaw['higher'])
                  |(u_lat['high'] & u_yaw['highest']), x_k['very_high'])
rule12 = ctrl.Rule((u_lat['highest'] & u_yaw['higher']) | (u_lat['higher'] & u_yaw['highest']), x_k['extremely_high'])
rule13 = ctrl.Rule((u_lat['highest'] & u_yaw['highest']) , x_k['highest'])

# 创建控制系统,应用编写好的规则
k_ctrl = ctrl.ControlSystem([rule1, rule2, rule3, rule4, rule5, rule6,
                             rule7, rule8, rule9, rule10, rule11, rule12, rule13])

解模糊，并运行模拟

# 创建控制系统,应用编写好的规则
k_ctrl = ctrl.ControlSystem([rule1, rule2, rule3, rule4, rule5, rule6,
                             rule7, rule8, rule9, rule10, rule11, rule12, rule13])

# 创建控制仿真器
k_con = ctrl.ControlSystemSimulation(k_ctrl)

# 输入数据
k_con.input['u_lat'] = abs(1)
k_con.input['u_yaw'] = abs(1)

# 设置去模糊方法，设置去模糊化方法为质心法
x_k.defuzzify_method = 'centroid'

# 计算结果
k_con.compute()
x_k.view(sim=k_con)  # 绘制结果
plt.show()

输出结果如下图所示

在这里插入图片描述

1.1

图中黑色粗线对应的横坐标即为计算结果，因此，当输入为 $(1, 1)$ 的时候，该系统计算出的 $k$ 值应为1.1。

对于我们设计模糊控制，我们可以将输入、输出关系使用三维空间曲面图表示出来，进一步验证我们设计的控制器是否符合预期，代码如下所示

x = np.linspace(0, 12, 20)
y = np.linspace(0, 1.2, 20)
X, Y = np.meshgrid(x, y)
Z = np.zeros_like(X)
pp = []
for i in range(0, 20):
    for j in range(0, 20):
        k_con.input['u_lat'] = X[i, j]
        k_con.input['u_yaw'] = Y[i, j]
        k_con.compute()
        Z[i, j] = k_con.output['x_k']
        pp.append(Z[i, j])

fig = plt.figure(figsize=(8, 8))  # 定义画布大小
ax = fig.add_subplot(111, projection='3d')
surf = ax.plot_surface(X, Y, Z, rstride=1, cstride=1, cmap='viridis', linewidth=0.4, antialiased=True)
ax.view_init(30, 200)  # 设置观察角度
plt.show()

输出结果如下

在这里插入图片描述

在实际工程应用中，我们通常会使用离线的方式对模糊控制器进行部署，及将上图中的关系定义为表格，作为系统的配置文件，根据输入进行表格查询，然后进行线性插值得到控制器的输出，该方法简单高效率，对计算能力低的处理器也十分友好。

4. 算法和仿真实现

k_fuzzy_controller.py

import numpy as np
import skfuzzy as fuzz
from skfuzzy import control as ctrl


def calculate_k_base_on_fuzzy_control(e_lat, e_yaw):

    u_lat = ctrl.Antecedent(np.arange(0, 13, 1), 'u_lat')  # 横向误差范围为[0，12]
    u_yaw = ctrl.Antecedent(np.arange(0, 1.3, 0.1), 'u_yaw')  # 航向误差范围为[0，1.2]
    x_k = ctrl.Consequent(np.arange(0, 2.5, 0.1), 'x_k')  # 预描距离系数k范围为[0，2.4]

    # 设置目标的模糊规则
    u_lat['lowest'] = fuzz.trimf(u_lat.universe, [0, 0, 2.0])
    u_lat['lower'] = fuzz.trimf(u_lat.universe, [0, 2, 4.0])
    u_lat['low'] = fuzz.trimf(u_lat.universe, [2.0, 4.0, 6.0])
    u_lat['middle'] = fuzz.trimf(u_lat.universe, [4.0, 6.0, 8.0])
    u_lat['high'] = fuzz.trimf(u_lat.universe, [6.0, 8.0, 10.0])
    u_lat['higher'] = fuzz.trimf(u_lat.universe, [8.0, 10.0, 12.0])
    u_lat['highest'] = fuzz.trimf(u_lat.universe, [10.0, 12.0, 12.0])

    u_yaw['lowest'] = fuzz.trimf(u_yaw.universe, [0, 0, 0.2])
    u_yaw['lower'] = fuzz.trimf(u_yaw.universe, [0, 0.2, 0.4])
    u_yaw['low'] = fuzz.trimf(u_yaw.universe, [0.2, 0.4, 0.6])
    u_yaw['middle'] = fuzz.trimf(u_yaw.universe, [0.4, 0.6, 0.8])
    u_yaw['high'] = fuzz.trimf(u_yaw.universe, [0.6, 0.8, 1.0])
    u_yaw['higher'] = fuzz.trimf(u_yaw.universe, [0.8, 1.0, 1.2])
    u_yaw['highest'] = fuzz.trimf(u_yaw.universe, [1.0, 1.2, 1.2])

    x_k['lowest'] = fuzz.trimf(x_k.universe, [0, 0, 0.2])
    x_k['very_low'] = fuzz.trimf(x_k.universe, [0, 0.2, 0.4])
    x_k['extremely_low'] = fuzz.trimf(x_k.universe, [0.2, 0.4, 0.6])
    x_k['much_lower'] = fuzz.trimf(x_k.universe, [0.4, 0.6, 0.8])
    x_k['lower'] = fuzz.trimf(x_k.universe, [0.6, 0.8, 1.0])
    x_k['below_average'] = fuzz.trimf(x_k.universe, [0.8, 1.0, 1.2])
    x_k['average'] = fuzz.trimf(x_k.universe, [1.0, 1.2, 1.4])
    x_k['above_average'] = fuzz.trimf(x_k.universe, [1.2, 1.4, 1.6])
    x_k['higher'] = fuzz.trimf(x_k.universe, [1.4, 1.6, 1.8])
    x_k['much_high'] = fuzz.trimf(x_k.universe, [1.6, 1.8, 2.0])
    x_k['very_high'] = fuzz.trimf(x_k.universe, [1.8, 2.0, 2.2])
    x_k['extremely_high'] = fuzz.trimf(x_k.universe, [2.0, 2.2, 2.4])
    x_k['highest'] = fuzz.trimf(x_k.universe, [2.2, 2.4, 2.4])

    # 根据规则表创建规则
    rule1 = ctrl.Rule(u_lat['lowest'] & u_yaw['lowest'], x_k['lowest'])
    rule2 = ctrl.Rule((u_lat['lower'] & u_yaw['lowest']) | (u_lat['lowest'] & u_yaw['lower']), x_k['very_low'])
    rule3 = ctrl.Rule((u_lat['lowest'] & u_yaw['low']) | (u_lat['lower'] & u_yaw['lower'])
                      |(u_lat['low'] & u_yaw['lowest']), x_k['extremely_low'])
    rule4 = ctrl.Rule((u_lat['lowest'] & u_yaw['middle']) | (u_lat['lower'] & u_yaw['low'])
                      |(u_lat['low'] & u_yaw['lower']) | (u_lat['middle'] & u_yaw['lowest']), x_k['much_lower'])
    rule5 = ctrl.Rule((u_lat['lowest'] & u_yaw['high']) | (u_lat['lower'] & u_yaw['middle'])
                      |(u_lat['low'] & u_yaw['low']) | (u_lat['middle'] & u_yaw['lower'])
                      |(u_lat['high'] & u_yaw['lowest']), x_k['lower'])
    rule6 = ctrl.Rule((u_lat['lowest'] & u_yaw['higher']) | (u_lat['lower'] & u_yaw['high'])
                      |(u_lat['low'] & u_yaw['middle']) | (u_lat['middle'] & u_yaw['low'])
                      |(u_lat['high'] & u_yaw['lower']) |(u_lat['higher'] & u_yaw['lowest']), x_k['below_average'])
    rule7 = ctrl.Rule((u_lat['lowest'] & u_yaw['highest']) | (u_lat['lower'] & u_yaw['higher'])
                      |(u_lat['low'] & u_yaw['high']) | (u_lat['middle'] & u_yaw['middle'])
                      |(u_lat['high'] & u_yaw['low']) |(u_lat['higher'] & u_yaw['lower'])
                      |(u_lat['highest'] & u_yaw['lowest']), x_k['average'])
    rule8 = ctrl.Rule((u_lat['lower'] & u_yaw['highest']) | (u_lat['low'] & u_yaw['higher'])
                      |(u_lat['middle'] & u_yaw['high']) | (u_lat['high'] & u_yaw['middle'])
                      |(u_lat['higher'] & u_yaw['low']) |(u_lat['highest'] & u_yaw['lower']), x_k['above_average'])
    rule9 = ctrl.Rule((u_lat['low'] & u_yaw['highest']) | (u_lat['middle'] & u_yaw['higher'])
                      |(u_lat['high'] & u_yaw['high']) | (u_lat['higher'] & u_yaw['middle'])
                      |(u_lat['highest'] & u_yaw['low']), x_k['higher'])
    rule10 = ctrl.Rule((u_lat['highest'] & u_yaw['middle']) | (u_lat['higher'] & u_yaw['high'])
                      |(u_lat['high'] & u_yaw['higher']) | (u_lat['middle'] & u_yaw['highest']), x_k['much_high'])
    rule11 = ctrl.Rule((u_lat['highest'] & u_yaw['high']) | (u_lat['higher'] & u_yaw['higher'])
                      |(u_lat['high'] & u_yaw['highest']), x_k['very_high'])
    rule12 = ctrl.Rule((u_lat['highest'] & u_yaw['higher']) | (u_lat['higher'] & u_yaw['highest']), x_k['extremely_high'])
    rule13 = ctrl.Rule((u_lat['highest'] & u_yaw['highest']) , x_k['highest'])

    # 创建控制系统,应用编写好的规则
    k_ctrl = ctrl.ControlSystem([rule1, rule2, rule3, rule4, rule5, rule6,
                                 rule7, rule8, rule9, rule10, rule11, rule12, rule13])
    # 创建控制仿真器
    k_con = ctrl.ControlSystemSimulation(k_ctrl)
    # 输入数据
    k_con.input['u_lat'] = abs(e_lat)
    k_con.input['u_yaw'] = abs(e_yaw)
    # 设置去模糊方法，设置去模糊化方法为质心法
    x_k.defuzzify_method = 'centroid'
    # 计算结果
    k_con.compute()

    return k_con.output['x_k']

pure_pursuit_controller.py

from k_fuzzy_controller import calculate_k_base_on_fuzzy_control
import numpy as np
import math

k = 0.1  # 预瞄距离系数
Lfc = 3.0  # 初始预瞄距离


def calc_preparation(vehicle, ref_path):
    """
    计算index、er和v
    """
    rx, ry, ryaw = ref_path[:, 0], ref_path[:, 1], ref_path[:, 3]
    dx = [vehicle.x - icx for icx in rx]
    dy = [vehicle.y - icy for icy in ry]
    d = np.hypot(dx, dy)
    index = np.argmin(d)

    vec_nr = np.array([math.cos(ryaw[index] + math.pi / 2.0),
                       math.sin(ryaw[index] + math.pi / 2.0)])
    vec_target_2_rear = np.array([vehicle.x - rx[index],
                                  vehicle.y - ry[index]])
    e_lat = np.dot(vec_target_2_rear, vec_nr)
    e_yaw = vehicle.yaw - ryaw[index]

    return index, e_lat, e_yaw, rx, ry


def pure_pursuit_steer_control(vehicle, ref_path, index_old, is_adaptive=False):
    ind, e_lat, e_yaw, rx, ry = calc_preparation(vehicle, ref_path)

    new_k = k
    if is_adaptive:  # 判断是否使用动态k系数
        new_k = calculate_k_base_on_fuzzy_control(e_lat, e_yaw)

    ld = new_k * abs(vehicle.v) + Lfc  # 根据速度更新预瞄距离
    while ld > math.hypot(rx[ind] - vehicle.x, ry[ind] - vehicle.y):
        if (ind + 1) >= len(rx):
            break
        ind += 1

    if index_old is None:
        index_old = ind
    ind = max(ind, index_old)
    tx = rx[ind]
    ty = ry[ind]

    alpha = math.atan2(ty - vehicle.y, tx - vehicle.x) - vehicle.yaw
    delta = math.atan2(2.0 * vehicle.L * math.sin(alpha), ld)
    return delta, ind, e_lat

kinematic_bicycle_model.py

import math
import numpy as np

class Vehicle:
    def __init__(self,
                 x=0.0,
                 y=0.0,
                 yaw=0.0,
                 v=0.0,
                 dt=0.1,
                 l=3.0):
        self.steer = 0
        self.x = x
        self.y = y
        self.yaw = yaw
        self.v = v
        self.dt = dt
        self.L = l  # 轴距
        self.x_front = x + l * math.cos(yaw)
        self.y_front = y + l * math.sin(yaw)

    def update(self, a, delta, max_steer=np.pi):
        delta = np.clip(delta, -max_steer, max_steer)
        self.steer = delta

        self.x = self.x + self.v * math.cos(self.yaw) * self.dt
        self.y = self.y + self.v * math.sin(self.yaw) * self.dt
        self.yaw = self.yaw + self.v / self.L * math.tan(delta) * self.dt

        self.v = self.v + a * self.dt
        self.x_front = self.x + self.L * math.cos(self.yaw)
        self.y_front = self.y + self.L * math.sin(self.yaw)


class VehicleInfo:
    # Vehicle parameter
    L = 2.0  # 轴距
    W = 2.0  # 宽度
    LF = 2.8  # 后轴中心到车头距离
    LB = 0.8  # 后轴中心到车尾距离
    MAX_STEER = 0.6  # 最大前轮转角
    TR = 0.5  # 轮子半径
    TW = 0.5  # 轮子宽度
    WD = W  # 轮距
    LENGTH = LB + LF  # 车辆长度

def draw_vehicle(x, y, yaw, steer, ax, vehicle_info=VehicleInfo, color='black'):
    vehicle_outline = np.array(
        [[-vehicle_info.LB, vehicle_info.LF, vehicle_info.LF, -vehicle_info.LB, -vehicle_info.LB],
         [vehicle_info.W / 2, vehicle_info.W / 2, -vehicle_info.W / 2, -vehicle_info.W / 2, vehicle_info.W / 2]])

    wheel = np.array([[-vehicle_info.TR, vehicle_info.TR, vehicle_info.TR, -vehicle_info.TR, -vehicle_info.TR],
                      [vehicle_info.TW / 2, vehicle_info.TW / 2, -vehicle_info.TW / 2, -vehicle_info.TW / 2,
                       vehicle_info.TW / 2]])

    rr_wheel = wheel.copy()  # 右后轮
    rl_wheel = wheel.copy()  # 左后轮
    fr_wheel = wheel.copy()  # 右前轮
    fl_wheel = wheel.copy()  # 左前轮
    rr_wheel[1, :] += vehicle_info.WD / 2
    rl_wheel[1, :] -= vehicle_info.WD / 2

    # 方向盘旋转
    rot1 = np.array([[np.cos(steer), -np.sin(steer)],
                     [np.sin(steer), np.cos(steer)]])
    # yaw旋转矩阵
    rot2 = np.array([[np.cos(yaw), -np.sin(yaw)],
                     [np.sin(yaw), np.cos(yaw)]])
    fr_wheel = np.dot(rot1, fr_wheel)
    fl_wheel = np.dot(rot1, fl_wheel)
    fr_wheel += np.array([[vehicle_info.L], [-vehicle_info.WD / 2]])
    fl_wheel += np.array([[vehicle_info.L], [vehicle_info.WD / 2]])

    fr_wheel = np.dot(rot2, fr_wheel)
    fr_wheel[0, :] += x
    fr_wheel[1, :] += y
    fl_wheel = np.dot(rot2, fl_wheel)
    fl_wheel[0, :] += x
    fl_wheel[1, :] += y
    rr_wheel = np.dot(rot2, rr_wheel)
    rr_wheel[0, :] += x
    rr_wheel[1, :] += y
    rl_wheel = np.dot(rot2, rl_wheel)
    rl_wheel[0, :] += x
    rl_wheel[1, :] += y
    vehicle_outline = np.dot(rot2, vehicle_outline)
    vehicle_outline[0, :] += x
    vehicle_outline[1, :] += y

    ax.plot(fr_wheel[0, :], fr_wheel[1, :], color)
    ax.plot(rr_wheel[0, :], rr_wheel[1, :], color)
    ax.plot(fl_wheel[0, :], fl_wheel[1, :], color)
    ax.plot(rl_wheel[0, :], rl_wheel[1, :], color)

    ax.plot(vehicle_outline[0, :], vehicle_outline[1, :], color)
    ax.plot(x, y, 'o')

    # 绘制箭头
    ax.arrow(x, y, vehicle_info.L * 0.5 * math.cos(yaw), vehicle_info.L * 0.5 * math.sin(yaw),
              head_width=vehicle_info.L * 0.2, head_length=vehicle_info.L * 0.2, fc=color, ec=color)
    ax.axis('equal')

path_generator.py

"""
路径轨迹生成器
"""

import math
import numpy as np

class Path:
    def __init__(self, isReverse = False):
        self.is_reverse = isReverse
        self.ref_line = self.design_reference_line()
        self.ref_yaw = self.cal_yaw()
        self.ref_s = self.cal_accumulated_s()
        self.ref_kappa = self.cal_kappa()
        if isReverse:
            # 反转数组
            self.ref_line[:, 0] = np.flip(self.ref_line[:, 0], 0)
            self.ref_yaw = np.flip(self.ref_yaw, 0)
            self.ref_kappa = np.flip(self.ref_kappa, 0)  # 确保self.ref_kappa是NumPy数组

    def design_reference_line(self):
        rx = np.linspace(0, 50, 2000) + 5 # x坐标
        ry = 20 * np.sin(rx / 20.0) + 60  # y坐标
        rv = np.full(2000, (-2 if self.is_reverse else 2))
        return np.column_stack((rx, ry, rv))

    def cal_yaw(self):
        yaw = []
        for i in range(len(self.ref_line)):
            if i == 0:
                yaw.append(math.atan2(self.ref_line[i + 1, 1] - self.ref_line[i, 1],
                                 self.ref_line[i + 1, 0] - self.ref_line[i, 0]))
            elif i == len(self.ref_line) - 1:
                yaw.append(math.atan2(self.ref_line[i, 1] - self.ref_line[i - 1, 1],
                                 self.ref_line[i, 0] - self.ref_line[i - 1, 0]))
            else:
                yaw.append(math.atan2(self.ref_line[i + 1, 1] - self.ref_line[i - 1, 1],
                                  self.ref_line[i + 1, 0] - self.ref_line[i - 1, 0]))
        return yaw

    def cal_accumulated_s(self):
        rx = self.ref_line[:, 0]
        ry = self.ref_line[:, 1]
        if self.is_reverse:
            rx = np.flip(rx, 0)
            ry = np.flip(ry, 0)  # 反转x和y坐标

        s = []
        distance = 0
        for i in range(len(self.ref_line)):
            if i == 0:
                s.append(0.0)
            else:
                distance = np.sqrt((rx[i] - rx[i-1]) ** 2 + (ry[i] - ry[i-1]) ** 2)
                # 将新计算的距离累加到累积距离数组中
                s.append(distance + s[i - 1])
        return s

    def cal_kappa(self):
        # 计算曲线各点的切向量
        dp = np.gradient(self.ref_line.T, axis=1)
        # 计算曲线各点的二阶导数
        d2p = np.gradient(dp, axis=1)
        # 计算曲率
        kappa = (d2p[0] * dp[1] - d2p[1] * dp[0]) / ((dp[0] ** 2 + dp[1] ** 2) ** (3 / 2))

        return kappa

    def get_ref_line_info(self):
        return self.ref_line[:, 0], self.ref_line[:, 1], self.ref_line[:, 2], self.ref_yaw, self.ref_s, self.ref_kappa

main.py

from kinematic_bicycle_model import Vehicle, VehicleInfo, draw_vehicle
from pure_pursuit_controller import pure_pursuit_steer_control
from path_generator import Path
import numpy as np
import matplotlib.pyplot as plt
import imageio.v2 as imageio

MAX_SIMULATION_TIME = 200.0  # 程序最大运行时间200*dt

def main():
    # 设置跟踪轨迹
    rx, ry, rv, ref_yaw, ref_s, ref_kappa = Path(True).get_ref_line_info()
    ref_path = np.column_stack((rx, ry, rv, ref_yaw, ref_s, ref_kappa))
    # 假设车辆初始位置为（50，60），航向角yaw=0.0，速度为-2m/s，时间周期dt为0.1秒
    vehicle1 = Vehicle(x=50.0,
                      y=60.0,
                      yaw=0.0,
                      v=-2.0,
                      dt=0.1,
                      l=VehicleInfo.L)
    vehicle2 = Vehicle(x=50.0,
                       y=60.0,
                       yaw=0.0,
                       v=-2.0,
                       dt=0.1,
                       l=VehicleInfo.L)
    time = 0.0  # 初始时间
    target_ind1 = 0
    target_ind2 = 0
    # 记录车辆轨迹
    trajectory_x1 = []
    trajectory_y1 = []
    trajectory_x2 = []
    trajectory_y2 = []
    lat_err1 = []  # 记录横向误差
    lat_err2 = []  # 记录横向误差

    i = 0
    image_list = []  # 存储图片
    plt.figure(1, dpi=100)

    old_index1 = None
    old_index2 = None
    last_idx = ref_path.shape[0] - 1  # 跟踪轨迹的最后一个点的索引
    while MAX_SIMULATION_TIME >= time and (last_idx > target_ind1 or last_idx > target_ind2):
        time += vehicle1.dt  # 累加一次时间周期

        if last_idx > target_ind1:
            delta_f1, target_ind1, e_y1 = pure_pursuit_steer_control(vehicle1, ref_path, old_index1, False)
            old_index1 = target_ind1
            # 横向误差
            lat_err1.append(e_y1)
            # 更新车辆状态
            vehicle1.update(0.0, delta_f1, np.pi / 10)  # 由于假设纵向匀速运动，所以加速度a=0.0
            trajectory_x1.append(vehicle1.x)
            trajectory_y1.append(vehicle1.y)

        if last_idx > target_ind2:
            delta_f2, target_ind2, e_y2 = pure_pursuit_steer_control(vehicle2, ref_path, old_index2, True)
            old_index2 = target_ind2
            lat_err2.append(e_y2)
            vehicle2.update(0.0, delta_f2, np.pi / 10)  # 由于假设纵向匀速运动，所以加速度a=0.0
            trajectory_x2.append(vehicle2.x)
            trajectory_y2.append(vehicle2.y)

        # 显示动图
        plt.subplots_adjust(hspace=0.5, wspace=0.5)  # 调整垂直和水平间距
        plt.subplot(3, 1, 1)
        plt.cla()
        plt.plot(ref_path[:, 0], ref_path[:, 1], '-.k', linewidth=1.0)
        draw_vehicle(vehicle1.x, vehicle1.y, vehicle1.yaw, vehicle1.steer, plt, color='blue')
        draw_vehicle(vehicle2.x, vehicle2.y, vehicle2.yaw, vehicle2.steer, plt, color='green')

        plt.plot(trajectory_x1, trajectory_y1, "-b", label="trajectory1")
        plt.plot(trajectory_x2, trajectory_y2, "-g", label="trajectory2")
        plt.plot(ref_path[target_ind1, 0], ref_path[target_ind1, 1], "b-o", label="target1")
        plt.plot(ref_path[target_ind2, 0], ref_path[target_ind2, 1], "g-o", label="target2")
        plt.xlim(min(vehicle1.x, vehicle2.x, ref_path[target_ind1, 0], ref_path[target_ind2, 0])-3, max(vehicle1.x, vehicle2.x, ref_path[target_ind1, 0], ref_path[target_ind2, 0])+3)
        plt.ylim(min(vehicle1.y, vehicle2.y, ref_path[target_ind1, 1], ref_path[target_ind2, 1])-3, max(vehicle1.y, vehicle2.y, ref_path[target_ind1, 1], ref_path[target_ind2, 1])+3)
        plt.grid(True)

        plt.subplot(3, 1, 2)
        plt.cla()
        plt.plot(ref_path[:, 0], ref_path[:, 1], '-.k', linewidth=1.0)
        plt.plot(trajectory_x1, trajectory_y1, 'b')
        plt.plot(trajectory_x2, trajectory_y2, 'g')
        plt.title("tracking effect")
        plt.xlim(min(trajectory_x1[-1], trajectory_x2[-1]) - 1, max(trajectory_x1[-1], trajectory_x2[-1]) + 1)
        plt.ylim(min(trajectory_y1[-1], trajectory_y2[-1]) - 0.5, max(trajectory_y1[-1], trajectory_y2[-1]) + 0.5)
        plt.grid(True)

        plt.subplot(3, 1, 3)
        plt.cla()
        plt.plot(lat_err1, 'b')
        plt.plot(lat_err2, 'g')
        plt.title("lateral error")
        plt.xlim((len(trajectory_x1) + len(trajectory_x2)) / 2 - 35, (len(trajectory_x1) + len(trajectory_x2)) / 2 + 35)
        plt.ylim(min(lat_err1[-1], lat_err2[-1]) - 0.1, max(lat_err1[-1], lat_err2[-1]) + 0.1)

        plt.grid(True)
        plt.pause(0.001)
        plt.savefig("temp.png")
        i += 1
        if (i % 5) == 0:
            image_list.append(imageio.imread("temp.png"))

    imageio.mimsave("display.gif", image_list, duration=0.1)

    plt.figure(2)
    plt.subplots_adjust(hspace=0.5, wspace=0.5)  # 调整垂直和水平间距
    plt.subplot(2, 1, 1)
    plt.plot(ref_path[:, 0], ref_path[:, 1], '-.k', linewidth=1.0)
    plt.plot(trajectory_x1, trajectory_y1, 'b')
    plt.plot(trajectory_x2, trajectory_y2, 'g')
    plt.title("actual tracking effect")
    plt.grid(True)

    plt.subplot(2, 1, 2)
    plt.plot(lat_err1, 'b')
    plt.plot(lat_err2, 'g')
    plt.title("lateral error")
    plt.grid(True)
    plt.show()


if __name__ == '__main__':
    main()

运行效果：

在这里插入图片描述

控制效果和横向误差：

在这里插入图片描述

图中绿色小车所呈现的倒车效果，是通过应用模糊控制技术进行优化后纯跟踪实现的，而蓝色小车则采用了传统的纯跟踪倒车方法。通过对比可以明显观察到，模糊控制优化后的绿色小车在纯跟踪性能方面取得了显著提升，具体表现在更合理的前视距离控制、更小的跟踪误差以及更短的调节时间上。此外，无论是最大跟踪误差还是全程的平均误差，都实现了显著降低。这表明，经过改进的算法不仅能够更好地适应实际车辆行驶过程中的动态变化，而且在提高路径跟踪效果方面也展现出了显著的成效。