一种基于MEMS传感器的全局渐进稳定的姿态估计算法

随着航空技术的发展,为飞控系统提供飞行器准确的姿态信息导航系统已经成为飞机系统的必备模块。然而军事、航空航天、航海等领域多采用高精度的惯性测量元件组成的导航系统,由于高精度的惯性测量元件价格昂贵,无法大量应用于普通民用领域。MEMS即微电子机械系统,是建立在微米/纳米技术基础上的21世纪前沿新型多学科交叉技术。由于微电子机械系统具有体积小、成本低和可靠性高的特点,已经广泛应用于消费类无人机、室内导航、智能穿戴设备、游戏手柄等领域。随着MEMS加工工艺的不断提高和数字信号处理技术的发展,其在姿态测量、空间角速度测量领域具有广泛的应用前景。低成本的航姿参考系统一般由MEMS陀螺仪、加速度计、磁力计组成。但是,低成本MEMS陀螺仪本身的精度不高,而且会随环境因素产生严重的漂移和随机误差。直接使用陀螺仪的数据积分来计算欧拉角会产生随时间不断积累的误差。因此,航姿参考系统通常会辅加速度计和磁力计。加速度计和磁力计通过测量重力加速度和地磁场矢量在机体上的投影来补偿陀螺仪的误差从而提高航姿参考系统的精度。

目前用来设计航姿参考系统的算法主要为互补滤波、扩展卡尔曼滤波和各种非线性状态观测器(NLO)三大类。文献[1]中的互补滤波器采用向量叉乘解算陀螺仪误差,并能够直接用于未校准的传感器。卡尔曼滤波(KF)是一种被广泛应用的最优状态估计算法[2]。基于扩展卡尔曼滤波的航姿参考算法通常将欧拉角对应的四元数以及三轴陀螺仪零偏作为状态变量,以加速度计和磁力计的输出值作为量测。Lefferts等[3]、Crassidis等[4]使用扩展卡尔曼滤波和其他估计方法来设计航姿参考系统。Madgwick等提出了基于四元数的梯度下降算法[5],利用加速度计和磁力计的量测值计算得出姿态欧拉角。文献[6]将最小二乘方法应用于航姿参考系统的设计,取得了不错的效果。

在状态观测器的应用领域,文献[7]证明了刚体的姿态信息可以从特定的量测信息中获取,为状态观测器设计航姿参考系统提供了理论依据。文献[8]提出了TRIAD算法,证明了姿态旋转矩阵可以由2个不平行的向量确定,但是TRIAD算法对噪声十分敏感,因而无法直接使用。文献[9]通过增加权重改进TRIAD算法,但该算法的估计值不能保证最下方差意义下的最优性。文献[10]提出了一种非线性的状态观测器估计出欧拉角和陀螺仪的零偏,并证明了该状态观察器满足李雅普诺夫稳定性,并在实际试飞中验证了算法的可靠性。文献[11]在文献[10]的基础上,将非线性状态观测器与扩展卡尔曼滤波级联,保证了扩展卡尔曼滤波的稳定性,进一步提高了姿态估计的精度。综上所述,基于卡尔曼滤波设计航姿参考系统具有稳态精度高和动态响应速度快等优点。但由于要计算P阵和增益系数K,使得系统的计算量显著增加[12]。而状态观测器虽然能克服卡尔曼滤波计算量大的缺点,但状态观测器在噪声的扰动下可能导致系统状态发散。因此,如何在保证系统的稳定性前提下,设计稳态精度高和动态响应速度快的状态观测器,是基于状态观测器设计航姿参考系统的难点。

