企业官网建设流程全解析

我一直觉得四旋翼轨迹跟踪这类题目，最能拉开差距的不是控制器写得花不花哨，而是“你有没有把控制问题想清楚”。前阵子把一套仿真从PID切到MPC模型预测控制，第一眼看到跟踪螺旋轨迹的效果，确实比我想象中好，尤其是高速转弯那段，MPC会提前减速并规划好姿态，而不是等误差出来了再去“救火”。这篇文章就围绕“四旋翼飞行器UAV_MPC模型预测轨迹跟踪控制研究（Matlab代码实现）”这个主题，把我从建模、控制器设计、代码实现到调参踩坑的完整过程写了一遍。内容适合正在做课程设计、毕业课题，或者想从PID/LQR往MPC方向过渡的工程师，我会尽量把每个“为什么这么做”讲透，不搞只有公式没有逻辑的堆砌。

1. 为什么轨迹跟踪这题，MPC比PID和LQR更适合当主角

先说个这些年做控制仿真的体会：轨迹跟踪难不在“稳定”，而在“既要跟得快、又要跟得稳、还不想突破执行器极限”。PID是把当前误差放大成控制量，天然是反应式的，等误差出来了才动作，遇到高速变化参考时总有一股说不出来的滞后感。LQR则是基于无限时域的线性最优控制，能处理多变量耦合，但要把输入饱和、姿态限制这类约束优雅地写进去，非常麻烦。MPC的核心差异在于它每次控制都在“向前看一段未来”，用模型预测未来行为，再在线求解一段最优控制序列，最后只执行第一步。这个机制决定了它更适合轨迹跟踪这种带前瞻性的任务。

1.1 轨迹跟踪的本质：不是捏住误差，而是提前规划未来运动

看一个直观的例子：四旋翼要沿一条圆形轨迹飞行，速度恒定但切线方向不断变化。PID控制下，横滚、俯仰角指令通常基于位置误差和速度误差生成，只有在系统已经偏离参考轨迹之后，控制器才会生成对应的倾斜角修正，这个“滞后”在高频转弯区域会被放大成可见的切向误差。MPC不一样，它在当前时刻就知道未来几秒钟的参考轨迹是一条圆弧，于是可以在进入弯道之前就开始分配横向加速度，像司机看到弯道会提前打方向盘一样。

另一个点：轨迹不只是一串位置点，它往往还伴随期望速度、期望加速度。MPC的预测模型可以把位置误差、速度误差、甚至加速度前馈统一放进代价函数里去权衡，这在PID里通常需要额外设计前馈通道，否则跟踪带宽上不去。所以轨迹跟踪这个场景，本质上是“带预见性的最优控制问题”，MPC天生就站在这个问题的正解位置上。

1.2 PID/LQR在四旋翼上的两个短板：约束问题和前瞻问题

先说约束。四旋翼有一个很现实的上限：油门不能无限大，姿态角不能随便倾斜到90度，电机的角加速度也有限制。PID在设计时如果想不让执行器饱和，要么把增益调得很保守，要么在外面加一堆限幅逻辑。限幅看起来简单，但限幅后的闭环行为实际上是不可分析的，工程上经常出现“仿真无事发生，实飞一限幅就抖”的情况。LQR本身是无约束最优控制，设计时根本不考虑输入饱和，如果强行把输出压到饱和边界，线性最优的假设就被破坏了。

再说前瞻。LQR虽然能给出状态反馈矩阵，但它假设的是无限时域，也就是“今天和明天的控制策略完全一样”，可轨迹跟踪的参考是时变的，天然不满足这个前提。虽然可以用LQR加前馈、加轨迹线性化来凑，但每一次参考变化都要重新计算增益，复杂度一点不低。相比之下，MPC把“当前时刻往前看Np步”写成一个有限时域优化问题，参考轨迹可以直接以序列形式塞进去，前瞻能力是内置的，不是靠额外模块补出来的。

1.3 四旋翼MPC的两种主流路线：全量NMPC与级联线性MPC

