低成本无人机姿态解算研究

马 敏，许中冲，常辰飞

(中国民航大学 航空自动化学院，天津 300300)



低成本无人机姿态解算研究

马 敏，许中冲，常辰飞

(中国民航大学 航空自动化学院，天津 300300)

针对小型四旋翼无人机姿态解算数据精度低、缺少余度控制、易发散等问题，提出一种基于GPS、三轴陀螺仪加速度计、三轴磁力计的随机加权自适应滤波算法估计无人机姿态；建立四旋翼无人机姿态旋转矩阵，搭建加速度计和磁力计获取无人机姿态信息的模型，以及采用四元数解算法的陀螺仪定姿解算模型;采用随机加权自适应估计法，依据多元函数求极值定理，在保证总体均方差最小的情况下导出最优随机加权因子，进而解算出姿态角信息，提高四旋翼无人机姿态解算滤波精度与稳定性;仿真与试验结果表明：随机加权自适应滤波与平均值滤波算法相比解算精度更高，输出结果更平稳，且无人机各项预期功能均能正常实现，能够满足四旋翼无人机自主飞行的需要。

姿态解算;多传感器数据融合;随机加权;无人机

0 引言

四旋翼无人机(unmanned aerial vehicle,UAV)可以实现倒飞、急转弯等机动飞行特点具有很广阔的应用前景，是当前各大科研单位的研究热点[1-2]。姿态信息的精度与可靠性是实现无人机自主飞行的前提条件，随着科学技术的快速发展，飞行器定姿理论和技术取得了显著进步[3]。

目前，常用的定姿传感器有陀螺仪、磁力计、GPS等。但陀螺仪易产生温漂、磁力计易受外部磁场干扰等，因此单个传感器很难得到相对准确的姿态角信息[4]。为了解决这类问题，进行多传感器信息融合，能够提高信号估计的精度。

多传感器信息融合通过整合多元信息的优点提高信息精度。文献[5]采用卡尔曼滤波算法进行数据融合解算出姿态角信息，有效的消除了测量白噪声，抑制了陀螺仪的漂移，但主要适用于噪声为高斯分布的线性系统，且量测方程为线性的，而实际导航系统则是非高斯噪声、非线性系统。文献[6]采用直接加权的方法将传感器信息进行加权平均得到融合值，适用于动态系统但精度较低。

针对三轴加速度计陀螺仪、三轴磁力计及GPS建立的姿态测量系统，建立其特征模型，利用随机加权自适应滤波的方法对采集的数据进行融合。该方法既能提高被处理漂移信号的精度又能充分利用各个权因子所占的比重，提高动态系统的定位解算精度，实验证明了所提算法的有效性和正确性。

1 系统构成

该四旋翼无人机采用三轴陀螺仪加速度计、三轴磁力计、GPS构成多传感器姿态测量系统，实现无人机的姿态测量功能，构成图如1所示。

图1 融合系统总体框图

通过对三轴加速度计、三轴磁力计的信息进行相关反三角函数处理可以得到无人机静止或匀速运动状态下的俯仰角、滚转角和偏航角；三轴陀螺仪的输出即为四旋翼的滚转角速率，俯仰角速率和偏航角速率，通过相应的解算方法可以得到无人机的姿态角信息。系统将前两者的数据与后者的数据进行随机加权自适应滤波，获得最终的姿态角信息。

针对加速度计只能测量静态或匀速运动物体重力加速度的问题，采用GPS数据与加速度计信息进行融合补偿运动加速度信息，测得机动状态下无人机的机体加速度。

2 姿态源值获取

2.1 坐标转换和姿态角原理

(1)

(2)

机体坐标系下的物体相对于导航坐标系运动的姿态角可解算得：

(3)

其中俯仰角的真值域为(-90°,90°)，偏航角ψ和滚转角θ的真值可通过真值表判断[7]。

2.2 加速度计/磁力计姿态解算

(4)

经过磁标定后的三轴磁力可获得无人机的偏航角。当机体处于任意姿态时，保持偏航角不变，将磁强计的输出投影到水平面上(令姿态旋转矩阵中ψ=0即可)，则有机体坐标系磁力计输出mb到水平面的投影ms为:

(5)

不考虑磁偏角的影响下偏航角ψ为:

(6)

2.3 陀螺仪定姿