针对上述问题,本文提出了一种基于姿态旋转矩阵的全局渐进稳定的状态观测器。针对实际AHRS系统设计的需求,改进了TRIAD算法,用2个不平行的向量来估计姿态旋转矩阵。本文给出了状态观测器的完整推导方法,并证明了在不需要估计陀螺仪零偏的情况下,依然能保证观测器的稳定性。与传统的卡尔曼滤波相比较,由于不用计算矩阵的逆,且只有一个需要调整参数,使得该算法的计算效率远高于卡尔曼滤波器。通过使用实际传感器的数据仿真,并于卡尔曼滤波器的输出比较显示,该状态观测器在系统处于稳态或动态时都能有较高精度的欧拉角输出。

1 符号描述

对于一个向量或矩阵X,X′表示X的转置。对于一个对称正定矩阵A,它的最小的特征值表示为λmin(A)。对于一个向量x∈R3,S(x)表示为以下形式的对称矩阵

操作符‖·‖对于矩阵表示的是矩阵的Frobenius范数,对于向量表示的是向量的Euclidean范数。

2 问题描述

首先建立2个参考坐标系,分别是地球北东地坐标系(NED)和载体坐标系。上标n和b用来区分这2个坐标系。姿态旋转矩阵的微分方程可以表示为

(1)

R表示从系统系到地球坐标系的姿态旋转矩阵,ωb表示载体系上真实的角速度。本文所要解决的问题就是通过已有的传感器输出估计出姿态旋转矩阵R。

2.1 量测方程

假设系统包含6轴IMU和3轴磁力计,这些传感器能提供一下信息:

·陀螺仪的量测满足这里b表示陀螺仪的零偏。

·加速度计的量测满足gn=Rab,这里gn表示在地球坐标系下的重力向量,ab为载体系上加速度计的量测。在本文中,测量条件为质心处于静止或匀速运动的稳态。

·磁力计的量测为满足mn=Rmb,mn表示当地的地磁矢量在地球坐标系下的投影,mb为地磁向量在载体系下的投影。

同时,在ab,mb,和ωb有界的条件下,有以下2点假设:

假设1 陀螺仪的零漂为常值,并且存在一个常数Mb>0,使得‖b‖≤Mb。

假设2 存在一个常数cobs>0,使得对于任意的t>0,都有‖ab×mb‖>cobs。

3 状态观测器的设计

3.1 矢量定姿基本原理

由前文假设可知,加速度计和磁力计的量测满足gn=Rab,mn=Rmb,并且gn,mn不共线,ab,mb不共线。所以进而可以构造以下2个矩阵[18]

(2)

(3)

并且矩阵An,Ab满足以下关系

An=RAb

(4)

这样就可以得到姿态旋转矩阵和量测向量的构成的矩阵的关系

(5)

由于Ab为载体上传感器输出构造的矩阵,所以Ab在系统每次采样的时候都会发生变化。因此,对Ab求逆会显著增加系统的计算量。为此,可以通过适当的调整,使得不需要求矩阵的逆,也能计算得到姿态旋转矩阵。

考虑向量这样通过(2)式计算得到的矩阵An为

(6)

矩阵An的逆为

(7)

由姿态旋转矩阵的性质R-1=R′,这样(2)式可以化简为

R=(AbAn)′

(8)

这样就得到了由量测向量确定的姿态旋转矩阵。

3.2 姿态旋转矩阵的状态观测器

定义如下形式的状态观测器

(9)

式中，Kp是一个正定对称矩阵,J定义为

(10)

Rmeas为上一小节定义的由加速度计和磁力计量测向量确定的姿态旋转矩阵。

定义姿态旋转矩阵的估计误差可以得到误差的微分方程

(11)

接下来证明(11)式表示的系统全局渐进稳定。

证明 注意到

可以将误差的微分方程重写为

(12)

由于An=RAb,进而可以重写为式可重写为

(13)

考虑系统

(14)

构造李亚普诺函数

(15)

在(14)式的条件下,方程(15)的微分为

接下来考虑上式中的每一项。首先考虑第二项

其次考虑第一项,由于为对称矩阵,则所以可以得到

