量測(cè)不確定的四元數(shù)約束CKF姿態(tài)估計(jì)

黃 蔚

(中國(guó)艦船研究設(shè)計(jì)中心，武漢430064)

量測(cè)不確定的四元數(shù)約束CKF姿態(tài)估計(jì)

黃 蔚

(中國(guó)艦船研究設(shè)計(jì)中心，武漢430064)

針對(duì)姿態(tài)估計(jì)系統(tǒng)在量測(cè)不確定和四元數(shù)約束下存在發(fā)散及估計(jì)精度差的缺陷，提出了一種基于不確定量測(cè)的四元數(shù)約束容積卡爾曼濾波算法(quaternion constrained cubature Kalman filter based on uncertain measurements, UCCKF).該算法克服了約束容積卡爾曼濾波算法的局限性，采用獨(dú)立的伯努利隨機(jī)變量來(lái)描述量測(cè)的不確定性，利用三階球面-相徑容積規(guī)則近似計(jì)算非線(xiàn)性函數(shù)的后驗(yàn)均值和協(xié)方差.并針對(duì)四元數(shù)規(guī)范化問(wèn)題，采用兩步投影理論來(lái)解決四元數(shù)約束限制.仿真結(jié)果表明，相比較于約束容積卡爾曼濾波(constrained cubature Kalman filter, CCKF)和無(wú)跡混合濾波 (unscented mixture filter, UMF)，提出的UCCKF算法在量測(cè)不確定情況下具有更好的收斂性和更高的估計(jì)精度，說(shuō)明該算法對(duì)量測(cè)不確定下的非線(xiàn)性姿態(tài)估計(jì)系統(tǒng)是有效、可行的.關(guān)鍵詞: 姿態(tài)估計(jì)；四元數(shù)約束；不確定量測(cè)；容積卡爾曼濾波；兩步投影理論

針對(duì)由星敏感器和陀螺組成的姿態(tài)估計(jì)系統(tǒng)，四元數(shù)因計(jì)算量小、非奇異性及可全姿態(tài)工作等優(yōu)點(diǎn)常被作為其姿態(tài)描述參數(shù)[1].因此，基于四元數(shù)非線(xiàn)性姿態(tài)估計(jì)濾波技術(shù)成為研究熱點(diǎn)[2-4].由于四元數(shù)存在歸一化約束限制，許多處理四元數(shù)約束的非線(xiàn)性姿態(tài)估計(jì)算法被提出，主要分為以下幾種：1)四元數(shù)強(qiáng)制性約束方法，如范數(shù)限制卡爾曼濾波算法[5]、平方根四元數(shù)CKF[6]等；2) 將誤差四元數(shù)作為狀態(tài)變量，對(duì)模型進(jìn)行降階處理，轉(zhuǎn)化為無(wú)約束的姿態(tài)估計(jì)模型，如乘性擴(kuò)展卡爾曼濾波算法[7]；3)將四元數(shù)看成是一個(gè)旋轉(zhuǎn)矢量，與濾波算法相結(jié)合，如四元數(shù)狀態(tài)切換無(wú)跡卡爾曼濾波器[8]等；4) 將四元數(shù)參數(shù)與其他無(wú)冗余的三維姿態(tài)參數(shù)進(jìn)行切換，如無(wú)跡四元數(shù)估計(jì)法[9]等.其中，兩步投影理論[6,10]是一種有效的方法，將狀態(tài)估計(jì)值分兩步投影到約束表面，從而解決四元數(shù)規(guī)范化問(wèn)題.

