自动驾驶控制-二/三自由度动力学MPC任意路径跟踪 是可以跟踪各种自定义路径，可以自己更改参考路径的 carsim和simulink联合仿真，基于车辆二自由度动力学模型的mpc跟踪任意路径。 蓝色为全局参考路径，黄色为预测时域内的 参考路径，红色为车辆行驶轨迹。

在自动驾驶领域，路径跟踪是核心任务之一。今天咱们就来聊聊基于车辆二自由度动力学模型，借助MPC（模型预测控制）实现任意路径跟踪，并且通过Carsim和Simulink联合仿真来验证这一过程。

可自定义的神奇路径跟踪

我们的目标是让车辆能够跟踪各种自定义路径，并且可以随心所欲地更改参考路径。想象一下，无论是蜿蜒曲折的山路，还是城市中复杂的街道布局，车辆都能精准按照设定路径行驶，这背后的技术魅力十足。

Carsim与Simulink联合仿真

Carsim 擅长高精度的车辆动力学建模，Simulink则在控制系统设计与仿真方面表现出色。二者联合，简直是天作之合。通过这种联合方式，我们能更真实地模拟车辆在实际行驶中的各种状况。

基于二自由度动力学模型的MPC

二自由度动力学模型简介

车辆的二自由度动力学模型主要考虑车辆的侧向运动和横摆运动。简单来说，它把车辆简化成一个在平面内运动的刚体，主要关注侧向力和横摆力矩对车辆运动状态的影响。以下是一个简单示意性的二自由度动力学模型的状态方程代码示例（以Python为例，实际应用可能更复杂且结合专业库）：

import numpy as np


# 车辆参数
m = 1500  # 车辆质量（kg）
lf = 1.2  # 前轴到质心的距离（m）
lr = 1.5  # 后轴到质心的距离（m）
Iz = 2500  # 车辆绕z轴的转动惯量（kg·m²）
Cf = 60000  # 前轮侧偏刚度（N/rad）
Cr = 70000  # 后轮侧偏刚度（N/rad）


def two_dof_model(x, u, dt):
    # x 状态变量 [y, psi, vy, r]
    # y 侧向位移（m）
    # psi 横摆角（rad）
    # vy 侧向速度（m/s）
    # r 横摆角速度（rad/s）
    # u 输入变量 [delta, a]
    # delta 前轮转角（rad）
    # a 纵向加速度（m/s²）

    y = x[0]
    psi = x[1]
    vy = x[2]
    r = x[3]
    delta = u[0]
    a = u[1]

    beta = np.arctan2(vy, a + 0.0001)  # 质心侧偏角，避免除零

    Fyf = -Cf * (delta - np.arctan2(vy + lf * r, a + 0.0001))
    Fyr = -Cr * (-np.arctan2(vy - lr * r, a + 0.0001))

    d_y = vy * np.cos(psi) - a * np.sin(psi)
    d_psi = r
    d_vy = (Fyf * np.cos(delta) + Fyr - m * a * beta) / m
    d_r = (lf * Fyf * np.cos(delta) - lr * Fyr) / Iz

    new_x = np.array([y + d_y * dt, psi + d_psi * dt, vy + d_vy * dt, r + d_r * dt])
    return new_x

这段代码定义了一个简单的二自由度动力学模型函数twodofmodel，它接收当前状态x、输入u和时间步长dt，返回下一时刻的状态。通过这个模型，我们能预测车辆在不同输入下的运动状态。

MPC实现路径跟踪原理

MPC的核心思想是在每个采样时刻，基于当前系统状态，预测未来一段时间（预测时域）内系统的输出，并通过求解一个优化问题，得到当前时刻的最优控制输入。在路径跟踪中，我们要让车辆行驶轨迹尽可能接近参考路径。

以MPC的目标函数为例，在Python中简单表示如下：

import numpy as np


def mpc_objective(x, ref_path, Q, R):
    # x 当前状态
    # ref_path 参考路径
    # Q 状态权重矩阵
    # R 控制输入权重矩阵
    cost = 0
    for i in range(len(ref_path)):
        y_error = x[0] - ref_path[i][0]
        psi_error = x[1] - ref_path[i][1]
        cost += np.dot(np.array([y_error, psi_error]), np.dot(Q, np.array([y_error, psi_error]).T))
    # 假设控制输入为 [delta, a]
    u = np.array([x[4], x[5]])
    cost += np.dot(u, np.dot(R, u.T))
    return cost

这个函数mpcobjective计算了当前状态x与参考路径refpath之间的误差代价，并且考虑了控制输入的权重。在实际应用中，我们会通过优化算法（比如二次规划）来求解使得这个目标函数最小的控制输入，从而让车辆更好地跟踪参考路径。

仿真结果可视化

在仿真结果中，蓝色的是全局参考路径，就像我们给车辆设定的远方目标。黄色表示预测时域内的参考路径，它更聚焦于车辆短期内要跟随的轨迹段。而红色的车辆行驶轨迹则展示了车辆实际跑出来的路线。理想情况下，红色轨迹应该紧紧贴着蓝色和黄色路径，这就证明了我们的MPC算法在路径跟踪上的有效性。

通过这样的二/三自由度动力学MPC方法，结合Carsim和Simulink联合仿真，我们在自动驾驶路径跟踪技术上又迈出了坚实的一步，为未来自动驾驶车辆更安全、高效地行驶奠定了基础。后续还可以进一步优化模型和算法，探索更多复杂场景下的路径跟踪应用。