吳勃， 徐歡， 喬相偉

(1．哈爾濱理工大學自動化學院，黑龍江哈爾濱 150080;2．北京環(huán)球信息應用開發(fā)中心，北京 100094;3．西安航天精密機電研究所系統(tǒng)工程事業(yè)部，陜西西安 710100)

0 引言

飛行器姿態(tài)確定是飛行器控制系統(tǒng)的關鍵組成部分。常用的姿態(tài)描述參數(shù)有方向余弦矩陣、歐拉角、姿態(tài)四元數(shù)(quaternion)、羅德里格參數(shù)(rodrigues parameters，RPs)、修正羅德里格參數(shù)(modi-fied rodrigues parameters，MRPs)、凱萊 － 哈密爾頓參數(shù)［1］等。其中姿態(tài)四元數(shù)因為計算量小、非奇異性、可全姿態(tài)工作而廣泛應用于實際系統(tǒng)當中［2－3］。但由于四元數(shù)的4個參數(shù)存在冗余，4個變量并不獨立，使他必須滿足規(guī)范化限制。如何克服四元數(shù)的規(guī)范化限制，讓他與先進的濾波算法相結合成為目前國內(nèi)外研究的熱點問題［3］。

目前最常用的飛行器姿態(tài)確定算法是狀態(tài)估計算法。然而該算法將不可避免地要涉及到非線性濾波問題。而基于泰勒展開的擴展卡爾曼濾波［4］(extended Kalman filter，EKF)，其本質(zhì)是 Kalman濾波在非線性系統(tǒng)中的推廣，該算法比較簡單，只達到了對非線性系統(tǒng)的一階估計精確度。針對此，之后的學者又提出了二階EKF算法［5］，濾波精確度得到較大提高，但是因為該算法涉及到Hassian矩陣的計算，使得計算負擔也大為增加，實際應用反而沒有EKF廣泛?？紤]到對非線性函數(shù)后驗均值和方差的近似遠比對非線性函數(shù)本身的近似簡單，近年來，Julier等提出了無跡卡爾曼濾波算法［6］。該算法首先以系統(tǒng)狀態(tài)的先驗均值和方差為基準，選取一組確定性采樣點，稱為Sigma點，使得這組采樣點的統(tǒng)計特性與狀態(tài)先驗統(tǒng)計特性相一致，然后將利用系統(tǒng)非線性傳遞函數(shù)得到一組新的采樣點，最后采用加權統(tǒng)計線性回歸技術得到這組樣本點的統(tǒng)計特性，作為非線性函數(shù)的后驗統(tǒng)計分布。然而早期的無跡卡爾曼濾波(unscented Kalman filter，UKF)算法并沒有考慮系統(tǒng)噪聲對濾波精確度和穩(wěn)定性的影響。為此，武元新和M．Briers等從非線性系統(tǒng)噪聲出發(fā)，分析了系統(tǒng)噪聲對UKF算法的濾波性能的影響，指出忽略噪聲的非線性傳遞特性的標準UKF算法會一定程度地降低濾波的精確度和穩(wěn)定性，并從理論上給出了擴維UKF(augmented UKF，AUKF)算法可以提高濾波精確度的證明［7］。在此基礎上，Merwe在2004年給出了3種形式的擴維UKF算法［8］，包括只將過程噪聲或者量測噪聲納入擴維狀態(tài)的單邊擴維UKF算法和噪聲全部納入擴維狀態(tài)的全維 UKF算法(all－dimensional UKF，AUKF)。其中全維UKF算法最為典型，應用也最廣。該算法通過擴維將過程噪聲和量測噪聲全部納入狀態(tài)變量中參與Sigma點選取，提高了UT變換的后驗逼近精確度，進而提高了濾波的精確度和穩(wěn)定性。但該算法同時也帶來了狀態(tài)維數(shù)倍增，計算負擔加重的問題。而事實上在多數(shù)情況下，系統(tǒng)的量測與過程是不相關的，即傳感器的量測精確度并不受載體機動變化的影響。為此，提出一種過程和量測不相關的狀態(tài)切換UKF濾波(state switching UKF，SSUKF)算法，并將其應用到飛行器姿態(tài)確定中去。針對姿態(tài)確定過程中四元數(shù)均值計算問題和協(xié)方差奇異性問題［9－11］，采取將四元數(shù)參數(shù)和修正羅德里格斯參數(shù)相結合的方法，即在均值和協(xié)方差計算時將四元數(shù)參數(shù)切換為修正羅德里格斯參數(shù)，而在狀態(tài)傳遞時采取四元數(shù)形式，這樣既避免了修正羅德里格斯參數(shù)積分帶來的高計算復雜度問題，又解決了四元數(shù)的規(guī)范化問題。針對大姿態(tài)誤差角的SINS/CCD飛行器姿態(tài)仿真結果表明，該算法與全維UKF算法相比，估計精確度相當，但估計時間縮短了約1/3。

