基于梯度下降法和互补滤波的航向姿态参考系统

陈 亮，杨柳庆，肖前贵

（1.南京航空航天大学 自动化学院，江苏 南京210016；2.南京航空航天大学 无人机研究院，江苏 南京210016）

基于梯度下降法和互补滤波的航向姿态参考系统

陈 亮1，杨柳庆1，肖前贵2

（1.南京航空航天大学 自动化学院，江苏 南京210016；2.南京航空航天大学 无人机研究院，江苏 南京210016）

针对微型无人机航向姿态参考系统低成本、小型化的工程实现需求，基于三轴陀螺仪、加速度计和磁力计，提出了一种在线实时姿态估计算法。该算法采用四元数描述系统模型，采用改进的梯度下降法预处理加速度计和磁力计的姿态信息，然后采用互补滤波融合陀螺仪的姿态信息，实现高精度实时姿态估计。最后通过在线性能测试，来验证算法的有效性。结果表明，该算法测量误差小、运算量小、实时性高，具有较高的工程应用价值。

航姿系统；梯度下降法；互补滤波；四元数

近年来随着科技和社会发展，无人机发展日新月异，其中微小型无人机在军用和民用领域以其低成本、小尺寸、生存性能强等特点发展尤其迅猛，在军事侦察、生态监测、视频拍摄和抢险救灾等有着广泛应用前景。航向姿态参考系统是无人机导航与飞行控制系统的重要组成部分，其精度直接影响到无人机的姿态控制精度及导航定位精度。微小型无人机自身有效载荷小，决定了在一段时间内都无法安装激光陀螺、光纤陀螺等高精度的姿态测量装置，于是以MEMS陀螺仪和加速度计，以及磁力计（MARG）等组成的低成本微小型航向姿态参考系统（AHRS）成为一个研究热点[1]。然而低成本的MEMS陀螺仪虽然瞬态性能较好，但误差漂移会随着时间逐渐累积，导致测量精度降低；加速度计没有积分误差，但当被测对象处于高动态时，会受到附加平动或转动加速度干扰；同样地，磁力计也会受到周围的软磁或硬磁的干扰。因此，单独采用MEMS陀螺仪或者加速度计和磁力计都无法独立给出足够好的姿态估计[2]。

一般的，姿态估计是采用卡尔曼滤波算法及其衍生形式，如扩展卡尔曼滤波（EKF），无迹卡尔曼滤波（UKF），联邦卡尔曼滤波等，在文献[3-7]中进行了不同方面深入的研究，分别采用了EKF，无迹卡尔曼滤波，粒子滤波等算法来提高姿态估计的精度。其中扩展卡尔曼滤波器算法是一种高精度的在飞行器中应用非常广泛的姿态解算算法，但EKF存在3大缺陷：1）在一般情况下计算雅可比矩阵是不容易实现的过程而且其计算量很大；2）当线性化假设不成立时，线性化会导致滤波器极度不稳定；3）实际应用中，噪声难以符合白噪声的要求[8-9]。一般情况下卡尔曼滤波需要构造多维的状态方程和量测方程，运算量较大，对嵌入式处理器资源占用率过高。文献[10]提出一种自适应显式互补滤波（AECF）来提高无人机的姿态估计精度，有效地解决了一般互补滤波器需要对重构无人机姿态估计的缺陷。文献[11]中提出采用一种梯度下降算法对加速度计和磁强计的数据进行预处理，然后利用互补滤波器将处理后的结果与陀螺仪的积分进行融合，但是在融合过程中参数为定值，导致算法在飞机高机动状态下的姿态估计误差较大。在实际情况中，微小型无人机的飞控和航姿系统是集成在一个模块中，由一个内核来处理所有数据，同时无人机飞控和航姿系统对实时性要求较高[12]。为了减小微型无人机中处理器的运算负担，降低航姿算法的解算复杂度和增加算法的实时性，保证一定的精确性并增强工程可实现性，文中分析并设计了基于梯度下降法和互补滤波的四元数混合滤波。

1 基本原理

1.1 算法总体结构

