这段时间我一直在折腾一件事：把一篇关于多旋翼飞行器动力学建模与控制系统设计的期刊论文，用Simulink完整复现一遍。标题里的PID控制、多旋翼飞行器建模、姿态环、位置环、串级结构这些关键词，大家应该都不陌生，但真正从论文公式一步步走到一个能跑、能调、能出仿真曲线的完整模型，中间的细节远比正文里写的要多得多。这篇文章记录的就是这次复现的全过程，包括动力学模型怎么搭、内外环PID控制器怎么设计、参数怎么一步步整定，以及我踩过的那些坑。如果你正在做课程设计、毕业课题，或者在工作中需要快速验证飞行控制算法，这篇内容应该能帮你省下不少摸索时间。
1. 论文复现的破局思路：从公式到可运行模型的工程路径
1.1 先搞清楚论文的“骨架”再动手
拿到一篇多旋翼控制类的期刊论文，第一反应往往是打开Simulink就想画框图。但我建议你先冷静一下，把论文的结构拆成三块来看：建模部分、控制器设计部分、仿真验证部分。
建模部分通常给出的是刚体动力学方程，常见形式是力方程和力矩方程的联立，其中包含质量、惯量、推力系数、阻力系数、力臂长度这些参数。控制器设计部分一般会给出控制结构框图，大多数情况下是串级PID，内环是姿态控制，外环是位置或速度控制。仿真验证部分则给出参考轨迹跟踪曲线、阶跃响应、抗扰测试等结果图。
这次我复现的论文结构就比较典型，先建了一个四旋翼的六自由度刚体模型，然后在Simulink里做了内外环控制器，最后一组仿真曲线来验证控制效果。这里的关键是：论文里给出的每个公式都要在模型里找到对应的模块或代码，不能跳步。比如质量矩阵是常数矩阵还是时变矩阵，论文里也许一句话带过，但你在模型里必须明确下来。
1.2 复现工作流中的三个关键决策
第一个决策是模型的复杂度级别。你可以只在Simulink里用6DOF（六自由度）模块库里的现成模块，也可以从零开始用S函数写动力学方程。前者上手快，但“黑盒”成分较大，参数设置不直观；后者代码可控、调试方便，也更接近论文公式的原始形式。我最后选择了“S函数为主、Simulink模块为辅”的混合方案：动力学部分用S函数编写，控制器部分用Simulink模块搭建，这样既保留了公式的透明性，又方便后续调参。
第二个决策是坐标系与旋转顺序。多数论文采用惯性系NED（北东地）加机体坐标系，旋转顺序是先偏航、再俯仰、最后横滚。如果你复现的论文用的是其他顺序，比如ZYX或者ZYZ，一定要在仿真初始就统一，否则后面算出来的姿态全是错的。
第三个决策是控制器执行频率与仿真步长。很多人直接把控制器和仿真模型放在同一个步长下跑，这在纯仿真阶段没问题，但如果后续要移植到嵌入式飞控里，控制器频率通常是姿态环200Hz到1kHz、位置环50Hz到200Hz，仿真步长要能匹配这种频率差。这次我采用固定步长1ms，控制器子系统的采样时间设为2ms，模拟了一种近似真实飞控的频率关系。
1.3 Simulink环境里的总体架构规划
整个仿真的顶层结构我分成四个主要子系统：信号源与参考轨迹生成、控制器、执行器模型、多旋翼刚体动力学。另外还单独留了一个数据记录区，用来输出姿态角、角速度、位置、速度、控制量到工作空间。
参考轨迹部分我设计了三种：悬停点、8字轨迹、阶跃切换。其中8字轨迹最关键，因为它同时考验了外环跟踪能力和内环快速响应能力。
控制器子系统内部是串级结构：外环位置控制器输出期望速度或期望俯仰角/横滚角，内环姿态控制器输出期望力矩，再经过一个混控矩阵输出到四个电机的期望转速。执行器模型里需要加入一阶惯性环节，模拟电机响应延迟，这个环节虽然简单，但能不能稳定跟踪信号，很大程度上取决于它。
整体架构定下来之后，后面的工作基本就是往模块里填细节。
2. 刚体动力学建模：坐标系、方程组与参数准备的完整过程
2.1 两套坐标系和多套角度的选择
多旋翼动力学建模的第一步就是定义坐标系，这一步决定了后面所有方程的形式。我采用地面惯性坐标系和机体坐标系两套坐标。惯性系选择NED，即北东地，机体坐标系原点在飞行器质心，x轴指向机头方向，y轴指向右翼，z轴向下。选择NED而不是北天东的原因是飞控领域大多数惯例用NED，在数据读取和后续对接传感器时会更顺手。
姿态表示方面，我用欧拉角加旋转矩阵联合建模。欧拉角本质上描述的是机体坐标系相对惯性系的三个旋转角度，旋转顺序采用ZYX，也就是先绕z轴偏航，再绕y轴俯仰，最后绕x轴横滚，这个顺序也叫“航向-俯仰-横滚”顺序。欧拉角在建模时直观，但在后续控制器里我往往会换算成旋转矩阵来传递，这样既能避免部分奇异性问题，也方便在Simulink矩阵运算模块里直接处理。
这里有一个容易忽略但很关键的细节：角速度的坐标转换矩阵。欧拉角的导数与机体角速度之间不是简单的相等关系。假设偏航角为ψ、俯仰角为θ、横滚角为φ，那么机体角速度p、q、r与欧拉角速度的关系会带上一个耦合矩阵，里面包含tan函数项。如果仿真中俯仰角比较大，比如超过60度，tan项会快速增大，导致数值发散。我在建模时把这个转换写成独立的子函数，单独测试过欧拉角在小角度和大角度下的输出一致性。
2.2 力与力矩方程拆解
四旋翼的刚体动力学方程可以拆成两个层面：线运动方程和角运动方程。
线运动方程在惯性系下写为：
m * V_dot = F_gravity + F_thrust + F_drag
其中V是惯性系下的速度向量，m是飞行器总质量，F_gravity是重力，F_thrust是四个旋翼产生的总推力在惯性系下的投影，F_drag是空气阻力简化项。
角运动方程的常见形式是：
J * omega_dot = -omega × (J * omega) + M
其中J是转动惯量矩阵，omega是机体角速度向量，M是外部控制力矩。这里的陀螺力矩项-omega × (J·omega)在仿真中必须保留，特别是在做大幅机动和快速旋转时，否则控制效果会明显偏差。论文里可能对这个项一笔带过，但模型里少它不行。
把力方程和力矩方程联立起来，就构成一个完整的六自由度刚体模型。我习惯在S函数里把输入定义为总推力、横滚力矩、俯仰力矩、偏航力矩这四个控制量，输出为位置、速度、姿态角、角速度共12个状态量。S函数的状态导数计算严格按方程组展开，积分环节交给Simulink求解器处理。
2.3 电机推力与力矩的混控模型
控制器的输出是四个期望转速还是四个控制量？这取决于你仿真的抽象程度。精细一点的模型会让控制器输出期望转速，然后经过转速到推力的二次方程，再叠加反扭矩特性。粗粒度模型则直接让控制器输出总推力和三轴力矩，再通过混控矩阵分配。
这次我采用了折中方案：控制器输出总推力、横滚力矩、俯仰力矩、偏航力矩四个虚拟控制量，然后通过一个4x4混控矩阵逆算四个电机的期望拉力，再根据电机拉力系数和力矩系数算期望转速。混控矩阵的具体形式是：
- 总推力：T = K_T * (w1² + w2² + w3² + w4²)
- 横滚力矩：M_phi = L * K_T * (w4² - w2²)
- 俯仰力矩：M_theta = L * K_T * (w3² - w1²)
- 偏航力矩：M_psi = K_D * (w1² + w3² - w2² - w4²)
这里的wi是四个旋翼的转速，K_T是推力系数，K_D是反扭矩系数，L是机臂长度。混控矩阵写出来之后，记得验证一下矩阵是否满秩、符号方向是否和坐标轴一致，这个方向错了，仿真里飞机会直接“斗转星移”。
2.4 模型参数从哪来
论文里通常列出了一组参数表，包含质量、惯量、力臂长度、推力系数、阻力系数等。复现时如果论文给全了参数，直接代入就行。但很多论文的参数表并不完整，或者惯量矩阵只给了对角线分量，这时就需要估算或者查类似机型的公开数据。
我的处理方法是：先按论文给的参数初始化，缺的项参考等效机型的经验值补上，然后在仿真里通过悬停测试反推合理性。具体来说，悬停状态下总推力必须等于重力，通过这个约束可以反推推力系数的量级。如果算出来某旋翼转速远超出实物的量程，那就要回头调整参数了。
参数这块还有一个衍生的经验：转动惯量不要只填对角项。虽然很多论文为了简化只列了Jxx、Jyy、Jzz，但仿真中如果加入交叉惯量项或者忽略陀螺力矩，在快速偏航时会看到明显的姿态耦合现象。我的做法是把论文里省略的交叉项先置为零，跑通之后再手动加上一个小的耦合系数，测试控制器是否能补偿掉这部分耦合。
3. 内外环串级PID控制系统设计：从结构选择到参数整定的实操逻辑
3.1 为什么用串级结构而不是单环PID
很多人一开始会想：既然要控制位置，直接把位置误差送给一个PID，输出控制量不就行了吗？实际上这样做的效果很差，因为位置和推力之间隔着一层姿态动力学，单环PID很难同时兼顾位置收敛速度和姿态稳定，容易出现超调大、抖动明显的问题。
串级控制的设计思路是把整个系统分层：外环负责位置跟踪，输出期望姿态角；内环负责姿态跟踪，输出期望力矩。这样每一层只做一件事，控制器设计难度大大降低，而且内环响应速度远快于外环，两个环不会打架。
我这次用的是经典三环结构：位置环在最外面，速度环在中间，姿态环在最里面。位置环的输出是期望速度，速度环的输出是期望俯仰角和横滚角以及期望偏航角速率，姿态环的输出是期望力矩。事实上很多飞控的PIX和PX4架构也是类似思路，把位置控制和姿态控制解耦，便于工程调试。
3.2 姿态环的PID设计与限幅
姿态环的输入是期望姿态角和实际姿态角的误差，输出是三轴期望力矩。俯仰和横滚轴我用PD控制器为主，比例项提供恢复力矩，微分项提供阻尼。偏航轴因为响应比较慢，加入了积分项来消除静态误差。
姿态角误差的处理有一个容易踩的细节：当期望角是170度，实际角是-170度时，误差不是340度而是20度，必须把误差做归一化处理，映射到[-π, π]区间。直接在S函数或模块里把两者相减，遇到边界情况模型会突然反转。我当时在模型里加了一个角度误差归一化函数，专门处理这个问题。
控制力矩输出之后还需要经过一个饱和限幅，防止控制器在误差较大时输出过大力矩导致电机转速饱和。我在限幅之前加了一个积分分离逻辑：当误差绝对值超过阈值时，积分项不累积，只有误差落入线性区间才恢复积分作用。这样既能消除稳态误差，又不会在大误差时积分饱和。
3.3 位置环与速度环的设计
位置环在仿真里的表现直接决定参考轨迹跟踪效果。位置环的PID参数设计重点在比例项，让飞行器尽快回到参考轨迹上。因为位置环的输出是期望速度，它本身不会直接产生过大的振荡，真正的振荡往往来自速度环和姿态环之间的相位差。
速度环的输出是期望俯仰角和期望横滚角，这里要做一个非常重要的转换：把平动方向的需求映射成姿态角需求。简单来说，想要往前加速度，就需要俯仰角；想要往右加速度，就需要横滚角。但这个角度不能直接等于误差乘以比例系数，还要限制最大倾斜角，一般限制在30度以内。我在仿真里对期望姿态角加了斜坡限制，避免阶跃信号直接砸到内环上，这样仿真曲线会更平滑，也更符合实际飞控的限幅逻辑。
偏航通道单独处理，和位置控制解耦。因为四旋翼可以独立偏航，所以偏航参考轨迹可以单独给定，位置环不需要考虑偏航角对位置的影响。
3.4 串级参数整定的常用顺序
串级PID有一个很实用的调参顺序：先内后外，先比例后微分，最后再加积分。我的实际操作步骤如下：
第一步，把外环全部禁用，手动给定一个固定姿态角，只调内环姿态环的PD参数。给一个期望俯仰角阶跃，观察响应曲线，调整比例增益直到既快又稳，再增大微分增益压住超调。
第二步，恢复姿态环，把位置环和速度环解开，单独调速度环。给一个期望速度阶跃，观察飞行器能否快速达到指定速度且没有明显振荡。
第三步，把速度环和位置环串起来，调位置环比例增益。这一步要特别留意位置环增益不能给太大，否则控制器会把小的位置误差放大成大的速度指令，速度环又快速去追，最终在内环造成剧烈振荡。
这个过程中我基本不用自动化整定工具，还是手动一点点试。每次调整完参数，我都会跑同一组参考轨迹，对比响应曲线变化。仿真环境的好处是修改参数只需要几十秒，能非常密集地做调参实验。
4. Simulink仿真架构搭建与验证：把控制器“焊”进模型里的细节
4.1 顶层模型结构设计
Simulink模型我分成了三个层级：顶层是总装图，包含参考轨迹信号源、控制器子系统、执行器模型、动力学模型、数据记录模块；第二层是控制器的内部细节，姿态环和位置环分开；第三层是各个环里的PID模块及限幅逻辑。
顶层的信号流向是单向的，不允许有代数环。如果信号在同一个步长内既作为输出又作为输入，Simulink会报代数环错误，或者求解器迭代次数过多导致仿真极慢。我遇到过一次因为输出限幅模块直接引用输入导致的代数环，解决办法是在反馈回路上加一个Memory模块或者单位延迟，打破直接耦合。
为了方便对比论文曲线，我在数据记录模块里加了一个多路输出结构，直接用To Workspace模块把需要的数据存成结构体，后处理用MATLAB脚本统一画图。这样比每次都截图Simulink里Scope方便得多，尤其在反复调参时。
4.2 S函数与模块化建模的选择
动力学部分用C语言写的S函数，主要考虑是执行效率高、公式可控。S函数的写法比较固定，需要实现初始化、导数计算、输出三个函数。在导数计算函数里，按照第二节里的动力学方程组逐个计算状态导数，然后从输入端口读入推力、力矩、电机转速等控制量。
如果你对S函数不熟悉，还有一个替代方案：用Simulink自带的6DOF（Euler Angles）模块。它内部已经实现了完整的多旋翼刚体运动方程，只需要配置质量和惯量等参数就行。但在配置时要注意，这个模块用的是惯性系到机体系的旋转矩阵配置，旋转顺序和你论文里的一致性要仔细核对。如果两者不一致，后面控制器做的所有姿态逻辑都要跟着调，非常麻烦。
我的建议是：如果你是复现论文且后续可能修改模型结构，用自建S函数更稳妥；如果你只是快速验证一个控制算法，用内置6DOF模块更快。
4.3 仿真配置与信号标定
仿真配置我用了固定步长求解器，选的是ode4（四阶龙格库塔），步长1ms。为什么不选变步长？因为控制器、执行器、动力学模型在变步长下可能因为事件触发导致步长调整，信号曲线会出现不该有的毛刺，固定步长可以让问题集中在算法本身，而不是数值求解。
信号标定方面，所有角度信号统一用弧度，除了在输出到Scope和绘图时转换回角度。这个看似小的问题，在调试时浪费了我不少时间。混控矩阵里的角度量、PID模块里的期望角速度、参考轨迹生成器中轨迹方程的角度参数，要么全用度、要么全用弧度，不统一就等着看飞出去的“火箭”。
执行器模型里的一阶惯性环节时间常数，我设为20ms。这个值来源于论文里电机响应时间的量级。如果时间常数太大，系统相位滞后的影响会非常明显，控制器需要降低增益来补偿；如果太小，仿真曲线会很“理想”，但和实物差距大。建议复现时保留这个环节，并做一组“有执行器延迟”和“无执行器延迟”的对照实验，你会看到控制器边界明显不同。
4.4 三种验证场景
跑通模型之后，我设计了三种验证场景，这样能系统性地判断复现是否成功。
第一种是悬停验证：初始位置偏离期望点1米，三个姿态角初始偏差10度，看飞行器能否收敛到期望位置并稳定。这个场景主要看姿态环的稳定性和位置环的收敛能力。
第二种是8字轨迹跟踪：参考轨迹的水平位置是正弦和余弦的组合，飞行器需要同步改变位置和偏航角。这个场景主要看内外环协同效果，以及轨迹跟踪动态误差。
第三种是抗扰动测试：在仿真过程中注入一个持续0.5秒的力矩扰动，模拟阵风作用，观察姿态角和位置误差能否快速回落。这个场景用来验证控制器鲁棒性和积分项抗扰能力。
每次验证都会把误差曲线保存下来，和论文里的仿真结果对比。论文里如果给出了稳态误差上界或者动态超调量，我会直接作为量化指标来对照。
5. 论文复现里绕不开的坑与排查经验
5.1 欧拉角奇异与矩阵求解失败
仿真跑了一段时间后，我突然发现姿态输出从正常值跳到了NaN，整个曲线瞬间崩溃。定位后发现是欧拉角转换矩阵里的cosθ分母项接近了零，俯仰角接近正负90度时，矩阵退化导致数值爆炸。
解决方式有两个方向：一个是在姿态环控制器里限制期望俯仰角，不让它超过±45度，从机制上避免进入奇异区；另一个是改用四元数表示姿态，避免欧拉角的奇异问题。复现论文阶段我选择了前一种，限制最大期望姿态角，和真实飞控的限幅逻辑也一致。如果你要模拟花式翻滚类机动，那必须用四元数，没有商量余地。
5.2 速度反馈的噪声与微分放大效应
刚开始纯理想模型里姿态角加速度没有噪声，仿真曲线很漂亮。但我想验证控制器在噪声下的表现，就给角速度反馈加了一个高斯白噪声，结果PID的微分项立刻把噪声放大了好几倍，控制量出现了高频抖振。这个抖振又会通过动力学模型反馈到姿态，形成恶性循环。
仿真里遇到这个问题，可以先检查PID微分项在误差微分时的噪声放大程度，再考虑加低通滤波。低通滤波的截止频率设置在30Hz到50Hz之间比较合理。加滤波之后控制量平滑了很多。从复现论文的角度，论文如果没提噪声处理，你可以在“改进与讨论”部分把这个作为优化点写出来。
5.3 积分项导致的超调与迟滞
悬停验证时，位置环加了积分项后，收敛时间变长，还出现了约10%的超调。排查发现是积分项在初始误差较大时就已经积了很大值，等飞行器接近目标时，积分还没有退下来，把飞行器“推过头”了。
处理办法是积分限幅加积分分离。我把积分输出限制在最大控制量的20%以内，并且在误差超过阈值时暂停积分累积。这样大误差阶段靠比例控制快速收敛，小误差阶段靠积分消除静态偏差。这个改动对改善超调非常有效，强烈建议你在模型里保留这两段逻辑。
5.4 参考轨迹信号不连续导致的内环冲击
8字轨迹跟踪实验时，我一开始直接用正弦信号的导数做期望速度指令，但没有考虑速度指令本身的变化率上限。在轨迹起点，位置期望从零瞬间跳到非零，导致速度指令出现阶跃，姿态环也被迫瞬间输出大角度，曲线出现了尖峰。
解决办法是在信号源和控制器之间加了一个动态限幅模块，对期望速度的加速度做斜坡限制。换到实际飞控里，这就相当于轨迹规划阶段对速度和加速度进行限制。复现论文时，如果论文里的参考轨迹是平滑的，你需要检查自己的轨迹生成是否足够平滑；不平滑的情况下，仿真结果看起来“炸了”不一定是控制器问题，先查轨迹输入。
6. 从复现走向改进：下一步还能做哪些扩展
6.1 做“论文结果对比”要避开的误区
复现论文时最容易犯的错误是直接拿自己的仿真曲线和论文曲线对比，然后因为“数值不完全一样”就开始怀疑自己。实际上完全一致的复现几乎不存在，因为论文里的初始条件说明往往不完整，控制器参数可能只给了最终整定值，没有给整定过程。
更合理的做法是定义一组量化指标来做对比，比如上升时间、超调量、稳态误差、跟踪误差RMS。把这些指标从论文图中读出，再在你自己模型里按统一场景重新算一遍，对比量级是否有显著差异。差异在10%到20%以内，基本可以认为复现成功。
6.2 模型保真度升级的方向
当前的模型里电机用一阶惯性环节简化，忽略了电压、电流、螺旋桨气动干扰这些因素。如果想往“数字孪生”方向走，可以逐步加入电池电压降、螺旋桨的拉力系数随前进速度变化模型、机架弹性形变的影响。每加一项，控制器参数大概率都要重新整定一遍。
我实际测试过一个小升级：在混控矩阵里加入拉力系数随转速变化的修正项，结果位置环的稳态误差改善了约5%。这说明模型层面的改进确实能提升控制效果，不是只在控制器里做文章。
6.3 从仿真到实验的鸿沟
仿真做得再漂亮，和实物还是有一段距离。仿真里参数是固定的，但实物的电池电量不同，推力系数随温度变化，传感器噪声也远不是高斯白噪声能描述的。我的经验是：仿真阶段主要验证控制结构和参数整定的大致方向，真正的微调一定是在实机上完成的。
如果你复现的目的不只是应付论文，还打算迁移到真实的飞控硬件上，建议在仿真里加入传感器延迟、数据丢包、执行器饱和这些工程因素。仿真的价值在于用低风险、低成本的方式先剔除那些不合理的控制参数，而不是替代实验。把这一层想清楚，复现的工作就不会停留在“调出一条漂亮的曲线”上，而是真正帮你理解飞行器控制系统的全貌。
