企业官网建设流程全解析

做无人机控制这些年，我和身边不少朋友在轨迹跟踪上栽过同一个跟头：经典四旋翼的四个电机，说到底只能产生一个沿机体轴方向的合推力，想往前走就必须先把机体斜过去，让推力在水平方向分出一股分量。这套姿态到位置的间接驱动链路，在低速小角度场景下没问题，可一旦参考轨迹给得稍微激进，姿态环和位置环就开始互相打架。后来我把目光转到倾转旋翼四旋翼上，配合 LMPC（线性模型预测控制）做轨迹跟踪，效果比原先好了一整个量级。这篇文章就是分享这套方案的建模思路、MATLAB 实现细节，以及我在仿真和调参过程中踩过的几个比较深的坑。

这篇文章适合两类人：一类是把四旋翼轨迹跟踪调到头、但总觉得欠驱动结构是天花板的人，另一类是刚开始接触模型预测控制、想找个能跑通又不太复杂的线性 MPC 实例来练手的人。这里头不会有什么高深到看不懂的理论，我尽量讲清楚“为什么这么干”和“代码里每行在干什么”，顺带把 QP 求解器、线性化方式、权重调节这些实操层面的经验都写出来。

1. 经典四旋翼欠驱动的天花板：轨迹跟踪为什么总是慢半拍

1.1 四个输入控六个自由度，位置和姿态只能“借力打力”

经典四旋翼的四个旋翼转速，经过混控后能产生一个总升力和三个机体轴力矩。也就是说，输入只有 4 个通道，但我们要控制的状态是位置 3 个自由度加姿态 3 个自由度，一共 6 个。这是一个典型的欠驱动系统。实际做轨迹跟踪时，控制器通常是把系统拆成内环姿态控制和外环位置控制：先由位置误差算出期望的姿态角，再让内环去追这个姿态角，然后靠倾斜后的推力水平分量来产生加速度。

这套思路本身没问题，不然市面上也不会那么多飞控都是这个架构。但问题也很直接：姿态动作是在位置误差产生之后才被驱动的，姿态响应又带来了额外相位滞后。如果你让无人机飞一条曲率比较大的轨迹，它一定会在急转弯处“切弯”，因为内环还没来得及把姿态斜到需要的角度，参考点已经跑到前面去了。想通过加大位置环增益来弥补，又会把姿态指令逼得越来越激进，直到超出小角度假设，线性控制器就开始发散。

我做过一个简单对比实验：同样的轨迹，用经典串级 PID 和 LMPC 各跑一遍，后者在跟踪精度上的优势并不是因为预测控制有多“智能”，而是因为它在计算控制量时，把未来一段时间内姿态和位置的耦合关系放进了同一个优化问题里，而不是拆成两个互不感知的环路。当然，如果只谈经典四旋翼，就算用 LMPC 也得面对欠驱动带来的本质限制：你不可能让机体保持水平的同时直接产生一个横向加速度，因为推力方向只有一个。

1.2 倾转旋翼的真正价值：把推力方向从姿态耦合里拆出来

倾转旋翼四旋翼的思路是在每根机臂末端加一个倾转机构，让旋翼产生的推力方向不再固定绑定在机体坐标系上。这样一来，输入就变成了四个推力大小加四个倾转角，一共 8 个独立控制量。系统从“4 输入控 6 状态”变成“8 输入控 6 状态”，从欠驱动变成了过驱动，代数意义上已经不存在轨迹跟踪的死结。

更重要的是，推力方向独立之后，无人机可以在保持机体姿态平稳的前提下，直接通过前侧旋翼向前倾斜产生水平推力分量，从而获得横向加速度。我设计仿真时用了一个比较简单但又很能说明问题的场景：让无人机从悬停开始水平加速到 5 m/s。经典四旋翼做这个动作，机体会明显前倾几十度；而倾转旋翼方案下，机体俯仰角始终控制在几度以内，位置却照样能跟上。这个差异对后续搭载传感器、云台或者其他机载设备来说意义非常大。

当然，倾转旋翼付出的代价是系统非线性显著增强。旋翼推力的方向由倾转角决定，倾转角和姿态角在力、力矩表达式中以三角函数耦合，再加上每个旋翼的反扭矩也会随着倾转方向改变作用轴，模型不再是简单的“推力+力矩”线性叠加。这也是为什么我后来选择 LMPC 而不是继续用 PID 串级控制的核心原因之一：一个高度耦合、强非线性的系统，靠自己手动去分开设计控制律很容易顾此失彼，用模型预测控制统一处理则要自然得多。

