企业官网建设流程全解析

前阵子我在调试一套四旋翼轨迹跟踪仿真系统时，把原来的串级PID换成了MPC（模型预测控制）。同样一条3 m/s的圆形轨迹，PID的横向误差开始超过40厘米，MPC只调了三组权重参数，就把最大误差压到了8厘米以内。这个反差让我特别想写一篇四旋翼飞行器UAV_MPC模型预测轨迹跟踪控制研究的Matlab实现笔记，把建模、控制器设计、代码骨架和调参坑点全串起来，给正在做无人机控制、毕设或者工程预研的朋友一条比较顺的路线。

MPC这几年在无人机领域确实火，但网上的开源工程大多只给一个求解器包装，很少有人把"动力学模型怎么拆""目标函数每一项怎么设""预测时域为什么取20不取100"这些关键决策讲透。这篇文章就是我自己的实操记录，包含可以直接抄走的Matlab代码结构，也包含那些在论文里不太会写、但实际调代码一定会遇到的细节。

1. 从PID换到MPC之后，仿真里到底发生了什么

先说一个很多人会问的问题：四旋翼轨迹跟踪明明PID也能做，为什么总有人说MPC是更优解？我自己的体会是，在低速、小机动、系统没进入约束边界的情况下，串级PID完全够用，甚至手感更好调。但只要轨迹速度提上来、跟踪精度要求变成厘米级、还要考虑推力饱和和姿态角限制，PID就会暴露两个硬伤：一是没有预测能力，控制器只对当前误差反应，误差已经变大了才开始去追；二是约束处理全靠限幅和抗积分饱和，无法在"即将进入约束"之前主动调整。

1.1 PID、LQR这些传统控制器输在哪

下面这张表是我在仿真里反复对比后整理的，不涉及硬件差异，纯看控制架构本身的特点。

PID本质上是一个"事后修正器"，它永远在根据当前的偏差生成纠正量。LQR虽然可以理解为无限时域的最优控制，但它针对的是线性化系统，当你希望它遵守"油门不能超过某个值"或者"横滚角不能超过30度"这类硬约束时，LQR很难直接办到。MPC的优势在于它把未来一段时间内的系统行为显式写进了优化问题，约束不再是外挂的限幅，而是优化问题的边界条件。

1.2 一个驾驶比喻弄懂MPC的滚动优化

我经常用开车来比喻MPC的工作方式：你坐在车里，计划要在接下来10公里内从A开到B，路上有急弯、有坡道、有速度限制。普通控制器就像只看车头前三米的驾驶员，看到弯道才开始打方向。MPC则像是拿着导航地图，每隔几秒把所有可预见的路径信息快速扫一遍，算出来接下来几秒钟最优的方向和油门，然后只执行这第一步，等下一次采样再重新规划。

这个过程在控制术语里叫滚动时域优化。每个采样周期都重新求解一个带约束的优化问题，求解结果只应用第一个控制量，然后利用最新的状态反馈把预测起点拉回到真实状态上。这就是"反馈校正"的来源，也是MPC不怕模型误差的一个关键原因。

1.3 哪些轨迹跟踪场景更适合MPC

我建议不要盲目上MPC，先问自己三个问题：

1. 轨迹是否提前已知，或者至少可以提前好几个采样周期预知？
2. 系统是否会经常执行速度较快、曲率较大的机动？
3. 是否存在必须严格遵守的执行器或状态约束？

如果三个答案都是肯定的，MPC带来的提升会非常大。比如无人机在树林里沿规划航线飞行、在移动平台上降落、或者追踪一个速度变化剧烈的高机动目标，这类场景里预测能力就是核心竞争力。如果你只是做定点悬停或者非常缓慢的巡航，那PID可能更省事，这也是工程上很常见的混合方案：内环用姿态PID，外环用MPC。

2. 动力学模型怎么拆，MPC的预测才站得住

