MPC的横向控制与算法仿真实现

1. 引言

随着智能交通系统和自动驾驶技术的发展，车辆的横向控制成为了研究的热点。横向控制指的是对车辆在行驶过程中的水平运动进行控制，包括车辆的转向、车道保持、避障等。这些控制任务对于提高道路安全性、减少交通事故、提升驾驶舒适性具有重要意义。模型预测控制（Model Predictive Control, MPC）作为一种先进的控制策略，因其在处理多变量系统、非线性系统以及约束条件下的优越性能，被广泛应用于车辆横向控制领域。

2. 模型预测控制（MPC）

2.1 基础知识

二次规划（Quadratic Programming, QP）是数学优化中的一个重要分支，它涉及寻找一个使得二次函数达到最小值的变量向量的优化问题。这类问题广泛应用于经济学、工程学、机器学习等领域。

二次规划问题的一般形式可以表示为：

$$\begin{array}{rl}\underset{u}{\mathrm{mini}}& J=\frac{1}{2}{\mathbf{u}}^T\mathbf{Hu}+{\mathbf{u}}^T\mathbf{f}\\ \text{subject to}& {\mathbf{M}}_{\mathbf{le}}\mathbf{u}\le {\mathbf{b}}_{\mathbf{le}}\\ & {\mathbf{M}}_{\mathbf{eq}}\mathbf{u}={\mathbf{b}}_{\mathbf{eq}}\\ & {\mathbf{u}}_{\mathbf{min}}\le \mathbf{u}\le {\mathbf{u}}_{\mathbf{max}}\end{array}\text{\hspace{1em}(1)}$$

其中：

• $\mathbf{u}$是决策变量的向量；

• $\mathbf{H}$是一个对称正定矩阵，表示二次项的系数；

• $\mathbf{f}$是线性项的系数向量；

• ${\mathbf{M}}_{\mathbf{le}}$和 ${\mathbf{b}}_{\mathbf{le}}$分别是不等式约束的系数矩阵和常数向量；

• ${\mathbf{M}}_{\mathbf{eq}}$和 ${\mathbf{b}}_{\mathbf{eq}}$分别是等式约束的系数矩阵和常数向量；

• ${\mathbf{u}}_{\mathbf{min}}$和${\mathbf{u}}_{\mathbf{max}}$是变量向量$\mathbf{u}$的取值范围的上限向量和下限向量。

注：当$\mathbf{H}$为正定矩阵时，目标函数存在全局唯一最优解，$\mathbf{H}$半正定时，目标函数存在全局最优解（不唯一），$\mathbf{H}$为不定矩阵，目标函数非凸，存在多个局部最小值和稳定点。

解决二次规划问题通常有多种方法，包括但不限于：

1. 解析方法：对于一些特殊形式的二次规划问题，可以通过解析方法直接求得最优解。例如，当 $Q$矩阵是正定时，问题可以通过求解相应的线性规划问题来简化。

2. 迭代方法：对于更一般的问题，可以使用迭代方法来求解。这类方法包括内点法、梯度投影法、序列二次规划（Sequential Quadratic Programming, SQP）等。

3. 软件包：存在多种优化软件包可以用于求解二次规划问题，如MATLAB的quadprog函数，Python的cvxpy库，以及专业的数学优化软件如Gurobi和CPLEX等。

在实际应用中，选择合适的方法取决于问题的规模、结构和特定要求。对于大规模或非常复杂的问题，通常需要使用专业的优化软件和算法来求解。

二次规划问题在实际中的重要性体现在其模型能够很好地描述现实世界中的许多优化问题，尤其是在目标函数包含二次项时。例如，在资源分配、投资组合优化、模型预测控制、机器学习中的支持向量机（SVM）模型训练等问题中，二次规划都扮演着关键角色。在模型预测控制中，我们需要在每个时刻通过预测模型来计算未来一段时间内的最优控制输入，而这个过程可以转化为一个带有约束的二次规划问题，进而使用二次规划求解方法求出最优控制结果。

2.2 MPC的整体流程

模型预测控制是一种先进的控制策略，它基于系统模型来预测未来的系统行为，并在此基础上优化控制输入。MPC的核心思想是在每一个控制迭代中，解决一个有限时间范围内的优化问题，以实现对系统未来行为的预测和控制。考虑到系统的不确定性、测量误差等因素，在实际的控制应用中，通常会选取预测区间内最优控制序列的第一项作为当前时刻的控制输入。

