本文共分为数篇文章介绍“基于MATLAB/Simulink的四旋翼无人机PID控制与仿真”：

1、前言与四旋翼动力学与控制模型（点此跳转）

2、四旋翼控制系统设计与基于MATLAB/Simulink的仿真模型搭建<<（您在此处）

3、系统模型完善与飞行控制测试（点此跳转）

4、针对正弦输入稳定控制的系统改进（待更新）

5、控制系统的离散化与c++飞控代码导出（待更新）

系统目前刚刚完成第一轮调参，随着后续优化会【持续更新】几篇文章...

欢迎大家阅览并提出意见！

---

目录

一、前言

二、位置控制器

1、位置控制器设计

2、位置控制器搭建

三、姿态控制器

1、姿态控制器设计

2、姿态控制器搭建

四、控制分配器

1、控制分配器设计

2、控制分配器搭建

五、电机控制器

六、基于6DOF的四旋翼运动信息解算器

七、集成仿真系统

---

一、前言

1、承前启后

在上一篇文章最后，我们已经基于四旋翼的基本假设和动力学理论得到了线性化的控制模型，特别是援引了《多旋翼无人机-设计与控制》中的底层飞行控制框架。本篇文章将据前文内容进行四旋翼控制系统设计，而后依照设计结果，在MATLAB/Simulink环境中搭建四旋翼控制的仿真模型。

2、控制算法说明

在本篇文章中，作者将采用基本的PID算法设计控制器。本篇文章所最终搭建的模型将不是测试中采用的最终版本，也不会对PID参数进行调整，而仅介绍搭建整个仿真系统框架的方法。实际测试时采用的控制仿真模型在PID的基础上可能有一些增改，最终仿真模型的呈现将在下一篇文章中进行介绍。

对于四旋翼无人机的控制器设计而言，按系统输出为欧拉角或旋转矩阵分为两类，二者有一定的差别，但私以为不大。考虑到以欧拉角为输出比较直观且能实现控制效果，故本文暂时仅设计该类控制器，后续也可能会添加以旋转矩阵为输出的控制器设计补充。

3、仿真方法说明

在设计完控制系统后，作者将在MATLAB/Simulink中搭建控制器，作者本人的MATLAB版本是R2019A。为了照顾不同层次的读者，作者也将在一定程度上简要介绍控制器搭建过程中相关的Simulink使用方法。

对于四旋翼无人机而言，目前基于Simulink的仿真方法共用两种：一种是利用Simscape模块的多体系统动力学仿真，主要通过在SolidWorks上搭建的装配体模型通过插件导入Simulink，输出机体运动动态视频，特点是能够有良好的gui展示，相关页面如下；

一种是利用Aerospace Blockset中的6DOF模块输出机体运动信息，特点是不需要考虑机体结构设计与外观，比较简便，后文会详细介绍。

关于这两种仿真实现方法，作者都尝试过，总的来说二者从算法上没有差别，后者是前者的基础，亦可被前者囊括。考虑到难以在SolidWorks中建立准确地无人机模型，本文先暂时仅就后者对应的方法实现四旋翼的控制仿真，后续若有时间增添Simscape的模型。

4、四旋翼无人机F450参数

对于四旋翼的真实设计参数，作者几寻无果。机缘巧合之下，从飞思实验室官网http://rflysim.com/得到了一款四旋翼无人机F450的模型参数，实在是难得可贵。四旋翼的设计及硬件参数对于系统控制仿真而言是极为重要的，在此将参数无偿地分享给大家，同时向大家推荐这个网站，感兴趣的话可以去该网站学习多旋翼控制相关知识并获取资料。

上述参数将在下面的仿真模型搭建中直接使用，届时将不再说明。 

5、符号说明

本文将用Kp，Ki，Kd分别表示PID控制中比例环节、积分环节、微分环节的增益系数；

用d表示当前该值为期望值，如pxd表示x方向上的预期位置，px则表示x方向上的状态反馈值/测量值，即前者表示希望值，后者表示实际值；

用e表示被控对象的偏差，如epz=pzd-pz；

二、位置控制器

1、位置控制器设计

2、位置控制器搭建

根据上面得到的设计图与上篇文章介绍的位置控制器控制模型，打开MATLAB/Simulink，新建一模型，拖拽相应的模块，搭建如下的系统：

注意图中的PID参数后期还需要调整。作者在最初系统的搭建过程中省略了速度环的控制，实践表明不需要速度环系统最终也得到了控制，但是最终为了追求更好的性能最后还是添加了速度环，详见下篇文章；此处的读者也可以先不考虑速度环。

另外在系统输出之前需要将信号经过一饱和环节，主要是因为无人机作为实际的物理系统，其物理量均存在上限，特别是两个水平方向的姿态角需要满足小角近似条件。因此必须引入非线性环节加以限制，饱和上限读者可以自行探索拟定。

对于初接触Simulink的读者，需要注意的是，对输入输出接口(port)务必要标注好名字，同时注意其上的编号，这将影响到马上要进行的子系统封装环节。

全选当前系统，右键点击create subsystem from selection（也可以用快捷键ctrl+G）得到封装后的系统，将新得到的子系统命名为“位置控制器”。一个小建议是在系统封装完毕后可以将系统外部的port接口和连接线全部删除，留下一个子系统方框即可，此时方才设定的接口名字便显示在了子系统方框的两侧，同时可以引出连接线，接口陈列顺序依接口编号而定。

三、姿态控制器

1、姿态控制器设计

与位置控制器类似，对姿态控制器进行设计。

2、姿态控制器搭建

在先前搭设的模型右侧空白处，同理搭建姿态控制器如下。

同理设置接口与饱和模块。作者在最初系统的搭建过程中省略了姿态环的PID控制，而仅保留一比例环节，更完善的控制环节将在下篇文章中经测试后添加，此处的读者也可以先不考虑姿态环的复杂控制。

搭建好系统后，同样全选后ctrl+G进行子系统封装，将封装好的子系统命名为“姿态控制器”。

四、控制分配器

1、控制分配器设计

控制分配器衔接控制器与电机，负责将先前得到的预期力和力矩信号转换为四旋翼的四个螺旋桨对应的四个预期转速，以将之提供给后续的电机控制器。

需要说明的是，式中的矩阵(未取逆者)，被称为多旋翼的控制分配矩阵，该矩阵是多旋翼控制的一个重要参数矩阵，其取值与多旋翼的设计参数、坐标系的设置、螺旋桨转向的设置均有关，需要分情况进行列写，不可照搬。

2、控制分配器搭建

控制分配器的模型搭建比较容易，仅需将上述运算关系移植至Simulink中即可，首先将上式中的矩阵输入到Simulink中。方法为拖拽一“constant”输入模块到系统中，将其值设为欲输入的矩阵，未知数直接先用字母代替即可，如下图中所示

此时，系统会提示错误，指出我们方才输入的未知数没有取值定义。我们找到屏幕左上角功能栏中的Model explorer。点击后，在左侧导航栏找到我们一直在编辑的模型文件，点开其下的Model Workplace（注意不要点成了Simulink Root之下的Base Workplace），然后逐个添加变量，将我们方才输入的未知量进行取值设定即可。后续如果无人机模型参数有变化，直接在此页面修改取值便可（若读者没有现成无人机参数，详见本文前言）。

参数添加完成后，apply后回到刚才的模型页面，将模型搭建如下。

为了求矩阵的逆，按如下路径或直接搜索，找到divide模块拖入系统；

 双击divide模块，弹出参数设置页面，将Multiplication设置为矩阵Matrix乘法，点击OK；

欲反转模块，选中模块后ctrl+R即可。

进一步搭建系统，如下所示。

需要说明的是，图中的黑条为mux和demux模块，分别用于将分量合并为向量和将向量拆分为分量。系统中接入的scope观测器用于后续测试，读者可选择性接入。信号经过矩阵乘法运算后，系统输出需要添加饱和模块，将螺旋桨最大转速（切记将单位转化为rad/s）作为上限，下限可设为0（防止后面取平方根时遇到负值而报错，不过正常飞行情况下螺旋桨转速不可能越过零点，关于这一点将在下一篇文章中详述）。随后将所得信号接入取平方根模块，即得四个旋翼的预期转速。

将搭建好的系统选中，封装为子系统，命名为“控制分配器”。

五、电机控制器

电机控制器接受控制分配器提供的预期转速作为系统输入，输出每个电机的期望油门指令。

由于电机系统形式比较简单，一般采用开环控制即可，即无需引入反馈。

根据上式，在Simulink中搭建模型如下。

 模型比较简单，不再赘述。将系统封装后，命名为“电机控制器”。

六、基于6DOF的四旋翼运动信息解算器

本节首先对6DOF模块的使用要点作简要介绍，预知详情可查阅matlab自带的help文档。

首先找到6DOF(Euler Angles)模块，可以在Library Browser直接搜索，也可以按下面的路径寻得。找到后将之拖入系统。

拖入后，双击6DOF模块，进行模块参数设置。将无人机转动惯量矩阵和质量输入，初始位置和速度后期按需输入即可。

在输入数据时需要考虑单位，系统默认采用Metric(MKS)即国际标准单位制。另外需要注意的是，6DOF模块接受三轴力和三轴力矩的输入，均以三维向量的形式输入，且二者均在载体坐标系下。

将所得的转速取平方，再同先前的控制分配矩阵相乘，得到四旋翼受到的控制力和力矩。

除此之外，还需手动设置无人机受到的重力，利用6DOF输出的坐标系转换矩阵将重力转到载体坐标系下，同控制力加和。

根据上述分析，模型搭建如下

 需要注意的是，作者在6DOF的位置输出之前，加入了一饱和环节，以限制飞行器的z轴位置大于0（显然飞行器不可能进入地面及以下），以模拟更真实的飞行情况。

将所得系统封装，子系统命名为“UAV-6DOF解算器”。

由此，四旋翼的控制系统和飞行模拟器便全部搭建完毕。

七、集成仿真系统

将所得的子系统对应接口连接起来，如下

 其中，位于底部的信号分配器此前没有介绍，其结构比较简单，单纯用于分配反馈信号。

至此，四旋翼控制仿真系统已搭建完毕。设置仿真时间后开始仿真，系统不报错说明至少模型搭建成功。如果报错也是可能的，作者在搭建该模型时遇到了少说也有数十个bug，下一篇文章将介绍PID参数的调整以及典型bug与报错的解决方案，欢迎继续阅读。