企业官网建设流程全解析

1. 项目整体设计：从控制问题到Simulink+MPC方案

1.1 这个项目解决什么问题

把标题翻译成大白话就是：让一架四旋翼无人机在Simulink仿真环境里，沿着一条预设的空间轨迹飞行，控制器用的是MPC（模型预测控制），目标是让位置和姿态跟踪误差尽可能小。

这类工作在实际工程里非常常见。无人机要做航拍、巡检、物流投递，第一步不是“飞得稳”，而是“按规划好的路径飞”。规划层给出一条轨迹，底层控制器要保证飞行器真实运动轨迹和规划轨迹尽量一致。过去很多项目用PID做这一层，调好了确实能飞，不过遇到大机动、强耦合、执行器饱和临近的情况，PID就显得“眼光太短”：它只根据当前误差做调整，没办法预估未来几秒会发生什么。

MPC的思路完全不同。它利用被控对象的模型，向前预测一段未来时域内的系统状态，在满足约束的前提下，求解一组最优控制输入，使得预测输出和期望轨迹偏差最小。通俗点说，PID是“看到墙再拐弯”，MPC是“提前看到墙的走向，提前规划方向盘”。这个特性用在轨迹跟踪上非常契合，因为参考轨迹本身就是已知的未来序列。

1.2 为什么选择Simulink做验证平台

四旋翼控制算法研究有两种常见路径：一是纯数学仿真，用MATLAB脚本或者Python把动力学方程全部手写出来；二是接真实飞控硬件做半实物或真机验证。对大部分做算法验证的开发者来说，纯脚本方式代码量可控、灵活，但模型复用性差，动态可视化弱。真机验证又涉及硬件成本、安全风险、调试周期长，不适合算法研究初期反复迭代。

Simulink处在两者中间，是个很舒服的折中方案。它能把四旋翼非线性动力学、电机执行器模型、传感器模型、控制器算法、轨迹生成模块全部放在同一张模型图里，信号流一目了然，改一个模块就能重新仿真，还能用线性化工具直接在悬停点提取MPC需要的线性预测模型。而且Simulink仿真结果能方便导出到MATLAB工作区做误差分析和图表绘制。换句话说，Simulink承担的是“算法验证与参数整定的预研平台”角色——模型可信，迭代快，还方便后续生成嵌入式代码移植到飞控。

1.3 MPC与PID/LQR对比：选型逻辑

我在项目初期也犹豫过要不要直接用LQR。LQR和MPC一样基于状态空间模型，在线性系统里性能优秀，但处理约束能力几乎为零。四旋翼模型最关键的约束恰恰在于控制输入有上限（电机推力不能为负、不能超过最大转速），姿态角也有物理限制。LQR得到的控制量超出电机能力时，只能做饱和截断，但截断之后闭环特性可能失去理论保证。MPC则把约束直接建模到优化问题里，求解出的控制量天然满足约束。

PID就更不用说了，参数整定在非线性强耦合的四旋翼模型上非常依赖经验，三个通道的PID参数互相影响，调一轮往往要花掉一两天。MPC虽然调参数也麻烦，但权重矩阵和预测时域的物理含义更明确——某个状态权重调大，对应状态跟踪误差就变小，调试方向可控。

考虑到项目定位是“轨迹跟踪控制研究”，而非单纯的姿态稳定控制，MPC对参考轨迹的前馈利用能力是明显优势。轨迹已知意味着MPC可以在目标到来之前提前调整姿态，这种“预告式控制”行为在跟踪圆形轨迹、8字轨迹时能看出来，响应滞后的现象比PID轻很多。

2. 四旋翼模型搭建：把被控对象先搞到可信

2.1 状态量定义与动力学方程

模型是整个项目的基石。模型错了，MPC再聪明也没用。四旋翼经典的12维状态向量通常定义为：

位置 [x, y, z]，速度 [u, v, w]，姿态角 [φ, θ, ψ]，角速度 [p, q, r]，分别对应导航系下的位置速度、机体欧拉角、机体角速度。

动力学方程分为平动和转动两组。先说平动，在导航坐标系下的加速度表达式为：

ẍ = (cosφ sinθ cosψ + sinφ sinψ) * T / m
ÿ = (cosφ sinθ sinψ − sinφ cosψ) * T / m
z̈ = (cosφ cosθ) * T / m − g

其中T是机体坐标系下的总推力，φ是横滚角，θ是俯仰角，ψ是偏航角。这个公式里推力被姿态角“掰”到了导航系的不同方向，所以改变姿态角就能改变水平加速度方向。

