CN110712750A - 一种微型四扑翼飞行器控制系统 - Google Patents

Abstract

本发明公开了一种微型四扑翼飞行器控制系统，包括两个空心杯电机、两个直线舵机，还包括机载控制板，遥控器。通过遥控器和七轴姿态检测单元的配合，可以有效地控制微型四扑翼飞行器的两个空心杯电机和两个直线舵机，从而使飞行器实现多姿态以及悬停；采用传统的串级PID控制，使得系统更加稳定。

Description

一种微型四扑翼飞行器控制系统

技术领域

本发明属于微型飞行器领域，具体涉及一种微型四扑翼飞行器控制系统。

背景技术

微型扑翼飞行器简称FMAV(Flapping Micro Air Vehicle)，是一种仿鸟类或者仿昆虫的微型飞行器，主要是通过自身翅膀的来回拍打提供升力以及改变自身的姿态。

微型扑翼飞行器的概念自提出以来，就掀起了一股热潮，从21世纪初开始，代尔夫特大学、布鲁塞尔大学、韩国建国大学、普渡大学、北京航空航天大学、南京航空航天大学、上海交通大学等高校相继对其展开了研究，“蜂鸟”、“DelFly”系列等优秀作品也展现出了此类飞行器的可行性以及可靠性。

微型扑翼飞行器在军事以及民用方面存在巨大的潜在价值。和常规飞行器相比，微型扑翼飞行器在体积上十分微小，由此具有易携带型、隐蔽性、机动灵活、噪声小等优点，对于军事上侦查、复杂地形勘探具有重要意义。同时，微型扑翼飞行器还可以应用在生化环境探测、交通监管等方面，具有非常广阔的发展前景。

在微型扑翼飞行器的研发中，除了要克服自身重力以外，还要能够在空中实现自主稳定，并以此作为基础，还要做到俯仰、滚转、偏航等姿态。为了解决上述问题，本发明提供了一种四扑翼飞行器的控制系统，来帮助扑翼飞行器实现自主稳定以及姿态变换等目标。

发明内容

为了解决上述已有技术存在的不足，本发明提出一种微型四扑翼飞行器控制系统，通过遥控器和七轴姿态检测单元的配合，可以有效地控制微型四扑翼飞行器的两个空心杯电机和两个直线舵机，从而使飞行器实现多姿态以及悬停。

本发明的具体技术方案如下：一种微型四扑翼飞行器控制系统，包括两个空心杯电机、两个直线舵机，其特征在于，还包括机载控制板，遥控器，其中，

所述机载控制板包括七轴姿态检测单元、微处理器、通讯模块；

所述七轴姿态检测单元包括三轴加速度计、三轴陀螺仪和气压计，所述三轴加速度计用于测量三轴加速度、所述三轴陀螺仪用于测量三轴角速度、所述气压计用于通过测量气压来估测高度；

所述微处理器用于数据处理和数据融合，通过数据处理和数据融合得到微型四扑翼飞行器的俯仰角、滚转角和偏航角，控制所述两个空心杯电机和两个直线舵机；

所述通讯模块用于所述机载控制板和所述遥控器之间的信息传输。

一种利用上述控制系统的控制方法，具体控制步骤如下：

步骤1：所述遥控器与所述通讯模块连接后，由所述遥控器给定微型四扑翼飞行器的期望俯仰角p_e、期望滚转角r_e、期望偏航角y_e以及期望高度值h_e；

步骤2：规定X轴为滚转轴、Y轴为俯仰轴、Z轴为偏航轴，所述三轴陀螺仪和所述三轴加速度计按周期T采样，得到三个角速度值分别为w_x、w_y、w_z，得到三个加速度值分别为a_x、a_y、a_z；

步骤3：计算实际俯仰角p_r、r_r、y_r，上一周期时四元数分别为q0₀、q1₀、q2₀、q3₀，根据姿态矩阵变换求得，重力方向在机体坐标系下的分量：

x方向的分量为：verxZ＝2×(q1₀×q3₀-q0₀×q2₀)，

y方向的分量为：veryZ＝2×(q0₀×q1₀+q2₀×q3₀)，

z方向的分量为：

取加速度方向向量和姿态矩阵中重力方向的向量叉乘来补偿陀螺仪的零漂，向量叉乘后所得向量在机体坐标系x轴上的投影为ex，在y轴上的投影为ey，在z轴上的投影为ez，则各分量为：

