一种基于光学定位的四旋翼飞行器系统的制作方法

本发明涉及无人机技术领域，尤其是一种基于光学定位的四旋翼飞行器系统。

背景技术：

四旋翼飞行器是一种近年来逐渐流行起来的无人飞行器，由于其特殊的飞行方式和较低的机械故障率而推广开来。尤其是在航拍和农业植保领域，作为一个稳定的飞行平台被发掘出了愈来愈多新的功能和用途，随之而来的是对其性能的要求也逐渐变高，尤其是对飞行器能够精确定位的要求。

多旋翼飞行器一般采用gps定位，通过gps模块接收卫星信号得到经纬度坐标来实现在空间中飞行坐标的保持。这样的定位方式能够获得较高的定位精度，但这样的定位方式在gps信号差或者没有信号的室内环境便无法进行定位。甚至出现飞出控制范围导致失控，或者因为漂移而撞到障碍物造成坠机。

技术实现要素：

为解决上述问题本发明提供一种基于光学定位的四旋翼飞行器系统，能够使飞行器在无gps信号进行定位及防止漂移。

为了实现上述目的，本发明采用的技术方案为：

一种基于光学定位的四旋翼飞行器系统，包括飞行器本体及控制模组，所述飞行器本体包括机座、机臂、螺旋桨、电机及控制模组，所述机臂设有四个并分别与所述机座固定连接，四所述机臂为正交的x形结构，所述机臂远离所述机座的一端与所述螺旋桨转动连接，所述电机用于所述螺旋桨的驱动；

所述控制模组装设在所述机座中，并包括坐标系模块、运动模块、应用模块、自稳模块及主控模块，

所述坐标系模块包括原点、x轴、y轴及z轴，所述原点位于所述机座上并设置在四所述机臂的交叉点上；所述x轴为两相邻所述机臂之间夹角的角平分线，所述y轴与所述x轴相差90°并与所述x轴处于同一平面上；所述z轴经过所述原点并与垂直于x轴；

所述运动模块用于通过分别对四个所述电机进行控制，进行升降、俯仰、横滚、偏航及水平运动；

所述应用模块包括定高子模块、定点子模块及避障功能，

所述定高子模块用于所述飞行器本体目标高度的设定，所述定高子模块通过超声波传感器测量当前高度与目标高度的距离，并控制所述电机将所述飞行器本体调整到目标高度的位置；

所述定点子模块用于所述飞行器本体目标平面位置的设定，所述定点子模块用于通过光流传感器配合定焦镜头采集所述飞行器本体飞行过程中在所述x轴及所述y轴的位移速率，根据所述x轴及所述y轴的位移速率获得所述飞行器本体当前平面位置与目标平面位置的距离，并通过控制所述电机将所述飞行器本体调整到目标平面位置；

所述避障功能用于所述飞行器本体运动方向前方障碍物的检测，所述避障功能通过红外传感器获取障碍物的信号，当所述红外传感器在设定的距离内检测到障碍物时产生控制量，所述运动模块控制所述飞行器本体往反方向飞行；当所述红外传感器检测到在设定的距离内检测没有检测到障碍物时清除所述控制量；

所述自稳模块与所述运动模块及所述应用模块相互反馈，并用于控制所述飞行器本体的自稳飞行；所述自稳模块包括滤波子模块、姿态获取子模块及串级pid控制子模块，所述滤波子模块用于清除所述螺旋桨因高频率振动产生的加速度误差；所述姿态获取子模块用于通过姿态传感器获得所述飞行器本体中的姿态角度；所述串级pid控制子模块用于根据所述姿态角度的信号所述飞行器本体的姿态，所述串级pid控制子模块采用内环角速度环pid控制，外环角度环pi控制的串级pid控制方法；

所述串级pid控制子模块将所述定点子模块中所述飞行器本体与z轴的相对水平位移量作为内环误差输入，进行pid控制，把内环的输出直接反馈到所述串级pid控制子模块的姿态pid的角度环，并使所述电机获得姿态信号，以根据漂移速率调整所述无人机本体往运动方向相反的倾斜角度；

所述主控模块用于所述坐标系模块、所述运动模块、所述应用模块及所述自稳模块的数据算法处理，并用于各个传感器的驱动。

进一步地，两相邻的所述电机转动方向相反。