转动方程相对复杂一点，考虑陀螺力矩和惯性耦合，按刚体欧拉方程写：

φ̈ = (Iyy − Izz)/Ixx * qr − Jr/Ixx * q * ω + l/Ixx * τφ
θ̈ = (Izz − Ixx)/Iyy * pr − Jr/Iyy * p * ω + l/Iyy * τθ
ψ̈ = (Ixx − Iyy)/Izz * pq + 1/Izz * τψ

这里的τφ、τθ、τψ是各轴控制力矩，l是电机到机体质心的力臂长度，Jr是转子转动惯量，ω是所有螺旋桨转速的代数和。最后一项是转子陀螺效应引起的耦合，四旋翼模型不能随手忽略它，尤其是做精确仿真时。

2.2 模型参数与执行器模型

我使用的是常见的某款开源四旋翼参数做基准，质量m=1.2 kg，臂长l=0.3 m，转动惯量为Ixx=Iyy=0.0152 kg·m²，Izz=0.0291 kg·m²，重力加速度9.8 m/s²。悬停状态下每个电机需要提供的拉力约为m*g/4，算下来约2.94 N。单片桨叶最大拉力取8 N，则总推力上限约32 N，这为后面MPC约束设置提供了依据。

电机执行器不能当作理想“说给多少就给多少”的部件，实际电机有响应延迟。我在Simulink里用一阶惯性环节近似电机动态：

F_motor = (1 / (τs + 1)) * F_cmd

其中τ取0.02 s。这个时间常数决定了电机从指令到实际拉力的过渡速度，如果MPC预测模型里不考虑这个延迟，控制器会以为电机能瞬间产生它要求的拉力，实际仿真就会出现振振或跟不上剧烈变化的情况。

2.3 Simulink模型的分层结构与信号流

我把Simulink模型做成了三层结构，方便后期替换模块。最底层是六自由度刚体动力学模块，输入是四个电机的拉力，输出是位置、速度、姿态、角速度状态。中间层是执行器模块，接收控制器来的推力指令和力矩指令，通过混控矩阵转换成四个电机的转速指令，再经过一阶惯性环节和旋翼拉力系数转换成实际作用到机体的力和力矩。最上层是控制器和轨迹生成模块。

混控矩阵在项目里很关键，它是连接“总推力/三轴力矩”和“四个电机拉力”的桥梁，形式是：

[F1; F2; F3; F4] = M * [T; τφ; τθ; τψ]

M矩阵取决于机架的X型还是十字型布局，以及对角线力臂长度。具体的数值，用机臂几何关系直接推导即可，推导完用一组已知的悬停状态验证一下：悬停时四电机拉力应该恰好都等于m*g/4。

我在项目初期犯过一个低级错误：把电机时间常数τ直接设为0，仿真快是快，但后面MPC参数是在这个“理想执行器”假设下整定的，换成一阶电机模型后跟踪误差大了一倍。所以从第一步就加入执行器动态，别省这个模块。

3. MPC控制器设计：预测模型、代价函数与约束

3.1 在悬停点线性化与离散化

四旋翼动力学是非线性的，MPC如果直接用非线性模型做预测，每一时刻都要解非线性优化，Simulink仿真时间会非常慢，不适合参数整定阶段反复迭代。项目里我采用的方案是在悬停点附近做线性化，得到一个线性时不变状态空间模型，再用线性MPC在线求解二次规划。

悬停工作点的状态是：位置任意，速度为零，姿态角φ=θ=ψ=0，角速度为零，总推力T=m*g，三轴力矩为零。在这个工作点对非线性方程求雅可比矩阵，就得到线性化矩阵A和B。这一步可以手推，也可以直接在Simulink模型上用线性化工具提取，后者更省事，但需要注意把模型内部状态都收敛到工作点后再做线性化，否则提取出的矩阵会不准。

我建议手动核对一遍A矩阵中位置加速度对姿态角的偏导项。比如x方向加速度对俯仰角的导数，理论上就是g，因为ẍ = θ*g的小扰动近似。如果你提取出的矩阵里这项数值接近g，说明线性化正确，差太远就得检查工作点是否设置错。

得到连续的A、B矩阵后，用零阶保持器法（c2d函数）按采样时间Ts=0.02 s离散化。离散模型用在这类数字控制器仿真里，时间推进是逐拍进行的，和后续真机实现代码的逻辑一致。

3.2 预测模型的内部结构

预测模型的状态量我分为外环和内环两组。外环是位置环，状态取三个位置量和三个速度量，控制输入是期望姿态角（横滚、俯仰）加上总推力；内环是姿态环，状态取三个欧拉角和三个角速度，控制输入是三轴力矩。外环MPC输出的期望姿态角，传给内环控制器去跟踪。