上述研究四元數(shù)非線(xiàn)性姿態(tài)估計(jì)，都是基于準(zhǔn)確的量測(cè)模型.而在實(shí)際應(yīng)用中，由于光學(xué)成像、星圖識(shí)別等復(fù)雜的處理步驟以及網(wǎng)絡(luò)傳輸?shù)牟豢煽啃?，?dǎo)致姿態(tài)信息輸出突然失效，從而引起星敏感器量測(cè)丟失現(xiàn)象的發(fā)生[11-12]，這在狀態(tài)空間模型上體現(xiàn)為量測(cè)模型不確定.此種情況下，基于四元數(shù)姿態(tài)估計(jì)濾波方法研究相對(duì)較少.文獻(xiàn)[13]針對(duì)量測(cè)模型不確定的情況采用協(xié)方差分配的方法來(lái)求取狀態(tài)估計(jì)值.而文獻(xiàn)[14]在此基礎(chǔ)上，采用校正擴(kuò)展和無(wú)跡卡爾曼濾波相結(jié)合的方法，雖然能有效解決量測(cè)丟失問(wèn)題，但無(wú)跡卡爾曼濾波算法在高維情況下存在算法穩(wěn)定性差，濾波精度低的現(xiàn)象，同時(shí)未考慮四元數(shù)約束.而CKF算法采用三階球面-相徑容積規(guī)則[15]產(chǎn)生一組容積點(diǎn)來(lái)近似非線(xiàn)性函數(shù)的后驗(yàn)分布，相比較于無(wú)跡卡爾曼濾波算法，CKF算法具有嚴(yán)格的數(shù)學(xué)理論支撐，針對(duì)高維系統(tǒng)，不存在采樣非局部效應(yīng)，濾波算法的數(shù)值穩(wěn)定性更好，濾波估計(jì)精度更高.

基于此，本文在推導(dǎo)基于不確定量測(cè)的誤差四元數(shù)非線(xiàn)性姿態(tài)估計(jì)模型基礎(chǔ)上，提出一種基于不確定量測(cè)的四元數(shù)約束容積卡爾曼濾波姿態(tài)估計(jì)算法(quaternion constrained cubature kalman filter based on uncertain measurements, UCCKF)，并通過(guò)仿真驗(yàn)證了該算法在量測(cè)不確定情況下的有效性.

1 不確定量測(cè)的非線(xiàn)性姿態(tài)估計(jì)模型

1.1 陀螺測(cè)量模型

一個(gè)連續(xù)形式的陀螺測(cè)量模型可表示為

1.2 系統(tǒng)狀態(tài)方程

根據(jù)文獻(xiàn)[1]，基于四元數(shù)的飛行器軌道動(dòng)力學(xué)方程能夠被描述為

(1)

由上述四元數(shù)描述的姿態(tài)矩陣為

利用四元數(shù)乘法，誤差四元數(shù)能夠被定義為

(2)

(3)

(4)

(5)

由于

(6)

(7)

然后利用式(5)、(6)、(7)可得

(8)

將式(8)進(jìn)行離散化得姿態(tài)估計(jì)系統(tǒng)離散狀態(tài)方程為

(9)

式中wk均值為零，方差為Qk的高斯白噪聲.

1.3 系統(tǒng)量測(cè)方程

若星敏感器的輸出為光軸在本體坐標(biāo)系上的參考矢量，則星敏感器測(cè)量模型表示為

式中:zk為星敏感器的輸出矢量；h(xk)為非線(xiàn)性量測(cè)函數(shù)；vk為高斯白噪聲，其均值為零，方差為Rk.

如果考慮星敏感器的量測(cè)存在丟失或是不確定的情況，則帶量測(cè)不確定的星敏感器測(cè)量模型為

(10)

式中：λk表示描述量測(cè)中的不確定性，作為獨(dú)立的伯努利隨機(jī)變量，取值為0或1，P[λk=1]=pk表示量測(cè)沒(méi)有出現(xiàn)錯(cuò)誤的概率，則λk的均值和方差分別為pk和(1-pk)pk.

2 UCCKF算法

2.1 三階球面-相徑容積規(guī)則

1)將Px進(jìn)行Cholesky分解.

Px=SST.

2)容積點(diǎn)的求取.

(11)