刚接触MPC的人很容易一头扎进全量非线性模型预测控制（NMPC），也就是把位置、速度、姿态角、角速度一共12个状态全部放进预测模型，控制量直接就是四个电机的推力或者期望力矩。这种路线理论最完整，但有两个现实问题：第一，每个控制周期要求解一个非线性优化问题，计算量明显高；第二，姿态动力学和位置动力学时间尺度差了一个量级，放在同一个优化问题里，数值刚度会让求解器非常痛苦。

工程里我更推荐另一条路线：级联MPC。外环用一个线性/轻度非线性MPC做位置控制，输出期望加速度；内环用姿态控制器去跟踪期望姿态角，姿态环可以用PID，也可以再用一层MPC。四旋翼本身的物理特性决定了姿态响应比位置响应快得多，这种级联结构完全符合系统的时间尺度分离特性，而且外环MPC只需要用到位置、速度状态，模型简单、求解快，Matlab实现起来特别顺手。后面我讲的整套代码就是基于这条路线，先把外环位置MPC跑通，再考虑要不要升级到NMPC。

2. 先把模型放到台面上：从牛顿-欧拉方程到能进MPC的简化模型

MPC是一个“模型越准，控制越好”的算法，但这里的“准”不是越复杂越好，而是“预测模型要抓住主导动态，同时计算量可控”。四旋翼全动力学模型虽然完整，直接塞进MPC里往往不是最优选择。先说完整的动力学长什么样，再说怎么简化，这样你能理解为什么最终代码里用的是那个简化版本。

2.1 从牛顿-欧拉方程到12维状态：每一步的物理含义

四旋翼的常见状态取12维：位置3个分量、速度3个分量、姿态角3个分量（或者四元数4个分量）、角速度3个分量。如果设位置为p = [x,y,z]^T，速度为v = [vx,vy,vz]^T，姿态角为Φ = [φ,θ,ψ]^T，角速度为ω = [p,q,r]^T，那么刚体运动学方程可以写成：

p_dot = v v_dot = g * e3 + (R(Φ) * [0;0;T]) / m Φ_dot = W(Φ) * ω J * ω_dot = τ - ω × (J * ω)

这里的R(Φ)是从机体坐标系到世界坐标系的旋转矩阵，e3是竖直方向的单位向量，T是总推力，τ是三个轴的力矩，W(Φ)是欧拉角速率与机体角速度之间的变换矩阵，包含了欧拉角的奇异项。每条方程的物理含义都很直接：位置微分是速度；速度微分由重力、推力、空气阻力共同决定；姿态角微分由角速度通过变换矩阵得到；角速度微分由力矩、转动惯量和陀螺力矩共同决定。

2.2 为什么MPC里常用“平动双积分近似+姿态响应一阶近似”

如果你把上面12个状态全部作为预测模型，每个控制周期里要同时优化推力、三个力矩，变量数量多，姿态块的强非线性还会让优化时间变长。实际中四旋翼的姿态内环可以做得很强，典型姿态闭环带宽可以做到位置外环带宽的5到10倍以上，所以位置MPC完全可以“假装”姿态已经稳定，只把期望加速度作为控制量，再通过一个一阶惯性环节近似姿态响应。

我的做法是：外环MPC状态只取[x,y,z,vx,vy,vz]和上一时刻的期望加速度（或姿态角），控制量u = [ax_cmd, ay_cmd, az_cmd]^T是期望加速度。在预测模型里，位置和速度的更新用双积分：

p(k+1) = p(k) + Ts * v(k) v(k+1) = v(k) + Ts * (u(k) - g * e3)

然后再用一个姿态响应近似，把u对应的实际加速度延迟一阶：

a_actual(k+1) = alpha * a_actual(k) + (1 - alpha) * u(k)

alpha由姿态环闭环时间常数决定，一般取 0.8到0.95之间。这个简化不是拍脑袋，它的意义在于：把“推力大小”和“姿态方向”这两个物理量从预测模型里抽离成加速度层的问题，MPC只负责回答“我这Np步要怎样的加速度”，至于“多大油门、什么角度”交给内环去算，职责分离。

2.3 离散化处理：零阶保持与RK4的选择

