基于卡尔曼滤波的小型无人机姿态估计算法研究

聂 鹏，李佩华，李正强，郑 旺，宋 平

(沈阳航空航天大学 机电工程学院，沈阳 110136)

姿态测量系统是无人机导航及姿态控制系统的重要组成部分，其精度直接影响到无人机各控制系统的精度。由于小型无人机带负载的能力有限，要求姿态测量系统的体积质量小、功耗低。随着MEMS(Micro-Electro-Mechanical Systems)技术和计算机技术的发展，小型无人机姿态的测量普遍采用捷联惯性测量单元(Inertial Measurement Unit,IMU)，其主要由低成本MEMS陀螺仪、加速度计和磁强计组成[1-2]。由于陀螺仪存在漂移，在姿态估计过程中存在累计误差，加速度计会受到无人机飞行过程中机体振动的影响，同时磁强计是一种磁阻传感器，容易受到外部磁场的干扰。单独使用无法满足控制系统的精度要求。本文采用加速度计及磁强计输出作为观测量，修正陀螺姿态解算的误差，利用扩展卡尔曼滤波器，对不同类型传感器的数据进行数据融合，得到较高精度的姿态信息。

1 坐标系定义及姿态矩阵

求取姿态角之前要定义载体坐标系和导航坐标系。本文的导航坐标系定义为东北天坐标系，原点选在无人机重心；载体坐标系原点选在无人机重心，xb沿机体横轴指向右，yb沿机体纵轴指向前，zb沿机体竖轴指向上，满足右手定则。θ，γ，Ψ分别是无人机的俯仰角，横滚角和航向角，则有载体坐标系到导航坐标系转换的姿态矩阵为

(1)

(2)

在得到随时间变化的姿态四元数后，通过公式(3)、(4)、(5)即可得到3个姿态角:

(3)

(4)

(5)

式中Tij由式(2)中姿态矩阵与四元数关系得出。

2 卡尔曼滤波姿态算法的设计

(6)

(7)

求解式(6)的微分方程即是对四元数q进行更新。此处采用四阶龙格库塔法直接求解四元数微分方程：

(8)

2.1 利用加速度计和磁强计计算测量四元数

MEMS陀螺仪动态响应特性良好，但运行时会发生温度漂移；同时积分运算会产生累积误差。加速度计和磁强计传感器测量姿态没有累积误差，但动态响应较差。因此，它们特性互补，可以采用算法融合这三种传感器的数据，利用无时间累积误差的重力场和磁场来修正陀螺给出的姿态角，提高测量精度。在融合之前通过重力场和磁场计算出最优姿态四元数，作为测量四元数。这个计算过程可以将其认为是最小平方误差准则的基础算法[5]。误差函数Sk定义为:

(9)

(10)

将计算测量四元数看成最小化问题，即通过使得加速度计和磁强计测量的误差函数Sk最小化来得到最优化的旋转四元数。梯度下降法作为一种基本的最优化算法，有工作量少，存储变量较少以及对初始点要求不高等优点[6]。式(9)描述了在初始点为q0和一个可变步长μ情况下，基于梯度下降法计算一个n次迭代的姿态四元数。式(11)表示计算误差函数，它由目标函数和其雅可比矩阵来定义。

(11)

(12)

(13)

误差函数Sk>1e-3将作为停止准则，避免无限迭代。实验表明，如果选择较小的值Sk<1e-3，相当于依靠传感器测量精度；同时选择较大的值Sk>1e-3，相当于假设测量是有噪声的。从IMU的测量噪声的结果中，本文选择Sk≤0.5。传感器融合另外一个问题就是选择初始四元数值，实验中发现，一个任意的初始条件会产生冗余的姿态信号。因此在系统开始运行后静止几秒钟，利用重力场和磁场通过算法得到初始四元数值。

2.2 扩展卡尔曼滤波

本文采用文献[7-8]中Kalman滤波器。考虑如下系统：

xk+1=Axk+Bwk

yk=Cxk+vk

(14)

将四元数的4个参数作为状态变量:

X=[q0q1q2q3]。

由式(7)得:

(15)

