基于加速度分离算法的姿态测量方法研究*

王向军,张朝阳,刘 峰

(1.天津大学精密测试技术及仪器国家重点实验室,天津 300072;2.天津大学微光机电系统技术教育部重点实验室,天津 300072)

姿态测量是实现精确惯性制导的核心技术[1,2],合适的姿态测量方法不仅是成像探测器求解视线角速率的基础,而且保证了惯性导航的精度和可靠性[3]。MEMS传感器具有体积小、重量轻、成本低、易集成等众多突出优点,基于MEMS传感器的姿态解算算法,在精确制导和姿态控制领域得到广泛的应用。在MEMS器件中,陀螺仪具有很高的精度,短时间内可以得到可靠的姿态信息,但会产生随时间累积的积分误差;在重力作用下加速度计的低频特性比较稳定,通过与陀螺仪数据融合可以消除陀螺仪的积分误差。然而,加速度计不仅容易受到外部振动干扰产生高频噪声,还会受到运动产生的加速度的影响,特别是在工作状态为变加速运动时,姿态解算精度受到严重影响。

针对加速度计的测量误差对成像探测器姿态测量精度的影响,传统的方法是对加速度计测量数据低通滤波,这些方法有效抑制了振动噪声,但是忽略了运动产生的加速度对姿态解算的影响。文献[4]提出了专家系统判断载体运动状态,降低了运动加速度对姿态解算精度的影响,但对于持续运动的载体会抑制加速度的修正作用,导致姿态解算误差的积累和发散。文献[5]提出了利用GPS修正加速度信息并融合陀螺仪参数完成姿态更新,但对GPS的依赖影响了微小型姿态测量系统的体积和成本。论文[6]提出静止时只采用加速度计进行姿态解算,该方法对于间歇性运动的载体如机器人姿态测量有很好的效果,对加速运动下的载体效果有限。论文[7]提出利用空速计对加速度计测量数据进行补偿,由于空速计精度的限制,导致最终姿态解算精度有限。

为解决以上问题,本文提出了针对成像探测器飞行规律的运动加速度分离算法,并根据此算法实现了基于卡尔曼滤波器的姿态解算。最后,在自主研制的集成有姿态测量模块的微型成像探测器上,进行姿态测量方法的可行性分析和精度评估。飞行仿真实验结果表明,该算法可以有效的分离出运动加速度,大幅提高了变加速运动状态下姿态测量精度和抗干扰能力。

1 姿态测量系统的总体结构

姿态测量系统以集成电路的形式嵌入在微型成像探测器中。系统可以分为信息采集、数据处理以及结果输出三部分。由加速度计和陀螺仪进行信息采集并提供初始参数,MCU实现对MEMS传感器数据的误差补偿和姿态解算,并以欧拉角的形式通过无线传输输出姿态信息。姿态测量系统的总体结构框图如1所示。

图1 姿态测量系统结构框图

2 椭球拟合算法补偿加速度计误差

由于MEMS传感器的安装误差、垂直误差以及量测误差等因素,需要对MEMS传感器进行误差标定补偿。陀螺仪短时间精度较高,直接采用简单的建模分析法即可取得较好效果[8],本文主要介绍利用椭球拟合法对加速度计采样数据进行误差补偿。该方法利用重力场完成加速度计的误差标定,稳定的重力矢量场和大量的采样数据保证了标定结果具有较高的精度,而且不需要借助外部设备或基准,具有简便易行等优点[9-11]。

2.1 椭球拟合算法的原理

当加速度计静止时,由于只受重力作用,各轴上的测量参数在空间坐标系上理想分布为球心在原点、半径为g的球面上。由于加速度计的刻度误差和轴间偏置误差等因素影响,加速度计所测数据会分布在近似椭球的表面上。在各种姿态下采样加速度计的测量数据并进行基于最小二乘法的椭球拟合,求出球心偏置位置和椭球的各个轴长,即可得到测量值与理想值之间的对应关系,实现误差补偿。椭球最小二乘拟合算法的采样点空间分布如图2所示。

图2 椭球拟合算法的采样点空间分布

椭球最小二乘拟合法如式(1)所示:

a1x2+a2y2+a3z2+a4xy+a5xz+a6yz+a7x+a8y+a9z=1

(1)

式中(x,y,z)表示加速度计测量数据,a1～9表示待估计值。为表示方便令:

(m1,m2,m3,m4,m5,m6,m7,m8,m9)=

(x2,y2,z2,xy,xz,yz,x,y,z)

(2)

