李艳涛+杨旭+肖银燕+鲁丹+宋好舒

摘要：针对现阶段四旋翼飞行器不能自主飞行的问题，提出了一种基于STM32的四旋翼自主飞行器控制系统，该系统通过单片机与MPU6050通信采集陀螺仪、加速度计数据，经卡尔曼滤波后由方向余弦矩阵解算出姿态，经PID调节器控制电机转速实现自主飞行功能。测试表明，该系统能够自主飞行且飞行高度在0～100m以内，起飞转速为50 r/s左右，最大巡航速度为11.2 m/s，续航能力为20 min。

关键词：四旋翼飞行器；STM32F103VCT6；MPU6050；卡尔曼滤波器；方向余弦矩阵；PID调节器

中图分类号：TP303 文献标识码：A 文章编号：1009-3044（2016）08-0212-03

1 背景

近年来，随着科技的发展，四旋翼飞行器在人们的生活当中扮演者越来越重的角色，可执行水灾、火灾、地震等灾情调查救援任务；化工厂等场所有毒气体浓度监测；重要设施连续监控；输油管线和输电线路的巡查；农田、林区农药喷洒；自然风景的取景拍照；当对特定地区进行日常环境监测，也可以使用这种飞行器，自动巡查完后自动返航并自动记录存储数据，大大减少人力成本。

在过去的几十年里，四旋翼飞行器相对固定翼飞行器发展却较为缓慢，这是因为四旋翼飞行器的控制较固定翼复杂，早期的技术水平无法实现飞行器的自主飞行控制。为了实现飞行器的自主飞行，文献[1]提出了一种双增益的PD控制算法对飞行器进行姿态控制； 将姿态估计算法和控制算法应用到飞行器中， 可以实现四旋翼的自主悬停等功能。但这种方法不能使飞行器自主的完成一些任务。文献[2]提出了一种新的消失点估计算法—VQME 算法，该算法的正确性、鲁棒性与实时性得到了验证，并且验证了其在工程应用中的可行性。同时针对四旋翼飞行器的欠驱动、多耦合等特性，提出了一种级联的多变量RBF神经网络PID自适应控制方法，运用该控制方法和以消失点为目标点的导航策略，但这种方法无法实现大范围飞行。文献[3]提出了一种基于磁传感器的自主导航系统，该系统硬件平台采MPU6000（集成了3轴陀螺仪和3轴加速度计）传感器实现姿态和速度解算，在此基础上设计了基于磁传感器的航向判断自主导航算法，通过实际飞行器的飞行方向与设定方向的偏差对飞行器的姿态角进行调节，但这种方法对于飞行途中的障碍物却无法规避。

综上所述，国内外在四轴飞行器的研究上得到了极大的发展，但有一些问题急需解决：第一，如何实现大范围自主飞行作业；第二，如何规避飞行器飞行途中的障碍物。针对这两个问题，受文[4-10]的启发，本文设计了一种利用GPS导航技术和超声波测距技术来实现自主飞行的方法，通过GPS导航来指引四旋翼飞行器的飞行路线，通过超声波测距可以实时测量周围障碍物距四旋翼飞行器的距离并使飞行器绕道飞行，从而实现飞行器的自主飞行。

2 系统基本工作原理

该系统工作原理框图如图1所示， 该系统由STM32F103单片机、骨架、MPU6050、GPS模块、超声波模块、压力计、地磁计、电池等构

成，其中骨架包括电机、桨翼、电调等组成，系统本着一体化的设计思路，将STM32F103、MPU6050、压力计、地磁计集成在一块3*5cm的控制板上，使用IIC总线的方式与控制系统连接。首先由GPS模块获取当前位置状态；然后通过MPU6050上的三轴陀螺仪及三轴加速器获取四旋翼飞行器的姿态角并调节飞行姿态；最后由GPS模块和超声波模块指引飞行器进行自主飞行。

3 主要硬件电路设计

3.1硬件总电路图

