采用自適應無跡卡爾曼濾波的衛(wèi)星姿態(tài)確定

肖 磊 ，王紹舉 ，常 琳 ，周美麗

（1. 中國科學院長春光學精密機械與物理研究所，吉林長春130033；2. 中國科學院大學，北京 100049；3. 中國科學院天基動態(tài)快速光學成像技術重點實驗室，吉林長春130033）

1 引 言

姿態(tài)參數(shù)是衛(wèi)星正常運行的關鍵參數(shù)［1-2］，其測量精度直接影響著衛(wèi)星的工作性能?；谛敲艉屯勇萁M成的衛(wèi)星姿態(tài)確定系統(tǒng)是衛(wèi)星姿態(tài)確定中較為廣泛應用的方案之一［3］。針對衛(wèi)星姿態(tài)確定的非線性問題，拓展卡爾曼濾波（Extended Kalman Filter，EKF）及其改進算法被廣泛應用于姿態(tài)確定的工程實踐中［4-5］。文獻［6］采用投影法確定時不變觀測矩陣，用恒增益矩陣代替雅可比矩陣，大大減少了算法的計算量，提高了算法的實時性。文獻［7］設計了基于新息的函數(shù)，制訂高低階數(shù)近似切換策略，改善了估計精度和計算負擔二者對立的處境。文獻［8］將RTS 平滑技術和EKF 算法相結合，應用到遙感衛(wèi)星定姿的后處理中，在傳感器精度不高的情況下，算法仍可以取得較好的姿態(tài)確定結果。

但EKF 本質上是將非線性問題線性化近似處理，這導致濾波精度的下降甚至濾波發(fā)散，具有局限性。而無跡卡爾曼濾波（Unscented Kalman Filter，UKF）克服了 EKF 的缺陷，擁有更高的估計精度和穩(wěn)定性［9］。在系統(tǒng)存在模型參數(shù)估計不準和量測不良的問題時，上述濾波方法［6-8］的精度和魯棒性較差，因此研究人員更傾向于使用改進的UKF 算法。文獻［10］結合殘差協(xié)方差匹配原理和最陡梯度下降算法，提出一種魯棒自適應UKF 算法，并應用于空間三維定位跟蹤上，具有良好的跟蹤精度和關于模型參數(shù)不準的魯棒性。文獻［11］提出一種基于自適應UKF 的皮納衛(wèi)星姿態(tài)確定算法，以歐拉角作為狀態(tài)矢量，對系統(tǒng)噪聲和量測噪聲的協(xié)方差矩陣進行自適應調整。文獻［12］提出一種修正的自適應Masreliez-Martin UKF 算法，應用于衛(wèi)星姿態(tài)確定，有效地提高了姿態(tài)確定精度。

本文在標準UKF 的基礎上，結合殘差協(xié)方差匹配原理和濾波發(fā)散判別準則，引入自適應因子矩陣，推導了自適應無跡卡爾曼濾波器（Adaptive Unscented Kalman Filter，AUKF）的衛(wèi)星定姿算法，并通過衛(wèi)星姿態(tài)測量仿真實驗驗證了該算法在模型參數(shù)估計準確和參數(shù)估計不準的情況下的高精度及強魯棒性。

2 衛(wèi)星姿態(tài)確定系統(tǒng)模型

2.1 陀螺量測模型

陀螺儀的功能是測量衛(wèi)星的三軸角速度，輸出衛(wèi)星本體系相對于參考坐標系的角速度。假設陀螺測量的坐標系與衛(wèi)星本體坐標系重合，其測量模型如下［13］：

式中：ωg(t)為陀螺的實際量測輸出值；ω(t)為衛(wèi)星三軸相對參考坐標系的真實角速度；b0為陀螺的常值漂移；d(t)為角速率隨機游走，ng(t)為角度隨機游走，d(t)，ng(t)二者之和構成陀螺隨機誤差；ng(t)，nd(t)為不相關的零均值高斯白噪聲，滿足：

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

以四元數(shù)作為姿態(tài)參數(shù)描述衛(wèi)星姿態(tài)，有：