MPC是数字控制，连续方程必须离散化。最常用的组合是：预测模型用相对简单的欧拉离散或零阶保持离散，真实仿真模型用四阶龙格库塔（RK4）。这样故意制造一点模型失配，反而能检验控制器的鲁棒性。采样时间Ts我一般取0.02~0.05秒，对应控制频率20~50Hz。如果你用fmincon做非线性优化，Ts=0.02、预测时域Np=30意味着每个周期要计算60步的预测轨迹，计算量需要提前评估；速度要求不高时可以把Ts放到0.05，预测时域缩短到20步。

3. MPC控制器的三块拼图：代价函数、约束和滚动优化怎么搭配

所有MPC都绕不开三件事：怎么定义“好坏”、哪些量可以动、哪些量不能越界、以及怎么让这个优化在线滚动起来。这三个设计点直接决定跟踪精度和执行器损耗之间的平衡。

3.1 代价函数怎么写：输出误差、输入变化量与参考权重之间的权衡

我用的代价函数是这一形式：

J = sum_{i=1}^{Np} (e_i^T * Q * e_i) + sum_{i=1}^{Nc} (u_i^T * R * u_i + du_i^T * Rd * du_i)

其中e_i = [p_pred(i)-p_ref(i); v_pred(i)-v_ref(i)]是预测状态与参考轨迹在预测时域内的误差，u_i是期望加速度，du_i = u_i - u_{i-1}是控制增量。Q的维度跟状态一致，R、Rd对应控制量和控制增量，都是对角矩阵。

权重矩阵的调法有规律可循。位置误差项的权重Q_pos决定跟踪精度，小了会看到轨迹偏离，大了会看到加速过猛；速度误差的权重Q_vel我一般给位置权重的一半左右，太大容易产生超调；R用来避免控制量过大，但对于轨迹跟踪，R不能给得太大，否则会出现“为了省力而不去追误差”的情况；Rd才是抑制抖振的关键，它管的是控制量变化得快不快，适当增大Rd比增大Q更有效。我常用的初始值是Q_pos = diag([1,1,3])，Q_vel = diag([0.5,0.5,1])，R = 0.01*I，Rd = 0.1*I，然后根据仿真现象再调。

3.2 约束都约束在哪儿：油门饱和、姿态角限制与轨迹偏差走廊

MPC的约束分为输入约束和状态约束两类。输入约束最直接：期望加速度的水平分量不能超过由最大倾斜角决定的上限，竖直分量要落在油门可提供的范围内。如果最大允许倾斜角是30度，那么水平加速度上限大约是g*tan(30°) ≈ 5.67 m/s²，这个值可以直接写成线性不等式约束：

-5.67 <= a_xy <= 5.67 a_z_min <= a_z <= a_z_max

状态约束也是可以加的，比如限定高度不能低于某值、不能高于某值，或者给位置误差一个走廊范围。注意一点：硬的输出约束在轨迹比较复杂时经常导致优化问题不可行，因为预测轨迹可能来不及在约束内转弯。我的处理方法是把这些状态约束做成软约束，也就是在代价函数里加一个惩罚项：

J_soft = J + rho * epsilon^2

epsilon是允许约束违反的松弛变量，rho是惩罚系数。这样求解器永远不会因为“预测越限”而直接罢工，但越得多、代价就越高，控制器会尽量避开受限区域，实际调试体验比硬约束好得多。

3.3 滚动优化的实现流程：预测、求解、只执行第一步

在每个控制周期k，完整的滚动优化步骤如下：

1. 获取当前状态x_k，既可以是仿真里的真值，也可以是估计状态。
2. 截取参考轨迹从k到k+Np-1的序列，包括位置参考和速度参考。
3. 将初始状态代入预测模型，把决策变量u_k,...,u_{k+Nc-1}映射成预测位置序列。
4. 求解带约束的优化问题，得到最优控制序列。
5. 只将第一个控制量u_k发给内环控制器。
6. 到下一个采样时刻，状态更新后重复第1步。

