基于模型预测控制(MPC)的车辆换道与轨迹跟踪：五次多项式换道轨迹的Matlab与Ca
rSim联合仿真控制
---
**手把手教你用五次多项式玩转MPC换道控制**
最近在调车辆横向控制时发现，用五次多项式做换道轨迹配合MPC真是香！今天咱们就抛开复杂的
公式推导，直接上代码实战，聊聊怎么用Matlab和CarSim搞联合控制。
**为啥非得是五次多项式？**
换道轨迹要满足起终点位置、速度、加速度连续，五次项刚好能塞进去六个边界条件。举个栗子，横
向位移从0变到3米，纵向速度保持20m/s，起终点加速度都得是零。用下面这段代码生成轨迹：
```matlab
% 五次多项式系数计算（横向位移y关于时间t）
t0 = 0; tf = 5; % 换道时间5秒
A = [1 t0 t0^2 t0^3 t0^4 t0^5;
0 1 2*t0 3*t0^2 4*t0^3 5*t0^4;
0 0 2 6*t0 12*t0^2 20*t0^3;
1 tf tf^2 tf^3 tf^4 tf^5;
0 1 2*tf 3*tf^2 4*tf^3 5*tf^4;
0 0 2 6*tf 12*tf^2 20*tf^3];
b = [0; 0; 0; 3; 0; 0]; % y(t0)=0, y'(t0)=0, y''(t0)=0 → y(tf)=3m
coeff = A; % 解线性方程组
```
生成曲线类似S型，加速度连续不会让乘客晕车。但实际场景中得考虑纵向速度变化，这时候需要做
轨迹参数化处理（比如用s-L坐标系）。
**MPC控制器核心操作**
模型预测控制的精髓在于滚动优化。这里用二自由度单车模型做预测：
```matlab
% 状态量：[横向位移y，横摆角psi，横向速度vy，横摆角速度r]
% 控制量：前轮转角delta
dt = 0.1; % 控制周期0.1秒