系统硬件部分由核心控制模块、动力模块、MPU6050模块、GPS导航模块、超声波模块等组成。其中核心控制模块由STM32F103芯片和外围电路组成，外围电路包括晶振电路、复位电路和电源电路。硬件总电路图如图2所示。

3.2无刷电机驱动电路

本设计采用四个电调驱动四个920KV自锁无刷电机。7脚和5脚为电机转向的控制引脚，当5脚为高电平，7脚为低电平时，电机将顺时针旋转，当5脚为低电平，7脚为高电平时，电机将逆时针旋转，当5脚和7脚的电平相同时，电机将不转动，6脚为使能端，可以通过控制输出到6脚的PWM信号的占空比来控制电机转速的大小，当PWM信号的占空比为0%时，电机将停止转动，当PWM信号的占空比为100%时，电机转速将达到最大[6-7]。

3.3 MPU6050模块

本设计采用MPU6050模块，它集成了3 轴MEMS 陀螺仪，3 轴MEMS加速度计，以及一个可扩展的数字运算处理器DMP，用IIC接口连接一个第三方的数字传感器。扩展之后就可以通过IIC 接口将飞行器的姿态角输出到控制核心模块。

MPU-6050 对陀螺仪和加速度计分别用了三个16 位的ADC，将其测量的模拟量转化为可输出的数字量。可精确跟踪快速和慢速的运动，传感器的测量范围是可控的，陀螺仪可测范围为±250，±500，±1000，±2000°/秒（dps），加速度计可测范围为±2，±4，±8，±16g。

3.4 GPS导航电路

GPS模块集成了RF射频芯片、基带芯片和核心CPU，并加上相关外围电路而组成的一个集成电路，GPS模组整合灵敏度高，功耗低，GPS芯片组解决方案在紧凑的设计里。可将同时追踪多达20颗卫星，并迅速确定1 Hz导航更新。

3.5超声波定高、测距电路

超声波是频率高于20000赫兹的声波，它方向性好，穿透能力强，易于获得较集中的声能，超声波测距的原理是利用超声波在空气中的传播速度为已知，测量声波在发射后遇到障碍物反射回来的时间，根据发射和接收的时间差计算出发射点到障碍物的实际距离，当测得距离小于10cm时，主程序将执行绕道飞行的命令。由单片机产生40KHz 的方波，直接驱动CD4049 芯片，下级的CD4049 则对40KHz 频率信号进行调理，使超声波传感器产生谐振。

4 系统软件设计

主程序流程图如图3所示，系统首先配置好STM32F103单片机、GPS导航模块、超声波模块、陀螺仪、加速度计等硬件设备；然后，通过遥控模块选择飞行器飞行模式：定高飞行或者GPS飞行；输入预设目的地，通过传感器获得当前状态参数，经控制系统执行算法、处理信息后，飞行器起飞进入预定飞行航道；最后通过时刻采集GPS导航模块、超声波模块的数据调节飞行姿态，指引飞行器到达目的地，飞行器到达目的地后主程序将返回到系统上电及初始化，等待下一个任务的下达。

4.1 GPS定位及导航

当主程序运行到GPS模块子程序时，系统首先关闭中断，对GPS模块进行初始化；然后打开中断，允许接收GPS信号，接着采集GPS数据并判断接收到的数据是否为推荐模式，然后再将所接收到的二进制码数据通过串口读入主控芯片；最后将单片射频收发芯片的通道0设置为飞行数据通道，通道1设置为接收数据通道，可实现单片射频收发芯片“多发单收”的功能，即多路设备发送数据，单路设备接收数据，可以使程序的运行效率更高及数据更新速度更快。GPS定位及导航流程图如图4所示。

4.2超声波定高及避障