上一周期所述三轴陀螺仪角速度的积分量在机体坐标系x轴上的投影为exInt₀，在y轴上的投影为eyInt₀，在z轴上的投影为ezInt₀，积分系数为Ki₀，则本周期所述三轴陀螺仪角速度的积分量exInt、eyInt、ezInt为：

更新后的角速度在机体坐标系的投影分量分别为：w_x1、w_y1、w_z1，比例系数为Kp₀，则

新的四元数为q₀、q₁、q₂、q₃，运用一阶龙格库塔法更新四元数：

由此得到实际俯仰角为：

步骤4：计算误差，三个姿态角的误差为Δp，Δr，Δy：

步骤5：角度环PID控制，即利用三个姿态角误差值Δp，Δr，Δy，通过PID控制算法，输出三个期望角速度值wx_e、wy_e、wz_e：

其中，K_p为比例系数、K_i为积分系数、K_d为微分系数；

步骤6：角速度环PID控制，即利用期望角速度值wy_e、wx_e、wz_e和实际角速度值w_y1、w_x1、w_z1的差值，通过PID控制算法，输出三个电机驱动量，分别为p，r，y：

步骤7：高度环PID控制，即利用期望高度值h_e和实际高度值h₀的差值，通过PID算法，输出电机驱动量h，由气压计和加速度计得出的估计高度值为h₀，则：

步骤8：姿态控制量和电机驱动量整合，微型四扑翼飞行器在h₀处悬停的电机驱动量为Thrust，则

第一空心杯电机的驱动量：motor1＝Thrust+h+r，

第二空心杯电机的驱动量：motor2＝Thrust+h-r，

第一直线舵机的驱动量：motor3＝p-y，

第二直线舵机的驱动量：motor4＝-p-y。

进一步地，所述机载控制板的主控芯片为STM32F405RGT6，用六轴传感器MPU6050替代所述三轴加速度计和三轴陀螺仪，所述通讯模块为NRF24L01+。

本发明的有益效果在于：

1.通过遥控器和七轴姿态检测单元的配合，可以有效地控制微型四扑翼飞行器的两个空心杯电机和两个直线舵机，从而使飞行器实现多姿态以及悬停；

2.采用传统的串级PID控制，使得系统更加稳定；

3.控制硬件可高度集成，控制板体积小、重量轻；

4.采用处理性能优越的STM32F4芯片，同时功耗得到了降低。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，通过参考附图会更加清楚的理解本发明的特征和优点，附图是示意性的而不应理解为对本发明进行任何限制，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，可以根据这些附图获得其他的附图。其中：

图1为本发明的局部结构爆炸图；

图2为本发明的整体结构示意图；

图3为本发明的齿轮组件结构示意图；

图4为本发明的部分驱动机构示意图；

图5为本发明的部分驱动机构爆炸图；

图6为本发明的机架底部示意图；

图7为本发明的扑翼机构示意图；

图8为本发明的翅根基座示意图；

图9为本发明的连接机构整体示意图；

图10为本发明的连接机构爆炸图；

图11为本发明的连接机架底部示意图；

图12为本发明俯仰姿态正视图；

图13为本发明俯仰姿态左视图；

图14为本发明滚转姿态正视图；

图15为本发明偏航姿态左视图；

图16为本发明的一种微型四扑翼飞行器的控制系统的过程流程图；

图17为本发明的一种微型四扑翼飞行器的控制系统的控制流程图。

附图标号说明：

1-空心杯电机；101-空心杯电机齿轮；

2-连接机架；201-定位孔；202-舵机定位孔；203-舵机定位孔；204-舵机定位孔；205-舵机定位孔；206-支柱定位孔；

3-直线舵机；301-限位孔；302-滑块；303-导槽；定位孔-304；定位孔-305；定位孔-306；定位孔-307；4-支柱；5-底座；501-支柱定位孔；

6-机架；601-电机固定孔；602-定位孔；603-限位孔；604-定位孔；605-小齿轮定位孔；606-大齿轮定位孔607-定位孔；608-定位孔；

7-翅根组件；7A-第一翅根；7B-第二翅根；7C-第三翅根；

8-翅脉组件；8A-第一翅脉；8B-第二翅脉；8C-第三翅脉；8D-第四翅脉；

9-翅根基座；901-定位孔；902-定位孔；903-定位孔；10-翼膜；

11-齿轮组件；11A-小齿轮；11B-大齿轮；11A01-中心孔；11B01-中心孔；11B02-定位孔；