除采用加速度计/磁力计的方法定姿外，可以用陀螺仪完成无人机的姿态解算。针对坐标系旋转矩阵利用陀螺仪进行姿态解算的方法有多种。考虑到参数个数以及计算量方面的优势设计中采用四元数法完成低空低动态实时姿态解算[8]。

(7)

常用的四元数微分方程求解方法有旋转矢量法、和龙格库塔法以及泰勒展开式法[9-10]，实际应用中根据控制器精度与更新时间的考虑可以采用单子样、多子样旋转矢量法，一阶、二阶、四阶龙格库塔法。当陀螺仪的输出为角度增量时多采用旋转矢量法，而MEMS三轴陀螺仪输出为角速率形式，鉴于设计中使用的STM32微控制器采用二阶龙格库塔法求解四元数。

(8)

式中，T为采样周期。

(9)

3 随机加权自适应滤波法姿态融合定姿

3.1 融合算法理论导出

对于图1中的无人机定姿系统而言，姿态角的获得方法有前面分析的两种：一种是陀螺仪导出姿态信息, 二是由加速度计、磁力计解算姿态角。陀螺仪不受无人机运动状态、飞行环境的磁场影响动态性能良好，但陀螺仪存在漂移误差，且随着工作时间的增加将出现严重失真。而磁力计与加速度计组合定姿则在动态与磁场干扰下出现姿态角解算的严重偏差，故采用姿态融合定姿是一个十分必要的解决方案。数据融合能提高系统的容错率、抗干扰能力与动态性能，常用的信息融合方法有卡尔曼滤波[11-12]、梯度下降法[13-14]，设计中采用自适应滤波中的随机加权法进行无人机姿态融合解算。

(10)

对于均方差σ2，因为Xi(i=1,2,...,n)相互独立，且为X的无偏估计，所以总方差可以表示为：

(11)

想求的总体均方差σ2最小时的权系数vi，由多元函数求极值定理(拉格朗日定理)，可知式(10)有解时，最优随机加权因子为：

(12)

3.2 导航融合算法流程

各传感器的数据经过初始标定矫正、滤波、补偿等处理后，由三轴陀螺仪采用四元数法导出的姿态角Xi=(ψiγiθi)T与加速度计和磁力计解算姿态角Xj=(ψiγiθi)T。利用以上随机加权自适应滤波估计的运算步骤为：

1)采用式(14)、(15)推算出K采样时刻的Yii和Yij；

4 实验结果分析

图2给出了Matlab中模拟两组姿态信息，采用随机加权在线自适应滤波得到的曲线图，仿真数据选定两组不相干的零均值白噪声进行模拟，其方差分别取为0.005、0.03，待估量为X=0.55，取400个点的采样数据进行模拟。表1给出了两种方法的输出平均值和方差值。

图2 估计值输出曲线及其局部放大曲线

随机加权融合法平均加权法平均值0.54990.5484方差0.00110.0025

通过对比图2以及表1可知，两组数据经过随机加权自适应滤波融合后的结果比采用平均值法输出结果变化幅度小，估计值的方差小，即前者能提供更加有效的数据融合方法。

实验中将无人机固定在高精度惯导测试平台上，对其进行验证飞行实验。微控制器采用STMF32F405，采集三轴陀螺仪加速度计、三轴磁力计(采用I2C总线接口读取传感器数据)及GPS(采用串口通信采集数据)的信息进行姿态解算，姿态解算周期为30ms，得到的姿态角拖过串口与上位机进行通讯，通过上位机软件可以获得姿态角的实时解算值。测试时先进行转台归零使姿态角回到相对基准，然后进行静态测试(让转台沿一个方向做匀速运动到一定的角度)和动态测试。动态测试时设定测试平台内框(俯仰角)、中框(滚转角)分别以的幅度和0.5Hz的频率做正弦摆动，设定测试平台的外框(偏航角)以的幅度和0.2Hz的频率做正弦摆动。表2为随机采样下姿态数据的静、动态误差性能指标，图3为动态测试下采集到的俯仰角、滚转角、偏航角绘制的曲线图。

表2 转台测试姿态角性能指标

图3 姿态角动态输出曲线

体的实时位置，满足无人机的飞行要求。与实际飞行时飞机面临的环境有所偏差，实际的误差可能会有所增加。

5 结论