MPC的"模型"是它预测未来的依据，模型拆得不对，后面的优化做得再漂亮都是空中楼阁。四旋翼的全状态模型包含位置、速度、姿态角、角速度等12个甚至更多状态，直接拿去做非线性MPC不是不行，但对大多数Matlab初学者来说，求解难度和实现复杂度会一下子高很多。

2.1 保留哪些状态，忽略哪些状态

我做位置环MPC时，把模型拆成了两层：内环姿态控制负责快速响应姿态角指令，外环MPC只负责轨迹跟踪，直接输出期望加速度。这样对外环MPC来说，控制量就是三轴加速度，被控对象近似成一个点质量模型。

状态向量取为

[ x = [p_x, p_y, p_z, v_x, v_y, v_z]^T \in \mathbb{R}^6 ]

控制向量取为

[ u = [a_x, a_y, a_z]^T \in \mathbb{R}^3 ]

这背后的假设是：内环姿态控制足够快，外环给一个加速度指令，飞行器可以近似在几十毫秒内把它跟踪上。在仿真和大多数实际飞控里，这个假设是可以站住脚的。忽略的细节包括电机延迟、气动阻力、机体倾斜导致的推力分量变化，这些要么靠串级内环去消除，要么在后续模型里补一个一阶惯性。

2.2 位置环MPC和全状态MPC怎么选

如果你想研究的是纯算法、做毕设的理论验证，位置环MPC是最好的切入方式，代码量小、调试快、效果直观。但如果你面对的是真实飞行器，并且想处理姿态角约束、角速度限制甚至电机饱和，那就需要考虑全状态MPC或者至少把姿态角加入约束模型。

我自己的经验是：先做位置环MPC把轨迹跟踪的框架跑通，再逐步把姿态动力学加进去。不要在第一天就把模型写成一个12维非线性系统，否则你很难分清代码出错还是模型出错。全状态MPC在Matlab里可以用CasADi这类工具实现，但如果你是第一次接触，我建议先用线性MPC配合内置quadprog来掌握主流程。

2.3 离散化：零阶保持与A、B矩阵

连续系统方程很简洁：

[ \dot{p} = v, \quad \dot{v} = a ]

控制器的采样周期为 (T_s)，假设在一个周期内控制量保持不变（零阶保持），离散化后的状态方程是：

[ x_{k+1} = \begin{bmatrix} I_3 & T_s I_3 \ 0_3 & I_3 \end{bmatrix} x_k + \begin{bmatrix} \frac{1}{2}T_s^2 I_3 \ T_s I_3 \end{bmatrix} u_k ]

写成代码就是：

nx = 6; nu = 3; dt = 0.1; A = [eye(3), dt*eye(3); zeros(3), eye(3)]; B = [0.5*dt^2*eye(3); dt*eye(3)];

注意这里的单位是国际单位制：位置是米，速度是米每秒，加速度是米每二次方秒。如果你的轨迹坐标用厘米，权重矩阵和约束边界就要全部换算，否则优化问题里会出现数值失配。

3. 目标函数和约束的写法，决定了控制器最终的性格

MPC把轨迹跟踪转化成了一个带约束的优化问题。优化问题写得对不对，直接决定了控制器是"激进型"还是"温和型"，也决定了它会不会抖、会不会在约束边界上卡死。

3.1 每一项权重的意义与设置经验

我用的目标函数是：

[ J = \sum_{k=1}^{N_p} \left| y_{ref}(k) - x(k) \right|_Q^2

• \sum_{k=0}^{N_c-1} \left| \Delta u(k) \right|{R{\Delta}}^2
• \rho \epsilon^2 ]

第一项是预测时域内的跟踪误差，Q矩阵决定了对位置和速度误差的重视程度。第二项是控制增量 (\Delta u = u_k - u_{k-1}) 的惩罚，这一项非常关键，因为如果不加，控制器会为了把误差压到零而频繁输出剧烈变化的控制指令，体现在仿真里就是加速度指令锯齿状抖动。

我的初始权重设置经验是：

• 位置误差权重在 5~20 左右；
• 速度误差权重在 0.1~1 左右；
• 控制增量权重在 0.1~1 左右；
• 松弛项权重取很大，比如 1000 或 10000。

注意这些数值不是绝对的，但它们之间的比例关系比绝对大小重要得多。如果位置权重远远大于其他项，控制器就会非常激进，很容易撞上约束边界；如果增量权重太大，控制器会过于保守，跟踪误差迟迟消不掉。

3.2 约束怎么写才不会让QP求解崩塌

约束主要分两类。一类是控制量幅值约束，比如加速度不能超过最大可用加速度：

[

• a_{max} \le u_k \le a_{max} ]

另一类是控制增量约束：

[

• \Delta u_{max} \le u_k - u_{k-1} \le \Delta u_{max} ]

这两种约束写进二次规划后都会得到不等式约束矩阵。如果一个点上的控制器同时满足不了所有约束，优化问题就会变得不可行，quadprog会直接报错或者返回一个奇怪的结果。解决方式就是加松弛变量。

一个非常实用的做法是：把位置约束或速度约束写成软约束，允许误差在极端情况下短暂超过边界，但付出很大代价。代价函数的第四项就是干这个的。松弛变量不应该理解成"作弊"，而是让优化问题在极端工况下仍然有解，控制器至少能给出一个尽量安全的输出。

3.3 Np和Nc怎么选：一个可复用的选参逻辑

预测时域 (N_p) 和控制时域 (N_c) 是最容易让新手犯迷糊的参数。太小了预测能力不够，太大了计算负担重而且远期预测本来就不可靠。

我得到一个简便选择方法：先看系统闭环期望的响应时间大概是多少秒，然后用采样周期去衡量。比如想让误差在大约2秒内收敛，采样周期是0.1秒，那预测时域取20左右就比较合理。控制时域 (N_c) 没必要和 (N_p) 一样大，通常取 (N_p) 的四分之一到二分之一，比如 (N_p=20, N_c=5)，这样变量数量少，求解更快。

我整理了一组仿真里常见的组合，供参考：

预测时域越长，控制器越"看得远"，但也越容易把远期不确定性放大。实战里有一个经验：当你发现轨迹跟踪误差曲线出现周期性起伏时，先考虑增大 (N_p)；当你发现控制量出现高频抖动时，先检查 (R_\Delta) 而不是增加 (N_p)。

4. Matlab代码骨架：离散化、预测矩阵、主循环这么搭

下面这部分是这篇笔记的核心干货。我给出一个可以直接跑通的Matlab代码框架，按文件组织为三个部分：模型参数初始化脚本、MPC矩阵构建函数、仿真主循环。

4.1 工程文件组织：model.m、mpc_matrices.m、sim_main.m

一个清晰的Matlab工程应该把参数、矩阵构建、滚动求解分开，不要全写在一个大脚本里。

• init_params.m：定义仿真时间、采样周期、预测时域、控制时域、权重矩阵、约束边界，以及期望轨迹生成函数。
• build_mpc_matrices.m：输入A、B、Np、Nc、Q、R，输出二次规划中的H矩阵、f矩阵、约束矩阵Aineq/bineq、边界lb/ub。
• sim_main.m：主循环里逐个时刻调用MPC求解函数，更新状态，记录历史，最后画图。

这样做的最大好处是调参时可以只改init_params，不会误碰矩阵构建代码。

4.2 预测矩阵构建的核心代码

预测模型的本质是把未来 (N_p) 步的状态写成当前状态和未来控制序列的线性组合：

[ X_{pred} = \Phi x_0 + \Gamma U ]

其中 (\Phi) 是把A的幂按预测步长堆叠起来的矩阵，(\Gamma) 是控制输入的历史影响矩阵。构建代码如下：

function [H, f, Aineq, bineq, lb, ub, Phi, Gamma] = build_mpc_matrices(A, B, Np, Nc, Q, R, du_max, u_max) nx = size(A, 1); nu = size(B, 2); Phi = zeros(Np*nx, nx); Gamma = zeros(Np*nx, Nc*nu); for k = 1:Np Phi((k-1)*nx+1:k*nx, :) = A^k; for j = 1:min(k, Nc) Gamma((k-1)*nx+1:k*nx, (j-1)*nu+1:j*nu) = A^(k-j) * B; end end Qbar = kron(eye(Np), Q); Rbar = kron(eye(Nc), R); H = Gamma' * Qbar * Gamma + Rbar; % f 在运行时根据 x0 和参考轨迹计算 end

注意H矩阵是可逆的对称正定矩阵，quadprog要求求解问题必须是凸的。如果H矩阵出现奇异，多半是Q或者R里面有零元素导致整个矩阵半正定，通常需要加一个小对角扰动。

4.3 主循环里的"求解-执行-反馈"

主循环是MPC落地的关键。核心逻辑是这样的：拿到当前状态 (x_{cur})，生成未来 (N_p) 步的参考轨迹序列，构建目标函数中的线性项f，然后调用quadprog，取求解结果的第一组控制量应用到系统，再更新状态。

for k = 1:steps x_cur = x_history(:, k); ref_seq = ref_history(:, k:k+Np-1); % 维度 Np*nx x 1 f = Gamma' * Qbar * (Phi * x_cur - ref_seq(:)); U0 = zeros(Nc*nu, 1); % 冷启动 [U_opt, ~, flag] = quadprog(H, f, Aineq, bineq, [], [], lb, ub, U0, options); if flag < 0 warning('第%d步QP求解失败', k); U_opt = zeros(Nc*nu, 1); end u_applied = U_opt(1:nu); % 用真实模型更新状态 x_history(:, k+1) = A * x_cur + B * u_applied; u_history(:, k+1) = u_applied; end

这里的update如果用的是简化模型A、B，那就是一个纯仿真；如果想加入模型误差，可以在状态更新方程里加一个扰动项。

4.4 quadprog调用细节与热启动

quadprog是Matlab自带的凸二次规划求解器，简单场景下完全够用。关键参数设置是：

options = optimoptions('quadprog', 'Display', 'off', 'Algorithm', 'interior-point-convex');

求解失败时flag是负值，一定要处理，否则直接索引U_opt就会满天飞错误。热启动技巧是把上一次的U_opt作为下一次的初值传进去：

U0 = [u_history(2:end, k); zeros(nu, 1)]; % 把上次解平移后作为新初值

这样通常能减少几次迭代，但一旦轨迹发生突变，热启动反而可能让求解器找不到路径，所以我的经验是：发现连续三次求解失败就立刻重置为冷启动。

5. 仿真曲线不会骗人：误差、饱和、抖振怎么看

代码跑起来之后，重要的不是看它有没有跑通，而是看曲线暴露了什么控制问题。我以一条三维8字轨迹为例，总结一下怎么看结果。

5.1 8字形轨迹跟踪结果：误差峰值出现在哪里

三维8字轨迹在转弯处有一个很明显的特征：曲率最大，所需加速度最大。MPC在这个区域的跟踪误差会突增，这很正常，问题在于增多少、增完之后能不能快速收敛。

我的一组仿真结果是：最大位置误差出现在第一个转弯点，峰值约9厘米；出弯后约1.2秒误差回到3厘米以内。这说明控制器在急剧转向时充分利用了预测提前减速，如果在同样位置用PID，误差峰值会到20厘米以上，而且恢复时间更长。

看误差曲线时，重点观察三点：

• 峰值是否在约束允许范围内；
• 峰值之后是单调收敛还是震荡；
• 误差曲线有没有明显的周期性"跟随滞后"。

5.2 控制量曲线中的"坏现象"排查法

控制量曲线比误差曲线更容易暴露控制器性格。最典型的坏现象是加速度指令呈现高频抖振，每个采样周期都在正负大幅度摆动。出现这种情况，大概率是 (R_\Delta) 太小，增量惩罚不足。处理方法是把 (R_\Delta) 从0.1往上调，直到抖振幅度降下来。

还有一种情况是控制量长期贴在约束边界上，比如期望加速度已经顶到最大允许值。这不一定坏，说明轨迹确实超出了飞行器的物理能力，但如果持续很长时间，说明轨迹规划本身不合理，要么降低速度，要么重新规划轨迹。

5.3 同一组轨迹下PID和MPC的对比

我给同一个仿真模型加了两套控制器，跑同一条三维螺旋上升轨迹，结果具有代表性。

MPC精度优势非常明显，但代价是前期参数整定和矩阵调试花的时间更多。工程上可以接受这个交换，因为MPC多出来的是部署前的开发成本，而不是运行时的性能损失。

6. 六个细节坑，每个都让我的MPC从能用到好用

最后分享我在Matlab实现里踩过的六个比较深的坑，每一个都直接影响仿真成败和结果可信度。

6.1 预测步长与离散化：模型失真的来源

采样周期 (T_s) 取0.05秒和0.2秒，得到的预测行为差别很大。(T_s) 太大时，离散模型传递函数失真严重，MPC预测的速度和真实系统误差越来越大，控制器会做出激进甚至错误的决策。(T_s) 太小也不行，因为一个采样周期内飞行器状态变化极小，Np要覆盖同样的时间跨度就得多算很多步，计算量剧增。

我的起步建议是取0.05秒到0.1秒之间，不要低于0.01秒，也不要高于0.2秒。

6.2 数值病态与权重归一化

Q矩阵里位置权重写成1000、R写成0.001，会让H矩阵的条件数非常差，quadprog可能警告"Hessian is not positive definite"。这种情况下即使能求解，数值误差也会让控制量非常敏感。我的做法是先统一尺度，把位置误差除以一个特征长度比如1米，速度误差除以一个特征速度比如1米每秒，这样权重矩阵的数值天然就在同一量级。

6.3 可行性保护：松弛变量不是摆设

我之前试过把加速度约束设为 ([-6, 6])，仿真跑一半quadprog直接报不可行。后来才发现轨迹规划在某些时刻要求的加速度超过了6，优化问题根本没有可行解。解决办法是引入一个松弛变量并施加很大的惩罚项，让约束在极端情况下可以被"软化"。这个技巧在论文里叫软约束，在工程里就是保命机制。

6.4 从仿真到实物：别忘了执行器特性

纯点质量模型在仿真里可以非常完美，但拿到真实飞行器上就会抖。原因是电机响应不是瞬时的，旋翼升力和力矩有延迟。一个简单做法是在模型里给控制输入加一个一阶惯性滤波，比如把电机时间常数设为0.03秒到0.05秒。这样MPC在预测时就能预见到"指令加速度在几十毫秒内才能达到"，控制决策会更保守、更贴近实际。

6.5 时域参数与计算耗时的平衡

Np一增大，求解时间很快上升。在第4节的框架里，Np=20和Np=60的求解时间差距可能不止3倍，因为矩阵维度和QP迭代步数都在涨。如果发现仿真里MPC占用的时间超过采样周期的30%，就该考虑减小Np或者改用更快的求解器。对Matlab仿真来说，这个瓶颈主要还是拖慢仿真速度，而对实物控制器就是致命问题。

6.6 热启动的正确做法

热启动能提速度，但它是一把双刃剑。轨迹连续平滑时，用上一轮的解做初值能显著减少迭代次数；但轨迹出现骤变、或者目标函数发生切换时，热启动会让求解器从错误的方向开始搜索，反而更慢甚至失败。

我最后采用了一个简单策略：记录当前采样周期的QP求解迭代次数，如果迭代次数超过一个阈值，下一次就自动改用冷启动U0=zeros。这个切换逻辑让我的仿真实测下来整体计算时间降低约20%，还避免了好几处求解崩溃。

说实话，MPC的入门曲线比PID陡得多，但一旦你理解了"预测、滚动、约束"这三个关键词，再回头看这套代码，会发现它其实是一条非常清晰的流水线。后续你可以在这个骨架上继续扩展：加入参考轨迹重规划、改为非线性MPC、把模型替换成实物辨识出来的参数分布。只要模型拆得稳、目标函数写得明白，这套框架还能陪你走很远。