MPC的基本步骤包括：

1. 系统模型：建立一个描述系统动态行为的数学模型。这个模型通常是系统的微分方程或差分方程，用于预测系统状态随时间的变化。
2. 预测：在当前时间点，使用系统模型预测未来一段时间内（称为预测视界）的系统状态。预测通常考虑到未来的控制输入和系统的初始状态。
3. 目标函数和约束：定义一个目标函数，用于评估系统状态和控制输入的性能。目标函数通常包括跟踪误差和控制努力的加权和。同时，定义一系列约束条件，包括系统的物理约束（如速度、加速度的上下限）、操作约束（如控制输入的变化率）和安全约束（如避免碰撞）。
4. 优化问题：在每个控制迭代中，基于当前状态和预测模型，求解一个优化问题以确定最优的控制序列。优化问题是在满足所有约束条件的前提下，最小化目标函数。
5. 反馈校正：从优化问题的解中提取当前时刻的控制输入，并将其应用于系统。在下一个控制迭代，根据新的系统状态重复上述过程。

2.3 MPC的设计求解

模型预测控制器多为数字控制，因此多应用于离散系统，这里我们使用离散线性时不变系统为例，假设其系统状态方程为

$${\mathbf{x}}_{k+1}=\mathbf{A}{\mathbf{x}}_k+\mathbf{B}{\mathbf{u}}_k\text{\hspace{1em}(2)}$$

其中${\mathbf{x}}_k$为$m\times 1$的状态向量，$\mathbf{A}$为$m\times m$的状态矩阵，$\mathbf{B}$为$n\times n$的输入矩阵，${\mathbf{u}}_k$为$n\times 1$的输入向量，其对应的二次型性能指标可以定义为

$$J=\sum _{k=0}^{N-1}({\mathbf{x}}_k^T\mathbf{Q}{\mathbf{x}}_k+{\mathbf{u}}_k^T\mathbf{R}{\mathbf{u}}_k)+{\mathbf{x}}_N^T\mathbf{F}{\mathbf{x}}_N\text{\hspace{1em}(3)}$$

其中$\mathbf{Q}$、$\mathbf{R}$和$\mathbf{F}$分别为$m\times m$的运行状态代价的对称矩阵、$n\times n$的输入代价的对称矩阵和$m\times m$的末端状态代价的对称矩阵。

我们将$k$时刻在$k+i$时的预测输入值记为${\mathbf{u}}_{k+i|k}$，其中$k=0,1,\dots ,N_p-1$，$N_p$为预测视界，即预测区间。同理其对应的的预测输出${\mathbf{x}}_{k+i|k}$。

设系统预测视界为$N_p$时间段内的预测控制输入为${\mathbf{u}}_k,{\mathbf{u}}_{k+1|k},\dots ,{\mathbf{u}}_{k+N_p-1|k}$，根据2式可得

$$\begin{array}{rl}{\mathbf{x}}_{k+1|k}& ={\mathbf{Ax}}_{k|k}+{\mathbf{Bu}}_{k|k}\\ {\mathbf{x}}_{k+2|k}& ={\mathbf{Ax}}_{k+1|k}+{\mathbf{Bu}}_{k+1|k}\\ & =\mathbf{A}({\mathbf{Ax}}_{k|k}+{\mathbf{Bu}}_{k|k})+{\mathbf{Bu}}_{k+1|k}\\ & ={{\mathbf{A}}^2\mathbf{x}}_{k|k}+{\mathbf{ABu}}_{k|k}+{\mathbf{Bu}}_{k+1|k}\\ {\mathbf{x}}_{k+3|k}& ={\mathbf{Ax}}_{k+2|k}+{\mathbf{Bu}}_{k+2|k}\\ & =\mathbf{A}({{\mathbf{A}}^2\mathbf{x}}_{k|k}+{\mathbf{ABu}}_{k|k}+{\mathbf{Bu}}_{k+1|k})+{\mathbf{Bu}}_{k+2|k}\\ & ={{\mathbf{A}}^3\mathbf{x}}_{k|k}+{{\mathbf{A}}^2\mathbf{Bu}}_{k|k}+{\mathbf{ABu}}_{k+1|k}+{\mathbf{Bu}}_{k+2|k}\\ \dots & \\ {\mathbf{x}}_{k+N_p}& ={{\mathbf{A}}^{{\mathbf{N}}_{\mathbf{p}}}\mathbf{x}}_{k|k}+{{\mathbf{A}}^{{\mathbf{N}}_{\mathbf{p}}-1}\mathbf{Bu}}_{k|k}+\cdots +{\mathbf{ABu}}_{k+N_p-2|k}+\mathbf{B}{u}_{k+N_p-1|k}\end{array}\text{\hspace{1em}(4)}$$