对于多组测量数据,mij表示第i组的mj,由于每一组测量数据都有测量误差,故有相应的测量残差方程组如式(3)所示:

(3)

按照最小二乘法原理,待求的aj应满足残差平和方最小,得到式(4):

(4)

分别对aj求偏导,且令其为零推导得正规方程组:

(5)

式中:mi表示列向量[m1i,m2i…mni]T,[mimj]表示两列向量的内积。根据正规方程组可以直接得出待估计值a1～a9。

2.2 椭球拟合法精度分析

在不同姿态下进行加速度计采样,得到使用椭球最小二乘拟合算法前后的加速度计初始误差曲线图,如图3所示。

图3 使用椭球拟合法前后初始误差曲线

比较椭球拟合法前后的初始误差数据可以发现,使用椭球拟合法可以将加速度计初始误差由0.2 m/s2降低到0.05 m/s2以内,说明该算法可以有效对加速度计测量数据进行误差补偿,提高了加速度计测量数据的初始精度。

3 自适应运动加速度分离算法

在微小型姿态测量系统中,为保证精度 MEMS传感器的坐标系与载体坐标系的对应轴平行或垂直。当成像探测器高速飞行时,沿着前进的方向(横滚轴)受运动影响会产生明显变化的加速度,探测器的前进方向上产生的运动加速度仅对加速度计的特定轴有影响。载体坐标系和MEMS传感器坐标系的相对位置关系如图4所示。

图4 载体坐标系和MEMS传感器坐标系

其中XZY表示载体坐标系,xyz表示MPU6050安装坐标系。从图中可以看到,沿载体坐标系的X轴运动,会在加速度计坐标系的z轴中产生运动加速度分量,而不会影响其他两轴的加速度分量。利用重力加速度恒定这一条件,可以由其他两轴上的重力加速度分量推导出运动方向上的重力加速度分量,进一步计算出运动加速度,实现加速度分离和姿态信息的解算。

在自适应加速度分离算法中,首先设定一个加速度阈值,假如加速度计测得数据与重力加速度的相差超出该阈值,表示运动产生的加速度对加速度计测量数据产生了一定的影响,此时进行加速度计分离算法。当运动产生的加速度影响较小时,判定条件为:

(6)

阈值ε根据载体的运动情况而定。加速度转换关系由载体坐标系XYZ和加速度计传感器坐标系xyz的相对位置关系而定,根据转换矩阵直接求出载体坐标系的三轴加速度对陀螺仪参数进行校准。以图4中建立的坐标系为例,加速度转换公式为:

(7)

当运动产生的加速度影响较大时,需要加速度分离算法分离出重力加速度分量与陀螺仪参数进行融合进行校准。由探测器的飞行规律可知,运动加速度主要作用于载体的前进方向X,导致加速度计传感器的z轴方向测量数据出现偏差,而加速度计的x轴和y轴和运动方向垂直,测量值为重力加速度的分量且不受运动影响。如式(8)所示,利用x轴和y轴的加速度计测量数据可以对z轴的重力加速度分量进行估计,获得重力加速度三轴分量,并可进一步获得运动加速度。式中aX的方向可以结合当前姿态角获得。

(8)

自适应加速度分离算法可以保证探测器在变加速运动状态下,能采用有效的加速度数据作为观测矩阵进行卡尔曼滤波,保证了姿态测量系统的精度和鲁棒性。

4 基于四元数的扩展卡尔曼滤波

姿态解算的方法主要有欧拉角法、方向余弦法和四元数法[12]。欧拉角法只需求解3个微分方程,具有描述直观、易于实现等优点,但是姿态矩阵会出现奇异点,导致万向节死锁现象,不能对目标进行全姿态解算。方向余弦法需要对运动目标的9个参数进行解算,计算量较大,因此实际中很少应用。四元数法可以表示刚体的定轴转动,避免了欧拉角万向节死锁现象,而且与方向余弦法相比计算量更小,所以得到广泛应用。在姿态信息融合的过程中需要使用滤波器保证姿态解算的精度,常用的滤波器有互补滤波器和卡尔曼滤波器及其改进算法。互补滤波器具有结构简单和实时性强等优点,但精度不如卡尔曼滤波器[13,14]。卡尔曼滤波器具有精度高、追踪效果好以及抗噪声能力强等优点,扩展卡尔曼滤波推广了卡尔曼滤波器在非线性领域的应用。随着人们对MEMS传感器姿态解算精度要求的提高,以及芯片性能的增加,卡尔曼滤波器的重要性越来越大。