式中：m=2n；假設(shè)n維的單位向量用e=[1,0,…,0]T來(lái)表示，任意改變e中元素的符號(hào)或是對(duì)其元素進(jìn)行全排列得到的所有單位向量的集合表示為點(diǎn)集[1]，則[1]i就為點(diǎn)集[1]中的第i個(gè)點(diǎn)；ξi為第i個(gè)容積點(diǎn)；ωi為相應(yīng)容積點(diǎn)的權(quán)值.

3)容積點(diǎn)經(jīng)過(guò)非線(xiàn)性函數(shù)的傳遞值為

2.2 不確定量測(cè)的統(tǒng)計(jì)特性

針對(duì)式(10)的不確定量測(cè)，由于λk、xk以及vk是相互獨(dú)立的，則zk的均值、方差以及互協(xié)方差分別為：

E[zk]=E[λkh(xk)+vk]=pkE[h(xk)],

(12)

(13)

Cov(xk,zk)=Cov(xk,λkh(xk)+vk)=Cov(xk,λkh(xk))=pkCov(xk,h(xk)).

(14)

(15)

(16)

(17)

式中χi為容積點(diǎn)，ωi為相應(yīng)的權(quán)值，可以根據(jù)式(11)求出.

2.3 UCCKF算法

針對(duì)由式(9)和式(10)組成的非線(xiàn)性姿態(tài)估計(jì)系統(tǒng)，同時(shí)考慮量測(cè)不確定和四元數(shù)約束兩種情況，并利用三階球面-相徑容積規(guī)則近似非線(xiàn)性函數(shù)的后驗(yàn)均值和協(xié)方差，給出了一種UCCKF算法.其算法流程如下.

2.3.1 時(shí)間更新

將上述容積點(diǎn)經(jīng)狀態(tài)非線(xiàn)性函數(shù)的傳遞值為

2.3.2 量測(cè)更新

將上述容積點(diǎn)經(jīng)量測(cè)非線(xiàn)性函數(shù)的傳遞值為

由式(15)～(17)可以求出關(guān)于y=h(xk)的統(tǒng)計(jì)特性，其均值、方差以及互協(xié)方差為：

再根據(jù)式(12)～(14)可以求出不確定量測(cè)下zk的均值、方差以及互協(xié)方差為：

則無(wú)四元數(shù)約束下的狀態(tài)估計(jì)及協(xié)方差為：

(18)

(19)

2.3.3 兩步投影理論

由于四元數(shù)存在歸一化的限制，即姿態(tài)估計(jì)系統(tǒng)的部分狀態(tài)量存在四元數(shù)約束情況，如果不考慮狀態(tài)約束直接作濾波處理，濾波精度差，甚至?xí)?dǎo)致協(xié)方差奇異.因此需要對(duì)濾波方法進(jìn)行改進(jìn)，而文獻(xiàn)[2]的強(qiáng)制性約束只是簡(jiǎn)單的將姿態(tài)估計(jì)值投影到四元數(shù)約束表面，對(duì)姿態(tài)估計(jì)方差沒(méi)有調(diào)整，這會(huì)引起濾波精度的下降，由于四元數(shù)約束本質(zhì)上是屬于非線(xiàn)性等式約束，因此，本文采用文獻(xiàn)[10]提出的兩步投影理論來(lái)解決非線(xiàn)性四元數(shù)約束問(wèn)題.第1步投影將無(wú)約束的狀態(tài)估計(jì)分布投影到約束表面，使得姿態(tài)估計(jì)分布滿(mǎn)足四元數(shù)約束，但是這會(huì)引起姿態(tài)估計(jì)方差的下降.第2步投影則將約束的狀態(tài)估計(jì)分布投影到約束表面，使得姿態(tài)估計(jì)均值滿(mǎn)足四元數(shù)約束，同時(shí)對(duì)姿態(tài)估計(jì)方差進(jìn)行補(bǔ)償，從而在保證四元數(shù)規(guī)范化的同時(shí)提高姿態(tài)估計(jì)的精度.

由于狀態(tài)變量xk中，誤差四元數(shù)δq滿(mǎn)足約束c(xk)=δqTδq-1，則將無(wú)約束狀態(tài)投影到約束表面為

