小型无人机飞行姿态串级控制系统

程方晓， 王海彪， 刘 璐

(长春工业大学 电气与电子工程学院， 吉林 长春 130012)

小型无人机飞行姿态串级控制系统

程方晓， 王海彪， 刘 璐

(长春工业大学 电气与电子工程学院， 吉林 长春 130012)

采用四元数互补滤波算法对IMU单元等多传感器采集到的数据进行解算融合，串级PID为小型无人机飞行姿态系统的控制算法。设计了实时监测子系统,实验结果表明,基于STM32的串级PID控制算法可以实现飞行姿态的有效控制。

无人机； 姿态； STM32； 监控

0 引 言

近几年，随着科学技术的快速发展，小型无人机在军用、民用以及日常抢险救灾中起到了至关重要的作用,正逐渐成为人们生活中不可或缺的一部分。文中针对无人机涉及的空气动力学原理、自动控制原理以及数据融合算法等具有非线性多变量的特点,采用捷联惯性测量单元(IMU)进行姿态检测，并用多传感器数据融合得到精确的姿态角[-2]。通过比较获取的当前姿态和给定姿态得到偏差，经串级PID调节PWM控制信号实现对电机转速的控制[3]，从而达到姿态调节的目的，通过无线传输与上位机进行通信，达到动态调整和实时监测的效果。

1 系统工作原理

无人机为十字形结构[4]，4个电机分别驱动4个安装于机体顶点的螺旋桨，1、3为一组，2、4为一组,分别产生顺时针方向扭矩和逆时针方向扭矩，顺逆方向反转矩相互抵消通过控制电机转速达到控制无人机飞行控制，如图1所示。

图1 四旋翼飞行器结构示意图

飞行控制由姿态控制和位置控制两部分组成，文中主要研究姿态控制。姿态控制就是在前期姿态解算的基础上对无人机实施有效的飞行控制，从而达到想要的控制效果。这就要求算法能够连续做决策，不断选择最佳的飞行控制，算法的评价应由其做出选择的长期质量来判定。控制过程以PID反馈控制器为主。飞行器主要通过改变四个旋翼的角速度来控制四旋翼无人机。每个旋翼产生一个推力(F1、F2、F3、F4)和一个力矩，其共同作用产生的合力构成四旋翼无人机的主推力、偏航力矩、俯仰力矩和滚转力矩。文中以四旋翼无人机为控制对象，其动力学模型可描述为将控制对象看作有一个力和三个力矩的三维刚体。四旋翼无人机在小角度变化条件下的动力学模型近似如下：

2 系统总体设计

无人机系统由主控芯片、电源管理、IMU单元、动力和无线通信5部分组成，系统结构如图2示。

图2 系统结构图

控制部分读取和处理传感器数据，同时接收无线控制信息对无人机进行控制；电源为各部分提供电压；IMU单元由加速度计、陀螺仪、磁阻传感器和气压传感器对无人机飞行姿态进行采集,并用四元数互补滤波法对无人机的飞行高度和姿态角进行计算[5]，结合串级PID控制算法输出合适的控制信号；通过驱动动力部分的电机已达到无人机的姿态调整；通过无线模块与上位机通信实现飞行数据实时更新和飞行姿态的动态显示。

3 系统硬件设计

3.1 主控芯片

主控部分采用主频为72 MHz基于ARM Cortex-M3内核32位控制器STM32F103VCTE[6]，该控制器功耗低，运算速度快，具有多路PWM信号输出通道，同时兼具I2C、SPI、USART等通信方式，方便数据采集、控制无人机和与上位机之间的通信等。

3.2 电源设计

采用锂电池供电，使用TI公司的TPS73633稳压芯片来产生稳定系统所需要的电压。

3.3 数据采集单元