这里最反直觉的一点是：明明算出来Np步的控制量，为什么只执行第一步？因为模型不完美、外部有扰动，预测轨迹一定会逐渐偏离真实状态。如果傻乎乎把Np步全部执行完，误差会越来越大。每步滚动、每步重新优化的过程相当于一个“不断修正的闭环规划器”，这也是MPC被称为“反馈控制”的原因——MPC不是开环最优控制，而是反馈最优控制。

3.4 Matlab求解器选型：quadprog、fmincon还是MPC Toolbox

在Matlab里写MPC，你面对的求解器选择直接影响代码写法和运行速度：

我的建议是：如果只是入门或者做课程验证，先用quadprog跑通“线性MPC位置控制”的完整链路，因为你的预测模型只要写成双积分近似，代价函数是二次型，约束是线性不等式，问题就是一个标准QP，代码结构清晰，调试容易。等你对MPC的逻辑有感觉了，再考虑用fmincon或者CasADi上非线性预测模型。别一上来就开大，优化问题一旦变成非凸，你会花掉大量时间在“为什么解不出来”上。

4. Matlab代码实现：主循环、优化求解和参考轨迹逐段拆解

这一节把核心代码结构直接拆开讲。完整工程文件里会包含初始化脚本、参考轨迹函数、MPC求解函数、真实动力学更新函数和绘图脚本，这里重点说几个容易卡住的地方。

4.1 总代码框架：初始化、主循环、可视化三段式结构

主程序框架大致是这样一个循环：

%% 初始化 p = struct('m',1.2,'g',9.81,'Ts',0.02,'Np',30,'Nc',10); p.Q = blkdiag(eye(3), 0.5*eye(3)); p.R = 0.01*eye(3); p.Rd = 0.1*eye(3); p.a_max = 5.67; % 水平加速度上限，对应30度最大倾角 p.az_min = -2; p.az_max = 8; % 竖直加速度范围 x_state = zeros(6,1); % [x,y,z,vx,vy,vz] N_steps = 5000; x_history = zeros(6,N_steps); u_history = zeros(3,N_steps); for k = 1:N_steps t_k = (k-1)*p.Ts; % 截取未来Np步参考轨迹 ref_seq = gen_reference(t_k, p.Np, p.Ts); % 求解MPC，得到第一个控制量 u_cmd = solve_mpc_linear(x_state, ref_seq, p); % 保存数据 u_history(:,k) = u_cmd; x_history(:,k) = x_state; % 真实模型更新（RK4，故意和预测模型不完全一样） x_state = dynamics_rk4(x_state, u_cmd, p.Ts, p); end

这里有一个小设计：参考轨迹生成函数一次性返回未来Np步的序列，而不是只返回当前点的参考，这样可以省去主循环里频繁拼接、切片的开销，也方便后面调试预测时域长度。

4.2 代价函数与QP约束矩阵的代码写法

线性MPC用quadprog时，核心工作是构造标准形式：

min 0.5 * U' * H * U + f' * U s.t. A_ineq * U <= b_ineq lb <= U <= ub

其中U = [a_x(k); a_y(k); a_z(k); ...; a_x(k+Nc-1); ...]是一个3*Nc维向量。通过双积分模型，可以把预测位置、预测速度写成U的线性函数，代入代价函数后展开就能得到H和f。代码大致是这样：

function u_opt = solve_mpc_linear(x0, ref_seq, p) Nc = p.Nc; Nu = 3*Nc; H = zeros(Nu); f = zeros(Nu,1); % 构造状态递推矩阵（根据双积分模型） A = eye(6); Ad = zeros(6,3); A(1:3,4:6) = p.Ts*eye(3); Ad(4:6,:) = p.Ts*eye(3); % 对每个预测步累加代价矩阵（省略展开细节） [H, f] = build_cost_quadratic(A, Ad, x0, ref_seq, p); % 输入约束 lb = [-p.a_max; -p.a_max; p.az_min] * ones(Nc,1); ub = [ p.a_max; p.a_max; p.az_max] * ones(Nc,1); % 控制增量约束如果走线性不等式，则在这里写A_ineq opts = optimoptions('quadprog','Display','off'); [U_opt, ~, flag] = quadprog(H, f, [], [], [], [], lb, ub, [], opts); if flag < 0 % 宽松约束后再求一次 lb(:) = -1e3; ub(:) = 1e3; U_opt = quadprog(H+1e-6*eye(Nu), f, [],[],[],[],lb,ub,[],opts); end u_opt = reshape(U_opt, 3, Nc); u_opt = u_opt(:,1); end

