四旋翼无人机飞行控制系统设计

山西中北大学机电工程学院　郭鲁奇　顾强　赵意鹏

四旋翼无人机飞行控制系统设计

山西中北大学机电工程学院郭鲁奇顾强赵意鹏

本文设计了以STM32F103C8为主控芯片的控制系统，该系统通过MPU6050整合性六轴运动处理组件采集数据并进行滤波处理，再利用四元数姿态解算算法得到姿态角，使用HC-SR04超声波来测量飞行器高度，通过PID控制实现飞行器姿态和高度的控制。测试结果表明，该系统能使飞行器完成起飞、悬停、降落一系列动作，且稳定可靠。

飞行控制系统；MPU6050；HC-SR04；四元数姿态解算；PID控制

引言

四旋翼无人机是一种依靠4个呈十字形或X形分布的电动螺旋桨提供动力的飞行器，特别适合在近地面环境中执行监视、侦察等任务，具有广阔的军事和民用前景。因此，设计一套可靠性高的飞行控制系统就显得尤为重要。

1　控制系统组成及工作原理

本系统主要由电源电路、主控芯片STM32F103C8、无线射频模块nRF2401、超声波测距模块HC-SR04、运动处理组件MPU-6050及电机等部分组成，其结构示意图如图1所示。

图1　控制系统结构示意图

本系统以STM32F103C8为主控芯片，通过无线射频模块nRF2401向主控芯片发送起飞、悬停及降落等命令，当其收到命令后，主控芯片通过提取HC-SR04和MPU-6050的信号，利用四元数姿态解算法得出飞行器当前的状态，再通过PID算法，利用PWM调节电机的转速，直到达到所要求的状态［1］。

2　控制系统硬件设计

2.1主控芯片STM32F103C8

STM32F103C8是基于32位ARM Cotex-M3内核，最高主频率72MHZ，具有高达90DMips、1.25DMips/MHz的处理能力，Cotex-M3内核采用哈佛结构及流水线技术，很大限度上提高了控制器的运算能力，能够满足该控制系统复杂而快速的计算要求，同时该芯片功耗小、性价比高，能够满足本系统的设计要求。

2.2无线通信模块nRF2401

该模块是集收发一体的无线通信模块，工作在2.4～2.5GHz频段，具有高达2M的数据传输速率，在功率放大器的配合下，可以达到100m的传输距离，满足设计要求［2］。

2.3超声波测距模块HC-SR04

HC-SR04是可测量2cm～400cm范围的非接触式测距模块，包括超声波发射器、接收器及控制电路。工作时，通过给TRIG引脚大于10us的高电平，触发模块开始工作，自动发送8个40KHz的方波，并检测是否有返回信号，若有信号返回，则通过ECHO引脚发出一个高电平，高电平引脚持续的时间就是超声波信号从发射到返回接受的时间，则测量距离=（时间*声速）/2。

2.4运动检测模块MPU-6050

MPU-6050是集3轴MEMS陀螺仪、3轴MEMS加速度计及1个可扩展的数字运动处理器的运动控制传感器，具有I2C和SPI两种接口形式，可以满足多方面的需要［3］，其连接电路图如图2所示。

图2　MPU-6050连接电路图

3　控制系统软件设计

3.1主程序设计

通过对飞行器进行起飞、悬停及降落三个动作的飞行来验证所设计控制系统的可靠性。在给系统上电后，首先进行主

图3　主程序流程图

3.2四元数姿态解算

飞行器的高度通过超声波测距模块HC-SR04来获取，姿态由MPU-6050通过I2C总线传输，用姿态角来表示。姿态角是载体坐标系b相对于导航坐标系n的状态。假设飞行器上一点在载体坐标系b中的坐标是（Xb，Yb，Zb），在导航坐标系n中的坐标是（Xn，Yn，Zn），则有姿态变换矩阵Cnb满足：

四元数是由一个实数和三个虚数i j、k单位组成的超复数［5］。如果将坐标系b、n的三维矢量扩展成四维矢量，可以表示为：

结合式（1）和式（3）以及四元数的乘法运算法则可得到如下的转换关系：Cnb=

根据坐标变化理论，导航坐标系B依次旋转φ、θ、γ角度后可以得到载体坐标系A，从而得到方向余弦矩阵［6］：

那么综合（5）和（6）可得：

［7］得如下关系式：

其中ωABA为四元数形式的向量，表示载体相对于导航坐标系转动的角速度在载体坐标系上的投影，Qc是CBA对应的四元数。方程即为四元数微分方程，也可表示为：

求解出该方程可得到四元数Qc。求出实时四元数之后结合式（4）（5）（6）（7）即可得到姿态角。

3.3PID控制算法设计

PID控制是根据系统的误差，利用比例、积分和微分计算出控制量进行反馈控制的算法。其控制框图如图4所示。

图4　PID控制框图

PID控制器的完整公式［8］是：

其中Kp、Ti、Td分别是比例系数、积分系数和微分系数，为满足数字控制系统的要求还需将其离散化：

其中k是采样次数，T是采样周期，进一步可以得到增量式PID控制：

其中，Ki=Kp（T/Ti），Kd=Kp（Td/T）从而可根据每次采样结束时ek、ek-1、ek-2的值可以计算出控制器的输出量。

本系统高度控制采用双闭环控制，内环控制的采样周期是5ms，外环是10ms，外环直接控制高度，为了避免外力影响，内环控制Z轴加速度，通过给定的重力加速度与Z轴加速度比较，输出控制量调节油门变化。高度PID控制如图5所示。

图5　高度PID控制

本系统姿态控制采用双闭环控制，内环控制的采样周期是2ms，外环是5ms，使得角速度控制较为灵敏，且抗干扰能力强，姿态PID控制如图6所示。

图6　姿态PID控制

4　系统测试

给系统上电后，先轻轻晃动飞行器，在Watch Windows窗口中，可以看到MPU-6050传回的数据，说明正常工作，如图7所示。接着给飞行器起飞命令，飞行器起飞，到达大约3m处，悬停10s，降落，完成预定的飞行轨迹。

5　结论

本系统以四元数姿态解算为载体，利用PID控制算法来控制飞行器的飞行，具有较高的精度和可靠性，对飞行器控制系统的后续改进具有一定的参考价值。

参考文献：

［1］谢义建，陈跃东，舒圣焱.基于STM32的四旋翼飞行器的设计与实现［J］.四川理工学院学报（自然科学版），2014，27（3）：42-45.

［2］陈志旺.STM32嵌入式微控制器快速上手［M］.厦门大学出版社，2014：57-60.

［3］王福超.四旋翼无人飞行器控制系统设计与实现［D］.哈尔滨工程大学，2013：8-9.

［4］郑震璇.基于PIC单片机的汽车空调控制器设计［J］.机电技术，2009，2：37-40.

［5］孙贵斌，杨福清，方遒等.基于模糊PD控制的客车前大灯随动转向研究［J］.机电技术，2015，8：121-126.

［6］史莹晶.航空飞行器控制与仿真［M］.电子科技大学出版社，2011：28-31.

［7］李尧.四旋翼飞行器控制系统设计［D］.大连理工大学，2013：37-38.

［8］胡寿松.自动控制原理［M］.科学出版社，2007：121-134.

郭鲁奇，山东菏泽人，在读研究生，研究方向：机电系统设计与分析。控芯片GPIO、无线通信模块、电机及测量模块等的初始化，然后发送起飞命令，使飞行器飞行到3m高处悬停大约10s，最后降落地面［4］，其程序流程图如图3所示。