飞行器的定高及避障是靠超声波对周围障碍物、地面的探测来实现的。因此，本部分的程序设计主要是针对超声波模块。首先对超声波模块初始化，选择飞行模式：定高飞行、GPS飞行。然后由单片机向模块的Trig管脚输入一个10us的高电平，模块检测到这个10us的高电平信号后就会发出8个40KHZ的超声波脉冲信号，此时打开定时器中断开始计时，当检测到回波信号后，停止定时器中断，单片机对数据进行处理得到此刻探测到的距离。最后当选择定高飞行模式时，若检测到距地面的距离到达设定数值时，保持飞行姿态；当选择GPS飞行模式时，若检测到距前方物体在1m之内，飞行器将改变飞行方向。超声波测距模块流程图如图5所示。

5实验结果与分析

实地飞行测试是检测对控制系统的功能和技术指标的最终手段，也是衡量四轴设计是否成功的重要标志。当前面的各项调试都顺利完成后，就要准备进行试飞实验。试飞前要确保系统各部分工作正常且稳定。确保各个接口连接正确，确保各部件安装牢固，确保电池电量充足。一切准备就绪，即可按以下步骤进行试飞。

5.1直线飞行测试

一，将四轴飞行器放在水平地面上，打开电源开关；二，将飞行高度设定在1.5m；三，将飞行距离设置为50m；四，按下按键，一键式起飞完成飞行任务。由实际情况可看出该设计实现了平稳起飞，定高等功能。在微风的干扰下机体晃动能够自动调整姿态，确保平稳飞行，且系统响应快。

5.2 矩形飞行测试

一，将四轴飞行器放在水平地面上，打开电源开关；二，将飞行高度设定在80cm；三，将飞行轨迹设定为延逆时针边长为5m的矩形；四，按下按键，一键式起飞完成飞行任务。由实际情况可看出该设计实现了循迹飞行，定高等功能。在微风的干扰下机体晃动能够自动调整姿态，确保平稳飞行，且系统响应快。

6 结束语

与以往的四旋翼飞行器相比，本文设计的四旋翼飞行器最大的优点是能够实现自主飞行，其飞行高度在0-100m以内，并利用超声波模块可在3m内定高，定高可精确到厘米级。由于现阶段能力及资源有限，本设计还有很多需要改进的地方，例如：可以添加电子罗盘，将偏航角引入到导航计算中，从而使飞行器的飞行路线更加精确与稳定；同时可以加入无线模块，使此系统分为机载控制与地面控制两块，便于数据的采集与飞行器的实时监控。

参考文献：

[1] WANG Shao-hua ， YANG Ying. Quadrotor aircraft attitude estimation and control based on Kalman filter[J]. Control Theory & Applications， 2013， 30（9）：1109-1115.

[2] Seong Hwang， Seung Yong Min， Choong Hee Lee， Seung Jo KiM. Development Of A Four-rotor Cyclocopter[J]. Journal of Aircraft， 2008， 45（6）：1-6.

[3] 张帆， 曹喜滨， 邹经湘. 一种新的全角度四元数与欧拉角的转换算法[J]. 南京理工大学学报， 2002， 26（4）： 3-8.

[4] 王学斌， 徐建宏， 张章. 卡尔曼滤波器参数分析与应用方法研究[J]. 北京： 北京北方计算中心， 2012， 29（6）： 212-215.

[5] 施文明， 徐彬， 陈利敏. 捷联式航姿系统中四元数算法Kalman滤波器的实现研究[J]. 自动化技术应用， 2005， 24（11）： 6-12.

[6] 李俊， 李运堂. 四旋翼飞行器的动力学建模及PID控制[J]. 辽宁工程技术大学学报， 2012， 31（1）： 4-7.

[7] Michael McRoberts. Arduino从基础到实践[M]. 杨继志， 郭敬，译.北京：电子工业出版社， 2013： 1-447.

[8] 程晨. Arduino开发实战指南：AVR篇[M]. 北京： 机械工业出版社， 2012： 1-321.

[9] 刘火良， 杨森.STM32库开发实战指南[M].北京： 机械工业出版社，2013：1-496.

[10] 胡琴， 施雅， 陈嘉聪，等. 四旋翼自主飞行器系统[J]. 科技致富向导， 2013， 35（24）：235-237.