企业官网建设流程全解析

说起模型预测控制（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 = Σ_{k=1}^{N} ( e_k^T Q e_k + u_k^T R u_k ) + 终端误差项

其中 e_k = x_k - x_ref,k，u_k = [ax_k; ay_k]。Q 和 R 是权重矩阵，它们相对大小的调节决定了"跟踪精度优先"还是"控制平顺优先"。

有了代价函数，加上动力学约束、控制量约束和初始条件约束，就构成了一个有限时域开环最优控制问题。MPC的妙处在于：它只在每个采样时刻执行第一个控制量，然后丢弃剩余预测，下一时刻用新测量状态重新求解。这种滚动优化机制让控制器具备反馈修正能力，即便模型存在偏差也不至于完全开环跑飞。

1.3 一个小直觉测试

我习惯在动手写代码前先做个脑内推演：如果预测时域 N=1，MPC退化成什么？退化成基于当前误差的瞬时优化，相当于一个带约束的比例控制。如果 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, N+1); % 决策变量：状态轨迹 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_{k+1} = x_k + dt * f(x_k, u_k)

欧拉法编程简单，但精度只有一阶。在采样周期 dt=0.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, N+1); U = opti.variable(nu, N); % 参数 X_ref = opti.parameter(nx, N+1); 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(:,N+1) - X_ref(:,N+1); J = J + eN' * QN * eN; opti.minimize(J); % 动力学约束 for k = 1:N opti.subject_to(X(:,k+1) == 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_level=0是让 Ipopt 不刷屏输出迭代信息。调试时可以关掉这个选项，方便观察收敛情况。

3.3 滚动时域仿真循环

优化问题只需要搭建一次，剩下的就是在每个采样周期更新参数、求解、取第一组控制量、推进真实系统。这里在Matlab里用一个简单的圆轨迹做参考轨迹：

% 仿真参数 T_sim = 100; % 仿真步数 X_log = zeros(nx, T_sim+1); % 状态记录 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, N+1); for k = 1:N+1 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(:,t+1) = full(F_disc(X_log(:,t), u_opt)); end

这里有三个容易踩的坑：

1. sol.value(U(:,1))返回的是 CasADi 的 DM 对象，要作为数值数组使用前最好用full()转成 Matlab 双精度数组；
2. F_disc的输入输出都是符号函数，在仿真推进时传数值进去返回的仍然是 DM 对象，同样需要full()转出来；
3. 每次循环opti.solve()会打印一行求解时间，如果你关心实时性，可以用sol.stats()拿到详细统计信息，或者开print_time=false关掉输出。

跑完这段代码，把 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');

如果 dt=0.1，x_test=[0;0;1;0]，u_test=[1;0]，那么一步之后 x 大约变为 0.005，vx 约 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参数没调好的抖动。

原因要从预测控制机制上去理解：N=5 意味着控制器只能看到未来 0.5s（dt=0.1）的轨迹信息。当车速是 2m/s 时，0.5s 内车辆只能前进 1 米，而圆轨迹的曲率半径是 5 米。对于前方 1 米的视野，弯道看起来接近直线，控制器根本没有意识到自己在转弯，自然就会滞后。

把 N 逐步增大到 20（视野 2 秒）、30（视野 3 秒）时，跟踪误差明显下降。N 太大也有问题：优化问题的变量数量正比于 N，求解时间显著上升；同时过长的预测视野会携带很多"遥远的、不准确的参考信息"，在模型失配时反而降低性能。我做下来，dt=0.1s 时 N 取 20~30 是个比较均衡的范围。你可以写一个小脚本扫参，画出误差随 N 的变化曲线，很快能找到针对你的参考轨迹的甜点值。

4.2 权重矩阵 Q 和 R：一个从零开始的调法

代价函数里的权重矩阵决定了"控制器多激进"。我通常的调参顺序是：

1. 先把位置权重 Q(1,1) 和 Q(2,2) 设大，比如 10，速度权重设小，比如 1；
2. R 矩阵从 0.01 开始调，每次翻倍，观察控制量曲线；
3. 如果轨迹跟踪误差大，则增大 Q 或减小 R；如果控制量震荡、声浪大，则减小 Q 或增大 R；
4. 最后微调终端权重。

这里有个扎心的事实：没有任何公式可以直接算出最优 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:N+1 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 的恐惧其实是来自"数学符号"而非"算法本身"。真上手写一遍，把一条圆形轨迹跟踪好，再回头看那些教材公式，你会发现它们只是把你已经在代码里表达的事情换了一种说法而已。如果这篇文章让你少花一天时间在配置环境上，那我就没白写。