基于间接姿态估计的初始对准方法研究*

薛博阳 常路宾 覃方君

(海军工程大学导航工程系 武汉 430000)

基于间接姿态估计的初始对准方法研究*

薛博阳 常路宾 覃方君

(海军工程大学导航工程系 武汉 430000)

姿态估计初始对准方法在姿态确定初始对准方法基础之上发展而来,其在保证姿态确定对准算法快速性、抗干扰性的基础之上,实现了对陀螺漂移的在线建模估计,因此特别适用于低精度捷联惯导系统。论文在姿态估计对准模型的基础之上,推导了其对应的大失准角欧拉误差方程,简化了滤波算法设计,车载实验验证本文所研究算法的有效性。

姿态估计; 初始对准; 惯性导航; 误差方程

(Navigation Engineering Department, Naval University of Engineering, Wuhan 43000)

Class Number V249.1

1 引言

作为一种航位推算系统,捷联惯导的初始对准精度和快速性直接关系到其后续的导航解算精度及系统机动性能,因此初始对准一直是捷联惯导技术领域研究的重点和难点。传统的初始对准方法一般包括粗对准和精对准两个过程。粗对准利用捷联惯导自身惯性器件输出与地球自转角速度和重力加速度之间的关系直接解析求解出载体姿态。精对准一般采用基于卡尔曼滤波的最优估计方法完成,因此需要粗对准提供较为精确的姿态信息以满足其滤波模型的线性化条件。然而对于基于MEMS的低精度捷联惯导系统,由于其惯性敏感器件的常值漂移一般是几度甚至几十度每小时,在这种条件下,作为解析粗对准必要条件之一的地球自转角速度(15deg/h)被完全湮没在陀螺常值漂移中。因此对于低精度捷联惯导而言,即使在静基座条件下也无法实现有效的解析粗对准。

为了克服外界干扰对解析粗对准的干扰,近年来一些学者提出了一种以惯性系为参考基准进行动基座初始对准的优化对准新方法[1～6]。该方法通过坐标系分解,将初始对准的核心问题转为求解对准初始时刻常值姿态矩阵的问题。优化对准将初始对准问题转化为利用矢量观测进行姿态确定的问题,建立了这两个平行研究数十年问题之间的联系,并将姿态确定问题中一些成熟算法引入到初始时刻常值姿态矩阵的求解中。不可否认,基于姿态确定的优化对准方法能够有效处理外界运动干扰,在中高精度捷联惯导(如激光和光纤捷联惯导)中已得到广泛的实际应用和推广。同时,这种方法能够对外测辅助信息反复利用,极大的提高了初始对准的速度。然而,基于姿态确定的初始对准方法未对惯性器件误差进行建模,因此其本质上仍然是一种解析粗对准方法。正如前文所述,造成传统初始对准方法不能正常完成的一个原因是惯性器件性能较差。因此不难推测,基于姿态确定的优化对准对准方法因其不能估计除姿态以外的任何量而不再适用于基于MEMS低精度捷联惯导系统。

为了将惯性系初始对准方法扩展到低精度系统中,常路宾提出了一种基于姿态估计的初始对准方法。该方法借鉴严恭敏和武元新等人对准方法中的姿态矩阵分解的思想[7～8],将姿态矩阵分解为两部分,一部分是含有初始误差的载体姿态变化矩阵,另一部分是可以借助外界辅助信息进行求解的导航系姿态变化情况。这种姿态矩阵分解方式的最大好处就是可以实现对陀螺漂移的建模估计,因此特别适用于低精度捷联惯导系统的初始对准[9]。车载实验表明,常路宾的方法在估计姿态的同时实现了对陀螺漂移的估计,取得了满意的效果。

基于姿态估计的初始对准方法是针对MEMS低精度捷联惯导系统设计的[ 10],但是该方法直接利用姿态解算方程作为滤波的状态方程,因此计算量很大,对低精度系统而言,不利于其实时应用。针对该问题,本文在姿态估计初始对准方法的基础之上,推导了其对应的误差方程,并设计相应的滤波算法,实验结果表明,该方法在保证姿态估计初始对准方法优势的基础之上,显著降低了算法计算量,从而为其实时应用提供了有利保证。

2 基于姿态估计的初始对准模型

状态模型为

(1)

用于修正的观测方程为

(2)

其中

(3)

(4)

(5)

(6)

微分方程在求解时的初始为:

(7)

