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

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

1 引 言

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

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

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

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

2.1 陀螺量測模型

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

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

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

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

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

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

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

真實(shí)角速度和估計(jì)角速度之間的差值為：

對式（6）求導(dǎo)并將式（7）代入，有［14］：

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

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

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

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

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

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

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

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

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

3 自適應(yīng)UKF 算法

3.1 標(biāo)準(zhǔn)UKF 算法

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

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

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

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

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

（4）計(jì)算k時刻的一步預(yù)測樣本點(diǎn)：

（5）計(jì)算量測變量預(yù)測值：

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

（7）計(jì)算增益矩陣：

（8）計(jì)算濾波值：

3.2 自適應(yīng)UKF 算法

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

圖2 自適應(yīng)UKF 估計(jì)器工作流程Fig.2 Work flow of adaptive UKF estimator

4 仿 真

4.1 仿真條件

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

4.2 仿真結(jié)果及分析

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

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

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

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

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

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

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

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

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

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

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

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

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

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

5 結(jié) 論

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