不同姿态表示方法下的姿态估计分析

鲁 鑫，高敬东，李开龙

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

0 引 言

对于基于捷联惯导系统的组合导航来说[1-2]，关键是构建姿态转换矩阵，即所谓的“数字平台”。而构建精准、高效的姿态转换矩阵的前提就是有效的姿态估计算法[3-4]。从理论上讲，姿态估计算法的提出主要基于2个方面，即信息融合技术和姿态表示方法[5]。在信息融合技术方面，随着现代滤波技术和组合导航模式的发展，使得高精度、快速的姿态估计成为可能，其主要成就体现为Kalman滤波技术的不断革新与发展，涌现出了线性Kalman滤波（KF），扩展Kalman滤波（EKF），无味Kalman滤波（UKF）以及粒子滤波（PF）等滤波算法，极大地推动了组合导航姿态估计领域研究的发展。另外，随着卫星姿态控制、航天器交会对接等问题的提出，对于姿态估计算法的性能要求更高，尤其体现为精度更高而计算量更小的高性能姿态估计算法的提出，将姿态估计从“姿态确定”阶段（即只对姿态单参量估计）推向了“姿态估计”阶段（即实现了导航全参量的估计）。在这方面，Crassidis、Markley等提出了乘性EKF滤波算法（MEKF）、四元数无味估计器（USQUE），Choukroun等提出新型四元数姿态估计滤波算法[6-7]，以上算法采用的姿态参数表示均是采用四元数。而Karlgaard等于2009年提出的修正罗德里格斯参数（MRP）姿态估计算法，解决了MRP奇异值问题，相比于基于四元数的姿态估计算法来说计算量更小。后续研究中，针对以上算法在精度、稳定性和计算量等方面，学者们也进行了比较深入的研究[8,9]。

然而，从姿态表达角度将多种姿态估计算法比较分析的研究却相对缺乏，因此本文重点开展了不同姿态方法下的姿态估计分析的研究。对四元数无味估计器、基于修正罗德里格斯参数和高阶罗德里格斯参数所构成的姿态估计算法进行研究分析，比较三者在计算量和精度的各方面的差异。

1 基于USQUE的姿态估计算法研究

假设一个非线性离散系统模型为

下面结合直接式，简单推导USQUE的计算流程。在USQUE算法中采用广义误差Rodrigues参数作为局部变量，而乘性误差四元数作为中间变量，定义误差四元数的广义误差Rodrigues参数形式表示为

时间更新

首先根据上步状态估计量和滤波方差矩阵计算相应的Sigma点

为了计算状态预测均值和滤波方差，根据乘性四元素公式得

则状态预测均值和滤波方差分别为

量测更新

采用松组合模式，量测方程是线性的，直接采用线性Kalman滤波量测更新过程进行。写出相应的量测转移矩阵形式，即

由于量测方程是线性的，因此量测更新直接采用线性Kalman滤波量测更新过程

姿态更新

其中

则四元数的更新根据乘性四元数公式得

2 基于MRP和HOMRP的姿态估计算法研究

Rodrigues参数族本质上是四元数在三维超平面上的投影。Rodrigues参数族并不存在四元数在非线性滤波中的约束问题，同时， Rodrigues参数少了一个冗余的标量，所以在理论上Rodrigues参数族要比四元数在姿态表示上的计算量更小，但存在着奇异性问题。Rodrigues参数族统一可表示为

相比四元数在直接式中姿态更新方程，Rodrigues参数族的姿态更新方程只有3个参数表达量，表达形式更简单。基于MRP的姿态更新方程如下所示：

MRP姿态更新方程

其姿态转移矩阵表示式为

将以上基于MRP的姿态更新方程构建入直接式中，即可实现基于MRP的直接式姿态的估计。

对于基于HOMRP的姿态估计算法，本文重点研究4阶HOMRP，即

由于HOMRP定义的特殊形式，一般的三角函数无法对其进行分解表示，采用Cayley变换的数学手段将HOMRP与其他姿态表示方法联系起来，以下为HOMRP与MRP和四元数姿态切换关系：

与MRP的关系：

与四元数的关系：

式（24）中的分母中含加减，一般取正，因此，用HOMRP表示四元数为

HOMRP姿态更新方程

3 基于MRP、HOMRP和USQUE的姿态估计结果分析

采用标准的基于捷联惯导的状态方程为组合导航模型，比较USQUE、MRP和HOMRP的姿态估计的效果。仿真真实运动轨迹的姿态和位置信息，仿真周期为0.1 s，运动轨迹的仿真时间T=1 050 s。