将上式转化为矩阵向量形式，可得预测空间内系统的状态方程为：

$$\begin{array}{r}\left[\begin{array}{c}{\mathbf{x}}_{k+1|k}\\ {\mathbf{x}}_{k+2|k}\\ \dots \\ {\mathbf{x}}_{k+N_P|k}\end{array}\right]=\left[\begin{array}{c}\mathbf{A}\\ {\mathbf{A}}^2\\ \dots \\ {\mathbf{A}}^{{\mathbf{N}}_{\mathbf{p}}}\end{array}\right]{\mathbf{x}}_{k|k}+\left[\begin{array}{cccc}\mathbf{B}& 0& \dots & 0\\ \mathbf{AB}& \mathbf{B}& \dots & 0\\ ⋮& ⋮& \ddots & ⋮\\ {\mathbf{A}}^{{\mathbf{N}}_{\mathbf{p}}-1}\mathbf{B}& {\mathbf{A}}^{{\mathbf{N}}_{\mathbf{p}}-2}\mathbf{B}& \dots & 0\end{array}\right]\left[\begin{array}{c}{\mathbf{u}}_{k|k}\\ {\mathbf{u}}_{k+1|k}\\ \dots \\ {\mathbf{u}}_{k+N_p-1|k}\end{array}\right]\end{array}\text{\hspace{1em}(5)}$$

为了简化后面的分析，我们定义

$$\begin{array}{rl}{\mathbf{X}}_k& =\left[\begin{array}{c}{\mathbf{x}}_{k+1|k}\\ {\mathbf{x}}_{k+2|k}\\ \dots \\ {\mathbf{x}}_{k+N_P|k}\end{array}\right]\\ \mathbf{M}& =\left[\begin{array}{c}\mathbf{A}\\ {\mathbf{A}}^2\\ \dots \\ {\mathbf{A}}^{{\mathbf{N}}_{\mathbf{p}}}\end{array}\right]\\ \mathbf{C}& =\left[\begin{array}{cccc}\mathbf{B}& 0& \dots & 0\\ \mathbf{AB}& \mathbf{B}& \dots & 0\\ ⋮& ⋮& \ddots & ⋮\\ {\mathbf{A}}^{{\mathbf{N}}_{\mathbf{p}}-1}\mathbf{B}& {\mathbf{A}}^{{\mathbf{N}}_{\mathbf{p}}-2}\mathbf{B}& \dots & 0\end{array}\right]\\ {\mathbf{U}}_k& =\left[\begin{array}{c}{\mathbf{u}}_{k|k}\\ {\mathbf{u}}_{k+1|k}\\ \dots \\ {\mathbf{u}}_{k+N_p-1|k}\end{array}\right]\end{array}\text{\hspace{1em}(6)}$$

则5式可以记为

$${\mathbf{X}}_k=\mathbf{M}{\mathbf{x}}_{k|k}+\mathbf{C}{\mathbf{U}}_k\text{\hspace{1em}(7)}$$

结合3式的代价函数，可得在$k$时刻，当预测区间为$N_p$的时候，该预测区间内的性能指标为

$$\begin{array}{rl}{J}_p& =\sum _{i=0}^{N_p-1}({\mathbf{x}}_{k+i|k}^T\mathbf{Q}{\mathbf{x}}_{k+i|k}+{\mathbf{u}}_{k+i|k}^T\mathbf{R}{\mathbf{u}}_{k+i|k})+{\mathbf{x}}_{k+N_p|k}^T\mathbf{F}{\mathbf{x}}_{k+N_p|k}\\ & =\sum _{i=0}^{N_p-1}({\mathbf{x}}_{k+i|k}^T\mathbf{Q}{\mathbf{x}}_{k+i|k})+\sum _{i=0}^{N_p-1}({\mathbf{u}}_{k+i|k}^T\mathbf{R}{\mathbf{u}}_{k+i|k})+{\mathbf{x}}_{k+N_p|k}^T\mathbf{F}{\mathbf{x}}_{k+N_p|k}\\ & ={\mathbf{x}}_{k|k}^T\mathbf{Q}{\mathbf{x}}_{k|k}+\sum _{i=1}^{N_p-1}({\mathbf{x}}_{k+i|k}^T\mathbf{Q}{\mathbf{x}}_{k+i|k})+\sum _{i=0}^{N_p-1}({\mathbf{u}}_{k+i|k}^T\mathbf{R}{\mathbf{u}}_{k+i|k})+{\mathbf{x}}_{k+N_p|k}^T\mathbf{F}{\mathbf{x}}_{k+N_p|k}\end{array}\text{\hspace{1em}(8)}$$

