基于STM32的四旋翼飞行器姿态解算的研究

李文鹏，唐海洋

李文鹏，唐海洋

(西安工程大学 电子信息学院，西安 710048)

摘要:分析了四旋翼飞行器的姿态解算原理，提出了一种基于STM32的姿态测量系统。系统由STM32F407微控制器和捷联惯性测量组件(IMU)组成。利用四元数描述姿态进行坐标换算，采用多传感器数据融合方案，通过互补滤波算法进行数据融合，获取精确的姿态角，并完成姿态解算。实验结果表明，采用互补滤波算法有效融合了捷联惯性测量组件的传感器数据，实现了四旋翼飞行器的高精度姿态解算。

关键词:四旋翼飞行器；飞行控制系统；姿态解算；互补滤波器；STM32F407

引言

四旋翼飞行器具有机械结构简单、操纵灵活方便、维修成本低等优点，近年来得益于微机电系统(MEMS)的飞速发展，四旋翼飞行器的姿态测量系统开始向着低成本、小体积、高性能的方向快速发展。四旋翼飞行器飞行过程中，是否可以实时准确获得飞行器的姿态信息决定了飞行器的控制精度和稳定性[1]，所以姿态解算是飞行控制系统的核心。本文设计一种以STM32F407为主控制器的姿态测量系统，系统整合有3轴陀螺仪、3轴加速度计的MPU6050传感器和AK8975(3轴电子罗盘)传感器作为捷联惯性测量组件(Inertial Measurement Unit，IMU)，利用四元数对运动坐标进行描述，通过互补滤波解算出欧拉角。

1四旋翼飞行器的姿态描述

姿态解算属于飞行控制系统的核心部分，通过姿态矩阵可以得到飞行器的姿态和导航参数计算需要的数据，飞行器的姿态和航向体现了载体坐标系与地理坐标系之间的方位关系[2]。目前描述动坐标相对参考坐标系方位关系的方法有欧拉角法、方向余弦矩阵法、四元数法等[3]。

1.1欧拉角法

欧拉角是用来唯一确定定点转动刚体位置的3个一组独立角参量[4]，定义ψ、θ、φ分别为绕Z轴、Y轴、X轴的旋转角度,如图1所示。用Tait-bryan angle分别表示为yaw(偏航角)、pitch(俯仰角)、roll(横滚角),如图2所示。

图1　旋转角度示意图

图2　Tait-bryan angle形式表示的欧拉角

1.2方向余弦矩阵法

载体坐标轴x、y、z分别与参考坐标轴X、Y、Z的方向余弦，可以组成方向余弦矩阵。方向余弦法和欧拉角法没有本质上的区别，都采用欧拉角表示。方向余弦法避免了欧拉角法的“奇异点”现象，没有方程退化问题，可以全姿态工作，但需要求解9个微分方程[6]，由于计算量大、实时性差，无法满足飞行控制要求。

1.3四元数法

四元数是一种超复数。如果把四元数的集合考虑成多维实数空间，四元数就代表着一个四维空间，相对于复数为二维空间[7]。复数是由实数加虚数单位i组成的，其中i2=-1，类似地，四元数是由实数加上三个元素i、j、k组成[8]，而且它们有如下关系：

i2=j2=k2=ijk=-1

通过旋转轴及绕其旋转的角速度可以构造一个四元数：

其中α是绕旋转轴旋转的角度，cos (βx)、cos (βy)、cos (βz)是旋转轴在x、y、z方向上的分量。四元数法只需求解4个一阶微分方程组(比方向余弦矩阵法的计算量小得多)，即可满足飞控系统对实时性的要求。

在姿态解算中，积分和修正采用运算量较少、无奇点误差的四元数法，并用角度比较直观的欧拉角输出控制PID。用欧拉角直接表示四元数为：

2四旋翼飞行器的姿态解算

四旋翼飞行器处于飞行状态时，电子罗盘易受外部磁场干扰[9]，加速度计受机身振动影响会产生测量白噪声，陀螺仪因存在温度漂移误差[10]会产生零点误差和累积误差，因此这几种传感器直接测得的偏航角、俯仰角、横滚角都存在一定的误差[11]，必须采用一定的姿态解算算法通过融合各种传感器的数据信息进行误差补偿，以实现精确的姿态解算[12]。捷联惯导系统中常用的算法有卡尔曼滤波算法和互补滤波算法。

2.1卡尔曼滤波算法的姿态解算原理

卡尔曼滤波为最优化自回归数据处理算法[13]，是一种能够从包含噪声的有限信号中预测出目标真实值的滤波算法，对于解决大部分数据融合方面的问题，它是非常有用的[14]。但卡尔曼滤波算法计算量大，对飞控MCU的运算速度和精度要求都比较高。

2.2互补滤波算法的姿态解算原理

