五次多项式换道轨迹规划+MPC轨迹跟踪控制simulink模型（有说明文档） 版本

五次多项式换道轨迹规划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 -(CfCr)/(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约束或者直接上七次多项式。不过那就是另一个故事了...