如果选fmincon路线，代码就朴素一些，用函数句柄把代价函数和约束函数传进去即可，但要注意fmincon对初值敏感，建议把上一时刻的最优解平移一拍作为当前初值，也就是“热启动”，能明显减少迭代次数。

4.3 状态更新与参考轨迹生成：螺旋线的参考信号怎么给

参考轨迹生成这部分很多人容易忽略速度参考。位置MPC同时跟踪位置和速度，会让轨迹跟踪更平滑。螺旋线参考可以这样生成：

function ref = gen_reference(t, Np, Ts) r = 3.0; omega = 0.4; z_spd = 0.5; n = (0:Np-1)*Ts + t; x = r * cos(omega * n); y = r * sin(omega * n); z = 0.8 + z_spd * n; vx = -r * omega * sin(omega * n); vy = r * omega * cos(omega * n); vz = z_spd * ones(size(n)); ref = [x; y; z; vx; vy; vz]; end

注意这里参考值的单位要保持一致：位置用米，速度用米每秒。如果你是分层结构，参考轨迹的加速度也应当生成出来，外环MPC可以把参考加速度作为前馈项加进代价函数，减少跟踪滞后，不过那会增加变量维度，初次复现可以先不做。

4.4 仿真与运行时间优化：雅可比矩阵、解析梯度能省多少时间

在Matlab里做MPC仿真，最容易发现的问题就是“仿真比实际慢非常多”。这里的关键不是算法复杂，而是优化求解器在数值求导上花了大量时间。fmincon默认用有限差分法估计梯度，一个3*Nc维决策变量就要反复调用几十次代价函数。如果你把代价函数写成可解析的形式，或者用optimoptions里的SpecifyObjectiveGradient选项提供梯度，运行时间通常能下降30%到60%。quadprog不存在这个问题，因为二次型的目标函数天然就是解析的，这也是我推荐先走QP路线的原因之一。

另一个实用技巧是降维：如果位置环只控制水平位置，竖向用单独的高度控制器，决策变量就少1/3。控制时域Nc也不一定非要等于预测时域Np，我先用Nc=10, Np=30，让控制器对远期轨迹有预见，但只优化前10步的控制量，后面10步控制量保持最后一个值，计算量大幅下降，跟踪效果几乎不受影响。

5. 仿真实验与参数影响：从悬停抗扰到3D螺旋线跟踪

代码跑通之后，真正花时间的其实是一轮又一轮的参数实验。这里分享我记录的一组典型现象。

5.1 实验设计与评价指标：位置误差包络、姿态限幅、执行器输出

我习惯于三类评价指标：位置跟踪的均方根误差（RMSE）和最大瞬时误差、控制量是否频繁触达约束边界、控制增量的波动程度。仿真对比场景包括悬停小扰动、水平圆轨迹和3D螺旋线轨迹。

以3D螺旋线为例，在一组典型参数下（Np=30, Nc=10, Q_pos=1, Q_vel=0.5, Rd=0.1），水平方向的位置RMSE大约能控制在0.08米以内，竖直方向因为有独立的油门通道，通常更小，约0.03米。最大瞬时误差出现在进入螺旋线的起始段，因为初始状态和参考轨迹之间有一段位置差，MPC需要先“追上”参考，这段误差不可避免，但控制器不会为此产生剧烈振荡，而是用接近约束上限的加速度尽快收敛，整体行为是合理的。

5.2 预测时域Np和权重Q/R分别怎么影响效果

预测时域Np的影响非常直观：它代表控制器“看得多远”。Np=10时，控制器只看到未来0.2秒（如果Ts=0.02），进入弯道前几乎没有提前动作，跟踪误差明显，转弯处还会出现超调；Np=30时，控制器能提前0.6秒规划，转弯更平滑。但Np不是越大越好，Np=50时计算耗时从10几毫秒涨到几十毫秒，而且远期参考的变化会被过大的不确定性抹平，对当前控制质量贡献不大。

