车辆三自由度动力学MPC跟踪双移线仿真研究：Matlab与Simulink联合应用
自动驾驶控制-车辆三自由度动力学MPC跟踪双移线 matlab和simulink联合仿真，基于车辆三自由度动力学模型的mpc跟踪双移线。
双移线轨迹跟踪算是自动驾驶领域经典的高难度科目二了，今天咱们用Matlab和Simulink搞点有意思的——基于三自由度车辆模型的MPC控制器实战。先看效果：方向盘自己疯狂打方向，车身在雪糕筒之间划出风骚走位，这可比驾校教练刺激多了。
先说清楚车辆怎么动起来的。三自由度模型可不是随便说说，横摆、侧向、纵向都得算明白。咱们的模型状态量是[横向速度v、横摆角速度r、纵向速度u]，控制量嘛就是前轮转角和驱动力矩。
% 车辆参数别用默认值，真车数据才有内味 m = 1723; % 整车质量 Iz = 2865; % 绕Z轴转动惯量 lf = 1.232; % 前轮到质心距离 lr = 1.468; % 后轮到质心距离 Caf = 80000; % 前轮总侧偏刚度 Car = 80000; % 后轮总侧偏刚度轮胎模型这里玩点花的，用Pacejka魔术公式的话计算量太大，咱们做线性化处理。注意看前轮侧向力的计算，这里藏着车辆失控的伏笔：
alpha_f = delta - (v + lf*r)/u; % 前轮侧偏角 alpha_r = (v - lr*r)/u; % 后轮侧偏角 Fyf = Caf * alpha_f; % 前轴侧向力 Fyr = Car * alpha_r; % 后轴侧向力MPC控制器才是重头戏，预测时域选5步，控制时域3步刚刚好。目标函数里藏着工程师的小心机——横向误差权重给10，航向角误差只给5，毕竟先别撞上才是王道。
% 二次规划参数设置 Q = diag([10, 5, 2, 1]); % 状态权重 R = diag([0.1, 0.05]); % 控制量权重 for k = 1:N cost = cost + (states(:,k)-ref_states(:,k))'*Q*(states(:,k)-ref_states(:,k)); if k < Nu cost = cost + U(:,k)'*R*U(:,k); end endSimulink里玩闭环控制得注意采样时间同步，0.05秒的步长能兼顾实时性和计算量。特别提醒：动力学模型输出记得加个Transport Delay，模拟真实传感器延迟，不然控制器会以为自己能预知未来。
自动驾驶控制-车辆三自由度动力学MPC跟踪双移线 matlab和simulink联合仿真，基于车辆三自由度动力学模型的mpc跟踪双移线。
双移线参考轨迹生成有讲究，别用五次多项式那么老套。试试这个带曲率连续的参数方程：
% 双移线参考轨迹 for t = 0:0.1:20 Y_ref = 3.5*sin(t/5) + 1.75*sin(t/2.5); Psi_ref = atan(3.5/5*cos(t/5) + 1.75/2.5*cos(t/2.5)); end跑完仿真别急着庆祝，看这三个关键点：1）60km/h速度下横向误差有没有超过0.3米；2）方向盘转角变化率是否突破2rad/s；3）后轮侧偏角有没有触及线性区域边界。要是某个指标飘红，回去调整预测时域或者松弛变量。
最后分享个压箱底的调试技巧：在MPC权重矩阵里给纵向速度误差加个死区，这样控制器不会因为1km/h的速度波动而瞎折腾油门。毕竟老司机都懂，只要不超速，差个两三码根本不叫事。