狀態切換UKF的飛行器姿態確定算法

吳勃， 徐歡， 喬相偉

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

0 引言

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

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

1 飛行器姿態確定狀態空間模型

1.1 飛行器狀態空間模型

本文以飛行器姿態參數和陀螺漂移為狀態變量，建立飛行器姿態確定系統狀態方程，其微分形式為

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

以星敏感器四元數輸出作為量測輸出，則姿態確定系統測量數學模型為

式中:q為真實姿態四元數;qn為星敏感器測量噪聲的四元數表示形式。

1.2 飛行器姿態動力學方程

以近地衛星為例，建立飛行器姿態動力學方程［12］為

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

2 狀態切換UKF算法

2.1 全維UKF算法

考慮如下非線性離散系統，即

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

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

1)初始化

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

2)時間更新

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

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

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

3)量測更新

量測更新階段Sigma點選取為

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

4)狀態更新

由上式可得系統狀態增益矩陣，估計值及狀態誤差方差值分別為

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

2.2 狀態切換UKF算法

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

1)初始化

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

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

2)時間更新

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

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

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

3)量測更新

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

量測更新階段Sigma點選取為

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

4)狀態更新

狀態更新與全維UKF相同，這里不再贅述。

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

3 飛行器姿態UKF算法

3.1 四元數規范化問題

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

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

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

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

用MRPs表示的四元數參數為

3.2 飛行器姿態UKF算法

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

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

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

1)初始化

2)時間更新

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

式中p為用修正羅德里格斯參數形式描述的飛行器姿態向量。

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

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

將MRPs參數點轉換為四元數Sigma點，即

為了計算姿態預測均值需要將四元數一步預測值轉換為MRPs形式的一步預測值，即

陀螺漂移的時間更新為

從而得到狀態預測均值為

對應的狀態預測誤差方差值為

(3)量測更新

測量更新部分過程噪聲未參與其中，切換擴維系統狀態為

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

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

將MRPs點轉換為四元數Sigma點，即

四元數量測一步預測值為

類似于時間更新，此時將四元數形式的姿態量測預測值轉換為MRPs形式的姿態預測值，即

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

從而可得狀態量測誤差方差陣及互協方差值分別為

4)狀態更新

狀態增益矩陣更新為

從而狀態誤差值及狀態估計值分別更新為

狀態估計誤差方差陣為

4 仿真實驗研究

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

1)飛行器參數設置

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

2)傳感器參數設置

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

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

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

圖1 橫滾角姿態誤差對比Fig．1 Comparison of roll angle errors

圖2 俯仰角姿態誤差對比Fig．2 Comparison of pitch angle errors

圖3 偏航角姿態誤差對比Fig．3 Comparison of yaw angle errors

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

5 結語

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

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