做轨迹跟踪这一年多我在Matlab里折腾过不少MPC方案。从最开始用MATLAB自带的Model Predictive Control Toolbox到后来手写QP二次规划求解器再到最终彻底转向CasADi框架做非线性MPC这一路走过来最深的体会是对于质点车辆模型轨迹跟踪这类问题CasADi IPOPT的组合几乎是Matlab环境下最省心的原型验证方案。它不需要你手动推导大规模符号雅可比矩阵也不需要为了凑一个标准二次型而牺牲模型精度建模方式极其贴近物理直觉非常适合把自己脑袋里的控制想法快速跑起来。这篇文章我会把整套思路完整拆开讲为什么选CasADi、质点模型怎么建、Matlab代码一步一步怎么写、闭环仿真怎么搭、以及那些文档里绝对不会告诉你的调参和踩坑经验。适合刚开始做MPC、或者已经用MPC工具箱但不满足于线性模型的读者参考内容能直接落地到你的课题或项目里。1. 为什么绕开MATLAB自带工具箱选择CasADi自己搭1.1 自带MPC工具箱的三个尴尬时刻MATLAB的Model Predictive Control Toolbox确实好用但它有一个前提你得把问题表述成它的标准形式。对于线性时不变系统来说这毫无问题——直接状态空间模型填进去权重矩阵设好控制器就出来了。但一旦遇到非线性被控对象事情就开始别扭了。我在做质点车辆轨迹跟踪时遇到的第一道坎是自带工具箱对非线性模型的支持方式。它要求你提供线性化后的模型也就是说在每个采样周期要么用雅可比线性化去近似要么提前在多个工作点做增益调度。这本身倒不算不可接受问题在于MPC的预测能力恰恰来自对未来多个时刻演化过程的准确推演而你在工具箱里塞进去的是一个线性化模型那么每一步的预测本质上都是在一个不准确的模型上做的轨迹一弯、速度一快跟踪误差就明显涨上去了。第二个尴尬是约束。质点车辆轨迹跟踪里最常见的约束是什么控制量限幅最大加速度、最大转向角速度、状态约束防止跑出道路边界。这些约束在标准工具箱里虽然都能加但如果你想加一些非常规约束比如控制量变化率约束、或者某个关于状态和控制的联立约束就得费不少力气去翻译成标准形式。第三个尴尬是速度。自带工具箱的底层求解本质上是针对线性MPC的高效QP二次规划求解这确实快。可代价是你永远被框在线性模型、线性约束的圈子里稍微非线性一点的东西都施展不开。1.2 CasADi到底解决了什么核心问题CasADi本质上是一个开源符号计算与自动微分框架它最值钱的地方不是能求解优化问题而是把建模和求解彻底解耦了。具体来说你在CasADi里干两件事用符号变量搭建数学模型这和你手推公式的方式完全一致。定义状态变量、控制变量、写出微分方程、离散化整个过程就是写代码逻辑和你在纸上推导动力学一模一样。让框架自动帮你求导模型建完代价函数和约束都写好后CasADi会自动利用前向/逆向AD算法微分把你的模型变成精确的导数值交给底层的非线性规划求解器去算。这一下就省掉了所有手推梯度的噩梦。对我这种建模想让物理直觉直接映射到代码里、调参心思比推公式多的人来说CasADi几乎是直觉友好度拉满的方案。代价函数怎么定就怎么写约束想加几条就加几条模型非线性强不强一点不怵——它本来就是干非线性优化出身的。1.3 几种方案的实际对比方案对非线性模型的支持符号自动微分约束灵活性求解速度上手难度MATLAB自带MPC工具箱弱需线性化/增益调度无中等极快低手写QP/梯度法仅适用于凸问题需手推弱快极高fmincon直接优化支持数值微分较慢中等慢易局部收敛低CasADi IPOPT强支持强快中偏高我实际用下来的体感是如果你的被控对象是线性或弱非线性的自带工具箱是第一选择但只要模型里出现三角函数、带积分的状态耦合、或者你希望今天想改模型就像改一行公式CasADi就是更合理的那个选项。如果你过去习惯用fmincon这类通用优化器做MPC那你换到CasADi后最大的感受会是求解速度明显上来了因为你不必在每次迭代里用数值差分去近似梯度而且底层IPOPT对稀疏大规模问题的处理能力远胜于你在fmincon里硬凹出来的稠密问题。这一点在预测时域N20甚至更高的时候体会特别明显。2. 先把被控对象数学写对质点模型的运动学与离散化2.1 质点车辆模型到底长什么样很多人一听质点车辆模型就觉得太简单——不就是个点在平面上动嘛牛顿第二定律一列不就完了实际做轨迹跟踪时这种简化模型的用意是用最少的自由度把跟踪这件事的核心矛盾暴露出来位置和航向之间靠速度方向耦合而不是纠结于轮胎侧偏、横向动力学这些二阶效应。在Matlab实现里常用的质点模型等价于一个运动学单轨模型unicycle model状态量x全局X坐标、y全局Y坐标、theta航向角控制量v纵向速度、omega角速度连续时间运动学方程dx/dt v * cos(theta) dy/dt v * sin(theta) dtheta/dt omega为什么选这个模型而不是双积分模型dx/dtvx, dvx/dtax因为轨迹跟踪最终关心的是车头朝向和位置共同对参考轨迹的贴合程度。如果只用双积分模型你控制的是质心加速度完全没有朝向信息跟踪弯道时控制动作会变得很别扭。而单轨模型里速度方向直接由航向角决定控制omega就等于直接操控车辆转弯物理因果链清清楚楚。在实际车辆控制里你还可以把控制量从omega替换成前轮转角delta用omega v/L * tan(delta)来建立关系其中L为轴距。不过在做原理验证阶段直接控制omega更直观后面前轮转角模型可以在同样框架下无缝替换。2.2 在CasADi里用符号变量定义连续模型这里直接给Matlab CasADi的代码。先理解这段的工作方式SX.sym是CasADi的符号变量创建函数它创建的不仅仅是普通Matlab变量而是一个参与符号计算的节点后续所有运算都基于这个符号图来构建。import casadi.* % 定义状态和控制符号变量 x SX.sym(x); y SX.sym(y); theta SX.sym(theta); states [x; y; theta]; v SX.sym(v); omega SX.sym(omega); controls [v; omega]; % 连续时间动力学 rhs [v * cos(theta); v * sin(theta); omega]; % 封成CasADi函数后续调用它做离散化和求解 f_continuous Function(f_continuous, {states, controls}, {rhs});这个时候的f_continuous还不直接用于MPC因为MPC是在离散时间域里做滚动优化你需要先把连续模型离散化。2.3 离散化采样周期和RK4的选法逻辑模型离散最常用的三种方法前向欧拉、零阶保持、四阶龙格-库塔RK4。我的经验是对于MPC预测模型除非采样时间极短小于1ms否则不要用前向欧拉。前向欧拉的精度是O(dt)在采样时间设到0.1s~0.2s这种常见范围时RK4比前向欧拉带来的模型失配误差要小一个数量级。MPC滚动优化的性能上限说白了受限于预测模型和被控对象本身之间的匹配程度你用粗离散化引入的误差属于还没开始控制就已经输在起跑线上了。RK4的递推公式在这里直接可以写成CasADi符号运算% 定义RK4单步积分函数dt是采样时间N_internal是内部子步数 function x_next rk4_step(f, dt, x, u) k1 f(x, u); k2 f(x dt/2*k1, u); k3 f(x dt/2*k2, u); k4 f(x dt*k3, u); x_next x dt/6 * (k1 2*k2 2*k3 k4); end在MPC里如果采样时间Ts0.1s你可以直接对每个预测步执行一次RK4即内部子步为1。如果Ts较大比如0.2s以上建议一个预测步内做M2或M3次子步进一步压低离散化误差。function x_next discrete_dynamics(f_continuous, dt, x, u, M) h dt / M; xd x; for i 1:M xd rk4_step(f_continuous, h, xd, u); end x_next xd; end这个discrete_dynamics函数在后面正式MPC代码里会在符号层面被反复调用从而把整个预测时域的状态序列展开成一系列符号表达式。这也是CasADi用起来最舒服的地方——你以循环的方式构建N步预测就像在写普通递推一样。2.4 目标函数和约束的设计轨迹跟踪MPC的目标函数通常拆三块状态跟踪误差代价每个预测步的状态位置和航向与参考轨迹对应点的偏差加权后累加。控制能量代价对控制量本身施加惩罚防止油门/转角动作过于激进。控制变化率代价对相邻两步的控制量差值施加惩罚起到平滑作用这一项在实际工程里非常管用不加它你很快会看到控制命令剧烈抖动的现象。代价函数写成数学形式就是J sum_{k0}^{N-1} ( || X_k - X_ref(k) ||_Q^2 || U_k ||_R^2 || U_k - U_(k-1) ||_S^2 ) || X_N - X_ref(N) ||_P^2其中Q是状态权重对角阵R是控制权重对角阵S是控制增量权重P通常设成和Q相同或稍大终端代价。约束设计上质点车辆至少应有控制量幅值约束v_min v v_maxomega_min omega omega_max如果仿真场景有道路边界可加状态约束y_min y y_max要注意一个经验约束宁少勿多、宁松勿紧。约束太多会让非线性优化求解失败的概率急剧上升尤其初期调试阶段先只留控制量约束跑通后再逐步加入状态约束。这个策略能帮你快速区分是控制算法有问题还是约束搞出来的不可行问题。3. Matlab里用CasADi搭MPC控制器核心代码逐段拆解3.1 环境准备Matlab安装CasADiCasADi是一个独立的C库提供Matlab接口安装方式非常直接。去GitHub找最新release版本下载和你Matlab版本对应的压缩包比如casadi-matlab-linux-x86_64-v3.6.x.tar.gz解压后得到一个casadi文件夹。然后在Matlab里把该文件夹及其子文件夹加入搜索路径addpath(genpath(/path/to/casadi));随后运行import casadi.*即可导入所有核心类。这里提醒一个细节不要把这个文件夹加到Matlab的起始路径里再自己设个工作区变量名冲突CasADi的类名比如Function、SX、Opti都比较通用如果你项目里恰好有同名变量或脚本容易出莫名冲突。我习惯专门建立一个init_casadi.m文件每次运行前手动执行。新版CasADi用的都是Opti栈式接口这比老版的nlpsol接口直观不少。下面全部用Opti来写。3.2 用Opti接口搭建优化问题Opti是CasADi在Matlab环境下提供的高层建模接口整体风格有点类似YALMIP——但它背后挂着的是完整的非线性规划求解链。核心思路三步opti.variable()创建优化变量矩阵形式opti.subject_to()加约束opti.minimize()设置目标函数对MPC问题优化变量是整段预测时域内的状态序列和控制序列。% ---------- MPC参数 ---------- N 20; % 预测时域步数 Ts 0.1; % 采样时间 nx 3; % 状态维度 nu 2; % 控制维度 % 权重矩阵 Q diag([10; 10; 1]); % 位置偏差权重高航向偏差权重相对低 R diag([0.1; 0.1]); % 控制量权重 S diag([1; 1]); % 控制增量权重 % 约束 v_max 2.0; v_min -0.5; omega_max 1.5; omega_min -1.5; % 创建优化问题对象 opti casadi.Opti(); % 预测时域内的状态变量3 x (N1)记作X X opti.variable(nx, N1); % 预测时域内的控制变量2 x N记作U U opti.variable(nu, N); % 上一步实际作用的控制量作为参数传入用于控制增量惩罚 U_prev opti.parameter(nu, 1); % 初始状态和参考轨迹序列也作为参数传入 X0 opti.parameter(nx, 1); X_ref opti.parameter(nx, N1);注意到我把X0、X_ref、U_prev都设成了parameter而不是变量。这是MPC里非常关键的一个操作在滚动优化中测量状态和参考轨迹每次都变但它们不属于优化变量不能每次重新建模而应作为参数在求解前赋值。这样优化问题结构不变求解器可以大量复用前一轮的信息尤其是做warm start时加速非常明显。3.3 动力学约束和代价函数的符号展开接下来把N步递推关系写进优化问题。核心是循环调用离散动力学函数将状态变量连成一条递推链。% 初始状态约束第一步的X必须等于当前测量状态 opti.subject_to(X(:, 1) X0); % 每一控制步的离散动力学约束 for k 1:N x_k X(:, k); u_k U(:, k); x_next X(:, k1); % 用RK4离散动力学约束状态递推关系 pred_x_next discrete_dynamics(f_continuous, Ts, x_k, u_k, 1); opti.subject_to(x_next pred_x_next); end这里discrete_dynamics把CasADi的Function对象作为输入传进去在循环里反复构造符号表达式。每次opti.subject_to实际上都是在累积约束条件最终全部传给底层NLPSolver。下面写目标函数。状态跟踪误差分两步走先用X_ref参数和当前X做差代价是二次型再把控制量代价和控制增量代价加上。cost 0; for k 1:N1 cost cost (X(:, k) - X_ref(:, k)) * Q * (X(:, k) - X_ref(:, k)); end for k 1:N cost cost U(:, k) * R * U(:, k); end for k 2:N cost cost (U(:, k) - U(:, k-1)) * S * (U(:, k) - U(:, k-1)); end % 第一步的控制增量是相对上一时刻实际控制量而言 cost cost (U(:, 1) - U_prev) * S * (U(:, 1) - U_prev); opti.minimize(cost);控制量约束opti.subject_to(v_min U(1, :) v_max); opti.subject_to(omega_min U(2, :) omega_max);然后配置求解器和初始猜测值opti.solver(ipopt, struct(print_level, 0), struct(max_iter, 200)); % 初始猜测给一个合理初值能极大加速求解 opti.set_initial(X, repmat([0; 0; 0], 1, N1)); opti.set_initial(U, zeros(nu, N));注意这里print_level设为0是为了最终闭环仿真时不刷屏实际调试阶段可以把打印打开观察求解器每轮迭代情况。3.4 闭环仿真主循环的完整框架MPC的闭环仿真逻辑很简单测量当前状态 - 生成当前时刻的参考序列 - 赋值给参数 - 求解 - 取第一个控制量 - 施加到被控对象 - 更新状态 - 进入下一个时刻。% ---------- 仿真参数 ---------- T_sim 20; N_steps T_sim / Ts; % 预分配存储 X_log zeros(nx, N_steps1); U_log zeros(nu, N_steps); % 初始状态 X_current [0; 0; 0]; X_log(:, 1) X_current; % 上一个实际控制量 U_pre [0; 0]; for k 1:N_steps % 获取当前时刻的参考轨迹序列后面第4节详细讲怎么生成 X_ref_k get_reference_sequence(k, N, Ts); % 给参数赋值 opti.set_value(X0, X_current); opti.set_value(X_ref, X_ref_k); opti.set_value(U_prev, U_pre); % 把上一轮的求解结果作为当前轮的初始猜测warm start if k 1 opti.set_initial(X, X_opt); opti.set_initial(U, U_opt); end % 求解 try sol opti.solve(); X_opt sol.value(X); U_opt sol.value(U); catch % 求解失败时的安全兜底用上一轮控制量 disp([Step , num2str(k), solve failed]); U_apply U_pre; % 继续下一步 % 这里实际生产必须停止运行并查明原因调试期可以临时这样处理 U_log(:, k) U_apply; % 仿真被控对象模型 X_current rk4_simulate_plant(f_continuous, Ts, X_current, U_apply); X_log(:, k1) X_current; U_pre U_apply; continue; end % 取第一个控制量施加给被控对象 U_apply full(U_opt(:, 1)); U_log(:, k) U_apply; % 仿真被控对象用同样的RK4离散化也可以引入过程噪声 X_current rk4_simulate_plant(f_continuous, Ts, X_current, U_apply); X_log(:, k1) X_current; U_pre U_apply; end这里rk4_simulate_plant直接用前面定义的rk4_step即可只不过被控对象仿真可以比预测模型更精细比如内部子步数取大一点。区分预测模型和仿真被控对象模型同样是一个工程经验MPC设计者应当刻意让被控对象模型和预测模型之间留一点差异这样才能测试控制器的鲁棒性。如果二者完全一致你在仿真里看到的完美跟踪性能在实车/实物上一定会打折扣因为你永远不可能得到精确等于预测模型的真实系统。求解失败时的处理逻辑我在这里只是简单continue实际情况下你需要记录失败发生的前后状态、控制轨迹、求解器的返回信息仔细分析原因第5节详细讲。调试期用上一时刻控制量兜底可以在早期开发时避免程序崩溃但一旦进入结果分析阶段必须保证全轨迹零失败。3.5 三个需要特别留心的代码细节第一sol.value(X)返回的是CasADi的DM对象如果你直接用在普通矩阵运算里可能有维度问题最好用full()转成Matlab double矩阵再赋值第二opti.set_initial(X, ...)传入的矩阵维度必须完全匹配变量维度否则静默出错很隐蔽第三每次循环求解时opti.solve()内部会自动更新优化变量初值为上次的解所以你手动设置的set_initial其实只是提供了更强的warm start底座配合使用效果最佳。4. 轨迹跟踪仿真真正需要反复调的三个旋钮4.1 参考轨迹生成最容易忽略但决定成败的一环跑MPC跟踪仿真参考轨迹不是随便画条曲线就行。它必须是这个动力学模型可行的轨迹也就是说参考轨迹上的每个点都应当能被某个控制序列精确产生。这看起来是废话实际操作中特别容易犯的错是用高次多项式拟合一条路径然后只给MPC提供位置参考(x_ref, y_ref)航向参考theta_ref却自己拍脑袋填0——结果就是控制器无论如何都追不上因为给定的\theta_ref根本与路径切线方向不一致。正确的做法是先生成每条参考段的控制输入期望速度v_ref和期望角速度omega_ref再用同样的运动学方程积分得到状态轨迹。这样生成的参考轨迹天然满足模型约束MPC的跟踪任务才是一个可达的目标。我用的是很常见的8字形轨迹参数方程function [states_ref, controls_ref] generate_figure8_reference(t, Ts, N, v_des) % t是当前仿真时刻 % 8字形参数方程为每个参考点生成速度和角速度 L 2.5; % 8字形尺度参数 tau linspace(t, t N*Ts, N1); x_ref L * sin(0.5 * tau); y_ref L * sin(tau); % 参考速度切向速度恒定v_des dx_dtau 0.5 * L * cos(0.5 * tau); dy_dtau L * cos(tau); theta_ref atan2(dy_dtau, dx_dtau); % 参考角速度航向角的数值差分 theta_dot_ref [diff(theta_ref), 0] / Ts; states_ref [x_ref; y_ref; theta_ref]; controls_ref [v_des * ones(1, N); theta_dot_ref(1:N)]; end这段代码生成的状态参考序列恰好满足dx/dt v*cos(theta)dy/dt v*sin(theta)的关系在数值意义下成立。闭环仿真里每一采样时刻都调用这个函数生成长度为N1的参考序列传给MPC参数。值得注意theta_ref用atan2计算从0到2pi之间会有跳变。如果在参考序列中间发生跳变比如从接近pi突然跳到接近-piMPC会认为这是一个巨大的跟踪误差从而产生疯狂的角速度命令。所以当你的参考轨迹包含多圈或大转角时记得做角度unwrap处理把参考航向角展成连续曲线。这个坑我在早期调试时踩得刻骨铭心——表现特征是误差突然出现脉冲尖峰控制量直接顶到约束边界。4.2 权重调参跟踪性能和控制能量的谈判桌MPC调参在工程上和PID调参有点类似但由于有预测时域的存在参数间相互作用更微妙。我的经验是分三个层次来调。先调Q和R的比例关系。Q矩阵控制的是跟踪精准度R矩阵控制的是控制动作的激进程度。如果跟踪缓慢滞后增加Q中对应位置权重如果控制量抖动或冲过头增加R。对质点车辆模型经验法则是位置权重Q(1,1), Q(2,2)与控制权重R(1,1), R(2,2)之间相差两个数量级算比较合理。比如我用的Q对角是[10, 10, 1]R对角是[0.1, 0.1]。然后是S矩阵也就是控制变化率惩罚。很多人调MPC时忽略这一项直接导致控制量在步与步之间剧烈跳变。加S本质上是在控制系统里引入了执行器舒适度的概念对加速度变化率和转角速率变化率加以限制。经验是S绝对值不要大于R的10倍否则跟踪误差会被迫增大因为控制器为了减小变化率而放弃了应有的跟踪能力。最后才是终端代价P。理论上有LQR或者终端约束可以将MPC的稳定性变成半全局渐近稳定但在工程实践里只要预测时域够长PQ甚至P0也能得到可用结果。如果你发现轨迹跟踪在末端有回弹现象可以把P加大来抑制。下面是我测试过的一组比较省心的参数针对8字形、最大速度2m/s的情况参数值调试备注Ts0.1s跟踪8字形足够更快轨迹需要更小TsN20预测时长2s覆盖弯道曲率变化Qdiag([10, 10, 1])位置权重同量级航向单独一档Rdiag([0.1, 0.1])控制代价防止命令幅度过大Sdiag([1, 1])平滑控制命令性能好坏的润滑剂v_max / v_min2.0 / -0.5允许小幅倒车omega_max / omega_min1.5 / -1.5限制最大转弯速率这组参数跑出来的典型表现起始阶段跟踪误差约0.5米量级1秒内收敛到0.05米以内稳态跟踪误差受离散化和模型失配影响稳定在小数点后两位。如果你发现跟踪误差持续在0.1米以上先别急着调参数很可能前面参考轨迹生成的可行性检查没做好。4.3 预测时域N和采样时间Ts的博弈预测时域N是MPC里最直白的一个旋钮N越大控制器看得越远提前规划能力越强但计算量越大且超出一定范围后收益递减。经验数值法则是预测时域的物理时长T_p N * Ts至少要覆盖参考轨迹中一个明显的特征长度。对8字形轨迹周期大概是多少秒参数方程里x方向频率是0.5Hzy方向是1Hz一个完整8字形的周期约需要6.3秒。如果NTs只有1秒控制器只能观测到一小段局部轨迹遇到弯道时反应较为迟钝。我通常建议让NTs覆盖轨迹周期的1/3以上8字形就是至少2秒也就是N20 at Ts0.1。但N也不能盲目的太大。非线性MPC每轮求解的计算量随N近似线性增长但求解的可行域复杂性会让单次求解时间有时不是线性增长而是跳跃式增加。在Matlab纯解释环境下N20配IPOPT一般能稳定在几十毫秒量级N50就存在卡顿感。通过选择合适的Ts而不是硬拉N也可以延长时间视野。比如Ts0.2、N20能让预测时长到4秒代价是控制分辨率变粗、离散化误差增大。关于Ts的另一种思路是使用多速率MPC预测模型里的控制量在每个步长内保持恒定但实际的被控对象仿真可以更细粒度更新。这样采样时间没那么敏感预测时间视野更容易拉长。不过这个属于进阶优化初学阶段还是建议老老实实用Ts0.1。4.4 一个完整仿真结果的行为分析跑通之后观察曲线要从三个维度审视状态跟踪误差曲线、控制量曲线、以及两者的耦合关系。如果你画误差曲线分离出一个有意思的现象在8字形轨迹的交叉点附近横向跟踪误差会出现一个小尖峰。这是因为交叉点处曲率变化极快而预测时域有限控制器在进入交叉点前没有足够前瞻时间做出反应。这不是代码bug也不是参数问题而是MPC预测时域有限这个本质属性的体现。把N加大或者把Ts减小可以部分缓解但永远无法彻底消除因为预测时域有限意味着信息永远不可能完全提前可知。控制量曲线则需要重点关注有没有明显的高频抖动。如果抖动频率和采样频率一致说明MPC在神经质地来回修正多半是S权重太小、或者R权重太大导致控制器过度补偿微小误差。如果控制量反而缓慢滞后、像加了低通滤波器那就是R太大或者Q太小控制器胆子太小了。最常见的理想状态是一条既贴合参考控制量、又没有尖峰的曲线。这意味着控制器既追踪了期望命令又在必要时以平滑的补偿动作修正了偏差。这样的曲线对应的MPC通常才是真正调好了的。5. 从NaN到求解失败我踩过的坑和完整排查思路5.1 常见报错背后的真实原因CasADi IPOPT这套组合在Matlab下报错的样子五花八门但错误根源其实非常集中。我复盘自己这一年踩过的所有雷归纳出四大类高频问题。第一类是维度不匹配。典型报错信息形如Error in SX::mx或者Argument dimensions mismatch。这类问题常见于你手写代价函数时X(:, k) - X_ref(:, k)的维度没对齐。建议在搭建优化模型前先打印size(X(:, k))和size(X_ref(:, k))自查。第二类是符号变量混用。CasADi里SX和MX是两个不同层次的符号图混在一起有时会提示MXFunction不兼容的报错。解决方式简单粗暴入门阶段统一用SX等需要大规模动态规划时再换MX。第三类是NaN传播。仿真中某个状态变量变成NaN后后续所有计算全部崩溃。根源几乎都是目标函数或约束里出现了除零或者无穷大比如atan2(y, x)当x和y同时接近零会出现数值奇异比如角度wrap导致跳变比如1/x形式的表达式进入求解器。排查方法是用断点检查NaN最先出现在哪个迭代值用sol.debug提取内点法的迭代信息。第四类是初值问题。IPOPT是局部求解器如果初始猜测离可行域太远很容易直接宣布求解失败。特别是带约束的MPC初值全设0通常不是一个好的可行点。我的经验是至少让初值里的第一个状态满足初始状态约束控制量初值设成上一轮的可行解这能让求解成功率大幅提升。5.2 一个真实排障过程的完整链路我遇到过最诡异的一次情况是仿真前几步一切正常走到某个时间点后突然求解失败报错显示Converged to a point of local infeasibility。当时第一反应是约束太紧了但松开约束后问题照旧。后来我把排查重点放到参考轨迹上发现那个失败时间点恰好是参考轨迹处于8字形交叉点附近相对应地theta_ref接近±pi。进一步检查后发现generate_figure8_reference函数里使用atan2得到的航向角在交叉点处从接近-pi跳变到了pi。这个跳变虽然物理上真实存在角度从-180度到180度但对MPC来说由于参考序列里包含了这种非连续跳变从pi-0.1到-pi0.1的误差差异约等于2pi被Q权重放大后会产生极其强烈且不合理的代价信号驱使求解器去优化一个本不该存在的巨大误差。查明原因后修复方式有两个方向一是将参考航向角序列做unwrap处理得到连续的角度值二是在目标函数里对角度误差做周期化处理比如用sin((theta - theta_ref)/2)的平方作为角度误差度量。两个方向都能解决问题我在实现时采用了第一种简单直观而且不引入额外的目标函数非线性。这个例子特别能说明一个通用排障方法论MPC求解失败时不要一头扎进求解器参数里先检查你喂给求解器的输入是什么。输入状态测量、参考轨迹本身有毛病后面调再多权重也是白费。5.3 性能优化从仿真原型到实时部署的距离纯Matlab环境下CasADi IPOPT做N20的MPC单步求解普遍在几十毫秒量级。这在离线仿真里没问题但如果你要上实车或者做硬件在环就需要考虑加速手段。CasADi提供的最直接加速方式是代码生成。你可以在Matlab里用codegen把MPC的优化问题导出为独立的C语言函数然后在C环境里编译结合内嵌的IPOPT运行时库单步求解速度能提升一到两个数量级。这对Matlab用户尤其友好建模阶段你完全用Matlab写、Matlab调试最后生成C代码部署。另一种方式是使用CasADi更基础的nlpsol接口配合自定义求解器参数绕开Opti栈式建模的额外开销。但就我的经验nlpsol的接口更底层开发效率低不少一般只有追求极致速度时才值得做。再补一个比较隐蔽的优化点在闭环仿真中如果被控对象模型参数有不确定性可以考虑在MPC代价函数里加入对末端状态的鲁棒项或者在求解前对X_ref序列做一阶低通滤波防止参考轨迹本身抖动传导到控制命令。这个属于锦上添花但实际运行效果确实会更稳。最后说点个人经验体会吧。MPC这套东西难的地方从来不是理论推导而是当一个非线性优化问题在你面前失败的时候你能不能快速定位问题的根源。CasADi这个框架的好处在于它把建模写得足够透明、足够贴物理让我能把注意力集中在模型、约束、参考轨迹这些真正属于自己系统的事情上而不是在繁琐的数值细节里挣扎。如果你也正在用Matlab做轨迹跟踪相关的控制算法验证我建议你花两天时间把CasADi这套流程跑通——哪个环节不是特别确定直接回看文中对应的代码段和排障逻辑这个投入绝对值回票价。