选择级联结构的理由是工程实践经验：纯单层MPC直接把推力、力矩作为控制输入，状态维度高、预测模型复杂，QP问题的规模大，仿真速度慢，整定权重的工作量也大。级联结构把轨迹跟踪问题拆成“外环带位置速度、内环带姿态角速度”两个子问题，虽然理论上不是最优的全局控制，但实践中调试友好，性能已经足够满足轨迹跟踪需求。

外环预测模型表达式为：

x(k+1) = A_pos * x(k) + B_pos * u(k)

其中A_pos里含着位置对速度的积分关系，B_pos里是悬停点附近单位俯仰角变化对水平加速度的影响。对水平通道来说，B_pos对应的数值就是重力加速度g，这是个很直观的验证点。

3.3 代价函数设计与权重经验

代价函数写成分段加权形式：

J(k) = Σ_{i=1}^{Np} ||e_x(k+i|k)||²_Q + Σ_{i=0}^{Nc-1} ||Δu(k+i|k)||²_R

第一项是预测时域内每个时刻的状态跟踪误差加权和，第二项是控制输入变化量的加权惩罚。加入Δu的惩罚能防止控制量高频抖动，这在四旋翼上特别重要，因为电机和结构受不了频繁大幅度变化的力矩指令。

我最终使用的权重矩阵经验值如下。外环位置环：位置误差权重取Q_p = diag(8, 8, 10)，速度误差权重取Q_v = diag(1, 1, 1)，控制增量权重R = 0.1 * eye(3)。内环姿态环：姿态角权重取Q_att = diag(12, 12, 6)，角速度权重Q_w = diag(2, 2, 2)，控制增量权重R_att = 0.05 * eye(3)。

这里有几个整定经验值得分享。第一，高度通道位置权重比水平通道高一点，因为z方向直接受推力影响，响应更快，给高权重能让高度跟踪更敏捷。第二，速度权重不要设太接近位置权重，否则MPC会过度抑制速度变化，轨迹拐弯处容易“钝”，跟踪滞后增大。第三，控制增量权重是最敏感的旋钮，增大R，控制动作变平滑，但跟踪带宽下降；减小R，跟踪精度提高但可能出现控制量抖振。调试时先保持R不变，调Q，找到一个额性能边界，再回头微调R。

3.4 约束设置与软约束处理

约束是MPC的招牌功能。我设置的物理约束如下：

0 ≤ T ≤ 32 N
−0.52 ≤ φ ≤ 0.52 rad
−0.52 ≤ θ ≤ 0.52 rad
|ΔT| ≤ 5 N 每步
|Δτ| ≤ 0.8 N·m 每步

推力下限设为0是因为电机只能正转产生向上拉力，负推力不存在。姿态角限幅±30度是四旋翼飞行安全的经验值，超过这个角度，水平推力分量下降很快，同时姿态非线性增强，线性预测模型在边界处失真。

这里要专门提醒一个坑：约束设得太紧会直接导致QP无解。最典型的是把推力上限设成正好等于某个机动所需的临界推力，MPC预测时发现未来无论如何都无法同时满足终点位置和推力约束，求解器报错退出。解决办法是引入松弛变量，把硬约束变成软约束。具体做法是在代价函数里加一项大权重的松弛惩罚项，让MPC在约束不可满足时，可以适当突破约束边界，优先保证控制变量平滑过渡，而不是仿真中断。

3.5 MPC参数的选择过程记录

我把整定过程记录在这里，方便读者复制调试路径。第一步，固定Ts=0.02 s，Nc=5，Np从10开始逐步增加，观察闭环是否稳定。第二步，在Np=20时，跟踪圆形轨迹的最大横向误差约0.35 m，继续增加Np到30，误差降到0.18 m，但单步计算时间上升约40%。第三步，固定Np=25，把Nc从5增加到8，误差进一步降到0.12 m，控制增量抖动略微增加。第四步，微调R从0.1降到0.05，误差降到0.09 m，但开始出现轻微力度抖振，最后综合权衡仍然选了R=0.1。这套流程花了大约一下午，全部在Simulink里自动化跑脚本批量仿真完成的。

如果你遇到跟踪误差怎么调都压不下去，先别急着加权重，检查预测时域是否太短。Np太短意味着MPC看得不够远，控制动作偏短视，类似“走一步看一步”，跟踪连续轨迹时会系统性地滞后。我的经验是预测总时长Np*Ts至少要覆盖到参考轨迹的半个振荡周期以上，圆形轨迹周期4秒，预测时域0.5秒以上才够。

