基于STM32的四旋翼飞行姿态串级控制的设计与实现

四旋翼飞行器与传统的单桨直升机相比，其飞行原理简单，结构紧凑，单位体积所提供的升力大，且可以扭矩自平衡而不需要反扭桨。四旋翼飞行器只通过改变螺旋桨的速度来实现各种动作，是一种六自由度的垂直起降机，非常适合在静态和准静态条件下飞行，近几年在军事和民用领域广泛应用。然而四旋翼飞行器是个具有4个输入量，6个输出量的欠驱动系统，控制器的设计要求高。

姿态解算是姿态参考系统的关键技术，算法的优劣直接决定了解算效率和系统的精度。目前姿态描述的主要方式有：欧拉角、方向余弦、四元素法。欧拉角物理意义明确，但存在“奇点”。方向余弦计算没有“奇点”，但三角函数运算量大，不适合实时运算。四元素法没有“奇点”，而且运算为一般代数运算，运算量小，方法简单，易于操作。采用四元素法进行姿态解算是理想的选择。

1、四旋翼飞行器工作原理

四旋翼飞行器在结构布局上有十字形、X形和H形。本文主要讨论X形布局，如图1所示。4个电机分别安装在对称结构的X形支架的4个顶点，电机1和3逆时针旋转，电机2和4顺时针旋转，当飞行器平衡飞行时，陀螺效应和空气动力扭矩效应均被抵消。四旋翼飞行器在空间中有6个自由度（分别沿3个坐标轴作平移和旋转动作），可通过调节4个电机的转速来实现控制。基本运动状态：垂直运动、俯仰运动、滚转运动、偏航运动、前后运动、侧向运动。

图1  四旋翼飞行器示意图

垂直运动：同相改变四个电机的输出功率，使总的拉力改变，便实现飞行器沿z轴的垂直运动。当升力等于自重时，飞行器便保持悬停状态。

俯仰运动和滚转运动：电机1、4的转速上升，电机2、3的转速下降。产生的不平衡力矩使机身绕x轴旋转，实现飞行器的俯仰运动。横滚运动与俯仰运动的原理相同。

偏航运动：四旋翼偏航运动可以借助旋翼产生的反扭矩来实现。四个旋翼对角线上的两个旋翼转动方向相同，当两条对角线上的电机转速不完全相同时，不平衡的反扭矩引起四旋翼转动，实现偏航运动。

前后运动和倾向运动：为实现水平面内运动，须在水平面内对飞行器施加一定的力。使飞行器做横滚或俯仰。飞行器发生一定程度的倾斜，从而使旋翼拉力产生水平分量，实现飞行器的水平运动。

2、系统硬件设计

飞行器控制器通过处理微型MEMS惯性器件和三维地磁传感器采集的数据，计算飞行器的姿态角，并根据飞行指令，结合相应的控制律给出适当的控制信号，控制飞行器姿态和位置。

本文采用模块化设计，四旋翼飞行控制系统主要由主控模块、数据采集模块、电机驱动模块等组成。采用STM32芯片作为主控芯片。系统硬件结构如图2所示。

图2  飞行控制器硬件结构图

2.1、主控模块

本文采用基于AＲMCortexM3内核的STM32F103VCTE微控制器为主控芯片。STM32微控制器具有多路信号采集通道和多路PWM输出通道，方便接收传感器数据和对电机进行控制，72MHz的主频满足系统对于运算速度的要求，且功耗低。

2.2、数据采集模块图

采用拥有3轴加速度计和3轴陀螺仪的组合传感器MPU-6050来测量加速度和角速度，3轴地磁传感器HMC5883L测量地磁场，气压计MS5611测量气压。

MPU-6050集成了3轴MEMS陀螺仪，3轴MEMS加速度计。陀螺仪可测范围和加速度计可测范围可根据实际应用调整。与设备寄存器之间的通信采用400kHz的I2C接口。

HMC5883L是一种带有数字接口的弱磁传感器芯片，测量范围从毫高斯到8高斯（Gauss），罗盘精度在1°～2°，以I2C总线接口与主控芯片进行通信。MS5611气压传感器是一款高分辨率气压传感器，分辨率可达到10cm。拥有I2C总线接口与微控制器连接。

2.3、电机驱动模块

