基于MATLAB/Simulink的四旋翼动力学建模与PID控制仿真

简介本资源是一套面向控制工程与无人机方向初学者及进阶学习者的Matlab/Simulink实践项目聚焦四旋翼飞行器动力学建模与PID控制器设计解决无人系统建模仿真与基础闭环控制的核心问题。压缩包共含多个Simulink模型文件.slx、MATLAB脚本.m及参数配置说明文档涵盖牛顿-欧拉运动方程建模、旋翼升力/扭矩计算、欧拉角姿态转换、六自由度动力学模块搭建以及高度/姿态双环PID控制器设计与参数整定过程便于读者理解非线性耦合系统的简化建模思路与经典控制应用逻辑。资源大小为841KB结构紧凑、模块清晰所有模型均支持直接仿真运行与参数调试。目前已有620人学习下载适合高校课程设计、毕业设计前期验证及自主无人机控制入门实践可快速掌握从物理建模到控制器实现的完整技术链路。 先说明一下这次要写的内容是很多做飞控、做课程设计、做毕设的朋友绕不开的一个题目——用 MATLAB/Simulink 给四旋翼做动力学建模再套上 PID 控制器跑通“从电机转速到位置姿态”的完整仿真链路。我从最早照着教材抄模型、一仿真就发散到现在能把模型结构、控制参数、仿真配置一次性理顺中间踩了不少坑这篇就把完整流程、核心公式、模块搭建方式、调参过程和常见报错一次性讲透。这篇内容适合三类人看一是刚接触四旋翼仿真、不知道从哪下手的学生二是已经搭好模型但控制不稳定、想系统性调 PID 的人三是想快速验证新控制算法、拿 Simulink 当试验床的开发者。你能从这里得到的不是一段演示代码而是一套可复现的建模与控制框架。1. 为什么要在 Simulink 里搭四旋翼动力学模型1.1 三种建模方式的对比和选型依据四旋翼的动力学建模常见路子有三条纯手写微分方程用 MATLAB 脚本跑仿真、用 Simscape Multibody 搭多体模型、用 Simulink 基础模块手搭六自由度模型。我三种都试过说下真实感受。纯脚本方式比如用 ode45 解状态方程优点是代码紧凑、完全可控缺点是可视化差控制器结构稍复杂一点代码维护成本就上来了。而且 PID 控制器的试凑过程让人抓狂改一个参数要重新跑一遍脚本看不到实时曲线变化交互性太差。Simscape Multibody 则正好相反三维可视化很强可以直观看到机体姿态变化但它更适合做机械结构验证对数学模型和控制算法的“透明度”不如手搭模型。加上它引入的求解器复杂度和运行开销都不小特别当你想把控制律细节看得很清楚时模型内部逻辑反而是模糊的。相比之下用 Simulink 基础模块手搭动力学模型是最平衡的方案。你可以把牛顿-欧拉方程一个个模块地搭出来每个积分器、每个增益都有明确的物理含义PID 控制器可以直接用现成的 PID Controller 模块也可以自己搭成比例、积分、微分三路想改哪一路就改哪一路更关键的是整个模型天然支持信号跟踪方便把控制量、姿态角、位置曲线一次性拉出来分析。这也是为什么绝大多数教材和飞控开源项目都选择这种形式。1.2 建模前必须理清的几个基础概念四旋翼动力学建模本质就是在描述“力怎么让机体加速、力矩怎么让机体转动”。这句话听起来简单但里面藏着三个容易翻车的概念点。第一个是参考坐标系。四旋翼的位置和速度通常在地面系导航系里描述而推力和气动力在机体系里描述两者之间要靠姿态角转换。很多人建模失败不是方程错了而是坐标系混了——比如把机体坐标系下的推力直接加到地面系运动方程里。第二个是姿态描述方式。最常用的是欧拉角横滚、俯仰、偏航它直观、好理解但存在万向节锁问题。四旋翼仿真里只要你不是刻意做剧烈翻滚到 90 度的机动欧拉角就够用。建模时用机体角速度作为状态量通过旋转矩阵跟欧拉角速率互转这一步关系必须理清。第三个是电机模型。很多入门教程把电机转速直接当成输入忽略了电机响应是一个一阶惯性环节。实际项目中如果电机响应延迟没建模控制器参数可能在仿真里看着没问题一上真机就振荡发散。所以在 Simulink 模型里我习惯在控制分配之后、动力学模型之前加一个一阶惯性环节来模拟电机和执行机构的响应延迟。这三个点理清了后面搭模型基本就是“体力活”而不是“烧脑活”。2. 四旋翼动力学建模的完整推导与 Simulink 实现2.1 坐标系定义和运动变量先说坐标系。地面坐标系 E 固定在地面X 指北、Y 指东、Z 向下注意这里如果用 Z 向上后面重力符号很容易写错我一般直接统一成 Z 向上重力加速度取负。机体坐标系 B 固连在机体上原点在质心X 指向机头Y 指向右翼Z 指向下右手定则这样角速度正方向跟右手定则一致后面写力矩方程不容易乱。四旋翼的状态变量一般取 12 个地面系位置 P_e [x, y, z]、地面系速度 V_e [u, v, w]、机体角速度 ω_b [p, q, r]、欧拉角 Θ [φ, θ, ψ]。为什么不用地面系角速度而用机体角速度因为转动惯量和力矩方程在机体坐标系下最简洁惯量矩阵是对角阵不用做复杂的坐标变换。欧拉角和机体角速度的关系公式是φ_dot p q * sin(φ) * tan(θ) r * cos(φ) * tan(θ)θ_dot q * cos(φ) - r * sin(φ)ψ_dot q * sin(φ) / cos(θ) r * cos(φ) / cos(θ)这个关系式看着复杂但在 Simulink 里实现很简单用一个 MATLAB Function 或 Fcn 模块就能搞定。需要注意俯仰角接近 90 度时tan 会发散这也是欧拉角描述方式的固有缺陷。2.2 力与力矩方程接下来是核心的动力学方程。四旋翼受到的力主要有三个重力、四个旋翼产生的总推力、空气阻力。在机体坐标系下总推力总是沿 Z 轴负方向因为旋翼向上拉机体被向上提而重力在地面系表达最方便。把线运动方程写在地面系下会更直观m * ddot(P_e) [0, 0, -m * g] R_be * [0, 0, -T_sum]其中 R_be 是从机体坐标系到地面坐标系的旋转矩阵T_sum 是四个旋翼升力之和。升力公式是 T_i k * w_i^2k 是升力系数w_i 是旋翼转速。空气阻力项可以加一个与速度成正比的阻尼力F_drag -k_d * V_e如果不加也可以但加上之后模型更接近真实飞行。角运动方程用牛顿-欧拉方程在机体坐标系下表达I * ω_dot ω × (I * ω) M其中 I diag(Jx, Jy, Jz)M 是合外力矩。展开后得到三个角加速度方程p_dot (Jy - Jz) * q * r / Jx M_x / Jxq_dot (Jz - Jx) * p * r / Jy M_y / Jyr_dot (Jx - Jy) * p * q / Jz M_z / Jz合外力矩来自四个方面横滚力矩由 2 号和 4 号电机转速差产生俯仰力矩由 1 号和 3 号电机转速差产生偏航力矩由两组对转旋翼的反扭矩差产生此外还有旋翼陀螺效应但小尺寸四旋翼在一般飞行条件下可以忽略先不加。2.3 Simulink 模型的具体搭建方式搭模型的时候我推荐按“输入-状态更新-输出”的思路拆分。模型的输入是四个电机的转速指令四个标量内部经过升力和力矩计算得到机体坐标系下的总力和总力矩再经过旋转矩阵和欧拉方程更新位置和姿态状态。输出则是 12 个状态量供控制器使用。具体模块安排可以参考下面的方式输入层四个 Inport 模块接收控制器给出的转速指令 w1、w2、w3、w4。力/力矩计算层用 Gain 模块做平方运算w^2然后用 Constant 模块配置升力系数、力臂长度、反扭矩系数通过 Sum 模块把力和力矩合成。线运动方程层矩阵相乘模块Matrix Multiply做旋转矩阵乘推力向量得到地面系下推力分量再跟重力向量求和除以质量得到加速度经过两组 Integrator 得到速度和位置。角运动方程层用 Fcn 模块或者 MATLAB Function 写入 p_dot、q_dot、r_dot 表达式经 Integrator 得到角速度再通过欧拉角速率关系更新欧拉角。输出层用 Mux 或 Bus Creator 把位置、速度、姿态、角速度打包输出方便控制器取用。这里有个特别要提醒的点旋转矩阵模块不要自己手推一堆三角函数模块容易错还不直观。建议直接用 MATLAB Function 写一个函数输入是欧拉角输出是旋转矩阵这样代码可读性好复查也方便。类似的欧拉角速率的计算也建议集中写在一个函数里。3. 串级 PID 控制器设计与参数整定3.1 为什么是串级而不是单级四旋翼的控制目标通常是跟踪期望位置和期望偏航角。如果只用一个 PID 直接输出电机转速等于让控制器同时面对位置误差、姿态误差、角速度误差三个层面的问题参数极难整定鲁棒性也很差。串级控制的思路是外环位置控制器算出期望姿态角主要是横滚和俯仰内环姿态控制器跟踪这个期望姿态角。而更稳妥的做法是三层嵌套——最外环位置环输出期望速度速度环输出期望姿态角姿态环内部又分角度环和角速度环。不过为了平衡复杂度和可调性四旋翼仿真里最常用的是“位置外环 姿态内外环”串级结构这里我把姿态环拆成外环角度、内环角速度两层。内外环带宽要有明显差距。简单理解就是内环要响应得比外环快得多一般内环带宽是外环的 5 到 10 倍否则外环给一个姿态指令内环半天追不上整个闭环就会振荡甚至发散。具体到参数上角速度环的 P 通常比角度环的 P 大一个数量级位置环的 P 又要比角度环小很多。3.2 PID 参数初值与整定顺序整定顺序我强烈建议从最内层开始一层一层往外调。因为外层依赖内层收敛如果内环没整好外环参数再漂亮也白搭。最内层是角速度环。给一个恒定角速度期望值比如期望横滚角速度 1 rad/s看实际角速度的响应曲线先调 P 让响应快速跟上再调 D 抑制超调最后如果需要消除稳态误差加少量 I。角速度环的 I 一般不用太大因为角速度环的稳态误差通常不明显I 加多了反而容易引起低频振荡。角度环在角速度环稳定后可调。期望角度给一个阶跃比如横滚角从 0 到 0.1 rad约 5.7 度观察角度响应曲线。P 太大容易让角速度指令饱和P 太小响应太慢D 用于增加阻尼。位置环最后调同样从 P 开始。我给出一个经过实测的初始参数参考表注意是仿真用单位统一为国际单位控制回路PID说明角速度环横滚/俯仰2.50.050.1输出限幅 ±12 rad/s角速度环偏航1.50.020.05偏航响应要求低角度环横滚/俯仰6.00.010.5输出限幅 ±0.5 rad角度环偏航2.00.00.2不轻易加积分位置环Z2.00.10.5输出是期望高度加速度位置环X/Y1.50.050.3输出转换为期望姿态角这套参数不是万能药但作为初值基本都能稳定之后按实际情况微调。3.3 控制分配从期望力和力矩到电机转速PID 控制器输出的是总推力、横滚力矩、俯仰力矩、偏航力矩四个量但实际执行机构是四个电机的转速所以必须做控制分配。四旋翼的分配矩阵很简单。假设电机 1 在前、电机 2 在右、电机 3 在后、电机 4 在左顺时针编号力臂为 L升力系数为 k反扭矩系数为 k_m那么四个期望量跟四个转速平方的关系是T_total k * (w1^2 w2^2 w3^2 w4^2)M_x k * L * (w2^2 - w4^2)M_y k * L * (w3^2 - w1^2)M_z k_m * (w1^2 - w2^2 w3^2 - w4^2)这个方程组可以写成矩阵形式然后用矩阵求逆直接得到四个转速平方。Simulink 里用 Gain 模块配合 Matrix Divide 就能实现或者直接手算逆矩阵写成四个线性表达式。注意计算出的转速平方要开方并限幅确保在电机可执行范围内。4. Simulink 模型实现与仿真实战4.1 推荐的文件结构与初始化脚本仿真项目我也建议保持清晰的文件组织不要把所有东西都塞在一个 slx 文件里。我的习惯是三个文件init_params.m 存放所有物理参数和控制参数main_model.slx 存放仿真模型plot_results.m 负责绘图分析。init_params.m 里面定义以下参数Simulink 模型里的所有 Gain、Constant 都引用工作区变量质量 m 1.2 kg典型 450 级四旋翼重力加速度 g 9.81 m/s^2力臂 L 0.3 m升力系数 k 1.5e-5 N/(rad/s)^2反扭矩系数 k_m 3e-6 N·m/(rad/s)^2转动惯量 Jx Jy 0.015 kg·m^2Jz 0.025 kg·m^2电机响应时间常数 tau_motor 0.02 s悬停转速 w_hover sqrt(m * g / (4 * k))这样做的最大好处是调整参数不用打开 Simulink 模型一个个找模块改一行脚本重新运行就行。我见过不少同学把参数硬编码在模型里最后想批量调参只能手动改几十个模块效率非常低。4.2 仿真配置和激励信号设计求解器选择这个细节很多人不注意但它对仿真稳定性影响很大。四旋翼动力学是一个刚性问题因为电机响应时间常数为 0.02 秒而位置响应的特征时间可能是几秒跨越多个时间尺度。我推荐使用变步长求解器 ode45默认相对误差设为 1e-5 或更小最大步长限制在 0.01 秒以内。如果固定步长比如用 ode4步长至少要取 0.001 秒才够稳但这样仿真速度会明显下降。仿真激励信号建议分阶段设计。先从悬停开始验证模型正确性期望位置设为原地悬停期望偏航角为 0初始姿态角全部设为 0观察四旋翼是否能保持稳定。模型正确的前提下即使控制参数不是最优也应该能维持在一个小范围内振荡而不是瞬间发散。稳定后再给定一个高度阶跃从 0 米上升到 1 米观察高度响应曲线最后加一个水平位置阶跃比如期望 x 从 0 变为 2 米观察整个位置跟踪的动态过程。期望信号在 Simulink 里可以用 Step 模块但更推荐用 Signal Builder 或者 From Workspace 模块加自定义时间序列因为可以方便地设置多段阶梯信号比如 0-5 秒悬停、5-10 秒上升、10-20 秒平移一次仿真把所有控制性能看个遍。4.3 实操调参记录我从发散到稳定的全过程这里分享一下我最开始调参数的完整记录这对新手参考价值很大。第一轮仿真我直接用了教材上的参数结果模型 2 秒内就发散位置和姿态曲线直接飞掉。检查发现原因有两个一是初始欧拉角设成了角度制单位而控制器内部用弧度制导致初始误差看起来巨大控制器输出饱和二是角速度环 D 参数为 0姿态环阻尼不足一旦出现微小扰动就被放大。把单位问题修正后第二轮仿真模型没有发散但出现了持续振荡横滚角在 ±0.3 rad 之间来回震荡。这是典型的比例增益过大、微分增益不足。我把角度环 P 从 8 降到 4同时把 D 从 0 加到 0.4振荡幅度明显收敛。第三轮仿真的问题是高度始终有约 0.1 米的稳态误差这是因为位置环缺少积分作用旋翼升力又跟转速平方成正比微小偏差无法消除。给位置环 Z 加了 I 0.05 之后稳态误差基本消失但出现了轻微的超调接着适当增加 D 到 0.8超调被压住了。这轮调参下来我得出的体会是参数整定不是一次到位的每次改参数只看一个指标的变化不要同时动多组参数否则你根本不知道是哪个改动起了作用。建议每次只改一个参数跑完记录下曲线再改下一个虽然慢但你能真正建立“参数-响应”的直觉。5. 常见问题与排查技巧实录5.1 一仿真就发散先查这五个地方四旋翼仿真发散的原因就那么几类排查顺序也很固定首先查单位。欧拉角是不是弧度位置是不是米速度是不是米每秒角度和弧度混用是最常见也最隐蔽的错误如果控制器里一个是角度输出、一个是弧度输出运算结果会完全乱套。其次查电机转速初始化。模型启动瞬间电机转速应该初始化为悬停转速附近的平衡值如果初始转速设为 0控制器一启动就会输出巨大的转速指令去补偿重力很容易让仿真发散或数值溢出。然后查控制极性。PID 输出的正负方向跟电机转速增减方向是否一致比如横滚力矩应该让左侧电机加速、右侧电机减速如果方向反了控制器会把姿态误差越拉越大形成正反馈这种发散速度极快很容易误判成模型问题。接着查积分饱和。PID 的积分项如果无限累积实际输出早就超过电机限幅了恢复过程会非常慢看起来像失控。解决方法是给积分项和输出都加饱和限幅。最后查代数环。Simulink 里如果出现信号环回也就是从输出又直接返回输入没有经过任何延迟模块求解器会报代数环错误或计算缓慢。解决办法是在反馈路径上插入一个 Memory 模块或单位延迟打破代数循环。这个在飞控仿真里特别常见因为控制器天然就是有反馈的回路读者如果看到仿真速度突然变慢或者报错优先检查这块。5.2 模型稳定但实际飞行不行模型保真度问题有一种情况是仿真里 PID 参数调得漂漂亮亮但放在真机上完全不工作。这里我要强调仿真模型的保真度决定了参数迁移的效果。最容易导致“仿真能飞、真机炸机”的模型假设有三条。一是忽略了电机延迟。如果模型里电机转速瞬间响应控制器参数容易调得偏激进真机上电机需要几十毫秒才能跟上指令直接导致高频振荡。建模时务必加上电机一阶惯性环节。二是忽略了测量噪声和采样延迟。真机上的陀螺仪和加速度计都有噪声姿态估计还有延迟如果仿真模型里反馈信号是完美干净的控制器的微分项就会非常敏感实际中会把噪声放大。建议在反馈信号上叠加一个小的高斯白噪声再测试控制器的鲁棒性。三是忽略了执行机构饱和。电机转速有上下限控制分配后给出的转速指令如果超过上限实际输出会卡在限幅值上形成非线性特性而这正是很多控制器“仿真时好、真机发散”的根源。在仿真里也要加上饱和模块。5.3 调参效率低用脚本批量扫描参数手动试凑参数虽然能积累经验但效率确实低。我的做法是先用 init_params.m 定义参数再用 sim 命令在脚本里跑仿真配合 for 循环批量扫描参数组合。比如我想看 P 从 1 到 10 的效果就写一个循环每次修改参数、运行 sim、提取输出曲线再把多条曲线画在一张图上对比。一次扫描几十组参数运行时间可能只要几分钟比手动点仿真快多了。批量扫描还能帮你画出参数-性能的响应面。比如把横滚角速度环的 P 作为横轴、角度环的 P 作为纵轴用超调量和调节时间作为颜色就能直观看出稳定区域在哪里。这个思路虽然朴素但在模型结构不同、参数耦合明显的四旋翼系统里非常实用能大幅减少盲目调参的时间。6. 让模型更接近真实工程的一些扩展思路6.1 加入传感器模型和噪声如果目标是做控制算法验证而不是纯理论仿真建议在模型反馈路径上加入传感器仿真。陀螺仪输出角速度但是带有零偏和噪声加速度计输出比力而不是纯加速度磁力计易受干扰GPS 更新率低且有延迟。这些在 Simulink 里都有现成模块或者用 Random Number 模块加噪声、用 Transport Delay 模块加延迟改造工作量不大但模型的理论参考价值会高很多。控制端也可以做数字控制器离散化。早期在连续域里调好的 PID实际飞控里都是离散执行的仿真时把 PID Controller 模块的采样时间设成跟真实飞控一致比如 250 Hz 甚至 1000 Hz看控制性能会不会恶化。如果性能退化明显就需要在离散域重新整定参数。6.2 从 PID 扩展到更现代的控制器PID 调通之后如果想继续深入可以在这个模型上做很多扩展。比如把位置环的 PID 改成 LQR 或 MPC姿态内环保留 PID 做快速响应也可以加前馈控制用期望加速度前馈补偿重力减小外环的负担还可以加抗风扰动模块用随机风场作为外部干扰测试控制器的鲁棒性。这些扩展最大的优势在于底层的动力学模型不用动只改控制器层的模块即可。这也是我在 Simulink 里搭建模型而不是写死脚本的最大原因——后期迭代极其方便。对于做科研的同学这个模型可以一路支撑从 PID 到滑模控制、模型预测控制的全部仿真需求。6.3 和硬件在环仿真的衔接最后提一个容易被忽视的点Simulink 四旋翼模型的另一个重要用途是硬件在环HIL仿真。把动力学模型编译成可执行代码跑在实时仿真机上飞控板通过真实接口跟仿真机通信就能在不插桨、不飞真机的前提下测试整个飞控代码。这一步对开发流程的价值极大很多团队在代码上机前都要先过一遍 HIL。从模型到 HIL关键是把模型离散化、固定步长化并且处理好接口信号的类型和时间戳。这里不展开但你在建模型时如果从一开始就把输入输出接口定义清楚后续迁移到 HIL 会比从头改模型省非常多的力气。最后分享一个让我少走弯路的小习惯每次仿真前我都会先跑一遍 init_params.m然后用 whos 检查工作区变量是否齐全再快速看一眼模型的初始状态和电机初始转速。这个十几秒的习惯帮我提前发现过单位错误、参数缺失、初始状态不合理等大量低级问题至少省下了几十个小时的 debug 时间。做四旋翼仿真模型结构本身不复杂复杂的是细节把每一个可能埋坑的地方提前排掉后面的路会顺畅得多。本文还有配套的精品资源点击获取