说起模型预测控制MPC很多刚接触的人第一反应是高大上然后去翻教材看到一大堆 QP、KKT、滚动优化术语直接劝退。我去年在Matlab里用CasADi框架重写了一套质点车辆模型的轨迹跟踪仿真做完之后最大的感受是MPC的代码骨架其实非常固定难的不是算法本身而是把物理模型-优化问题-数值求解三块衔接好。今天把完整思路和代码实现拆开讲一遍希望能帮你少走点弯路。这套内容适合两类人一是已经学过控制理论、但不知道怎么把MPC落到代码里的学生二是做自动驾驶、机器人路径跟踪的工程师想快速验证自己的想法又不想从零手写数值优化库。质点车辆模型虽然简单但它是理解MPC工作机理的最佳载体——你不需要对付复杂的轮胎侧偏角、横摆角速度就能看到预测控制看向未来、滚动决策的整个闭环过程。1. 质点车辆模型与轨迹跟踪问题——先从物理直觉聊起1.1 为什么用质点模型来入门MPC质点模型就是把车辆抽象成一个有质量、有位置、有速度的点不考虑车身的姿态和转向几何。最常见的状态取四维横向位置 x、纵向位置 y、横向速度 vx、纵向速度 vy。控制输入则是横向加速度 ax 和纵向加速度 ay。这里有个容易被新手忽略的点质点模型的控制量是加速度而不是方向盘转角或油门踏板位置也就是说我们默认车辆可以在任意方向产生加速度。这当然是一种理想化但对MPC入门来说恰恰是优点——它让你先把注意力集中在预测-优化-执行的循环上而不是被车辆动力学方程淹没。连续时间下质点模型的动力学方程为x_dot vx y_dot vy vx_dot ax vy_dot ay写成状态空间形式就是标准的二阶积分器模型。这个模型虽然简单却保留了MPC的核心矛盾控制量 ax、ay 是有限的而轨迹误差必须通过有限的加速度来消除这就形成了约束下的最优控制问题。1.2 轨迹跟踪问题的数学表述轨迹跟踪的输入是一条参考轨迹通常用序列表示X_ref { [x_ref(0), y_ref(0), vx_ref(0), vy_ref(0)], [x_ref(1), y_ref(1), vx_ref(1), vy_ref(1)], ... [x_ref(N), y_ref(N), vx_ref(N), vy_ref(N)] }N 是预测时域长度。控制目标是在每个采样时刻 k基于当前状态 x(k)求解未来 N 步的最优控制序列使预测状态轨迹尽量贴合参考轨迹同时惩罚控制量的大小。典型代价函数J Σ_{k1}^{N} ( e_k^T Q e_k u_k^T R u_k ) 终端误差项其中 e_k x_k - x_ref,ku_k [ax_k; ay_k]。Q 和 R 是权重矩阵它们相对大小的调节决定了跟踪精度优先还是控制平顺优先。有了代价函数加上动力学约束、控制量约束和初始条件约束就构成了一个有限时域开环最优控制问题。MPC的妙处在于它只在每个采样时刻执行第一个控制量然后丢弃剩余预测下一时刻用新测量状态重新求解。这种滚动优化机制让控制器具备反馈修正能力即便模型存在偏差也不至于完全开环跑飞。1.3 一个小直觉测试我习惯在动手写代码前先做个脑内推演如果预测时域 N1MPC退化成什么退化成基于当前误差的瞬时优化相当于一个带约束的比例控制。如果 N 足够大MPC 能预见到前方轨迹的弯道提前减速、提前转向。这就是预测控制比传统反馈控制强的地方——它不是等误差出现才反应而是把未来的误差一起考虑进去。这个直觉在做轨迹跟踪时非常重要。比如参考轨迹是一条 S 形弯道预测时域短的控制器会在弯道处出现明显的超调因为等你发现横向误差时已经来不及修正了而预测时域足够长的控制器会在进入弯道前就开始输出横向加速度轨迹跟随曲线明显更顺滑。2. CasADi在MPC里的角色符号优化与Opti栈的取舍2.1 CasADi到底解决什么问题CasADi 是一个开源工具库核心能力是符号运算、自动微分和非线性优化。在Matlab里你可以把它理解成一个帮你把优化问题算明白的黑盒你把代价函数和约束写成代码它自动求导、自动组装成优化问题并调用底层求解器比如 Ipopt、qpOASES求解。手动实现MPC最痛苦的地方在于梯度推导。如果你的模型是非线性的——比如后面扩展到自行车模型时会出现三角函数项、轮胎力非线性项——手推雅可比矩阵极其容易出错。CasADi 的自动微分会把这一步全部省掉你只写代价是什么、约束是什么求解器需要的梯度、黑塞矩阵都由框架自动生成。在我们这个质点模型例子里动力学是线性的用 CasADi 看起来有点杀鸡用牛刀但价值在于当 N 变大、约束变复杂、模型变非线性时代码结构几乎不用改。你写的符号表达式只是从二阶积分器换成自行车模型后面的求解流程原封不动。2.2 Opti栈面向用户的优化建模接口CasADi 在 Matlab 里有多种接口方式最推荐新手用的是 Opti 栈。它的设计非常贴近自然语言创建优化变量、设定目标、添加约束、求解每一行代码对应一个数学概念。一个典型的 Opti 结构长这样import casadi.* opti Opti(); % 创建优化栈 X opti.variable(4, N1); % 决策变量状态轨迹 U opti.variable(2, N); % 决策变量控制序列 opti.minimize(J); % 目标函数 opti.subject_to(...); % 约束 opti.solver(ipopt); % 指定求解器 sol opti.solve(); % 求解并取结果Opti 还支持参数化问题。预测时域内每个时刻的参考轨迹点、当前状态初值都可以用opti.parameter声明为参数。在循环中只需要opti.set_value更新参数再重新求解不需要重建整个优化问题。这是把MPC跑成实时闭环的关键技巧。2.3 与Matlab自带MPC工具箱的对比以及适用边界MathWorks 官方也有 Model Predictive Control Toolbox内置了线性MPC、自适应MPC和部分非线性MPC的功能UI 和 Simulink 集成都做得很完善。那为什么还要用 CasADi我的体会是官方工具箱对非线性模型的表达自由度有限且调起来有种被框架牵着走的感觉CasADi 在学术论文复现和算法迭代上更灵活自定义代价函数、自定义约束比如避障距离约束很方便最关键的是符号建模方式可以直接移植到 Python 或 C换平台成本低。但 CasADi 也不是没有代价你需要自己处理模型离散化、参考轨迹规划、仿真循环没有 SIMULINK 那样的图形化界面。对纯理论验证来说这种多一点控制权反而是优点。Matlab 版本上建议用 R2021b 及以上兼容性更好CasADi 的 Matlab 接口装起来比较省心直接下载对应版本把路径加进去就行。3. 完整实现从动力学离散化到滚动时域循环3.1 离散化欧拉法还是龙格库塔法MPC 求解的是一个离散时间问题所以第一步要把连续动力学离散化。很多教程直接给欧拉法x_{k1} x_k dt * f(x_k, u_k)欧拉法编程简单但精度只有一阶。在采样周期 dt0.1s 的场景下误差还能接受如果你把 dt 加到 0.5s欧拉法的离散误差会让MPC预测的状态轨迹明显偏离真实系统控制效果大打折扣。我推荐直接用四阶龙格库塔RK4做离散化代价只是多写几行代码。CasADi 的 Symbolic 类型天然支持这种嵌套计算dt 0.1; % 连续动力学函数 x SX.sym(x); y SX.sym(y); vx SX.sym(vx); vy SX.sym(vy); states [x; y; vx; vy]; ax SX.sym(ax); ay SX.sym(ay); controls [ax; ay]; rhs [vx; vy; ax; ay]; f_cont Function(f_cont, {states, controls}, {rhs}); % RK4离散化 k1 f_cont(states, controls); k2 f_cont(states 0.5*dt*k1, controls); k3 f_cont(states 0.5*dt*k2, controls); k4 f_cont(states dt*k3, controls); states_next states (dt/6) * (k1 2*k2 2*k3 k4); F_disc Function(F_disc, {states, controls}, {states_next});这段代码的输入输出都是 CasADi 的 SX 符号对象F_disc 就是我们放进MPC约束里的一步预测模型。后面如果要换成更复杂的车辆动力学模型只需要改rhs的定义和状态、控制的维度其余代码不用动。3.2 搭建优化问题代价函数、约束与求解器设置下面这段是MPC的核心。我用 Opti 栈将状态轨迹、控制序列、参考轨迹参数声明出来然后逐项添加约束。N 20; % 预测时域 nx 4; % 状态维度 nu 2; % 控制维度 opti Opti(); % 决策变量 X opti.variable(nx, N1); U opti.variable(nu, N); % 参数 X_ref opti.parameter(nx, N1); x0 opti.parameter(nx, 1); % 代价函数 Q diag([10, 10, 1, 1]); % 位置误差权重更大 R 0.1 * eye(nu); % 控制量惩罚 QN 20 * Q; % 终端权重可以适当加大 J 0; for k 1:N e X(:,k) - X_ref(:,k); J J e * Q * e U(:,k) * R * U(:,k); end eN X(:,N1) - X_ref(:,N1); J J eN * QN * eN; opti.minimize(J); % 动力学约束 for k 1:N opti.subject_to(X(:,k1) F_disc(X(:,k), U(:,k))); end % 控制量约束加速度上下限 opti.subject_to(-2 U(1,:) 2); % ax opti.subject_to(-2 U(2,:) 2); % ay % 初始状态约束 opti.subject_to(X(:,1) x0); % 求解器 opti.solver(ipopt, struct(print_time, false), struct(print_level, 0));几个设计要点终端权重 QN 要不要加大我的经验是加。因为预测时域有限最后一步的状态没有后续约束如果终端误差惩罚不够大控制器容易出现最后一刻还不想收手的现象导致轨迹末端偏差。把 QN 调成 Q 的 1.5~2 倍通常会明显改善闭环跟踪效果。控制量约束为什么写成 -2 到 2这是对最大加速度的标定你可以根据车辆特性改。约束的数值直接影响系统能跟踪多急的弯道约束太小容易跟不上约束太大则会让控制器动作粗糙。print_level0是让 Ipopt 不刷屏输出迭代信息。调试时可以关掉这个选项方便观察收敛情况。3.3 滚动时域仿真循环优化问题只需要搭建一次剩下的就是在每个采样周期更新参数、求解、取第一组控制量、推进真实系统。这里在Matlab里用一个简单的圆轨迹做参考轨迹% 仿真参数 T_sim 100; % 仿真步数 X_log zeros(nx, T_sim1); % 状态记录 X_log(:,1) [0; 0; 2; 0]; % 初始位置(0,0)速度2m/s沿x方向 % 参考轨迹生成句柄圆形路径 radius 5; ref_curve (t) [radius*sin(0.2*t); radius - radius*cos(0.2*t); 0.2*radius*cos(0.2*t); 0.2*radius*sin(0.2*t)]; for t 1:T_sim % 生成当前时刻往后N步的参考轨迹 ref_seq zeros(nx, N1); for k 1:N1 ref_seq(:,k) ref_curve(t k - 1); end % 更新参数 opti.set_value(X_ref, ref_seq); opti.set_value(x0, X_log(:,t)); % 求解 sol opti.solve(); % 取第一步控制并推进真实系统 u_opt sol.value(U(:,1)); X_log(:,t1) full(F_disc(X_log(:,t), u_opt)); end这里有三个容易踩的坑sol.value(U(:,1))返回的是 CasADi 的 DM 对象要作为数值数组使用前最好用full()转成 Matlab 双精度数组F_disc的输入输出都是符号函数在仿真推进时传数值进去返回的仍然是 DM 对象同样需要full()转出来每次循环opti.solve()会打印一行求解时间如果你关心实时性可以用sol.stats()拿到详细统计信息或者开print_timefalse关掉输出。跑完这段代码把 X_log 的前两行x 和 y画出来叠加参考圆轨迹你就能看到MPC跟踪的效果。如果道路曲率变化剧烈你会发现控制量在弯道处提前作用轨迹偏差明显小于延迟反馈的方法。3.4 用符号求值验证动力学约束是否正确新手很容易在动力学约束上翻车特别是 RK4 离散化式子写错位置。我建议在搭建优化问题之前先单独用几个数值测试一下离散化函数x_test [0; 0; 1; 0]; u_test [1; 0]; x_next full(F_disc(x_test, u_test)); % 期望结果约等于 [0.1; 0; 1.1; 0] disp(x_next);如果 dt0.1x_test[0;0;1;0]u_test[1;0]那么一步之后 x 大约变为 0.005vx 约 1.1而不是精确的 0.1 和 1.1。为什么因为阶跃加速度输入下 RK4 对线性系统的积分结果等同于精确积分x 方向位移应该是 vxdt 0.5ax*dt^2 0.1 0.005 0.105。先用这个手算结果对照一遍能避免后续排查问题时分不清是控制器问题还是离散化bug。4. 调参实验复盘预测时域、权重矩阵与求解器配置4.1 预测时域 N调太小的后果比你想的更严重N 是MPC最重要的参数之一。我刚开始做这个项目时图省事把 N 设成 5心想反正每一时刻都重新求解预测短一点也没关系吧。实际跑下来系统在圆形轨迹上出现了持续的稳态误差而且控制量一直在小幅震荡看上去就像PID参数没调好的抖动。原因要从预测控制机制上去理解N5 意味着控制器只能看到未来 0.5sdt0.1的轨迹信息。当车速是 2m/s 时0.5s 内车辆只能前进 1 米而圆轨迹的曲率半径是 5 米。对于前方 1 米的视野弯道看起来接近直线控制器根本没有意识到自己在转弯自然就会滞后。把 N 逐步增大到 20视野 2 秒、30视野 3 秒时跟踪误差明显下降。N 太大也有问题优化问题的变量数量正比于 N求解时间显著上升同时过长的预测视野会携带很多遥远的、不准确的参考信息在模型失配时反而降低性能。我做下来dt0.1s 时 N 取 20~30 是个比较均衡的范围。你可以写一个小脚本扫参画出误差随 N 的变化曲线很快能找到针对你的参考轨迹的甜点值。4.2 权重矩阵 Q 和 R一个从零开始的调法代价函数里的权重矩阵决定了控制器多激进。我通常的调参顺序是先把位置权重 Q(1,1) 和 Q(2,2) 设大比如 10速度权重设小比如 1R 矩阵从 0.01 开始调每次翻倍观察控制量曲线如果轨迹跟踪误差大则增大 Q 或减小 R如果控制量震荡、声浪大则减小 Q 或增大 R最后微调终端权重。这里有个扎心的事实没有任何公式可以直接算出最优 Q、R它本质上是对跟踪精度和执行器寿命的权衡。加速度约束本身已经限制了执行器最大负担但如果 R 太小控制器会把加速度在上下限之间来回打形成抖振R 太大则会出现在弯道内懒洋洋的现象误差收敛慢。4.3 求解器配置Ipopt的关键选项CasADi 默认搭配 Ipopt这个求解器对中小规模非线性规划问题稳定且够快。在控制循环中除了 print_level 之外还有几个选项值得注意ipopt_opts struct(tol, 1e-4, max_iter, 1000, acceptable_tol, 1e-6); opti.solver(ipopt, struct(print_time, false), ipopt_opts);tol是求解器的收敛容差默认 1e-8 对实时控制来说过于严格放宽松到 1e-4 能让求解时间大幅下降控制性能几乎不受影响max_iter设太小会导致求解失败特别是第一次求解时初始猜测差迭代次数需求更大。默认 3000 通常够用如果你的优化问题是二次规划线性模型线性约束二次代价可以换成qrqp或osqp这类更快的 QP 求解器速度能再上一个台阶。但注意它们只支持凸二次规划模型一旦非线性就必须回到 Ipopt。4.4 实时性的实测感受用 Matlab 跑这个模型在配置一般的笔记本上单步求解大约 30~80ms取决于 N 和初始猜测质量对于采样周期 100ms 的控制任务勉强够用。如果你的采样周期更短有两条路一是把 N 和求解容差调小二是把代码转到 Coder 工具箱生成 C 代码。CasADi 支持代码生成导出后的求解速度通常能提高 5~10 倍。我这套验证代码没有做这步优化但它的架构从第一天起就兼容代码生成不用中途推翻重写。5. 从质点模型走向更高阶的拓展路线与个人体会5.1 升级到自行车模型改动路径很短质点模型验证通过后最自然的升级是换成运动学自行车模型。状态变成 [x; y; yaw; v]控制变成 [a; delta]连续动力学变为x_dot v * cos(yaw) y_dot v * sin(yaw) yaw_dot v / L * tan(delta) v_dot a其中 L 是轴距。在 CasADi 里你只需要修改rhs的定义和状态、控制的维度声明MPC 求解框架完全不变。我把这个升级过程实测过改动量不到半小时——这正是符号建模带来的最大红利模型和算法彻底解耦。当然自行车模型下代价函数的权重需要重新调因为 yaw 误差和位置误差的量纲不同。还有一点要注意在低速场景下运动学模型够用高速场景下必须引入动力学模型考虑轮胎侧偏角、质心侧偏角否则控制器会在极限工况下给出激进但不可执行的控制指令。5.2 加入避障约束MPC真正的杀手锏质点模型非常适合演示预测避障能力。你可以在优化问题里加一条非线性约束车辆位置与障碍物中心的距离必须大于安全半径。比如圆形障碍物obs_x 3; obs_y 2; % 障碍物位置 safe_r 0.8; % 安全半径 for k 1:N1 dist_sq (X(1,k) - obs_x)^2 (X(2,k) - obs_y)^2; opti.subject_to(dist_sq safe_r^2); end光加这一条约束MPC 就会在预测到未来将驶入障碍物范围时提前规划绕行轨迹。这比传统的势场法、人工场法平滑得多因为优化是在整个预测时域上全局协调的。强烈建议跑一下这个例子它能让你直观感受预测控制的魅力——控制器是在躲避未来可能发生的碰撞而不是等碰撞边缘才紧急转向。5.3 我做完这个项目后的几点体会第一不要急着追求复杂的车辆模型。先用质点模型把 MPC 的代码骨架、参数调节手感、求解器配置熟悉一遍后面升级模型时才有底气。第二仿真和实物之间的鸿沟体现在模型失配和时间延迟上质点模型里我们假设控制量即时生效真实系统中执行器有响应延迟这会让控制器振荡实际部署时需要在预测模型里加一拍延迟补偿。第三也是最实际的一条把参考轨迹的生成和 MPC 求解分开写。我一开始在循环里生成参考轨迹代码又乱又慢。后来封装成独立的 reference_trajectory 函数测试五条不同的轨迹只需要调用不同函数整个项目清爽太多。这个例子跑通之后我觉得很多人对 MPC 的恐惧其实是来自数学符号而非算法本身。真上手写一遍把一条圆形轨迹跟踪好再回头看那些教材公式你会发现它们只是把你已经在代码里表达的事情换了一种说法而已。如果这篇文章让你少花一天时间在配置环境上那我就没白写。
