基于CasADi的MPC轨迹跟踪：从质点建模到滚动优化实现

做轨迹跟踪的工程实现，我一直有个习惯：控制律在草稿纸上推完，先不急着写代码，而是问自己一句“这个优化问题今天约束变不变”。像 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 秒，轨迹误差也只比 dt=0.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 跟踪不好你都不知道是控制器问题还是路径本身不可达。我通常给 R=2m、omega=0.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, N+1); % 状态序列 U = opti.variable(2, N); % 控制序列 opti.minimize(J); opti.subject_to(X(:,1) == x0); opti.subject_to(X(:,k+1) == A*X(:,k) + B*U(:,k)); opti.solver('ipopt'); sol = opti.solve();

这种写法的好处在于：你只需要描述“问题本身”，剩下的自动微分、雅可比计算、NLP 求解全部由框架代劳。刚开始用的时候会有种不真实的轻松感——以前要手推一个上午的矩阵维度，现在几行约束就搞定了，而且改约束条件也只是增删一行 subject_to 而已。

2.3 CasADi 与 Matlab MPC 工具箱的取舍

很多用 Matlab 的人会问：不是有 MPC Toolbox 吗？实话实说，官方工具箱在产线部署和线性 MPC 快速验证上很成熟，但它更像一个封装好的黑盒。当你需要自定义非线性模型、非标准代价项（比如避障势场）或者苛刻的求解器调试时，工具箱的限制就暴露出来了。我整理了一个简单对比：

我个人的选择标准很直接：如果只是标准线性 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_{k=1}^{N+1} (x_k - x_{ref,k})^T Q (x_k - x_{ref,k}) + \sum_{k=0}^{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。第一版的参数组合我推荐 dt=0.1s，N=20，这样预测总时长为 2 秒。对于质点模型这种快速动态系统，2 秒的视野足够收敛；对于真实车辆平台，预测时域通常也落在这个区间。

dt 的设定要看执行器更新率。如果底层执行器只能 10Hz 更新，那你把 dt=0.01s 纯属自己为难自己——优化结果再精确也送不过去。反过来，dt 太大又会让离散模型丢掉连续系统的动态细节。N 的设定主要受求解时间限制：N=20 时 IPOPT 一次求解通常只有几毫秒到十几毫秒，远小于采样周期 0.1 秒，完全可以实时跑。如果你的电脑性能较弱，可以先降到 N=15，预测时域 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, N+1); % 状态序列 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, N+1); for k = 1:N+1 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(:, k+1) == A * X(:, k) + B * U(:, k)); % 动态方程 opti.subject_to(-umax <= U(:, k) <= umax); % 输入限幅 end % 代价函数 J = 0; for k = 1:N+1 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, steps+1); pos_history(:, 1) = xnow(1:2); for i = 1:steps t_now = (i-1) * dt; % 按真实全局时间更新参考轨迹 for k = 1:N+1 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(:, i+1) = xnow(1:2); end

这里必须强调一个我踩过的坑：参考轨迹一定要用全局时间 t_now 累加，而不是每个周期从 0 开始重新生成。如果你照抄前文建模代码里那个(k-1)*dt去填参考轨迹，那么每一拍参考路径都会从头开始，MPC 永远在追一个同一起点出发的“影子轨迹”，仿真画出来是一条原地打转的线。这个问题非常隐蔽，因为你单独看每个时刻的参考轨迹都觉得是对的。

跑完之后绘图很简单：

ref_history = zeros(2, steps+1); for i = 1:steps+1 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(:, k+1) - 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 的收敛速度和成功率都有明显提升。

第三步是重新审视约束的物理意义。质点模型里 umax=5 无论在 x 还是 y 方向都很自然；自行车模型里 a 是纵向加速度，delta 是前轮转角，它们的上下界差异很大，而且往往还伴随质心侧偏角或者横摆角速度的约束。约束越复杂，越能体会 CasADi 的价值——增加一条约束只是多写一行 subject_to，而不是重推整张约束矩阵。

我在实际项目里用这套流程从质点模型迁到自行车模型，前后只花了半天时间，而手写 QP 时期同样的迁移至少要一周。对想做真实车辆控制的人来说，这个迁移路径是绕不开的，建议第一次迁移就顺便把控制增量惩罚加进去，后面调参能省不少力气。

如果你也是刚接触 MPC，我建议先别急着追求各种花哨的代价项。把质点模型、圆形轨迹、IPOPT 这套链路吃透，然后逐步加非线性、加约束、加复杂轨迹。整个调试过程中，最有用的习惯就是：每次遇到异常，先把参考轨迹变成最慢最平滑的直线，确认控制器本身正常，再去怀疑模型和参数。这个习惯帮我避开了大量本不必要的折腾。