传感器对无人机的飞行姿态进行数据采集，通过采集到的加速度、角速度、飞行高度和磁阻进行姿态融合算法，从而控制飞行。考虑到飞行系统的体积和重量要求，系统全部采用高集成度的MEMS器件。姿态传感器采用nvensense公司的MPU-9250，该传感器是融合了三轴加速度、三轴陀螺仪和三轴磁阻传感器于一体的九轴运动传感器[7]。此芯片也可自行配置量程，加速度可设置为±2、±4、±8、±16 g，角速度可设置为±250、±500、±1 000、±2 000°/s，磁阻满量程为±4 800 μT。通信方式采用最高达到400 kHz的I2C通信。无人机的高度信息则采用MS5611MEMS气压传感器提供，该传感器响应时间为1 ms、测量精度可达10 cm，可通过I2C与系统通信。

3.4 电机选取

作为无人机的动力来源，该部分选用4个电子调速器驱动的新西达2212型无刷直流电机，为提高系统稳定性，防止无刷电机产生的冲击电流在STM32的PWM输出与电机驱动间加入数字隔离器，电机由PWM进行调速。

3.5 无线通信部分

无线通信采用SPI通信方式与MCU进行数据传输、工作频段在2.4～2.5 GHz的nRF24L01芯片，该芯片具有功耗低、信号稳定、传输速度快的特点。

3.6 飞行器控制

无人机控制部分主要由姿态控制和位置控制两部分组成，一般情况下姿态控制由无人机系统本身自动完成，而位置控制则由远程遥控控制。姿态控制是无人机控制部分的重点，因为无人机的姿态变化可以导致位置变化。姿态和位置两个环路控制如图3所示。

图3 姿态和位置环路控制

PID控制器凭借其对系统模型精确度要求不高和简单易懂等特点被广泛应用。无人机通过对4个无刷电机转速的调节达到对飞行姿态的调节，飞行器俯仰、滚转、偏航等飞行动作由4个电机不同的转速组合来实现。通过PID控制器在3个姿态控制通道进行控制达到想要的控制功能[8]。由于本系统对动态要求较高，所以采用串级PID控制器(PID控制器的一种改进)，控制结构如图4所示。

图4 控制结构

PID、PD控制器分别为主控制器和副控制器，分别实现对姿态角和角速度的变化趋势进行快速精确调整，使系统具有稳定的飞行姿态控制性能和动态扰动下的控制能力。

在Simulink搭建仿真模型[9]，整定串级PID控制器参数，滚转角和俯仰角的阶跃响应仿真曲线如图5所示。

从图5可以看出，串级PID具有很好的控制能力，能够达到控制要求。

(a) 滚转角阶跃响应曲线 (b) 俯仰角角阶跃响应曲线

4 系统软件设计

系统软件设计流程如图6所示。

图6 系统软件流程

5 姿态解算

姿态角测量和解算是控制系统的重要部分，无人机飞行姿态的实时性与稳定性直接由解算速度和精度决定。由于陀螺仪存在温漂特性和长时间积分后累积的积分误差，使得数据精度在短时间内较高；飞行过程中产生影响加速度计的震动,磁阻传感器易受外部磁场干扰，二者的静态误差不随时间变化，应采用长时间姿态角采集,而不宜采用短期测量。所以采用四元数互补滤波算法对传感器采集的数据进行融合，提高飞行控制精度[10]。

首先通过四元数互补滤波法将传感器检测到的数据进行融合，然后利用向量叉积法求得姿态误差，最后由一阶龙格库塔法更新数据，得到最终精确的飞行器姿态值。由于欧拉角参数表示法物理意义明确、简单明了，而四元数法计算简单方便，所以采用欧拉角和四元数法结合使用表达无人机的飞行姿态。由陀螺仪得到欧拉角更新四元数矩阵为：

对陀螺仪积分获取姿态角存在的误差，由加速度计和电子罗盘得到偏差E1和E2的和对陀螺仪x、y、z三轴的误差E(ex,ey,ez)进行校正[11]，其叉积求解如下：

由E和式(4)的叉积误差对陀螺仪的三轴角速度值进行PI修正:

其中式(5)为积分修正，式(6)为比例积分修正。

四元数法积分校正原理如图7所示。

图7 四元数法积分校正原理

其中,a为加速度计实测矢量，m为数字罗盘实测矢量，a1为四元数转换后的加速度向量，m1为四元数转换后磁场向量，E为实测矢量与四元数转换矢量的误差值，T为误差校正更新时间常数。