12-连杆组件；12A-第一连杆；12B-第二连杆；12C-第三连杆；12A01-销钉孔；12A02-销钉孔；12B01-销钉孔；12B02-销钉孔；12C01-销钉孔；12C02-销钉孔；

13-摇杆组件；13A-第一摇杆；13B-第二摇杆；13C-第三摇杆；13A01-销钉孔；13A02-销钉孔；13A03-销钉孔；13B01-销钉孔；13B02-销钉孔；13C01-销钉孔；13C02-销钉孔；13C03-定位孔。

具体实施方式

为了能够更清楚地理解本发明的上述目的、特征和优点，下面结合附图和具体实施方式对本发明进行进一步的详细描述。需要说明的是，在不冲突的情况下，本发明的实施例及实施例中的特征可以相互组合。

在下面的描述中阐述了很多具体细节以便于充分理解本发明，但是，本发明还可以采用其他不同于在此描述的其他方式来实施，因此，本发明的保护范围并不受下面公开的具体实施例的限制。

如图16-17所示，本发明的控制系统控制微型四扑翼飞行器，首先进行外设初始化，再进行传感数据的采集，对采集的数据进行姿态解算，求解得到飞行器实际的姿态和高度，将其与遥控器传来的期望姿态和期望高度作比较，进行PID调节，将其转化为电机和舵机的控制量。

具体的，一种微型四扑翼飞行器控制系统，包括两个空心杯电机、两个直线舵机，其特征在于，还包括机载控制板，遥控器，其中，

机载控制板包括七轴姿态检测单元、微处理器、通讯模块；

七轴姿态检测单元包括三轴加速度计、三轴陀螺仪和气压计，三轴加速度计用于测量三轴加速度、三轴陀螺仪用于测量三轴角速度、气压计用于通过测量气压来估测高度；

微处理器用于数据处理和数据融合，通过数据处理和数据融合得到微型四扑翼飞行器的俯仰角、滚转角和偏航角，控制两个空心杯电机和两个直线舵机；

通讯模块用于机载控制板和遥控器之间的信息传输。

一种利用上述控制系统的控制方法，具体控制步骤如下：

步骤1：遥控器与通讯模块连接后，由遥控器给定微型四扑翼飞行器的期望俯仰角p_e、期望滚转角r_e、期望偏航角y_e以及期望高度值h_e；

步骤2：规定X轴为滚转轴、Y轴为俯仰轴、Z轴为偏航轴，三轴陀螺仪和三轴加速度计按周期T采样，得到三个角速度值分别为w_x、w_y、w_z，得到三个加速度值分别为a_x、a_y、a_z；

步骤3：计算实际俯仰角p_r、r_r、y_r，上一周期时四元数分别为q0₀、q1₀、q2₀、q3₀，根据姿态矩阵变换求得，重力方向在机体坐标系下的分量：

x方向的分量为：verxZ＝2×(q1₀×q3₀-q0₀×q2₀)，

y方向的分量为：veryZ＝2×(q0₀×q1₀+q2₀×q3₀)，

z方向的分量为：

取加速度方向向量和姿态矩阵中重力方向的向量叉乘来补偿陀螺仪的零漂，向量叉乘后所得向量在机体坐标系x轴上的投影为ex，在y轴上的投影为ey，在z轴上的投影为ez，则各分量为：

上一周期三轴陀螺仪角速度的积分量在机体坐标系x轴上的投影为exInt₀，在y轴上的投影为eyInt₀，在z轴上的投影为ezInt₀，积分系数为Ki₀，则本周期三轴陀螺仪角速度的积分量exInt、eyInt、ezInt为：

更新后的角速度在机体坐标系的投影分量分别为：w_x1、w_y1、w_z1，比例系数为Kp₀，则

新的四元数为q₀、q₁、q₂、q₃，运用一阶龙格库塔法更新四元数：

由此得到实际俯仰角为：

步骤4：计算误差，三个姿态角的误差为Δp，Δr，Δy：

步骤5：角度环PID控制，即利用三个姿态角误差值Δp，Δr，Δy，通过PID控制算法，输出期望角速度值wx_e、wy_e、wz_e：

其中，K_p为比例系数、K_i为积分系数、K_d为微分系数；

步骤6：角速度环PID控制，即利用期望角速度值wy_e、wx_e、wz_e和实际角速度值w_y1、w_x1、w_z1的差值，通过PID控制算法，输出三个电机驱动量，分别为p，r，y：

步骤7：高度环PID控制，即利用期望高度值h_e和实际高度值h₀的差值，通过PID算法，输出电机驱动量h，由气压计和加速度计得出的估计高度值为h₀，则：

