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