动态系统建模仿真 实验报告（2）
姓 名 ： 学 号 ： 指导教师 ： 院 系 ：
2014.12.28
四旋翼飞行器仿真
1实验内容
基于Simulink 建立四旋翼飞行器的悬停控制回路，实现飞行器的悬停控制； 建立GUI 界面，能够输入参数并绘制运动轨迹；
基于VR Toolbox建立3D 动画场景，能够模拟飞行器的运动轨迹。 2实验目的
通过在 Matlab 环境中对四旋翼飞行器进行系统建模，使掌握以下内容： 四旋翼飞行器的建模和控制方法
在Matlab 下快速建立虚拟可视化环境的方法。 3实验器材
硬件：PC 机。
工具软件：操作系统：Windows 系列；软件工具：MATLAB 及simulink 。 4实验原理 4. 1四旋翼飞行器
四旋翼飞行器通过四个螺旋桨产生的升力实现飞行， 原理与直升机类似。 四个旋翼位于一个几何对称的十字支架前，后，左，右四端，如图 1 所示。旋翼由电机控制；整个飞行器依靠改变每个电机的转速来实现飞行姿态控制。
图1四旋翼飞行器旋转方向示意图
在图 1 中， 前端旋翼 1 和后端旋翼 3 逆时针旋转， 而左端旋翼 2 和右端的旋翼 4 顺时针旋转， 以平衡旋翼旋转所产生的反扭转矩。 由此可知， 悬停时， 四只旋翼的转速应该相等，以相互抵消反扭力矩；同时等量地增大或减小四只旋翼的转速，会引起上升或下降运动；增大某一只旋翼的转速，同时等量地减小同组另一只旋翼的转速，则产生俯仰、横滚运动；增大某一组旋翼的转速，同时等量减小另一组旋翼的转速，将产生偏航运动。
4.2建模分析
四旋翼飞行器受力分析, 如图 2 所示
图2四旋翼飞行器受力分析示意图
旋翼机体所受外力和力矩为： 重力mg , 机体受到重力沿-z w 方向；
四个旋翼旋转所产生的升力F i (i= 1 , 2 , 3 , 4)，旋翼升力沿z b 方向； 旋翼旋转会产生扭转力矩M i (i= 1 , 2 , 3 , 4)。M i 垂直于叶片的旋翼平面，与旋转矢量相反。
k 力模型为：F i =k F ωi 2 ，旋翼通过螺旋桨产生升力。F 是电机转动力系数，
-82
ω6.11⨯10N /rpm 可取，i 为电机转速。旋翼旋转产生旋转力矩Mi(i=1,2,3,4)，
力矩Mi 的旋向依据右手定则确定。力矩模型为M i =k M ωi 2 ，其中k M 是电机转动力系数，可取1.5⨯10-9Nm /rpm 2ωi 为电机转速。当给定期望转速后，电机的实际转速需要经过一段时间才能达到。实际转速与期望转速之间的关系为一阶延迟：
ωi =k m (ωi des -ωi ) 响应延迟时间可取0.05s(即k =20) 。期望转速ωdes 则需要限
m i
s
制在电机的最小转速和最大转速之间，范围可分取[1200rpm，7800rpm]。
飞行器受到外界力和力矩的作用，形成线运动和角运动。线运动由合外力引起，符合牛顿第二定律：
⎡0⎤⎡0⎤
⎢⎥⎢⎥mr =⎢0⎥+R ⎢0⎥ ⎢F ⎥⎢⎣-mg ⎥⎦⎣∑i ⎦
r 为飞机的位置矢量。
角运动由合力矩引起。四旋翼飞行器所受力矩来源于两个方面：1）旋翼升力作用于质心产生的力矩；2）旋翼旋转产生的扭转力矩。角运动方程如下式所示。其中，L 为旋翼中心建立飞行器质心的距离，I 为惯量矩阵。
L (F 2-F 4) ⎤⎡p ⎤⎡p ⎤⎡p ⎤⎡
⎥-⎢q ⎥⨯I ⎢q ⎥ ⎥=⎢I ⎢q L (F -F ) 31⎥⎢⎥⎢⎥⎢⎥⎢
⎢⎣r ⎥⎦⎢⎣r ⎥⎦⎢⎣r ⎥⎦⎣M 1-M 2+M 3-M 4⎥⎦⎢
4.3控制回路设计
控制回路包括内外两层。外回路由Position Control 模块实现。输入为位置误差，输出为期望的滚转、俯仰和偏航角(φdes (t ) 、θdes (t ) 、ψdes (t )) 。内回路由Attitude Control 模块实现，输入为期望姿态角，输出为期望转速。Motor Dynamics 模块模拟电机特性，输入为期望转速(∆ωφ、∆ωθ、∆ωψ) ，输出为力和力矩。Rigid Body Dynamics 是被控对象，模拟四旋翼飞行器的运动特性。
图3包含内外两个控制回路的控制结构
（1）内回路：姿态控制回路
对四旋翼飞行器，我们唯一可用的控制手段就是四个旋翼的转速。因此，这里首先对转速ω产生的作用进行分析。假设我们希望旋翼1的转速达到ω1，那么它的效果可分解成以下几个分量：
des
ωh ：使飞行器保持悬停的转速分量；
∆ωF ：除悬停所需之外，产生沿ZB 轴的净力； ∆ωθ：使飞行器负向偏转的转速分量；
∆ωψ：使飞行器正向偏航的转速分量；
因此，可以将期望转速写成几个分量的线性组合：
ω1des =ωh +∆ωF -∆ωθ+∆ωψ
其它几个旋翼也可进行类似分析，最终得到：
⎡ω1des ⎤⎡1⎢des ⎥⎢⎢ω2⎥⎢1⎢ωdes ⎥=⎢1⎢3⎥⎢des 1⎢⎣ω4⎥⎦⎣
10-1-1011
1⎤⎡ωh +∆ωF ⎤⎢∆ω⎥-1⎥φ⎥ ⎥⎢
1⎥⎢∆ωθ⎥
⎥⎥⎢
-1⎦⎢∆ω⎥ψ⎣⎦
在悬浮状态下，四个旋翼共同的升力应抵消重力，因此：
2
4K F ωh =mg
此时，可以把旋翼角速度分成几个部分分别控制，通过“比例-微分”控制律建立如下公式：
∆ωφ=k p , φ(φdes -φ) +k d , φ(φdes -φ)
∆ωθ=k p , θ(θdes -θ) +k d , θ(θdes -θ) ∆ωψ=k p , ψ(ψdes -ψ) +k d , ψ(ψdes -ψ)
综合以上三式可得到期望姿态角-期望转速之间的关系，即内回路。 外回路：位置控制回路
外回路采用以下控制方式：通过位置偏差计算控制信号（加速度）；建立控制信号与姿态角之间的几何关系；得到期望姿态角，作为内回路的输入。期望位置记为r i des 。可通过PID 控制器计算控制信号：
(r i , T -r i des ) +k d , i (r i , T -r i ) +k p , i (r i , T -r i ) +k i , i ⎰（r i , T -r i ）=0
r i , T 是目标悬停位置是我们的目标悬停位置(i=1,2,3)，r i des 是期望加速度，
即控制信号。注意：悬停状态下线速度和加速度均为0，即r i , T =r i , T =0。
通过俯仰角和滚转角控制飞行器在XW 和YW 平面上的运动，通过∆ωψ控制偏航角，通过∆ωF 控制飞行器在ZB 轴上的运动。可得：
mr 1=(cosψsin θ+cos θsin φsin ψ) ∑F i
mr 2=(sinψsin θ-cos ψcos θsin φ) ∑F i mr 3=-mg +cos φcos θ∑F i
根据上式可按照以下原则进行线性化：
（1）将俯仰角、滚转角的变化作为小扰动分量，有sin θ≈θ，sin φ≈φ，
cos θ≈1，cos φ≈1; （2）偏航角不变，有ψ=ψT =ψ0，其中ψ0初始偏航角，ψT 为期望偏航角（3）在悬停的稳态附近，有∑F i ≈mg
根据以上原则线性化后，可得到控制信号（期望加速度）与期望姿态角之间的关系：
r 1des =g (θdes cos ψT +φdes sin ψT ) r 2des =g (θdes sin ψT -φdes cos ψT ) r 3des =
则内回路的输入为：
8k F ωh
∆ωF
m
1des
(r 1sin ψT -r 2des cos ψT ) g 1
θdes =(r 1des cos ψT +r 2des sin ψT )
g m
∆ωF =r 3des
8k F ωh
φdes =
5实验步骤与结果
（1）根据控制回路的结构建立simulink 模型；
（2）为了便于对控制回路进行参数调整，利用Matlab 软件为四旋翼飞行器创建GUI 参数界面；
（3）利用Matlab 的VR Toolbox建立四旋翼飞行器的动画场景
（4）根据系统的结构框图，搭建Simulink 模块以实现模拟飞行器在指定位置的悬停。使用默认数据，此时xdes=3，ydes=4，zdes=5，开始仿真，可以得到运动轨迹x 、y 、z 的响应函数，同时可以得到在xyz 坐标中的空间运动轨迹。然后点击GUI 中的VR 按钮使simulink 的工作空间中载入系统仿真所需的参数，把x 、y 、z 的运动轨迹和Roll ，Pitch ，Yaw 输入至VR 中的模拟飞行器中，观察飞行器的运动轨迹和运动姿态，然后再使用一组新的参数xdes=-8，ydes=3，zdes=6进行四旋翼飞行器运动进行仿真模拟，可以看出仿真结果和动画场景相吻合。
6实验总结与心得
此次MATLAB 实验综合了SIMULINK 、GUI 和VR 场景等多个部分，对四旋翼飞行器运动进行了仿真模拟。由仿真结果可以看出，四旋翼飞行器最终位置达到了期望给定的位置，三个方向的响应曲线最终平稳，对应飞行器悬停在期望位置，达到了控制要求。
本次试验收获很多，学习到了很多知识，首先是熟悉了SIMULINK 由简至繁搭建系统的过程，学习了利用VR 建立虚拟模型，并在SIMULINK 中连接。其次是熟悉了MATLAB GUI界面的编写和搭建过程。Matlab 提供了强大的用户图形界面，以帮助用户不必编写底层程序而直接在软件包基础上进行自行开发，这点在诸多软件中都有所体现。另外通过实验，对四旋翼飞行器的受力分析、模型建立、控制回路设计等有了较为细致的了解。
动态系统建模仿真 实验报告（2）
姓 名 ： 学 号 ： 指导教师 ： 院 系 ：
2014.12.28
四旋翼飞行器仿真
1实验内容
基于Simulink 建立四旋翼飞行器的悬停控制回路，实现飞行器的悬停控制； 建立GUI 界面，能够输入参数并绘制运动轨迹；
基于VR Toolbox建立3D 动画场景，能够模拟飞行器的运动轨迹。 2实验目的
通过在 Matlab 环境中对四旋翼飞行器进行系统建模，使掌握以下内容： 四旋翼飞行器的建模和控制方法
在Matlab 下快速建立虚拟可视化环境的方法。 3实验器材
硬件：PC 机。
工具软件：操作系统：Windows 系列；软件工具：MATLAB 及simulink 。 4实验原理 4. 1四旋翼飞行器
四旋翼飞行器通过四个螺旋桨产生的升力实现飞行， 原理与直升机类似。 四个旋翼位于一个几何对称的十字支架前，后，左，右四端，如图 1 所示。旋翼由电机控制；整个飞行器依靠改变每个电机的转速来实现飞行姿态控制。
图1四旋翼飞行器旋转方向示意图
在图 1 中， 前端旋翼 1 和后端旋翼 3 逆时针旋转， 而左端旋翼 2 和右端的旋翼 4 顺时针旋转， 以平衡旋翼旋转所产生的反扭转矩。 由此可知， 悬停时， 四只旋翼的转速应该相等，以相互抵消反扭力矩；同时等量地增大或减小四只旋翼的转速，会引起上升或下降运动；增大某一只旋翼的转速，同时等量地减小同组另一只旋翼的转速，则产生俯仰、横滚运动；增大某一组旋翼的转速，同时等量减小另一组旋翼的转速，将产生偏航运动。
4.2建模分析
四旋翼飞行器受力分析, 如图 2 所示
图2四旋翼飞行器受力分析示意图
旋翼机体所受外力和力矩为： 重力mg , 机体受到重力沿-z w 方向；
四个旋翼旋转所产生的升力F i (i= 1 , 2 , 3 , 4)，旋翼升力沿z b 方向； 旋翼旋转会产生扭转力矩M i (i= 1 , 2 , 3 , 4)。M i 垂直于叶片的旋翼平面，与旋转矢量相反。
k 力模型为：F i =k F ωi 2 ，旋翼通过螺旋桨产生升力。F 是电机转动力系数，
-82
ω6.11⨯10N /rpm 可取，i 为电机转速。旋翼旋转产生旋转力矩Mi(i=1,2,3,4)，
力矩Mi 的旋向依据右手定则确定。力矩模型为M i =k M ωi 2 ，其中k M 是电机转动力系数，可取1.5⨯10-9Nm /rpm 2ωi 为电机转速。当给定期望转速后，电机的实际转速需要经过一段时间才能达到。实际转速与期望转速之间的关系为一阶延迟：
ωi =k m (ωi des -ωi ) 响应延迟时间可取0.05s(即k =20) 。期望转速ωdes 则需要限
m i
s
制在电机的最小转速和最大转速之间，范围可分取[1200rpm，7800rpm]。
飞行器受到外界力和力矩的作用，形成线运动和角运动。线运动由合外力引起，符合牛顿第二定律：
⎡0⎤⎡0⎤
⎢⎥⎢⎥mr =⎢0⎥+R ⎢0⎥ ⎢F ⎥⎢⎣-mg ⎥⎦⎣∑i ⎦
r 为飞机的位置矢量。
角运动由合力矩引起。四旋翼飞行器所受力矩来源于两个方面：1）旋翼升力作用于质心产生的力矩；2）旋翼旋转产生的扭转力矩。角运动方程如下式所示。其中，L 为旋翼中心建立飞行器质心的距离，I 为惯量矩阵。
L (F 2-F 4) ⎤⎡p ⎤⎡p ⎤⎡p ⎤⎡
⎥-⎢q ⎥⨯I ⎢q ⎥ ⎥=⎢I ⎢q L (F -F ) 31⎥⎢⎥⎢⎥⎢⎥⎢
⎢⎣r ⎥⎦⎢⎣r ⎥⎦⎢⎣r ⎥⎦⎣M 1-M 2+M 3-M 4⎥⎦⎢
4.3控制回路设计
控制回路包括内外两层。外回路由Position Control 模块实现。输入为位置误差，输出为期望的滚转、俯仰和偏航角(φdes (t ) 、θdes (t ) 、ψdes (t )) 。内回路由Attitude Control 模块实现，输入为期望姿态角，输出为期望转速。Motor Dynamics 模块模拟电机特性，输入为期望转速(∆ωφ、∆ωθ、∆ωψ) ，输出为力和力矩。Rigid Body Dynamics 是被控对象，模拟四旋翼飞行器的运动特性。
图3包含内外两个控制回路的控制结构
（1）内回路：姿态控制回路
对四旋翼飞行器，我们唯一可用的控制手段就是四个旋翼的转速。因此，这里首先对转速ω产生的作用进行分析。假设我们希望旋翼1的转速达到ω1，那么它的效果可分解成以下几个分量：
des
ωh ：使飞行器保持悬停的转速分量；
∆ωF ：除悬停所需之外，产生沿ZB 轴的净力； ∆ωθ：使飞行器负向偏转的转速分量；
∆ωψ：使飞行器正向偏航的转速分量；
因此，可以将期望转速写成几个分量的线性组合：
ω1des =ωh +∆ωF -∆ωθ+∆ωψ
其它几个旋翼也可进行类似分析，最终得到：
⎡ω1des ⎤⎡1⎢des ⎥⎢⎢ω2⎥⎢1⎢ωdes ⎥=⎢1⎢3⎥⎢des 1⎢⎣ω4⎥⎦⎣
10-1-1011
1⎤⎡ωh +∆ωF ⎤⎢∆ω⎥-1⎥φ⎥ ⎥⎢
1⎥⎢∆ωθ⎥
⎥⎥⎢
-1⎦⎢∆ω⎥ψ⎣⎦
在悬浮状态下，四个旋翼共同的升力应抵消重力，因此：
2
4K F ωh =mg
此时，可以把旋翼角速度分成几个部分分别控制，通过“比例-微分”控制律建立如下公式：
∆ωφ=k p , φ(φdes -φ) +k d , φ(φdes -φ)
∆ωθ=k p , θ(θdes -θ) +k d , θ(θdes -θ) ∆ωψ=k p , ψ(ψdes -ψ) +k d , ψ(ψdes -ψ)
综合以上三式可得到期望姿态角-期望转速之间的关系，即内回路。 外回路：位置控制回路
外回路采用以下控制方式：通过位置偏差计算控制信号（加速度）；建立控制信号与姿态角之间的几何关系；得到期望姿态角，作为内回路的输入。期望位置记为r i des 。可通过PID 控制器计算控制信号：
(r i , T -r i des ) +k d , i (r i , T -r i ) +k p , i (r i , T -r i ) +k i , i ⎰（r i , T -r i ）=0
r i , T 是目标悬停位置是我们的目标悬停位置(i=1,2,3)，r i des 是期望加速度，
即控制信号。注意：悬停状态下线速度和加速度均为0，即r i , T =r i , T =0。
通过俯仰角和滚转角控制飞行器在XW 和YW 平面上的运动，通过∆ωψ控制偏航角，通过∆ωF 控制飞行器在ZB 轴上的运动。可得：
mr 1=(cosψsin θ+cos θsin φsin ψ) ∑F i
mr 2=(sinψsin θ-cos ψcos θsin φ) ∑F i mr 3=-mg +cos φcos θ∑F i
根据上式可按照以下原则进行线性化：
（1）将俯仰角、滚转角的变化作为小扰动分量，有sin θ≈θ，sin φ≈φ，
cos θ≈1，cos φ≈1; （2）偏航角不变，有ψ=ψT =ψ0，其中ψ0初始偏航角，ψT 为期望偏航角（3）在悬停的稳态附近，有∑F i ≈mg
根据以上原则线性化后，可得到控制信号（期望加速度）与期望姿态角之间的关系：
r 1des =g (θdes cos ψT +φdes sin ψT ) r 2des =g (θdes sin ψT -φdes cos ψT ) r 3des =
则内回路的输入为：
8k F ωh
∆ωF
m
1des
(r 1sin ψT -r 2des cos ψT ) g 1
θdes =(r 1des cos ψT +r 2des sin ψT )
g m
∆ωF =r 3des
8k F ωh
φdes =
5实验步骤与结果
（1）根据控制回路的结构建立simulink 模型；
（2）为了便于对控制回路进行参数调整，利用Matlab 软件为四旋翼飞行器创建GUI 参数界面；
（3）利用Matlab 的VR Toolbox建立四旋翼飞行器的动画场景
（4）根据系统的结构框图，搭建Simulink 模块以实现模拟飞行器在指定位置的悬停。使用默认数据，此时xdes=3，ydes=4，zdes=5，开始仿真，可以得到运动轨迹x 、y 、z 的响应函数，同时可以得到在xyz 坐标中的空间运动轨迹。然后点击GUI 中的VR 按钮使simulink 的工作空间中载入系统仿真所需的参数，把x 、y 、z 的运动轨迹和Roll ，Pitch ，Yaw 输入至VR 中的模拟飞行器中，观察飞行器的运动轨迹和运动姿态，然后再使用一组新的参数xdes=-8，ydes=3，zdes=6进行四旋翼飞行器运动进行仿真模拟，可以看出仿真结果和动画场景相吻合。
6实验总结与心得
此次MATLAB 实验综合了SIMULINK 、GUI 和VR 场景等多个部分，对四旋翼飞行器运动进行了仿真模拟。由仿真结果可以看出，四旋翼飞行器最终位置达到了期望给定的位置，三个方向的响应曲线最终平稳，对应飞行器悬停在期望位置，达到了控制要求。
本次试验收获很多，学习到了很多知识，首先是熟悉了SIMULINK 由简至繁搭建系统的过程，学习了利用VR 建立虚拟模型，并在SIMULINK 中连接。其次是熟悉了MATLAB GUI界面的编写和搭建过程。Matlab 提供了强大的用户图形界面，以帮助用户不必编写底层程序而直接在软件包基础上进行自行开发，这点在诸多软件中都有所体现。另外通过实验，对四旋翼飞行器的受力分析、模型建立、控制回路设计等有了较为细致的了解。