黃 蔚

(中國艦船研究設計中心，武漢430064)

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

黃 蔚

(中國艦船研究設計中心，武漢430064)

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

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

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

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

1 不確定量測的非線性姿態(tài)估計模型

1.1 陀螺測量模型

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

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

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

(1)

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

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

(2)

(3)

(4)

(5)

由于

(6)

(7)

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

(8)

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

(9)

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

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

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

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

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

(10)

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

2 UCCKF算法

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

1)將Px進行Cholesky分解.

Px=SST.

2)容積點的求取.

(11)

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

3)容積點經(jīng)過非線性函數(shù)的傳遞值為

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

針對式(10)的不確定量測，由于λk、xk以及vk是相互獨立的，則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為容積點，ωi為相應的權(quán)值，可以根據(jù)式(11)求出.

2.3 UCCKF算法

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

2.3.1 時間更新

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

2.3.2 量測更新

將上述容積點經(jīng)量測非線性函數(shù)的傳遞值為

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

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

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

(18)

(19)

2.3.3 兩步投影理論

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

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

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

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

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

3 仿真實驗及分析

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

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

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

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

圖1 當p=0.1時姿態(tài)誤差

圖2 當p=0.5時姿態(tài)誤差

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

圖3 當p=0.8時姿態(tài)誤差

圖4 當p=0.8時姿態(tài)角均方誤差

4 結(jié) 論

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

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

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

[2] 錢華明, 黃蔚, 孫龍. 基于改進的迭代容積卡爾曼濾波姿態(tài)估計[J]. 哈爾濱工業(yè)大學學報, 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]. 控制與決策, 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)切換無跡卡爾曼濾波器估計的飛行器姿態(tài)[J]. 控制理論與應用, 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

國家自然科學基金(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)