不同姿態表示方法下的姿態估計分析

魯 鑫，高敬東，李開龍

(海軍工程大學 導航工程系，湖北 武漢 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.