魯 鑫，高敬東，李開龍

(海軍工程大學(xué) 導(dǎo)航工程系，湖北 武漢 430000)

0 引 言

對于基于捷聯(lián)慣導(dǎo)系統(tǒng)的組合導(dǎo)航來說[1-2]，關(guān)鍵是構(gòu)建姿態(tài)轉(zhuǎn)換矩陣，即所謂的“數(shù)字平臺”。而構(gòu)建精準(zhǔn)、高效的姿態(tài)轉(zhuǎn)換矩陣的前提就是有效的姿態(tài)估計(jì)算法[3-4]。從理論上講，姿態(tài)估計(jì)算法的提出主要基于2個(gè)方面，即信息融合技術(shù)和姿態(tài)表示方法[5]。在信息融合技術(shù)方面，隨著現(xiàn)代濾波技術(shù)和組合導(dǎo)航模式的發(fā)展，使得高精度、快速的姿態(tài)估計(jì)成為可能，其主要成就體現(xiàn)為Kalman濾波技術(shù)的不斷革新與發(fā)展，涌現(xiàn)出了線性Kalman濾波（KF），擴(kuò)展Kalman濾波（EKF），無味Kalman濾波（UKF）以及粒子濾波（PF）等濾波算法，極大地推動(dòng)了組合導(dǎo)航姿態(tài)估計(jì)領(lǐng)域研究的發(fā)展。另外，隨著衛(wèi)星姿態(tài)控制、航天器交會(huì)對接等問題的提出，對于姿態(tài)估計(jì)算法的性能要求更高，尤其體現(xiàn)為精度更高而計(jì)算量更小的高性能姿態(tài)估計(jì)算法的提出，將姿態(tài)估計(jì)從“姿態(tài)確定”階段（即只對姿態(tài)單參量估計(jì)）推向了“姿態(tài)估計(jì)”階段（即實(shí)現(xiàn)了導(dǎo)航全參量的估計(jì)）。在這方面，Crassidis、Markley等提出了乘性EKF濾波算法（MEKF）、四元數(shù)無味估計(jì)器（USQUE），Choukroun等提出新型四元數(shù)姿態(tài)估計(jì)濾波算法[6-7]，以上算法采用的姿態(tài)參數(shù)表示均是采用四元數(shù)。而Karlgaard等于2009年提出的修正羅德里格斯參數(shù)（MRP）姿態(tài)估計(jì)算法，解決了MRP奇異值問題，相比于基于四元數(shù)的姿態(tài)估計(jì)算法來說計(jì)算量更小。后續(xù)研究中，針對以上算法在精度、穩(wěn)定性和計(jì)算量等方面，學(xué)者們也進(jìn)行了比較深入的研究[8,9]。

然而，從姿態(tài)表達(dá)角度將多種姿態(tài)估計(jì)算法比較分析的研究卻相對缺乏，因此本文重點(diǎn)開展了不同姿態(tài)方法下的姿態(tài)估計(jì)分析的研究。對四元數(shù)無味估計(jì)器、基于修正羅德里格斯參數(shù)和高階羅德里格斯參數(shù)所構(gòu)成的姿態(tài)估計(jì)算法進(jìn)行研究分析，比較三者在計(jì)算量和精度的各方面的差異。

1 基于USQUE的姿態(tài)估計(jì)算法研究

假設(shè)一個(gè)非線性離散系統(tǒng)模型為

下面結(jié)合直接式，簡單推導(dǎo)USQUE的計(jì)算流程。在USQUE算法中采用廣義誤差Rodrigues參數(shù)作為局部變量，而乘性誤差四元數(shù)作為中間變量，定義誤差四元數(shù)的廣義誤差Rodrigues參數(shù)形式表示為

時(shí)間更新

首先根據(jù)上步狀態(tài)估計(jì)量和濾波方差矩陣計(jì)算相應(yīng)的Sigma點(diǎn)

為了計(jì)算狀態(tài)預(yù)測均值和濾波方差，根據(jù)乘性四元素公式得

則狀態(tài)預(yù)測均值和濾波方差分別為

量測更新

采用松組合模式，量測方程是線性的，直接采用線性Kalman濾波量測更新過程進(jìn)行。寫出相應(yīng)的量測轉(zhuǎn)移矩陣形式，即

由于量測方程是線性的，因此量測更新直接采用線性Kalman濾波量測更新過程

姿態(tài)更新

其中

則四元數(shù)的更新根據(jù)乘性四元數(shù)公式得

2 基于MRP和HOMRP的姿態(tài)估計(jì)算法研究

Rodrigues參數(shù)族本質(zhì)上是四元數(shù)在三維超平面上的投影。Rodrigues參數(shù)族并不存在四元數(shù)在非線性濾波中的約束問題，同時(shí)， Rodrigues參數(shù)少了一個(gè)冗余的標(biāo)量，所以在理論上Rodrigues參數(shù)族要比四元數(shù)在姿態(tài)表示上的計(jì)算量更小，但存在著奇異性問題。Rodrigues參數(shù)族統(tǒng)一可表示為

相比四元數(shù)在直接式中姿態(tài)更新方程，Rodrigues參數(shù)族的姿態(tài)更新方程只有3個(gè)參數(shù)表達(dá)量，表達(dá)形式更簡單?；贛RP的姿態(tài)更新方程如下所示：

MRP姿態(tài)更新方程

其姿態(tài)轉(zhuǎn)移矩陣表示式為