进一步地，所述运动模块包括升降运动子模块、俯仰运动子模块、横滚运动子模块、偏航运动子模块及水平运动子模块，所述升降运动子模块用于通过控制四所述电机以相同的转速转动；所述俯仰运动子模块用于通过控制两相邻所述电机以一相同的转速转动，另外两相邻所述电机以另一相同的转速转动；所述横滚运动子模块用于通过控制两相对所述电机以一相同的转速转动，另外两相对所述电机以一相同的转速转动；所述偏航运动子模块用于控制两相对所述螺旋桨受到一相同的反作用力，另外两相对所述螺旋桨受到另一相同的反作用力；所述水平运动子模块用于通过所述自稳模块控制所述飞行器本体倾斜，使所述飞行器本进行前进、后退、左侧及右侧飞行。

进一步地，所述定高子模块能够将所述超声波模块测量得到的实际高度与目标高度进行比较并获得高度误差，当所述高度误差超出设定值时，将所述高度误差乘以比例系数获得pid控制比例，以使所述运动模块对所述飞行器本体进行驱动调整；对于所述定高子模块产生的静态误差，通过引入积分项，通过将误差累加后作用于所述运动模块。

进一步地，所述定高子模块具有超前调节作用，通过采集两次所述飞行器本体当前高度与目标高度的距离的高度差，以获得所述飞行器本体的相对移动方向和速度，并将分别将移动方向和速度的差值乘上微分系数作为微分输出，以对所述飞行器本体的速度进行抑制。

进一步地，所述定点子模块通过所述主控模块将所述飞行器本体飞行过程中在所述x轴及所述y轴的位移速率进行积分，以获得所述飞行器本体与z轴的相对水平位移量，将所述位移量通过所述串级pid控制子模块作为外环误差输入，进行pi控制，并将外环总的输出融入内环误差，通过内环间接控制所述飞行器本体姿态，以消除所述飞行器本体产生漂移后自动返回目标平面位置。

进一步地，还包括气压传感器，所述气压传感器用于所述飞行器本体距离地面较高的位置进行定高飞行时获取飞行的高度。

进一步地，所述滤波子模块将前面多次的采样值保存，采用环形队列的形式，定义一个元素个数与采集次数相同的数组，将采样数据依次装入数组，并在装入一个新数据去掉一个旧数据，以获得前面多次的采样数据的平均值；所述滤波子模块采用窗口滑动滤波，通过输入最新的采样数据，和前面多次的采样数据进行求平均值，前面采集数据的次数决定了平滑处理的力度。

进一步地，所述姿态获取子模块将所述飞行器本体绕所述x轴、所述y轴及所述z轴的旋转用四元数表示出来，再运用四元数的乘法将各个轴的旋转四元数相乘，以获得表示所述飞行器本体姿态角的四元数，通过将所述飞行器本体姿态角的四元数转换为欧拉角，通过所述欧拉角获得所述飞行器本体的实时姿态角度。

进一步地，其特征在于：还包括遥控模块，所述遥控模块用于远程为所述主控模块提供控制的信号。

本发明的有益效果是，

1.定点子模块用于通过光流传感器配合定焦镜头采集数据，采用视觉模块对外部环境进行图像分析来得到飞行器本体的相对运动数据或相对环境坐标，不依赖外界的信号来定位。因此能够在无gps信号乃至室内实现定位；通过将定点子模块引入串级pid控制子模块，将飞行器本体相对地面运动速率作为内环误差输入，进行pid控制，把内环的输出直接反馈到姿态pid的角度环，使得漂移速率越快则无人机往反方向倾斜的角度越大，实现在无gps信号的环境内也能够抵抗漂移。

2.由于螺旋桨质量很难达到均匀分布，因此当四个电机高速运行时，会产生较高频率的振动。而加速度计受振动影响比较明显，在机体振动时采集到的加速度数据将发生较大的误差。通过滤波子模块清除螺旋桨因高频率振动产生的加速度误差，并采用窗口滑动滤波，通过输入最新的采样数据，和前面多次的采样数据进行求平均值，减少高频干扰。

