车辆轨迹跟踪控制这块我一直觉得最磨人的不是控制器理论本身而是怎么把一套带约束的非线性模型预测控制算法在Matlab里跑通还得让车辆动力学模型不中途发散。这篇文章是我把一个NMPC车辆仿真项目从零搭起来的过程记录包含车辆单轨模型推导、带约束NMPC控制器设计、CasADi/IPOPT求解以及整套Matlab仿真代码的核心解析。适合正在做车辆控制课题的学生也适合想从线性MPC切到NMPC的工程师参考。我会尽量把推导和代码串起来讲不写那种“点到为止”的伪教程每一个关键决策都会解释为什么这么做。1. 为什么最终选用带约束的非线性模型预测控制1.1 线性MPC在车辆控制中的“天花板”在哪我最早做的车辆横向控制用的是线性时变MPC方法本身很成熟在每个控制周期把车辆模型在当前工作点线性化然后求解一个二次规划问题。跑低速双移线和定圆工况效果其实不错横向误差能控制在0.05米以内转向动作也说得过去。但一旦速度提上来比如80km/h紧急变道问题就来了。车辆进入非线性区后轮胎侧偏角明显增大线性化模型里的“等效侧偏刚度”已经不再是一个常数。线性MPC预测的是理想化线性车辆的运动轨迹预测出来的轨迹跟车辆实际响应越差越大控制器还会因为模型失准给出偏大或偏振荡的转向指令。我在某个高速双移线工况里实测过横向误差直接从0.08米涨到0.3米以上而且方向盘出现明显的锯齿形抖动。这个问题的根源在于线性化模型只在一个很小的邻域内有效而车辆在极限工况下的动力学本质上是强非线性的。只要预测时域内工作点变化足够大线性MPC的预测优势就会打折扣。想从根本上解决就得把非线性模型直接放进预测环节这就是我用NMPC的原因。1.2 NMPC带来的三样东西预测精度、约束处理、多目标优化选择NMPC不是因为它“听起来高级”而是它同时解决了三个实际问题。第一是预测精度。NMPC直接使用非线性车辆动力学方程做状态递推预测轨迹更接近真实车辆响应模型失配的问题被降到了最低。第二是约束处理。车辆控制器最怕的就是给执行器发一个“根本执行不了”的指令。转向角有物理极限加速度受发动机和制动能力限制横摆角速度也有安全性上限。线性MPC处理这些约束时通常只能做局部线性化近似NMPC则把这些约束全部放进一个非线性规划NLP问题的约束条件里求解器在找最优控制量的同时必须满足这些硬约束所以解出来的控制序列天然就是可执行的。第三是多目标优化。跟踪误差、控制量大小、控制量变化率、横向位置偏离道路中心的风险这些东西在NMPC框架下可以写成一个统一的代价函数。我可以在不改变算法结构的前提下通过调整权重矩阵让控制器在“激进跟踪”和“平顺执行”之间自由切换。这种灵活性线性MPC很难给到这么直接。1.3 仿真研究中的整体架构与工具选择整个仿真项目围绕Matlab展开核心组件是三个模块车辆动力学模块用非线性单轨模型描述车辆纵向、侧向和横摆运动用离散时间方程递推。参考轨迹生成模块生成目标路径和目标速度曲线作为NMPC的跟踪输入。NMPC控制器模块基于CasADi构建非线性优化问题调用IPOPT求解得到最优转向角和加速度序列。为什么用CasADi而不是Matlab自带的fmincon我在早期版本里确实用过fmincon但很快就发现一个问题车辆动力学方程本身已经比较复杂再加上状态约束和控制约束优化问题的有限差分梯度计算非常耗时而且数值精度差经常因为梯度噪声导致求解失败。CasADi的符号引擎会自动求导还能把动力学方程编译成高效函数建模效率高出一个量级求解速度也能满足仿真需求。2. 车辆动力学模型从单轨模型到可预测的非线性系统2.1 为什么用单轨自行车模型而不是复杂多体模型NMPC是滚动优化算法每个控制周期都要重新求解一次NLP。如果预测模型用CarSim级别的多体动力学模型算力根本扛不住尤其是后续想往实时控制器上迁移时模型复杂度直接决定了可行性。所以我选择了经典的单轨自行车模型。这个模型的核心假设是忽略左右轮载荷转移把同轴的两个轮胎合并成一个等效轮胎车辆看作一个刚性车身只考虑纵向运动、侧向运动和横摆运动三个自由度。对于普通乘用车在正常路面上的轨迹跟踪问题单轨模型的精度已经足够。有人说单轨模型是不是太简单了我的经验是它抓主特征的能力远高于它失去的细节。高速换道时车辆响应最关键的是轮胎侧偏特性和横摆转动惯量单轨模型正好保留这两个主要参数而悬架运动学、侧倾特性这些对纵向和横向控制的影响在慢变工况下可以近似忽略。当然如果研究极端操控极限单轨模型就不够用了这部分我会在最后提到。2.2 轮胎力建模从线性到非线性退化轮胎力是车辆与地面之间唯一的力的来源所以轮胎模型的好坏直接决定了NMPC预测是否可信。最简单的线性模型是Fy C * alpha其中C是侧偏刚度alpha是轮胎侧偏角。这个模型在小侧偏角范围内一般小于3度精度很好但在极限工况下完全不成立。轮胎进入非线性饱和区域后侧向力增长变缓再用线性模型预测出来的转弯能力会偏高控制器就会低估实际需要的转向角。我最终在代码里采用了一种带饱和特性的简化轮胎模型Fy min(C * alpha, mu * Fz)也就是先按线性增长达到路面附着极限mu*Fz后保持恒定。这种做法比纯线性模型更接近实际又不像Pacejka魔术公式那样参数众多、计算复杂。做NMPC预测时这种简单非线性模型足以捕捉轮胎饱和的关键趋势。侧偏角的计算也要严格推导。前轮侧偏角定义为前轮速度方向与轮胎朝向之间的夹角alpha_f atan((vy lf * wz) / vx) - delta后轮侧偏角类似alpha_r atan((vy - lr * wz) / vx)其中lf和lr分别是质心到前后轴的距离delta是前轮转角。这个式子里的关键点是横摆角速度wz会在前后轴处产生不同的侧向速度分量所以前后轮侧偏角天然不同这也解释了为什么高速时车辆对前轮转角的响应与低速时差别很大。2.3 状态方程的完整推导与离散化状态向量取[x, y, psi, vx, vy, wz]对应的物理量是全局坐标系下的纵向位置、横向位置、横摆角、纵向速度、侧向速度和横摆角速度。控制向量取[delta, a]即前轮转角与纵向加速度。运动学方程dx vx * cos(psi) - vy * sin(psi) dy vx * sin(psi) vy * cos(psi) dpsi wz动力学方程基于牛顿第二定律和横摆力矩平衡我做了两个工程上的简化忽略纵向力对横摆力矩的贡献忽略前轮转角对纵向力的耦合。这样得到dvx a wz * vy - Fyf * sin(delta) / m dvy (Fyf * cos(delta) Fyr) / m - wz * vx dwz (lf * Fyf * cos(delta) - lr * Fyr) / Iz其中Fyf和Fyr分别按前文的非线性轮胎模型计算。纵向方程里wz*vy是坐标系旋转带来的耦合项有些人容易漏掉但高速大横摆角时这一项影响很大必须保留。模型离散化我用的是固定步长欧拉法仿真步长Ts0.05秒。为什么不用四阶Runge-Kutta欧拉法代码简单、计算量小在50ms步长下精度足够但如果你发现仿真结果发散第一个怀疑对象就应该是离散化步长过大可以改RK4验证。3. NMPC控制器设计代价函数、约束与求解策略3.1 预测时域、控制时域与决策变量维度预测时域Np代表控制器向前看多少步控制时域Nc代表优化变量包含多少个控制量。我在这套代码里取Np15Ts0.05秒相当于向前预测0.75秒。这个时间长度对车辆高速操纵来说是够的因为车辆横摆运动的时间常数一般不到0.5秒0.75秒足以看到当前控制作用的完整效果。Nc我直接取和Np相等也就是每一步都有一个独立的控制量待优化。这样决策变量是2*1530个对IPOPT来说完全是小规模问题。实际工程中如果Np很大为了降维度通常会令NcNp让后段控制量保持不变但会增加一点逻辑复杂度在自己的仿真项目里没必要。需要提醒的是预测时域不是越长越好。时域太长非线性模型的预测误差会累积导致求解出的控制量偏向保守同时NLP的边界约束更容易被不可行解卡住求解时间也急剧上升。我的经验是先从小Np开始比如Np10如果跟踪误差太大再逐渐加长。3.2 代价函数误差加权、控制惩罚与终端代价代价函数我写成J sum_{k0}^{Np-1} (||x_k - x_ref,k||_Q^2 ||u_k - u_ref,k||R^2 ||u{k1} - u_k||_S^2) ||x_Np - x_ref,Np||_P^2第一项惩罚状态跟踪误差第二项惩罚控制量偏离期望值第三项惩罚控制增量用来抑制转向抖动。最后一项是终端代价用来近似无穷时域最优控制减少末端性能恶化。权重矩阵Q、R、S的初始值要格外小心。状态向量的各个分量量纲差异很大位置是米角度是弧度速度是米每秒。如果Q直接填一个大的对角阵数值上位置误差会主导代价横摆角和速度误差很容易被忽略。我的初始设定是Q diag([10, 10, 5, 0.5, 0.5, 0.5]) R diag([0.1, 0.1]) S diag([1, 1])这里Q前两项对应横向和纵向位置误差取10让控制器优先贴线Q第三项对应横摆角误差取5避免车辆“斜着跑”完成跟踪速度相关取小权重因为轨迹跟踪时纵向速度更多由上层规划决定。3.3 约束条件的工程含义与软约束处理NMPC能把约束直接写进优化问题这正是它相比线性MPC最大的工程价值。我在代码里设置的约束包括前轮转角限幅delta ∈ [-0.5, 0.5] rad约±28.6度。纵向加速度限幅a ∈ [-3, 3] m/s²。纵向速度下限vx ≥ 1.0 m/s避免低速时轮胎侧偏角公式里的除法出现奇异。输入增量限幅|delta_{k1} - delta_k| ≤ 0.3 rad/步模拟转向执行器的速率限制。这些约束全部用硬约束形式放进NLP。但有一个问题参考轨迹如果给得过于激进控制器可能找不到满足所有约束的可行解IPOPT会直接报“infeasible problem”。解决这个问题的一个通用技巧是引入松弛变量把部分约束变成软约束。比如我希望限制横向跟踪偏差不要超过0.5米但又不希望在遇到极端参考轨迹时直接无解我可以写成dy_k - slack_k 0.5 slack_k 0并在代价函数里加上对slack的二次惩罚。这样一来当跟踪误差不可能小于0.5米时优化器会选择付出一定代价来放松这个边界而不是无解退出。这个技巧在仿真后期非常实用。3.4 求解器选型CasADiIPOPT vs 手写fmincon我把两者的优劣放在一张表里方便你按自己的情况选方案优点缺点适用场景CasADi IPOPT自动微分精确、建模灵活、求解效率高需要额外安装CasADi和IPOPT环境配置多一步研究生科研、中小规模NMPC、实时性要求较高的仿真fminconMatlab自带、无需额外安装梯度计算依赖有限差分、NLP建模繁琐教学入门、问题规模小、不想装工具箱时我最终选择了CasADiIPOPT。一个重要原因是NMPC要在仿真循环里不断求解同一个“结构”的优化问题只是参数当前状态、参考轨迹在变。CasADi支持将求解器实例化后反复求解可以避免重复建模fmincon每次调用都要重新接收函数句柄大规模预测时效率和数值稳定性都不占优。4. Matlab代码实现与关键环节解析4.1 整体代码结构与仿真流程代码按模块拆分大概是这样参数初始化车辆参数、NMPC权重、约束边界、仿真时长。参考轨迹生成目标路径和目标速度。车辆动力学函数用CasADi符号定义连续时间模型。NMPC求解器构建NLP并在每个控制周期更新参数并求解。主仿真循环状态递推、控制量应用、数据记录。主循环的伪逻辑如下for k 1:T_total/Ts ref ref_gen(x(:,k), time(k)); u_opt nmpc_solve(x(:,k), ref); u_applied u_opt(:,1); x(:,k1) vehicle_update(x(:,k), u_applied, Ts); end里面的核心是nmpc_solve函数下面重点说这一块的代码。4.2 车辆动力学函数的符号建模代码使用CasADi时我习惯先把连续时间模型写成符号表达式再封装成CasADi Function。这样后续在NLP里调用时既可以用在动力学约束中也可以让CasADi自动求出状态梯度不用自己手动推导雅可比矩阵。import casadi.* % 状态变量 X SX.sym(X); Y SX.sym(Y); psi SX.sym(psi); vx SX.sym(vx); vy SX.sym(vy); wz SX.sym(wz); states [X; Y; psi; vx; vy; wz]; n_states length(states); % 控制变量 delta SX.sym(delta); a SX.sym(a); controls [delta; a]; n_controls length(controls); % 车辆参数 m 1500; % 质量 kg Iz 2500; % 横摆转动惯量 kg*m^2 lf 1.2; % 质心到前轴距离 m lr 1.4; % 质心到后轴距离 m Cf 19000; % 前轮等效侧偏刚度 N/rad Cr 21000; % 后轮等效侧偏刚度 N/rad mu 0.9; % 路面附着系数 g 9.8; % 前轮和后轮垂直载荷 Fzf m * g * lr / (lf lr); Fzr m * g * lf / (lf lr); % 轮胎侧偏角 alpha_f atan((vy lf * wz) / max(vx, 0.1)) - delta; alpha_r atan((vy - lr * wz) / max(vx, 0.1)); % 侧向轮胎力线性饱和 Fyf min(Cf * alpha_f, mu * Fzf); Fyr min(Cr * alpha_r, mu * Fzr); Fyf max(Fyf, -mu * Fzf); Fyr max(Fyr, -mu * Fzr); % 状态导数 X_dot vx * cos(psi) - vy * sin(psi); Y_dot vx * sin(psi) vy * cos(psi); psi_dot wz; vx_dot a wz * vy - Fyf * sin(delta) / m; vy_dot (Fyf * cos(delta) Fyr) / m - wz * vx; wz_dot (lf * Fyf * cos(delta) - lr * Fyr) / Iz; rhs [X_dot; Y_dot; psi_dot; vx_dot; vy_dot; wz_dot]; % 封装为函数 f(x,u) - x_dot f Function(f, {states, controls}, {rhs}, {x, u}, {x_dot});代码里的max(vx, 0.1)是为了防止低速时除零。如果你不想让这种“非光滑”操作进入符号函数也可以直接把vx下限定到一个安全值然后在约束里保证vx不会低于那个值。4.3 优化问题构建与求解Opti栈CasADi的Opti栈是最直观的NLP构建方式语法接近数学表达式非常适合NMPC。Np 15; % 预测时域 Ts 0.05; % 采样周期 opti casadi.Opti(); % 优化变量 X opti.variable(n_states, Np1); U opti.variable(n_controls, Np); % 参考轨迹参数 X_ref opti.parameter(n_states, Np1); U_ref opti.parameter(n_controls, Np); % 初始状态参数 x0 opti.parameter(n_states, 1); % 代价函数权重 Q diag([10, 10, 5, 0.5, 0.5, 0.5]); R diag([0.1, 0.1]); S diag([1, 1]); % 累积代价 J 0; for k 1:Np err X(:,k) - X_ref(:,k); du U(:,k) - U_ref(:,k); J J err * Q * err du * R * du; end % 控制增量惩罚 for k 1:Np-1 dU U(:,k1) - U(:,k); J J dU * S * dU; end % 终端代价 errN X(:,Np1) - X_ref(:,Np1); J J errN * Q * errN; opti.minimize(J); % 初始状态约束 opti.subject_to(X(:,1) x0); % 动力学约束 for k 1:Np x_next X(:,k) Ts * f(X(:,k), U(:,k)); opti.subject_to(X(:,k1) x_next); end % 控制量约束 opti.subject_to(-0.5 U(1,:) 0.5); opti.subject_to(-3 U(2,:) 3); % 控制增量约束 for k 1:Np-1 opti.subject_to(-0.3 U(1,k1) - U(1,k) 0.3); end % 状态约束 opti.subject_to(vx_min X(4,:)); % 求解器配置 opti.solver(ipopt, struct, struct(max_iter, 200)); % 编译求解器 solver opti.to_function(solver, {x0, X_ref, U_ref}, {U, X});这里把整个Opti栈封装成了一个casadi.Function输入是当前状态和参考轨迹输出是最优控制序列和状态序列。封装的好处是主循环里只用调用solver函数不需要反复重建NLP仿真速度能快不少。4.4 参考轨迹生成与仿真循环示例下面给一个圆形参考轨迹的例子方便直接跑通。% 圆形参考轨迹 r 20; % 半径 20m vd 5; % 纵向速度 5 m/s t (0:Ts:T_total); theta_ref vd / r * t; X_ref_global r * cos(theta_ref); Y_ref_global r * sin(theta_ref); psi_ref theta_ref pi/2; % 保存为参考状态序列 X_ref [X_ref_global; Y_ref_global; psi_ref; vd * ones(1,length(t)); zeros(2,length(t))];主循环里执行x_current x0; for k 1:length(t)-Np xref_k X_ref(:, k:kNp); uref_k zeros(2, Np); [u_opt, x_opt] solver(x_current, xref_k, uref_k); u_applied full(u_opt(:,1)); x_current full(x_current Ts * f(x_current, u_applied)); % 记录数据 log_u(:,k) u_applied; log_x(:,k1) x_current; end这里把真实车辆模型用和预测模型相同的f函数来更新属于“模型一致性”下的仿真。如果你想做得更严谨可以单独用一个更高精度的车辆模型比如四阶RK4离散的模型作为“被控对象”而NMPC内部仍然用欧拉离散的预测模型这样能观察到模型失配对控制的影响。5. 仿真结果分析与调参避坑5.1 双移线工况下的跟踪效果我拿代码跑过经典双移线工况车速固定80km/h参考轨迹是标准ISO双移线路径。结果是令人满意的最大横向跟踪误差约0.09米横摆角跟踪平滑控制量在移线段会短暂触及转向角上限但未出现振荡。相比之下之前用线性MPC跑同一工况时最大误差超过0.3米而且转向指令有高频抖振。这个对比很有说服力NMPC把非线性和约束同时纳入优化模型预测更准约束提前被考虑所以同样的工况只需要更小的峰值转向角就能完成任务控制动作也更干净。5.2 权重矩阵调整的“手感”经验调权重是NMPC仿真里最耗时间的一环我积累了一些规律性的手感Q(1,1)和Q(2,2)位置误差权重越大车辆轨迹贴合度越高但转向角变化会更剧烈如果权重过高会引发小幅振荡。Q(3,3)横摆角误差权重不能设得太小否则车辆会以较大的侧偏角“漂着”走完轨迹横向位置虽然没误差但车身姿态不好。R(1,1)转向角惩罚加大后控制器会明显减少大转角的使用跟踪高速工况时容易产生滞后所以这个权重要小并不建议一上来就调得很大。S控制增量惩罚是抑制转向抖动的关键。高速双移线工况我把S(1,1)设为2左右抖动基本消失如果设置太小控制指令会在每个采样周期之间跳变执行器模型里看着都难受。5.3 仿真发散、求解失败与约束饱和的根因我调试过程中踩过的坑可以列一个清单方便你对照检查现象常见根因对策仿真发散、状态变成NaN欧拉离散步长过大模型数值不稳定减小Ts到0.01-0.02或改用RK4IPOPT报“infeasible problem”约束过紧、参考轨迹突变、初始控制量不在可行域内添加松弛变量软约束或放宽状态边界求解时间太长预测时域Np过大、CasADi频繁重复建模封装solver函数降低Np或使用编译的Function转向指令抖振缺少控制增量惩罚或Q位置权重过大调大S矩阵或降低Q(1,1) Q(2,2)低速时轮胎力异常vx接近0侧偏角公式出现奇异加vx下界约束或max(vx,0.1)保护控制量长期处于边界参考轨迹过于激进或权重设置不合适检查参考轨迹的曲率、加速度是否超过车辆能力最隐蔽的一个问题是“约束不可行”不一定出现在第一个求解周期而是可能在第5步、第10步突然出现。这是因为滚动优化的起点本身是可行解但参考轨迹拐弯太急到了某个时刻硬约束和动力学约束之间已经没有交集。这种问题排查起来很费神我的经验是一开始就把横向位置偏差做成软约束宁可允许瞬时超限也不要让整个仿真崩溃。5.4 如何把仿真代码扩展到更接近工程如果你希望把研究往工程方向推有几个扩展方向非常顺滑一是换用更真实的轮胎模型比如Pacejka魔术公式或Fiala模型代价是NLP非线性更强求解时间上升。二是在预测模型里加入路面附着系数估计模块让控制器能适应湿滑路面。三是把单轨模型扩展为双轨模型加入载荷转移这样能捕捉左右轮载荷不同带来的附加横摆力矩。四是把控制目标从“跟踪参考轨迹”扩展到“轨迹规划跟踪一体”让优化问题同时决策路径和速度。如果要做CarSim与Simulink联合仿真可以直接把这里的NMPC求解器封装成Simulink模块CarSim提供高精度车辆状态NMPC模块输出控制量给CarSim用来验证模型不一致时的鲁棒性。这也是我推荐的下一个验证步骤因为在纯Matlab模型里预测模型和仿真模型是同一个控制器表现好是“应该的”换到CarSim里预测模型仍用简化单轨模型被控对象变成高精度多体模型才能看出NMPC真正面对模型失配时的表现。在我实际跑这个项目的过程中最意外的不是控制器性能而是模型离散精度对NMPC的约束可行性影响特别大。预测模型和仿真模型如果离散方式不一致哪怕只差一点滚动优化里的约束边界就会“漂移”最后要么导致控制量在边界处反复振荡要么误触发松弛变量。所以我建议你在做这类研究时先严格保证预测模型和仿真模型出自同一个f函数跑通之后再逐步引入模型差异。另一个小技巧是调试时先把所有约束全部放宽让NMPC以纯跟踪模式跑起来确认模型和代价函数没问题之后再逐步收紧约束这样可以把“模型发散”和“约束冲突”两类问题彻底分开。
车辆轨迹跟踪控制这块我一直觉得最磨人的不是控制器理论本身而是怎么把一套带约束的非线性模型预测控制算法在Matlab里跑通还得让车辆动力学模型不中途发散。这篇文章是我把一个NMPC车辆仿真项目从零搭起来的过程记录包含车辆单轨模型推导、带约束NMPC控制器设计、CasADi/IPOPT求解以及整套Matlab仿真代码的核心解析。适合正在做车辆控制课题的学生也适合想从线性MPC切到NMPC的工程师参考。我会尽量把推导和代码串起来讲不写那种“点到为止”的伪教程每一个关键决策都会解释为什么这么做。1. 为什么最终选用带约束的非线性模型预测控制1.1 线性MPC在车辆控制中的“天花板”在哪我最早做的车辆横向控制用的是线性时变MPC方法本身很成熟在每个控制周期把车辆模型在当前工作点线性化然后求解一个二次规划问题。跑低速双移线和定圆工况效果其实不错横向误差能控制在0.05米以内转向动作也说得过去。但一旦速度提上来比如80km/h紧急变道问题就来了。车辆进入非线性区后轮胎侧偏角明显增大线性化模型里的“等效侧偏刚度”已经不再是一个常数。线性MPC预测的是理想化线性车辆的运动轨迹预测出来的轨迹跟车辆实际响应越差越大控制器还会因为模型失准给出偏大或偏振荡的转向指令。我在某个高速双移线工况里实测过横向误差直接从0.08米涨到0.3米以上而且方向盘出现明显的锯齿形抖动。这个问题的根源在于线性化模型只在一个很小的邻域内有效而车辆在极限工况下的动力学本质上是强非线性的。只要预测时域内工作点变化足够大线性MPC的预测优势就会打折扣。想从根本上解决就得把非线性模型直接放进预测环节这就是我用NMPC的原因。1.2 NMPC带来的三样东西预测精度、约束处理、多目标优化选择NMPC不是因为它“听起来高级”而是它同时解决了三个实际问题。第一是预测精度。NMPC直接使用非线性车辆动力学方程做状态递推预测轨迹更接近真实车辆响应模型失配的问题被降到了最低。第二是约束处理。车辆控制器最怕的就是给执行器发一个“根本执行不了”的指令。转向角有物理极限加速度受发动机和制动能力限制横摆角速度也有安全性上限。线性MPC处理这些约束时通常只能做局部线性化近似NMPC则把这些约束全部放进一个非线性规划NLP问题的约束条件里求解器在找最优控制量的同时必须满足这些硬约束所以解出来的控制序列天然就是可执行的。第三是多目标优化。跟踪误差、控制量大小、控制量变化率、横向位置偏离道路中心的风险这些东西在NMPC框架下可以写成一个统一的代价函数。我可以在不改变算法结构的前提下通过调整权重矩阵让控制器在“激进跟踪”和“平顺执行”之间自由切换。这种灵活性线性MPC很难给到这么直接。1.3 仿真研究中的整体架构与工具选择整个仿真项目围绕Matlab展开核心组件是三个模块车辆动力学模块用非线性单轨模型描述车辆纵向、侧向和横摆运动用离散时间方程递推。参考轨迹生成模块生成目标路径和目标速度曲线作为NMPC的跟踪输入。NMPC控制器模块基于CasADi构建非线性优化问题调用IPOPT求解得到最优转向角和加速度序列。为什么用CasADi而不是Matlab自带的fmincon我在早期版本里确实用过fmincon但很快就发现一个问题车辆动力学方程本身已经比较复杂再加上状态约束和控制约束优化问题的有限差分梯度计算非常耗时而且数值精度差经常因为梯度噪声导致求解失败。CasADi的符号引擎会自动求导还能把动力学方程编译成高效函数建模效率高出一个量级求解速度也能满足仿真需求。2. 车辆动力学模型从单轨模型到可预测的非线性系统2.1 为什么用单轨自行车模型而不是复杂多体模型NMPC是滚动优化算法每个控制周期都要重新求解一次NLP。如果预测模型用CarSim级别的多体动力学模型算力根本扛不住尤其是后续想往实时控制器上迁移时模型复杂度直接决定了可行性。所以我选择了经典的单轨自行车模型。这个模型的核心假设是忽略左右轮载荷转移把同轴的两个轮胎合并成一个等效轮胎车辆看作一个刚性车身只考虑纵向运动、侧向运动和横摆运动三个自由度。对于普通乘用车在正常路面上的轨迹跟踪问题单轨模型的精度已经足够。有人说单轨模型是不是太简单了我的经验是它抓主特征的能力远高于它失去的细节。高速换道时车辆响应最关键的是轮胎侧偏特性和横摆转动惯量单轨模型正好保留这两个主要参数而悬架运动学、侧倾特性这些对纵向和横向控制的影响在慢变工况下可以近似忽略。当然如果研究极端操控极限单轨模型就不够用了这部分我会在最后提到。2.2 轮胎力建模从线性到非线性退化轮胎力是车辆与地面之间唯一的力的来源所以轮胎模型的好坏直接决定了NMPC预测是否可信。最简单的线性模型是Fy C * alpha其中C是侧偏刚度alpha是轮胎侧偏角。这个模型在小侧偏角范围内一般小于3度精度很好但在极限工况下完全不成立。轮胎进入非线性饱和区域后侧向力增长变缓再用线性模型预测出来的转弯能力会偏高控制器就会低估实际需要的转向角。我最终在代码里采用了一种带饱和特性的简化轮胎模型Fy min(C * alpha, mu * Fz)也就是先按线性增长达到路面附着极限mu*Fz后保持恒定。这种做法比纯线性模型更接近实际又不像Pacejka魔术公式那样参数众多、计算复杂。做NMPC预测时这种简单非线性模型足以捕捉轮胎饱和的关键趋势。侧偏角的计算也要严格推导。前轮侧偏角定义为前轮速度方向与轮胎朝向之间的夹角alpha_f atan((vy lf * wz) / vx) - delta后轮侧偏角类似alpha_r atan((vy - lr * wz) / vx)其中lf和lr分别是质心到前后轴的距离delta是前轮转角。这个式子里的关键点是横摆角速度wz会在前后轴处产生不同的侧向速度分量所以前后轮侧偏角天然不同这也解释了为什么高速时车辆对前轮转角的响应与低速时差别很大。2.3 状态方程的完整推导与离散化状态向量取[x, y, psi, vx, vy, wz]对应的物理量是全局坐标系下的纵向位置、横向位置、横摆角、纵向速度、侧向速度和横摆角速度。控制向量取[delta, a]即前轮转角与纵向加速度。运动学方程dx vx * cos(psi) - vy * sin(psi) dy vx * sin(psi) vy * cos(psi) dpsi wz动力学方程基于牛顿第二定律和横摆力矩平衡我做了两个工程上的简化忽略纵向力对横摆力矩的贡献忽略前轮转角对纵向力的耦合。这样得到dvx a wz * vy - Fyf * sin(delta) / m dvy (Fyf * cos(delta) Fyr) / m - wz * vx dwz (lf * Fyf * cos(delta) - lr * Fyr) / Iz其中Fyf和Fyr分别按前文的非线性轮胎模型计算。纵向方程里wz*vy是坐标系旋转带来的耦合项有些人容易漏掉但高速大横摆角时这一项影响很大必须保留。模型离散化我用的是固定步长欧拉法仿真步长Ts0.05秒。为什么不用四阶Runge-Kutta欧拉法代码简单、计算量小在50ms步长下精度足够但如果你发现仿真结果发散第一个怀疑对象就应该是离散化步长过大可以改RK4验证。3. NMPC控制器设计代价函数、约束与求解策略3.1 预测时域、控制时域与决策变量维度预测时域Np代表控制器向前看多少步控制时域Nc代表优化变量包含多少个控制量。我在这套代码里取Np15Ts0.05秒相当于向前预测0.75秒。这个时间长度对车辆高速操纵来说是够的因为车辆横摆运动的时间常数一般不到0.5秒0.75秒足以看到当前控制作用的完整效果。Nc我直接取和Np相等也就是每一步都有一个独立的控制量待优化。这样决策变量是2*1530个对IPOPT来说完全是小规模问题。实际工程中如果Np很大为了降维度通常会令NcNp让后段控制量保持不变但会增加一点逻辑复杂度在自己的仿真项目里没必要。需要提醒的是预测时域不是越长越好。时域太长非线性模型的预测误差会累积导致求解出的控制量偏向保守同时NLP的边界约束更容易被不可行解卡住求解时间也急剧上升。我的经验是先从小Np开始比如Np10如果跟踪误差太大再逐渐加长。3.2 代价函数误差加权、控制惩罚与终端代价代价函数我写成J sum_{k0}^{Np-1} (||x_k - x_ref,k||_Q^2 ||u_k - u_ref,k||R^2 ||u{k1} - u_k||_S^2) ||x_Np - x_ref,Np||_P^2第一项惩罚状态跟踪误差第二项惩罚控制量偏离期望值第三项惩罚控制增量用来抑制转向抖动。最后一项是终端代价用来近似无穷时域最优控制减少末端性能恶化。权重矩阵Q、R、S的初始值要格外小心。状态向量的各个分量量纲差异很大位置是米角度是弧度速度是米每秒。如果Q直接填一个大的对角阵数值上位置误差会主导代价横摆角和速度误差很容易被忽略。我的初始设定是Q diag([10, 10, 5, 0.5, 0.5, 0.5]) R diag([0.1, 0.1]) S diag([1, 1])这里Q前两项对应横向和纵向位置误差取10让控制器优先贴线Q第三项对应横摆角误差取5避免车辆“斜着跑”完成跟踪速度相关取小权重因为轨迹跟踪时纵向速度更多由上层规划决定。3.3 约束条件的工程含义与软约束处理NMPC能把约束直接写进优化问题这正是它相比线性MPC最大的工程价值。我在代码里设置的约束包括前轮转角限幅delta ∈ [-0.5, 0.5] rad约±28.6度。纵向加速度限幅a ∈ [-3, 3] m/s²。纵向速度下限vx ≥ 1.0 m/s避免低速时轮胎侧偏角公式里的除法出现奇异。输入增量限幅|delta_{k1} - delta_k| ≤ 0.3 rad/步模拟转向执行器的速率限制。这些约束全部用硬约束形式放进NLP。但有一个问题参考轨迹如果给得过于激进控制器可能找不到满足所有约束的可行解IPOPT会直接报“infeasible problem”。解决这个问题的一个通用技巧是引入松弛变量把部分约束变成软约束。比如我希望限制横向跟踪偏差不要超过0.5米但又不希望在遇到极端参考轨迹时直接无解我可以写成dy_k - slack_k 0.5 slack_k 0并在代价函数里加上对slack的二次惩罚。这样一来当跟踪误差不可能小于0.5米时优化器会选择付出一定代价来放松这个边界而不是无解退出。这个技巧在仿真后期非常实用。3.4 求解器选型CasADiIPOPT vs 手写fmincon我把两者的优劣放在一张表里方便你按自己的情况选方案优点缺点适用场景CasADi IPOPT自动微分精确、建模灵活、求解效率高需要额外安装CasADi和IPOPT环境配置多一步研究生科研、中小规模NMPC、实时性要求较高的仿真fminconMatlab自带、无需额外安装梯度计算依赖有限差分、NLP建模繁琐教学入门、问题规模小、不想装工具箱时我最终选择了CasADiIPOPT。一个重要原因是NMPC要在仿真循环里不断求解同一个“结构”的优化问题只是参数当前状态、参考轨迹在变。CasADi支持将求解器实例化后反复求解可以避免重复建模fmincon每次调用都要重新接收函数句柄大规模预测时效率和数值稳定性都不占优。4. Matlab代码实现与关键环节解析4.1 整体代码结构与仿真流程代码按模块拆分大概是这样参数初始化车辆参数、NMPC权重、约束边界、仿真时长。参考轨迹生成目标路径和目标速度。车辆动力学函数用CasADi符号定义连续时间模型。NMPC求解器构建NLP并在每个控制周期更新参数并求解。主仿真循环状态递推、控制量应用、数据记录。主循环的伪逻辑如下for k 1:T_total/Ts ref ref_gen(x(:,k), time(k)); u_opt nmpc_solve(x(:,k), ref); u_applied u_opt(:,1); x(:,k1) vehicle_update(x(:,k), u_applied, Ts); end里面的核心是nmpc_solve函数下面重点说这一块的代码。4.2 车辆动力学函数的符号建模代码使用CasADi时我习惯先把连续时间模型写成符号表达式再封装成CasADi Function。这样后续在NLP里调用时既可以用在动力学约束中也可以让CasADi自动求出状态梯度不用自己手动推导雅可比矩阵。import casadi.* % 状态变量 X SX.sym(X); Y SX.sym(Y); psi SX.sym(psi); vx SX.sym(vx); vy SX.sym(vy); wz SX.sym(wz); states [X; Y; psi; vx; vy; wz]; n_states length(states); % 控制变量 delta SX.sym(delta); a SX.sym(a); controls [delta; a]; n_controls length(controls); % 车辆参数 m 1500; % 质量 kg Iz 2500; % 横摆转动惯量 kg*m^2 lf 1.2; % 质心到前轴距离 m lr 1.4; % 质心到后轴距离 m Cf 19000; % 前轮等效侧偏刚度 N/rad Cr 21000; % 后轮等效侧偏刚度 N/rad mu 0.9; % 路面附着系数 g 9.8; % 前轮和后轮垂直载荷 Fzf m * g * lr / (lf lr); Fzr m * g * lf / (lf lr); % 轮胎侧偏角 alpha_f atan((vy lf * wz) / max(vx, 0.1)) - delta; alpha_r atan((vy - lr * wz) / max(vx, 0.1)); % 侧向轮胎力线性饱和 Fyf min(Cf * alpha_f, mu * Fzf); Fyr min(Cr * alpha_r, mu * Fzr); Fyf max(Fyf, -mu * Fzf); Fyr max(Fyr, -mu * Fzr); % 状态导数 X_dot vx * cos(psi) - vy * sin(psi); Y_dot vx * sin(psi) vy * cos(psi); psi_dot wz; vx_dot a wz * vy - Fyf * sin(delta) / m; vy_dot (Fyf * cos(delta) Fyr) / m - wz * vx; wz_dot (lf * Fyf * cos(delta) - lr * Fyr) / Iz; rhs [X_dot; Y_dot; psi_dot; vx_dot; vy_dot; wz_dot]; % 封装为函数 f(x,u) - x_dot f Function(f, {states, controls}, {rhs}, {x, u}, {x_dot});代码里的max(vx, 0.1)是为了防止低速时除零。如果你不想让这种“非光滑”操作进入符号函数也可以直接把vx下限定到一个安全值然后在约束里保证vx不会低于那个值。4.3 优化问题构建与求解Opti栈CasADi的Opti栈是最直观的NLP构建方式语法接近数学表达式非常适合NMPC。Np 15; % 预测时域 Ts 0.05; % 采样周期 opti casadi.Opti(); % 优化变量 X opti.variable(n_states, Np1); U opti.variable(n_controls, Np); % 参考轨迹参数 X_ref opti.parameter(n_states, Np1); U_ref opti.parameter(n_controls, Np); % 初始状态参数 x0 opti.parameter(n_states, 1); % 代价函数权重 Q diag([10, 10, 5, 0.5, 0.5, 0.5]); R diag([0.1, 0.1]); S diag([1, 1]); % 累积代价 J 0; for k 1:Np err X(:,k) - X_ref(:,k); du U(:,k) - U_ref(:,k); J J err * Q * err du * R * du; end % 控制增量惩罚 for k 1:Np-1 dU U(:,k1) - U(:,k); J J dU * S * dU; end % 终端代价 errN X(:,Np1) - X_ref(:,Np1); J J errN * Q * errN; opti.minimize(J); % 初始状态约束 opti.subject_to(X(:,1) x0); % 动力学约束 for k 1:Np x_next X(:,k) Ts * f(X(:,k), U(:,k)); opti.subject_to(X(:,k1) x_next); end % 控制量约束 opti.subject_to(-0.5 U(1,:) 0.5); opti.subject_to(-3 U(2,:) 3); % 控制增量约束 for k 1:Np-1 opti.subject_to(-0.3 U(1,k1) - U(1,k) 0.3); end % 状态约束 opti.subject_to(vx_min X(4,:)); % 求解器配置 opti.solver(ipopt, struct, struct(max_iter, 200)); % 编译求解器 solver opti.to_function(solver, {x0, X_ref, U_ref}, {U, X});这里把整个Opti栈封装成了一个casadi.Function输入是当前状态和参考轨迹输出是最优控制序列和状态序列。封装的好处是主循环里只用调用solver函数不需要反复重建NLP仿真速度能快不少。4.4 参考轨迹生成与仿真循环示例下面给一个圆形参考轨迹的例子方便直接跑通。% 圆形参考轨迹 r 20; % 半径 20m vd 5; % 纵向速度 5 m/s t (0:Ts:T_total); theta_ref vd / r * t; X_ref_global r * cos(theta_ref); Y_ref_global r * sin(theta_ref); psi_ref theta_ref pi/2; % 保存为参考状态序列 X_ref [X_ref_global; Y_ref_global; psi_ref; vd * ones(1,length(t)); zeros(2,length(t))];主循环里执行x_current x0; for k 1:length(t)-Np xref_k X_ref(:, k:kNp); uref_k zeros(2, Np); [u_opt, x_opt] solver(x_current, xref_k, uref_k); u_applied full(u_opt(:,1)); x_current full(x_current Ts * f(x_current, u_applied)); % 记录数据 log_u(:,k) u_applied; log_x(:,k1) x_current; end这里把真实车辆模型用和预测模型相同的f函数来更新属于“模型一致性”下的仿真。如果你想做得更严谨可以单独用一个更高精度的车辆模型比如四阶RK4离散的模型作为“被控对象”而NMPC内部仍然用欧拉离散的预测模型这样能观察到模型失配对控制的影响。5. 仿真结果分析与调参避坑5.1 双移线工况下的跟踪效果我拿代码跑过经典双移线工况车速固定80km/h参考轨迹是标准ISO双移线路径。结果是令人满意的最大横向跟踪误差约0.09米横摆角跟踪平滑控制量在移线段会短暂触及转向角上限但未出现振荡。相比之下之前用线性MPC跑同一工况时最大误差超过0.3米而且转向指令有高频抖振。这个对比很有说服力NMPC把非线性和约束同时纳入优化模型预测更准约束提前被考虑所以同样的工况只需要更小的峰值转向角就能完成任务控制动作也更干净。5.2 权重矩阵调整的“手感”经验调权重是NMPC仿真里最耗时间的一环我积累了一些规律性的手感Q(1,1)和Q(2,2)位置误差权重越大车辆轨迹贴合度越高但转向角变化会更剧烈如果权重过高会引发小幅振荡。Q(3,3)横摆角误差权重不能设得太小否则车辆会以较大的侧偏角“漂着”走完轨迹横向位置虽然没误差但车身姿态不好。R(1,1)转向角惩罚加大后控制器会明显减少大转角的使用跟踪高速工况时容易产生滞后所以这个权重要小并不建议一上来就调得很大。S控制增量惩罚是抑制转向抖动的关键。高速双移线工况我把S(1,1)设为2左右抖动基本消失如果设置太小控制指令会在每个采样周期之间跳变执行器模型里看着都难受。5.3 仿真发散、求解失败与约束饱和的根因我调试过程中踩过的坑可以列一个清单方便你对照检查现象常见根因对策仿真发散、状态变成NaN欧拉离散步长过大模型数值不稳定减小Ts到0.01-0.02或改用RK4IPOPT报“infeasible problem”约束过紧、参考轨迹突变、初始控制量不在可行域内添加松弛变量软约束或放宽状态边界求解时间太长预测时域Np过大、CasADi频繁重复建模封装solver函数降低Np或使用编译的Function转向指令抖振缺少控制增量惩罚或Q位置权重过大调大S矩阵或降低Q(1,1) Q(2,2)低速时轮胎力异常vx接近0侧偏角公式出现奇异加vx下界约束或max(vx,0.1)保护控制量长期处于边界参考轨迹过于激进或权重设置不合适检查参考轨迹的曲率、加速度是否超过车辆能力最隐蔽的一个问题是“约束不可行”不一定出现在第一个求解周期而是可能在第5步、第10步突然出现。这是因为滚动优化的起点本身是可行解但参考轨迹拐弯太急到了某个时刻硬约束和动力学约束之间已经没有交集。这种问题排查起来很费神我的经验是一开始就把横向位置偏差做成软约束宁可允许瞬时超限也不要让整个仿真崩溃。5.4 如何把仿真代码扩展到更接近工程如果你希望把研究往工程方向推有几个扩展方向非常顺滑一是换用更真实的轮胎模型比如Pacejka魔术公式或Fiala模型代价是NLP非线性更强求解时间上升。二是在预测模型里加入路面附着系数估计模块让控制器能适应湿滑路面。三是把单轨模型扩展为双轨模型加入载荷转移这样能捕捉左右轮载荷不同带来的附加横摆力矩。四是把控制目标从“跟踪参考轨迹”扩展到“轨迹规划跟踪一体”让优化问题同时决策路径和速度。如果要做CarSim与Simulink联合仿真可以直接把这里的NMPC求解器封装成Simulink模块CarSim提供高精度车辆状态NMPC模块输出控制量给CarSim用来验证模型不一致时的鲁棒性。这也是我推荐的下一个验证步骤因为在纯Matlab模型里预测模型和仿真模型是同一个控制器表现好是“应该的”换到CarSim里预测模型仍用简化单轨模型被控对象变成高精度多体模型才能看出NMPC真正面对模型失配时的表现。在我实际跑这个项目的过程中最意外的不是控制器性能而是模型离散精度对NMPC的约束可行性影响特别大。预测模型和仿真模型如果离散方式不一致哪怕只差一点滚动优化里的约束边界就会“漂移”最后要么导致控制量在边界处反复振荡要么误触发松弛变量。所以我建议你在做这类研究时先严格保证预测模型和仿真模型出自同一个f函数跑通之后再逐步引入模型差异。另一个小技巧是调试时先把所有约束全部放宽让NMPC以纯跟踪模式跑起来确认模型和代价函数没问题之后再逐步收紧约束这样可以把“模型发散”和“约束冲突”两类问题彻底分开。