1 飛行器姿態(tài)確定狀態(tài)空間模型

1.1 飛行器狀態(tài)空間模型

本文以飛行器姿態(tài)參數(shù)和陀螺漂移為狀態(tài)變量，建立飛行器姿態(tài)確定系統(tǒng)狀態(tài)方程，其微分形式為

式中:Ωd［ω(t)］為姿態(tài)更新函數(shù);q(t)為姿態(tài)四元數(shù);ω(t)=(t)－β(t)－ηv(t);(t)為角速度量測值;β(t)為一階馬爾科夫過程;ηv(t)，ηu(t)均為零均值白噪聲。

以星敏感器四元數(shù)輸出作為量測輸出，則姿態(tài)確定系統(tǒng)測量數(shù)學模型為

式中:q為真實姿態(tài)四元數(shù);qn為星敏感器測量噪聲的四元數(shù)表示形式。

1.2 飛行器姿態(tài)動力學方程

以近地衛(wèi)星為例，建立飛行器姿態(tài)動力學方程［12］為

式中:J為飛行器本體的轉動慣量;hw為飛行器內(nèi)部運動部件的總動量;Tc為控制力矩;Td為干擾力矩。

2 狀態(tài)切換UKF算法

2.1 全維UKF算法

考慮如下非線性離散系統(tǒng)，即

式中:xk∈Rn為k時刻的系統(tǒng)狀態(tài)向量;zk∈Rm為k時刻的系統(tǒng)量觀測向量;ωk∈Rn，vk∈Rm分別為服從N(0，Q)和N(0，R)分布的高斯白噪聲。

選取擴維系統(tǒng)狀態(tài)變量xa=［xTwTvT］T。其中，x，w，v分別為系統(tǒng)狀態(tài)變量，過程噪聲變量和量測噪聲變量。

1)初始化

式中:x0為系統(tǒng)狀態(tài)初始值;P0，Q0，R0分別為系統(tǒng)狀態(tài)初始誤差方差陣，過程噪聲初始誤差方差陣和量測噪聲初始誤差方差陣。

2)時間更新

時間更新階段Sigma點及權值選取為

上述Sigma點對應的均值權值和方差權值分別為

式中:L=2n+m;λ=α2(n+κ)－n為尺度因子，α決定了這些Sigma點集到均值點的距離，通常設置為一個很小的正數(shù)，κ通常設置為零;β用于融入隨機變量的驗前信息［8］。

3)量測更新

量測更新階段Sigma點選取為

通過非線性量測函數(shù) Zk|k－1(i)=］，可得k時刻系統(tǒng)量測一步預測均值，方差及互協(xié)方差分別為

4)狀態(tài)更新

由上式可得系統(tǒng)狀態(tài)增益矩陣，估計值及狀態(tài)誤差方差值分別為

由擴維UKF算法可知，量測噪聲參與了時間更新過程，而過程噪聲也參與了量測更新過程。而在實際系統(tǒng)中，大多數(shù)情況下量測與過程是不相關的。此時，量測噪聲在時間更新過程中只是單純地增加了濾波的計算量，對狀態(tài)預測更新起不到太大作用，另外由整個濾波過程可知，噪聲向量并沒有得到更新。這是因為系統(tǒng)假設的噪聲為零均值高斯白噪聲，所以噪聲的最優(yōu)估計值為零，只需要在每次濾波開始前賦值為零即可。