文中所设计的混合滤波算法总体结构为:首先，利用陀螺仪输出的角速度和四元数微分方程得出角速度微分四元数；然后运用梯度下降法对加速度计和磁力计测得数据进行预处理，得到最小的误差四元数的微分；再用互补滤波将两者进行融合，尽可能的减小陀螺仪的漂移误差和加速度计的高频干扰，以及磁力计引起的姿态估计误差；最后对互补滤波后的姿态微分四元数进行积分，估算出最优的姿态值。该混合滤波算法运算过程不涉及三角和反三角函数运算，全部使用简单的四元数加减乘除运算，计算工作量很小，极大地提高了运算效率。

图1 算法总体结构图

1.2 四元数姿态描述

通常将惯性测量单元中陀螺仪、加速度计和磁力计的测量数据定义到机体坐标系，取机体的重心为机体坐标系原点，3个轴分别与机体的纵轴、横轴和竖轴相互重合，分别定义为右前上（XYZ）。相对应的绝对坐标系称为导航坐标系，文中采用东北天ENU坐标系n。机体坐标系相对于导航坐标系的转换可采用旋转四元数表示[13]：

又由姿态四元满足微分方程：

设在很小的时间段（t，t+Δt）内，Δt为姿态四元数更新时间，wbnb值不变。可以得到基于陀螺仪输出的姿态四元数的迭代方程为：

方向余弦矩阵与四元数的关系可表示为：

但由于四元数实质上是旋转矢量，它的4个变量并不独立，必须对其规范化处理。本文所用四元数均为规范化的模值为1的四元数，即：

并且在每次滤波周期，都要对解算得到的四元数进行规范化处理。

姿态角与四元数之间的关系可表示为

2 互补滤波器的设计

2.1 互补滤波器原理

互补滤波简单地来说，就是把滤波器的两部分基于一阶微分系统来融合在一起，成为一个新滤波器，从而得到一个更加准确的线性估计输出。在无人机航姿系统中，由于陀螺仪虽然短时间内可提供高精度的姿态数据，但因漂移的影响，精度随着时间就会降低。加速度计和磁力计则相反，其量测误差不会随着时间累积，长时间内有良好的稳定性，但瞬态的加速度和磁场干扰对测量精度影响较大。从频域上来说，陀螺仪具有高通特性，而加速度计和磁力计有低通特性，因此它们正好在频域上相互补充[15]。

式中ya为加速度计和磁力计姿态量测信息；yb为陀螺仪输出值；wa、wb表示系统量测噪声；b为陀螺漂移。一般工程中，互补滤波器结构可用如图2所示。

图2 互补滤波器结构示意图

2.2 基于梯度下降算法的四元数最优估计

梯度下降法是在沿梯度下降的方向求解表达式的最小值。其迭代公式为yk+1=yk-ρkgk，其中gk表示函数的梯度，前面加负号表示梯度的负方向，即梯度下降方向；ρk表示在梯度方向上的搜索步长，即求取一点使函数的值达到最小。具体到文中，设经过单位规范化后的机体加速度计、磁力计三轴输出分别为 ab=[0 axayaz]T、mb=[0 mxmymz]T，当地参考重力矢量为gn=[0 0 0 1]T，当地参考磁场矢量为mn=[0 px0 pz]T，令

故对重力场，其误差向量矩阵为：

同样地，对磁场，其误差向量矩阵为：

误差向量F的梯度为：

则误差向量‖F‖下降最快的方向时的单位四元数梯度即是：

式中w是陀螺仪角速率构成的四元数向量，即w=[0 wxwywz]。

综上可得，基于梯度下降算法的四元数最优估计的迭代方程为：

其中qam，t为由加速度计和磁力计输出数据经梯度下降算法处理后得到的姿态更新四元数。

2.3 互补滤波

利用陀螺仪的高通特性，加速度计和磁力计的低通特性，在频域上的互补关系，采用互补滤波对两者的信息进行融合，来提高航姿系统的姿态测量精度。由上文2.1节分析，为使算法更加简单高效，提高系统姿态结算的快速性和实时性，令kp=K，ki=1，则互补滤波融合后的姿态估计值为：

其时域表达式为：

其中θa，m为加速度计和磁力计输出数据基于梯度下降法得到的姿态角估计，可由迭代式（16）得到；wcor为陀螺仪经过校正补偿后的角速率，可由下文3.2节式（20）、式（21）得到；为融合后的姿态四角估计；适当选取低通滤波器系数K值，可使系统具有恰当的截止频率，K值的大小主要根据无人机的飞行状态来变化。