3.由于欧拉角的解算运算量较大，对mcu运算能力要求较高，为了降低运算量，提高运算速度，姿态获取子模块通过飞行器本体姿态角的四元数间接获得方便控制使用的欧拉角，增加飞行器本体的姿态改变效率；串级pid控制子模块采用内环角速度环pid控制，外环角度环pi控制的串级pid控制方法，根据姿态获取子模块的信号，对所需的姿态进行调整；并以角速度作为内环误差，达到的效果则是抑制飞行器在三个轴向上的角速度，使飞行器本体尽可能地保持当前姿态不发生突变，通过引入外环的角度控制，实则是在内环的稳定控制下对姿态角进行定时修正，使其保持在我们想要的角度。

4.由于在单纯的比例控制下太高的比例输出会导致振荡，减小比例系数虽然能避免振荡但会导致接近目标时的调节力度不足，以至于出现静态误差，定高子模块通过将误差累加后作用于运动模块，能够有效地消除静态误差，使飞行器本体达到设定的目标高度；定高子模块的超前调节作用，能够起到加大系统阻尼的效果。

附图说明

图1是本发明一较佳实施方式的基于光学定位的四旋翼飞行器系统的飞行器本体结构示意图。

图2是本发明一较佳实施方式的基于光学定位的四旋翼飞行器系统的结构框图。

图中，1-机座，11-机臂，12-螺旋桨，13-电机，2-坐标系模块，3-运动模块，31-升降运动子模块，32-俯仰运动子模块，33-横滚运动子模块，34-偏航运动子模块，35-水平运动子模块，4-应用模块，41-定高子模块，42-定点子模块，43-避障子模块，5-自稳模块，51-滤波子模块，52-姿态获取子模块，53-串级pid控制子模块，6-控制模组，7-遥控模块，81-超声波传感器，82-光流传感器，83-定焦镜头，84-红外传感器，85-姿态传感器，86-气压传感器。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

需要说明的是，当组件被称为“固定于”另一个组件，它可以直接在另一个组件上或者也可以存在居中的组件。当一个组件被认为是“连接”另一个组件，它可以是直接连接到另一个组件或者可能同时存在居中组件。当一个组件被认为是“设置于”另一个组件，它可以是直接设置在另一个组件上或者可能同时存在居中组件。本文所使用的术语“垂直的”、“水平的”、“左”、“右”以及类似的表述只是为了说明的目的。

除非另有定义，本文所使用的所有的技术和科学术语与属于本发明的技术领域的技术人员通常理解的含义相同。本文中在本发明的说明书中所使用的术语只是为了描述具体的实施例的目的，不是旨在于限制本发明。本文所使用的术语“及/或”包括一个或多个相关的所列项目的任意的和所有的组合。

请同时参见图1及图2，一种基于光学定位的四旋翼飞行器系统，包括飞行器本体及控制模组，飞行器本体包括机座1、机臂11、螺旋桨12、电机13及控制模组，机臂11设有四个并分别与机座1固定连接，四机臂13为正交的x形结构，机臂11远离机座1的一端与螺旋桨12转动连接，电机13用于螺旋桨12的驱动。

在本实施例中，两相邻的电机13转动方向相反。机座1、机臂11、螺旋桨12采用工程塑料或是碳纤维材料，以减少自身的重量并且不容易产生形变。

控制模组装设在机座1中，包括坐标系模块2、运动模块3、应用模块4、自稳模块5、主控模块6及遥控模块7。

坐标系模块2包括原点、x轴、y轴及z轴，原点位于机座1上并设置在四机臂11的交叉点上；x轴为两相邻机臂11之间夹角的角平分线，y轴与x轴相差90°并与x轴处于同一平面上；z轴经过原点并与垂直于x轴。飞行器本体绕各个轴向的转动的角度便具有了飞行器各个方位的姿态角度意义，由此飞行器本体在三维空间中的姿态可以用飞行器在机体坐标系中绕三个轴转过的角度表示。

运动模块3运用于通过分别对四个电机13进行控制，进行升降、俯仰、横滚、偏航及水平运动。

在本实施例中，运动模块3包括升降运动子模块31、俯仰运动子模块32、横滚运动子模块33、偏航运动子模块24及水平运动子模块35，升降运动子模块31用于通过控制四电机13以相同的转速转动；俯仰运动子模块32用于通过控制两相邻电机13以一相同的转速转动，另外两相邻电机13以另一相同的转速转动；横滚运动子模块33用于通过控制两相对电机13以一相同的转速转动，另外两相对电机13以一相同的转速转动；偏航运动子模块34用于控制两相对螺旋桨12受到一相同的反作用力，另外两相对螺旋桨12受到另一相同的反作用力；水平运动子模块35用于通过自稳模块5控制飞行器本体倾斜并进行前进、后退、左侧及右侧水平飞行。