2.2 狀態(tài)切換UKF算法

由擴維UKF算法可知，通過擴維狀態(tài)維數(shù)由n維增加為2n+m維，對應的Sigma點也由2n+1個增加到4n+2m+1個。這無疑將極大增加UKF算法的計算量。對此，本文提出一種過程與量測不相關的狀態(tài)切換UKF算法。下面給出該算法的具體步驟。

1)初始化

選取擴維狀態(tài)變量 xa=［xTwTvT］T。其中，x為待估計的系統(tǒng)狀態(tài)向量，w和v分別為過程噪聲向量和量測噪聲向量。

擴維狀態(tài)初始估計值和誤差方差值如式(5)所示。

2)時間更新

時間更新過程因為觀測噪聲沒有參與，選取擴維狀態(tài)變量為狀態(tài)向量和過程噪聲。k時刻濾波狀態(tài)變量及方差選取為

式中:xk，wk分別為k時刻的狀態(tài)向量和過程噪聲;Pk，Qk則分別為k時刻的狀態(tài)向量和過程噪聲對應的誤差方差矩陣。

k時刻時間更新Sigma點選取為

3)量測更新

量測更新過程中，由于過程噪聲不參與更新，切換狀態(tài)維數(shù)L2=n+m，此時擴維狀態(tài)變量選取為狀態(tài)向量和量測噪聲，即

量測更新階段Sigma點選取為

通過非線性量測函數(shù) Zk|k－1(i)=h［(i)，］，其中，)分別為量測更新Sigma點的狀態(tài)向量部分和量測噪聲部分，可得k時刻系統(tǒng)量測一步預測均值，方差及互協(xié)方差為

4)狀態(tài)更新

狀態(tài)更新與全維UKF相同，這里不再贅述。

由上面狀態(tài)切換算法可知，在時間更新和量測更新濾波過程中狀態(tài)維數(shù)分別由2n+m維減小為2n維和n+m維，對應的Sigma點分別由4n+2m+1個點減小為4n+1個點和2n+2m+1個點。狀態(tài)維數(shù)的減小不僅影響了Sigma點選點的減少，而且在濾波時間更新和量測更新階段也大大減小了計算量。在不影響濾波精確度的前提下，節(jié)省了濾波的時間，提高了系統(tǒng)的實時性。

3 飛行器姿態(tài)UKF算法

3.1 四元數(shù)規(guī)范化問題

相比于其他姿態(tài)描述參數(shù)，四元數(shù)因為計算量小、非奇異性和可全姿態(tài)工作成為姿態(tài)描述的首選。然而，四元數(shù)規(guī)范化要求卻限制了其在非線性濾波中的應用。四元數(shù)規(guī)范化問題的核心是四元數(shù)加權均值計算問題，四元數(shù)加權均值ˉqsimple最直接的方法［13］為

但是這樣就會帶來兩個問題，其一，均值四元數(shù)ˉqsimple將不再是一個規(guī)范的四元數(shù);其二，q和－q表示相同的矢量旋轉，也就是說改變四元數(shù)的符號并不應該影響四元數(shù)的加權均值，但是由式(21)所得的均值四元數(shù)顯然不能滿足這一點。

解決四元數(shù)規(guī)范化問題的一個方法就是姿態(tài)參數(shù)切換算法，具體來講就是在涉及四元數(shù)均值及協(xié)方差計算時，將四元數(shù)通過姿態(tài)參數(shù)切換轉化為3參數(shù)的MRPs，利用MRPs進行均值及協(xié)方差計算就避免了四元數(shù)協(xié)方差的奇異性問題。用四元數(shù)表示的MRPs參數(shù)為

式中:g為MRPs參數(shù);a，b均為標量;ρ為四元數(shù)的向量部分;q4為四元數(shù)標量部分。

用MRPs表示的四元數(shù)參數(shù)為

3.2 飛行器姿態(tài)UKF算法

本文在濾波過程中采取四元數(shù)和修正羅德里格斯參數(shù)實時切換的方法，有效解決了四元數(shù)的規(guī)范性問題。同時考慮到姿態(tài)確定中噪聲非線性傳遞的特性，將前面的狀態(tài)切換UKF算法應用到姿態(tài)確定中去。下面給出基于狀態(tài)切換UKF的飛行器姿態(tài)確定算法的具體流程。