下表是我对不同构型的一个粗略对比，方便还没有基础的读者先建立一个直观印象：

2. LMPC 建模：强非线性系统如何变成每步可解的标准 QP

2.1 12 维状态方程与 8 维输入的定义

在实现 LMPC 之前，我得先把被控对象的数学描述理顺。仿真里我采用的是全独立倾转构型：四个旋翼推力可以分别调，四个倾转角也可以分别调。

状态向量取 12 维：

x = [px, py, pz, vx, vy, vz, φ, θ, ψ, p, q, r]'

其中 px、py、pz 是位置，vx、vy、vz 是速度，φ、θ、ψ 是欧拉角，p、q、r 是机体角速度。为了简化代码说明，姿态表达我用了欧拉角。这里有个坦白：欧拉角在大角度机动时会有万向节锁问题，如果真机要做超过 60 度甚至 90 度的翻转，建议换成四元数状态。LMPC 框架本身不受影响，只需要把运动学方程替换成四元数微分方程，然后重新求一遍雅可比矩阵就行。

输入向量取 8 维：

u = [f1, f2, f3, f4, δ1, δ2, δ3, δ4]'

其中 f_i 是第 i 个旋翼的推力大小，和转速的平方近似成正比；δ_i 是第 i 个旋翼相对机体平面的倾转角。为了建模时不至于太失控，我把旋翼推力的大小当成直接输入，忽略电机转速响应动态，用控制目标里对推力变化率的约束来间接模拟执行机构的物理限制。

完整非线性动力学方程在这里就不逐行展开了，核心结构是：

% 位置部分：世界坐标系下牛顿方程 % p_dot = v % v_dot = (R_wb * sum_f_body + [0;0;-m*g]) / m % 姿态部分：欧拉角运动学 + 力矩方程 % [phi_dot; theta_dot; psi_dot] = T(phi,theta) * [p;q;r] % omega_dot = I^-1 * (tau_control - omega x I*omega)

每个旋翼的推力和反扭矩在机体坐标系下的具体分量，会随着 δ_i 的变化而变化，这就是非线性项的来源。用符号工具把力、力矩表达式展开，后面求雅可比就很方便。

2.2 每步雅可比线性化：这就是 LMPC 里“线性”的真正用法

你可能会有个疑问：既然系统是强非线性的，题目里说的“线性模型预测控制”凭什么能用？答案是：它并不是对一个全局线性模型做预测，而是对当前工作点附近做线性化，得到一个线性时变模型（LTV），然后在预测时域内滚动更新。每一步都重新线性化，所以叫 LMPC 也说得通。

具体做法是，在当前状态 x(t) 和参考输入 u_ref(t) 处，对连续非线性方程求偏导：

A = ∂f/∂x | x(t), u_ref B = ∂f/∂u | x(t), u_ref

然后得到离散化的线性偏差方程：

δx(k+1) = A_k * δx(k) + B_k * δu(k)

其中 δx 是状态与参考状态之差，δu 是输入与参考输入之差。

这里有两类做法，我在仿真里都试过。第一种是“单点线性化”：只在每个采样时刻的当前状态处求一次 A、B，预测域内后续全用它，计算量最小，但倾转角变化较大时误差明显。第二种是“逐点线性化”：沿参考轨迹在预测域里每个点上分别求 A_k、B_k，构造一组随时间变化的矩阵，精度高一些，代码量也就多了一些。文章里的代码采用后者，因为当无人机要做高速机动时，单点线性化往往过不了多久就已经偏离线性近似范围，跟踪性能会打折扣。

2.3 目标函数与约束怎么写成标准形式

LMPC 的每个控制周期里，求解的是一个有限时域凸二次规划问题。优化变量是整个预测域内的控制偏差序列和状态偏差序列：

z = [δu0; δu1; ...; δu_{N-1}; δx1; δx2; ...; δxN]

目标函数我取四部分加权之和：

1. 状态跟踪误差：让预测的 δx 尽量收敛到零；
2. 输入误差：让实际输入不要太偏离参考输入；
3. 输入变化率：抑制控制量抖振；
4. 终端误差：对最后一个预测时刻的状态误差施加惩罚，代替终端约束，有效避免 QP 不可行。

写成矩阵形式就是：

J(z) = z' * H * z + f' * z