采用4个ST2210无刷直流电机并用ST20A电调对电机进行驱动。STM32通过配置GPIO接口输出PPM信号到电调控制电机转速。ST20A电调接受20～400Hz，脉宽1～2ms的PWM信号，电平支持3.3V和5V。0%～100%的油门由PPM信号1～2ms的脉宽表示，即0%油门对应1ms脉宽，100%油门对应2ms脉宽，以达到调节电机转速的目的。

3、系统软件设计

系统软件设计在KeilMDK-AＲM开发环境里编写。MDK-AＲM软件为基于Cortex-M、Cortex-Ｒ4、AＲM7、AＲM9处理器设备提供了一个完整的C/C++开发环境。程序流程如图3所示。

图3  程序流程图

3.1、姿态解算

对姿态角的测量和解算是飞行控制系统中重要的组成部分。姿态解算的速度和准确度直接影响到姿态控制效果。本文采用运算速度快，精度高的四元素互补滤波算法对姿态进行解算，能够达到飞行控制的要求。

姿态角是通过陀螺仪、加速度计和数字罗盘等传感器测量得到的。MEMS陀螺仪具有温度漂移特性，长期积分运算存在累积误差，短期数据精度高。飞行器在飞行过程中机体振动会对加速度计产生影响，同时数字罗盘是一种磁阻传感器，很容易受到外部磁场的干扰，两者不适合短期测量，但其测量误差为静态误差，不随时间的累加而变化，可用以长时间姿态角采集。

四元素互补滤波方法将这几种传感器的优势互补。此方法的主要思想是用加速度计和数字罗盘的测量姿态修正陀螺仪的测量姿态。加速度计和数字罗盘测量出来的数据是地球重力与地球磁场在机体空间坐标上的分量，可认为是实际姿态。陀螺仪测量出来的姿态经实际姿态修正后输出给控制器以计算控制量。欧拉角、四元素和方向余弦矩阵间的关系如式（1）所示。

根据余弦矩阵和欧拉角的定义，地理坐标系的重力向量转到机体坐标系为第三行三个元素，即：

式（2）中，aX，aY，aZ为加速度计测量出的重力向量在各个轴向上的分量，即实际测出来的重力向量;vX，vY，vZ为陀螺积分后的姿态来推算出的重力向量，它们都是机体坐标系上的重力向量。它们之间的误差，就是陀螺积分后的姿态和加速度计测出来的姿态之间的误差。计算前aX，aY，aZ需规范化处理。

陀螺仪各轴转动的角速度经修正后，再以一阶龙格库塔算法进行积分更新四元素，如式（6）所示。

3.3、控制律设计

PID（比例-积分-微分）控制器简单易懂，使用中不需精确的系统模型等先决条件，因而成为应用最为广泛的控制器。

四旋翼飞行器姿态的调节是通过对4个带桨电机的转速调节实现的，4个电机不同的转速组合，能够实现飞行器俯仰、横滚、偏航等不同的飞行动作。通过在3个姿态控制通道分别设计PID控制律，实现所需的控制功能。

针对本系统动态要求较高的特点，采用PID控制器的改进型控制器———串级PID控制器进行，控制结构框图如图4所示。

图4  控制结构框图

主控制器采用PID控制器对姿态角进行精确调整，副控制器采用PD控制器对角速度的变化趋势进行快速调整，提高了系统动态扰动下的控制能力，保证了系统控制性能和飞行姿态的稳定。

在Simulink搭建模型进行仿真，整定串级PID控制器参数，横滚角和俯仰角的阶跃响应仿真曲线如图5（a）、（b）所示。从图5可以看出，串级PID具有很好的控制能力，能够发到控制要求。

图5  横滚角和俯仰角阶跃响应曲线

四旋翼飞行器实物如图6所示。通过实验验证采用四元素融合滤波和串级PID算法控制下，四旋翼飞行器在空中悬停时各个姿态角的稳定性。实验结果表明，本文所设计的飞行控制器能够稳定控制四旋翼飞行器的各个姿态角，在空中悬停的过程中使各个姿态角都在合理的范围内波动，符合控制的要求。

图6   四 旋翼飞行器实物图

4、结语

基于STM32系列设计了四旋翼飞行器飞行控制系统。通过传感器采集飞行器的飞行姿态数据，STM32微控制器进行数据融合滤波和控制律运算，输出控制量到四个电机，最终实现飞行姿态的稳定控制。空中悬停实验结果表明，采用四元素融合滤波算法和串级PID控制器的飞行控制系统具有良好的控制效果，能够实现飞行姿态的稳定控制，达到了设计的目的。同时为以后进一步实现更高飞行任务的研究奠定了基础。