《无人驾驶车辆模型预测控制》第四章基于运动学模型和MPC模型预测控制的轨迹跟踪控制CarSim/Simulink联合仿真模型
已针对原书及代码中的错误进行勘误，该联合仿真模型可完美复现圆形
在学习龚建伟教授等所著《无人驾驶车辆模型预测控制》一书时，第四章“基于运动学模型的模型预测控制”是理解MPC在轨迹跟踪中应用的关键章节。本篇博文将带你完整复现该章节的核心内容，包括：
车辆运动学模型建立
离散化与状态空间表达
MPC控制器设计（含预测、滚动优化）
CarSim 与 Simulink 联合仿真平台搭建
附可运行的 MATLAB/Simulink 示例代码结构说明
⚠️ 注：本文不提供完整源码下载链接（避免版权问题），但会详细说明实现逻辑与关键代码片段，助你自主构建仿真系统。
一、为什么选择运动学模型？
在低速（通常 < 15 m/s）或路径曲率变化平缓的场景下，车辆运动学模型（Kinematic Bicycle Model）因其结构简单、计算高效，常被用于轨迹跟踪控制的初步验证。其核心假设包括：
忽略轮胎侧偏力（即无侧滑）
前后轮速度方向与车体方向一致
车辆为刚体，仅考虑平面运动
该模型虽忽略动力学细节，但足以反映车辆转向几何关系，非常适合MPC算法入门与教学演示。
二、运动学模型建模与离散化
- 连续时间模型
车辆状态向量定义为：
x = [ x , y , ψ , v ] T \mathbf{x} = [x,\ y,\ \psi,\ v]^Tx=[x,y,ψ,v]T
其中：
x , y x, yx,y：车辆质心位置
ψ \psiψ：航向角
v vv：纵向速度
控制输入为：
u = [ a , δ f ] T \mathbf{u} = [a,\ \delta_f]^Tu=[a,δf]T
a aa：纵向加速度（或通过油门/制动间接控制）
δ f \delta_fδf：前轮转角
运动学方程如下：
x ˙ = v cos ( ψ ) y ˙ = v sin ( ψ ) ψ ˙ = v L tan ( δ f ) v ˙ = a \begin{aligned} \dot{x} &= v \cos(\psi) \\ \dot{y} &= v \sin(\psi) \\ \dot{\psi} &= \frac{v}{L} \tan(\delta_f) \\ \dot{v} &= a \end{aligned}x˙y˙ψ˙v˙=vcos(ψ)=vsin(ψ)=Lvtan(δf)=a
其中L LL为轴距。
2. 离散化处理（一阶欧拉法）
设采样周期为T s T_sTs，则离散状态更新为：
matlab
x(k+1) = x(k) + Ts v(k) cos(psi(k));
y(k+1) = y(k) + Ts v(k) sin(psi(k));
psi(k+1) = psi(k) + Ts v(k)/L tan(delta_f(k));
v(k+1) = v(k) + Ts a(k);
✅ 实际MPC中常对模型进行线性化（围绕当前工作点）以构建线性时不变（LTI）预测模型，便于求解二次规划（QP）问题。
三、MPC控制器设计要点
- 预测模型构建
将非线性运动学模型在当前状态点线性化，得到：
x k + 1 = A k x k + B k u k + c k \mathbf{x}_{k+1} = A_k \mathbf{x}_k + B_k \mathbf{u}_k + \mathbf{c}_kxk+1=Akxk+Bkuk+ck
利用该模型递推未来N p N_pNp步的状态预测，形成预测输出向量Y \mathbf{Y}Y与控制序列U \mathbf{U}U的线性关系：
Y = H x ( k ) + G U \mathbf{Y} = \mathbf{H} \mathbf{x}(k) + \mathbf{G} \mathbf{U}Y=Hx(k)+GU
2. 目标函数设计
典型二次型代价函数：
J = ( Y − R ) T Q y ( Y − R ) + U T Q u U + Δ U T Q Δ u Δ U J = (\mathbf{Y} - \mathbf{R})^T \mathbf{Q}_y (\mathbf{Y} - \mathbf{R}) + \mathbf{U}^T \mathbf{Q}_u \mathbf{U} + \Delta \mathbf{U}^T \mathbf{Q}_{\Delta u} \Delta \mathbf{U}J=(Y−R)TQy(Y−R)+UTQuU+ΔUTQΔuΔU
其中：
R \mathbf{R}R：参考轨迹（来自全局路径）
Q y \mathbf{Q}_yQy：跟踪误差权重
Q u , Q Δ u \mathbf{Q}_u, \mathbf{Q}_{\Delta u}Qu,QΔu：控制量及变化率惩罚项
3. 约束处理
前轮转角限幅：$ \delta_f \leq \delta_{\max}$
转角变化率限制：$ \Delta \delta_f \leq \Delta \delta_{\max}$
纵向加速度范围：a min ≤ a ≤ a max a_{\min} \leq a \leq a_{\max}amin≤a≤amax
这些约束可直接嵌入QP求解器（如 quadprog 或 mpc toolbox）。
四、CarSim / Simulink 聚合仿真搭建
- 接口配置
在 CarSim 中选择 “Send/receive via Simulink” 模式
设置通信变量：
输出（CarSim → Simulink）：x , y , ψ , v , ψ ˙ x, y, \psi, v, \dot{\psi}x,y,ψ,v,ψ˙等
输入（Simulink → CarSim）：前轮转角δ f \delta_fδf、驱动/制动指令（可简化为加速度a aa）
💡 建议使用 CarSim 内置的 “User-defined variables” 功能自定义接口信号。 - Simulink 模型结构
[Reference Path]
↓
[MPC Controller] ← [Vehicle State from CarSim]
↓
[Control Output: δ_f, a] → CarSim
MPC模块：可用 MATLAB Function Block 或 S-Function 实现
参考轨迹生成：可读取预存的( x r e f , y r e f ) (x_{ref}, y_{ref})(xref,yref)数组，并通过插值得到当前期望点
3. 关键参数设置（示例）
参数 值
采样时间T s T_sTs0.02 s
预测步长N p N_pNp20
控制步长N c N_cNc5
轴距L LL2.8 m
δ max \delta_{\max}δmax30° (≈0.52 rad)
Δ δ max \Delta \delta_{\max}Δδmax10°/step
五、仿真结果展示（典型场景）
在 双移线（Double Lane Change） 场景下：
横向位置误差稳定在 ±0.1 m 以内
航向角跟踪平滑，无明显超调
前轮转角响应符合车辆物理极限
📊 可通过 CarSim 的 Plot 功能或 Simulink Scope 对比实际轨迹与参考轨迹。
六、代码结构说明（MATLAB部分）
虽然无法直接分享完整工程文件，但核心MPC求解函数结构如下：
matlab
function [delta_f, a] = mpc_kinematic_controller(x_current, ref_traj, params)
% x_current: [x, y, psi, v]
% ref_traj: [x_ref(1:Np), y_ref(1:Np), psi_ref(1:Np)]
% 1. 线性化运动学模型（计算A, B, C）
[A, B, C] = linearize_kinematic_model(x_current, params.L, params.Ts);
% 2. 构建预测矩阵 H, G
[H, G] = build_prediction_matrices(A, B, C, params.Np);
% 3. 构建QP问题：min 0.5U’H_qpU + f_qp’U
[H_qp, f_qp] = build_cost_function(H, G, x_current, ref_traj, params.Qy, params.Qu);
% 4. 定义约束（A_ineq U <= b_ineq）
[A_ineq, b_ineq] = build_constraints(params);
% 5. 调用quadprog求解
U_opt = quadprog(H_qp, f_qp, A_ineq, b_ineq, [], [], [], [], [], opts);
% 6. 提取第一步控制量
delta_f = U_opt(1);
a = U_opt(2);
end
🔧 提示：首次调试建议先在纯Simulink环境中用简化的车辆模型验证MPC逻辑，再接入CarSim。
七、常见问题与调试建议
CarSim 与 Simulink 通信失败
→ 检查变量名是否完全匹配，单位是否一致（CarSim 默认使用 km/h、deg 等）MPC求解不收敛或控制量突变
→ 调整权重矩阵（增大Q u Q_uQu抑制控制剧烈变化）
→ 缩短预测时域N p N_pNp轨迹跟踪滞后
→ 引入 前馈补偿（如根据曲率调整参考速度）
→ 使用 时变参考点（Look-ahead point）而非最近点
结语
通过复现《无人驾驶车辆模型预测控制》第四章的内容，不仅能深入理解MPC在轨迹跟踪中的数学原理，还能掌握工业级仿真工具链（CarSim + Simulink）的协同开发方法。此基础框架可进一步扩展至动力学模型、多目标优化、障碍物避让等高级场景。
如果你正在学习自动驾驶控制算法，不妨动手搭建这一系统——从理论到仿真的跨越，正是工程师成长的关键一步。