步骤8：姿态控制量和电机驱动量整合，微型四扑翼飞行器在h₀处悬停的电机驱动量为Thrust，Thrust应与微型四扑翼飞行器的自重相对应，则

第一空心杯电机的驱动量：motor1＝Thrust+h+r，

第二空心杯电机的驱动量：motor2＝Thrust+h-r，

第一直线舵机的驱动量：motor3＝p-y，

第二直线舵机的驱动量：motor4＝-p-y。

机载控制板的主控芯片为STM32F405RGT6，用六轴传感器MPU6050替代三轴加速度计和三轴陀螺仪，通讯模块为NRF24L01+，气压计为BMP280。

本发明还涉及一种具体的微型四扑翼飞行器，左右结构相同且完全对称，故只阐述一边。如图1-2所示，一种微型四扑翼飞行器，包括连接机构、驱动机构和扑翼机构，其中，

如图9-11所示，连接机构包括连接机架2、支柱4和底座5，支柱4通过连接机架2上的支柱定位孔206和底座5上的支柱定位孔501固定连接于连接机架2和底座5之间；

驱动机构关于连接机构的中心轴线对称、左右两侧结构相同，包括两个空心杯电机1、两个直线舵机3、两个机架6、两个齿轮组件11、两个连杆组件12、两个摇杆组件13；

如图3所示，空心杯电机1通过电机固定孔601过盈配合固定于机架6上，空心杯电机1包括空心杯电机齿轮101；直线舵机3通过连接机架2上的舵机定位孔202、舵机定位孔203、舵机定位孔204和舵机定位孔205以及自身的定位孔304、定位孔305、定位孔306、定位孔307固定于连接机架2上；

如图4-6所示，机架6通过定位孔201与连接机架2连接，机架6能够绕定位孔201转动；

齿轮组件11包括小齿轮11A、大齿轮11B，小齿轮11A通过中心孔11A01和小齿轮定位孔605销接于机架6上，并能够绕中心孔11A01转动，大齿轮11B通过中心孔11B01和大齿轮定位孔606销接于机架6上，并能够绕中心孔11B01转动，空心杯电机齿轮101与小齿轮11A相切连接，小齿轮11A与大齿轮11B相切连接，空心杯电机齿轮101带动小齿轮11A转动，小齿轮11A带动大齿轮11B转动；

连杆组件12包括第一连杆12A、第二连杆12B和第三连杆12C，所第一连杆12A和第二连杆12B的一端分别通过销钉孔12A01和销钉孔12B01销接于大齿轮11B上的定位孔11B02，大齿轮11B带动第一连杆12A和第二连杆12B做往复运动；

摇杆组件13包括第一摇杆13A、第二摇杆13B和第三摇杆13C，

第一摇杆13A通过销钉孔13A02和定位孔607销接于机架6，并能绕着销钉孔13A02转动；第二摇杆13B通过销钉孔13B02销接于机架6，并能绕着销钉孔13B02转动；第三摇杆13C通过销钉孔13C02和定位孔608销接于机架6，并能绕销钉孔13C02转动；

第一连杆12A的另一端通过销钉孔12A02和销钉孔13A01销接于第一摇杆13A，带动第一摇杆13A做往复运动；第二连杆12B的另一端通过销钉孔12B02和销钉孔13B01销接于第二摇杆13B，带动第二摇杆13B做往复运动；

第一摇杆13A通过销钉孔13A03和销钉孔12C01销接于第三连杆12C的一端，并带动第三连杆12C做往复运动；第三连杆12C的另一端通过销钉孔12C02和销钉孔13C01销接于第三摇杆13C，并带动第三摇杆13C做往复运动；

扑翼机构关于连接机构的中心轴线对称、左右两侧结构相同。

如图7-8所示，扑翼机构关于连接机构的中心轴线对称、左右两侧结构相同，包括两个翅根组件7、四个翅脉组件8、两个翅根基座9、四片翼膜10，其中，

翅根组件7包括第一翅根7A、第二翅根7B、第三翅根7C，翅脉组件8包括第一翅脉8A、第二翅脉8B、第三翅脉8C、第四翅脉8D；翅根基座9为三个顶点处分别设置有定位孔的中空三角形；

第一翅根7A、第二翅根7B和第三翅根7C的一端分别固定于翅根基座9的定位孔901、定位孔902和定位孔903内，第二翅根7B和第三翅根7C的另一端通过定位孔固定于机架6上；

