基于STM32的四旋翼自主投靶飞行器

巩梦岩，孟 青，程思强，李 梦



基于STM32的四旋翼自主投靶飞行器

巩梦岩，孟 青，程思强，李 梦

（中北大学，山西 朔州 036000）

本项目在各模块相互协作下，四旋翼飞行器可以按照规定航向以某一高度稳定飞行，精确降落，并进行拾取，投放铁片等各项任务。该飞行器由主控制板、飞行控制板、定点模块、电磁铁模块、超声波模块、电机模块等部分组成。采用意法半导体集团的STM32F103ZET6芯片作为主控制板，飞行控制板MCU为STM32F103RCT6，通过MPU6050（加速度计，陀螺仪），IST8310（磁力计），HC-SR04（超声波模块）获得姿态与高度数据进行PID运算后实现定高飞行。定点和循迹模块使用Openmv摄像头对黑点位置进行采样，经过数据处理后控制飞行器可以保持在目标黑点正上方从而完成定点和循迹任务。磁铁模块在MCU的控制下可以准确的完成拾取、投放铁片的任务。

STM32单片机；飞行姿态；自主悬停；PID算法

0 引言

四旋翼飞行器通过调节占空比来调节四个电机转速，实现升力的变化，从而控制飞行器进行横滚，俯仰来改变姿态和位置。近年来，无人机的研究和应用[1]广泛受到各个方面的重视。四旋翼飞行器作为无人机的一种，能够垂直起降、空中悬停、可适用于各种飞行场景，具有灵活度高，安全性好的特点，适用于警务监控、航拍摄影、火场指挥、交通管理、地质灾害调查、管线巡航等领域实现空中时时移动监控。因此，四旋翼飞行器的研究非常符合当今社会的发展趋势。

1 系统理论分析与计算

1.1 飞行器的模型建立

飞行器运动过程可以分解成机体绕三个轴运动来描述：当飞行器前后运动时，飞行器即做俯仰运动，飞行器在-平面进行绕轴运动，产生的角度即俯仰角（pitch）；当飞行器发生转向运动时，飞行器将产生偏航，飞行器在-平面进行绕轴运动，产生的角度即航向角（yaw）；当让飞行器侧身移动时，飞行器将要做横滚运动，飞行器在-平面进行绕轴转动，产生的角度即横滚角（roll）。如下图。

图1 机体坐标图

1.2 滤波器的设计

针对传感器内部低通后输出数据具有滞后性，在此飞控中对传感器采集时不设置内部低通，比较原始信号在四旋翼悬停油门附近与静止时的频谱图，确定因震动产生的传感器噪声截止频率，设计二阶巴特沃斯滤波器[2]对噪声信号进行处理。同时针对姿态解算与惯性导航主导传感器不同，本项目中同一量在不同使用条件下的截止频率也不一样。

1.3 姿态解算

姿态解算[3]是四旋翼飞行器飞行的基础，需要结合数据融合以及数据滤波，以及空间旋转描述，从而确定飞行器的绝对或相对旋转（姿态）。利用互补滤波、梯度下降、EKF算法完成四元数的更新，经欧拉角转换得出姿态的俯仰和横滚角。此飞控中，避免磁力计干扰引起水平姿态角解算错误，磁力计不参与水平姿态矫正，直接采用一阶互补滤波算法获取飞行器的偏航角。

1.4 惯性导航

根据加速度计比力模型，将三轴加速度计原始数字量，通过在载体系统到导航系下的旋转矩阵归一化处理后，得到导航坐标系下比力加速度，或去重量加速度后，得到用以导航的运动加速度，本项目前期对飞控采用的基于经典回路反馈法的三阶互补方案原理进行了论证与设计实现，接着对竖直位置、速度采用单观测量的卡尔曼滤波进行惯导融合，接着针对气压计观测传感器滞后性，导致快速运动时，惯导收敛慢问题，提出了一种带延时修正的惯性融合算法。目前水平方向惯导融合采用的是双观测量带延时修正的卡尔曼滤波惯导算法。

1.5 PID控制

为了达到较好的飞行器系统控制效果，该项目采用了串级PID[4-6]。PID的流程通过误差信号控制被控量，这里我们规定（在t时刻）：

1. 输入量（期望值）为rin(t)；

2. 当前量（实际值）为rout(t)；

3. 偏差量（期望值减去实际值）为e(t)=rin(t)-rout(t)；

PID的控制规律为：

在只有角度单环的控制下，若遇到外力或磁场干扰下，系统很难稳定运行。因此，加入角速度作为内环，由陀螺仪采集数据输出的角速度值一般不受外界影响，抗干扰能力强。所以四轴飞行器采用双闭环PID控制，角度作为外环，角速度作为内环，进行姿态PID控制；当定高时，高度作为外环，轴加速度作为内环，进行高度PID控制。其中，PID输出为油门值，控制电机转速，使空间三轴欧拉角和高度发生变化。

1.6 飞行高度控制

采用超声波测距模块，测量距离2 cm-120 cm。利用系统板向模块发送触发模块，通过测量echo脚返回的信号的高电平部分时长计算测量到的距离。原理图如图2所示。

通过Echo端输出一高电平，可根据这一高电平的持续时间来计算距离值。即距离值为声速与高电平时间之积/2，通过得到的高度后进行计算可以得到油门调节值对初始设定油门进行调节，使得电机的转速所形成的升力恰好可以与重力平衡，这样就可以使飞机保持在某一高度。

1.7 飞行路径控制

飞行路径的控制[7]主要是根据Openmv探测到的坐标值，计算路面黑线的中心是否在飞行器的正下方，主控制板进行PID运算后输出PWM波经过飞控板处理后控制电机使飞行器进行横滚，使得飞行器沿着黑线方向前进。