本文基于四元数姿态表示,使用陀螺仪角速度更新四元数作为卡尔曼滤波的状态矩阵,结合自适应加速度分离算法得到的重力加速度分量作为卡尔曼滤波的观测矩阵,通过改进的扩展卡尔曼滤波器,对运动状态下的微惯性导航系统进行高精度姿态解算。四元数可以通过陀螺仪参数直接进行更新,四元数更新的微分方程为:

(9)

式中:ωxyz表示经过误差补偿后的陀螺仪三轴角速度,q0～3表示四元数。对四元数进行实时更新,并根据四元数和欧拉角的旋转矩阵对应关系,可以获得实时欧拉角信息。

卡尔曼滤波过程的迭代公式分别为状态矩阵的预测、协方差预测、滤波增益方程、状态矩阵更新方程以及协方差更新方程,如式(10)所示:

(10)

式中:状态量X为四元数向量,Ak-1表示状态转移矩阵,由四元数微分方程的雅可比矩阵确定;Qk-1为观测量的协方差矩阵,可以对加速度计采样信号分析得出;P为协方差矩阵,具有自收敛性,K为卡尔曼增益;Hk为观测矩阵,由重力加速度g通过四元数旋转矩阵在加速度计三轴上的投影和加速度计测量数据关系确定。

5 飞行试验仿真及结果分析

基于自主研制的微小型成像探测器,验证上述惯性导航系统的有效性。该探测器目前集成了图像采集、目标智能识别跟踪以及姿态测量等多个模块。在姿态测量系统中,主控芯片为采用C8051F383,该芯片尺寸微小,主频达到24 MHz,能够完成姿态解算、数据通信以及视线角速率的计算等功能。姿态芯片采用MPU6050,包含三轴加速度和三轴陀螺仪,在必要时可以外接磁力计进行辅助校准。仿真实验以SBG公司的高精度AHRS器件Ellipse2-A作为姿态参考器件,俯仰和横滚角精度达到0.1°,航向角精度达到0.8°,输出速率达到200 Hz,而且具有低噪声的加速度计和陀螺仪,对运动加速度有很好的抗干扰能力。图5为探测器姿态测量模块和Ellipse2-A实物图。

图5 姿态测量模块和AHRS Ellipse2-A

图6 普通卡尔曼滤波姿态误差曲线

将探测器和Ellipse2-A固定在一起,进行姿态解算精度的分析和比较。通过串口将各自的姿态解算结果发送到上位机,使用MATLAB搭建姿态解算评估系统上位机,不仅可以保证探测器和Ellipse2-A同时采样,还可以对姿态解算结果和精度进行直观的实时显示和即时评价。

为保证仿真实验时探测器的运动规律和实际工作时相同,对探测器施加外力使其沿飞行方向(横滚轴方向)做变加速往返运动,比较不同姿态解算算法对探测器姿态解算精度的影响。如图6为存在一定运动加速度情况下,未使用加速度分离算法,仅使用普通卡尔曼滤波进行姿态解算的三轴姿态角误差曲线。

从图6中可以发现,当探测器沿横滚角方向做变加速运动时,运动加速度对探测器姿态角中的俯仰角有很大影响。

图7为在相同的运动加速度情况下,应用本文提出的基于加速度分离算法的改进卡尔曼滤波进行姿态解算的姿态误差曲线。

图7 应用加速度分离算法的姿态误差曲线

从图7中可以发现,本文算法在横滚角和偏航角解算上和原算法接近,保留了原有算法的精度,横滚角和航向角误差在±1°以内;在最易受到运动加速度影响的俯仰角方面,该算法改善效果明显,最大误差从±4°减小到±1°以内,误差降低了70%以上。本文提出的基于加速度分离算法的姿态测量系统可以有效的抑制运动加速度对姿态解算的影响,提高了运动状态下姿态解算的精度。

6 结束语

随着人们对姿态测量系统高精度和小型化的期待,基于MEMS传感器的微小型姿态测量系统及相应姿态解算算法成为重要研究方向。传统的姿态测量系统只是在静止情况下有较好的效果,针对运动条件下的姿态解算往往只能舍弃加速度计数据或降低其信任度,并没有从根本上实现运动状态下的数据有效融合。本文首先对MEMS传感器建立误差模型,然后针对成像探测器飞行规律提出了运动状态下加速度分离算法,结合扩展卡尔曼滤波实现了高精度姿态解算,并在自主设计的微小型成像探测器上进行模拟飞行试验。实验结果表明,该姿态测量系统在运动状况下可以抑制运动加速度的干扰,对比未使用加速度分离算法精度得到大幅提高,该姿态测量方法满足了设计要求。