對于低年級孩子來說，作文評語有時可能被孩子忽略了，有時孩子還不是很懂老師的意思。所以，只寫評語是不夠的。每次習作我都堅持及時面批，讓孩子看著老師改他的作文，具體地進行指導。面批，讓孩子直觀了解自己的作文，哪些地方這樣寫是好的，哪些地方怎樣改會更好。要讓孩子體會到老師的鼓勵，老師對他作文的熱情和重視。低年級學生的作文篇幅不長，老師若利用好零散的課余時間，是可以做到人人面批的。

選取擴維狀態(tài)變量為 xa=［xT，wT，vT］T=［pT，其中 x=［pT，βT］T，w=［，p 為 MRPs形式的姿態(tài)變量，β 為陀螺漂移，ηv，ηu為過程噪聲向量，ηs為量測噪聲。

1)初始化

2)時間更新

時間更新階段，沒有量測噪聲參與，此時擴維狀態(tài)變量選為

式中p為用修正羅德里格斯參數(shù)形式描述的飛行器姿態(tài)向量。

時間更新階段Sigma點及權值選擇，在已知k－1時刻狀態(tài)分布的基礎上，選取k時刻狀態(tài)Sigma點為

由于狀態(tài)變量在時間更新時涉及到四元數(shù)更新，首先將上述Sigma點分為MRPs和非MRPs兩部分，即

將MRPs參數(shù)點轉換為四元數(shù)Sigma點，即

為了計算姿態(tài)預測均值需要將四元數(shù)一步預測值轉換為MRPs形式的一步預測值，即

陀螺漂移的時間更新為

從而得到狀態(tài)預測均值為

對應的狀態(tài)預測誤差方差值為

(3)量測更新

測量更新部分過程噪聲未參與其中，切換擴維系統(tǒng)狀態(tài)為

測量更新過程中Sigma點選取為

由于量測更新中只有姿態(tài)觀測矢量，此時需要用四元數(shù)形式進行更新，首先將上述Sigma點分為MRPs部分和非MRPs部分，即

將MRPs點轉換為四元數(shù)Sigma點，即

四元數(shù)量測一步預測值為

類似于時間更新，此時將四元數(shù)形式的姿態(tài)量測預測值轉換為MRPs形式的姿態(tài)預測值，即

由于量測量中沒有陀螺漂移，所以狀態(tài)量測預測均值為

從而可得狀態(tài)量測誤差方差陣及互協(xié)方差值分別為

4)狀態(tài)更新

狀態(tài)增益矩陣更新為

從而狀態(tài)誤差值及狀態(tài)估計值分別更新為

狀態(tài)估計誤差方差陣為

4 仿真實驗研究

本文以近地衛(wèi)星為例，建立SINS/CCD姿態(tài)估計系統(tǒng)仿真平臺，為各種姿態(tài)確定算法提供統(tǒng)一的仿真實驗平臺，該系統(tǒng)軟件開發(fā)語言主要是Matlab和C++。

1)飛行器參數(shù)設置

飛行器軌道為典型的近地圓軌道，軌道高度為800 km，傾角為60°，飛行器飛行軌道角速度 Ωo=0.001 rad/s，飛行器初始姿態(tài)角速度分別為0.01°/s，0.01°/s，0.05°/s。衛(wèi)星三軸初始姿態(tài)角分別為 1°，2°，10°。飛行器的轉動慣量矩陣為

2)傳感器參數(shù)設置

光纖陀螺測量白噪聲為0.5°/h，驅動白噪聲為0.02°/h，陀螺輸出采樣頻率為100 Hz，星敏感器采用兩個光軸垂直安裝，其輸出頻率為4 Hz，飛行器初始姿態(tài)誤差設定:橫滾角，俯仰角，偏航角分別為1°，2°，10°。初始陀螺漂移在三軸上分別為 1°/h，1°/h，1°/h。星敏感器運動速率為 0.05°/s，測量白噪聲標準差為20″，輸出頻率為4 Hz?？柭鼮V波器中的姿態(tài)和陀螺漂移估計值均設定為零。

