★ FEATURED ARTICLE 四旋翼的3D轨迹跟踪说难不难说简单也绝不简单。圈子里聊得最多的方案MPC-LPV模型预测控制绝对算一个尤其是当你想在Matlab里把姿态大范围变化下的三维轨迹稳稳跟住时这套组合比传统PID和单一工作点线性MPC都靠谱。这篇文章我打算从模型搭建、预测模型处理、QP求解到调参踩坑完整拆一遍我的实现过程和经验。1. 为什么最终选MPC-LPV这套组合从一次失败的机动飞行说起1.1 传统PID在3D轨迹跟踪上的“三道坎”大概半年多前某无人机控制项目把四旋翼的3D轨迹跟踪任务交到我手上。当时项目里飞控PID那一套已经用了很久日常的小范围悬停和慢速巡航都没问题但一要求按复杂三维航迹做大机动时姿态就开始拉不住位置误差也越滚越大。第一道坎是没有前瞻性。PID本质上是误差驱动轨迹还没偏离时它不知道前方有弯道等误差出来了再去追大机动下这个滞后非常要命。第二道坎是约束处理粗糙。执行器有饱和、姿态角有安全限制PID的做法是加限幅、加积分分离但这都是被动的一旦多个约束同时生效整条环路就可能失稳。第三道坎是工作点单一。常规PID参数在某个小姿态范围内调好了换个大坡度的航迹线性化模型失效增益就不合适了表现就是姿态抖动、轨迹发散。我当时做了一次测试让飞机按一条空间折线飞行转弯处要求45度滚转结果PID控制器给出的期望姿态已经超出物理允许范围执行器饱和后高度掉了好几米轨迹跟踪误差直接爆表。这件事让我下定决心换控制架构。1.2 MPC的优势和短板有远见的控制器也需要好模型模型预测控制MPC的思路完全不同。它在每个控制周期里基于当前状态和系统模型向前预测未来一段时间预测时域内的状态变化然后求解一个带约束的最优控制问题得到一串最优控制序列但只执行第一个控制量下一个周期重新优化。这就是“滚动优化反馈校正”。这个机制天然适合轨迹跟踪它能提前知道未来轨迹的走向能显式处理输入饱和、姿态限制这类约束而且滚动优化让控制器对模型误差和环境扰动有一定的自适应能力。在Matlab里做仿真验证MPC对复杂三维轨迹的适应性明显好于PID这一点我当时跑仿真就确认了。但MPC有个众所周知的问题预测模型不准一切都白搭。如果模型只是一个固定工作点附近的线性化模型那么一旦飞行姿态偏离这个工作点太远预测误差就会迅速增大控制器算出来的“最优”控制量也不再可靠。这就是单纯线性MPC在大包线机动时的死穴。1.3 LPV补的正是MPC最需要的那块拼图线性参数变化LPV系统给我的感觉就像是把无数个线性模型塞进了一个“参数可调”的壳子里。它不要求在全局用一个固定线性模型而是把非线性特性表示为对一组可测变量调度变量的依赖系统矩阵A(ρ)、B(ρ)会随着调度变量实时变化。对四旋翼来说姿态角、角速度这些变量的变化正好决定了系统的非线性程度。把MPC的预测模型换成LPV形式相当于让MPC在不同飞行状态下“换挡”——在大姿态角时模型自动调整而不是继续用悬停点的小扰动模型硬撑。把这套东西在Matlab里落地就是我最终选型MPC-LPV的直接原因。2. 模型搭建四旋翼非线性方程到调度变量可测的LPV形式2.1 状态和输入定义先统一坐标系和单位LPV建模的第一步不是写矩阵而是把坐标系、状态量定义和单位统一好。我习惯用惯性坐标系下的位置和速度加机体坐标系下的姿态角和角速度组一个12维状态向量位置p [x, y, z]单位米惯性系。速度v [vx, vy, vz]单位米/秒惯性系。姿态角Φ [φ, θ, ψ]分别对应横滚角、俯仰角、偏航角单位弧度。这里用的是欧拉角表示。角速度ω [p, q, r]单位弧度/秒机体坐标系。控制输入取4个总推力T单位牛沿机体Z轴方向和三轴力矩M [Mx, My, Mz]单位牛·米。注意欧拉角表示在俯仰角接近±90度时存在奇异性。一般轨迹跟踪控制我们都会限制姿态角范围所以用欧拉角做仿真足够但如果要做全姿态特技飞行就得换四元数或旋转矩阵表示LPV的调度变量选取也要跟着变。2.2 非线性动力学方程中哪些项必须“藏”进调度变量四旋翼的位置动力学方程可以写成m * \dot{v} R(φ, θ, ψ) * [0, 0, T]^T - [0, 0, mg]^T d其中m是质量g是重力加速度R(φ, θ, ψ)是从机体坐标系到惯性坐标系的旋转矩阵d是外部扰动。旋转矩阵展开后是一堆三角函数cosθcosψ、cosθsinψ、-sinθ、sinφsinθcosψ - cosφsinψ之类。这些三角函数就是最核心的非线性来源。姿态动力学方程是J * \dot{ω} -ω × (J * ω) M其中J是转动惯量矩阵右侧第一项是陀螺效应项包含角速度之间的乘积耦合。这一项在低角速度时可以忽略但大机动飞行时角速度高它会让姿态响应出现明显耦合。LPV化的思路就是把这些非线性项看成“随工作点变化”的参数。比如旋转矩阵中的角度的三角函数以及陀螺效应项中的角速度乘积都可以作为调度变量ρ的一部分。这样一来非线性系统就被写成了一个形式上线性、但系数依赖ρ的系统\dot{x} A(ρ) * x B(ρ) * u但这只是思路落实到矩阵上有几种不同的做法。2.3 最常用的三种LPV化处理方式第一种叫直接代数布置法。不把原始非线性方程线性化而是把非线性项原样保留在矩阵里但这些项的值由当前调度变量计算。例如旋转矩阵中的sinφ、cosθ这些量在每一控制周期直接根据当前姿态角算出数值填入矩阵。这样在调度变量的每个取值点上系统矩阵都是精确的没有线性化误差。第二种是工作点线性化加插值法。选取多个典型工作点比如俯仰角0度、15度、30度在每个工作点做雅可比线性化得到一组LTI模型。然后根据当前调度变量在多个模型之间线性插值或加权。这个方法的优势是模型个数可控方便嵌入到现有MPC框架里缺点是在工作点之间的区域会有一定近似误差。第三种是状态依赖的SDRE风格。把系统写成A(x)x B(x)u的形式把“伪线性化”后的矩阵作为LPV模型。这个方法形式统一但要注意A(x)的选取不唯一不合理的分解可能让模型失真。2.4 调度变量怎么选可测性、覆盖范围和频率约束调度变量的选取是整个LPV建模里最需要经验的环节。我在仿真里主要取姿态角φ、θ和偏航角ψ或其三角函数组合作为调度变量理由很简单它们是系统非线性特性的主要来源而且通过惯性测量单元可以直接获得满足“可测”这个硬性要求。还要注意三个原则覆盖范围要够。调度变量的变化空间必须覆盖整个设计飞行包线。如果你的轨迹要求滚转角能到40度那么LPV模型里的调度范围至少要到40度留10%余量更好。避免高频抖动。调度变量如果直接用原始测量值噪声大时LPV模型参数会剧烈波动导致MPC预测模型不断跳变。我在仿真中会对调度变量做一阶低通滤波比如取τ0.1s的时间常数效果明显。变量数量别贪多。每个调度变量都意味着建模复杂度和计算量的增加而且会稀释每个变量的影响。对四旋翼轨迹跟踪这个场景取欧拉角为主调度变量已经足够角速度耦合项可以直接作为时变参数处理不必全部纳入调度向量。3. MPC控制器设计预测模型离散化、QP问题构建和求解3.1 冻结时刻的离散预测模型怎么写把LPV模型应用到MPC里最直接也最常用的方式是“冻结法”。在每个控制周期k测量当前状态x(k)读取当前调度变量ρ(k)代入LPV模型计算出A(ρ)、B(ρ)然后在预测时域内假设这个模型保持不变将连续模型离散化x_d(k1) A_d * x_d(k) B_d * u_d(k)离散化我用的是零阶保持假设即在每个控制周期内控制量保持不变。一阶近似可以直接用 A_d I Ts * A精确一点用矩阵指数A_d expm(A * Ts)。在Matlab里用expm很方便但要注意仿真中如果Ts较大一阶近似误差会积累建议优先用expm。冻结法之所以好用是因为它在每个瞬间都把MPC的在线优化问题变成了一个标准的线性二次规划问题求解速度快、稳定性好。它的代价是预测时域内如果调度变量变化很快模型会有一定的失配。所以调度变量的变化率要作为设计指标去观察如果发现模型失配导致控制效果变差可以考虑在预测时域内做调度变量的外推但这属于进阶玩法了。3.2 目标函数用“误差平方增量惩罚”而不是单纯误差平方MPC的目标函数也就是代价函数我强烈建议用下面这种形式J Σ ||y(ki) - y_ref(ki)||^2_ Q Σ ||Δu(kj)||^2_ R其中第一项是预测输出与参考轨迹的误差惩罚第二项是控制增量的惩罚。注意是控制增量不是控制量本身。为什么用增量而不是控制量这里有三个实操层面的原因。第一增量惩罚相当于给控制器加了一个隐形的积分环节可以消除稳态误差。第二它天然抑制高频抖振因为控制器不允许控制量变化太快。第三它让约束设置更合理因为执行器关心的恰恰是“下一时刻推力和力矩变化多少”而不是绝对大小。权重矩阵Q和R的初始值我用的是位置误差权重Q_pos diag([8, 8, 8])速度误差权重Q_vel diag([2, 2, 2])姿态误差权重Q_att diag([1, 1, 1])R diag([0.05, 0.05, 0.05, 0.05])。这个比例给位置误差最高优先级符合轨迹跟踪的最终目标。具体怎么调我后面专门用一节讲。3.3 约束设置哪些约束真的有用哪些约束让求解器崩溃MPC最大的卖点之一就是能显式约束但约束不是越多越好。我总结了几类约束的实际作用输入约束是最基本的。总推力T不能为负因为四旋翼的旋翼只能向上产生推力上限由电机和旋翼性能决定。三轴力矩M也有饱和范围。这些约束直接写在QP的lb和ub里就行。状态约束里最有价值的是姿态角约束。比如限制||φ| ≤ 30度、||θ| ≤ 30度这样能保证LPV模型和欧拉角表示都处于安全范围内。位置约束也可以加比如飞行区域边界。但状态约束的代价是预测时域内每一步都要检查计算量会上去。还有一个常见的坑如果初始状态违反了约束或者参考轨迹突然跳变到约束边界之外QP会直接无解。quadprog报错的现场让人头疼的不行。我的处理方案是引入松弛变量把硬约束变成软约束约束可以轻微违反但违反量会在代价函数里被惩罚。这样做的基本逻辑是保证控制器在任何情况下都能给出一个可行解而不是让整个系统宕机。3.4 用quadprog求解标准QP矩阵怎么拼出来Matlab里求解MPC的QP问题我用的是自带的quadprog函数而不是MPC工具箱。理由有三个一是手写QP让我对每个矩阵的含义都一清二楚调参时更容易定位问题二是代码结构简单方便移植到别的环境三是quadprog用的是内点法和活动集法对于中小规模的预测控制问题性能足够。标准MPC-QP形式是这样的min_u 0.5 * U * H * U f * Usubject to A_ineq * U ≤ b_ineq, lb ≤ U ≤ ub其中U是未来控制增量序列的向量。构造H和f的过程本质上是把预测方程代入代价函数后展开。这一步在代码里会比较长但逻辑很固定先构建预测矩阵Phi把状态递推展开成初始状态和控制序列的线性组合然后H Phi * Q * Phi R_barf -2 * (Y_ref * Q * Phi)最后把所有约束写成A_ineq和b_ineq的形式。一个容易出错的细节H矩阵必须是半正定的否则quadprog会报错。这个只要保证Q半正定、R正定结构上就成立。另外如果状态量中位置和角度量纲差异太大比如位置是几十米级别姿态角是零点几弧度级别H的条件数会非常大求解精度下降。这个坑我后面还会再讲。4. Matlab仿真实现主循环框架、参考轨迹与参数表4.1 仿真主循环状态反馈、模型冻结、优化、作用、积分整个仿真最核心的就是主循环我用Matlab脚本实现结构清晰方便逐段调试初始化参数和参考轨迹。从初始状态开始进入for循环。测量当前状态仿真中就是从状态向量直接读。根据当前调度变量计算A(ρ)、B(ρ)离散化得到A_d、B_d。构建预测矩阵Phi组装H、f、约束矩阵。调用quadprog求解得到最优控制增量序列。取第一个控制增量累加到当前控制量上作为实际作用量。用四旋翼非线性动力学模型就是最原始的带三角函数模型不是LPV简化模型进行数值积分得到下一时刻状态。把状态更新回循环变量记录历史数据继续下一步。这里有个关键认知预测模型和控制对象模型是两回事。预测模型用LPV简化式是为了在线求解效率而被控对象仿真用的是完整的非线性方程这样才能验证LPV建模和控制器的真实效果。否则等于拿自己的简化模型去验证自己的简化模型结果没有说服力。4.2 参考轨迹设计如何构造一条能真正考验控制器的三维轨迹参考轨迹的设计直接影响调试效果。第一条轨迹我建议用简单的空间直线让系统先跑通确认基本功能正常。之后再切换到复合轨迹比如我用的组合螺旋上升段加空间折线转弯段最后回到起始点高度。螺旋上升段考验持续的大角度姿态变化下的跟踪能力空间折线转弯段考验轨迹突变时的动态响应和约束处理能力。参考轨迹的生成要注意一点航点之间的切换必须平滑最好用三次样条或正弦过渡不要用阶跃式切换。阶跃参考会让MPC从第一步就处于约束边界看起来像控制器性能差其实是参考轨迹设计的问题。时间参数也重要。我设的总仿真时间为20秒螺旋段从2秒开始到12秒结束折线转弯段从12秒到18秒最后2秒回到悬停。速度参考取1.5米/秒左右加速度不超过1米/秒²这样既不算太激烈又能看出控制器的跟踪性能差异。4.3 参数表一版能直接跑起来的初始配置下面是让我第一次成功跑通全过程的参数配置适合作为起点。不同对象要根据实际情况微调但数量级和比例很有参考价值。参数符号数值说明质量m1.2 kg四旋翼总质量重力加速度g9.81 m/s²常量转动惯量Jxx/Jyy/Jzz[0.012, 0.012, 0.022] kg·m²常见小型四旋翼量级采样周期Ts0.05 s控制周期20Hz预测时域Np20预测未来1秒控制时域Nc5前5步优化位置权重Q_posdiag([8,8,8])位置误差惩罚速度权重Q_veldiag([2,2,2])速度误差惩罚姿态权重Q_attdiag([1,1,1])姿态误差惩罚控制增量权重Rdiag([0.05,0.05,0.05,0.05])抑制抖振推力约束T_min / T_max[5, 20] N悬停推力约12N力矩约束M_max0.8 N·m三轴力矩上限姿态约束φ/θ限制±30 度限制预测时域内姿态仿真积分步长我取0.001秒远小于控制周期避免数值积分误差干扰控制效果评估。如果你用ode45需要设置合适的事件函数或固定步长我直接用for循环加欧拉积分反而更可控。4.4 核心代码骨架下面给出主循环的核心代码骨架方便对照。省略了矩阵构造的完整展开但结构是完整的% 参数初始化 Ts 0.05; Np 20; Nc 5; state zeros(12, 1); % [x,y,z,vx,vy,vz,phi,theta,psi,p,q,r] state(3) 1.0; % 初始高度1米 ref_traj generateRefTraj(t_total, Ts); % 生成参考轨迹 U_prev [12; 0; 0; 0]; % 上一时刻控制量 for k 1:N_steps % 1. 读取当前状态 x_now state(:, k); rho_now x_now(7:9); % 欧拉角作为调度变量 % 2. 计算LPV矩阵并离散化 [A, B] buildLPVModel(x_now, rho_now, params); [Ad, Bd] discretizeZOH(A, B, Ts); % 3. 构建预测模型矩阵Phi [Phi, Psi] buildPredictionMatrix(Ad, Bd, Np, Nc); % 4. 组装QP矩阵 [H, f] assembleQP(Phi, Q, R, ref_traj(:, k:kNp), x_now, U_prev); % 5. 约束矩阵 [Aineq, bineq, lb, ub] buildConstraints(Nc, params, U_prev); % 6. 求解QP [dU_opt, fval, exitflag] quadprog(H, f, Aineq, bineq, [], [], lb, ub); % 7. 取第一个控制增量并更新控制量 U_now U_prev dU_opt(1:4); U_prev U_now; % 8. 用真实非线性模型积分一步 state(:, k1) nonlinearDroneStep(state(:, k), U_now, params, dt_sim); end这段代码是骨架但每一步都对应真实可实现的函数。我在首次调通后把矩阵构造部分也整理成了独立的函数方便后面换模型、换约束条件时复用。5. 调参与坑点记录从震荡发散到平滑跟踪的过程5.1 高频抖振增量惩罚和R权重怎么调第一次完整跑通时我的轨迹跟踪效果并不理想最明显的问题是高频抖振。控制量在很窄的范围内来回摆动虽然轨迹误差不算大但这样的控制信号根本不能用于实际执行器。我排查时首先想到的就是R权重。R太小时代价函数里控制增量惩罚过低控制器会倾向于用大幅度的控制变化去压缩误差导致抖振。把R从0.01逐步调到0.05再把增量权重单独加强比如在增量惩罚矩阵里乘以3倍抖振幅度明显下降。这里有个判断经验如果你发现控制量抖振先看R矩阵如果轨迹跟踪误差增大但控制量平滑再看Q矩阵。两者是跷跷板关系调参时要记住一个目标在误差可接受范围内让控制量尽量平滑这比追求极限精度更重要。5.2 QP不可行松弛变量的引入时机仿真跑到折线转弯段时quadprog好几次返回了“问题不可行”的报错。第一次遇到时我以为是参数问题反复调R和Q都没用后来打了断点看约束矩阵和当前状态才发现是参考轨迹的转折太急预测时域内有些状态组合超出了约束范围。解决方式是引入松弛变量s到约束里原来要求每个状态严格在约束内现在改成允许超出一点但超出的量会在代价函数里被惩罚。公式上相当于把A_ineq * U ≤ b_ineq变成A_ineq * U ≤ b_ineq s同时在目标函数里加W_s * ||s||²。这个改动看起来轻巧但实际意义很大MPC从“要么有解要么破裂”变成“永远有解最优解退化为轻微违约”。仿真中加了松弛变量之后quadprog再也没有报过不可行。建议你在一开始就把松弛变量设计进去不要等遇到问题再改。5.3 调度变量突变导致控制量跳变低通滤波与受限变化率还有一个很容易被忽视的问题调度变量的剧烈变化会让LPV模型矩阵在相邻控制周期之间跳变进而导致MPC求解出的控制量跳变。我当时在快速转弯段抓到了这个现象控制量在一个采样周期内跳了将近20%。根因是欧拉角在被控对象里变化太快而冻结法把模型矩阵在高频下切换。解决方式有两层一层是对调度变量做低通滤波让ρ平滑变化另一层是在LPV模型计算时对矩阵元素本身做限幅限制相邻周期内A(ρ)、B(ρ)的最大变化量。低通滤波的时间常数我取了0.1秒和采样周期0.05秒配合既没有明显相位滞后又消除了模型跳变。限幅方面设置矩阵元素每步变化不超过其变化范围的10%实测有效。5.4 量纲差异带来的数值病态问题这是所有MPC实现里最隐蔽的一个坑。四旋翼状态里位置可能是几米速度是每秒几米而姿态角是零点几弧度角速度是每秒零点几弧度。这些量级差了一到两个数量级放进同一个代价函数后H矩阵的条件数会变得很大。条件数大意味着quadprog对输入数据的微小扰动极其敏感仿真中表现为同样的参数下偶尔一次求解结果异常导致轨迹出现毛刺。我试过两个解决方案。第一个是对状态量做归一化把位置除以10、速度除以5、角度除以0.5让所有状态量都在0.1到1的范围内。第二个是调整Q矩阵时有意加大姿态项的权重将姿态误差的影响拉到位移误差的同一量级。最后我用的是归一化加权重调整的组合H矩阵的条件数从10^7量级降到了10^4量级求解稳定性明显改善。5.5 初始状态与参考轨迹不匹配的处理刚起步时还有一个不起眼但影响全局的问题初始状态是悬停状态而参考轨迹的起点在另一个位置两者之间存在较大偏差。MPC在第一个控制周期就会拼命试图消除这个偏差导致控制量饱和后续好几个周期都在恢复。解法是让参考轨迹的起点从当前状态开始前1到2秒做一个平滑过渡。具体做法是在参考轨迹生成时把前几个时刻的点插值到当前位置和目标轨迹的起点之间让控制器有时间调整状态而不是一上来就极限操作。6. 从仿真走向实物还差哪几步6.1 计算效率Np、Nc、采样周期的取舍仿真跑通之后我认真考虑了如果把它搬到真机上会遇到的现实问题首当其冲就是计算效率。在Matlab里Np20、Nc5的QPquadprog求解一次大约需要几十毫秒和控制周期50毫秒相比还勉强够用。但真机的计算资源远不如桌面PC而且CPU要同时处理状态估计、通信等任务不可能把全部算力都给MPC。我自己的经验底线是Np可以从20降到10Nc从5降到3采样周期从0.05秒放宽到0.02秒控制器要更快不是更慢。这样QP规模会小很多再用专用的嵌入式QP求解器比如带热启动的快速内点法或活动集法才能在硬件上达到实时。另外建议在仿真阶段就把求解时间作为指标记录下来提前评估硬件余量不要等到真机测试才发现算力不够。6.2 状态估计、执行器延迟和风扰的建模仿真中状态是直接从模型里取的不存在估计误差但真机上必须依赖惯性测量单元和视觉/雷达等传感器融合才能得到位置和姿态。这些估计值带有延迟和噪声而LPV调度变量又是从这些状态中提取的噪声会直接影响模型参数的准确性。我的建议是仿真阶段就把这些因素加进去在状态反馈路径上加入一阶低通滤波模拟传感器延迟在状态量上叠加高斯白噪声模拟测量噪声。这样LPV-MPC控制器设计时就会考虑到观测误差的影响而不是默认“状态完美”。执行器延迟也是仿真里容易忽略的一环。四旋翼的电机和桨叶响应不是瞬时的通常有一个几十到一百毫秒的滞后。MPC预测模型里如果不包含这个滞后相当于控制器“过度自信”实际效果会打折扣。入门做法是在控制输入通道上串联一个一阶惯性环节延迟常数按实际执行器参数设置。6.3 我的建议仿真阶段就应该做的几件事如果让我重新做一遍这个项目我会在仿真阶段就做三件之前偷懒没做的事。第一从第一天就把扰动加入仿真。风扰是最常见的扰动源最简洁的建模是给位置动力学加一个随时间变化的有界加速度。没有扰动时控制器看起来性能完美但在扰动下权重设置和松弛变量参数的脆弱性会立刻暴露出来。第二把求解失败的情况记录下来并回放。不要只看轨迹曲线要记录每个控制周期quadprog的exitflag、迭代次数、目标函数值。曾经有一个看起来正常的仿真回放后发现有连续好几个周期QP返回了次优解原因是约束矩阵条件数偏大这些信息只看轨迹是发现不了的。第三给调度变量的时间曲线画图观察。调度变量是否平滑、是否超出设计范围、与模型矩阵的变化是否匹配这些问题通过曲线一目了然。我在调通整个系统后回头看很多“玄学”问题其实都是调度变量处理不当造成的如果早期就关注这条曲线能省一半调试时间。每一步踩坑、每一次回到代码里修正逻辑本质都是对模型和控制器之间关系的理解在加深。MPC-LPV不是一套能拿来就用的现成玩具它需要你从模型层面理解“预测为什么准”从求解层面理解“优化为什么稳”再把这两层用一套经得起验证的代码串起来。能把这里面的逻辑理通后面换机型、换轨迹、换约束条件都只是参数和矩阵维度的变动而已。
阅读完成 · 觉得有帮助？