文中介绍了小型四旋翼无人机利用低成本传感器进行解算航姿解算的两种方法，由于单一的姿态解算方法的航姿可信度不高的问题，采用随机加权融合这种自适应滤波方法，对两种不同的解算输出结果进行融合滤波。实验结果统计说明：采用随机加权滤波融合后的姿态信息提高了四旋翼无人机的测量精度，计算量小，能够适应低成本的航姿控制系统。

[1] 聂博文，马宏绪，王 剑，等.微小型四旋翼飞行器的研究现状与关键技术[J].电光与控制，2007，14(6):113-117.

[2] 赵 勃，鲜 斌，张 鑫，等.四旋翼飞行器硬件在环仿真平台研究[A].第三十一届中国控制会议论文集[C].合肥，2012，c卷:5008-5013.

[3] 吴 勃，徐 欢，乔相伟.状态切换UKF的飞行器姿态确定算法[J].电机与控制学报，2012，16(6):98-104.

[4] 冯智勇，曾 瀚，张 力，等.基于陀螺仪及加速度计信号融合的姿态角度测量[J].西南师范大学学报:自然科学版，2011，36(4): 137-141.

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

[6] 李媛媛,张立峰,多传感器自适应加权融合算法及其应用研究[J].自动化与仪器仪表，2008(2):10-13.

[7]吴 杰,闫建国.基于修正的卡尔曼滤波的姿态估计算法研究[J].计算机真,2012, 29(2):54-57.

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

[9]葛泉波,李文斌,孙若愚,等.基于EKF的集中式融合估计研究[J].自动化学报, 2012, 39(6):816-825.

[10]郭晓鸿,杨 忠,陈 喆,等.EKF和互补滤波器在飞行姿态确定中的应用[J].传感器与微系统,2011,30(11):149-152.

[11] 辛 琪,史忠科.基于多源信息的飞行姿态估计方法[J].飞行力学,2012,30(6): 527-531.

[12] 梁延德,程 敏,何福本,等.基于多源信息的飞行姿态估计方法[J].飞行力学, 2011,30(11):56-58.

[13] Madgwick S O H,Harrison A J L,Vaidyanathan R H.Estimation of IMU and MARG orientation using a gradient descent algorithm[A].Proceedings of the 2011 IEEE International Conference on Rehabilitation Robotics (ICORR)[C].Switzerland:IEEE,2011：1-7.

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

[15] 振 江,康健一,张 青,等.数据融合技术在温室温度检测中的应用[J].农业机械学报,2006, 37(10):101-103.

[16] 李 伟,何鹏举,高社生.多传感器加权信息融合算法研究[J].西北工业大学学报,2010, 28(5):674-678.

Low Cost UAV Attitude Algorithm Research

Ma Min, Xu Zhongchong, Chang Chenfei

(Aviation Institute of Automation, Civil Aviation University of China, Tianjin 300300, China)

In order to improve the drawbacks of low accuracy, lack of redundancy and divergence that occurred in the attitude calculation of micro quadrotor unmanned aerial vehicle (UAV), a random weighted adaptive filter algorithm is proposed based on GPS, 3-axis gyroscope-accelerometer and 3-axis magnetometer, to estimate the UAV attitude. A Four rotor UAV attitude rotation matrix is established，A model for obtaining the attitude information of UAV with accelerometer and magnetometer，and Attitude calculation model of gyroscope sensor using four element method.By randomly weighted adaptive estimation method, based on the multiple function extremum theorem, in ensuring the overall variance under the condition of minimum to derive the optimal random weighting factor, and then calculate the attitude information, improve the quadrotor UAV attitude estimation filtering accuracy and stability.The simulation and experimental results show that the random weighting adaptive filtering and the average filtering algorithm are more accurate and more stable, and the expected function of the UAV can be realized, which can meet the needs of the autonomous flight of the quadrotor UAV.

attitude algorithm; multi-sensor data fusion; random weighted;UAV

2015-11-30；

2016-01-19。

天津市应用基础与前沿技术研究计划(13JCYBJC39000)。

马 敏(1971-)，女，安徽霍邱人，教授，主要从事航空发动机状态监测及故障诊断、电学成像技术、航空无损检测技术及新型传感器等方向的研究。

1671-4598(2016)06-0161-03

10.16526/j.cnki.11-4762/tp.2016.06.044

TP274

A