利用兩步投影理論對(duì)式(18)、(19)中未約束的狀態(tài)估計(jì)進(jìn)行處理.

第1步投影 為使無(wú)約束狀態(tài)估計(jì)分布滿(mǎn)足非線(xiàn)性等式約束，將其投影到約束表面，從而得到約束的狀態(tài)估計(jì)分布均值和方差分別為：

從上述推導(dǎo)可以看出，當(dāng)p=1，即不存在量測(cè)不確定情況，則UCCKF退化為約束容積卡爾曼濾波(constrained cubature Kalman filter, CCKF).

3 仿真實(shí)驗(yàn)及分析

基于上述仿真實(shí)驗(yàn)平臺(tái)，為更好驗(yàn)證本文提出的UCCKF算法的有效性，將其與CCKF算法以及文獻(xiàn)[14]中的無(wú)跡混合濾波算法(unscented mixture filter, UMF)進(jìn)行比較和分析.同時(shí)，UMF算法用于非線(xiàn)性姿態(tài)估計(jì)系統(tǒng)，需考慮四元數(shù)約束，因此，在其每步狀態(tài)更新后加上強(qiáng)制性的四元數(shù)約束.仿真時(shí)間設(shè)為800 s.仿真結(jié)果如圖1～3所示.

圖1、2顯示的是當(dāng)星敏感器未出現(xiàn)量測(cè)丟失的概率分別為p=0.1和p=0.5時(shí)濾波得到的姿態(tài)誤差對(duì)比圖.從這兩個(gè)圖中可以明顯看出，CCKF濾波算法隨著仿真時(shí)間的增加姿態(tài)誤差不斷增大，而UMF算法和本文提出的UCCKF算法則未出現(xiàn)明顯的濾波發(fā)散現(xiàn)象，算法較為穩(wěn)定，算法收斂精度都能達(dá)到20 arcsec.同時(shí)隨著星敏感器未出現(xiàn)量測(cè)丟失的概率p不斷增大，這3種算法的濾波精度均有所提高.這些現(xiàn)象出現(xiàn)的原因是CCKF濾波算法設(shè)計(jì)未考慮量測(cè)丟失的影響從而導(dǎo)致在出現(xiàn)量測(cè)丟失的情況下濾波算法不穩(wěn)定，估計(jì)精度降低.而UMF算法和UCCKF算法均在算法設(shè)計(jì)中考慮了量測(cè)丟失的影響，當(dāng)星敏感器量測(cè)丟失出現(xiàn)時(shí)，算法均能收斂，保持穩(wěn)定.圖3顯示的是當(dāng)星敏感器未出現(xiàn)量測(cè)丟失的概率p=0.8時(shí)姿態(tài)誤差的對(duì)比圖，從圖3中可以看出，3種算法均能收斂，估計(jì)精度相當(dāng).這是由于星敏感器出現(xiàn)正確量測(cè)的概率較高，因此對(duì)CCKF算法的影響減小，CCKF算法能夠收斂，且估計(jì)精度影響不大.

為了體現(xiàn)算法比較的公正性以及更好地比較算法穩(wěn)態(tài)時(shí)的估計(jì)性能，姿態(tài)角的均方根誤差(root-mean square error, RMSE)被采用來(lái)描述姿態(tài)估計(jì)的質(zhì)量，它被定義為

式中:aei(k)為第i次蒙特卡羅仿真姿態(tài)誤差向量；NMC為蒙特卡羅仿真的總次數(shù)，仿真中將其設(shè)為NMC=50.

圖1 當(dāng)p=0.1時(shí)姿態(tài)誤差

圖2 當(dāng)p=0.5時(shí)姿態(tài)誤差

