企业官网建设流程全解析

《无人驾驶车辆模型预测控制》第四章基于运动学模型和MPC模型预测控制的轨迹跟踪控制CarSim/Simulink联合仿真模型
已针对原书及代码中的错误进行勘误，该联合仿真模型可完美复现圆形
在学习龚建伟教授等所著《无人驾驶车辆模型预测控制》一书时，第四章“基于运动学模型的模型预测控制”是理解MPC在轨迹跟踪中应用的关键章节。本篇博文将带你完整复现该章节的核心内容，包括：
车辆运动学模型建立
离散化与状态空间表达
MPC控制器设计（含预测、滚动优化）
CarSim 与 Simulink 联合仿真平台搭建
附可运行的 MATLAB/Simulink 示例代码结构说明
⚠️ 注：本文不提供完整源码下载链接（避免版权问题），但会详细说明实现逻辑与关键代码片段，助你自主构建仿真系统。

一、为什么选择运动学模型？

在低速（通常 < 15 m/s）或路径曲率变化平缓的场景下，车辆运动学模型（Kinematic Bicycle Model）因其结构简单、计算高效，常被用于轨迹跟踪控制的初步验证。其核心假设包括：
忽略轮胎侧偏力（即无侧滑）
前后轮速度方向与车体方向一致
车辆为刚体，仅考虑平面运动

该模型虽忽略动力学细节，但足以反映车辆转向几何关系，非常适合MPC算法入门与教学演示。

二、运动学模型建模与离散化

1. 连续时间模型

车辆状态向量定义为：
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(ψ)=Lv​tan(δ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控制器设计要点

1. 预测模型构建

将非线性运动学模型在当前状态点线性化，得到：
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​=Ak​xk​+Bk​uk​+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)+UTQu​U+Δ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 聚合仿真搭建

1. 接口配置
   在 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” 功能自定义接口信号。
2. 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_sTs​0.02 s
预测步长N p N_pNp​20
控制步长N c N_cNc​5
轴距L LL2.8 m
δ max ⁡ \delta_{\max}δmax​30° (≈0.52 rad)
Δ δ max ⁡ \Delta \delta_{\max}Δδmax​10°/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。

七、常见问题与调试建议

1. CarSim 与 Simulink 通信失败
   → 检查变量名是否完全匹配，单位是否一致（CarSim 默认使用 km/h、deg 等）

2. MPC求解不收敛或控制量突变
   → 调整权重矩阵（增大Q u Q_uQu​抑制控制剧烈变化）
   → 缩短预测时域N p N_pNp​

3. 轨迹跟踪滞后
   → 引入 前馈补偿（如根据曲率调整参考速度）
   → 使用 时变参考点（Look-ahead point）而非最近点

结语

通过复现《无人驾驶车辆模型预测控制》第四章的内容，不仅能深入理解MPC在轨迹跟踪中的数学原理，还能掌握工业级仿真工具链（CarSim + Simulink）的协同开发方法。此基础框架可进一步扩展至动力学模型、多目标优化、障碍物避让等高级场景。

如果你正在学习自动驾驶控制算法，不妨动手搭建这一系统——从理论到仿真的跨越，正是工程师成长的关键一步。