因为${\mathbf{x}}_{k|k}$为系统初对应的就是$k$时刻的系统状态，即为当前时刻$k$的初始状态，是已知量，因此后面的${\mathbf{x}}_{k+i+1|k}$可以通过2式用${\mathbf{u}}_{k+i|k}$和${\mathbf{x}}_{k+i|k}$来表达，因此$J_p$的大小仅与输入变量$\mathbf{u}$有关，此时最优问题已经转换为二次规划问题，使用现有的二次规划进行求解，获取预测区间内的最优序列，然后取第一个最优控制作为当前系统的输入。

注：对于8式不需要把${\mathbf{x}}_{k+i+1|k}$用${\mathbf{u}}_{k+i-1|k}$去展开表示，添加2式的等式约束就可以实现这个效果，详见后面的代码实现。

3. 车辆运动学MPC设计

在《车辆运动学模型的线性化和离散化及代码实现》中，我们详细介绍了单车模型的线性化和离散化，其离散线性化后的微分方程如下

$$\begin{array}{rl}{\mathbf{x}}_{\mathbf{e}}(k+1)& ={\mathbf{A}}_{\mathbf{e}}{\mathbf{x}}_{\mathbf{e}}(k)+{\mathbf{B}}_{\mathbf{e}}{\mathbf{u}}_{\mathbf{e}}(k)\\ & ={\mathbf{A}}_{\mathbf{e}}(\mathbf{x}(k)-{\mathbf{x}}_{\mathbf{r}}\mathbf{ef}(k))+{\mathbf{B}}_{\mathbf{e}}({\mathbf{u}}_{\mathbf{e}}(k)-{\mathbf{u}}_{\mathbf{ref}}(k))\\ & =\left[\begin{array}{ccc}1& 0& -T{v}_{r}sin{\phi }_r\\ 0& 1& T{v}_{r}cos{\phi }_r\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}x-x_r\\ y-y_r\\ \phi -{\phi }_r\end{array}\right]+\left[\begin{array}{cc}Tcos{\phi }_r& 0\\ Tsin{\phi }_r& 0\\ \frac{T{v}_{r}tan{\delta }_{fr}}{L}& \frac{T{v}_r}{Lco{s}^2{\delta }_{fr}}\end{array}\right]\left[\begin{array}{c}v-v_r\\ \delta -{\delta }_r\end{array}\right]\end{array}\text{\hspace{1em}(9)}$$

其中$T$为采样步长，定义代价函数
$$J=\sum _{k=0}^{N-1}({\mathbf{x}}_k^T\mathbf{Q}{\mathbf{x}}_k+{\mathbf{u}}_k^T\mathbf{R}{\mathbf{u}}_k)+{\mathbf{x}}_N^T\mathbf{F}{\mathbf{x}}_N\text{\hspace{1em}(10)}$$

其中：

• ${\mathbf{x}}_k$ 是在离散时间步$k$的系统状态，即车辆在$k$时刻的状态。
• ${\mathbf{u}}_k$是在时间步$k$的控制输入。
• ${{\mathbf{x}}_{\mathbf{ref}}}_k$ 根据参考线计算出来在离散时间步$k$的理想系统状态。
• ${{\mathbf{u}}_{\mathbf{ref}}}_k$是根据参考线在时间步$k$的理想控制输入。
• $\mathbf{Q}$是状态权重$m\times m$的半正定方阵，用于衡量状态的代价，通常将其设计为对角矩阵。
• $\mathbf{R}$是控制权重$n\times n$的正定对称矩阵，用于衡量控制输入的代价，通常将其设计为对角矩阵。
• $\mathbf{F}$是末端状态权重$m\times m$的半正定方阵，用于衡量最终状态的代价，通常将其设计为对角矩阵。
• $\mathbf{N}$是控制的总时间步数。