圖4顯示的是當(dāng)星敏感器出現(xiàn)正確量測(cè)的概率p=0.8時(shí)3種算法的姿態(tài)角均方誤差對(duì)比圖.從圖4中可以看出，此種情況下UCCKF算法的穩(wěn)態(tài)精度要高于CCKF算法和UMF算法.這是因?yàn)?，相比較于CCKF算法，UCCKF算法能夠處理星敏感器量測(cè)丟失，濾波估計(jì)精度要高于CCKF算法.UMF算法雖然也能補(bǔ)償星敏感器量測(cè)丟失導(dǎo)致的模型誤差，但是它是以UKF算法為基礎(chǔ)，且采用強(qiáng)制性的四元數(shù)約束方法，對(duì)算法的精度有一定的影響，而UCCKF算法是以CKF算法為基礎(chǔ)，利用兩步投影理論確保滿(mǎn)足四元數(shù)約束條件，因此，在高維以及量測(cè)不確定的情況下，UCCKF的濾波性能更好.

圖3 當(dāng)p=0.8時(shí)姿態(tài)誤差

圖4 當(dāng)p=0.8時(shí)姿態(tài)角均方誤差

4 結(jié) 論

1)針對(duì)星敏感器量測(cè)丟失導(dǎo)致的量測(cè)模型不確定問(wèn)題，建立了基于不確定量測(cè)的誤差四元數(shù)非線(xiàn)性姿態(tài)估計(jì)模型，所建立的模型對(duì)姿態(tài)估計(jì)算法的研究提供支撐.

2)提出了一種基于不確定量測(cè)的四元數(shù)約束容積卡爾曼濾波算法，解決了上述模型中存在的四元數(shù)約束和量測(cè)不確定問(wèn)題.仿真結(jié)果表明，該算法在量測(cè)不確定情況下具有更好的收斂性和更高的估計(jì)精度，驗(yàn)證了算法的合理性和有效性.

[1]CHANGLubin,HUBaiqing,CHANGGuobin.Modifiedunscentedquaternionestimatorbasedonquaternionaveraging[J].JournalofGuidance,ControlandDynamics， 2013, 37(1):305-309.DOI: 10.2514/1.61723.

[2] 錢(qián)華明, 黃蔚, 孫龍. 基于改進(jìn)的迭代容積卡爾曼濾波姿態(tài)估計(jì)[J]. 哈爾濱工業(yè)大學(xué)學(xué)報(bào), 2014, 46(6): 116-122. QIAN Huaming, HUANG Wei, SUN Long. Attitude estimation based on improved iterated cubature Kalman filter[J]. Journal of Harbin Institute of Technology, 2014, 46(6): 116-122.

[3] 王碩, 宋申民, 史小平,等. 噪聲特性未知的多傳感器協(xié)方差交叉融合姿態(tài)估計(jì)[J]. 控制與決策, 2016, 31(2): 273-278. DOI: 10.13195/j.kzyjc.2014.1417. WANG Shuo, SONG Shenmin, SHI Xiaoping, et al. Multi-sensor covariance intersection fusion attitude estimation with unknown noise characteristics[J]. Control and Decision, 2016, 31(2): 273-278. DOI: 10.13195/j.kzyjc.2014.1417.

[4] QIAN Huaming, HUANG Wei, QIAN Linchen, at al. Robust extended Kalman filter for attitude estimation with multiplicative noises and unknown external disturbances[J]. IET Control Theory & Applications, 2014, 8(15): 1523-1536. DOI: 10.1049/iet-cta.2014.0293.

[5] ZANETTI R, MAJJI M, BISHOP R H, at al. Norm-constrained Kalman filtering[J]. Journal of Guidance, Control, and Dynamics, 2009, 32(5): 1458-1465. DOI: 10.2514/1.43119.

[6] TANG Xiaojun, LIU Zhenbao, ZHANG Jiasheng. Square-root quaternion cubature Kalman filtering for spacecraft attitude estimation [J]. Acta Astronautica, 2012, 76: 84-94. DOI: 10.1016/j.actaastro.2012.02.009.