直线舵机3上设置有滑块302和导槽303，滑块302上设置有限位孔301，第一翅根7A的另一端穿过直线舵机3上的限位孔301和机架6上的限位孔603后通过机架6上的定位孔602固定在机架6上，滑块302沿着导槽303滑动时，第一翅根7A发生变形，从而改变扑翼机构的倾斜度；

第一翅脉8A通过定位孔13C03固定于第三摇杆13C上；翼膜10粘接在翅脉组件8和第一翅根7A上。

为了方便理解本发明的上述技术方案，以下通过具体使用方式上对本发明的上述技术方案进行详细说明。

在具体使用时，启动空心杯电机1，使得空心杯电机齿轮101转动，带动小齿轮11A绕着中心孔11A01转动，小齿轮11A带动大齿轮11B绕着中心孔11B01转动；第一连杆12A以及第二连杆12B通过销钉与大齿轮11B相连，构成曲柄连杆机构，大齿轮11B带动第一连杆12A和第二连杆12B运动，第一连杆12A和第一摇杆13A相连，第二连杆12B和第二摇杆13B相连，第一连杆12A带动第一摇杆13A绕着机架6上的定位孔607转动，同样，第二连杆12B带动第二摇杆13B绕着机架6转动，定位孔布局如图5所示，接着，第一摇杆13A带动第三连杆12C摆动，第三连杆12C带动第三摇杆13C绕着机架6上的定位孔608转动；第三摇杆13C的周期性往复转动，带动扑翼的拍动，因为大齿轮11B所做的运动为周期性圆周运动，所以扑翼的运动在一系列传动之后也做周期性等幅拍动；如图2、10、12所示，直线舵机限位孔301内横穿过第一翅根7A，当滑块302沿着导槽303移动，第一翅根7A发生弯曲变形，带动机架6绕着定位孔201转动，从而使翅膀的角度发生倾斜。

如图12-15所示，在本发明上建立空间直角坐标系，规定X轴为滚转轴、Y轴为俯仰轴、Z轴为偏航轴。规定X正方向为前方，Z轴正方向为上方。规定方向左、右从机体自身判断，即Y轴正半轴为机体左侧，Y轴负半轴为机体右侧。

初始时保持对称姿势。当要做正方向俯仰运动时，只需要将左侧直线舵机和右侧直线舵机朝后运动，左侧扑翼和右侧扑翼随着翅根向后倾斜，飞行器产生一个向前的分力，实现俯仰运动。两个直线舵机是180度安装的，当两个直线舵机一起朝后运动时，一个PWM波增加，另一个PWM波减小，左侧直线舵机朝后运动时所需PWM波增加，右侧直线舵机朝后运动时所需PWM波减小；两侧直线舵机上的滑块分别沿着导槽向后滑动，带动左侧第一翅根和右侧第一翅根发生变形，使得左侧机架和右侧机架分别绕着定位孔转动，从机体坐标系看来，左侧机架和右侧机架绕着俯仰Y轴做顺时针运动。如图12、13可见，升力产生滚转X轴上的分力，即向前的分力，使得机体做俯仰运动。

当要做正方向滚转运动时，如图14所示，左侧空心杯电机转速增大，右侧空心杯电机转速减小，升力改变使机体产生沿滚转X轴正方向的扭矩，使得机体做滚转运动。

当要做正方向偏航运动时，如图15所示，左侧直线舵机上的滑块沿着左侧导槽向前滑动，右侧直线舵机上的滑块沿着右侧导槽向后滑动，带动左侧第一翅根和右侧第一翅根发生变形，使得左侧机架和右侧机架分别绕着定位孔转动，从机体坐标系看来，左侧机架绕着Y轴做逆时针运动，右机架绕着Y轴做顺时针运动，升力改变使机体产生沿偏航Z轴正方向上的扭矩，使得机体做偏航运动。

在本发明中，除非另有明确的规定和限定，术语“安装”、“相连”、“连接”、“固定”等术语应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或成一体；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通或两个元件的相互作用关系。对于本领域的普通技术人员而言，可以根据具体情况理解上述术语在本发明中的具体含义。

在本发明中，除非另有明确的规定和限定，第一特征在第二特征之“上”或之“下”可以包括第一和第二特征直接接触，也可以包括第一和第二特征不是直接接触而是通过它们之间的另外的特征接触。而且，第一特征在第二特征“之上”、“上方”和“上面”包括第一特征在第二特征正上方和斜上方，或仅仅表示第一特征水平高度高于第二特征。第一特征在第二特征“之下”、下方”和“下面”包括第一特征在第二特征正下方和斜下方，或仅仅表示第一特征水平高度小于第二特征。