由一阶龙格库塔法对四元数互补法积分校正后的精确角速度值姿态进行更新如下:

为方便下次更新，将更新后四元数规范化表示如下:

四元数互补滤波法获得精确的姿态值实现了无人机的稳定控制，将四元数转换为欧拉角，用欧拉角表示无人机姿态，如下：

6 实验结果与分析

上位机显示的飞行姿态如图8所示(左侧显示PWM控制量，右侧显示姿态角和更新的数据)。

图8 四旋翼姿态显示界面

使用Matlab根据悬停实验测得的数据绘制的无人机在三维空间悬停位置如图9所示。

图9 四旋翼悬停坐标位置图

采样周期和样本分别取20 ms和100个。其中，X,Y,Z分别为左右、前后和高低。分析实验结果可知,姿态角控制精度为3°到5°，悬停位置的最大误差不大于5 cm。

7 结 语

基于STM32主控芯片设计了无人机飞行控制系统。由多传感器进行飞行姿态数据采集，然后控制器对采集到的数据进行处理，由四元数互补滤波法进行数据融合得到稳定精确的姿态角，通过串级PID控制算法进行电机调整量计算，通过微处理器输出的PWM实现对电机转速的控制，达到调整无人机飞行姿态的目的。悬停实验和横滚角、俯仰角的阶跃响应仿真曲线表明,该系统飞行姿态稳定，达到了对无人机飞行姿态控制的要求和效果。

[1] 刘方滔.多传感器融合的四旋翼飞行器关键技术研究[D].天津:天津科技大学，2011.

[2] 代君，任淑红，王晓璐.小型无人机姿态航向参考系统信息融合算法[J].长春工业大学学报，2016，37(1)：52-55.

[3] 李秀英，刘彦博.基于PWM的四旋翼飞行器控制方法[J].吉林大学学报，2011，29:464-472.

[4] 卢秀和，李建波.基于STM32的四旋翼姿态控制系统[J].计算机测量与控制，2014,22(3):761-763，772.

[5] 杜杉杉，吴昊，张继文，等.一种面向AHRS的改进补滤波融合算法[J].国外电子测量技术，2015(3)：13-18.

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

[7] 郑健.基于9轴传感器的姿态参考系统研究与实现[D].成都:电子科技大学，2013.

[8] 冀明，卢京潮.基于Cortex-M4的四旋翼飞行控制系统设计[J].计算机测量与控制，2013,21(7):1816-1817，1821.

[9] 潘海珠.四旋翼无人机自适应导航控制[J].计算机仿真，2012，29(5):98-102.

[10] 张浩，任芊.四旋翼飞行器航姿测量系统的数据融合方法[J].兵工自动化，2013，32(1)：28-31.

[11] 廖懿华，张铁民，廖贻泳.基于模糊-比例积分偏差修正的多旋翼飞行器姿态测算系统[J]．农业工程学报，2014,20：19-27.

ThesystemdesignofsmallunmannedaerialvehicleflightattitudecontrolbasedonSTM32

CHENG Fangxiao, WANG Haibiao, LIU Lu

(School of Electrical & Electronic Engineering, Changchun University of Technology, Changchun 130012， China)

The quaternions complementary filter algorithm is used for data fusion, which are sampled from IMU unit and other sensors. Cascade PID algorithm is applied into the Unmanned Aerial Vehicle (UAV) pose control system with a real-time monitor. Experimental results indicate that the cascade PID algorithm based on STM32 is effective for the UAV pose control.

Unmanned Aerial Vehicle (UAV)； pose； STM32； monitor.

2017-07-28

吉林省科技厅基金资助项目(20170204038SF)

程方晓(1969-)，女，汉族，吉林长春人，长春工业大学副教授，博士，主要从事测控技术与智能系统方向研究,E-mail:chengfangxiao@ccut.edu.cn.

10.15923/j.cnki.cn22-1382/t.2017.5.08

TP 273

A

1674-1374(2017)05-0453-06