其中 H 是一个非常大但非常稀疏的块对角矩阵，由权重矩阵 Q、R、P 拼装而成。等式约束则是由 δx_{k+1} = A_k δx_k + B_k δu_k 逐层展开得到的。不等式约束包括推力大小上下界、倾转角范围，以及倾转角变化率约束。

3. MATLAB 代码落地：把 LMPC 从公式变成能跑的仿真

3.1 整体代码骨架

我在 MATLAB 里的实现分成三个模块：模型模块、线性化模块、控制器模块。主仿真循环用全量非线性模型做“真实被控对象”，控制器内部用线性化模型做预测，这样才能验证 LMPC 在模型失配下的鲁棒性。

主循环的核心结构大致是这样：

% 基础参数 param.m = 1.5; param.g = 9.81; param.l = 0.3; param.dt = 0.02; % 参考轨迹由五次多项式生成 [x_ref, u_ref] = generate_reference_traj(param); % 主仿真循环 x0 = [0; 0; 0; 0; 0; 0; 0; 0; 0; 0; 0; 0]; u0 = zeros(8,1); for k = 1:N_sim % 1. 在当前状态和参考输入处线性化 [A, B] = linearize_model(x0, u0, param); % 2. 调用 LMPC 求解当前控制量 u_opt = lmpc_controller(param, A, B, x0, ... x_ref(:, k:k+N), u_ref(:, k:k+N)); % 3. 用全量非线性模型积分一个控制周期 [~, x_next] = ode45(@(t,x) full_dynamics(x, u_opt), ... [0, param.dt], x0); x0 = x_next(end, :)'; % 4. 记录历史数据 x_history(:, k+1) = x0; u_history(:, k) = u_opt; end

这套结构看起来简单，但有几个容易忽略的点。第一个是 u_ref 怎么来，这一点我后面会专门说。第二个是 ode45 在仿真里的计算量，如果跑 5 秒仿真，循环 250 次，每次调 20 步预测域的 QP，普通笔记本大概十几秒到几十秒就能跑完，属于可接受范围。

3.2 雅可比矩阵：用 syms 求 A、B，别自己手推

倾转旋翼的动力学表达式里三角函数和旋转矩阵分量很多，手工推导雅可比矩阵既容易出错又浪费时间。我自己用的办法是用符号工具箱把非线性方程写出来，然后直接求偏导生成数值函数，改旋翼构型时只需要改表达式，不用改求导逻辑。

关键代码类似这样：

syms px py pz vx vy vz phi theta psi p q r real syms f1 f2 f3 f4 d1 d2 d3 d4 real X = [px py pz vx vy vz phi theta psi p q r]'; U = [f1 f2 f3 f4 d1 d2 d3 d4]'; % 这里把完整非线性方程写进 dynamics_expression F = full_dynamics_symbolic(X, U); % 分别对状态和输入求雅可比 A_sym = jacobian(F, X); B_sym = jacobian(F, U); % 生成可调用的数值函数 A_func = matlabFunction(A_sym, 'Vars', {X, U}); B_func = matlabFunction(B_sym, 'Vars', {X, U});

在仿真循环里调用这两个函数时，只需要传入当前状态和参考输入即可得到 A、B 矩阵。需要注意：这里是对连续模型求雅可比，实际预测方程还需要离散化。我用的是最简单的零阶保持近似：

A_d = eye(12) + A_dt * dt; B_d = B_dt * dt;

如果采样周期比较大，或者控制精度要求较高，可以换成矩阵指数离散。我在 20ms 采样周期下对比过，零阶保持和矩阵指数两种方式的跟踪误差差距很小，所以仿真里用轻量方案。

3.3 QP 求解器选型与核心控制函数

求解 QP 这一步，我试过三种方案，简单对比一下：

纯 MATLAB 仿真阶段，我推荐直接上 quadprog，省事。给一个 lmpc_controller 内部构造 QP 的简化版本：