在本发明中，术语“第一”、“第二”、“第三”、“第四”仅用于描述目的，而不能理解为指示或暗示相对重要性。术语“多个”指两个或两个以上，除非另有明确的限定。

以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (3)

1.一种微型四扑翼飞行器控制系统，包括第一空心杯电机和第二空心杯电机、第一直线舵机和第二直线舵机，其特征在于，还包括机载控制板，遥控器，其中，

所述机载控制板包括七轴姿态检测单元、微处理器、通讯模块；

所述七轴姿态检测单元包括三轴加速度计、三轴陀螺仪和气压计，所述三轴加速度计用于测量三轴加速度、所述三轴陀螺仪用于测量三轴角速度、所述气压计用于通过测量气压来估测高度；

所述微处理器用于数据处理和数据融合，通过数据处理和数据融合得到微型四扑翼飞行器的俯仰角、滚转角和偏航角，控制所述两个空心杯电机和两个直线舵机；

所述通讯模块用于所述机载控制板和所述遥控器之间的信息传输。

2.一种利用权利要求1所述的控制系统的控制方法，其特征在于，具体控制步骤如下：

步骤1：所述遥控器与所述通讯模块连接后，由所述遥控器给定微型四扑翼飞行器的期望俯仰角p_e、期望滚转角r_e、期望偏航角y_e以及期望高度值h_e；

步骤2：规定X轴为滚转轴、Y轴为俯仰轴、Z轴为偏航轴，所述三轴陀螺仪和所述三轴加速度计按周期T采样，得到三个角速度值分别为w_x、w_y、w_z，得到三个加速度值分别为a_x、a_y、a_z；

步骤3：计算实际俯仰角p_r、r_r、y_r，上一周期时四元数分别为q0₀、q1₀、q2₀、q3₀，根据姿态矩阵变换求得，重力方向在机体坐标系下的分量：

x方向的分量为：verxZ＝2×(q1₀×q3₀-q0₀×q2₀)，

y方向的分量为：veryZ＝2×(q0₀×q1₀+q2₀×q3₀)，

z方向的分量为：

取加速度方向向量和姿态矩阵中重力方向的向量叉乘来补偿陀螺仪的零漂，向量叉乘后所得向量在机体坐标系x轴上的投影为ex，在y轴上的投影为ey，在z轴上的投影为ez，则各分量为：

上一周期所述三轴陀螺仪角速度的积分量在机体坐标系x轴上的投影为exInt₀，在y轴上的投影为eyInt₀，在z轴上的投影为ezInt₀，积分系数为Ki₀，则本周期所述三轴陀螺仪角速度的积分量exInt、eyInt、ezInt为：

更新后的角速度在机体坐标系的投影分量分别为：w_x1、w_y1、w_z1，比例系数为Kp₀，则

新的四元数为q₀、q₁、q₂、q₃，运用一阶龙格库塔法更新四元数：

由此得到实际俯仰角为：

步骤4：计算误差，三个姿态角的误差为Δp，Δr，Δy：

步骤5：角度环PID控制，即利用三个姿态角误差值Δp，Δr，Δy，通过PID控制算法，输出三个期望角速度值wx_e、wy_e、wz_e：

其中，K_p为比例系数、K_i为积分系数、K_d为微分系数；

步骤6：角速度环PID控制，即利用期望角速度值wy_e、wx_e、wz_e和实际角速度值w_y1、w_x1、w_z1的差值，通过PID控制算法，输出三个电机驱动量，分别为p，r，y：

步骤7：高度环PID控制，即利用期望高度值h_e和实际高度值h₀的差值，通过PID算法，输出电机驱动量h，由气压计和加速度计得出的估计高度值为h₀，则：

步骤8：姿态控制量和电机驱动量整合，微型四扑翼飞行器在h₀处悬停的电机驱动量为Thrust，则

第一空心杯电机的驱动量：motor1＝Thrust+h+r，

第二空心杯电机的驱动量：motor2＝Thrust+h-r，

第一直线舵机的驱动量：motor3＝p-y，

第二直线舵机的驱动量：motor4＝-p-y。

3.根据权利要求1或2所述的一种微型四扑翼飞行器控制系统，其特征在于，所述机载控制板的主控芯片为STM32F405RGT6，用六轴传感器MPU6050替代所述三轴加速度计和三轴陀螺仪，所述通讯模块为NRF24L01+。

Images