3 间接姿态估计初始对准方法

由于各种误差源的影响,实际上捷联惯导的模拟的惯性坐标系与理想惯性坐标系之间存在转动误差,记实际的惯性坐标系为i′系,系先后经过三次转动可得i系,假设三次转动角为ax、ay、az,称它们为欧拉误差角,并且有a=[ax、ay、az],则坐标变换矩阵为

(8)

其中cα=cos(α),sα=sin(α)。将上式中三个矩阵逆序相乘得系i到i′系的变换矩阵。

(9)

(10)

从而得到欧拉误差角微分方程

(11)

其中有

(12)

式(11)欧拉误差角微分方程描述了欧拉角与载体运动间的关系,建立以欧拉误差角a表示的捷联惯性系误差模型。

3.1 惯性系姿态误差方程

(13)

(14)

现在定义姿态矩阵的加性计算误差为

(15)

(16)

(17)

式(16)和(17)相等,得到:

(18)

(19)

(20)

(21)

3.2 基于姿态误差的矢量观测方程

上一小节推导了姿态估计初始对准的误差方程形式,为了完成滤波,仍需要推导对应的观测方程。这里将式(2)的矢量观测方程重写如下

(22)

(23)

(24)

上式即为含有姿态误差状态量的观测方程。

3.3 滤波器设计

基于误差方程的姿态估计初始对准算法示意图如图1所示。

图1 基于误差方程的姿态估计初始对准算法示意图

这种形式的滤波本质上是一种闭环滤波,其基本流程是:首先任意给定一个初始姿态信息,然后根据陀螺的输出进行姿态解算,由于初始姿态中不可避免含有误差,递推求解的每个时刻的姿态也必然存在误差；同时,根据构造的矢量观测同样可以求解一个姿态解算值,利用这两种姿态解算结果的差值即可进行滤波,从而得到姿态解算的误差；通过对该姿态误差的补偿即可获得姿态估计信息。

4 车载实验研究

为了验证本章所研究算法的有效性,设计了基于MEMS惯性测量单元的低精度捷联惯导车载实验。车载导航设备主要包括低精度MEMS惯性测量单元XW-IMU5220和姿态航向参考系统(attitude and heading reference system,AHRS)XW-ADU7612。其中XW-ADU7612是一个由三个高精度闭环光纤陀螺仪、三个硅加速度计和两个GPS天线(XW-ADU3601)构成的组合系统,用来提供车载实验所需的高精度姿态、速度和位置参考。

进行对比试验的算法分别有:文献[9]中的算法,记为USQUE(滤波算法的缩写)；本文所提出的算法UKF(针对非线性模型,本文采用的UKF滤波算法)。实验中我们发现间接滤波对准算法对于特别大的失准角(航向角误差大于100°)效果不是很理想,因此实验中增加一种混合算法,即前5s时间内使用传统算法USQUE,后续时间内采用间接滤波方法UKF,该方法记为USQUE+UKF。具体试验结果如下所示。

图2 不同算法下的横滚角估计值

图3 不同算法下的横滚角误差估计值

图4 不同算法下的纵摇角估计值

图5 不同算法下的纵摇角误差估计值

从图2～7可以看出,整体来看,在较长时间下直接姿态估计算法和间接姿态估计算法都能估计出姿态角,且估计效果与真实参考数据较为一致,达到了较高的预期精度要求,但在短时间内来看,USQUE算法和混合算法相对来说更加精确,稳定性和可靠性都较优于UKF算法,并且,通过图2～7可以看出在整个实验过程中,各种算法下的姿态误差估计值都趋于收敛,相对来看USQUE算法和混合算法的收敛速度要快于UKF算法,UKF算法在个别时间点姿态估计误差偏大,且突变型较强,稳定性差,因此相比来看,USQUE算法和混合算法姿态估计效果较好。

图6 不同算法下的航向角估计值

图7 不同算法下的航向角误差估计值

同时为了进一步分析算法的特点,我们对陀螺偏差进行估计,并计算不同算法下的滤波时间,得到不同算法下的滤波时间和不同方向的陀螺偏差估计值如下。

图8 不同算法下的x-陀螺偏差估计值