function u_apply = lmpc_controller(param, A_d, B_d, x_curr, x_ref_seq, u_ref_seq) N = param.N; nx = 12; nu = 8; % 构造块对角权重矩阵 Qbar = blkdiag(kron(eye(N-1), param.Q), param.P); Rbar = kron(eye(N), param.R); H = blkdiag(Rbar, Qbar); % 把动态约束展开成等式约束 E_all * z = d_all E_all = zeros(N*nx, N*nu + N*nx); d_all = zeros(N*nx, 1); % 第一层：δx1 = A_d * δx0 + B_d * δu0 dx0 = x_curr - x_ref_seq(:,1); E_all(1:nx, 1:nu) = -B_d; E_all(1:nx, nx+1:2*nx) = eye(nx); d_all(1:nx) = A_d * dx0; % 后续层依次展开，代码省略，用循环拼装 % ... % 控制输入上下界约束 lb = zeros(N*nu + N*nx, 1); ub = zeros(N*nu + N*nx, 1); % 根据 u_ref_seq 拼装每个时刻的输入上下界 % ... % 调用 quadprog opts = optimoptions('quadprog', 'Display', 'off', ... 'Algorithm', 'interior-point-convex'); z_opt = quadprog(H, [], G_ineq, h_ineq, E_all, d_all, ... lb, ub, x0_warm, opts); % 实际输出第一组控制增量 du0 = z_opt(1:nu); u_apply = u_ref_seq(:,1) + du0; end

这段代码里有一个非常关键的细节：H 矩阵里的变量顺序 I 必须和 E_all、约束矩阵的列顺序完全一致，否则 QP 求解出来的 z 序列是乱的，控制量自然全错。这个低级错误我犯过一次，表现是跟踪曲线前几十步看着正常，后面突然跳变，排查了很久才发现是矩阵拼装时列索引偏了一位。建议读者在写这段之前，先在纸上把 z 的分块顺序写清楚，再用小维度数据做单步调试，确认每个分块的位置都对上了再接入完整仿真。

3.4 离散化与预测域的选择

关于预测时域长度，我给一个比较实用的经验范围：采样周期 20ms、预测域 N=20 就是一个很好的起点。N 太小（小于 10），预测信息太少，控制器会偏近于一个高增益状态反馈，轨迹跟踪精度下降；N 太大（大于 40），quadprog 的变量规模达到 8×40+12×40 = 800 维左右，计算时间明显增加，而且对模型误差更敏感。

控制周期也是同理。仿真里 50Hz 完全够用，真机上如果你用的是 MATLAB Coder 生成 C 代码，再配一个合适的稀疏 QP 求解器，20Hz 到 50Hz 之间都可以跑，我倾向于取中间值 30Hz 左右，给执行机构留足响应时间，也给 QP 求解留点余量。

4. 调参与仿真验证：四个坑我替你踩过了

4.1 稳态误差不是 Q 不够大，而是 u_ref 给错了

一开始我把参考轨迹只定义了位置和速度，输入参考 u_ref 随便给了个悬停值。结果不出意外，位置跟踪有明显稳态误差。我当时的第一反应是调大 Q 里的位置权重，结果误差没消掉，反而开始出现抖振。

问题的根因在于：如果 u_ref 和 x_ref 不匹配，QP 同时要最小化状态误差和输入误差，最后的最优解只能在两者之间妥协，稳定下来之后输入必然偏离真实需要值，状态自然也就偏了。

解决方法是把 u_ref 和 x_ref 一并由轨迹生成模块输出。具体做法是，对参考轨迹上的每一个点，用静态平衡方程解析求解期望的推力大小和倾转角。比如匀速直线飞行时，水平推力分量应等于阻力平衡所需的力，竖直推力分量应等于重力。这样 u_ref 在预测域内才是“顺路”的，LMPC 只需要在它附近做小修正，跟踪误差自然不会再有系统性偏差。

如果你的参考轨迹变化太快，静态平衡解跟不上动态项，又不想引入复杂的微分平坦逆解算，另一个折中办法是在目标函数里把输入偏差项的权重 R 调得很小，让优化器主要依赖状态反馈来决定输入，输入参考只起辅助作用。代价是控制量可能稍微偏离最经济的工作点，但对跟踪精度影响很小。

4.2 倾转角变化率约束不加，控制器会抖到怀疑人生

我的第一版仿真里，只给了倾转角上界下界，没有限制它的变化速度。结果位置跟踪曲线很漂亮，但倾转角曲线像锯齿一样高频抖动，推力大小反而相对平稳。这种抖动的本质是优化器在利用倾转角的高频变化来“偷”额外的加速度，以极小代价补偿高频参考误差。

解决方法是把倾转角变化率约束加入 LMPC 的预测模型。最直接的一种方式是构造差分约束：

|δ_i(k+1) - δ_i(k)| <= Δδ_max * Ts

