本文共分为数篇文章介绍“基于MATLAB/Simulink的四旋翼无人机PID控制与仿真”：
1、前言与四旋翼动力学与控制模型（点此跳转）
2、四旋翼控制系统设计与基于MATLAB/Simulink的仿真模型搭建（点此跳转）
3、系统模型完善与飞行控制测试<<（您在此处）
4、针对正弦输入稳定控制的系统改进（待更新）
5、控制系统的离散化与c++飞控代码导出（待更新）
感谢读者朋友们的支持，建议大家按上述顺序分篇阅读本文~
时隔小半年，终于更新了本系列的第三篇~欢迎大家阅览并提出意见！
————————————————
目录
一、前言
1、承上启下
在上一篇文章最后，我们已经搭建了基础的四旋翼无人机PID控制系统的数字仿真模型~
需要再次补充说明的是，上一篇最终的模型我们所期望达到的成果是，仿真程序在点击“运行”后能够完整通顺地运行完毕，期间不报红色error错误（跑通即可，不必理会黄色warning警告，无论有多少警告）。
有很多读者朋友私信我或者评论反映，按照文章中的模型进行搭建出了很多问题，没有办法正常跑通，于是在这里进行一下如下的几点说明：
(1) 【情绪稳定】首先，从写程序到搞仿真，无论是任何工程设计都会出现bug。遇到bug不要着急~首先根据报错的内容进行仔细的研读。在这里，不少读者因为英语水平受限，无法get到所报错误的含义，这就需要加倍耐心~
(2) 【夯实基础】其次，当然是在完备地掌握程序或仿真系统的基本原理之后，更容易地从原理意义上理解错误并解决。以本篇为例，个人认为，对于本专业领域的读者在具备基本的线性代数、理论力学、信号与系统、自动控制原理知识基础上，能够完全掌握本项目所涉及的全部理论。对于来自其他专业领域的读者，可以简单参考这几门课程的基础内容~
(3) 【分而治之】对于读者朋友们目前反映的两个主要的系统报错：
1、系统报存在singularity错误，或是sqrt环节存在negative value input等，此时建议在每一个sqrt环节前接入一个scope，检查仿真过程中的信号输入，大概率是出现了负值，解决办法为在sqrt环节前接入sat（saturation饱和）环节，设置信号下限为某一小正值即可；
2、系统报matrix multiply dimension云云的错误，并且在系统中数个接口处显示问号，此时检查每一个接口的向量或矩阵维数即可，保证每个矩阵乘法和加法都符合最基本的矩阵理论即可。
上面两个错误都是十分容易解决的错误，如果有读者遇到其他错误也是人之常情，可以评论或私信~
(4) 【细水长流】最后，上一篇文章结尾所搭建完毕的系统，是本项目的v0版本，也是最基础的版本。鉴于本人在本项目的研究，目前已经更新至v11版本，后面进行了非常多的更新和改进。
本篇作为本文的第三篇，将为大家继续展示并完善v0版本。本文将在前文的基础上，完成控制环节增补、参数设置、测试等工作，在不考虑性能（低超调、快、准等）的前提下，保证系统对于一般输入（阶跃、斜坡）的响应稳定性。
如果对于后续版本以及本人研究方向感兴趣的读者，欢迎进一步评论与私信~
2、其他的一些说明
本文适读人群：飞行器控制与导航相关专业，如自动化、电子信息工程、机械工程、仪器科学与技术、控制科学与工程、航空航天工程等专业的本科生及研究生。
本文关键词：飞行器控制、多旋翼、动力学系统仿真、控制系统仿真。
主要参考书籍：《多旋翼飞行器-设计与控制》——全权——电子工业出版社
同样地，为了照顾其他专业领域的读者，本文将尽可能详细地讲述操作过程~
【敬告】 如将本文用于毕业设计、论文，或以任何形式转载、转发，均务必声明引用并详注出处！在未经声明的情况下，在毕业设计、论文、互联网或其他任何公开场合使用本文中的语句、段落、仿真模型等内容者，一经发现，本人必然追究版权及学术道德责任！
其他的废话不在前言赘述了，下面进入正文~
二、初始模型回顾与环节增补
基于上篇文章搭建的simulink模型，即方才讲到的v0版本系统，在这里进行一下重新展示（具体的搭建过程，见前两篇文章）。
需要注意的是，为了保证仿真输出即控制系统的稳定性，系统进行了一些环节增补和修改，下面会同步进行说明。
1 <系统总览-封装模块与输入接口之间的连接>
2 <位置控制器-PositonController-内部环节设置>
3 <姿态控制器-AttitudeController-内部环节设置>
这里需要注意，姿态控制器在最后输入之前接入了一个人工LPF(LowerPass Filter，低通滤波)环节：
>>LPF环节接入原因：系统易出现高频噪声，影响系统稳定性与仿真速度；基于基本的信号与系统原理，引入LPF环节（也可替换为积分环节），在不高频噪声的影响（读者可以先不接入，进行对比尝试）。
>>LPF环节传递函数：基于基本的自动控制原理，LPF环节基本传递函数为
其中，Ts为LPF环节的时间参数，后续将与PID环节中的参数设置一样，同样作为影响系统性能的参数，需要频繁调整。
>>LPF环节接入方法：点击屏幕左上角形如“四个方块”的“library browser”，按Simulink-Continuous路径点击，然后在右侧子页面中找到“Transfer Fcn”，如下图所示。
找到后，将之拖入系统中并接入。接入后，双击该模块进行参数设置，如下图所示。
设置过程可以简单理解为，第一行为传递函数中分子多项式的系数，第二行为分母多项式的系数，后两行如上设置即可。以上图设置的为例，即表示传递函数为1/(0.6s+1)的LPF环节。
LPF环节接入完毕后，回到系统姿态控制器即可。
还需说明的是，系统中接入的scope(观测器)为按需接入即可：读者朋友可以在任何线路上接入scope以检测信号的变化过程，是debug的重要手段之一。
4 <控制分配器-ControlDistributor-内部环节设置>
5 <电机控制器-MotorController-内部环节设置>
6 <四旋翼6DOF动力学解算器-6DOF_DynamicsSolver-内部环节设置>
考虑到系统较大使得截图显示不清，故分左部(升力与三轴力矩输入解算)和右部(6DOF解算)进行分别展示，如下。
7 <信号分解器-SignalDistributor-内部环节设置>
8 <系统控制输入-ControlInput-设置>
为了便于我们后续进行仿真实验和测试，针对于本系统的四个控制输入：x轴预期位置pxd，y轴预期位置pyd，z轴预期位置pzd，预期偏航角yawd（若此处不明，见本文第一篇关于飞控原理与框架的介绍），提前准备好阶跃信号输入(定点输入)、斜坡信号输入(定速输入)、正弦信号输入(定轨输入)三类环节，如下图所示。
下面分别介绍三类信号输入的含义及其接入方法。
>>阶跃信号输入含义：在本项目中，阶跃信号输入可以理解为对无人机的定点控制输入，即固定位置的控制输入；意为给予无人机一个固定的位置指令，使其飞至该点坐标。
如设置(pxd,pyd,pzd,yawd)=(2m,4m,8m,1rad)，则意为给予无人机从原点(0,0,0)处起飞并飞至目标点(2,4,8)(单位：m)，并将偏航角从零位置沿机体z轴正方向旋转至1(单位：rad)处，到达目标点和目标偏航角后，无人机原地悬停。
>>阶跃信号接入方法：点击屏幕左上角形如“四个方块”的“library browser”，按Simulink-Commonly Used Blocks路径点击，然后在右侧子页面中找到“Constant”后拖入系统即可。双击以设置参数数值。
>>斜坡信号输入含义：在本项目中，斜坡信号输入可以理解为对无人机的定速控制输入，即固定速度的控制输入；意为给予无人机一个固定的速度指令，使其起飞后以该速度进行巡航；也可意为给予无人机一空间直线运动轨迹，使其沿该直线飞行。
如设置(pxd,pyd,pzd,yawd)=(0.2m/s,0.5m/s,8m,1rad)，则意为给予无人机从原点(0,0,0)处起飞并飞至目标高度8(单位：m)，同时分别按0.2m/s和0.5m/s的速度向x轴和y轴正方向巡航，并将偏航角从零位置沿机体z轴正方向旋转至1(单位：rad)处，到达目标高度和目标偏航角后，无人机不再升高亦不再改变偏航角，但仍保持x轴和y轴的巡航速度。
>>斜坡信号接入方法：点击屏幕左上角形如“四个方块”的“library browser”，按Simulink-Sources路径点击，然后在右侧子页面中下拉找到“Ramp”后拖入系统即可，如下图所示。
双击以设置参数数值。
Ramp的参数页面中，易于理解地，Slope为斜率，Start time为开始时间，Initial output为信号初值；读者们可以根据测试需要按需设置，进行定速控制输入则仅需修改Slope的值即可，开始时间和信号初值设为0。
>>正弦信号输入含义：在本项目中，正弦信号输入可以理解为对无人机的定轨控制输入，即固定轨迹的控制输入；意为给予无人机一个固定的正弦轨迹飞行指令，使其起飞后沿该轨迹进行飞行。
正弦信号控制输入测试的意义是十分重大的，简述而言有三个：
(1) 基于信号与系统的傅里叶变换原理，任何运动轨迹都可拆解为若干不同频率的正弦轨迹的复合。故单一频率正弦信号输入下的系统响应稳定性，是任意轨迹信号输入下系统稳定的基础。
(2) 基于自动控制原理的频域分析原理，正弦信号控制输入测试，有助于理解系统的频率响应特性。
(3) 对于系统的稳定控制而言，实现定点、定速、定轨稳定控制的难度逐渐增大。在不考虑系统其他性能的前提下，仅考虑系统稳定性时：能实现定点、定速输入下的稳定控制的系统，未必能实现正弦控制输入下的稳定控制（本文后续会用测试结果说明此点）；然而，能实现正弦控制输入下稳定控制的系统，基本上亦能胜任定点、定速输入下的稳定控制。本条是本人的经验论，不一定适用于所有系统。
>>正弦信号接入方法：点击屏幕左上角形如“四个方块”的“library browser”，按Simulink-Sources路径点击，然后在右侧子页面中下拉找到“Sine Wave”后拖入系统，或按Simulink-User-Defined Functions路径点击，然后在右侧子页面中找到“Fcn”后拖入系统，如下图所示。
两种引入正弦信号输入的方法没有本质区别，本文以自定义函数Fcn为例进行演示。双击Fcn模块进行参数设置，输入5*sin(0.1*u)，如下图所示。
其中，u表示函数的自变量，自Fcn模块的左端接入。在本项目中，为了生成正弦控制信号，显然以时间作为函数自变量，故需要将时钟环节接入Fcn模块左端。点击屏幕左上角形如“四个方块”的“library browser”，按Simulink-Sources路径点击，然后在右侧子页面中找到“Clock”，将之拖出并接入到方才设置好的Fcn模块左端即可。
自此，便完成了初始仿真模型的全部搭建工作，可以开始仿真。仿真时，针对于四个控制输入：x轴预期位置pxd，y轴预期位置pyd，z轴预期位置pzd，预期偏航角yawd，选择需要的阶跃、斜坡、正弦输入环节，更改参数接入相应的封装模型接口即可。
三、飞行控制测试
基于上文搭建的模型，开始进行仿真测试。
需要注意的是，下文中呈现的测试结果均是在v0版本目前模型的基础上进行的，其中PID参数设置均按上文中图示中所示。如果读者使用未经调整的系统参数（包括各环PID环节、Saturation环节、LPF环节等参数），系统出现不稳定的、与下文所展示的结果不同的结果，均是极为正常的。PID的调参过程是随缘而缓慢的，需要充分的耐心。下面先展示测试结果，于本文结尾处再对PID的调参注意事项进行说明。
开始仿真前，将屏幕上方的数字改为“80”，意为设置仿真时间为80s（仿真系统时间，而非真实世界时间），而后点击屏幕上方形如“播放”按钮的“run”键。
此时系统开始运行，可见屏幕右下角的T=**s和**%的进度条，证明仿真正在进行中；待进度条加载至100%，仿真完成；这个过程的持续时间，视电脑性能，在瞬间到10s不等；如果超过20s，或者进度条卡死不动，说明仿真解算过程受阻，此时需要检查系统在解算中是否遇到奇点。
仿真完成后，双击模型尾部的各个scope环节检查仿真结果。
1 <定点悬停测试>
控制输入：设置(pxd,pyd,pzd,yawd)=(2m,4m,8m,1rad)，如图所示
仿真结果：(双击相应的scope获取)
根据系统位置响应曲线和姿态响应曲线可以看出，无人机如期到达了目标位置和姿态，并在该位置处悬停。系统收敛稳定。
当然，基于自动控制原理中的系统“稳快准”指标，系统性能还有很大的提升空间，可以通过改进模型结构和PID参数来实现，目前的系统已经实现了最重要的“稳”的保障。关于这一点，读者朋友可以先自行进行系统调试，本人将在后续的篇章中介绍。
顺带附上定点悬停测试的机体角速度响应曲线，通过该曲线可以更加具象地理解系统的动力学过程。
事实上，在系统中的任何位置接入的scope都有助于我们理解系统的动力学系统。在这里建议初学的读者朋友们，可以多在系统中各个线路中接入scope环节并查看动力学量的变化过程，根据自动控制原理对图像进行分析，这是我们进行控制系统改进、PID调参以改善系统性能的唯一准则。
例如，我们先前在姿态控制器中引入的LPF环节，通过scope的图像显示，能够帮助我们理解该环节的作用，如图所示。
其中，黄线为LPF的输入，蓝线为LPF的输出。可以看到，经过LPF环节后，信号的振动幅度和频率显著降低。试想，假如不设置LPF环节而直接将黄色信号接入后续的控制分配器，将给系统的稳定性引入可能致命的不利影响；同时，剧烈变化的信号也将对系统的算力增添负担。
所以，通过接入这样的scope环节并对图像进行分析，有助于我们深刻地理解复杂的宏观系统的微观变化，就好像医生在病人的血管中插入一个探针以检测病人该处的血液流速和物质浓度一般，对于医生把握病人的病症是极具意义的。
关于scope的使用方法，简而言之：通过鼠标滚轮在图像的某处进行尺度伸缩，通过上方倒数第三个箭头图案来自适应尺度或复原尺度，通过最后一个尺子图案来检测图像中定点的函数值。
如果点开scope后发现看不到图像，可能是因为当前所在的图像视角或者尺度不合适，可以点击箭头图案以获得可见图像，不过需要注意图像左侧的纵轴单位。读者朋友们可以自行探究尝试。
2 <定点悬停与定速巡航复合测试>
控制输入：(pxd,pyd,pzd,yawd)=(0.2m/s,0.5m/s,8m,1rad)
仿真结果：
根据系统位置响应曲线和姿态响应曲线可以看出，无人机如期到达了目标高度和姿态，并良好地完成了定速定向巡航运动。系统收敛稳定。
3 <正弦输入复合测试>
控制输入：(pxd,pyd,pzd,yawd)=(2m,5*sin(0.1*t)m,8m,1rad)
仿真结果：
可以看出，系统较预期出现了较大的偏差，不收敛不稳定。由此也验证了我们前面对于正弦控制输入测试意义的分析：即能实现定点、定速稳定控制的系统，未必能实现正弦轨迹稳定控制。
由于本系统为初始v0版本，后续版本将针对于正弦输入的稳定控制进行模型和参数改进。这里，先给出一个后续版本的正弦控制输入稳定结果，作为预告。
可见，经过对系统的优化，系统能够非常好地同时跟踪阶跃、斜坡和正弦控制输入，几乎没有稳态误差。
对于该系统的介绍，将在后续文章中更新，欢迎读者朋友们关注！
四、结语
1、 PID调参基本思路与经验
对于四旋翼PID飞行控制系统，大概有三十到四十个参数需要同时调整，这个过程是繁琐且随缘的，需要充分的耐心。在此本人仅给出几点建议：
(1) 充分理解领会自动控制原理中的稳快准思想，密切关注各通路节点处的图像；
(2) 在各系统的各线路中，接入充分多的scope环节，通过对图像的监测以找到不稳定的因素的根源，然后重点针对根源处的问题进行解决；
(3) 借鉴算法设计中“分而治之”的思想，四旋翼控制系统是一个复杂的多环系统，可以分别根据每个环的输出稳定性，针对单环系统调整参数，以达到整体的稳定；
(4) 保持耐心和情绪稳定。忙碌地调试一整天都调不出好的结果甚至比开始时更坏，都是可能的，但要相信一定存在一个稳定的参数设置！本人在每个仿真版本更新时，基本都花了1天或者至少数个小时的时间找到适应当前系统版本的参数设置。另外，在调出阶段性的较好结果后，及时保存当前参数和模型。
2、 感谢的话
首先感谢读者朋友们对本人和《基于MATLAB/Simulink的四旋翼无人机PID控制与仿真》系列文章的支持和关注！
起初在2023年的7月中旬，本人只是打算在暑期实习之余，写两篇文章记录一下这方面的工作，随缘更新；没想到在半年的时间里，陆续收到了相当多的关注、私信、催更、点赞和好评，实在是读者朋友们对本人莫大的鼓励！
在2024年上半年，随着私信和催更越来越多，本人几经忙碌、整理和辗转，终于在五一假期前夕的今天，不间断地连续码字7个小时完成了本篇，也是本系列第三篇文章的更新~
在此非常感谢读者朋友们的支持和鼓励！这份热情得到回应的喜悦，是各位赋予的惊喜~
3、 关于本系列
本人创作的《基于MATLAB/Simulink的四旋翼无人机PID控制与仿真》系列文章，保证为原创内容，特别是本篇文章几乎没有复制粘贴，100%出自本人的思考和前期工作。
本人大概于2023年7月中旬便完成了仿真模型的初始版本v0（截止至本篇中的版本），于8月中旬更新到了v7版本（实现正弦稳定控制、系统离散化、离散系统稳定控制、c++代码等）。去年11月份已经更新至最新的v11版本（主要是适应本人研究方向的一些内容，关于一个复杂数倍的无人机动力学系统）。
关于本篇文章所涉及的仿真系统（matlab/simulink文件）v0版本，本人将视读者反响的热烈程度，择期以资源的形式发布在本人主页，以便更好地帮助到读者朋友们~
关于本仿真系统的后续版本，将具有更完备且优良的性能，本人拟于后续的文章中呈现，诚邀读者朋友们关注！对文章有任何问题，欢迎私信或者评论~
4、 关于本人
本人研究兴趣包括：无人机飞行控制与导航系统，非线性动力学系统，惯性与视觉导航系统等。欢迎同行朋友和前辈们指点交流。
1万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
