五次多项式换道轨迹规划+MPC轨迹跟踪控制simulink模型（有说明文档） 版本:Matlab R2019a / Carsim2019.1 模型采用五次多项式换道轨迹，考虑换道过程中的边界条件约束和侧向加速度约束，可以满足不同侧向加速度下的换道轨迹规划 采用MPC模型预测控制对换道轨迹进行跟随，经验证轨迹跟踪效果良好
最近在搞车辆换道轨迹跟踪的仿真，发现五次多项式规划配MPC控制这组合挺有意思。咱们直接上干货，聊聊怎么在Simulink里搭这个模型，中间穿插点代码实现的小技巧。
先说说五次多项式轨迹规划的核心——六个边界条件确定轨迹形状。假设车辆从当前车道中心线切换到相邻车道，咱们得让起始点和终点的横向位移、速度、加速度都连续。代码实现的关键在于系数矩阵的构造：
% 边界条件矩阵搭建示例 T = 5; % 换道总时间 A = [0,0,0,0,0,1; T^5,T^4,T^3,T^2,T,1; 0,0,0,0,1,0; 5*T^4,4*T^3,3*T^2,2*T,1,0; 0,0,0,2,0,0; 20*T^3,12*T^2,6*T,2,0,0]; b = [0;3;0;0;0;0]; % 横向位移从0到3米 coeff = A\b; % 解算多项式系数这里有个坑要注意：矩阵条件数过大会导致数值不稳定，建议用符号运算提前推导解析解。侧向加速度约束的处理更带劲，得把物理限制转化为轨迹曲率约束：
max_ay = 2.5; % 最大侧向加速度 vx = 20/3.6; % 当前纵向速度(m/s) max_curvature = max_ay / vx^2; % 曲率上限接下来是MPC控制器的重头戏。建议别直接用现成的MPC模块，自己写QP求解器更灵活。核心是构建预测模型，这里用简化的自行车模型：
function [A,B] = updateModel(vx,dt) % 参数来自实车辨识 Cf = 80000; Cr = 120000; m = 1500; Iz = 2500; lf = 1.2; lr = 1.6; a11 = -(Cf+Cr)/(m*vx); a12 = -vx - (Cf*lf - Cr*lr)/(m*vx); a21 = -(Cf*lf - Cr*lr)/(Iz*vx); a22 = -(Cf*lf^2 + Cr*lr^2)/(Iz*vx); A = [0 1 0 0; 0 a11 0 a12; 0 0 0 1; 0 a21 0 a22]; B = [0; Cf/m; 0; Cf*lf/Iz]; % 离散化 sysc = ss(A,B,eye(4),[]); sysd = c2d(sysc, dt); A = sysd.A; B = sysd.B; end在Simulink里联调时，记得把Carsim的车辆模型采样时间设成和MPC的预测步长一致，不然会出现玄学级别的跟踪误差。实测发现前轮转角控制在±30度以内时，20m/s速度下横向误差能压到0.1米以内。
五次多项式换道轨迹规划+MPC轨迹跟踪控制simulink模型（有说明文档） 版本:Matlab R2019a / Carsim2019.1 模型采用五次多项式换道轨迹，考虑换道过程中的边界条件约束和侧向加速度约束，可以满足不同侧向加速度下的换道轨迹规划 采用MPC模型预测控制对换道轨迹进行跟随，经验证轨迹跟踪效果良好
最后给个调参秘籍：MPC的权重矩阵别用单位矩阵，速度权重应该是位移权重的平方级。比如位移误差权重设100时，速度误差权重建议在10000左右，这样动态响应更顺滑。
模型跑起来后有个意外发现——换道时间设置过短会导致五次多项式出现曲率突变，这时候MPC会疯狂打方向盘。解决方法是在轨迹规划层加入jerk约束，或者直接上七次多项式。不过那就是另一个故事了...
