自动驾驶规划控制-nmpc路径规划和mpc路径跟踪 matlab和simulink联合仿真非线性mpc路径规划线性mpc路径跟踪最近在搞自动驾驶的规划控制方案试了非线性MPC做路径规划线性MPC做跟踪的组合拳。实测发现这俩货配合起来效果挺带劲尤其是面对复杂路况的时候。今天咱们就边撸代码边唠嗑看看怎么用Matlab和Simulink整活这个方案。先甩个Simulink结构镇楼见图1。整个系统分两层上层的NMPC规划器啃下全局路径下层线性MPC控制器负责跟车。中间用Vehicle Model做桥梁这种结构改参数特方便谁用谁知道。!Simulink联合仿真结构图自动驾驶规划控制-nmpc路径规划和mpc路径跟踪 matlab和simulink联合仿真非线性mpc路径规划线性mpc路径跟踪搞非线性MPC规划时成本函数得玩点花样。来看这段Matlab代码的核心部分function [optTraj, exitflag] nmpc_planner(current_state, obstacles) % 状态权重矩阵 Q diag([10, 10, 5, 2]); % x,y,heading,speed R diag([0.5, 0.2]); % 加速度和方向盘角权重 % 非线性约束生成 nonlcon (u) dynamic_constraints(u, current_state, obstacles); % 求解器配置 options optimoptions(fmincon,MaxIterations,50,... Algorithm,sqp,Display,notify); % 初始猜测很重要用上一时刻最优解 initial_guess load(last_optimal_u.dat); [opt_u, ~, exitflag] fmincon((u) cost_function(u, Q, R),... initial_guess, [], [], [], [], lb, ub, nonlcon, options); % 轨迹预测 optTraj predict_trajectory(current_state, opt_u); end这里有几个骚操作权重矩阵Q里y方向惩罚比x大——防止车辆画龙方向盘角权重设得小毕竟紧急情况得优先避障初始猜测用上次解提速50%不是梦约束函数里藏了动态避障逻辑后面展开说重点看动态约束这个黑魔法function [c, ceq] dynamic_constraints(u, state, obstacles) ceq []; % 等式约束暂时不用 c zeros(1, length(obstacles)); % 预测未来5秒轨迹 pred_traj predict_trajectory(state, u); % 计算每个障碍物的安全距离 for i 1:length(obstacles) % 动态安全距离相对速度越大安全距离越大 rel_speed norm(state(4) - obstacles(i).speed); min_dist 2 rel_speed*0.3; % 找最近距离点 [dist, ~] min(vecnorm(pred_traj(:,1:2) - obstacles(i).pos, 2, 2)); c(i) min_dist - dist; % 小于0就违反约束 end end这里实现了动态避障的核心逻辑安全距离随相对速度自适应调整遍历预测轨迹所有点找最小距离约束条件转化成了标准形式规划完该跟踪了线性MPC上场。这里有个坑得把非线性模型在工作点附近线性化。咱们用自动微分省事% 在平衡点处线性化车辆模型 [Ad, Bd, Cd, Dd] linearize_vehicle_model(... current_speed, current_steering); % 生成线性MPC控制器 mpcobj setConstraints(mpcobj, MV, [1 1], RateMin, [-0.3, -0.1],... RateMax, [0.3, 0.1]); % 控制量变化率约束 % 设置预测时域 mpcobj.PredictionHorizon 20; mpcobj.ControlHorizon 5;注意这里限制了控制量的变化率防止方向盘突变。实际调试时发现RateMax设0.3以上容易让乘客晕车...联合仿真时Simulink模型里有个关键配置规划层和跟踪层的执行频率要分开。通常规划层跑2Hz跟踪层跑10Hz。在Model Settings里设置多速率模式Solver - Type: Fixed-step Fixed-step size: 0.05 % 对应20Hz Tasking mode: MultiTasking最后看个仿真结果图2蓝色是NMPC规划的路径红色是实际跟踪轨迹。注意在急弯处线性MPC会有约0.2m的滞后这个通过缩短控制时域能改善但得小心计算量爆炸。!路径规划与跟踪效果踩过的坑总结非线性求解器初值不对就发散得加个reset逻辑线性化时忽略轮胎侧偏刚度会飘移Simulink仿真加速时可能丢约束记得勾选Enforce constraint satisfaction选项这套方案在工控机上能跑到15Hz基本满足实车需求。下次试试把深度学习预测模块整合进来应该能更风骚地处理鬼探头场景。完整代码已传GitHub假装有链接欢迎拍砖。