將以上基于MRP的姿態(tài)更新方程構(gòu)建入直接式中，即可實(shí)現(xiàn)基于MRP的直接式姿態(tài)的估計(jì)。

對于基于HOMRP的姿態(tài)估計(jì)算法，本文重點(diǎn)研究4階HOMRP，即

由于HOMRP定義的特殊形式，一般的三角函數(shù)無法對其進(jìn)行分解表示，采用Cayley變換的數(shù)學(xué)手段將HOMRP與其他姿態(tài)表示方法聯(lián)系起來，以下為HOMRP與MRP和四元數(shù)姿態(tài)切換關(guān)系：

與MRP的關(guān)系：

與四元數(shù)的關(guān)系：

式（24）中的分母中含加減，一般取正，因此，用HOMRP表示四元數(shù)為

HOMRP姿態(tài)更新方程

3 基于MRP、HOMRP和USQUE的姿態(tài)估計(jì)結(jié)果分析

采用標(biāo)準(zhǔn)的基于捷聯(lián)慣導(dǎo)的狀態(tài)方程為組合導(dǎo)航模型，比較USQUE、MRP和HOMRP的姿態(tài)估計(jì)的效果。仿真真實(shí)運(yùn)動(dòng)軌跡的姿態(tài)和位置信息，仿真周期為0.1 s，運(yùn)動(dòng)軌跡的仿真時(shí)間T=1 050 s。

初始化參數(shù)為

圖1為假設(shè)運(yùn)動(dòng)軌跡，3種姿態(tài)參數(shù)比較結(jié)果如圖2～圖4所示。

圖1 仿真軌跡Fig.1 Simulation of trajectory

圖2～圖4主要從姿態(tài)、速度、位置比較了基于USQUE、MRP和HOMRP的姿態(tài)估計(jì)效果。無論是在姿態(tài)、速度還是位置估計(jì)，都是呈漸進(jìn)穩(wěn)定的趨勢，并且走勢基本相同。在姿態(tài)估計(jì)結(jié)果中，USQUE前期震蕩明顯小于HOMRP和MRP，但同時(shí)達(dá)到穩(wěn)定。在速度和位置估計(jì)結(jié)果中，USQUE的整體震蕩也較小，但是收斂時(shí)間較長。為了給大家更直接的感覺，更為清晰的分辨出三者間的效果以及優(yōu)缺點(diǎn)，下面將其以數(shù)值的形式呈現(xiàn)。表格中數(shù)據(jù)為算數(shù)平均數(shù)。

圖2 姿態(tài)估計(jì)比較結(jié)果Fig.2 Comparison results of pose estimation

圖3 速度估計(jì)比較結(jié)果Fig.3 Comparison results of velocity estimation

圖4 位置估計(jì)比較結(jié)果Fig.4 Comparison results of position estimation

從表可以看出USQUE在姿態(tài)、速度以及經(jīng)度、高度的均值均小于MRP和HOMRP，證明其在這些方面的估計(jì)效果較好，但是時(shí)間在三者中最長。同時(shí)可以看出在姿態(tài)的結(jié)果估計(jì)中HOMRP的均值要明顯小于MRP，但是在速度和位置方面MRP有明顯小于HOMRP，可以得到在對姿態(tài)的估計(jì)HOMRP的效果要好于MRP，而速度和位置方面MRP則要好于HOMRP，三者間HOMRP所用時(shí)間最短。結(jié)果表明基于USQUE的姿態(tài)估計(jì)算法的估計(jì)精度最高，同時(shí)具有全局非奇異性，而基于MRP和HOMRP的姿態(tài)估計(jì)算法需要設(shè)計(jì)奇異值避免方法，但計(jì)算量要明顯小于四元數(shù)。

表1 HOMRP下估計(jì)結(jié)果均值Tab.1 Mean value of the estimated results under HOMRP

表2 MRP下估計(jì)結(jié)果均值Tab.2 Mean value of the estimated results under MRP

表3 USQUE下估計(jì)結(jié)果均值Tab.3 Mean value of the estimated results under USQUE

4 結(jié) 語

本文重點(diǎn)針對四元數(shù)無味估計(jì)器、基于修正羅德里格斯參數(shù)和高階羅德里格斯參數(shù)所構(gòu)成的姿態(tài)估計(jì)算法進(jìn)行理論推導(dǎo)與性能比較分析。結(jié)果表明四元數(shù)無味估計(jì)器精度最優(yōu)，但計(jì)算量較大，而基于高階羅德里格斯參數(shù)的姿態(tài)估計(jì)算法計(jì)算量最小，相對精度較高。目的是為了在不同環(huán)境要求下選擇最優(yōu)算法，從而獲得更高效益。

[1]李文魁, 高敬東, 陳永冰, 等.艦船綜合導(dǎo)航系統(tǒng)[M].武漢:海軍工程大學(xué), 2015.11.

[2]李增科, 王堅(jiān), 高井祥.精密單點(diǎn)定位在GPS/INS組合導(dǎo)航中的應(yīng)用[J].武漢大學(xué)學(xué)報(bào): 信息科學(xué)版, 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組合導(dǎo)航聯(lián)邦濾波算法[J].中國慣性技術(shù)學(xué)報(bào), 2013, 21(2): 226–230.

[5]周琪.大飛機(jī)全球慣性導(dǎo)航算法研究[D].西安: 西北工業(yè)大學(xué), 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濾波及其在捷聯(lián)慣導(dǎo)中的應(yīng)用研究[D].武漢: 海軍工程大學(xué), 2014.