基于姿態(tài)參數(shù)切換的四元數(shù)快速傳遞對(duì)準(zhǔn)算法

周衛(wèi)東， 吉宇人， 喬相偉

(哈爾濱工程大學(xué)自動(dòng)化學(xué)院，黑龍江哈爾濱150001)

0 引言

Kain J E提出的速度+姿態(tài)匹配算法是目前最為常用的傳遞對(duì)準(zhǔn)算法之一［1］，其推導(dǎo)的快速傳遞對(duì)準(zhǔn)模型在小角條件下進(jìn)行了合理的線性化處理。在艦載武器的傳遞對(duì)準(zhǔn)中，粗對(duì)準(zhǔn)階段完成之后，主子慣導(dǎo)間的相對(duì)誤差依然較大，無法進(jìn)行線性簡(jiǎn)化(比如艦載機(jī)與載艦的方位失準(zhǔn)角往往為一個(gè)大角)。由于歐拉角法具有簡(jiǎn)單、直觀的幾何意義，因此基于歐拉角的非線性誤差模型得到了較多的應(yīng)用［2］。但在大角度的姿態(tài)描述中，歐拉角法會(huì)受到奇異性及超越函數(shù)的限制。與此相比，四元數(shù)法具有非奇異性、計(jì)算量小、對(duì)大角度旋轉(zhuǎn)描述更精確等優(yōu)點(diǎn)，因此被越來越多地應(yīng)用于傳遞對(duì)準(zhǔn)研究中［3－4］。同時(shí)，建立四元數(shù)誤差模型也可使濾波器直接采用慣導(dǎo)設(shè)備即時(shí)修正得到的姿態(tài)四元數(shù)，因此精確度更高。

