1. 项目概述：五次多项式换道轨迹规划与MPC跟踪控制

在自动驾驶和高级驾驶辅助系统（ADAS）开发中，紧急避撞场景下的轨迹规划与控制一直是核心难点。这个Simulink模型通过五次多项式生成满足多阶连续性的换道轨迹，再结合模型预测控制（MPC）实现精准跟踪，完整复现了从轨迹生成到执行控制的闭环流程。

模型最突出的特点是同时考虑了边界条件（如侧向位移约束）和动态性能约束（如侧向加速度限制）。这种双重约束设计使得系统既保证了避撞路径的可行性，又确保了车辆动力学稳定性。从实际测试数据来看，横向跟踪误差能控制在0.14米以内，前轮转角变化率保持在10度/秒以下，完全满足ISO 11270标准对车道保持系统的要求。

2. 五次多项式轨迹生成原理

2.1 多项式阶次选择依据

为什么选择五次而非更低阶次？三次多项式虽然能保证位移和速度连续，但加速度会出现突变。这在高速换道时会导致方向盘突然打角，影响乘坐舒适性甚至引发失稳。五次多项式的数学表达为：

y(t) = a₀ + a₁t + a₂t² + a₃t³ + a₄t⁴ + a₅t⁵

通过六个边界条件（起止点的位移、速度、加速度）可以唯一确定系数。在Matlab中构建的系数矩阵如下：

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 t1 t1^2 t1^3 t1^4 t1^5; 0 1 2*t1 3*t1^2 4*t1^3 5*t1^4; 0 0 2 6*t1 12*t1^2 20*t1^3];

2.2 临界安全距离计算

换道起点的确定依赖于纵向安全距离计算模型：

D_min = Vx·τ + (Vx²)/(2|a_max|) - (Vf²)/(2|a_max|)

其中τ为系统响应延迟（典型值1.2-1.5秒），a_max为最大减速度（通常取0.3-0.4g）。在S-function模块中，这个计算过程会实时更新，当雷达检测距离小于D_min时触发换道。

关键提示：实际工程中会引入安全系数η（通常1.2-1.5），最终采用D_safe = η·D_min

3. 车辆动力学建模细节

3.1 三自由度模型构建

模型包含横向运动、横摆运动和航向运动三个自由度，状态变量为：

• β：质心侧偏角
• r：横摆角速度
• ψ：航向角

核心微分方程体现在这个函数中：

function dx = vehicleModel(x, u) % 参数定义（需与Carsim保持一致） m = 1850; % 质量(kg) Iz = 3500; % 横摆惯量(kg·m²) a = 1.4; % 前轴到质心距离(m) b = 1.6; % 后轴到质心距离(m) C_alpha = [65000; 65000]; % 前后轮侧偏刚度(N/rad) beta = x(1); r = x(2); psi = x(3); delta = u(1); % 前轮转角输入 Vx = 20; % 假设恒定车速(m/s) % 轮胎侧向力（线性区域） Fyf = C_alpha(1)*(delta - (beta + a*r)/Vx); Fyr = C_alpha(2)*(-(beta - b*r)/Vx); % 状态方程 dx = zeros(3,1); dx(1) = (Fyf + Fyr)/(m*Vx) - r; dx(2) = (a*Fyf - b*Fyr)/Iz; dx(3) = r; end

3.2 轮胎线性区间约束

为保证模型有效性，需要添加侧偏角约束：

|α_f| ≤ tan⁻¹(0.02·μ·Fz_f / C_α) |α_r| ≤ tan⁻¹(0.02·μ·Fz_r / C_α)

在Simulink中通过Saturation模块实现，当侧偏角超过阈值时会触发警告并调整MPC权重。

4. MPC控制器设计实现

4.1 模型离散化处理

采用零阶保持法对连续模型离散化，采样时间Ts=0.05s（对应20Hz控制频率）。离散化后的状态空间方程：

x(k+1) = A_d·x(k) + B_d·u(k) y(k) = C_d·x(k)

在Matlab中通过c2d函数实现：

sys_cont = ss(A, B, C, D); sys_disc = c2d(sys_cont, Ts, 'zoh'); [A_d, B_d, C_d, D_d] = ssdata(sys_disc);

4.2 目标函数设计

MPC的核心是以下多目标优化问题：

min Σ [ (y-y_ref)'·Q·(y-y_ref) + u'·R·u + Δu'·S·Δu ] s.t. x_min ≤ x ≤ x_max u_min ≤ u ≤ u_max Δu_min ≤ Δu ≤ Δu_max

典型权重矩阵取值：

• Q = diag([10, 5, 2]) % 跟踪误差权重
• R = 1 % 控制量权重
• S = 0.1 % 控制增量权重

4.3 约束条件设置

执行器物理限制转化为约束：

• 前轮转角：|δ| ≤ 30°（约0.52rad）
• 转角速率：|Δδ| ≤ 10°/s（约0.17rad/s）
• 侧向加速度：|a_y| ≤ 0.3g

在Simulink中通过MPC模块的Constraint属性页配置：

mpcobj.MV.Min = -0.52; mpcobj.MV.Max = 0.52; mpcobj.MV.RateMin = -0.17; mpcobj.MV.RateMax = 0.17;

5. Simulink-Carsim联合仿真配置

5.1 接口信号配置

关键信号映射关系：

5.2 参数同步要点

必须保持一致的参数包括：

• 整车质量及轴荷分配
• 轮胎侧偏刚度（建议先在Carsim中做侧偏特性测试）
• 悬架K&C特性（影响载荷转移）
• 转向系统传动比

常见坑点：Carsim默认使用英寸单位制，而Simulink使用国际单位制，需在Carsim界面勾选"SI Units Output"

6. 调试经验与性能优化

6.1 权重参数整定技巧

采用分层调参法：

1. 先调Q矩阵使跟踪误差收敛
2. 再调R矩阵抑制控制量振荡
3. 最后用S矩阵平滑控制增量

典型问题现象及对策：

• 超调过大 → 增大Q(1)或减小R
• 响应迟缓 → 减小Q(3)或增大R
• 执行器抖动 → 增大S值

6.2 实时性优化手段

1. 减少预测时域Np（通常取20-30）
2. 使用qpOASES求解器替代默认算法
3. 开启MATLAB Coder生成加速代码

在模型属性中设置：

set_param(gcs, 'AcceleratorUseTrueIdentifier', 'on'); set_param(gcs, 'Accelerator', 'rapid');

7. 扩展应用方向

7.1 考虑道路曲率补偿

在目标轨迹中引入道路曲率项：

y_ref = y_poly + K_road·x²/2

其中K_road为道路曲率（直路为0）

7.2 耦合纵向控制

将MPC扩展为横向+纵向联合控制：

• 新增状态变量：纵向速度vx
• 控制输入增加：油门/制动指令
• 需建立发动机/制动系统模型

7.3 引入驾驶员模型

在环路中加入预瞄驾驶员模型：

delta_driver = K1·e + K2·∫e dt + K3·φ_preview

可实现人机共驾仿真

模型的实际测试数据显示，在干燥路面条件下（μ=0.8），80km/h紧急避撞时：

• 最大侧向加速度0.28g
• 横摆角速度误差<0.5°/s
• 完成3.5m换道耗时2.8秒

这些指标均满足ISO 11270标准要求。对于想深入研究的开发者，建议重点关注轮胎模型从线性到非线性的扩展，以及考虑路面附着系数估计的鲁棒控制设计。