做轨迹跟踪的工程实现我一直有个习惯控制律在草稿纸上推完先不急着写代码而是问自己一句“这个优化问题今天约束变不变”。像 PID、LQR 这类方法调好增益之后就是一套固定反馈模型一换、约束一加反馈增益往往要重新推导MPC 的核心优势就在于把“当前时刻的优化问题”原封不动交给求解器约束、目标都在问题里描述滚动执行即可。可真在 Matlab 里徒手搭 MPC又会撞上另一堵墙预测模型怎么写、代价函数怎么拼成大矩阵、梯度怎么算——如果每一步都手推一遍一个晚上基本就交代给矩阵维度对不对得上了。这也是我后来把 CasADi 拉进工作流的原因。CasADi 是一个开源符号运算与非线性优化框架Matlab 和 Python 都有接口它最大的价值是能把你脑子里的 MPC 原样“翻译”成计算机可求解的优化问题。这篇文章就用 CasADi 搭一个基于质点车辆模型的 MPC 轨迹跟踪器模型不复杂但完整覆盖状态方程离散化、滚动优化求解、闭环仿真三个环节。适合正在入门 MPC、或者想脱离 Matlab 内置工具箱限制的工程师和学生。看完代码就能拿去改自己的控制问题。1. 质点车辆模型先搞清楚被控对象到底长什么样很多看 MPC 教程的朋友会卡在第一步上来就给我自行车模型、动力学模型一堆参数还没调明白优化器已经报错了。我的建议是第一版轨迹跟踪器能多简单就多简单——先把控制链路跑通再往被控对象上加复杂度。1.1 质点模型的边界与适用场景质点车辆模型就是把车辆缩成一个质量点。系统状态是平面坐标 (px, py) 和平面速度 (vx, vy)控制输入是加速度 (ax, ay)。数学上就是二维平面上每个通道各放一个二重积分器$$\begin{cases} \dot{p}_x v_x \ \dot{p}_y v_y \ \dot{v}_x a_x \ \dot{v}_y a_y \end{cases}$$这个简化不是拍脑袋它过滤掉了轮胎侧偏、转向几何、横摆角速度等一大堆细节只保留“位置-速度-加速度”的运动学骨架。它特别适合三类场景一是 MPC 算法学习期的跑通验证二是全向移动机器人、无人机位置环这类执行器天然解耦的平台三是在方案论证阶段快速估算“这个 MPC 能不能跟上这条轨迹”。至于真实车辆的高速工况它必然是不够的——没有横摆角就没有转向能力这一点心里要有数。1.2 连续方程与离散化从积分器链到 A、B 矩阵MPC 需要的是离散时间模型。以采样周期 dt 对上述积分器链做零阶保持器ZOH离散可以直接写出精确的 A、B 矩阵$$A \begin{bmatrix} 1 0 dt 0 \ 0 1 0 dt \ 0 0 1 0 \ 0 0 0 1 \end{bmatrix}, \quad B \begin{bmatrix} \frac{1}{2}dt^2 0 \ 0 \frac{1}{2}dt^2 \ dt 0 \ 0 dt \end{bmatrix}$$位置增量是 dtv 0.5dt²a速度增量是 dta这个形式比一阶欧拉法多保留了加速度对位置的二阶贡献。我实测下来即使 dt 放到 0.2 秒轨迹误差也只比 dt0.05 时差一点但如果用简单欧拉离散dt 一大就容易看出位置滞后。所以能用精确离散就尽量别偷懒用欧拉代价几乎为零精度却更稳。1.3 参考轨迹为什么首选圆形轨迹跟踪需要一条期望轨迹第一版我强烈推荐圆形。半径 R 和角速度 omega 一给参考状态可以解析写成$$x_{ref}(t) \begin{bmatrix} R\cos(\omega t) \ R\sin(\omega t) \ -R\omega\sin(\omega t) \ R\omega\cos(\omega t) \end{bmatrix}$$选圆形有三个理由曲率恒定、速度平滑、加速度连续。试想如果一上来就用方波路径或者带尖角的 8 字轨迹MPC 跟踪不好你都不知道是控制器问题还是路径本身不可达。我通常给 R2m、omega0.5 rad/s此时切向线速度 1 m/s所需向心加速度只有 0.5 m/s²远小于执行器能力第一次跑通的把握就大了很多。2. 选型思路CasADi 为什么比手推 QP 更适合做 MPC控制问题里最怕的不是模型复杂而是“方案换一个代码全重写”。在没见过 CasADi 之前我用过最原始的方式把 MPC 化成二次规划QP然后丢给 quadprog这种方式对质点模型完全可行但只有做过的人才知道里面有多少隐性成本。2.1 手写 QP 实现 MPC 的那些痛点线性模型、二次代价、线性约束理论上都能重构成标准 QP。实际操作时你要自己把预测模型展开成增广矩阵代价函数里的 Q、R 铺成块对角大矩阵再处理约束矩阵的行列匹配。模型维度一变所有矩阵的尺寸全部跟着变。最难受的是一旦你想加点“花样”比如避障距离的倒数项、终端罚函数的非线性项整个 QP 框架就得推翻重新设计。研究生阶段的毕设这么写没问题但如果你是为了快速验证控制思路这个时间成本太高了。2.2 Opti 栈式建模把优化问题写成“它本身的样子”CasADi 的做法完全不同。它提供了一个名为 Opti 的建模接口你不需要关心最终 NLP 问题的稀疏模式直接按照数学形式写约束和代价就行。随便感受一下import casadi.* opti casadi.Opti(); X opti.variable(4, N1); % 状态序列 U opti.variable(2, N); % 控制序列 opti.minimize(J); opti.subject_to(X(:,1) x0); opti.subject_to(X(:,k1) A*X(:,k) B*U(:,k)); opti.solver(ipopt); sol opti.solve();这种写法的好处在于你只需要描述“问题本身”剩下的自动微分、雅可比计算、NLP 求解全部由框架代劳。刚开始用的时候会有种不真实的轻松感——以前要手推一个上午的矩阵维度现在几行约束就搞定了而且改约束条件也只是增删一行 subject_to 而已。2.3 CasADi 与 Matlab MPC 工具箱的取舍很多用 Matlab 的人会问不是有 MPC Toolbox 吗实话实说官方工具箱在产线部署和线性 MPC 快速验证上很成熟但它更像一个封装好的黑盒。当你需要自定义非线性模型、非标准代价项比如避障势场或者苛刻的求解器调试时工具箱的限制就暴露出来了。我整理了一个简单对比维度Matlab MPC ToolboxCasADi IPOPT模型形式线性/经线性化的对象线性、非线性均可代价函数标准二次型为主可任意定义约束常用约束封装良好任意非线性约束代码自由度受工具箱接口限制完全开放学习成本界面化操作上手快需要理解 NLP 建模费用商业授权开源免费我个人的选择标准很直接如果只是标准线性 MPC工具箱更省事但只要是搞算法研究、自定义问题或者想彻底搞懂 MPC 内部机制CasADi 的路径明显更宽。3. MPC 滚动优化拆解预测、代价、约束三者怎么协同在写代码之前有一层理论需要先想清楚MPC 每一拍到底在算什么。这不是推导公式而是建立一种“控制直觉”。很多代码跑通了但效果不对往往就是这一层没理顺。3.1 预测模型与控制序列的关系MPC 的基本操作分三步在 k 时刻获取当前状态 x0在预测窗口内求解有限时域最优控制序列 u0, u1, ..., u_{N-1}只把第一个控制量 u0 送给被控对象。下一拍重复这个过程所以叫滚动时域控制。CasADi 建模时我推荐把所有状态序列直接作为决策变量引入即所谓的多段打靶Multiple Shooting形式。状态变量 x1, x2, ..., x_N 和输入变量 u0, ..., u_{N-1} 都是优化变量系统动力学依靠等式约束串联。这种形式比只优化输入序列的单步打靶更容易给求解器提供初始猜测尤其在后文要扩展到非线性模型的时候多段打靶的鲁棒性明显更好。3.2 代价函数里的 Q 和 R跟踪精度与控制代价的博弈我用的代价函数是这个样子$$J \sum_{k1}^{N1} (x_k - x_{ref,k})^T Q (x_k - x_{ref,k}) \sum_{k0}^{N-1} u_k^T R u_k$$Q 的物理含义是“状态偏差多严重”R 的物理含义是“控制量多用多少代价”。之所以每个预测步都要累加误差是为了让控制器兼顾眼前和远方而不是只顾一步到位。对于这个质点模型我习惯把 Q 设为 diag([10, 10, 1, 1])位置误差权重大速度误差权重小R 设为 diag([0.1, 0.1])让控制器有足够的勇气输出加速度。权重调整有个经验顺序先固定 R逐步增大 Q观察跟踪误差和控制幅度的变化。如果位置误差一直压不下来先别急着把 Q 调上千先看参考轨迹的加速度需求是否已经被执行器上限卡住。R 太大会让控制器变得“麻木”误差大也懒得纠正R 太小则容易诱发输入抖振。这个平衡要在仿真里多试几次才能找到手感。3.3 采样周期与预测步数先定时间尺度再调参数初学很容易陷入“参数越多越慌”的局面所以我习惯先把时间尺度定死再去微调 Q/R。第一版的参数组合我推荐 dt0.1sN20这样预测总时长为 2 秒。对于质点模型这种快速动态系统2 秒的视野足够收敛对于真实车辆平台预测时域通常也落在这个区间。dt 的设定要看执行器更新率。如果底层执行器只能 10Hz 更新那你把 dt0.01s 纯属自己为难自己——优化结果再精确也送不过去。反过来dt 太大又会让离散模型丢掉连续系统的动态细节。N 的设定主要受求解时间限制N20 时 IPOPT 一次求解通常只有几毫秒到十几毫秒远小于采样周期 0.1 秒完全可以实时跑。如果你的电脑性能较弱可以先降到 N15预测时域 1.5 秒效果差别不大。4. Matlab 下的 CasADi 实现从建模到闭环仿真全链路纸上谈兵就到此为止下面给出可直接运行的核心代码。我尽量把每个块都拆开解释这样你发现问题时不至于对着整个脚本干瞪眼。4.1 环境准备与接口版本CasADi 支持 Matlab 接口去官网下载对应版本的压缩包解压后把文件夹用 addpath 添加进去即可。这里最容易出问题的坑是版本和平台位数不匹配如果运行时提示“Invalid MEX file”十有八九是下载了不匹配的版本重新下载对应平台和 Matlab 位数的那份就好。IPOPT 求解器已经打包在 CasADi 内部一般不需要另外安装反而建议不要自己装第二套 IPOPT免得路径冲突。4.2 核心建模求解代码逐段拆解import casadi.* dt 0.1; % 采样周期 N 20; % 预测步数 R_circle 2.0; % 圆形轨迹半径 omega 0.5; % 圆形轨迹角速度 umax 5.0; % 加速度上限 % 离散模型矩阵 A [1, 0, dt, 0; 0, 1, 0, dt; 0, 0, 1, 0; 0, 0, 0, 1]; B [0.5*dt^2, 0; 0, 0.5*dt^2; dt, 0; 0, dt]; % 优化问题声明 opti casadi.Opti(); X opti.variable(4, N1); % 状态序列 U opti.variable(2, N); % 控制序列 x0 opti.parameter(4, 1); % 初始状态滚动更新 Q diag([10, 10, 1, 1]); R diag([0.1, 0.1]); % 参考轨迹填充 x_ref zeros(4, N1); for k 1:N1 t_k (k-1) * dt; x_ref(:, k) [R_circle * cos(omega * t_k); R_circle * sin(omega * t_k); -R_circle * omega * sin(omega * t_k); R_circle * omega * cos(omega * t_k)]; end % 约束 opti.subject_to(X(:, 1) x0); % 初始状态 for k 1:N opti.subject_to(X(:, k1) A * X(:, k) B * U(:, k)); % 动态方程 opti.subject_to(-umax U(:, k) umax); % 输入限幅 end % 代价函数 J 0; for k 1:N1 e X(:, k) - x_ref(:, k); J J e * Q * e; end for k 1:N J J U(:, k) * R * U(:, k); end opti.minimize(J); % 求解器设置 opti.solver(ipopt, struct(print_time, false, print_level, 0));这段代码的核心有两个。第一个核心是opti.parameter它声明了一个“每拍会更新但结构不变”的参数相当于优化问题常量。这样每步仿真只需要set_value更新初始状态不必重新构建整个优化问题效率会高很多。第二个核心是代价函数采用两个循环累加这是最直观的表述方式CasADi 会自动把它编译成高效的表达式不用手动展开成块对角矩阵。4.3 闭环仿真滚动求解与控制指令下发下面这段是滚动求解的闭环逻辑。每步迭代里用当前状态填充 x0求解优化问题提取 U 的第一列作为控制量再用模型方程推进被控对象到下一时刻% 仿真参数 simT 10; steps ceil(simT / dt); % 初始状态 xnow [0; 0; 0; 0]; pos_history zeros(2, steps1); pos_history(:, 1) xnow(1:2); for i 1:steps t_now (i-1) * dt; % 按真实全局时间更新参考轨迹 for k 1:N1 t_k t_now (k-1) * dt; x_ref(:, k) [R_circle * cos(omega * t_k); R_circle * sin(omega * t_k); -R_circle * omega * sin(omega * t_k); R_circle * omega * cos(omega * t_k)]; end % 更新初值并求解 opti.set_value(x0, xnow); sol opti.solve(); % 取第一个控制量推进模型 u0 sol.value(U(:, 1)); xnow A * xnow B * u0; % 记录轨迹 pos_history(:, i1) xnow(1:2); end这里必须强调一个我踩过的坑参考轨迹一定要用全局时间 t_now 累加而不是每个周期从 0 开始重新生成。如果你照抄前文建模代码里那个(k-1)*dt去填参考轨迹那么每一拍参考路径都会从头开始MPC 永远在追一个同一起点出发的“影子轨迹”仿真画出来是一条原地打转的线。这个问题非常隐蔽因为你单独看每个时刻的参考轨迹都觉得是对的。跑完之后绘图很简单ref_history zeros(2, steps1); for i 1:steps1 t_k (i-1) * dt; ref_history(:, i) [R_circle * cos(omega * t_k); R_circle * sin(omega * t_k)]; end figure; plot(ref_history(1, :), ref_history(2, :), k--, LineWidth, 1.5); hold on; plot(pos_history(1, :), pos_history(2, :), b-, LineWidth, 1.5); xlabel(x/m); ylabel(y/m); legend(参考轨迹, MPC实际轨迹); axis equal; grid on;我跑下来的典型结果是车辆从原点出发大约 1 秒内切入圆形轨迹之后位置误差能稳定在 0.1 米量级。如果你的误差远大于此先别怀疑模型回头检查参考速度分量是不是也一起跟踪了——很多初版代码只让位置跟踪参考速度项给的是 0那控制器自然会在切向方向出现持续的滞后误差。5. 实测中的拦路虎求解失败、抖振与参数调优代码能跑出好看的圆形轨迹只是第一步真实控制场景中你会遇到各种各样“看起来没道理”的异常。我把自己实际处理过的三类问题总结在下面顺序就是排查优先级。5.1 IPOPT 报 Infeasible一例可复现的排查链路有一次我把参考轨迹从圆形换成了 Lissajous 图形也就是 8 字轨迹第一轮求解直接报错Infeasible Problem Detected。新手这时候最容易慌开始怀疑建模写错了、矩阵算错了。我的排查链路是这样的你可以照着走一遍第一步把参考轨迹改成一个静止点——比如让 x_ref 恒等于当前初始状态或者非常缓慢的直线。如果这个简化问题能求解说明 CasADi 建模和求解器配置没有问题。第二步恢复 8 字轨迹但把参考速度整体缩小一半如果又能解了问题一定出在“参考轨迹的物理需求超出了执行器能力”。第三步自己手算 8 字轨迹在最大曲率点的向心加速度对比你的 umax大概率发现是参考轨迹曲率过猛要求加速度超过了限幅值于是优化问题在预测时域内根本找不到可行解。这个排查链路的核心原则是把模型和问题解耦。任何求解失败先不要怀疑优化器先问“这个参考轨迹在约束下到底可不可能做到”。MPC 的跟踪项本来放在代价里是软约束通常不会报 infeasible一旦报错基本上都是硬约束之间互相冲突最常见的元凶就是参考轨迹所隐含的加速度需求超过输入约束。5.2 控制量抖动的来源与处理圆形轨迹跑通之后你可能会看到控制量在很小的位置误差下来回冲击上下界这就是抖振。抖振的本质是代价函数里控制量的权重偏小控制器认为“猛打一下”和“温柔修正”产生的代价差异不大于是优化器选择了看起来很激进、数值上却不稳定的解。处理顺序我建议是先增大 R从 0.1 调到 1.0 试试如果还抖增大预测步数 N让控制器能预见到更远的目标避免每一步都像“近视眼”一样过度修正再不行就在代价里加入控制增量惩罚项S diag([0.5, 0.5]); % 控制增量惩罚权重 for k 1:N-1 du U(:, k1) - U(:, k); J J du * S * du; end控制增量惩罚的物理含义很直观我不想让两次指令之间变化太猛。这个手段比低通滤波平滑控制量更治本因为滤波器是事后削弱执行器的响应速度增量惩罚是从优化目标上抑制抖振的来源。还有一个容易被忽略的抖动源头参考速度不连续。圆形轨迹很平滑看不出问题一旦参考路径改成折线或者方波速度参考在拐角处突变控制器就会在每一拍拼命追赶阶梯状的速度曲线表现为控制量的高频切换。解决方式是给参考路径本身做平滑比如用梯形速度规划。5.3 从线性质点模型迁移到非线性模型的注意事项跑通质点模型之后下一个自然需求就是换成更真实的车模比如运动学自行车模型。这个迁移会让系统从线性变成非线性但好消息是CasADi 几乎就是为了这个场景设计的。第一步是把离散化从解析矩阵改成积分器函数。以 RK4 为例% 连续动力学 x_sym MX.sym(x_sym, 4); u_sym MX.sym(u_sym, 2); v x_sym(3); theta x_sym(4); xdot [v * cos(theta); v * sin(theta); u_sym(1); u_sym(2)]; % 用 Function 封装 RK4 离散 f_cont casadi.Function(f_cont, {x_sym, u_sym}, {xdot}); % 然后在循环里调用 runge-kutta 或者直接用 casadi.integrator第二步是给优化变量设置初始值。线性 MPC 里变量初始为 0 问题不大非线性 MPC 对初始猜测很敏感。我一般这样给从当前状态出发按照一条直线插值到参考轨迹的终点作为状态序列的初值控制序列初值给 0 或者给参考速度对应的名义加速度。这个细节让 IPOPT 的收敛速度和成功率都有明显提升。第三步是重新审视约束的物理意义。质点模型里 umax5 无论在 x 还是 y 方向都很自然自行车模型里 a 是纵向加速度delta 是前轮转角它们的上下界差异很大而且往往还伴随质心侧偏角或者横摆角速度的约束。约束越复杂越能体会 CasADi 的价值——增加一条约束只是多写一行 subject_to而不是重推整张约束矩阵。我在实际项目里用这套流程从质点模型迁到自行车模型前后只花了半天时间而手写 QP 时期同样的迁移至少要一周。对想做真实车辆控制的人来说这个迁移路径是绕不开的建议第一次迁移就顺便把控制增量惩罚加进去后面调参能省不少力气。如果你也是刚接触 MPC我建议先别急着追求各种花哨的代价项。把质点模型、圆形轨迹、IPOPT 这套链路吃透然后逐步加非线性、加约束、加复杂轨迹。整个调试过程中最有用的习惯就是每次遇到异常先把参考轨迹变成最慢最平滑的直线确认控制器本身正常再去怀疑模型和参数。这个习惯帮我避开了大量本不必要的折腾。