基于运动学模型的MPC轨迹跟踪算法纯Matlab实现与解析

张开发
• 2026/4/10 6:41:51 • 15 分钟阅读

最新文章

推荐文章

相关文章

分享文章

基于运动学模型的MPC轨迹跟踪算法纯Matlab实现与解析
基于运动学模型mpc轨迹跟踪纯Matlab实现 直接上代码前咱们先想明白个事儿——轨迹跟踪就像让自动驾驶车走钢丝得预判未来几步怎么打方向盘。Matlab里整MPC模型预测控制最大的坑不是算法本身而是怎么把矩阵拼对了。先看自行车模型的核心代码20行搞定状态更新function x_next bicycle_model(x, u, L, dt) beta atan(tan(u(2))/2); % 这个β计算90%的人会写错 x_next x [ x(4)*cos(x(3)beta); x(4)*sin(x(3)beta); x(4)*sin(beta)/(L/2); u(1); ] * dt; end注意这里的方向盘转角u(2)处理实际车辆转向比会直接影响β角。有次我忘了除以2小车直接走出麻花轨迹调试两小时才发现这行的问题。预测环节是MPC的重头戏看这个预测矩阵构造pred_states zeros(N1, 4); pred_states(1,:) x_current; for k 1:N pred_states(k1,:) bicycle_model(pred_states(k,:), [u_opt(2*k-1); u_opt(2*k)], L, dt); end这里有个隐藏技巧u_opt是拉直的列向量前一半是油门后一半是方向盘。新手容易在这里维度对不上出现玄学报错。基于运动学模型mpc轨迹跟踪纯Matlab实现 目标函数搭建才是真的烧脑看代价矩阵怎么堆Q kron(eye(N), diag([10, 10, 5, 2])); % 状态权重 R 0.1 * eye(2*N); % 控制量权重 H blkdiag(Q, R); f [repmat(-Q(1:4,1:4)*xref, N, 1); zeros(2*N,1)]; % 参考轨迹追击权重系数别随便抄上次我用[1,1,1,1]结果小车蛇形走位。后来发现航向角权重得是位置的2倍才稳当。求解器调用看着简单实则坑多options optimoptions(quadprog,Display,off); u_opt quadprog(H, f, Aineq, bineq, [], [], lb, ub, [], options);注意这个Aineq矩阵得包含动力学约束比如最大转向角限制。有次忘记约束方向盘转角仿真时车轮直接扭成180度跟现实中的转向限制差太远。最后放个实战效果跟踪圆形轨迹时预测时域N20比N5的轨迹平滑度提升40%但单步计算时间从5ms飙到50ms。在200米直道场景测试横向误差能压在0.2米内——这精度足够低速园区物流车用了。建议自己跑代码时重点观察两个点一是速度突变时控制量会不会剧烈抖动二是急转弯处预测轨迹和实际路径的贴合度。调参时先动状态权重再调控制量惩罚别一上来就改步长。

更多文章