依據(jù)上述初始條件，分別采用全維UKF和狀態(tài)切換UKF算法進行仿真，仿真結果如圖1～圖3和表1所示。

由圖1和2可知，當初始姿態(tài)誤差角較小取為1°和2°時，兩種算法精確度相當且都以較快的速度收斂;而當初始姿態(tài)誤差角為大角度時，由圖3可知，兩種算法在精確度相當?shù)那闆r下，SUKF能以更快的速度收斂，收斂時間與AUKF相比。這是由于SUKF算法通過實時切換系統(tǒng)狀態(tài)減小了狀態(tài)的維數(shù)，降低了運算的復雜度。另外由表1平均一次迭代濾波所用時間也可以看出，SUKF算法與UKF算法相比濾波時間減少了大約1/3，這也再次驗證了該算法在提高系統(tǒng)實時性方面的有效性。

圖1 橫滾角姿態(tài)誤差對比Fig．1 Comparison of roll angle errors

圖2 俯仰角姿態(tài)誤差對比Fig．2 Comparison of pitch angle errors

圖3 偏航角姿態(tài)誤差對比Fig．3 Comparison of yaw angle errors

表1 平均一次迭代濾波時間對比Table 1 Time consumption comparison of average one time iteration filter

5 結語

為解決全維姿態(tài)UKF算法帶來的維數(shù)倍增，計算負擔加重的問題，本文提出了一種過程和量測不相關的狀態(tài)切換UKF算法，通過在預測和量測階段選取不同的狀態(tài)變量，減小了實時濾波的維數(shù)，有效降低了濾波計算量。針對四元數(shù)非線性濾波算法中四元數(shù)規(guī)范化的限制，給出了參數(shù)切換UKF算法，通過濾波過程中四元數(shù)與修正羅德里格斯參數(shù)的實時切換，解決了四元數(shù)均值計算和協(xié)方差奇異性問題。以實驗室SINS/CCD組合姿態(tài)估計系統(tǒng)為平臺，進行了仿真實驗，實驗結果驗證了所提算法的有效性。

［1］MARKLEY F L．Attitude error representations for Kalman filtering［J］．Journal of Guidance，Control，and Dynamics，2003，63(2):311－317．

［2］CHUNG D Y，LEE J G．Strap-down INS error model for multi-position alignment［J］．IEEE Transactions on Aerospace and Electronic Systems，1996，32(4):1362 －1366．

［3］SHUSTER M D．Constraint in attitude estimation part I:constrained estimation［J］．Journal of the Astronautical Sciences，2003，51(1):51－74．

［4］MARKLEY F Landis，CRASSIDIS J L，CHENG Yang．Nonlinear attitude filtering methods［C］//Collection of Technical Papers-AIAA Guidance，Navigation，and Control Conference，August 15－18，2005，San Francisco，USA．2005，1:753－784．

［5］PSIAKI M L．The super-iterated extended Kalman filter［C］//Collection of Technical Papers-AIAA Guidance，Navigation，and Control Conference，August 16 － 19，2004，Providence，USA．2004，5:3384－3398．

［6］WU Yuanxin，HU Dewen，WU Meiping，et al．Unscented Kalman filtering for additive noise case:augmented vs．non-augmented［C］//Proceedings of the American Control Conference，June 8－10，2005，Portland，USA．2005，6:4051－4055．

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

［8］MERWE R．Sigma-point Kalman Filters for Probabilistic Inference in Dynamic State-space Models［D］．OGI School of Science and Engineering at Oregon Health and Science University，2004．

［9］MARKLEY F L．Attitude determination using vector observations and the singular value decomposition［J］．Journal of the Astronautically Sciences，1988，36(3):245 －258．

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

［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］姜雪原．衛(wèi)星姿態(tài)確定及敏感器誤差修正的濾波算法研究［D］．哈爾濱:哈爾濱工業(yè)大學控制科學與工程系，2006．

［13］MARKLEY F L，CHENG Yang，CRASSIDIS J L，et al．Averaging quaternions ［J］．Journal of Guidance，Control，and Dynamics，2007，30(4):1193 －1197．