陀螺仪动态响应特性比较好，瞬时测得的角速度精度高，因此常对陀螺仪测得的角速度进行积分来获取三轴姿态角，但由于积分是离散的，长时间会产生累积误差。采用加速度计和电子罗盘测量姿态时，虽然没有累积误差，但噪声比较大，而且在飞行过程中，受机体振动的影响比陀螺仪明显，短时间内的可靠性不高。

该算法相较于传统的卡尔曼滤波算法，具有运算量小、对采样频率要求低等优点[15]，能够满足四旋翼飞行器对姿态测量的要求。

陀螺仪积分所得数据与加速度计、电子罗盘的测量数据进行对比，使用其偏差来修正陀螺仪的输出。滤掉加速度计的高频信号和陀螺仪的低频信号，然后相加得到整个频带的信号。它们在频域上可以实现特性互补，因此采用互补滤波算法融合两者的数据，可以提高测量精度和系统的动态性能。互补滤波算法应用示意图如图3所示。

图3　互补滤波算法应用示意图

由图3可得一阶低通滤波器GL(s)和一阶高通滤波器GH(s)，传递函数为：

其中角度补偿参数K的确定在互补滤波算法中起着至关重要的作用，它决定了该滤波器是否能够较好地结合加速度计的低频精度和陀螺仪的动态性能[16]。K值较大，截止频率较高，数据融合结果主要依靠陀螺仪输出得到，陀螺仪累计误差小，但加速度计噪声大；K值较小， 截止频率较低，数据融合结果主要靠加速度计输出得到，但陀螺仪累计误差大，容易受陀螺仪漂移影响。所以其太大或者太小在四旋翼飞控系统姿态解算中都是不允许的。

则互补滤波算法估计的姿态矩阵为：

其中，C0(s)为加速度计和电子罗盘获取到的姿态矩阵；Cω(s)为陀螺仪数据计算得到的姿态矩阵；C(s)为四旋翼飞行器运动过程中的真实姿态矩阵；μH为C0(s)的高频观测噪声；μL为Cω(s)中的低频误差累积；K为角度补偿参数。

所以，互补滤波算法能够同时滤除低频干扰和高频干扰，能够有效实现多路传感器数据的融合。

2.3互补滤波算法在STM32中的实现

利用互补滤波算法实现姿态解算的程序段如下：

void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az){

float norm;

float vx, vy, vz;float ex, ey, ez;

//把加速度计的三维向量转换成单位向量

norm =invSqrt(ax * ax + ay * ay + az * az);

ax*= norm;

ay*= norm;

az*= norm;

//陀螺仪积分后的姿态推算出重力分量

vx = 2 * (q1 * q3 - q0 * q2);

vy = 2 * (q0 * q1 + q2 * q3);

vz = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3;

//得到误差并消除

ex = (ay * vz - az * vy);

ey = (az * vx - ax * vz);

ez = (ax * vy - ay * vx);

exInt+= ex * Ki;

eyInt+= ey * Ki;

ezInt+= ez * Ki;

gx+= Kp * ex + exInt;

gy+= Kp * ey + eyInt;

gz+= Kp * ez + ezInt;

//四元数微分方程，将陀螺仪与四元数结合起来，为姿态解算的核心算子

q0+= (-q1 * gx - q2 * gy - q3 * gz) * halfT;

q1+= (q0 * gx + q2 * gz - q3 * gy) * halfT;

q2+= (q0 * gy - q1 * gz + q3 * gx) * halfT;

q3+= (q0 * gz + q1 * gy - q2 * gx )* halfT;

//四元数规一化

norm =invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);

q0*= norm;

q1*= norm;

q2*= norm;

q3*= norm;

}

完成姿态解算后，就得到了表示姿态的四元数，一般进一步转换为较为直观的欧拉角。

3姿态解算的试验验证与分析

搭建的四旋翼飞行器的姿态测量平台如图4所示。其中机架采用DJI的F450机架，动力部分采用DJI的2312 960 kV无刷电机、420S电调和9450自紧螺旋桨，基于STM32的姿态测量系统固定于机架的中心位置。MPU6050的角速度测量范围为±250 °/s、±500 °/s、±1000 °/s和±2000 °/s，可程序配置的加速度计测量范围为±2g、±4g、±8g和±16g。

图4　四旋翼飞行器的姿态测量平台

操控四旋翼飞行器尽可能保持定点悬停状态，通过串口读取出传感器采集到的原始数据以及姿态解算后的数据，在上位机软件中使用模拟示波器观测姿态角的变化，并进行数据分析。

飞行器做俯仰运动时，因存在温度漂移，陀螺仪会产生零点误差和累积误差，即采用陀螺仪测量得到的角速度进行积分得到姿态角会产生积分误差，并且随着时间的推移会产生累积误差。同时由于无刷电机引起的机架振动、机体重力分布不均等因素也会导致零点误差增大。四旋翼飞行器在定点悬停状态下经姿态解算后的俯仰角和横滚角图略——编者注，可以看出随着时间的推移，零点误差逐渐趋于平稳。

