无人机飞行约束下的模型预测控制：从Matlab仿真到实现解析

做无人机控制的人应该都有过这种经历飞机在空旷场地怎么飞都稳一进狭窄走廊、机库门、或者贴着建筑物巡检就开始“犯浑”。我最早是用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 vxd(py)/dt vyd(vx)/dt axd(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(k1) px(k) vx(k) * dt 0.5 * ax(k) * dt²vx(k1) vx(k) ax(k) * dtpy和vy同理。从这个离散模型可以看出当前时刻的加速度会影响下一时刻的速度再下一时刻的位置。这就是MPC预测的基础。后面所有约束表达和代码实现都是围绕这个递推关系展开的。2.3 预测时域和控制时域的选择预测时域Np和控制时域Nc是MPC最重要的结构参数。预测时域决定控制器“向前看多远”控制时域决定未来输入序列有多少个自由度。对无人机跟踪问题我习惯把预测时域设在1.5到2.5秒之间控制时域可以比预测时域短一些比如Np对应20步、Nc对应10步这样既能保证轨迹平滑又能减少优化变量的数量。预测时域太短控制器会变成“近视眼”约束起作用的时间太晚预测时域太长优化问题变大求解时间变长而且模型偏差会积累预测末端的轨迹基本失真。一般操作是先固定dt50ms取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_maxpy_min ≤ py(k) ≤ py_maxpz_min ≤ pz(k) ≤ pz_max速度约束vx_min ≤ vx(k) ≤ vx_maxvy_min ≤ vy(k) ≤ vy_maxvz_min ≤ vz(k) ≤ vz_max输入约束ax_min ≤ ax(k) ≤ ax_maxay_min ≤ ay(k) ≤ ay_maxaz_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: N1 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, N1); X(:, 1) x0; for k 1:N X(:, k1) P.A * X(:, k) P.B * U((k-1)*nu1 : k*nu); end cost 0; constraints []; for k 1:N e X(:, k1) - ref(k1, :); cost cost e * P.Q * e U((k-1)*nu1:k*nu) * P.R * U((k-1)*nu1:k*nu); constraints [constraints, P.x_min X(:, k1) P.x_max]; constraints [constraints, -P.u_max U((k-1)*nu1: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(k1) α * 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求解器的退出标志或者求解状态只要出现一次非成功状态就把当时的输入和预测轨迹存盘。这些故障快照是后面调模型参数、找约束失效原因最宝贵的材料比任何日志都直观。