與之對(duì)應(yīng)的非線性四元數(shù)濾波算法主要有四元數(shù)擴(kuò)展卡爾曼濾波算法(quaternion extended Kalman filter，QEKF)和四元數(shù)無跡卡爾曼濾波(quaternion unsented Kalman filter，QUKF)。文獻(xiàn)［5］將 EKF 算法與四元數(shù)結(jié)合，利用誤差四元數(shù)的量測(cè)更新保證后驗(yàn)姿態(tài)四元數(shù)的規(guī)范性。但EKF算法存在高階截?cái)嗾`差及雅克比矩陣求解的限制，而基于sigma點(diǎn)選取的UKF算法在保持與EKF計(jì)算量相當(dāng)?shù)耐瑫r(shí)，其濾波精確度達(dá)到泰勒展開式的二階［6］，因此將其與四元數(shù)結(jié)合可在保證實(shí)時(shí)性的前提下，進(jìn)一步提高濾波精確度［7－11］。但在四元數(shù)UKF算法中，需要解決四元數(shù)sigma點(diǎn)選取及加權(quán)求和的問題。針對(duì)前一個(gè)問題，文獻(xiàn)［8］提出取四元數(shù)向量部分參與sigma點(diǎn)計(jì)算，再通過單位化約束得到標(biāo)量的方法。而針對(duì)加權(quán)求和問題，文獻(xiàn)［9］提出構(gòu)建代價(jià)函數(shù)的方法，文獻(xiàn)［10］通過奇異值分解進(jìn)行計(jì)算，但都受到計(jì)算量較大的限制。對(duì)此，文獻(xiàn)［11］提出了利用修正羅德里格斯參數(shù)(modified rodriguez parameters，MRPs)與四元數(shù)相互轉(zhuǎn)換以保證四元數(shù)規(guī)范性的方法?；谠撍悸?，將兩種參數(shù)間的映射關(guān)系引入四元數(shù)UKF算法中，利用MRPS較低的轉(zhuǎn)換復(fù)雜度及其不受單位化限制的特性，解決四元數(shù)UKF算法中的加權(quán)求和問題，從而使其計(jì)算量較其他四元數(shù)UKF算法更低。

針對(duì)艦載武器中大失準(zhǔn)角的快速傳遞對(duì)準(zhǔn)問題，在建立四元數(shù)誤差模型的基礎(chǔ)上，利用姿態(tài)參數(shù)轉(zhuǎn)換的方法解決四元數(shù)的加權(quán)求和問題，并對(duì)四元數(shù)sigma點(diǎn)的求取及誤差方差陣進(jìn)行推導(dǎo)。

1 基于乘性四元數(shù)的誤差模型

1.1 姿態(tài)誤差模型

首先對(duì)推導(dǎo)過程中將要用到的符號(hào)變量進(jìn)行定義:n為導(dǎo)航坐標(biāo)系;e為地球坐標(biāo)系;s*為子慣導(dǎo)系統(tǒng)解算的載體坐標(biāo)系;s為子慣導(dǎo)系統(tǒng)所在的真實(shí)載體坐標(biāo)系;m為主慣導(dǎo)系統(tǒng)解算的載體坐標(biāo)系;ψm為s*系與m系之間的量測(cè)失準(zhǔn)角;ψa為s系與m系之間的實(shí)際失準(zhǔn)角;為從m系到s系的方向余弦矩陣;為從m系到n系的旋轉(zhuǎn)角速度在m系上的投影，其他角速度定義與此類似;rn為桿臂在n系上的投影;為加速度計(jì)隨機(jī)偏置誤差為陀螺儀隨機(jī)偏置誤差;為從m系到s系的乘性四元數(shù)為其轉(zhuǎn)置;為從m系到s*系的乘性四元數(shù)為其轉(zhuǎn)置。

快速傳遞對(duì)準(zhǔn)算法引入了相對(duì)失準(zhǔn)角的概念，即s*系與m系之間的量測(cè)失準(zhǔn)角ψm和s系與m系之間的實(shí)際失準(zhǔn)角ψa。該算法在由Kain J E首次提出時(shí)，其姿態(tài)誤差模型是在小失準(zhǔn)角的假設(shè)下推導(dǎo)建立的。在艦載武器的實(shí)際應(yīng)用中，經(jīng)常會(huì)出現(xiàn)初始誤差角為大角度的情況。此時(shí)，基于歐拉角的非線性姿態(tài)誤差模型［1，3］為

四元數(shù)法作為一種非奇異姿態(tài)描述方式，其定義為

式中:q0為四元數(shù)Q的標(biāo)量部分;ρ為四元數(shù)Q的向量部分。將向量與看作零標(biāo)量的四元數(shù)，其四元數(shù)乘法［12］表示為

為使推導(dǎo)清晰，定義向量自動(dòng)轉(zhuǎn)化為零標(biāo)量的四元數(shù)后再進(jìn)行四元數(shù)乘法運(yùn)算，其結(jié)果自動(dòng)轉(zhuǎn)回向量形式。根據(jù)式(3)，式(1)可寫為

再根據(jù)四元數(shù)微分方程及角速度定義式，即

得到基于乘性四元數(shù)的非線性姿態(tài)誤差模型為

1.2 速度誤差模型

考慮桿臂效應(yīng)的速度誤差模型為

式中:δV為主、子慣導(dǎo)間的速度誤差。根據(jù)四元數(shù)的相繼轉(zhuǎn)動(dòng)性［12］，即

得到基于乘性四元數(shù)的速度誤差模型為

1.3 量測(cè)方程

針對(duì)狀態(tài)方程中過程噪聲為乘性噪聲的情況，將過程噪聲擴(kuò)維至狀態(tài)變量中。取系統(tǒng)狀態(tài)為

式中:Xe為擴(kuò)維后的非四元數(shù)部分;2c和εc分別為加速度計(jì)和陀螺儀的常值偏置和常值漂移;ω為過程噪聲;ρm和ρa(bǔ)分別為量測(cè)失準(zhǔn)角及實(shí)際失準(zhǔn)角的等效姿態(tài)四元數(shù)的向量部分，滿足

快速傳遞對(duì)準(zhǔn)在觀測(cè)方程中選擇主子慣導(dǎo)間的相對(duì)姿態(tài)與速度誤差作為量測(cè)量，其量測(cè)方程為

2 基于姿態(tài)參數(shù)轉(zhuǎn)換的四元數(shù)UKF算法設(shè)計(jì)

2.1 姿態(tài)參數(shù)轉(zhuǎn)換方法

在采用四元數(shù)法描述三維空間中剛體的定點(diǎn)轉(zhuǎn)動(dòng)時(shí)，其4個(gè)元素并不相互獨(dú)立，因此需要對(duì)獲得的四元數(shù)進(jìn)行規(guī)范化運(yùn)算以保持其單位性，即

在四元數(shù)UKF算法中，sigma點(diǎn)經(jīng)非線性函數(shù)傳播后需進(jìn)行加權(quán)求和得到后驗(yàn)均值。而姿態(tài)四元數(shù)作為描述剛體一次旋轉(zhuǎn)的旋轉(zhuǎn)矢量，直接進(jìn)行四則運(yùn)算意義下的計(jì)算將破壞均值四元數(shù)的規(guī)范性。對(duì)此，本文基于姿態(tài)參數(shù)轉(zhuǎn)換原理，對(duì)四元數(shù)的加權(quán)求和公式進(jìn)行推導(dǎo)。首先給出另一種姿態(tài)參數(shù)—修正羅德里格斯參數(shù)(MRPs)的定義及其與姿態(tài)四元數(shù)間的相互轉(zhuǎn)換關(guān)系。MRPs是一種基于剛體定點(diǎn)轉(zhuǎn)動(dòng)的3參數(shù)姿態(tài)描述方法，其各個(gè)分量之間相互獨(dú)立，因此并不受到規(guī)范化的限制。其定義為

MRPs轉(zhuǎn)化為姿態(tài)四元數(shù)的公式為

式中，Φ為MRPs，姿態(tài)四元數(shù)轉(zhuǎn)化為MRPs的公式為

2.2 姿態(tài)四元數(shù)加權(quán)求和算法

對(duì)其加權(quán)求和，得到均值MRPs為

2.3 基于單位四元數(shù)的sigma點(diǎn)選取

在四元數(shù)UKF算法中，選取sigma點(diǎn)時(shí)需保證其四元數(shù)部分為單位四元數(shù)。對(duì)此，文獻(xiàn)［11］提出只取四元數(shù)向量部分參與sigma點(diǎn)計(jì)算，之后再通過單位化約束得到標(biāo)量部分的方法。其計(jì)算步驟為

1)計(jì)算擾動(dòng)sigma點(diǎn)，即

式中:Pk為狀態(tài)誤差方差陣;n為系統(tǒng)狀態(tài)維數(shù);為方陣第i行的轉(zhuǎn)置;λ為尺度參數(shù);為擾動(dòng)sigma點(diǎn)中的四元數(shù)向量部分;為 非四元 數(shù)部分。

2)根據(jù)單位化約束計(jì)算標(biāo)量部分，即

從而得到單位擾動(dòng)四元數(shù)為

sigma點(diǎn)的計(jì)算公式為

式中:χq，k為 sigma 點(diǎn)的四元數(shù)部分;χe，k為非四元數(shù)部分。

2.4 基于單位四元數(shù)的誤差方差陣計(jì)算

在四元數(shù)法中，兩次旋轉(zhuǎn)之差一般通過右乘旋轉(zhuǎn)四元數(shù)的逆實(shí)現(xiàn)。因此在計(jì)算誤差方差陣時(shí)，需要與非四元數(shù)部分區(qū)別實(shí)行。定義均值四元數(shù)與擾動(dòng)四元數(shù)之差為

非四元數(shù)部分之差為

從而得到自協(xié)方差公式為

2.5 四元數(shù)UKF濾波算法

根據(jù)前4節(jié)的推導(dǎo)，得到基于姿態(tài)參數(shù)轉(zhuǎn)換的四元數(shù)UKF濾波算法流程如下。

設(shè)定狀態(tài)初值為

式中:P0為非噪聲部分的誤差方差陣，根據(jù)2.4節(jié)的推導(dǎo)進(jìn)行計(jì)算，為使表述清晰，將采用經(jīng)典UKF;算法的表達(dá)式，具體方法則按前4節(jié)推導(dǎo)進(jìn)行操作;Rw為過程噪聲方差陣。

1)sigma點(diǎn)選取與權(quán)值計(jì)算

2)時(shí)間更新

sigma點(diǎn)通過非線性四元數(shù)誤差狀態(tài)方程傳播得到，即

3)量測(cè)更新

sigma點(diǎn)通過四元數(shù)量測(cè)方程傳播得到，即

4)狀態(tài)更新

3 仿真分析

以艦載武器的快速傳遞對(duì)準(zhǔn)為例，對(duì)不同姿態(tài)參數(shù)下的非線性誤差模型及相應(yīng)的非線性濾波算法進(jìn)行仿真對(duì)比。在風(fēng)浪及減搖鰭作用下艦船產(chǎn)生的搖擺運(yùn)動(dòng)模型為

搖擺幅度為:θxm=5°;θym=4°;θzm=2°。搖擺頻率為:ωx=0.15 rad/s;ωy=0.1 rad/s;ωz=0.06 rad/s。初始角為:θx0=0°;θy0=0°;θz0=45°。

艦船的對(duì)地運(yùn)動(dòng)模型為

子慣導(dǎo)系統(tǒng)陀螺常值漂移取為0.1°/h，隨機(jī)漂移取為0.01°/h，加速度計(jì)零偏取為10－3g，隨機(jī)偏差為10－4g。桿臂取為(15 15 5)，單位為m;水平方向上的兩個(gè)姿態(tài)失準(zhǔn)角分別為ψx=20′，ψy=40′，航向失準(zhǔn)角為ψz=10°。系統(tǒng)狀態(tài)非四元數(shù)部分初始值設(shè)為0，四元數(shù)部分設(shè)為［1，0，0，0］T;設(shè) INS 解算周期為0.01 s，濾波周期為0.5 s，仿真時(shí)間為50 s。

方案1采用歐拉角模型和經(jīng)典UKF濾波算法，方案2采用四元數(shù)模型和基于奇異值分解的四元數(shù)UKF濾波算法，方案3采用四元數(shù)模型和基于姿態(tài)參數(shù)切換的四元數(shù)UKF濾波算法。

3個(gè)方案對(duì)姿態(tài)失準(zhǔn)角的估計(jì)誤差如圖1～圖3所示。

圖1和圖2分別為3種方案下水平方向姿態(tài)失準(zhǔn)角的估計(jì)誤差曲線圖。從圖中可以看出，3種方案在非線性程度不高的條件下都可以保持較高的估計(jì)精確度及收斂速度，在10 s后都達(dá)到穩(wěn)定。圖3為航向失準(zhǔn)角的估計(jì)誤差曲線圖?；跉W拉角法的方案一出現(xiàn)較大偏差，精確度明顯降低。方案2采用基于奇異值分解的四元數(shù)UKF算法，由于受到算法復(fù)雜度的限制，其收斂速度較慢，但仍保持較高的估計(jì)精確度。方案3的收斂速度略慢于方案1，但比方案2快，這主要?dú)w因于四元數(shù)濾波算法的種種約束性條件在一定程度上提高了算法的運(yùn)算量，但姿態(tài)參數(shù)適中的轉(zhuǎn)換復(fù)雜度及四元數(shù)自身較小的計(jì)算量使得方案2依然能夠滿足實(shí)時(shí)性要求，而四元數(shù)法的應(yīng)用則使其在非線性提高時(shí)依然能夠保持較高的估計(jì)精確度，其對(duì)航向失準(zhǔn)角的估計(jì)精確度較方案1提高了4倍以上。表1為3種方案的估計(jì)精確度比較。

圖1 縱搖失準(zhǔn)角估計(jì)誤差ψxFig.1 Estimation error of pitching misalignment ψx

圖2 橫搖失準(zhǔn)角估計(jì)誤差ψyFig.2 Estimation error of rolling misalignment ψy

圖3 航向失準(zhǔn)角估計(jì)誤差ψzFig.3 Estimation error of heading misalignment ψz

表1 3種方案的估計(jì)精確度比較Table 1 Comparison of estimation accuracy of 3 schemes

4 結(jié)語

本文利用修正羅德里格斯參數(shù)與四元數(shù)相互轉(zhuǎn)化的方法對(duì)四元數(shù)加權(quán)求和公式進(jìn)行了推導(dǎo)，克服了UKF算法中一步預(yù)測(cè)均值的四參數(shù)規(guī)范化限制。同時(shí)對(duì)擾動(dòng)sigma點(diǎn)中四元數(shù)部分的單位化問題及誤差方差計(jì)算問題給出了相應(yīng)的解決方法。仿真結(jié)果表明，該算法與傳統(tǒng)UKF算法相比，對(duì)大方位失準(zhǔn)角具有更高的濾波精確度。而與基于奇異值分解的四元數(shù)UKF算法相比，其實(shí)時(shí)性更好，適用于主子慣導(dǎo)間姿態(tài)失準(zhǔn)角較大的快速傳遞對(duì)準(zhǔn)技術(shù)。

［1］ KAIN J E，CLOUTIER J R.Rapid transfer alignment for tactical weapon application［C］//AIAA Guidance，Navigation and Control Conference，Auguest 14 － 16，1989，Boston，USA.1989:1290－1300.

［2］ 譚紅力，黃新生，岳冬雪.捷聯(lián)慣導(dǎo)大失準(zhǔn)角誤差模型在快速傳遞對(duì)準(zhǔn)中的應(yīng)用［J］.國(guó)防科技大學(xué)學(xué)報(bào)，2008，30(6):19－23.

TAN Hongli，HUANG Xinsheng，YUE Dongxue.Rapid transfer alignment based on large misalignment SINS error model［J］.Journal of National University of Defense Technology，2008，30(6):19－23.

［3］ 熊芝蘭，郝燕玲，孫楓.基于四元數(shù)的慣導(dǎo)系統(tǒng)快速匹配對(duì)準(zhǔn)算法［J］.哈爾濱工程大學(xué)學(xué)報(bào)，2008，29(1):28 －34.

XIONG Zhilan，HAO Yanling，SUN Feng.Rapid matching alignment algorithm of inertial navigation system based on quaternion［J］.Journal of Harbin Engineering University，2008，29(1):28－34.

［4］ 戴邵武，李娟.一種快速傳遞對(duì)準(zhǔn)方法的誤差模型研究［J］.宇航學(xué)報(bào)，2009，30(3):942 －946.

DAI Shaowu，LI Juan.Study of rapid transfer alignment error model［J］.Journal of Astronautics，2009，30(3):942 －946.

［5］ LEFFERTS E J，MARKELY F L，SHUSTER M D.Kalman filtering for attitude estimation［J］.Journal of Guidance，Control and Dynamics，1982，5(5):417 －429.

［6］ JULIER S J，UHLMANN J K.Unscented filtering and nonlinear estimation［J］.Proceeding of the IEEE，2004，92(3):401－422.

［7］ CHOUKROUN D，BAR-ITZHACK I Y，OSHMAN Y.Novel quaternion Kalman filter［J］.Transactions on Aerospace and Electronic Systems，2006，42，(1):174 －190.

［8］ HENZEH LEEGHIM，YOONHYUK CHOI.Uncorrelated unscented filtering for spacecraft attitude determination［J］.Journal of Acta Astronautica，2010，67(2):135－144.

［9］ OSHMAN Y，CARMI A.Attitude estimation from vector observations using genetic algorithm embedded quaternion particle filter［J］.Journal of Guidance，Control and Dynamics，2006，29(4):879－891.

［10］ MARKLEY F L，CHENG Y，CRASSIDIS J L，et al.Averaging quaternion ［J］.Journal of Guidance，Control and Dynamics，2007，30(4):1193－1197.

［11］ CRASSIDIS J L，MARKLEY F L.Unscented filtering for spacecraft attitude estimation［J］.Journal of Guidance，Control and Dynamics，2003，26(4):536 －542.

［12］ 秦永元.慣性導(dǎo)航［M］.北京:科學(xué)出版社，2006:305－381.