应用模块4包括定高子模块41、定点子模块42及避障子模块43。

定高子模块41用于飞行器本体目标高度的设定，定高子模块41通过超声波传感器81测量当前高度与目标高度的距离，并控制电机13将飞行器本体调整到目标高度的位置。

定点子模块42用于飞行器本体目标平面位置的设定，定点子模块42用于通过光流传感器82配合定焦镜头83采集飞行器本体飞行过程中在x轴及y轴的位移速率，根据x轴及y轴的位移速率获得飞行器本体当前平面位置与目标平面位置的距离，并通过控制电机13将飞行器本体调整到目标平面位置。定点子模块42用于通过光流传感器82配合定焦镜头83采集数据，采用视觉模块对外部环境进行图像分析来得到飞行器本体的相对运动数据或相对环境坐标，不依赖外界的信号来定位。因此能够在无gps信号乃至室内实现定位。

避障子模块43用于飞行器本体运动方向前方障碍物的检测，避障子模块43通过红外传感器84获取障碍物的信号，当红外传感器84在设定的距离内检测到障碍物时产生控制量，运动模块3控制飞行器本体往反方向飞行；当红外传感器84检测到在设定的距离内检测没有检测到障碍物时清除控制量。由于采用的是外传感器84，当在设定的距离内检测到障碍物时返回的是低电平信号，程序中只需要根据触发的外传感器84方位产生一定程度的控制量，将其输入姿态控制环，使得飞行器本体往反方向飞行即可，当远离障碍物后，清除这一控制量，使飞行器恢复水平状态。

自稳模块5与运动模块3及应用模块4相互反馈，并用于控制飞行器本体的自稳飞行；自稳模块5包括滤波子模块51、姿态获取子模块52及串级pid控制子模块53。

滤波子模块51用于清除螺旋桨12因高频率振动产生的加速度误差。由于螺旋桨12质量很难达到均匀分布，因此当四个电机13高速运行时，会产生较高频率的振动。而加速度计受振动影响比较明显，在振动时采集到的加速度数据将发生较大的误差。滤波子模块51将前面多次的采样值保存，采用环形队列的形式，定义一个元素个数与采集次数相同的数组，将采样数据依次装入数组，并在装入一个新数据去掉一个旧数据，以获得前面多次的采样数据的平均值；滤波子模块51采用窗口滑动滤波，通过输入最新的采样数据，和前面多次的采样数据进行求平均值，前面采集数据的次数决定了平滑处理的力度。滤波子模块51能够有效抑制周期性的扰动，将数据的变化变得平滑。

姿态获取子模块52用于通过姿态传感器85获得飞行器本体中的姿态角度。姿态获取子模块52将飞行器本体绕x轴、y轴及z轴的旋转用四元数表示出来，再运用四元数的乘法将各个轴的旋转四元数相乘，以获得表示飞行器本体姿态角的四元数，通过将飞行器本体姿态角的四元数转换为欧拉角，通过欧拉角获得飞行器本体的实时姿态角度。

飞行器的自稳控制，最终的控制目标是飞行器的姿态角，因而准确且实时的姿态解算就显得尤为重要。空中的姿态变化可以看作是机体坐标系分别绕参考坐标系的三个轴旋转一定的角度得到的新的位置。其绕参考坐标系x、y、z三个轴转过的角度用欧拉角表示，分别为角φ、角θ和角若以z-y-x的顺序旋转则可以得到飞行器姿态的旋转矩阵如公式1所示。

尽管欧拉角的表示比较直观，可以较为方便进行姿态控制计算，但由公式1可以看出欧拉角的解算运算量较大，对mcu运算能力要求较高。为了降低运算量，提高运算速度，这里采用四元数来表示旋转。

将飞行器本体绕x、y、z三轴的旋转用四元数表示出来，再运用四元数的乘法将各个轴的旋转四元数相乘，以得到可以表示飞行器姿态角的四元数[q0，q1，q2，q3]，最后再将其转换为方便控制使用的欧拉角。其转换公式如式2所示。