在实验过程中使飞行器尽可能保持航向，机头朝向地理正北方时，偏航角为0°，顺时针偏转航向，偏航角为正且角度绝对值增大，逆时针偏转航向，偏航角为负且角度的绝对值增大，持续顺时针偏航，偏航角将增大至180°并跳变为-180°，继续偏航，偏航角将由-180°增大至0°，之后以此类推，逆时针偏航同理。飞行器做偏航运动时，电子罗盘经姿态解算后测量得到偏航角图略——编者注。本系统中采用陀螺仪积分值对电子罗盘进行校正，对于偏航角，以电子罗盘采集的数据为主。电子罗盘受外界因素干扰严重，零点误差较大，经过互补滤波后，虽然消除了部分干扰，电子罗盘数据波动减弱，误差稳定保持在±0.8°以内，但由于电子罗盘自身具有不可忽略的零位、灵敏度及非正交误差，因此通过软件算法无法彻底消除零点误差[17]。

结语

本文分析了四旋翼飞行器的姿态解算算法理论、互补滤波数据融合的方法，以及姿态解算在STM32上的实现。在以STM32F407微控制器为控制核心， MPU6050和AK8975姿态传感器模块作为捷联惯性测量组件的四旋翼飞行实验平台上对软件算法进行了验证。结果表明，互补滤波算法较好地融合了姿态测量系统陀螺仪、加速度计和电子罗盘的数据，为四旋翼飞行器稳定飞行提供了可靠的数据，满足了四旋翼飞行器姿态解算的要求。

参考文献

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

[2] 张荣辉,贾宏光,陈涛,等.基于四元数法的捷联式惯性导航系统的姿态解算[J].光学精密工程,2008(10):1963-1970.

[3] 李敏行.非线性控制理论在三轴转台姿态控制中的应用[D].西安:西北工业大学,2001.

[4] 邵涛,李明,姜良银,等.基于LabVIEW的miniAHRS型传感器信号采集研究[J].现代机械,2010(3):33-35.

[5] 贺毅岳,耿国华,周明全,等.基于VTK的三维模型切割研究[J].计算机应用研究,2010(11):4322-4323,4327.

[6] 贺海鹏,阎妍,马良,等.基于IEKF的四旋翼无人机姿态测量方法研究[J].计算机仿真,2015(4):56-60.

[7] 邢燕.四元数及其在图形图像处理中的应用研究[D].合肥:合肥工业大学,2009.

[8] 李晓琴,田垄,孙波.基于复数矩不变性的遥感边缘信息提取[J].遥感信息,2011(4):7-10.

[9] 于春晓.人形机器人的电子罗盘航向估计[D].重庆:重庆大学,2010.

[10] 徐丽娜,邓正隆,张广莹,等.陀螺仪温度试验与建模研究[J].宇航学报,1999(2):100-104.

[11] 沈晓卫,姚敏立,赵鹏.基于互补滤波的动中通天线姿态估计[J].中国惯性技术学报,2011(2):194-197.

[12] 万晓凤,康利平,余运俊,等.基于多传感器数据融合的四旋翼飞行器的姿态解算[J].科技导报,2014(19):31-35.

[13] 王一夫,陈松乔.非加性噪声情形下卡尔曼滤波器的推广[J].计算机工程与应用,2008(4):24-26.

[14] 钱鋆,欧阳红林,范祝霞,等.基于EKF的双Y移30°六相PMSM无速度传感器控制系统研究[J].科技导报,2009(17):65-68.

[15] 施闻明,徐彬,陈利敏.捷联式航姿系统中四元素算法Kalman滤波器的实现研究[J].自动化技术与应用,2005(11):9-11.

[16] 余彦霖,祖家奎,曾国贵.微小型无人直升机姿态信号的互补滤波融合算法[J].直升机技术, 2014(3).

[17] 何川,李智,王勇军.基于STM32的四旋翼飞行器的姿态最优估计研究[J].电子技术应用,2015(12):61-64.

李文鹏(硕士研究生)，主要研究方向为嵌入式系统应用；唐海洋(硕士研究生)，主要研究方向为电力电子与电力传动。

Li Wenpeng,Tang Haiyang

(College of Electronic and Information,Xi’an Polytechnic University,Xi’an 710048,China)

Abstract：The quad-rotor aircraft principle of attitude solution is analyzed,and the attitude determination system is proposed which consists of STM32F407 and inertial measurement unit(IMU).The quaternion is used to do coordinate conversion.The complementary filter data fusion is used to obtain the accurate attitude angle,and then completes the attitude calculation.The experiment results show that the filtering algorithm effectively combines the complementary sensor data of the inertial measurement unit to achieve high precision attitude calculation.

Key words:quad-rotor aircraft;flight control system;attitude solution;complementary filter;STM32F407

中图分类号:TP271

文献标识码:A

收稿日期：(责任编辑：薛士然2016-01-14)