4. Simulink联合仿真实现：从模型到闭环回路

4.1 控制器实现的三种方式

在Simulink里实现MPC，有几种路径可选。第一种是直接用Model Predictive Control Toolbox里的MPC Controller模块，优点是有图形化界面建约束和权重，缺点是线性MPC模块状态结构固定，想改预测模型细节比较麻烦。第二种是用MATLAB Function模块自己写MPC逻辑，内部调用quadprog求解QP。第三种是写成S-Function，适合以后要生成代码移植嵌入式的场景。

我这次选的是中间路线：MATLAB Function实现。核心原因是它对预测模型的计算过程完全透明，方便调试，也方便把非线性预测模型扩展进去。对纯仿真验证来说，quadprog在每个采样时刻求解一个维度为（状态6维、控制3维、预测时域25步）的QP问题，开销完全在可接受范围内。

MATLAB Function的内部结构大致是这样的：初始化阶段把离散A、B矩阵、权重矩阵、约束矩阵作为参数常量传入。每次调用计算当前状态与参考轨迹未来Np步序列的偏差，组装成标准QP形式，然后用quadprog求解得到最优控制增量序列，取第一步作为当前控制量输出。

4.2 轨迹生成模块的构建

轨迹生成在Simulink里可以用MATLAB Function或Signal Editor做。我采用了时间基准的轨迹生成方式，输入仿真时钟t，输出参考位置、参考速度、参考加速度序列。因为MPC预测需要未来Np步的参考值，所以轨迹生成函数必须做成“给定当前时刻和未来时刻，返回对应参考序列”的形式。

项目里我用了几组不同难度轨迹做验证。最简单的是圆形轨迹，方程是：

x_ref(t) = 2 * cos(0.5t)
y_ref(t) = 2 * sin(0.5t)
z_ref(t) = 2

半径2米，角速度0.5 rad/s，周期约12.6秒。这个轨迹考察水平通道连续跟踪能力。第二组是8字形轨迹：

x_ref(t) = 2 * sin(0.2t)
y_ref(t) = 2 * sin(0.4t)

8字形轨迹有连续变曲率段，MPC需要频繁调整角速度，比圆形更难跟。第三组是螺旋上升轨迹，x、y做圆周运动同时z线性增加，考察三维轨迹同步跟踪能力。三组都跑完，跟踪控制器的性能画像就比较完整了。

参考轨迹的导数也要给出来，位置环MPC里参考速度直接作为前馈项参与预测，这一步能显著提高高速段的跟踪精度。轨迹生成函数的输出我做成Np×6的矩阵，第i行对应预测时域内第i个时刻的参考位置和参考速度。

4.3 完整仿真回路的连接方式

仿真回路从参考轨迹生成模块开始，输出参考序列给MPC控制器模块。MPC模块接收当前状态反馈（来自四旋翼模型的状态输出）和参考序列，计算控制指令。控制指令包括期望总推力T_cmd、期望横滚角φ_ref、期望俯仰角θ_ref。这三个量作为内环控制器（我用的是姿态角速度PD控制器）的参考输入，PD控制器输出三轴力矩指令。

力矩和推力一起进入混控矩阵模块，转换四个电机拉力指令，经过一阶惯性环节，进入六自由度动力学模块，更新状态。状态测量模型里我还加入了简单的噪声模块，模拟传感器测量不确定度，避免控制器在无噪声条件下“过度自信”。

仿真求解器设置也值得说。模型里有连续积分环节，控制器是离散采样，所以我在Simulink里把求解器设置为“固定步长，离散求解器”，步长取0.001 s。控制器模块每0.02 s触发一次，用单位延时模块做采样保持。这样做的好处是仿真时间推进逻辑和真实数字控制器一致，不会出现控制器输出的时间分布不规则的问题。

4.4 参数配置与初始化陷阱

整个模型里最容易忽略的初始化问题是积分器的初始状态。四旋翼模型的位置初始值如果和参考轨迹起点不一致，MPC第一拍就会输出很大的控制量试图修正误差，可能触发约束边界。我的做法是让模型初始状态离轨迹起点保持一个较小的偏差，例如0.3 m，方便观察MPC的收敛行为。如果你想把起点误差设得很大，例如3 m，建议在跟踪前期使用路径平滑过渡，直接在轨迹生成函数里加一段连接段，让参考轨迹从模型当前点平缓过渡到设定轨迹，否则优化问题一开始就处于难解状态。