即微分方程(14)表示的系统全局渐进稳定。即对于任意的r>0,存在c(r)>0,使得当时,有同时,由文献[19]可知:可以找到一个李雅普诺夫函数满足

(16)

(17)

(18)

式中，α1,α2,α3为满足严格单调递增且α(0)=0的函数。

接下来证明方程(13)表示的系统全局渐进稳定。对于方程(13)表示的系统,定义李雅普诺夫函数对υ(t)求微分得

(19)

对于任意的X∈R3×3,有将(17)式和(18)式带入(19)式可得

(20)

令将(20)式两端同时乘以e(t-τ0)并整理得

(21)

将(20)式两边积分得

υ(t)e(t-τ0)≤υ(τ0)+α(r)(e(t-τ0)-1),∀t>τ0

(22)

将(22)式两端同时乘以e-(t-τ0)得

υ(t)≤υ(τ0)e-(t-τ0)+α(r)(1-e-(t-τ0)),∀t>τ0

(23)

令τ0=t0,将(16)式代入(23)式得

(24)

所以为严格单调递增且γ(0)=0的函数。由全局渐进稳定的定义可知,(12)式所表示的系统全局渐进稳定。图1为该状态观测器算法完整的流程图。

图1 本文提出的状态观测器流程图

4 实际传感器数据验证与结果分析

4.1 实际传感器数据验证

由于数字仿真环境很难考虑到实际环境的外部因素干扰,本文采用法国SBG公司的Ellipse2-N型组合导航系统提供的数据进行验证。Ellipse2-N是一个小型高精度的惯性导航系统(INS),并集成了GNSS接收机。可以提供俯仰、横滚、航向、升沉和导航数据。这个轻型的传感器包含了一个基于MEMS的惯性测量单元,惯性测量单元中集成了3个陀螺仪、3个加速度计和1个三轴磁力计。该组合导航系统将惯性数据和GNSS、里程计、以及DGPS信息等进行融合,在最具挑战的环境中,可以提供非常优越的姿态和导航数据。可以采集SBG中的MEMS传感器的数据来验证本文提出的算法,将算法输出的欧拉角与Ellipse2-N型组合导航系统输出的欧拉角做对比。

Ellipse2-N组合导航系统的采样频率为50 Hz。采样条件为在室内手持晃动Ellipse2-N组合导航系统。将实际采样数据导入MATLAB环境下进行仿真。考虑滚转角通道,单纯做陀螺仪积分得到的滚转角如图2所示。

图2 Ellipse2-N输出滚转角与直接角速度积分滚转角

由图2可知,陀螺仪的原始数据存在较大的零偏,因此,Ellipse2-N组合导航系统的原始数据能更好地验证本文所提出的状态观测器算法的性能。

本文提出的状态观测器算法的输出值,卡尔曼滤波器的输出值和Ellipse2-N组合导航系统的输出值如图3至5所示。

图3 Ellipse2-N、NLO及卡尔曼滤波输出滚转角 图4 Ellipse2-N、NLO及卡尔曼滤波输出俯仰角 图5 Ellipse2-N、NLO及卡尔曼滤波输出偏航角

状态观测器输出姿态角,卡尔曼滤波器输出姿态角与Ellipse2-N组合导航系统的输出值的误差如图6至8所示。

参考文献[9]将系统处于静态的条件定义为系统的角速度模值小于5°/s,处于动态时角速度模值大于等于5°/s。这个阈值的选取保证了陀螺仪的噪声不会对系统状态的分析产生影响。仿真时间段内陀螺仪角速度模值如图9所示,加速度计输出的模值如图10所示。将关于欧拉角的静、动态均方根RMS误差汇总在表1中。分析表1的数据,可以发现本文所提出的NLO算法较卡尔曼滤波对于三轴欧拉角的动、静态的估计精度具有明显的提升。

图6 Ellipse2-N与NLO及卡尔曼滤波输出滚转角误差 图7 Ellipse2-N与NLO及卡尔曼滤波输出俯仰角误差 图8 Ellipse2-N与NLO及卡尔曼滤波输出偏航角误差