衛(wèi)星的四元數(shù)姿態(tài)運動學方程描述如下：

式中ω=[ωx ωy ωz]T，是衛(wèi)星三軸相對參考坐標系轉動角速度在本體系中的投影，且有：

由于陀螺測量的星體角速度中含有隨機誤差，采用星敏補償陀螺漂移時只能得到估計的和。而四元數(shù)存在約束性原則，若直接將它作為狀態(tài)變量處理，在計算過程中容易出現(xiàn)方差矩陣奇異的問題，因此采用誤差四元數(shù)作為變量。定義真實四元數(shù)q和估計四元數(shù)之間的誤差四元數(shù)為 Δq=[Δq1Δq2Δq3Δq4]T。采用增量式表示四元數(shù)誤差時，有：

真實角速度和估計角速度之間的差值為：

對式（6）求導并將式（7）代入，有［14］：

由式（8）可以看出，誤差四元數(shù)降階為3 個獨立變量，從而避免了因系統(tǒng)變量冗余而導致的系統(tǒng)協(xié)方差矩陣奇異問題。取誤差四元數(shù)矢量部分和陀螺常值漂移誤差Δb作為系統(tǒng)狀態(tài) 變 量 ，即構 成 系 統(tǒng) 狀 態(tài)方程：

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

假設星敏測量的坐標系與衛(wèi)星本體坐標系重合，且安裝誤差矩陣為單位陣。則星敏量測模型可表示為：

式中：rs為星敏量測輸出，r為對應參考矢量，ns為量測噪聲。

將星敏輸出轉換成四元數(shù)，可得星敏輸出四元數(shù)qs為［15］：

設Nout為nout的矢量部分，令星敏輸出四元數(shù)qs和估計四元數(shù)之間的偏差四元數(shù)qe為：

由式（14）可得系統(tǒng)量測方程為：

陀螺和星敏組合姿態(tài)確定系統(tǒng)的工作原理如圖1 所示。

圖1 組合定姿估計器的工作原理Fig.1 Working principle of combined attitude determination estimator

3 自適應UKF 算法

3.1 標準UKF 算法

設置采樣時間為 Δt，對于式（9）和式（15）組成的衛(wèi)星姿態(tài)確定系統(tǒng)，其離散化方程可由四階龍格庫塔法計算得到：

式中：Xk∈Rn和Zk∈Rm分別為k時刻的系統(tǒng)狀態(tài)估計量和系統(tǒng)量測量；f和h分別為系統(tǒng)狀態(tài)轉移函數(shù)和系統(tǒng)量測函數(shù)；wk，vk分別為系統(tǒng)噪聲和量測噪聲，為相互獨立的零均值高斯白噪聲，其噪聲協(xié)方差矩陣分別為Qk和Rk，可表示為：基于非線性系統(tǒng)的標準UKF的算法步驟如下［16］：

（1）系統(tǒng)參數(shù)初始化：

（2）計算k-1 時刻的 2n+1 個 Sigma 樣本點及權系數(shù)：

（3）計算k時刻的一步預測模型值和預測誤差協(xié)方差：

（4）計算k時刻的一步預測樣本點：

（5）計算量測變量預測值：

（6）計算量測協(xié)方差和互協(xié)方差：

（7）計算增益矩陣：

（8）計算濾波值：

3.2 自適應UKF 算法

UKF 算法的執(zhí)行過程需要系統(tǒng)噪聲和量測噪聲的先驗信息［17］，然而量測過程中各種干擾因素的存在或者系統(tǒng)統(tǒng)計模型估計的不準確，先驗信息和真實值誤差過大時會增大濾波的跟蹤誤差，甚至引起濾波發(fā)散。標準UKF 算法將系統(tǒng)噪聲協(xié)方差Qk和量測噪聲協(xié)方差Rk等先驗信息設置為常值，不能很好地應對實際情況中的變化，因此需要對先驗信息做自適應處理。本文提出一種AUKF 算法，對量測噪聲協(xié)方差和系統(tǒng)噪聲協(xié)方差進行自適應調整，以獲得更有效的卡爾曼增益，增強濾波器的穩(wěn)定性和跟蹤精度。