根据第二节的推导，我们可以得到其对应的预测空间$N_p$内的状态方程和代价函数如下
$$\begin{array}{rl}\underset{u}{\mathrm{mini}}{J}_p& ={{\mathbf{x}}_{\mathbf{e}}}_{k|k}^T\mathbf{Q}{{\mathbf{x}}_{\mathbf{e}}}_{k|k}+\sum _{i=1}^{N_p-1}({{\mathbf{x}}_{\mathbf{e}}}_{k+i|k}^T\mathbf{Q}{{\mathbf{x}}_{\mathbf{e}}}_{k+i|k})+\sum _{i=0}^{N_p-1}({{\mathbf{u}}_{\mathbf{e}}}_{k+i|k}^T\mathbf{R}{{\mathbf{u}}_{\mathbf{e}}}_{k+i|k})+{{\mathbf{x}}_{\mathbf{e}}}_{k+N_p|k}^T\mathbf{F}{{\mathbf{x}}_{\mathbf{e}}}_{k+N_p|k}\\ \text{subject to}& {\mathbf{x}}_{\mathbf{e}}(k+1)={\mathbf{A}}_{\mathbf{e}}{\mathbf{x}}_{\mathbf{e}}(k)+{\mathbf{B}}_{\mathbf{e}}{\mathbf{u}}_{\mathbf{e}}(k)\\ & {\mathbf{x}}_{\mathbf{e}}(0)={{\mathbf{x}}_{\mathbf{e}}}_0\\ & |\mathbf{u}|\le {\mathbf{u}}_{\mathbf{max}}\end{array}\text{\hspace{1em}(11)}$$

其中

• 公式11.2：车辆模型的运动学约束。
• 公式11.3：初始状态约束，${{\mathbf{x}}_{\mathbf{e}}}_0$为车辆当前状态，即预测区间内初始状态。
• 公式11.4：输入量约束。

4. 算法和仿真实现

kinematicsMPC.py

import numpy as np
import math
import cvxpy as cp
from kinematic_bicycle_model import VehicleInfo, update_ABMatrix


# 系统配置
NX = 3  # 状态向量的个数: x = [x, y, yaw]
NU = 2  # 输入向量的个数: u = [v, delta]
NP = 5  # 有限时间视界长度：预测过程中考虑的时间范围的有限长度
MAX_V = 20  # 最大车速(m/s)

# MPC config
Q = np.diag([2.0, 2.0, 2.0])  # 运行状态代价
F = np.diag([2.0, 2.0, 2.0])  # 末端状态代价
R = np.diag([0.01, 0.1])  # 输入状态代价


def calc_preparation(vehicle, ref_path):
    """
    计算xref、uref、index和er
    """

    rx, ry, rv, ryaw, rkappa = ref_path[:, 0], ref_path[:, 1], ref_path[:, 2], ref_path[:, 3], ref_path[:, 5]
    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]])
    er = np.dot(vec_target_2_rear, vec_nr)

    xref = np.zeros((NX, NP + 1))
    uref = np.zeros((NU, NP + 1))

    for i in range(NP+1):
        ind = min(index + i, len(rx)-1)
        xref[0, i] = rx[ind]
        xref[1, i] = ry[ind]
        xref[2, i] = ryaw[ind]
        uref[0, i] = rv[ind]
        uref[1, i] = math.atan2(vehicle.L*rkappa[ind], 1)
    return xref, uref, index, er



def MPCController(vehicle, ref_path):
    xref, uref, index, er = calc_preparation(vehicle, ref_path)

    x0 = [vehicle.x, vehicle.y, vehicle.yaw]
    x = cp.Variable((NX, NP + 1))
    u = cp.Variable((NU, NP))
    cost = 0.0  # 代价函数
    constraints = []  # 约束条件

    x[:, 0] == x0
    for i in range(NP):
        cost += cp.quad_form(u[:, i] - uref[:, i], R)
        if i != 0:
            cost += cp.quad_form(x[:, i] - xref[:, i], Q)
        A, B = update_ABMatrix(vehicle, uref[1, i], xref[2, i])
        constraints += [x[:, i + 1] - xref[:, i + 1] == A @ (x[:, i] - xref[:, i]) + B @ (u[:, i] - uref[:, i])]

    cost += cp.quad_form(x[:, NP] - xref[:, NP], F)

    constraints += [(x[:, 0]) == x0]
    constraints += [cp.abs(u[0, :]) <= MAX_V]
    constraints += [cp.abs(u[1, :]) <= VehicleInfo.MAX_STEER]

    problem = cp.Problem(cp.Minimize(cost), constraints)
    problem.solve(solver=cp.ECOS, verbose=False)

    if problem.status == cp.OPTIMAL or problem.status == cp.OPTIMAL_INACCURATE:
        opt_delta = u.value[:, 0]
        return opt_delta[1], index, er

    else:
        print("Error: MPC solution failed !")
        return None, index, er

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.axis('equal')