这个约束是线性的，加在 QP 里很自然。我用的增量限幅是 1.5 rad/s 左右，对应的物理意义是舵机能在单个控制周期内完成的倾转幅度。如果不清楚舵机实际参数，可以把这个值写保守一点，因为倾转角响应过慢只是损失一点性能，控制量抖振才是真机上更容易损坏执行机构的问题。

顺便说一句，如果你在做真机，倾转角变化率不光要约束，最好还在执行机构层面做二阶滤波，把控制器给的阶跃式倾转指令平滑成梯形速度曲线，否则舵机本身就容易被激振。

4.3 quadprog 超时和 infeasible：两个方案很管用

当预测域 N 到 30 以上，或者参考轨迹有突变导致约束过紧时，quadprog 偶尔会出现求解时间超过控制周期，或者干脆报 infeasible。

我的处理经验有两条。第一条是 warm-start。把上一个控制周期的最优变量 z_opt 作为本次 quadprog 迭代的初始点，通常能把求解时间缩短一半以上，尤其在轨迹平滑的情况下效果更明显。代码上就是给 quadprog 传进 x0 参数，值得注意变量维度要和 z 的长度一致，很多人在拼 z_opt 时忘记把状态块也塞进去，导致维度对不上。

第二条是软约束。位置误差、速度误差这些约束如果需要加，尽量不要做成硬约束，换成松驰变量放进目标函数。终端约束是 infeasible 的另一大来源，所以我在模型里只用了终端代价 P 而不用终端等式约束。这样 LMPC 在预测末端可以有轻微误差，不会为了“强行到达终点”而牺牲前段轨迹的可行性。

4.4 权重矩阵的调节方向参考

调 Q 和 R 是 LMPC 里最需要耐心也最没有标准答案的一步。我给一个我常用的初始权重表，注意一定要结合自己模型的量纲来改，不要照抄：

还有一个我自己的习惯：先把位置和速度权重拉开一个量级，让位置主导跟踪，再慢慢加大倾转角输入权重，直到倾转角曲线变得平滑。位置权重一下子给得太大，本质上相当于把 LMPC 变成了一个高增益比例控制器，QP 虽然能解出来，但控制信号会很难看。

5. 从仿真到真机：模型失配和工程细节

5.1 扩张状态观测器补齐 LMPC 的模型依赖短板

LMPC 说到底还是基于模型的控制器，仿真里模型是完美的，真机上有气动阻力、重心偏移、电池电量导致的推力系数变化等等。这些误差不会让 LMPC 完全失效，但会留下稳态偏差或者在小角度机动时表现不理想。

我比较推荐的轻量级方案是在位置和速度环外面加一个扩张状态观测器，把未建模扰动和模型失配统一估计成一个扰动项，然后在控制回路里反向补偿。这样 LMPC 的线性模型只需要描述主体动态，剩下的给观测器兜底。观测器设计和 LMPC 本身解耦，调试起来也比直接去修改模型参数要方便。

5.2 执行机构动态不能省略

仿真里直接把推力指令打进动力学方程，和真机的情况差别很大。倾转舵机有明显的响应延迟，电机也有转速响应延迟，如果这两个动态不建模，你在仿真里调出来的控制参数往往偏激进，上真机就会看到高频颤振。

最简单的建模方式是给倾转角通道加一个一阶惯性环节：

delta_dot = (delta_cmd - delta_act) / tau_servo;

电机推力通道同理，我用的是：

f_dot = (f_cmd - f_act) / tau_motor;

一般倾转舵机的时间常数在 0.05 到 0.15 秒之间，电机稍快一些。把这两个时间常数加进全量仿真模型之后，再去调 LMPC 的输入变化率约束，会得到一个比理想模型保守得多、也可靠得多的参数组合。如果时间充足，再跑几组蒙特卡洛扰动，把推力系数、阻力系数、重心位置在正负百分之十范围内随机打乱，看看跟踪误差是否仍然有界。

我在实际调试中最大的体会是，LMPC 的吸引力在于把“轨迹跟踪”问题变成了一个每步都在线优化的问题，你不用再手工设计各种增益曲线来应对不同飞行阶段。但模型预测控制也把压力转移到了“模型精度”和“参考轨迹质量”上，u_ref 给得好，QP 求解轻松，控制量稳如老狗；u_ref 给得随意，再大的 Q 权重也救不回来。所以如果你正在调自己的倾转旋翼四旋翼 LMPC，我建议先把仿真轨迹生成和逆模型的环节做扎实，再去碰控制器内部参数，这还是那条路：预测控制的效果上限，往往取决于你把“预测”这件事准备得有多充分。