3.2.1 量測噪聲協(xié)方差R自適應

量測量是反映系統(tǒng)狀態(tài)變量的唯一來源。在受到干擾時，量測量的作用應該被降低以減少跟蹤誤差。因此在這個步驟，通過自適應調整噪聲協(xié)方差Rk的數(shù)值，從而減小增益Kk，以達到減少量測量作用的目的。定義殘差εk為濾波器每一步量測量實際值與預測值的差值：

KF 算法中，在濾波最優(yōu)的情況下，殘差協(xié)方差理論值滿足［18］：

在UKF 算法中，其等價描述為：

由文獻［19］可知，殘差協(xié)方差的理論值可由其估計值得到：

通常，在量測量受到干擾或量測不良時，殘差協(xié)方差的估計值和理論值滿足：

為減小跟蹤誤差，引入自適應矩陣，表述如下：

式中：Sk=diag{s1s2...sm}，m為量測量的維數(shù)。結合殘差協(xié)方差匹配理論，即可求得自適應矩陣：

由式（37）求得的自適應矩陣Sk很可能并非對角陣，且不能保證非負定性。為避免這種情況，修正Sk的求解方式為：

式中：Nk(i，i)，Rk(i，i)分別為矩陣Nk和Rk第i行、第i列的對角線元素；μ為可調參數(shù)，且μ≥ 1。

3.2.2 系統(tǒng)噪聲協(xié)方差Q自適應

當系統(tǒng)模型參數(shù)準確時，在濾波過程中預測誤差協(xié)方差Pk|k-1的范數(shù)會逐漸減小，實時觀測量對狀態(tài)估計的修正作用變弱。而在系統(tǒng)參數(shù)不準和外界干擾下，量測協(xié)方差陣可能非正定，濾波極易發(fā)散，為避免濾波發(fā)散，引入對濾波發(fā)散趨勢的判斷規(guī)則為：

式中：γ為可調因子（γ≥ 1，值越大判定條件越寬松）；tr( )為求取矩陣的跡。在式（39）成立時，濾波器穩(wěn)定；當式（39）不成立時，濾波器有發(fā)散趨勢，因此引入自適應矩陣Λk對系統(tǒng)噪聲協(xié)方差Qk做自適應調整，使預測誤差協(xié)方差陣Pk|k-1在保持正定性的基礎上增大范數(shù)值，提高實時觀測量對濾波的修正作用，即有：

式中：Λk=diag{Λ1Λ2...Λn}，Λk的求取方式如下：

3.2.3 AUKF 濾波算法實現(xiàn)

衛(wèi)星姿態(tài)測量中的量測不良和系統(tǒng)模型建立不準會導致姿態(tài)確定精度下降。本文在標準UKF 的基礎上，基于殘差協(xié)方差理論提出了AUKF 算法，其工作流程如圖2 所示。在對衛(wèi)星進行姿態(tài)確定時，由陀螺和星敏獲取角速度ωg和姿態(tài)四元數(shù)qs，處理后輸入到濾波器中，濾波器先執(zhí)行一次標準UKF 主程序，獲得殘差向量εk，然后利用式（38）和式（41）計算得到自適應矩陣因子，對量測噪聲協(xié)方差Rk和系統(tǒng)噪聲協(xié)方差Qk進行自適應更新，再按照式（18）～式（30）進行量測更新，得到狀態(tài)估計量X^k和誤差協(xié)方差Pk，作為下次濾波循環(huán)的初值。

圖2 自適應UKF 估計器工作流程Fig.2 Work flow of adaptive UKF estimator

4 仿 真

4.1 仿真條件

表1 敏感器參數(shù)Tab.1 Sensor parameters

4.2 仿真結果及分析

圖3 姿態(tài)運動真實軌跡Fig.3 True trajectories of attitude motion

衛(wèi)星真實姿態(tài)運動軌跡如圖3 所示。為驗證算法性能，實驗分別考慮了系統(tǒng)建模準確和不準確兩種情況，選取標準UKF 算法和文獻［20］提出的魯棒自適應UKF（RAUKF）算法進行分析對比，3 個濾波器的初值設置相同。