另外，混控矩阵的输出要经过合理性检查。实际电机推力不可能为负，如果混控矩阵解算出某个电机的负指令，机体会产生错误的推理力矩。我在执行器模块里加了限幅处理，把负推力钳位到0，同时在控制器预测模型里也要把这个饱和特性考虑进去，否则预测的控制效果和实际模型执行效果不一致，MPC的预测优势会被抵消。

5. 常见问题与排查技巧实录

5.1 仿真发散：权重数量级失衡是首因

我在项目一开始跑MPC闭环，第一版矩阵权重直接用单位阵，结果仿真500步左右状态直接发散。排查看原因，不是稳定性理论问题，而是位置量级和速度量级天然不同：位置动辄好几米，速度也就每秒几米。用单位权重加权时，位置误差占主导，MPC为了减小位置误差拼命给速度指令，速度误差的惩罚相对被湮没，控制量剧烈变化，最终振荡发散。

这个问题的排查经验是：先看代价函数各项的数值量级是否接近，而不是先调参数。解决办法是把权重按物理量归一化设计，例如位置权重10，速度权重1，这样误差项数值量级大致相当，MPC优化出来的控制量就比较平滑。后续所有调参都从这个基准出发。

5.2 轨迹跟踪持续滞后

调好权重后，追踪圆形轨迹的最大误差仍然有0.2米左右，而且是系统性的滞后——无人机位置始终在参考点后面。排查思路：参考轨迹给的是当前位置参考，如果MPC参考序列没有包含未来预测时域内的参考值，控制器每个时刻都只“盯着当前目标”，预测能力就等于零。

检查原因之后发现，我第一次实现轨迹生成时，MATLAB Function里返回的参考序列用的是当前时刻的值填充整个预测时域，也就是没有未来信息。改成按预测时域时间轴逐点计算参考轨迹后，滞后显著减小，最终误差降到0.09米左右。这是一个非常容易犯但很难察觉的Bug，因为模型不会报错，收敛性也不会崩溃，只是性能白白损失。

对MPC来说，参考轨迹的未来信息是核心资产，务必确认轨迹生成函数给出的参考矩阵每一行对应当前时刻之后的第1、2、…、Np个采样时刻，而不是重复广播当前时刻。

5.3 QP求解失败与约束冲突

发生过一次内环姿态MPC在快速跟踪8字轨迹时求解失败的情况。排查时看到quadprog返回的信息是“约束条件不一致”，说明预测时域内存在不可行解。原因是参考轨迹有急转段，期望姿态角快速变化，姿态环MPC预测发现按当前约束无法同时满足角度限幅和跟踪目标。

这个问题最终的解法是给约束加入松弛变量，即在代价函数里增加对松弛变量的高权重惩罚。我实现的软约束形式是：

J = 原代价 + ρ * ε²

其中ε是约束越界量，ρ取1e5。这次修改让控制器的鲁棒性明显提升，约束偶尔被突破时，优化器会平滑地过渡到可行域内，而不是直接返回无解。这个技巧在MPC工程化中属于标准做法，强烈建议一开始就实现进去。

5.4 仿真速度延迟与计算量优化

预测时域Np从20增加到30后，单个仿真步的计算时间从约15 ms增加到约25 ms，虽然仿真能跑，但一个60秒的仿真场景要等好几分钟，不方便批量调参。优化办法是减少不必要的预测模型维度。外环MPC其实只关心位置和速度状态，内环MPC关心姿态和角速度，每个MPC模块的状态维度都控制在6维，控制维度3维，整体QP规模很小。

还有一个实用技巧：把quadprog的算法选项设置为内点法而非默认的active-set，在低维度问题上内点法通常更快更稳定。批量调参时，我用循环脚本自动跑几十组参数组合，把所有结果记录到一张表里，节省了大量的手动操作时间。这种做法对参数整定效率提升极其明显。

我这套方案做完，最大的体会是：MPC在四旋翼轨迹跟踪上的性能优势是实打实的，代价是仿真搭建和调试复杂度比PID高不少。如果你正在做类似的方向，建议先老老实实把非线性模型搭好、验证混控矩阵、用线性化工具提取模型，再上MPC，别跳过模型验证直接调控制器。模型阶段花的时间，会在控制器调试阶段加倍省回来。

最后分享一个我踩了几次坑后养成的小习惯：对每个版本的MPC控制器，都保留一份对应的参数配置文件和仿真对比数据，参数一改就知道效果变好还是变坏，避免反复横跳。这个项目后续还可以往非线性MPC、分布式MPC或者机群协同轨迹跟踪方向扩展，但前提是把当前这套线性MPC的接口和调参流程吃透。先把这一步走稳，后面才走得快。