3 误差补偿

3.1 有害加速度的考虑

当无人机加速状态下，由于加速度计会敏感到有害加速度，因此在长时间机动后姿态测量会出现较大误差，导致滤波发散，所以必须对有害加速度进行考虑。设其中分别为加速度计白噪声零偏的均方差，根据试验经验取高加速飞行状态阀值为ζ=0.3gn，则

当λt≤η，无人机处于静止或匀速飞行状态，K值不变，根据多次试验取经验值Kt0=20；

当λt＞ζ，K值应趋于无穷小，可取Kt=0，即无人机在高机动飞行状态主要依靠基于陀螺仪的姿态导出。

3.2 陀螺仪误差补偿

陀螺的随机误差模型[10-16]表示为：

其中式中εb为随机常值漂移，εm为可以用一阶马尔可夫过程描述的相关漂移，wg为零均值高斯白噪声。由于系统的采样频率远高于陀螺零偏的变化速率，采用随机游走描述陀螺仪随机漂移，因此可以忽略式中第二项相关漂移。按照四元数微分方程可以反求得到，陀螺仪角速率误差wε与误差四元数微分之间的关系可以表示为：

陀螺零偏wb为陀螺漂移wε中的常值部分，可以通过下式来消除：

其中wcor，t为陀螺角速率在零偏补偿后组成的四元数矢量。

4 硬件平台与算法验证

文中采用MPU9150九轴惯性传感器测量单元进行测试，MPU9150由MPU6050传感器（其中包括三轴加速度计和三轴陀螺仪）与HMC5883L三轴磁力计集成，数据通过I2C总线进行读取，利用文中设计的滤波算法对传感器输出的原始数据进行处理来获得机体准确的姿态信息，并通过SPI口将解算后的数据经由2.4 GHz无线数传模块上传到上位机中。上位机监控显示界面采用C++编写，可显示各传感器的测量数据和姿态角的数据，航姿系统以50 Hz频率对传感器进行采样，并将数据导出到Excel中，以便进一步地分析。系统硬件结构图如图3。

图3 系统硬件结构示意图

将本系统加载到某型微小型无人机中，采集实际飞行数据，其中主要分为四段飞行状态，分别为无人机加速、转弯、平飞和大幅连续转弯过程。实验结果如图4，5，6和表1所示。

图4 俯仰角曲线图

图5 偏航角曲线图

图6 滚转角曲线图

表1 各飞行状态下的无人机姿态误差

在0～400 s时段，无人机在进行加减速运动，由上图4和表1可知基于梯度下降算法的四元数互补滤波能够较好地跟踪无人机真实姿态。在400～700 s时段内，无人机连续转弯，表1给出了此阶段梯度下降算法的姿态误差平均值和误差均方差值，3个姿态角的误差值均小于1°，姿态误差均方差也都小于1.8°，因此改进后的梯度下降算法也能较好地跟踪无人机连续机动状态下的姿态变化。在800～1 100 s时段，无人机在进行平稳飞行，表1给出了此阶段无人机的姿态误差，可以看出其误差平均值和误差均方差都很小，很好地完成姿态跟踪。在1 100～1 500 s，飞机在大幅连续转弯过程中，算法解算结果仍能较好地完成无人机姿态确定。

5 结束语

文中提出了基于梯度下降法和互补滤波相结合的混合滤波算法，加速度计和磁力计测量数据，经过梯度下降算法处理后，反馈到参数可调的四元数互补滤波中，完成对陀螺漂移校正，进而获得高精度的姿态信息。实验结果表明，该算法具有较高的姿态估算精度，快速性好，对系统负载较低，具有较高的工程实用价值，适合低成本的航姿平台工程应用。

[1]刘兴川，张盛，李丽哲，等.基于四元数的MARG传感器姿态测量算法[J].清华大学学报：自然科学版 ，2012，52（5）: 627-631.

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

[3]Tarhan M，Altu E.EKF based attitude estimation and stabilization of a quadrotor UAV using vanishing points in catadioptricimages[J].Journal of Intelligent&Robotic Systems，2011，62（3-4）:587-607.