图2 超声波原理图

1.8 薄铁片拾取与投放控制

根据电磁铁的通电具有磁性，断电磁性消失的原理，从A起飞时我们让单片机控制电磁铁通电，让飞行器吸取薄铁片飞向B区，到达B区后让电磁铁断电，从而投下薄铁片，让其落到B区。

2 电路与程序设计

2.1 系统整体框架设计

按键，摄像头模块，超声波模块，电磁铁模块均与STM32主控制板通过串口相连接，经过主控制板处理数据后输出PWM波由飞控板处理后控制电机转速来使飞行器做出相应的飞行姿态。

图3 整体流程图

2.2 系统整体电路图

图4 整体电路图

2.3 程序设计

程序流程图如下所示：

图5 程序流程图

主要软件设计如图6所示。

图6 软件设计图

3 测试方案与测试结果

3.1 测试方案

（1）硬件测试

检查飞行器各部分的连接，卸掉飞行器的机桨，运行程序，检查电调声音，感受各个电机的转速，测试期间将各部分数据发送至地面站，通过观察数据和电路连接检查电路各硬件部分状况。

（2）软件仿真测试

使用ST-Link线上调试飞控板，RL78G13仿真板线上调试主控制板，观察软件运行是否流畅。

（3）硬件软件联调

将飞行器放在圆形区域，让它自主飞行，观察飞行器的飞行高度与飞行方向和时间，若飞行器不能按照预定方案飞行就逐项调试程序的PID参数和pitch，roll,throttle,yaw等参数，再进行调试。

3.2 测试条件与仪器

测试条件：多次检查，硬件电路必须和设计原理图完全一致。检查焊点，确保无虚焊。电机转速平稳，转向正常，电调声音正常，不会有突变的情况发生。数据收发正常，无数据乱跳的情况。上桨后，检查桨的正反，保证安装正确。上桨飞行必须在安全防护网里。

测试仪器：学生电源，示波器，数字式万用表。

3.3 测试结果及分析

在早期的测量过程中，硬件软件都出现过不少问题，很多时候飞行器会出现向一个方向偏或自旋，而且遥控器需要很大的调节力度才能调节过来，有的时候甚至直接炸机。这些情况的出现，在硬件上很多情况下是飞行器的重心不在中心；或是在某次撞机或长时间实验后，电机电调等偏离正常参数性能；传感器接口接触不良导致数据混乱。软件上，很多情况下是参数没有调好；或者是中断程序的相互冲突，数据处理周期过大等。当然还有编程算法方面的错误。在多次测量后，最后几次稳定的数据如表1所示。

表1 测试结果表

Tab.1 Test results table

4 结束语

该四旋翼飞行系统以单片机控制为核心，能够实时的采集飞行器当前的姿态信息并融合解算出当前的欧拉角。采用先进的串级PID控制方法同时结合超声波以及摄像头模块反馈回来的信息对当前姿态角进行控制，从而完成俯仰，横滚、偏航等各种动作以实现按照规定航路自主飞行，并通过电磁铁吸引铁片完成投靶任务。

[1] 宋洪达, 郝桂丽. 四旋翼飞行器的发展与应用[J]. 科技风, 2018, 31(1): 134.

[2] 王赛男, 邢冬梅. 基于MATLAB 的小波阈值滤波与应用[J]. 软件, 2015, 36(10): 102-104.

[3] 曹延超. 基于STM32 的四旋翼飞行器姿态测量系统设计[J]. 软件, 2015, 36(1): 104-109.

[4] 靳亚磊, 李虹, 李昕涛. 四旋翼飞行器串级PID控制设计与实现[J]. 太原科技大学学报, 2018, 39(3): 177-183.

[5] 范云飞, 任小洪, 袁文林. 基于并联PID 的四旋翼飞行控制策略设计[J]. 软件, 2015, 36(4): 37-39.

[6] 王贞琪, 马洁. 四旋翼几种控制方法比较[J]. 北京信息科技大学学报(自然科学版), 2016, 31(2): 35-39.

[7] 王忠生, 闫超. 四旋翼飞行器的悬停控制问题探讨[J]. 赤峰学院学报(自然科学版), 2017, 38(8): 12-13.

Four Rotor Autonomous Target Vehicle Based on STM32

GONG Meng-yan, MENG Qing, CHENG Si-qiang, LI Meng

(North University of China, Shuozhou 03600)

This project under each module mutual cooperation, the Four-rotor aircraft may according to the stipulation heading fixed flight, precise landing, and picks up, throws the iron sheet and so on each task. The aircraft is composed of main control board, row control board, fixed-point module, electromagnet module, ultrasonic module, motor module and so on. The STM32F103ZET6 chip of STMicroelectronics is used as the main control board, the flight control board MCU is STM32F103RCT6, and the attitude and altitude data are obtained by MPU6050 (accelerometer, gyroscope), IST8310 (magnetometer), HC-SRO4 (ultrasonic module) to realize the fixed altitude flight after PID operation. Fixed-point and tracking module uses the Openmv to sample the position of the black spot. After data processing, the control aircraft can keep above the target black spot to complete the fixed-point and tracking tasks. Under the control of MCU, the magnet module can complete the task of picking up and throwing iron sheets.

STM32 microcontroller; Flight attitude; Autonomous hovering; PID algorithm

TP

J

10.3969/j.issn.1003-6970.2018.11.026

巩梦岩，男，本科生，主要研究方向：物联网工程；孟青，女，博士研究生，主要研究方向：电路与系统；程思强，男，本科生，主要研究方向：物联网工程；李梦，女，本科生，主要研究方向：物联网工程。

巩梦岩，孟青，程思强，等. 基于STM32的四旋翼自主投靶飞行器[J]. 软件，2018，39（11）：116-119