图9 陀螺仪输出角速度的模值

图10 加速度计输出的模值

由MATLAB的仿真结果可以看出,本文提出的状态观测器算法在系统处于稳态的时候有良好的精度,在系统处于动态的时候也能有较快的动态响应速度。系统处于动态时,本文提出的NLO算法输出的姿态角RMS误差在1.5°以内。当系统恢复稳态时,俯仰角和滚转角的RMS误差降低到0.17°以内,偏航角的误差降低到1.1°以内。由此得出结论,本文提出的NLO算法具有较快的收敛速度以及高精度的稳态和动态响应。

表1 卡尔曼滤波和本文提出的NLO的动、静态RMS误差

由于实验室条件下很难获得稳定的常值加速度干扰,因此将系统放置在震动平台上转动,以检验随机加速度干扰对算法的影响。图11和图12显示在震动平台下转动系统时的加速度计输出模值和角速度模值。

图11 陀螺仪输出角速度的模值

图12 加速度计输出的模值

可以观察到相比较前一次仿真,角速度的模值明和干扰加速度噪声明显增大,且角速度模值在仿真的大部分时间段内均大于5°/s,因此可近似认为本次仿真系统处于动态过程。本次数据加速度计输出值的方差为7.853(m/s2)2,在随机加速度噪声干扰下,本文提出的NLO和卡尔曼滤波的输出如图13至15所示。

NLO输出姿态角,卡尔曼滤波器输出姿态角与Ellipse2-N组合导航系统的输出值的误差如图16至18所示。

图13 Ellipse2-N、NLO及卡尔曼滤波输出俯仰角 图14 Ellipse2-N、NLO及卡尔曼滤波输出俯仰角 图15 Ellipse2-N、NLO及卡尔曼滤波输出偏航角

图16 Ellipse2-N、NLO及卡尔曼滤波输出滚转角误差 图17 Ellipse2-N、NLO及卡尔曼滤波输出俯仰角误差 图18 Ellipse2-N、NLO及卡尔曼滤波输出偏航角误差

将系统在加速度噪声干扰下的关于欧拉角的动态均方根RMS误差汇总在表2中。可以看到在加速度噪声干扰下,系统输出欧拉角的误差较第一仿真已有明显增加,但依然保证了较高的精度。与第一仿真时的稳态欧拉角RMS误差比较,本次仿真欧拉角RMS误差平均增大2.430 8°。误差增大的原因是由于当系统处于动态的时候,加速度计的量测为重力加速度与干扰加速度在系统上的投影之和。因此,(8)式表示的求姿态旋转矩阵的方法不再成立。

表2 卡尔曼滤波和本文提出的NLO的动态RMS误差

5 结 论

为了避免扩展卡尔曼滤波计算量过大的问题,本文提出了一种基于状态观测器的航姿参考算法。该状态观测器在不估计陀螺仪零偏的情况下,依然能保证滤波器收敛。通过使用实际数据仿真显示,该滤波器在系统处于稳态和动态的时候都能有较好的表现。因此,本文所提出的算法仅适用于微小型四旋翼无人机,以及手机防抖云台,游戏手柄等在运动过程中加速度和角速度都不大的设备。而对于固定翼无人机,由于其在运动过程中会产生较大的加速度和角速度,本文提出算法不再适用,需要寻找新的观测量来估计载体的姿态。

本文只考虑了系统处于稳态或者加速度不大的动态情况。对于固定翼飞机,在飞机起飞爬升和空中巡航过程中都会产生较大的加速度。本文所提出的状态观测器算法不适用于加速度过大的情况。为了解决飞行器处于大机动时候的姿态估计问题,可以采用GPS的位置和速度信息作为量测来估计飞行器的加速度。因此,如何利用GPS所提供的信息,设计状态观测器来观测载体的加速度值得进一步研究。