权重方面，我把两个对比结果列成一张表，方便你参考：

5.3 模型失配和噪声下的韧性测试

MPC的运行效果最终要看模型的鲁棒性。我在真实仿真模型里加入两类异常：一是把实际质量调成预测模型质量的1.1倍，二是加入标准差为0.1的测量噪声和常值风扰。结果不出意料：稳态精度稍微下降，水平RMSE从0.08米变到0.12米左右，但控制器没有发散。这里有一个经验：如果模型失配导致固定偏移，单纯靠MPC的反馈修正能压住一部分，但压不干净，更稳妥的办法是在预测模型中加一个扰动状态的增广，比如把恒定风扰作为附加状态估计出来，再补偿到预测方程里。我的代码里为了演示清晰只做了反馈修正，如果你要做更深入的研究，增广状态是非常有价值的下一步。

6. 我在这套实现中踩过的坑和留给后来者的几条建议

最后这部分是我最想说的，因为网上能找到的MPC教程大多止步于“能跑”，但真正折磨人的全是在跑通之后的事情。

6.1 求解器陷入局部最优与不可行解的处理

fmincon处理非线性MPC时，最难缠的就是生成的优化问题不可行，或者解出来一个看起来很怪的控制量。我的排查顺序是：先看约束是不是给得太紧，比如水平加速度上限3 m/s²但参考轨迹要求4 m/s²的加速度，那必然不可行；再看代价函数里有没有除以0的隐患；最后看初值。初值问题很阴险，每次都从全零初值出发，前几拍因为跟踪误差大，优化会跑到奇怪的地方，后面还回不来。改成“上一时刻最优解平移一拍作为当前初值”之后，这个问题几乎消失。

6.2 控制频率与计算延迟：仿真也不算账就完了

Matlab仿真里默认“求解完立刻执行”，但真实系统里控制量总有延迟。哪怕放着不做实飞，我也建议你在仿真中加入“求解耗时”的模型：假设求解耗时t_solve，那么在k时刻应该用k-1时刻解出来的第三拍控制量来顶住，而不是空等。实际操作中，我通常把控制周期Ts设成预期求解耗时的3倍以上，比如quadprog线性MPC在15毫秒解完，我就把Ts设0.05秒，留出充分余量。

6.3 别再把欧拉角奇异带进预测模型：姿态表示的选择建议

如果你用的是全量非线性MPC，姿态表示这一关早晚要过。欧拉角在俯仰角接近90度时会遇到万向锁，变换矩阵直接退化，预测模型瞬变。解决思路是预测模型里改用旋转矩阵或四元数表示姿态。但说实话，对于大部分轨迹跟踪任务，四旋翼很少做极限倒飞姿态，此时级联结构里位置环根本不碰欧拉角，只需要在把期望加速度转成期望姿态角时用到一点三角函数，完全避开了奇异问题。所以，能用级联就用级联，这是我在工程上最实际的一句建议。

6.4 如果下一步要做真实飞行器，先改这几点

这套Matlab仿真离实飞还有几步路要走。第一，状态必须是估计值而不是真值，外环MPC通常假设位置和速度完全已知，实飞时要接视觉里程计、IMU融合或者RTK，估计滞后必须建模；第二，执行器模型要考虑电机响应延迟和推力映射曲线，尤其油门小角度附近的非线性会让MPC预测偏差变大；第三，传输掉线和时钟不同步会让主循环的参考轨迹切片产生跳变，代码里需要加上超时保护和时间戳对齐。把这些补上之后，MPC的工程价值才真正体现出来。

最后再讲一个我自己的体会：MPC之所以难，不是算法本身有多玄，而是它把所有控制问题都变成了“建模+调参+求解”的组合问题。你不需要一开始就追求大预测时域和复杂模型，先把线性MPC的位置环跟一套内环PID跑通，再慢慢把约束、扰动补偿、非线性预测模型加进来。我自己就是这样一步步过来的，中间踩坑无数，但每次把一个问题想透，后面换任何控制对象都能很快上手。希望这篇记录能让你少走几段弯路。