由于飞行器的角速度便是飞行器本体绕三轴旋转的速度，将各个轴的角速度分别进行积分便是绕相应的轴转过的角度，陀螺仪的动态性能比较好，在短时间内使用角速度积分得到的旋转角度是比较可靠的，但由于是积分是离散的，累积误差在所难免。而加速度数据尽管动态性能不如角速度，但加速度数据始终是在实际的姿态角附近小幅度波动的，因此结合二者的特性，通过互补融合的方式将两种数据的优势集合起来。将加速度数据作为参考对角速度数据进行校正，防止累积误差进一步增大，

串级pid控制子模块53用于根据姿态角度的信号飞行器本体的姿态，串级pid控制子模块53采用内环角速度环pid控制，外环角度环pi控制的串级pid控制方法。

角速度环是对三个轴向上的角速度进行pid控制，控制对象是角速度，因为各个轴向上的角速度是其运动状态最直接的体现。当飞行器本体保持静止时，在三个轴上的角速度理论值为0。以角速度作为内环误差，达到的效果则是抑制飞行器在三个轴向上的角速度，使飞行器本体尽可能地保持当前姿态不发生突变，也就是角速度为理想化的零。

角速度控制能够使飞行器本体保持相对稳定，但飞行器本体并不能保持长时间的水平飞行。引入外环的角度控制，实则是在内环的稳定控制下对姿态角进行定时修正，使其保持在我们想要的角度。外环控制对象为角度，对角度的偏差进行pi控制。而外环的输出结果则作为内环的误差的一部分，使飞行器本体能迅速反应消除在姿态角度上的误差，实现保持角度飞行的功能。

在本实施例中，还包括气压传感器86，气压传感器86用于飞行器本体距离地面大于10米的位置进行定高飞行时获取飞行的高度。

主控模块6用于坐标系模块2、运动模块3、应用模块4及自稳模块5的数据算法处理，并用于各个传感器的驱动。遥控模块7用于远程为主控模块6提供控制的信号。

定高子模块41能够将超声波模块测量得到的实际高度与目标高度进行比较并获得高度误差，当高度误差超出设定值时，将高度误差乘以比例系数获得pid控制比例，以使运动模块3对飞行器本体进行驱动调整；对于定高子模块41产生的静态误差，通过引入积分项，通过将误差累加后作用于运动模块3。由于在单纯的比例控制下太高的比例输出会导致振荡，减小比例系数虽然能避免振荡但会导致接近目标时的调节力度不足，以至于出现静态误差，因此要想达到设定的目标高度，必须引入积分项，通过将误差累加后输出，这样便能有效地消除静态误差。

定高子模块41具有超前调节作用，通过采集两次飞行器本体当前高度与目标高度的距离的高度差，以获得飞行器本体的相对移动方向和速度，并将分别将移动方向和速度的差值乘上微分系数作为微分输出，以对飞行器本体的速度进行抑制，从而起到加大系统阻尼的效果。

串级pid控制子模块53将定点子模块42中飞行器本体与z轴的相对水平位移量作为内环误差输入，进行pid控制，把内环的输出直接反馈到串级pid控制子模块53的姿态pid的角度环，并使电机13获得姿态信号，以根据漂移速率调整无人机本体往运动方向相反的倾斜角度。使得漂移速率越快则无人机往反方向倾斜的角度越大，实现在无gps信号的环境内也能够抵抗漂移。

定点子模块42通过主控模块6将飞行器本体飞行过程中在x轴及y轴的位移速率进行积分，以获得飞行器本体与z轴的相对水平位移量，将位移量通过串级pid控制子模块53作为外环误差输入，进行pi控制，并将外环总的输出融入内环误差，通过内环间接控制飞行器本体姿态，以消除飞行器本体产生漂移后自动返回目标平面位置。

在本实施例中，定点子模块42首先是读取光流传感器82原始数据，接着便是使用角速度融入光流数据进行姿态变化补偿，再结合实际高度计算出飞行器位移。当总位移达到3厘米时才进行外环位移pi控制，留下3厘米的死区可以避免飞行器在原点附近过度振荡。始终进行调节的是内环的速度控制，通过对飞行器水平位移的实时抑制，达到防止漂移的效果。

在定点子模块42中，由于采用了定焦镜头83，定焦镜头83根据近大远小的视觉原理，当飞行器与地面的相对高度加大时，相同的水平位移在光学传感器上产生的像素位移会减小。具体表现为水平移动等长的一段距离，飞行器相对地面高度越高，传感器采集到的运动速率越小，积分得到的位移也相应越小，给人一种传感器变得迟钝的错觉。因此为了获得较为准确的测量位移，不能直接将传感器的速率积分作为飞行器的位移，而需要结合实际的高度计算得出。根据透镜成像原理可以知道，物体的实际长度和成像长度之比等于物距与像距之比，飞行器本体实际位移应该等于定焦镜头83位移乘上物距与像距的比值。

