四旋翼姿态检测及数据处理系统优化设计

江 杰,刘 娜,杜永兴

(内蒙古科技大学信息工程学院,内蒙古包头 014010)

四旋翼姿态检测及数据处理系统优化设计

江 杰,刘 娜,杜永兴

(内蒙古科技大学信息工程学院,内蒙古包头 014010)

姿态角作为四旋翼飞行器重要飞控参数,对稳定飞行起着至关重要作用。针对姿态的检测问题,搭建了以STM32F407VG、MPU6050和AK8975组成的姿态检测系统。采取四元数法描述飞行器姿态,通过共轭梯度法对加速度计和电子罗盘测量数据做最优处理,得出的数据融合参数修正陀螺仪累积漂移误差,从而达到最佳融合效果。结果表明该检测及数据处理系统简单可靠,性能稳定,计算量小,能够满足四旋翼飞行器对精度和实时性的要求。

姿态检测系统;四元数;共轭梯度法;互补滤波

0 引言

四旋翼飞行器因其具有较强的机动性能等优势而被广泛应用在工业和军事上。飞行器最终的飞行效果与控制系统的性能密切相关,而高效、精确的姿态检测是保障优质控制系统的关键环节[1]。

集成度高、性价比好、功耗低的MEMS传感器是完成微型姿态测量系统的最佳选择,但同时这类传感器有一定的缺陷。陀螺仪动态响应好,但温度漂移较严重;加速度计精度高、静态性能好,但其动态响应比较慢;电子罗盘在测量地磁场的过程中易受周边环境中外部磁场的干扰而无法独立给出测量结果。因此考虑综合陀螺仪、加速度计和电子罗盘各自的优势,将三者采集的数值融合处理,不但可提高系统的动态性能,还可以减小偏差,使测量的角度与真实的角度更吻合。

系统由主控制器STM32F407VG、9轴运动处理器MPU6050和3轴磁力计AK8975组成。由陀螺仪检测角速度,经过数学运算得出角度,将加速度计和电子罗盘测量出的加速度和地磁场这两个不相关的三维矢量,作为四旋翼姿态的观测矢量来校准陀螺仪的测量值[2]。

在数据融合方面,利用四元数描述姿态,结合计算量少且效果较好的共轭梯度法和互补滤波法。结果表明该检测及数据处理系统能够满足四旋翼对精度和实时性的要求。

1 系统设计

姿态检测系统由上位机和下位机两部分组成,下位机是硬件架构,主要由STM32F407VG、MPU6050和AK8975及无线模块等构成。传感器主要作用是实时测量飞行器的原始姿态信息,并将检测的模拟测量信号转化为数字信号输出,主控制器及其外围电路的主要作用是实时采集传感器输出的数字测量信号以及融合原始姿态数据,得到姿态信息。无线通信模块[3]将收到的数据通过USB传送给上位机,实现整个系统的通信。系统硬件电路结构如图1所示。

图1 姿态检测系统硬件电路结构

系统总体方案如图2所示。

图2 系统总体方案

基于ARM CortexM4内核的STM-32F407VG处理器,处理速率快,拥有128个外设通用的输入输出接口,故选其作为飞行器数据处理的核心单元。角速度和加速度数据的测量选用6轴运动处理器MPU6050,该芯片集成了3轴陀螺仪和3轴加速度计[4],其高分辨率能够测量到不到1°的倾角变化,电子罗盘选用3轴磁强计AK8975,分辨率达0.3 μT/LSB,满足飞行器对于精度的要求。

2 姿态检测

2.1 姿态检测原理

加速度传感器可用来测量三轴加速度值。把传感器固定在四旋翼的主控板上,在处于相对静止时,当四旋翼姿态改变,相对于重力场加速度计X、Y、Z三个方向的感应轴产生变化,从而产生重力的分量。当四旋翼处于变速运动时,加速度计测出的加速度值是物体的重力加速度和线性加速度的总和。但因为加速度计动态响应速度慢,存在累积漂移误差,并且在飞行器运动的过程中给测量带来白噪声,所以其不能独立使用测量系统的姿态。

电子罗盘通过磁场在Z轴与Y轴的分布,测量相对于地磁场的方向,提供绝对航向姿态信息,前提是电子罗盘必须保持水平才可以测出精确的偏航角。飞行器处于三维空间中,故通过磁场传感器难以单独实现姿态检测。

陀螺仪检测出三轴角速度,通过对其积分得到姿态角。然而由于其本身的局限性,积分运算倾向于形成漂移,从而导致误差累计。因此需要结合MEMS传感器的优势,发挥加速度计及磁力计在稳定下的姿态角测量,以及陀螺仪在动态条件下的姿态角测量,通过对其结果的融合,得出三维空间下准确的姿态角。

2.2 飞行器姿态描述

飞行器在三维空间中有3种不同的姿态,即俯仰、横滚、偏航。将陀螺仪、加速度计和电子罗盘的输出值定义在一个坐标系,记飞行器的载体坐标系为b(X,Y,Z),选择东北天右手坐标系作为导航坐标系n(E,N,U)[5]。飞行器在运动过程中,保持n不变,b相对于n经过一次或数次的旋转之后得到新的载体坐标系。飞行器坐标b系与坐标n系之间关系如图3所示。

图3 飞行器载体坐标系与导航坐标系的关系示意图

其中,角ψ、θ、φ表述载体相对于导航坐标系的转角,即偏航角、俯仰角、横滚角[6]。由3个姿态角可表述b系到n系的关系为:

(1)

一个单位四元数可以描述为:

(2)

四元数可以看作绕一个轴的一个旋转,其关系为:

(3)

式中:μx、μy和μz是角矢量的分量,μ是角矢量大小。用四元数表述式(1)为:

(4)

由式(1)和式(4)得出由四元数转换姿态角的关系式为:

(5)

3 数据处理

3.1 姿态角度源值获取

(6)

从而解出φ和θ分别是:

(7)

(8)

(9)

利用三角函数关系计算航向角:

(10)

陀螺仪检测出的是角速度值,经过积分运算,获得相应的角度值。设陀螺仪输出为ωb=[ωxωyωz]T,则得姿态角为:

(11)

3.2 多传感器的数据融合

姿态四元数q的微分方程用矩阵形式表述为:

(12)

陀螺仪在坐标b系中输出的X、Y、Z轴的角速度对应式(12)中ωx、ωy、ωz。设采样周期为T,每隔T读取ωx、ωy、ωz,代入式(12)得出更新后的四元数,从而得出更新后的方向余弦矩阵[7]。然而,陀螺仪的输出值因漂移而产生误差,所以需要合理的补偿算法校正误差,文中融合加速度计和电子罗盘的数据,采用共轭梯度法进行数据处理,以误差分析和噪声补偿获得高精度校正结果。

共轭梯度法顾名思义是将共轭性与最速下降方法联合,根据已知点处的梯度建造一组共轭方向,沿着这组方向查找,直到找到目标函数的极小点[8]。相对于其它优化算法优点显现在获得高精度的校正结果的同时极大的提高了计算效率。

对于该系统中,加速度计与磁感器在坐标b系的输出,经过归一化计算,得出由坐标n系到坐标b系投影的四元数表示为:

(13)

(14)

式中:

将式(13)、式(14)中转换得出的数据与传感器测量数据相减得到测量误差。定义误差函数:

对e(q)取2范数的平方,设为共轭梯度法的目标函数f(q),对四元数的优化即为对f(q)求最小值。按照共轭梯度法,载体更新的方程为:

Qm(k+1)=Qm(k)+λkdk

(15)

式中:λk为最优步长;dk为搜索方向。根据共轭梯度算法,定义:f=JT(q)e(q),式中JT(q)为e(q)的雅克比矩阵。

d1=-b1,fk=gk,则λk=-,βk=,dk=-gk+βkdk-1。

当利用四元数得到当前时刻的姿态四元数后,用式(15)进行次数迭代得到四元数最优值,为了降低运算量,文中实际程序中只进行一次迭代。加速度计与电子罗盘的输出值经过共轭梯度法修正后,获得优化的四元数记为Qgh。将陀螺仪测量数据进行解算后记为Qω。由于Qgh和Qω在频域上的互补特性,因此使用互补滤波方法融合两者,得到新姿态四元数Q,能够达到准确测量载体的姿态信息。互补滤波的公式为:Q=αQgh+(1-α)Qω(0≤α≤1),当步长为0.03时,姿态静态值稳定。

图4 融合的共轭梯度下降法框图

4 实验分析和算法验证