由图8～10可以看出,整体来看,不同方向陀螺偏差估计值都呈收敛的趋势,且收敛值大致相同,一定程度上验证了算法的准确性,同时可以看到,从稳定性和精确度来看, USQUE算法最优,混合算法次之,UKF算法较差,尤其在短时间内,UKF算法陀螺偏差估计值振荡较大,同时从图11中可以直观的看出三种算法的滤波时间,其中,UKF算法时间最短,USQUE算法滤波时间最长,混合算法时间居中,且与UKF算法时间较为接近,呈现出较好的算法特点。

图9 不同算法下的y-陀螺偏差估计值

图10 不同算法下的z-陀螺偏差估计值

图11 不同算法下的滤波时间

综上分析,总体来看,本文提出的基于误差方程的姿态估计UKF方法在精度和稳定性上较上一章的姿态估计的USQUE算法有明显不足,但从滤波原理和实验结果来看,它能够有效的缩短滤波时间,简化运算,在快速性初始对准方面呈现巨大的优势,并且从本文提出的一种混合算法效果来看,混合算法能够充分结合USQUE算法和UKF算法的优点,不仅能够在姿态估计中保持良好的的精度和稳定性,并且在滤波过程中能够有效减少运算,提高结算效率。

5 结语

本文研究了基于姿态估计的低精度捷联惯导初始对准方法,针对传统姿态估计初始对准方法计算量较大不利于实时应用的缺陷,本文在姿态估计对准模型基础之上推导了对应的误差模型并设计了相应的滤波算法。根据实际应用效果,本文进一步提出了一种直接和间接姿态估计的混合初始对准算法。试验结果表明本文所研究算法在保证对准精度的前提下有效降低了算法计算量,为姿态估计对准方法走向实用提供了有效途径。

[1] Chang L B,Hu B Q,Li A,et al.Transformed unscented Kalman filter [J].IEEE Transactions on Automatic Control,2013,49(2):1400-1402.

[2] 严恭敏,秦永元,卫育新,等.一种适用于SINS动基座初始对准的新算法[J].系统工程与电子技术,2009,31 (3):634-637.

[3] 严恭敏,翁浚,白亮,等.基于惯性参考系的动基座初始对准与定位导航[J].系统工程与电子技术,2011,33 (3):618-621.

[4] Wu M P,Wu Y X,Hu X P,et al.Optimization-based Alignment for Inertial Navigation Systems:Theory and Algorithm [J].Aerospace Science and Technology,2011,15(1):1-17.

[5] Wu Y X, Zhang H L,Wu M P,et al.Observability of SINS alignment:A global perspective [J].IEEE Transactions on Aerospace and Electronic Systems,2012,48(1):78-102.

[6] Wu Y X,Pan X F.Velocity/position integration formula,Part I:Application to in-flight coarse alignment[J].IEEE Transactions on Aerospace and Electronic Systems,2013,49(2):1006-1023.

[7] Chang L B, Li J S, Li K L. Optimization-based Alignment for Strapdown Inertial Navigation System: Comparison and Extension[J]. IEEE Transactions on Aerospace and Electronic Systems, to be published, 2016. 8.

[8] Chang L B, Li J S, Chen S Y. Initial Alignment by Attitude Estimation for Strapdown Inertial Navigation Systems [J].IEEE Transactions on Instrumentation and Measurement, 2015, 64 (3): 784-794.

[9] 李杨,胡柏青,覃方君,冯国利.MEMS陀螺的抗野值自适应滤波降噪方法[J].压电与声光,2015,37(4):590-594.

[10] 苏瑞祥.船用惯性导航设备虚拟检测系统设计与实现[J].系统实践,2013(7):40.

Initial Alignment Research Based on Attitude Estimation Indirectly

XUE Boyang CHANG Lubin QIN Fangjun

The attitude estimation based initial alignment developed from the attitude determination based initial alignment. Based on insuring the rapidity and anti-interference of attitude determination based initial alignment, it achieved the online modeling estimation for gyroscope drift, which makes it suitable for low-accuracy SINS. Based on the attitude determination based initial alignment, this paper deducted the corresponding large misalignment angle euler error equation and simplified the design of filtering algorithm. And vehicle tests have verified the efficiency of algorithm researched in this paper.

attitude estimation, initial alignment, inertial navigation, error equation

2016年6月10日,

2016年7月30日

薛博阳,男,硕士,研究方向:惯性导航及其应用。常路宾,男,博士,讲师,研究方向:惯性导航及其应用。覃方君,男,博士,副教授,研究方向:惯性导航及其应用。

V249.1

10.3969/j.issn.1672-9730.2016.12.016