自动驾驶中的五次多项式轨迹规划与MPC跟踪控制

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_min3. 车辆动力学建模细节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; end3.2 轮胎线性区间约束为保证模型有效性需要添加侧偏角约束|α_f| ≤ tan⁻¹(0.02·μ·Fz_f / C_α) |α_r| ≤ tan⁻¹(0.02·μ·Fz_r / C_α)在Simulink中通过Saturation模块实现当侧偏角超过阈值时会触发警告并调整MPC权重。4. MPC控制器设计实现4.1 模型离散化处理采用零阶保持法对连续模型离散化采样时间Ts0.05s对应20Hz控制频率。离散化后的状态空间方程x(k1) 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 接口信号配置关键信号映射关系Carsim输出Simulink输入端口单位Vehicle SpeedVxm/sYaw Raterrad/sLateral Accelaym/s²Steer Angledelta_measrad5.2 参数同步要点必须保持一致的参数包括整车质量及轴荷分配轮胎侧偏刚度建议先在Carsim中做侧偏特性测试悬架KC特性影响载荷转移转向系统传动比常见坑点Carsim默认使用英寸单位制而Simulink使用国际单位制需在Carsim界面勾选SI Units Output6. 调试经验与性能优化6.1 权重参数整定技巧采用分层调参法先调Q矩阵使跟踪误差收敛再调R矩阵抑制控制量振荡最后用S矩阵平滑控制增量典型问题现象及对策超调过大 → 增大Q(1)或减小R响应迟缓 → 减小Q(3)或增大R执行器抖动 → 增大S值6.2 实时性优化手段减少预测时域Np通常取20-30使用qpOASES求解器替代默认算法开启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为道路曲率直路为07.2 耦合纵向控制将MPC扩展为横向纵向联合控制新增状态变量纵向速度vx控制输入增加油门/制动指令需建立发动机/制动系统模型7.3 引入驾驶员模型在环路中加入预瞄驾驶员模型delta_driver K1·e K2·∫e dt K3·φ_preview可实现人机共驾仿真模型的实际测试数据显示在干燥路面条件下μ0.880km/h紧急避撞时最大侧向加速度0.28g横摆角速度误差0.5°/s完成3.5m换道耗时2.8秒这些指标均满足ISO 11270标准要求。对于想深入研究的开发者建议重点关注轮胎模型从线性到非线性的扩展以及考虑路面附着系数估计的鲁棒控制设计。