式(13)为建立的Kalman滤波的状态方程。wk=[δxδyδz]近似为系统噪声，其协方差矩阵Qk为的非负定常值对角阵。接着建立观测方程，量测失量和状态变量相同。但是，量测四元数由方程(11)(12)得出。量测矩阵C=I4,其中I4是4×4单位矩阵。

设采样周期为T，将状态方程和观测方程离散化，同时略去二阶以上项得到:

X(k)=A(k,k-1)X(k-1)+B(k-1)W(k-1)

(16)

Y(k)=C(k)X(k)+V(k).

(17)

式中

A(k,k-1)=

V(k)为量测噪声，其协方差阵R(k)为正定常数对角阵。通过上面的分析，根据离散型扩展卡尔曼滤波的递推方程，建立卡尔曼滤波的时间传播方程。

状态一步预测：

(18)

状态估计：

(19)

滤波增益：

K(k)=P(k,k-1)HT(k)[H(k)P(k,k-1)HT(k)+Rk]-1

(20)

一步预测均方误差：

P(k,k-1)=Φ(k,k-1)P(k-1)ΦT(k,k-1)+Γ(k-1)Q(k-1)ΓT(k-1)

(21)

估计均方误差：

P(k)=[I-K(k)H(k)]P(k,k-1)

(22)

姿态估计的流程图如图1所示。

图1 姿态估计框图

3 滤波器测试结果

本文采用的惯性测量单元包括Invensense公司的MPU6050(三轴陀螺仪和三轴加速度)和霍尼韦尔公司的HMC5883L(三轴磁强计)和导航计算机。测量单元安装在单轴转台上，计算机串口波特率为38 400 bps，数据长度为10 000点。本文进行了多组实验，通过转台匀速转动，采集测量单元所测数据并进入姿态解算程序，得出当前姿态测量单元姿态角。在进行转台实验时，单轴转台保持水平，惯性测量单元的横滚角和俯仰角保持为零，只能利用偏航角进行比较，在这里选取转台在5°/s和80°/s速率下的数据来评价滤波器。

图2的结果是使转台处于静止状态，为采用本文所设计算法，只利用MEMS陀螺仪输出的数据解算出的3个姿态角，可以看出姿态角是发散的。图5、图6中可见，采用本文滤波器解算姿态，航向角动态误差小于3°，有效补偿了陀螺漂移引起的姿态误差。

图2 陀螺仪姿态解算

图3 5°/s时滤波后姿态角输出

图4 80°/s时滤波后姿态角输出

图5 5°/s时航向角误差

图6 80°/s时航向角误差

4 结论

本文提出了一种基于扩展卡尔曼滤波技术的姿态融合算法，利用加速度计和磁强计的数据有效的补偿了陀螺仪漂移引起的姿态误差，很好的抑制了MEMS陀螺的漂移，提高了MEMS惯性器件在姿态测量应用中的测量精度。实验结果表明：本文所设计的算法具有较好的稳定性和姿态估计精度，为下一步的姿态测量系统的研发奠定了理论基础。

参考文献(References)：

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

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

[3] 薛亮,姜澄宇,常洪龙，等.基于状态约束的MIMU/磁强计组合姿态估计滤波算法[J].中国惯性技术学报,2009,17(3):338-343.

[4] Eugenio Denti,Roberto Galatolo,Francesco Schettini.An AHRS based on a kalman filter for the integration of inertial,magnetometric and GPS data[C].27th Internation Congress of the Aeronautical Sciences,2010.

[5] Cordova Alarcon J R,Rodriguez Cortes H,Vicente Vivas E.Extended kalman filter tuning in attitude estimation from inertial and magnetic field measurements[C].Electrical Engineering,Computing Science and Automatic Control,CCE,2009 6thInternational Conference on,2009.

[6] Sebastian O H.Madgwick,Andrew J L.Harrison,Ravi Vaidyanathan.Estimation of IMU and MARG orientation using a gradient descent algorithm[C].IEEE International Conference on Rehabilitation Robotics,2011.

[7] 黄旭,王常虹,伊国兴，等.利用磁强计及微机械加速度计和陀螺的姿态估计扩展卡尔曼滤波器[J].中国惯性技术学报,2005,13(2):27-34.

[8] 杨淑洁,曾庆双,伊国兴.低成本无人机姿态测量系统研究[J].传感器与微系统,2012,31(2):15-22.