初始化参数为

图1为假设运动轨迹，3种姿态参数比较结果如图2～图4所示。

图1 仿真轨迹Fig.1 Simulation of trajectory

图2～图4主要从姿态、速度、位置比较了基于USQUE、MRP和HOMRP的姿态估计效果。无论是在姿态、速度还是位置估计，都是呈渐进稳定的趋势，并且走势基本相同。在姿态估计结果中，USQUE前期震荡明显小于HOMRP和MRP，但同时达到稳定。在速度和位置估计结果中，USQUE的整体震荡也较小，但是收敛时间较长。为了给大家更直接的感觉，更为清晰的分辨出三者间的效果以及优缺点，下面将其以数值的形式呈现。表格中数据为算数平均数。

图2 姿态估计比较结果Fig.2 Comparison results of pose estimation

图3 速度估计比较结果Fig.3 Comparison results of velocity estimation

图4 位置估计比较结果Fig.4 Comparison results of position estimation

从表可以看出USQUE在姿态、速度以及经度、高度的均值均小于MRP和HOMRP，证明其在这些方面的估计效果较好，但是时间在三者中最长。同时可以看出在姿态的结果估计中HOMRP的均值要明显小于MRP，但是在速度和位置方面MRP有明显小于HOMRP，可以得到在对姿态的估计HOMRP的效果要好于MRP，而速度和位置方面MRP则要好于HOMRP，三者间HOMRP所用时间最短。结果表明基于USQUE的姿态估计算法的估计精度最高，同时具有全局非奇异性，而基于MRP和HOMRP的姿态估计算法需要设计奇异值避免方法，但计算量要明显小于四元数。

表1 HOMRP下估计结果均值Tab.1 Mean value of the estimated results under HOMRP

表2 MRP下估计结果均值Tab.2 Mean value of the estimated results under MRP

表3 USQUE下估计结果均值Tab.3 Mean value of the estimated results under USQUE

4 结 语

本文重点针对四元数无味估计器、基于修正罗德里格斯参数和高阶罗德里格斯参数所构成的姿态估计算法进行理论推导与性能比较分析。结果表明四元数无味估计器精度最优，但计算量较大，而基于高阶罗德里格斯参数的姿态估计算法计算量最小，相对精度较高。目的是为了在不同环境要求下选择最优算法，从而获得更高效益。

[1]李文魁, 高敬东, 陈永冰, 等.舰船综合导航系统[M].武汉:海军工程大学, 2015.11.

[2]李增科, 王坚, 高井祥.精密单点定位在GPS/INS组合导航中的应用[J].武汉大学学报: 信息科学版, 2013(1): 48–51.

[3]MARKLEY F L, CRASSIDIS J L.Fundamentals of spacecraft attitude determination and control[M].Springer New York,2014.

[4]张科, 刘海鹏, 李恒年, 等.SINS/GPS/CNS组合导航联邦滤波算法[J].中国惯性技术学报, 2013, 21(2): 226–230.

[5]周琪.大飞机全球惯性导航算法研究[D].西安: 西北工业大学, 2013.

[6]CHANG L, HU B, LI Y.Backtracking integration for fast attitude determination-based initial alignment[J].IEEE Transactions on Instrumentation and Measurement, 2015,64(3): 795–803.

[7]CHANG L, LI J, CHEN S.Initial alignment by attitude estimation for strapdown inertial navigation systems[J].IEEE Transactions on Instrumentation and Measurement, 2015,64(3): 784–794.

[8]TANG X, LIU Z, ZHANG J.Square-root quaternion cubature Kalman filtering for spacecraft attitude estimation[J].Acta Astronautica, 2012, 76: 84–94.

[9]O′KEEFE S A, SCHAUB H.Shadow set considerations for modified rodrigues parameter attitude filtering[J].Journal of Guidance, Control, and Dynamics, 2014, 37(6): 2030–2035.

[10]BITTANTI S, SAVARESI S M.On the parametrization and design of an extended Kalman filter frequency tracker[J].IEEE Transactions on Automatic Control, 2000, 45(9): 1718–1724.

[11]KIM K H, LEE J G, PARK C G, Adaptive two-stage extended Kalman filter for a fault-tolerant INS-GPS loosely coupled system [J].IEEE Transactions on Aerospace and Electronic Systems, 2009, 45(1): 125–137.

[12]常路宾.Unscented Kalman滤波及其在捷联惯导中的应用研究[D].武汉: 海军工程大学, 2014.