[4]Jing L，Xu L，Li X，et al.A federal UKF algorithm in INS/ GPS/aerial image integrated attitude determination system[C] //Imaging Systems and Techniques（IST），2013 IEEE International Conference on.IEEE，2013:60-63.

[5]Kai Xiong，Tang Liang，Lei Peng.Multiple Model Kalman Filter for Attitude Determination of Precision Pointing Spac-ecraft[J].ActaAstro-Nautica，2011（68）:843-845.

[6]Marina H G，Espinosa F，Santos C.Adaptive UAV Attitude Estimation Employing Unscented Kalman Filter，FOAM and Low-Cost MEMS Sensors[J].Sensors，2012，12（7）:9566-9585.

[7]Won S P，Melek W W，Golnaraghi F.A Kalman/Particle Filter-based Position and Orientation Estimation Method Using a Position Sensor/Inertial Measurement Unit Hybrid System[J].IEEETransactionsonIndustrialElectronics，2010，57（5）:1787-1798.

[8]赵琳，王小旭，丁继成，等.组合导航系统非线性滤波算法综述[J].中国惯性技术学报，2009，17（1）:46-58.

[9]Fourati H，Manamanni N，Afilal L，et al.A complementary observer-based approach for the estimation of motion in rigid bodies using inertial and magnetic sensors[C]//Control Applications（CCA），2010IEEEInternationalConferenceon.IEEE，2010:422-427.

[10]Euston M，Coote P，Mahony R，et al.A complementary filter for attitude estimation of a fixed-wing UAV[C]//Intelligent Robots and Systems，2008.IROS 2008.IEEE/RSJ International Conference on.IEEE，2008:340-345.

[11]Madgwick S O H，Harrison A J L，Vaidyanathan R.Estimation of IMU and MARG orientation using a gradient descent algorithm [C]//Rehabilitation Robotics（ICORR），2011 IEEE International Conference on.IEEE，2011:1-7.

[12]徐玉，李平，韩波.一种面向机动的低成本姿态测量系统[J].传感技术学报，2008（10）:2272-2275.

[13]Markley F L.Attitude error representations for Kalman filtering[J].Journal of Guidance，Control，and Dynamics，2003，26（2）:311－317．

[14]刘建业，曾庆化，赵伟，等.导航系统理论与应用[M].西安:西北工业大学出版社，2010.

[15]Mahony R，Hamel T，Pflimlin J M.Complementary filter design on the special orthogonal group so（3）[C]//Proc of the IEEE Conference on Decision and Control and European Control Conference，2005:1477-1484.

[16]Robert Mahony，Tarek Hamel，Pascal Morin，et al.Nonlinear complementary filters on the special linear group[J]. International Journal of Control，2012，85（10）:1557-1573.

Attitude and heading system based on gradient descent algorithm and quaternion complementary filter

CHEN Liang1，YANG Liu-qing1，XIAO Qian-gui2
（1.College of Automation Engineering，Nanjing University of Aeronautics and Astronautics，Nanjing 210016，China；2.Research Institute of Unmanned Aircraft，Nanjing University of Aeronautics and Astronautics，Nanjing 210016，China）

In order to satisfy the requirement of the low cost micro unmanned aerial vehicles'attitude and heading reference system（AHRS），the online real-time attitude determination method is proposed based on the combination of triaxial gyroscope，accelerometer and magnetometer.In the system，the system model is described by quaternion method，and an optimized gradient descent algorithm is used to process the data measured by accelerometer and magnetometer.And the complementary filter whose parameters are adjustable is used to fuse the data with triaxial gyroscope measurement to obtain high-precision attitude information.Online experiments were conducted to assess the performance of algorithm and verify the effectiveness of the algorithm.And the results show that the proposed scheme has characteristics of low error of attitude tracking，parameter adjustment，low computer load，and easy to realize，therefore the designed algorithm is applicable for low-cost and small-sized attitude and heading reference system of micro UAV.

attitude and heading reference system；gradient-descent algorithm；complementary filter；quaternion method

TN967.2

A

1674－6236（2016）24-0038-04

2015-11-30 稿件编号：201511291

陈 亮（1991—），男，安徽滁州人，硕士研究生。研究方向：无人机导航制导与控制。