[7] LEFFERTS E J, MARKLEY F L, SHUSTER M D. Kalman filtering for spacecraft attitude estimation[J]. Journal of Guidance, Control, and Dynamics, 1982, 5(5): 417-429.DOI: 10.2514/3.56190.

[8] 喬相偉, 周衛(wèi)東, 吉宇人. 用四元數(shù)狀態(tài)切換無(wú)跡卡爾曼濾波器估計(jì)的飛行器姿態(tài)[J]. 控制理論與應(yīng)用, 2012, 29(1): 97-103. DOI: 10.7641/j.issn.1000-8152.2012.1.CCTA100514. QIAO Xiangwei, ZHOU Weidong, JI Yuren. Aircraft attitude estimation based on quaternion state-switching unscented Kalman filter[J]. Control Theory & Applications, 2012, 29(1): 97-103. DOI: 10.7641/j.issn.1000-8152.2012.1.CCTA100514.

[9] CRASSIDIS J L, MARKLEY F L. Unscented filtering for spacecraft attitude estimation[J]. Journal of Guidance, Control, and Dynamics, 2003, 26(4): 536-542.DOI: 10.2514/2.5102.

[10]JULIER S J, LAVIOLA J J. On Kalman filtering with nonlinear equality constraints [J]. IEEE Transactions on Signal Processing, 2007, 55(6): 2774-2784. DOI: 10.1109/TSP.2007.893949.

[11]WANG Shuang, GENG Yunhai, JIN Rongyu. A novel error model of optical systems and an on-orbit calibration method for star sensors [J]. Sensors, 2015, 15(12):31428-31441. DOI:10.3390/s151229863.

[12]HUANG Wei, XIE Hongsheng, SHEN Chen, et al. A robust strong tracking cubature Kalman filter for spacecraft attitude estimation with quaternion constraint [J]. Acta Astronautica, 2016, 121: 153-163. DOI: 10.1016/j.actaastro.2016.01.009.

[13]NANACARA W, YAZ E E. Recursive estimator for linear and nonlinear systems with uncertain observations[J]. Signal Processing, 1997, 62(1):215-228. DOI:10.1016/S0165-1684(97)00126-6.

[14]HERMOSO-CARAZO A, LINNARES-PEREZ J. Different approaches for state filtering in nonlinear systems with uncertain observations[J]. Applied Mathematics and Computation, 2007, 187(2):708-724. DOI: 10.1016/j.amc.2006.08.083.

[15]ARASARATNAM I, HAYKIN S. Cubature Kalman filters[J]. IEEE Transactions on Automatic Control, 2009, 54(6): 1254-1269. DOI: 10.1109/TAC.2009.2019800.

(編輯 張 紅)

Quaternion constrained cubature Kalman filter attitude estimation based on uncertain measurements

In terms of the uncertainty measurement and the constraint on the attitude quaternion, this paper proposes a quaternion constrained cubature Kalman filter based on uncertain measurements (UCCKF) for the spacecraft attitude estimation system. The proposed filter algorithm overcomes the limitation of the quaternion-constrained cubature Kalman filter. An independent Bernoulli random process is introduced to describe the measurement uncertainty, and the three degree spherical radial cubature rule is adopted to computer the posterior mean and covariance of the system state. To deal with quaternion normalization problem, the two-step projection theory is applied to solve the quaternion constraint. Simulation results show that, compared with constrained cubature Kalman filter and unscented mixture filter, the proposed UCCKF can help to implement the better convergence and higher estimation accuracy in the case of measurement uncertainty. This illustrates that the proposed algorithm is effective and feasible for the nonlinear attitude estimation system.

attitude estimation; quaternion constraint; uncertainty measurement; cubature Kalman filter; two-step projection theory

10.11918/j.issn.0367-6234.201509022

2015-09-07

國(guó)家自然科學(xué)基金(61573113)

黃 蔚(1986—)，男，博士

黃 蔚，huangwei2393@163.com

U666.12

A

0367-6234(2017)04-0116-06

HUANG Wei

(China Ship Development and Design Center, Wuhan 430064, China)