本文将详细展示如何在Matlab中建立四旋翼飞行模型，能正确对飞行器在力和力矩作下的线运动、角运动进行6DOF仿真，并实现对指定 3D 空间点的悬停控制。

（注：本文将只展示悬停控制数字建模过程，轨迹跟踪控制和编队飞行控制将不具体展示，只展示其仿真结果。）

原文件下载地址：悬停控制simulink仿真源文件

悬停控制GUI源文件

轨迹跟踪控制simulink&gui源文件

编队飞行simulink&gui源文件

一、姿态内回路搭建

先在simulink中进行控制内回路，即姿态控制回路的搭建。内回路由Attitude Control模块实现，输入为期望姿态角，输出为期望转速，模型如下：

其中“期望姿态角-期望转速”之间的关系通过“比例–微分”控制律建立，如下图所示：

二、电机模型

电机模型由Motor Dynamics 模块实现，输入为期望角速度，输出为四旋翼四个电机的输出力和力矩。模型如下图所示：

首先，四个欧拉角转速要转化成四个电机的转速。然后，四个电机的实际转速转化为四个电机的输出力和力矩。以上功能都有Function模块编程实现。另外，由于电机实际特性影响，期望转速与实际转速之间存在延迟，延迟时间可取0.05s。期望转速还应限制在一定转速范围内，范围可取【1000rpm，8000rpm】。

综上所述，模块内部关系如下图所示；

三、动力学和运动学模型建立

无人机动力学和运动学由Rigid Body Dynamics模块实现。

模型如下图所示：

其输入为四个电机输出的力和力矩，输出为无人机的位置、欧拉角、速度等参数。

无人机运动学模型使用的是simulink自带模块6DOF实现。如下图所示：

无人机输入的合力由以下公式获得：

合力矩由以下公式获得：

搭建模型如下图所示：

四、位置外回路搭建

位置外回路由Position Control模块实现。

模型如下图所示：

其输入为XYZ三个坐标的位置误差，输出为期望的滚转、俯仰、和偏航角。首先位置误差通过PID控制器转换为期望的位置加速度，再由期望加速度计算出期望的姿态角。

模型如下图所示：

至此，无人机数字模型搭建完毕。

总体模型如下图所示：

五、仿真结果：

5.1 悬停控制仿真结果

将XYZ期望位置分别设置为10、10、10。得到响应曲线如下图所示：

（注：因模型建立在北东地参考坐标系，因此z轴正方向为负，若想修正加个“-”即可）

5.2 轨迹跟踪仿真结果

5.3 编队飞行仿真结果