为了使传感器输出的结果更加准确,在数据采集前,通过软件编程对器件的零漂进行处理,对加速度计采取滑动平均滤波法、电子罗盘运用限幅平均滤波法、陀螺仪使用均值滤波法[9],采集的数据通过优化处理实现软件修正,最大可能的减少扰动。通过硬件电路采集各个传感器的信号,检测的数据经过简单的处理传送到STM32F407上,再将处理后的数据进行姿态解算和数据融合。通过串口发送到PC机上,实时显示姿态信息,最终运用MATLAB实现数据曲线的绘制。系统验证主要是通过四旋翼处于静止和运动时的反复测试结果进行比较,验证算法的有效性。

4.1 静止状态

如图5所示,根据四旋翼飞行器在采用数据融合前后姿态角度的变化曲线得出,未经过数据处理回到初始位置有明显的角度偏移误差,而且误差范围较大。经过数据融合处理后的数据能够保持在0°左右。互补滤波前后误差比较对比如表1所示。

表1 静止状态滤波前后误差角度

4.2 运动状态

如6图所示,姿态角度在数据融合前的起始时间均在0°左右,经过一定时间的滚动后,三个角度均有一定范围的偏差角度,无法实现实时准确的姿态角测量。经过融合滤波后的数据,在飞行器经过一段时间的转动后,横滚角、俯仰角、偏航角均能保持在0°。互补滤波前后误差比较对比如表2所示。

表2 运动状态滤波前后误差角度

图5 静止状态下滤波前后对比图

图6 运动状态下滤波前后对比图

5 结论

由上述实验验证分析可以看出,该系统能够有效的检测无人机的姿态变化。通过无线模块实现数据高效传输,有效融合了加速度计、陀螺仪、电子罗盘在测量过程中的优势。实验结果表明,这种数据融合算法处理,可以使姿态检测系统具有较好的静态特性和动态特性,能够满足四旋翼飞行器对精度和实效性的要求。

[1] 杨永. 基于PD控制的航天飞行器控制系统的设计与实现 [J]. 计算机测量与控制, 2014, 22(6): 1738-1740.

[2] 刘春阳, 徐军领, 程洪涛, 等. MPU9250传感器的姿态检测与数据融合 [J]. 河南科技大学学报(自然科学版), 2015, 36(4): 14-17.

[3] 王俊生, 马宏绪, 蔡文澜, 等. 基于ADRC的小型四旋翼无人直升机控制方法研究 [J]. 弹箭与制导学报, 2008, 28(3): 31-34.

[4] 符秀辉, 赵茂鑫, 周文俊. 基于互补滤波算法的移动机器人姿态检测 [J]. 测控技术, 2015, 34(6): 31-33.

[5] 刘凤鸣. 重力梯度辅助导航定位技术研究 [D]. 哈尔滨: 哈尔滨工程大学, 2010.

[6] 龙达峰, 刘俊, 张晓明, 等. 高速旋转弹飞行姿态磁测解算方法 [J]. 弹道学报, 2013, 25(2): 69-73.

[7] QNUNKA Chiemela, BRIGHT Glen, STOPFORTH Riaan. USV attitude estimation: an approach using quaternion in direction cosine matrix [J]. Robotica, 2016,34(5):995-1009.

[8] 乔熔岩, 赵新国. 改进共轭梯度法求解无约束二次凸规划问题 [J]. 大学数学, 2014, 30(6): 38-42.

[9] 何瑜. 四轴飞行器控制系统设计及其姿态解算和控制算法研究 [D]. 成都: 电子科技大学, 2015.

Optimization Design of Attitude Detection and Data Processing System of a Quad-rotor Aircraft

JIANG Jie,LIU Na,DU Yongxin

(Information Engineering College, Inner Mongolia University of Science and Technology, Inner Mongolia Baotou 014010, China)

As the importance flight control parameter of the four rotor aircraft, attitude angle played an important role in stable flight. For gesture detection problem, attitude detection system was built with STM32F407VG, MPU6050 and AK8975. Spacecraft attitude was described using quaternion method, and the data of the accelerometer and the electronic compass were optimized by using the conjugate gradient method, then the data fusion parameters were used to correct the gyro drift error, so as to achieve the best fusion effect. Results showed that the detection and data processing system was simple, reliable, stable, easy to calculate and could satisfy the requirements of the accuracy and instantaneity of four rotor aircraft.

attitude detection system；quaternion；conjugate gradient method; complementary filter

2016-09-29

国家自然科学基金(61661044)资助

江杰(1959-),男,内蒙古包头人,教授,研究方向:嵌入式系统、工业控制网络、智能机器人。

TP216

A