飞行器本体的姿态变化对光流传感器82会产生影响，如当飞行器本体绕机体坐标系的x轴产生旋转。同样光流传感器也发生同样的旋转，而在其旋转过程中所扫过的画面将导致传感器产生像素位移。但在这样的情况下飞行器本体并没有发生实际的水平位移，因此由飞行器绕机体坐标旋转所造成的光流数据不能用来表示飞行器的水平位移速率，应该将其过滤掉。而与飞行器本体的旋转联系最直接的便是角速度，并且角速度越大，光流传感器82扫过地面的相对速度也越快，同样捕获到的移动速率也越大。因此将角速度乘上一定的系数加入到对应轴向上的光流数据中便能起到补偿的作用。

在本实施例中，姿态传感器85采用mpu6050姿态传感器，选择的mpu6050姿态传感器能够采集飞行器的重力加速度在三个轴向上的分量，以及飞行器绕三个轴转动的角速度。通过i2c总线与主控芯片进行通信只需要两根信号线即可，使用较为方便。

气压传感器86采用ms5611气压传感器，ms5611气压计可以输出最高精度的压力值，将大气压强数值结合温度数值计算出的海拔高度，误差在一分米左右。该传感器可通过spi或i2c通信，但为了简化系统电路，采用的是i2c通信和mpu6050共用一条i2c总线。

光流传感器82采用adns-3080光流传感器，其具备两个自由度的现成光流传感器，可以直接输出其在两个维度的位移速率。该传感器分辨率为30×30像素，帧速率最高可达6400fps。为了更短的快门速度和在不良照明条件下获得更好的结果，通常将该传感器的帧速率设置为2000fps，通过高速spi总线进行通信。光流传感器82安装在机座1的底部，以正对地面。定焦镜头采用4.2mm定焦镜头。

超声波传感器81采用hc-sr04超声波传感器，其测量精度为3个毫米，测量范围为2厘米到400厘米，很好地满足了飞行器在低空定高的需求。超声波传感器81安装在机座1的底部，以正对地面。

红外传感器84当在测量范围内出现障碍物时，障碍物会将模块发射出的红外光反射回去，被模块接收。当模块检测到漫反射回来的红外光线时，则判定为检测到障碍物。模块将输出一个低电平信号给主控，而当没有检测到障碍物时，模块输出的是高电平信号。

电机13选用的是kingkong牌型号为2204-2300kv的无刷电机，并采用12v-2200ma的锂电池供电。还设有用于电机13调速的电子调速计，电子调速计选用银燕牌12a电子调速计，其最大电流能达到15a，满足电机的大电流驱动需求。

主控模块6选择型号为stm32f103c8t6的主控芯片，该芯片有44个引脚，带3组串口、两组硬件spi总线，硬件资源非常丰富，完全满足本发明的需求。

遥控模块7的无线通信部分采用nrf24l01无线模块。是一款工作在2.4～2.5ghz世界通用ism频段的单片无线收发器芯片，最高支持支持2mbps传输速率。在空旷的环境中可以实现最远2公里的数据传输。满足飞行器进行遥控飞行、高速数据传输的无线通信需求。

本发明的操作流程为：

1.飞行器上电后，首先执行的是系统各硬件部分及各变量的初始化工作。依次初始化主控模块6的定时器，以便产生2.5ms一次的定时中断，接着进行各传感器的初始化。

2.完成系统初始化后开始定时运行姿态获取子模块52，以获得实时的姿态角度。

3.主控模块6根据遥控模块7的指令遥控飞行器本体进行飞行运动；通过在遥控模块7设定目标位置，定高子模块41控制飞行器本体靠近目标位置的高度，定点子模块42控制飞行器本体靠近目标位置的水平位置，在定高子模块41和定点子模块42共同作用下，使飞行器本体到达设定目标位置。在飞行的过程中，姿态获取子模块52获得飞行器本体的姿态，并通过串级pid控制子模块53进行实时姿态调节，并通过电机13输出，以使飞行器本体顺利达到目标位置。