在沒有噪聲干擾，系統(tǒng)建模準確時，3 種算法的濾波結果如圖4～圖6 所示。

圖4 建模準確時UKF 三軸姿態(tài)估計誤差Fig.4 Three-axial attitude estimation errors by UKF with accurate model

圖5 建模準確時RAUKF 三軸姿態(tài)估計誤差Fig.5 Three-axial attitude estimation errors by RAUKF with accurate model

圖6 建模準確時AUKF 三軸姿態(tài)估計誤差Fig.6 Three-axial attitude estimation errors by AUKF with accurate model

分析上述仿真結果可知，在建模準確時，傳統(tǒng)UKF 三軸姿態(tài)估計的誤差波動較大，精度控制在0.02°以內；RAUKF 三軸估計姿態(tài)的誤差波動較小，偏航和滾轉軸精度控制在0.01°以內，俯仰軸穩(wěn)定后精度控制在0.005°以內；AUKF 三軸估計的誤差波動最小，三軸精度都控制在 0.005°以內。和 RAUKF 相比，AUKF 算法在引入自適應因子時，采用多變量因子矩陣，使得各個濾波通道擁有不同的自適應調整能力，三軸精度大致相同。

在有噪聲干擾時，模擬建模不準的情況下，仿真數(shù)據(jù)中系統(tǒng)噪聲和量測噪聲均方差增大為原設定值的2 倍，但濾波器的相應參數(shù)初值不變，令濾波器參數(shù)不能反映噪聲的真實統(tǒng)計特性，3種算法的濾波結果如圖7～圖9 所示。

圖7 建模不準時UKF 三軸姿態(tài)估計誤差Fig.7 Three-axial attitude estimation errors by UKF with inaccurate model

圖8 建模不準時RAUKF 三軸姿態(tài)估計誤差Fig.8 Three-axial attitude estimation errors by RAUKF with inaccurate model

由圖可見，在模型參數(shù)估計不準確時，3 種算法的三軸姿態(tài)估計精度都有所下降，標準UKF和RAUKF 算法的估計精度的下降幅度較大，分別在 0.05°和 0.02°以內，說明 RAUKF 的自適應能力較差，不能很好地應對建模不準的情況，但是對野值具有一定的抑制作用；而AUKF 算法精度的下降幅度較小，其濾波結果仍能快速收斂，在0.01°以內，魯棒性較好。

為更好比較3 種算法的三軸姿態(tài)估計精度，統(tǒng)計三軸姿態(tài)估計的均方根誤差（Root Mean Square Error，RMSE），仿 真 結 果 如 表 2 所 示 。RMSE 表達式為：

表2 三種算法的性能對比Tab.2 Performance comparison of three algorithms

由表2 可以看出，參數(shù)估計準確時，AUKF的三軸姿態(tài)角精度最高，較UKF 分別提升了70.6%，71.3%，71.9%，較RAUKF 分別提升了17.7%，14.8%，46.8%；參數(shù)估計不準時，UKF和RAUKF 算法的濾波性能迅速下降，而AUKF三軸估計誤差也有增大的趨勢，但下降幅度較其余兩種算法較小，其三軸估計精度較兩種算法分別 提 升 74.9%，75.9%，74.9% 和 30.0%，34.1%，22.4%。不過，AUKF 算法耗時也大幅增加，較UKF 平均增加37.0%，和RAUKF 耗時相當。綜合來看，所提出的AUKF 算法相較于UKF 和RAUKF 算法擁有更高的精確性和魯棒性。

5 結 論

本文根據(jù)衛(wèi)星姿態(tài)確定精度高、魯棒性強的要求，提出了一種AUKF 算法，介紹了衛(wèi)星組合定姿的工作原理和濾波算法的工作流程，然后在模型參數(shù)估計準確和不準確兩種情況下對UKF，RAUKF 和AUKF 3 種算法進行性能對比仿真驗證。實驗結果表明，在模型估計不準時，AUKF較RAUKF 三軸估計精度分別提升了30.0%，34.1%，22.4%，能夠滿足衛(wèi)星姿態(tài)確定對精度和魯棒性的要求。