基于MPC的四旋翼航点导航Matlab仿真实现

四旋翼航点导航这事我最早是用串级PID加几何跟踪扛过去的。当时项目要求不算高就是让飞机按顺序飞几个点悬停一下再飞下一个。PID那套在直线段上表现还行但一到切换航点或者要绕开障碍物的时候问题就来了姿态容易晃、速度控制不住、经常冲到目标点附近再硬刹回来。后来我把控制方案换成了模型预测控制MPC用Matlab做完整仿真验证实现了多目标航点导航。这个项目的核心思路不复杂给四旋翼一串目标航点让它在满足动力学约束、输入饱和约束、避障约束的前提下预测未来一小段时间内的运动轨迹滚动优化出当前最合适的控制量平稳地逐个飞完所有航点最终悬停。用了一套纯Matlab代码实现不依赖外部硬件和特殊飞行平台主要面向做无人机自主导航的同学、想把控制方案从PID往MPC切换的工程师以及对预测控制感兴趣想快速跑通一个完整Demo的研究者。这篇文章我就从方案选型、动力学建模、控制设计、Matlab实现到调试排查把整个项目的关键细节讲清楚。1. 项目整体设计与控制方案选型1.1 为什么是MPC而不是PID或纯几何跟踪先说说我为什么放弃PID。PID本质上是一个基于当前误差和过去误差累积的反馈控制器它对“未来会发生什么”没有感知。四旋翼在两点之间飞行时当前航点的参考位置是固定的如果下一段轨迹需要提前转向PID反馈看到的是位置误差已经变小它不会主动去规划减速和转向路径于是飞机会以较高的速度冲到目标点附近产生明显超调。这个问题在多个航点、每条轨迹段都比较短时尤其明显整条路径会被割裂成一段段“冲一下、刹一下”的折线飞行。几何跟踪算法比如L1导航或者纯追踪比PID聪明一些它可以根据前视距离去跟踪一条路径。但这类方法本质上也没有利用系统的动力学模型无法显式考虑“当前油门已经在饱和边界”“姿态角不能超过某个值”这类物理约束。遇到狭窄走廊、障碍区绕行或者速度受限场景几何跟踪往往只能通过调低前视距离来妥协控制品质非常被动。MPC的思路完全不一样。它每一步都基于当前状态和飞机的动力学模型向前预测未来N步的状态演化然后求解一个带约束的优化问题在预测时域内同时最小化跟踪误差、控制量大小、控制量变化率并且显式把输入饱和、姿态限幅、速度限制甚至避障约束写进优化问题里。它有点像一个会提前“想几步”的棋手而不是只会看眼前一步的玩家。每步只取优化结果中的第一个控制量执行下一步再滚动重新求解这就是“滚动时域”的含义。这个特性让MPC特别适合多目标航点导航这种既有路径跟踪任务、又有硬性约束的场景。1.2 控制架构选择统一MPC还是内外环级联做四旋翼MPC第一个绕不开的问题是控制架构。业界和学术界常见的做法分两种第一种是内外环级联位置环用MPC输出期望的加速度或者期望的横滚/俯仰角姿态环继续用PID之类的高频控制器去跟踪。这种方案的好处是姿态环可以保持原有的成熟控制器不动MPC只负责慢速的位置规划实现风险低实机上很多飞控就这么改的。第二种是统一MPC一个控制器直接输出四个电机的推力/转速指令同时处理位置、速度、姿态和角速度的跟踪。这种方案模型更完整所有约束都能在一个优化问题里统一处理但模型复杂度和求解负担明显更高对模型准确性也更敏感。我在这个项目里用的是第二种也就是统一MPC。原因有几个第一项目是纯Matlab仿真不需要考虑真实飞行器上的姿态控制保护逻辑模型精度反而可以做得更饱满一些第二我选了线性MPC而非非线性MPC用悬停工作点附近线性化的小角度模型问题规模不大在Matlab里用quadprog就能快速求解第三统一MPC输出的物理量直接就是电机推力航点切换、避障、姿态限幅这些约束可以写进同一个优化问题逻辑上非常干净。两种方案的优劣对比我整理成了一张表对比维度内外环级联位置MPC 姿态PID统一MPC位置姿态一体模型复杂度低位置环模型简单高需要完整刚体模型求解负担低状态量和约束少高预测时域内状态多约束处理能力只能处理位置/速度约束可以同时处理姿态、角速度、输入约束可移植性好可迁移到真实飞控差需充分验证模型适合场景实机部署、快速原型仿真验证、算法研究如果你打算后续把代码搬上真机我建议第一版先做内外环级联如果只是做算法研究和仿真验证统一MPC更有优势也更贴合这个标题里“研究”的定位。1.3 多目标到底怎么理解标题里“多目标”这个词一开始容易让人想到多目标优化Pareto前沿但实际在这个项目里多目标有三个层面的含义第一层是多个目标航点。也就是飞控需要依次经过一系列空间点而不是只飞到单个目标点。这个需要轨迹生成模块把离散航点序列转成连续的参考轨迹MPC跟踪的是参考轨迹而不是跳跃的离散点。第二层是优化目标函数里有多项性能指标。我会在设计成本函数时同时惩罚位置误差、速度误差、姿态误差、控制量幅值和控制量增量。这几个目标之间往往相互冲突想让位置响应快就要给较大的位置权重但代价是姿态动作更猛、消耗更多控制能量想让飞行平稳就得增加控制增量的权重但跟踪误差会变大。MPC的滚动优化本质上是每一步都在这些目标之间做权衡。第三层是约束目标里同时包含物理限制和安全限制。输入饱和、姿态角度限制属于物理限制速度限制、障碍物避碰属于安全限制。这些都是通过不等式约束写进优化问题的这也是区别于传统PID的重要能力。理解了这三点后面所有设计就不会跑偏了。2. 四旋翼动力学模型与MPC建模2.1 状态量选取与坐标系定义MPC的预测质量完全取决于模型的准确性。做四旋翼建模我习惯用地面惯性坐标系描述位置和速度用机体坐标系描述姿态和角速度。状态向量取12个x [x, y, z, φ, θ, ψ, u, v, w, p, q, r]其中(x, y, z)是地面系下的位置(φ, θ, ψ)是欧拉角横滚、俯仰、偏航(u, v, w)是地面系下的线速度(p, q, r)是机体系下的角速度。有人喜欢把线速度放到机体坐标系下表示那后面求导和坐标变换会多几个科氏力项我建议初学者用地面系表示线速度模型更直观线性化也容易。欧拉角表达在±90度附近有奇异问题对于航点导航这种小角度飞行场景完全够用。如果你的项目以后要扩展到特技飞行、大机动那建议换成四元数或者旋转矩阵代价是状态向量维度更高、模型更复杂。2.2 刚体动力学方程四旋翼动力学可以分成平动和转动两部分。平动方程写在地面惯性系下m·v_dot -m·g·e3 R·F_prop其中m是质量e3是地面系Z轴单位向量F_prop是机体坐标系下的总推力向量R是机体到地面的旋转矩阵。旋转矩阵用欧拉角表示时标准形式是R Rz(ψ)·Ry(θ)·Rx(φ)Matlab里可以用angle2dcm函数生成文档里对每个欧拉角的转动顺序定义得很清楚建议先用这个函数验证一下自己的坐标变换对不对再往下写。转动方程一般写在机体坐标系下J·ω_dot -ω × (J·ω) τ其中J是转动惯量矩阵τ是机体上的力矩。如果假设机体结构近似对称J可以取对角阵。把欧拉角的微分和角速度之间也建立起关系[φ_dot; θ_dot; ψ_dot] W(φ, θ)·[p; q; r]W是一个与欧拉角有关的变换矩阵。小角度假设下W近似等于单位阵这是后面线性化的重要一步。完整的非线性状态方程可以整理成f(x, u) x_dot的形式其中控制输入u取四个电机的推力[F1, F2, F3, F4]或者更常用的是总推力T和三个力矩τφ、τθ、τψ的组合。我建议用总推力三轴力矩作为控制输入因为电机转速映射那部分可以单独做一个模块让MPC的模型更干净。2.3 悬停点的线性化与离散化直接拿非线性模型做MPC就是非线性MPCNMPC求解需要fmincon这类非线性优化器Matlab代码写起来吃力实时性也差。这个项目里我选择在悬停工作点做线性化得到一个线性状态空间模型x_dot A·x B·u具体做法是令状态量都在零附近悬停时欧拉角为零、角速度为零、速度为零对非线性方程求雅可比矩阵。理论上需要手推雅可比现在Matlab有符号计算工具箱可以先定义符号变量用jacobian函数自动求偏导再把数字代进去这样效率高很多也不容易手算出错。线性化之后还要进行离散化因为MPC的预测模型是离散的。采样时间我取0.1秒10Hz用零阶保持方法A_d, B_d c2d(A, B, 0.1)Matlab的c2d函数一行代码搞定。之所以选0.1秒是因为四旋翼位置控制环典型带宽在1~3Hz左右MPC预测时域如果取30步也就是向前预测3秒这个时间足够覆盖大多数航点段的飞行需求。姿态角速度的响应比位置快但统一MPC里已经包含姿态状态不需要单独提高采样频率。3. 航点导航与MPC控制器设计3.1 从离散航点到连续参考轨迹MPC不能直接跟踪一串离散航点因为航点之间没有定义时间上的参考值。比如飞机在第一个航点悬停时下一个航点突然跳动到远处MPC的误差信号会瞬间暴增导致第一帧控制量剧烈变化。所以我在航点之间生成时间参数化的连续参考轨迹。最简单的做法是分段线性插值两个航点之间根据设定好的巡航速度V_ref算出这段路的飞行时间T_seg 距离 / V_ref然后在时刻序列上线性插值出位置参考。线性轨迹会导致速度参考在航点切换处跳变MPC要强行跟踪一个速度阶跃依然会抖。更好的方案是用梯形速度曲线约束的轨迹生成就是在航点之间按“加速—匀速—减速”三段规划每段的位置用二次多项式表示整条轨迹的位置连续、速度连续、加速度连续。我在实现时写了一个traj_from_waypoints.m函数输入航点矩阵和巡航速度输出的是时间数组和每个时刻的位置、速度参考。这部分代码本质上是一个小型的轨迹规划器不复杂但能显著改善MPC的跟踪平滑度。轨迹生成时还需要指定偏航角参考。室内航点导航我一般让偏航始终指向飞行方向户外巡检则偏向固定朝向这个看项目需求我这里做成可选参数默认反向next航点方向。3.2 成本函数设计一场多目标加权博弈MPC每一步的优化问题是这样定义的min Σ_{k0}^{N-1} ‖X_k - X_ref,k‖²_Q ‖U_k‖²_R ‖ΔU_k‖²_S其中Q是状态误差加权矩阵R是控制量加权矩阵S是控制增量加权矩阵N是预测时域步数。这样的设计第一个求和项负责让状态跟踪参考轨迹第二项抑制过大的控制输入第三项让控制变化平缓三者就是三个“目标”的权重。Q矩阵我按状态维度展开。位置误差权重最大因为航点导航最终看的是位置准不准速度误差次之起到阻尼作用防止位置环产生震荡姿态误差权重适度保证飞机飞行姿态不过分倾斜角速度误差用于提供姿态动态阻尼。一个可以直接拿来跑通仿真的初始权重配置是这样状态权重值位置 x, y, z10速度 u, v, w2姿态角 φ, θ1偏航 ψ0.5角速度 p, q, r0.1R矩阵给总推力和三个力矩各配0.1S矩阵四个控制增量各配0.5。这个配置的直观效果是MPC更优先保证位置跟踪同时会主动避免过于剧烈的控制动作。需要特别提醒的是单位问题。位置单位是米姿态单位如果是弧度数值比较小如果不缩放位置误差会主导整个优化目标姿态失控可能被掩盖。我的做法是姿态角在状态向量里直接用弧度然后通过Q矩阵里的权重把姿态误差的量级调上去保证它不会被位置项淹掉。3.3 约束条件从输入饱和到避障MPC相比PID的最大优势就是可以显式处理约束。本项目里我设置了三类约束第一类是输入饱和约束。单个电机的推力不能为负也不能超过最大值所以总推力的范围是[0, 4·F_max]三轴力矩的范围根据电机推力差值的物理上限计算直接写成上下界约束。第二类是状态约束。姿态角约束最常见我限制横滚和俯仰角在±30度以内防止飞行姿态过激速度约束限制最大飞行速度为2m/s在室内场景尤其重要避免飞机撞墙速度过大。第三类是避障约束。我把障碍物建模成圆柱或球形区域约束写成预测时域内所有位置的解算必须满足‖p_k - p_obs‖ ≥ d_safe。这个约束是不等式非线性约束严格来说会让问题变成非线性规划。我的处理技巧是把每个预测点的约束在当前参考值附近线性化在每一轮求解时用上一轮的解算状态更新约束的线性化点这样问题仍然是二次规划quadprog可以直接解。不过约束不是越多越好。航点导航中如果同时加上姿态约束、速度约束、避障约束和终端约束预测时域稍微长一点优化问题就容易变成无解原因是约束过强导致可行域为空。工程上的妥协是软约束把避障约束从硬约束改成目标函数里的惩罚项只有当飞机靠近障碍物时才产生大的代价这样大部分情况下问题仍然是可行的。我在实现里默认避障走软约束路线只有电机推力和姿态角严格走硬约束。4. Matlab代码实现与关键代码解析4.1 代码结构设计这个项目的Matlab代码我按功能分成了几个文件夹结构清晰后续改参数、换模型都很方便。quad_mpc_navigation/ ├── model/ │ ├── quad_model.m # 四旋翼非线性动力学模型 │ ├── linearize_model.m # 在悬停点线性化并离散化 │ └── quad_params.m # 质量、惯量、推力上限等参数 ├── trajectory/ │ ├── traj_from_waypoints.m # 航点转参考轨迹 │ └── plot_trajectory.m # 轨迹可视化 ├── controller/ │ ├── mpc_build_matrices.m # 构造QP的Hessian和梯度 │ ├── mpc_solve_step.m # 调用quadprog求解下一步控制量 │ └── mpc_constraints.m # 生成约束矩阵 ├── main_sim.m # 主仿真脚本 └── visualize_results.m # 结果可视化主仿真的数据流向是这样的初始化模型参数 → 生成航点参考轨迹 → 进入主循环每步测量当前状态、更新MPC优化问题、求解获取控制量、作用到非线性模型往前推一步 → 记录数据 → 循环直到到达终点 → 可视化。4.2 核心代码逐段拆解线性化这部分我直接用了Matlab符号工具箱来实现避免手推雅可比出错。核心代码syms x y z phi theta psi u v w p q r real syms T tau_phi tau_theta tau_psi real syms m g real syms Jx Jy Jz real % 旋转矩阵ZYX顺序 R angle2dcm([psi theta phi], ZYX); % 注意这里按坐标变换调整 % 平动方程 v_vec [u; v; w]; F_body [0; 0; T]; a [0; 0; -g] R * F_body / m; % 转动方程角速度在机体系 omega [p; q; r]; J diag([Jx Jy Jz]); omega_dot J \ (-cross(omega, J*omega) [tau_phi; tau_theta; tau_psi]); % 姿态运动学 Phi [1 sin(phi)*tan(theta) cos(phi)*tan(theta); 0 cos(phi) -sin(phi); 0 sin(phi)/cos(theta) cos(phi)/cos(theta)]; euler_dot Phi * omega; % 组装全状态导数 f [u; v; w; euler_dot; a; omega_dot];在这段代码里R是从机体系转换到地面系的旋转矩阵注意angle2dcm默认返回的是地面到机体的转换方向不要弄反了不然模型会变成“正反馈”仿真两秒就炸。我建议在写完旋转矩阵后先验证一个简单姿势比如偏航90度时机体X轴应该指向地面系Y轴方向。线性化和离散化% 悬停工作点 x0 zeros(12,1); u0 [m*g; 0; 0; 0]; % 求雅可比矩阵 A_sym jacobian(f, [x y z phi theta psi u v w p q r]); B_sym jacobian(f, [T tau_phi tau_theta tau_psi]); A_num double(subs(A_sym, [x y z phi theta psi u v w p q r T tau_phi tau_theta tau_psi], [x0; u0])); B_num double(subs(B_sym, [x y z phi theta psi u v w p q r T tau_phi tau_theta tau_psi], [x0; u0])); % 离散化 dt 0.1; sys_d c2d(ss(A_num, B_num, eye(12), zeros(12,4)), dt, zoh); Ad sys_d.A; Bd sys_d.B;这里有一个容易被忽略的细节悬停时总推力U0必须等于重力否则模型线性化就不是在平衡点展开后面MPC解出来的控制量在稳态时会有一个恒定偏差。MPC求解部分我用quadprog解决标准的二次规划问题。核心代码如下function u_opt mpc_solve_step(x_current, x_ref_trajectory, params) N params.N_pred; nx params.nx; nu params.nu; % 构造预测矩阵基于Ad, Bd [A_bar, B_bar] build_prediction_matrices(Ad, Bd, N); % 参考轨迹展开 X_ref reshape(x_ref_trajectory, nx*N, 1); % Q和R的块对角权重 Q_bar kron(eye(N), params.Q); R_bar kron(eye(N), params.R); S_bar kron(eye(N-1), params.S); % 决策变量 z [U(0), ..., U(N-1)] H B_bar * Q_bar * B_bar R_bar (B_bar * S_bar * B_bar形式的增量项); f ... % 约束 [Aineq, bineq] build_ineq_constraints(params, N); [Aeq, beq] build_eq_constraints(params, N); options optimoptions(quadprog, Display, off, MaxIterations, 300); z quadprog(H, f, Aineq, bineq, [], [],... lb_repmat, ub_repmat, z_init, options); u_opt z(1:nu); end增量惩罚项的精确构造在代码里稍微繁琐一些思路是先构造一个矩阵D把相邻两步的控制增量表示成D·z其中z是所有控制量的堆叠向量那么增量惩罚就成了‖D·z‖²_S展开后叠加到H和f上。对第一次接触MPC代码的同学我建议先不加入增量项把跟踪和控制量两项跑通再逐步加增量项这样排查问题容易很多。航点切换逻辑放在主循环里代码本身并不复杂function [idx, is_finished] update_waypoint_index(pos, waypoints, idx_current, radius_arrive) dist_to_current norm(pos - waypoints(idx_current, :)); if dist_to_current radius_arrive idx idx_current 1; else idx idx_current; end is_finished (idx size(waypoints, 1)); end但这里有个设计问题如果MPC跟踪的是参考轨迹而参考轨迹是提前生成好的那么航点切换实际发生在轨迹生成阶段而不是控制循环阶段。所以更严谨的写法是先一次性生成从起点经过所有航点到终点的完整参考轨迹主循环里就只跟踪轨迹。除非你要做“动态目标点突然变化”的响应否则不需要在控制循环里做切换判断。4.3 一组直接可用的参数与运行效果我在仿真里用的四旋翼参数可以参考下面这张表不需要真实机型数据作为仿真验证足够参数数值质量 m1.2 kg转动惯量 Jx, Jy, Jz0.008, 0.008, 0.015 kg·m²重力加速度 g9.81 m/s²单个电机最大推力8 N采样时间 dt0.1 s预测时域 N30巡航速度1.5 m/s到达半径0.3 m轨迹生成时我给了四个航点形成一个拐弯的飞行路径。主循环跑200步也就是20秒仿真时间。单步quadprog求解耗时在Matlab里大概是8~15毫秒远小于100毫秒的采样周期说明实时性完全够用就算以后接仿真器做硬件在环也有富余。运行结束看位置曲线飞机会沿着参考轨迹平滑地经过每个航点在转弯处有轻微超调但很快收敛。综合来看这套参数已经能跑出比较理想的航点导航效果。5. 仿真调试与常见问题排查5.1 典型故障现象与对症下药我在这个项目调试过程中把常见问题整理成了一张速查表几乎每条都踩过故障现象可能原因处理方案仿真一开始位置就发散旋转矩阵方向错误用简单姿态验证R是否正确控制量剧烈震荡R或S权重太小增大R和S限制快速变化的控制跟踪误差大且收敛慢Q中位置权重太小增大位置误差权重飞机姿态一直往一个方向偏悬停点推力未设成mg检查线性化工作点航点切换时控制量跳变参考速度不连续使用梯形速度曲线生成参考轨迹位置跟踪稳定但偏航乱转ψ的权重太小或参考偏航突变增加ψ权重让偏航参考平滑避障约束导致无解硬约束过强/预测时域太长改成软约束或缩短预测时域5.2 数值与模型层面的坑有几个坑是Matlab仿真特有的专门拿出来说一说。第一个坑是quadprog要求Hessian矩阵H必须是对称半正定。如果你在权重矩阵构造时不小心让Q或R不对称或者Q中有对角线外的耦合项quadprog会直接报Hessian must be symmetric的错。解决方法是构造完H之后加一行H (H H)/2强制对称同时检查最小特征值是否大于0。第二个坑是状态量尺度差异过大。位置误差动辄几米姿态误差只有零点几弧度如果权重不匹配QP求解时条件数会很差数值上表现为控制量抖动。我在代码里把所有角度状态统一用弧度然后通过权重调整数值尺度必要的时候还可以对状态做归一化处理。第三个坑是线性模型在航点偏离悬停点很远时误差变大。如果航点分布在距离起点几十米的地方飞机在转弯时姿态角可能到20度以上悬停点附近线性化的模型精度会明显下降MPC的预测误差会累积。这时候有两个选择一是沿参考轨迹分段线性化在每个预测点重新线性化模型计算量增加但精度提高二是直接上非线性MPC用fmincon做优化求解。我建议先沿轨迹做分段线性化效果和计算复杂度之间平衡更好。5.3 从仿真走向实机的注意点如果这个项目要迁移到真实飞行器上有几个事情必须提前准备否则仿真里的“成功”在实机上会很狼狈。第一是状态估计。MPC需要完整状态反馈仿真里状态是直接测得的真机上要靠IMU、光流、GPS融合出位置、速度和姿态估计器延迟会直接影响MPC性能。实机调试时建议从高度2米内的悬停开始用MPC控制位置小阶跃响应确认估计值与飞控内部数据一致后再飞航点。第二是执行器延迟与响应模型。电机转速不是瞬时达到的有响应延迟仿真里U_k直接作用到模型上其实比较理想。我在仿真模型里加了一阶惯性环节模拟电机响应滞后这样MPC预测结果更接近真实代价是模型阶数增加QP求解会出现微小变化。第三是MPC每个周期求解时间的抖动。quadprog的求解时间在极端情况下可能超过采样周期真机上一定要设定硬超时保护超时就沿用上一拍的控制量并触发警告。实测里我发现求解时间波动主要来自QP的迭代次数不一样可以通过设置MaxIterations和OptimalityTolerance来控制最坏情况耗时。第四是参数辨识。Q矩阵和R矩阵的权重虽然在仿真里可以反复试但实机上最好从飞控已有渠道获得电机的推力曲线、转动惯量参数甚至做一次系统辨识得到输入输出模型。模型越贴近真实MPC的预测优势才越能发挥出来。这段项目做完之后我自己最大的一个体会是MPC难的不是优化原理而是模型和约束的工程化表达。模型里一个小数点符号的错误密度上可能远大于控制器参数调不到理想效果的影响。所以如果你也在跑类似的仿真项目请务必先花时间把动力学模型验证好再做控制器否则调试效率会很低。另一个经验是参考轨迹的生成质量直接决定了MPC的最终表现轨迹速度一点不光滑MPC怎么调权重都会抖轨迹参考先调平滑了MPC的权重反而好配很多。