def update_ABMatrix(vehicle, ref_delta, ref_yaw):
    """
    计算离散线性车辆运动学模型状态矩阵A和输入矩阵B
    return: A, B
    """
    A = np.matrix([
        [1.0, 0.0, -vehicle.v * vehicle.dt * math.sin(ref_yaw)],
        [0.0, 1.0, vehicle.v * vehicle.dt * math.cos(ref_yaw)],
        [0.0, 0.0, 1.0]])

    B = np.matrix([
        [vehicle.dt * math.cos(ref_yaw), 0],
        [vehicle.dt * math.sin(ref_yaw), 0],
        [vehicle.dt * math.tan(ref_delta) / vehicle.L,
         vehicle.v * vehicle.dt / (vehicle.L * math.cos(ref_delta) * math.cos(ref_delta))]])
    return A, B

path_generator.py

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

import math
import numpy as np

class Path:
    def __init__(self):
        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()

    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)
        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):
        s = []
        for i in range(len(self.ref_line)):
            if i == 0:
                s.append(0.0)
            else:
                s.append(math.sqrt((self.ref_line[i, 0] - self.ref_line[i-1, 0]) ** 2
                                 + (self.ref_line[i, 1] - self.ref_line[i-1, 1]) ** 2))
        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 kinematicsMPC import MPCController
from path_generator import Path
import numpy as np
import matplotlib.pyplot as plt
import imageio.v2 as imageio
import sys

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

def main():
    # 设置跟踪轨迹
    rx, ry, rv, ref_yaw, ref_s, ref_kappa = Path().get_ref_line_info()
    ref_path = np.column_stack((rx, ry, rv, ref_yaw, ref_s, ref_kappa))
    # 假设车辆初始位置为（5，60），航向角yaw=0.0，速度为2m/s，时间周期dt为0.1秒
    vehicle = Vehicle(x=5.0,
                      y=60.0,
                      yaw=0.0,
                      v=2.0,
                      dt=0.1,
                      l=VehicleInfo.L)

    time = 0.0  # 初始时间
    target_ind = 0
    # 记录车辆轨迹
    trajectory_x = []
    trajectory_y = []
    lat_err = []  # 记录横向误差

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

    last_idx = ref_path.shape[0] - 1  # 跟踪轨迹的最后一个点的索引
    while MAX_SIMULATION_TIME >= time and last_idx > target_ind:
        time += vehicle.dt  # 累加一次时间周期
        # MPC
        delta_f, target_ind, e_y = MPCController(vehicle, ref_path)
        if delta_f is None:
            print("An error occurred, exit...")
            sys.exit(1)

            # 横向误差
        lat_err.append(e_y)

        # 更新车辆状态
        vehicle.update(0.0, delta_f, np.pi / 10)  # 由于假设纵向匀速运动，所以加速度a=0.0
        trajectory_x.append(vehicle.x)
        trajectory_y.append(vehicle.y)

        # 显示动图
        plt.cla()
        plt.plot(ref_path[:, 0], ref_path[:, 1], '-.b', linewidth=1.0)
        draw_vehicle(vehicle.x, vehicle.y, vehicle.yaw, vehicle.steer, plt)

        plt.plot(trajectory_x, trajectory_y, "-r", label="trajectory")
        plt.plot(ref_path[target_ind, 0], ref_path[target_ind, 1], "go", label="target")
        plt.axis("equal")
        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.subplot(2, 1, 1)
    plt.plot(ref_path[:, 0], ref_path[:, 1], '-.b', linewidth=1.0)
    plt.plot(trajectory_x, trajectory_y, 'r')
    plt.title("actual tracking effect")

    plt.subplot(2, 1, 2)
    plt.plot(lat_err)
    plt.title("lateral error")
    plt.show()


if __name__ == '__main__':
    main()

运行结果: