做无人机控制的人应该都有过这种经历：飞机在空旷场地怎么飞都稳，一进狭窄走廊、机库门、或者贴着建筑物巡检，就开始“犯浑”。我最早是用PID来调室内悬停的，悬停本身没毛病，结果让它穿过门框时，飞机直接朝着门框撞过去。后来我把基于MPC的带飞行约束控制方案搬到Simulink和Matlab里跑了一遍，才真正体会到什么叫“把约束写进控制器”。模型预测控制本身并不是新东西，但在无人机这种高度受限、状态耦合和输入饱和都很明显的系统上，它的优势特别突出：控制器会基于当前状态把未来一段时间的轨迹和输入一起“预演”一遍，在预演阶段就把边界、速度限制和油门上限全部纳入优化求解。这篇文章我就把整个实现过程拆开讲一遍，包括无人机模型怎么简化、飞行约束怎么用数学方式表达、Matlab代码怎么组织，以及仿真和实飞前通常会踩到哪些坑。材料是按带完整可运行代码的思路准备的，适合正在做无人机路径规划或者飞控代码验证的读者参考。
1. 为什么“先把飞机飞起来再考虑约束”是个伪命题
很多新手对无人机控制的第一个想法是：先把位置环控制做好，让飞机能按期望轨迹飞，然后遇到障碍物再用避障算法去躲。这个思路听起来很自然，但实际实现时你会发现，约束并不是飞行过程中的“附加条件”，而是控制器本身必须满足的硬边界。如果控制器模型里完全没有约束信息，等到发现快要撞墙再去“躲”，往往已经来不及了。
1.1 无人机身上到底有哪些约束
无人机不是质点，也不是理想的速度源。它在空中受到的飞行约束非常具体：
- 位置边界：比如室内飞行时不能越过墙体边界，巡检时不能离桥梁支臂太近，起降平台内有指定的起降区域。
- 速度限制：机架结构、云台稳定能力和IMU的量程都会限制最大飞行速度，超过这个速度姿态角就容易饱和。
- 姿态角限制：俯仰角、滚转角直接和水平加速度耦合，角度过大就会导致推力矢量倾斜，高度维持不住。
- 输入饱和：电机转速有上限和下限，PWM控制量不能超过油门极限，这在快速机动时尤其明显。
- 安全距离约束：如果挂载了视觉传感器，还会有一个基于检测结果生成的动态禁区，要求飞行器与障碍物保持一个最小距离。
这些约束不能靠“软件里限制目标点”来完成。比如你让飞机飞向某个目标点，即使目标点设在走廊正中间，但控制器在追赶期望值的过程中可能会产生很大的横向误差，飞机照样会碰到走廊壁。关键不在于目标点在哪里，而在于飞行全过程中的状态都不能越界。
1.2 PID为什么很难处理这类问题
PID控制器本质上是基于当前误差的反馈调节器。它只知道现在的位置和期望位置差多少，然后根据比例、积分、微分项去给出修正指令。它不知道自己未来几步会跑到哪里。当我们给PID输出加上限幅器来限制油门时，这个限幅确实是一种输入约束，但它和当前时刻的控制目标是割裂的。误差一大，限幅器瞬间饱和，积分项继续累积，等误差回小之后，控制量还在饱和区里出不来，飞机会出现过冲甚至震荡。
在飞行约束中，最麻烦的是预测不到未来可能发生的“违规”。PID在控制周期内根本不会去推算“如果我继续保持这个速度飞，下一轮会不会越过位置边界”这类问题。所以大多数实际项目里，PID负责被降级到底层姿态控制或者内环控制，而位置级制导和约束避障必须由更高层的规划器去完成。MPC刚好在这个层次代替了传统的位置环控制器。
1.3 MPC的“预演”逻辑
MPC每一步都做同一件事情：以当前状态为起点，用辨识好的模型把未来N个控制周期内的状态推演出来，算出未来一段时间输入序列，然后只把第一步输入送给无人机，下一周期重新滚动计算。因为预测轨迹是在优化问题里和约束一起求解的，所以只要优化问题有解，这N步内的状态就不会违反你写进去的约束。
这就像开车过窄路，老司机不是盯着车头一点点修方向盘，而是会先看入弯轨迹、预估后轮内切量，再决定打方向和踩刹车。MPC做的就是类似的事情：它把“未来一段路的轨迹”拿来一起规划，而不是只根据当前误差做条件反射。这也是为什么在带飞行约束的无人机控制里，MPC比串级PID和纯路径规划方案都更直接。
2. 简化无人机模型：从六自由度到预测模型
要跑MPC，你现在就得有模型。无人机完整六自由度模型包含位置、速度、姿态角、角速度、油门拉力和重力耦合，用它直接做预测模型计算量太大，也不太适合线性MPC框架。工程上最常见的做法是做一个内外环拆分：外环是位置和速度的平移运动模型，内环是姿态控制。整个MPC控制器只负责位置和速度层面的决策，输出的控制量是期望加速度或期望推力方向，然后交给底层姿态控制器去执行。
2.1 点质量模型为什么够用
把无人机看成一个点质量，忽略姿态动力学细节，只在水平面或者三维空间里建立运动方程，看起来非常“简陋”，但对位置级飞行约束来说精度完全够用。原因是姿态内环的响应速度远快于位置外环，一般内环能达到数十毫秒量级收敛，而MPC的预测时域通常覆盖一到两秒。在这么长的时间尺度上，位置和速度的关系可以用一个双积分器模型近似。
举个例子，水平面上的状态向量为：
x = [px, py, vx, vy]^T
其中px和py是水平位置，vx和vy是水平速度。控制输入u = [ax, ay]^T是期望水平加速度。如果忽略姿态响应的滞后，那么连续时间模型就是：
d(px)/dt = vx
d(py)/dt = vy
d(vx)/dt = ax
d(vy)/dt = ay
写成矩阵形式：dx/dt = A_c * x + B_c * u。
这个模型没有直接加入推力、滚转角、俯仰角这些东西，但已经把无人机平移运动最主要的动态特征抓住了。实际做的时候，如果你希望更精确，可以把内环闭环等效成一阶惯性环节，在加速度通道上加一个滞后时间常数。不过刚开始跑MPC，用纯双积分器模型足够让你理解整个控制逻辑。
2.2 离散状态空间方程
MPC是离散时间控制器，需要把连续模型转换成离散模型。假设控制周期为dt，常用的离散化方式是零阶保持，也就是在一个采样周期内保持控制输入不变，然后对线性系统求矩阵指数：
A_d = expm(A_c * dt)
B_d = ∫₀^dt expm(A_c * τ) dτ * B_c
在Matlab里用c2dm或者c2d函数都能直接完成。对双积分器模型来说，离散结果非常直观：
px(k+1) = px(k) + vx(k) * dt + 0.5 * ax(k) * dt²
vx(k+1) = vx(k) + ax(k) * dt
py和vy同理。
从这个离散模型可以看出，当前时刻的加速度会影响下一时刻的速度，再下一时刻的位置。这就是MPC预测的基础。后面所有约束表达和代码实现都是围绕这个递推关系展开的。
2.3 预测时域和控制时域的选择
预测时域Np和控制时域Nc是MPC最重要的结构参数。预测时域决定控制器“向前看多远”，控制时域决定未来输入序列有多少个自由度。对无人机跟踪问题，我习惯把预测时域设在1.5到2.5秒之间，控制时域可以比预测时域短一些，比如Np对应20步、Nc对应10步，这样既能保证轨迹平滑，又能减少优化变量的数量。
预测时域太短，控制器会变成“近视眼”，约束起作用的时间太晚；预测时域太长，优化问题变大，求解时间变长，而且模型偏差会积累，预测末端的轨迹基本失真。一般操作是先固定dt=50ms，取Np在20到40之间，然后根据仿真效果微调。
3. 把“不能撞墙”变成一个数学约束
带飞行约束的MPC和普通MPC最大的区别，就是把飞行禁区、速度极限、输入饱和这些东西翻译成优化问题里的不等式约束。这一节是文章的核心，因为很多代码跑不动或者效果差，往往是约束写错了，而不是MPC求解器有问题。
3.1 状态约束和输入约束的基本形式
假设状态量是x = [px, py, pz, vx, vy, vz]^T，输入u = [ax, ay, az]^T。位置边界的约束就是简单的不等式：
px_min ≤ px(k) ≤ px_max
py_min ≤ py(k) ≤ py_max
pz_min ≤ pz(k) ≤ pz_max
速度约束：
vx_min ≤ vx(k) ≤ vx_max
vy_min ≤ vy(k) ≤ vy_max
vz_min ≤ vz(k) ≤ vz_max
输入约束：
ax_min ≤ ax(k) ≤ ax_max
ay_min ≤ ay(k) ≤ ay_max
az_min ≤ az(k) ≤ az_max
在预测模型中，每个预测步k对应的状态是决策变量u序列的函数。所以这些约束最终会变成关于u的线性不等式。MPC求解器要做的，就是在满足所有不等式和动力学等式的前提下，找到使目标函数最小的输入序列。
3.2 安全走廊与避障约束
实际飞行中的约束往往不是一个简单的矩形框。比如无人机要在两个障碍物之间穿过，或者沿着狭窄走廊飞行，这种场景可以抽象成安全走廊。安全走廊由多个凸多面体组合而成，飞机在每一时刻必须处于其中一个多面体内。将三维空间划分为多个凸区域，每个区域用一组线性不等式描述，然后在MPC里为每个预测步分配一个活动区域，强制对应状态满足这组不等式。
一个更工程化的做法是把避障约束写成凸近似。对障碍物，常用约束是把飞机和障碍物中心之间的欧氏距离限制在安全半径以上。但欧氏距离约束在优化里是非凸的，不能让求解器直接处理。常见做法是用线性化逼近，或者在水平面内把它近似成多个超平面约束。假设障碍物中心是o，障碍物半径为r_obs，安全距离是d_safe，则可以写成一个不等式约束组，例如：
(ox - px(k)) * cosθ + (oy - py(k)) * sinθ ≥ d_safe
对若干组θ取值同时要求成立，就能近似得到一个凸的安全区域。选择几组关键方向，约束数量不会太多，求解速度也还能接受。
3.3 硬约束不可行时怎么办
带约束MPC在预测时域较长时经常出现一个“优化问题无解”的尴尬情况。原因往往是：预测起点离约束太近，而控制量幅度又不够，导致找不到一条未来轨迹能同时满足所有约束。比如飞机正以高速撞向墙壁，输入饱和上限又把减速度限制死了，那么再厉害的预测器也算不出解决方案。
工程处理通常有两种。第一种是改用软约束，把位置约束改成惩罚项。不是严格强制px ≤ px_max，而是在目标函数里加一项：
penalty = ρ * max(0, px(k) - px_max)²
ρ是一个很大的权重。这样约束“尽量满足”，实在不行时求解器也能返回一个可用的输入，而不是把控制器卡死。第二种是保留硬约束，但在控制器前面加一个状态可行性检查。一旦检测到当前状态已经越过约束边界，就切换到保守的“紧急减速模式”，让飞机先收油门、把速度降下来，回到可行状态后再重新起MPC。
我个人建议，安全距离和位置边界用软约束加高权重，输入饱和和速度极限用硬约束。因为飞行员的感受是：输入饱和是硬物理边界，不可能违反；位置边界是很重要但允许在极端情况下有一点点超出，否则整个系统太脆，一个传感器噪声就会让控制器宕机。
4. Matlab实现：从零手写一个MPC控制器
Matlab里做MPC有现成的Model Predictive Control Toolbox，也有基于YALMIP或CVX的通用优化求解方案。如果你还没用过现成工具箱，我的建议是先手写一个简单的QP求解过程，这样你对每个信号是怎么算出来的会很清楚。等逻辑通了，再回头用工具箱提升开发速度。
4.1 控制器整体流程
整个MPC控制器在每个控制周期做的事大概如下：
- 读取当前状态x0，以及当前时刻的参考轨迹ref序列。
- 构造预测模型A_d、B_d。
- 将未来Np步状态写成关于输入序列U的线性表达式。
- 将目标函数整理成二次型：min U^T * H * U + f^T * U。
- 将状态约束和输入约束整理成形如A_in * U ≤ b_in的线性不等式。
- 调用quadprog或其他QP求解器，得到最优输入序列。
- 取第一个输入u0下发，下一周期重复。
这个流程看起来繁琐，但好处是每一步都能单独调试。我最常遇到的问题就是第4步的目标函数写错符号，导致控制器不是在跟踪轨迹而是在推离轨迹；还有第5步的约束矩阵维度不对，导致约束完全没生效。
4.2 核心代码片段
下面这个函数是我在Matlab里验证MPC时常用的一个简化版本。它依赖YALMIP这个开源建模层，优化求解器可以选quadprog或OSQP。代码里省去了很多边界检查，但结构是完整的。
function [u_opt, x_pred] = mpc_pos_ctrl(x0, ref, P) % x0: 当前状态 [px; py; pz; vx; vy; vz] % ref: N+1 x 6 参考轨迹 % P: 参数结构体 % P.A, P.B: 离散状态空间模型 % P.Q, P.R: 状态与输入权重 % P.N: 预测时域 % P.x_min, P.x_max: 状态约束 % P.u_max: 输入约束（对称） N = P.N; nx = length(x0); nu = size(P.B, 2); U = sdpvar(N*nu, 1); % 决策变量：未来N步输入 X = zeros(nx, N+1); X(:, 1) = x0; for k = 1:N X(:, k+1) = P.A * X(:, k) + P.B * U((k-1)*nu+1 : k*nu); end cost = 0; constraints = []; for k = 1:N e = X(:, k+1) - ref(k+1, :)'; cost = cost + e' * P.Q * e + U((k-1)*nu+1:k*nu)' * P.R * U((k-1)*nu+1:k*nu); constraints = [constraints, P.x_min <= X(:, k+1) <= P.x_max]; constraints = [constraints, -P.u_max <= U((k-1)*nu+1:k*nu) <= P.u_max]; end ops = sdpsettings('solver', 'osqp', 'verbose', 0); optimize(constraints, cost, ops); u_opt = value(U(1:nu)); x_pred = value(X); end这段代码的好处是结构几乎和数学公式一一对应，理解起来非常容易。缺点是YALMIP会引入额外开销，不适合直接上嵌入式飞控。如果你要做实时验证，我建议把QP问题手动展开成H矩阵和f向量，然后直接用OSQP的Matlab接口或MEX版求解。
4.3 目标函数里的权重该怎么设
位置误差权重Q的对角元素、加速度惩罚R的对角元素决定了控制器“输出是什么”。一个基本经验是：Q中速度项的权重不要比位置项大太多，否则飞机会为了压住速度而绕远路，跟踪延迟会很明显；R太小会让控制量出现高频抖振，电机和执行机构受不了，R太大则轨迹会变软，过弯时会明显甩尾。
我用的一套起始参数是：
Q = diag([10, 10, 15, 2, 2, 3])
R = diag([1, 1, 2])
这组参数适合室内小型四旋翼，位置误差为主、加速度惩罚适中。你看仿真轨迹如果出现来回震荡，就把R调大；如果过弯太钝，就把R调小一点。注意一次只动一个权重，不要同时调三个。
5. 仿真场景：窄走廊穿行与阵风扰动
代码跑通之后，我建了两个典型的仿真试验。一个是窄走廊的穿越任务，另一个是在约束边界附近给一个外部阵风扰动。这两个场景基本覆盖了“飞行约束在起作用”和“输入饱和在起作用”两种状态。
5.1 窄走廊穿越
走廊宽度设成2.2米，无人机初始位置在走廊入口左侧偏了0.3米，目标点设在走廊出口正中央。参考轨迹是一条直线。MPC预测时域采30步，每步50ms，位置边界设置成走廊范围，同时加上速度上限。
仿真结果里最能说明问题的现象是：MPC并没有让飞机先快速偏向中心线再直飞出去，而是在进入走廊前就提前调整了横向位置，整个过程横向误差被压在0.1米以内。对比没有约束的普通控制器，同样的初始位置偏差，轨迹在入口附近会有一个很大的横向超调，最大误差超过0.5米。这就是“预演”和“事后修正”的区别。MPC知道预测轨迹里第10步会撞墙，所以它在第1步就开始提前减速和回正。
5.2 输入饱和阵风测试
第二个场景是在飞机沿着走廊中轴线飞行时，第2秒给一个持续0.5秒的外力扰动，相当于突发的侧向阵风。因为没有约束的控制器会试图立刻把横向误差拉回零，需要的横向加速度超过执行器上限，导致PWM信号饱和。饱和之后，实际状态变化跟不上控制器期望，飞机在走廊里来回摆动，最糟糕时横向位置差一点撞上边界。
换成MPC之后，因为模型里带了输入约束，优化问题从一开始就只会选择“能执行出来的加速度”。阵风扰动期间MPC给出的控制量贴近加速度边界，但不会超过。扰动结束后，它沿着可行轨迹逐渐回中，整个过程位置一直留在走廊边界内。这个现象和PID限幅器的表现完全不同：限幅器是被迫截断，MPC是主动选择这个幅值。
5.3 怎么看约束是否真正生效
我在调试时会在Matlab里把每个控制周期的最大约束违反量记录下来。做法很简单：算完MPC之后，把预测轨迹中每一步的状态和约束边界比较，记下超出最大值的量。正常运行时这个量应该是0或者非常小的数。如果在某个瞬间出现明显超过边界，那就说明约束没有正确传给求解器，或者当前模型和真实系统偏差太大。
另外一个值得注意的点是仿真步长和控制周期的关系。Simulink仿真步长必须明显小于MPC控制周期，最好差一个量级。比如MPC周期是50ms，那么仿真器用5ms甚至1ms的固定步长。否则你从模型中读到的“当前状态”其实已经落后于理想采样时刻，控制性能会大打折扣。
6. 调参和实飞前容易踩的坑
这一节是我最想写的内容。因为是理论模型看起来漂亮，真正放到Matlab里一连串跑，或者准备装到无人机上测试时，问题一个接一个。
6.1 求解器超时问题
MPC最怕的就是求解时间超过控制周期。我一开始用内置的fmincon求解，预测时域调到30步之后，求解时间直接飙到一两百毫秒，飞机根本没法用。后来改用二次规划求解器，速度快了很多，典型求解时间可以压到10毫秒以内。如果预测时域还要更长，建议把控制时域Nc缩短到Np的一半，减少决策变量个数。
在Matlab里可以这样估计求解耗时：
t0 = tic; for i = 1:100 [u, ~] = mpc_pos_ctrl(x0, ref, P); end avg_time = toc(t0) / 100;如果平均值超过控制周期的一半，就需要认真做代码优化或降低Np。实际操作中还要预留状态估计和其他外围任务的执行时间，不要等逼近极限了才去调。
6.2 模型偏差带来的约束失效
MPC是模型预测控制，模型错了，再强的求解器也白搭。最常见的问题是我在前面提过的内环滞后。如果你把MPC输出的期望加速度直接当实际加速度输入到物理模型，仿真里很完美，但真实飞机上姿态内环不可能瞬间实现这个加速度。滞后会让预测轨迹比实际状态超前，约束看起来满足了，实际却已经越界。
解决思路是在预测模型里加一个加速度通道的等效时间常数。比如水平加速度的实际响应可以写成：
a_real(k+1) = α * a_cmd(k) + (1 - α) * a_real(k)
α取值范围在0.3到0.8之间，根据具体飞控内环响应速度来确定。加了这层之后，模型更贴近真实系统，约束执行效果明显变好。
6.3 参考轨迹的连续性
MPC每步都在跟踪参考轨迹，但参考轨迹如果包含不连续的跳变，预测轨迹即使满足约束，也会产生很大的加速度要求。比如你让目标点从A点瞬间跳到B点，MPC会在第一阶段输出最大加速度，然后马上要求最大减速度，整个飞机处于剧烈机动状态。
正确做法是先对参考轨迹做平滑处理。常见的有多项式规划、最小加加速度轨迹，或者简单的低通滤波。把目标位置跟踪问题改成目标轨迹跟踪问题之后，MPC的压力会小很多，约束违背的概率也会明显下降。这个点经常被忽略，很多人以为“MPC自带轨迹规划功能”就可以随意给目标点，这是误解。
6.4 状态估计的重要性
最后一个坑是状态估计。MPC需要完整状态信息，包括位置和速度。GNSS在室外开旷环境能用，但室内或密集城区，测距定位和视觉里程计的噪声会直接影响MPC效果。MPC的鲁棒性主要体现在约束处理和滚动优化上，但它不会替你把状态估计噪声过滤干净。所以实际系统里我会建议在MPC外侧加一个卡尔曼滤波器或者互补滤波器，把速度和位置先融合一遍。
如果在仿真里发现轨迹有高频抖动，先不要急着调Q和R，去看一下当前状态是不是本身就有毛刺。状态估计噪声大时，再大的R都压不住抖动，因为控制器是在追踪一个带噪声的状态序列。
7 从Matlab走向机载代码的扩展思路
这一段属于“如果要做真机下一步怎么办”的路线建议，不算标准教程，但会让你的整体方案更完整。
7.1 从线性模型到非线性规划
上述MPC是线性时不变模型。如果无人机的飞行包线很大，比如要做大角度机动或者高速竞赛，线性模型就不够了，需要转成非线性MPC。NMPC的代价是求解速度慢很多，实时性难以保证。还有一种折中方案是使用扰动线性化，每一步基于当前状态重新做线性化和离散化，形成一个时变的MPC问题。这个方案在计算量和控制精度之间平衡得比较好。
7.2 和视觉感知结合
在较新的研究方向里，MPC往往不只用来做轨迹跟踪，而是直接和感知模块耦合。用深度相机或单目前向视觉获得的障碍物检测结果，在每一个控制周期动态生成安全约束，让MPC在飞行的同时避障。这个方向的难点在如何把检测的不确定性转换成约束的保守程度。检测框稍微抖一下，约束区域也跟着抖，控制器就会频繁调整轨迹，所以通常需要加一层检测结果的时间滤波和障碍物跟踪器。
7.3 代码迁移到飞控
Matlab验证完之后，真正的移植还是得落到C/C++代码。我见过几种迁移路径：第一种是用Matlab Coder将MPC求解部分直接生成C代码；第二种是自己将OSQP移植到STM32上，再用飞控的实时线程调用；第三种是用Simplified MPC，比如把QP退化成显式MPC，离线把不同可行域对应的控制律预先算好，在线只需要查表。显式MPC对机载内存很有吸引力，但状态量一多，查表空间会非常庞大，需要谨慎评估。
从我个人的实操体会来看，在真机测试前一定要花时间把约束的“软硬策略”想清楚。不是所有约束都适合硬性写死，也不是所有边界都能靠权重硬拉。软约束加高权重、硬约束保安全的混合做法，是我目前觉得最稳妥的配置。最后再分享一个细节技巧：Matlab仿真里记录MPC求解器的退出标志或者求解状态，只要出现一次非成功状态，就把当时的输入和预测轨迹存盘。这些故障快照是后面调模型参数、找约束失效原因最宝贵的材料，比任何日志都直观。
