基于ATSUKF算法的衛星姿控系統故障估計

陳雪芹，孫瑞，吳凡，蔣萬程

哈爾濱工業大學 衛星技術研究所，哈爾濱 150080

增廣狀態估計法常用于故障估計，該方法將故障視為系統狀態進行增廣，然后進行卡爾曼濾波處理。隨著系統及偏差維數的增加，增廣狀態估計法的計算負荷急劇增加，計算速度和估計精度都會下降。基于偏差分離原理的故障估計算法同樣源于增廣狀態估計的設計思路，但能有效避免該狀況的發生。偏差分離原理的基本思想是：先分別估計無偏狀態和偏差，然后利用兩者之間的耦合關系得到最優狀態估計結果，也被稱為二階卡爾曼濾波(Two-Stage Kalman Filter, TSKF)算法。文獻[1]中給出的偏差分離估計算法是TSKF算法最早的形式。此后，Keller等給出了TSKF標準形式[2-3]，并被廣泛引用。

Mendel[4]首次將該方法擴展到非線性系統中，提出二階擴展卡爾曼濾波(Two-Stage Extended Kalman Filter, TSEKF)算法。針對偏差維數較大的情況，Mendel和Washburn提出多階卡爾曼濾波(MSKF)算法[5]，并將其推廣到了系統動態估計中。Ignagni[6-7]對文獻[1]中的公式進行優化完善，給出了TSKF的一般通用形式，但給出的濾波器只能得到次優的估計結果，此后，Ignagni又提出了TSKF最優和次優結果對應的一般形式[8]。

此后，為了得到TSKF的最優解，大量學者進行了深入研究。Alouani等用一個代數約束條件表示偏差噪聲和系統噪聲的相關性，給出了一種最優的二階卡爾曼濾波(OTSKF)算法[9]，OTSKF算法在約束條件下等價于擴展狀態卡爾曼濾波(EKF)算法[10]。同樣為了得到最優的估計結果，通過對TSKF中的無偏狀態估計器進行優化。2013年，Keller和Sauter針對隨機離散線性系統中包含間歇未知輸入的情況，通過最小化狀態均方誤差矩陣的跡，解耦當前時間的未知輸入和狀態估計，利用TSKF算法得到間歇未知輸入和系統狀態的最優估計結果，該算法雖然有一步延遲，但計算量小[11]。Hsieh等基于TSKF算法進行了深入系統的研究，提出了OTSKF算法的2種表示形式[12-13]，并提出了GTSKF[14]、RTSKF[15]、HTSKF等算法。

TSKF算法最早由Friedland[1]提出時，并沒有針對故障估計的應用進行描述，但實際系統模型中的偏差可以準確地描述控制系統的執行機構故障和敏感器故障。因此，從理論上看來，可認為該算法一經提出就是適用于線性系統故障估計的算法，即TSKF算法可直接應用于線性系統故障估計。

在應用TSKF算法處理非線性系統故障時，通常的做法是利用EKF的思想，通過計算雅可比矩陣將非線性系統模型進行線性化，然后對線性化后的系統模型應用TSKF算法進行故障估計，這是TSEKF算法的基本思想。在此基礎上，很多學者又推導了TSEKF的各種形式，使濾波器具有魯棒性[16]、自適應性[17]等特性。

近年來，筆者將TSKF、TSEKF算法應用于衛星姿控系統執行機構/敏感器故障估計，經過前期研究發現[18-19]：當姿控系統中飛輪等執行機構、星敏感器等角度測量敏感器出現加性或乘性故障時，基于TSEKF的故障估計算法可以正確估計出故障的幅值；然而光纖陀螺等角速度測量敏感器出現故障時，該算法雖然能在一定程度上估計出故障的幅值，但估計結果中會出現周期性偏差項，且無法通過調整卡爾曼濾波參數得到改善。出現該偏差的原因是：在TSEKF算法中使用EKF的思想對系統進行了線性化處理，導致二次及以上高階項被忽略，從而引起角度測量故障估計結果的不準確。而UT變換在處理非線性項時，能夠保持以至少三階泰勒精度逼近任何非線性高斯系統狀態的后驗均值和協方差，因此基于UT變換的無損卡爾曼濾波(UKF)算法的理論估計精度優于EKF算法。

因此，本文針對該問題提出了一種使用UKF思想的二階無損卡爾曼濾波(Two-Stage Unscented Kalman Filter, TSUKF)算法，用以解決TSEKF中線性化處理造成估計不準確的問題。不同于EKF將非線性模型線性化的技術手段， UKF直接處理非線性模型，目前在處理非線性系統狀態估計問題時應用廣泛，如導航系統定位[20-21]、航天器姿態確定[22]、電源狀態估計[23]等問題。將UKF算法與TSKF算法相結合，能夠在將非線性項與線性項分離之后有效地處理非線性模型的估計問題。相較于文獻[24-25]中提出的類似TSUKF算法，本文算法完全不需要非線性系統模型的線性化。同時，為解決過程噪聲和量測噪聲協方差等統計數據不準確導致的濾波器收斂過慢問題，在TSUKF的基礎上提出了一種使用量測數據進行自適應的故障估計算法——自適應二階無損卡爾曼濾波(Adaptive Two-Stage Unscented Kalman Filter, ATSUKF)算法，實現參數的自適應調節。對本文算法進行對比仿真，驗證有效性。

1 ATSUKF算法

1.1 基本TSUKF算法

考慮如下非線性離散隨機系統：

(1)

其中：Qx為系統噪聲方差陣；Qb為故障噪聲方差陣；R為量測噪聲方差陣；δk,j為Kronecker符號。

根據UKF算法的一般形式，可知UKF與EKF具有相同的算法結構，只是UKF算法直接使用了系統模型，避免了線性化帶來的誤差。為解決TSEKF算法對姿態角測量故障無法準確估計的問題，直接將UKF思想引入TSKF濾波器中，得到與TSEKF[19]形式類似的TSUKF算法。

TSUKF算法給出的TSUKF濾波器如下：

(2)

(3)

i=0,1,…,2n

(4)

(5)

(6)

(7)

(8)

(9)

(10)

(11)

(12)

(13)

(14)

(15)

(16)

(17)

(18)

式中：Ep為p階單位矩陣。狀態與偏差的耦合關系式為

(19)

(20)

(21)

(22)

λ=α2(n+κ)-n

1.2 自適應改進

TSUKF算法能夠估計出衛星姿控系統中執行機構和敏感器故障的大小，但統計參數(Qx、Qb和R)的選取對故障估計的精度和速度影響較大，而實際系統中往往存在系統模型的統計參數無法準確已知的情況。針對這一情況，在TSUKF算法的基礎上提出ATSUKF算法，在濾波過程中對以上參數進行自適應調整，保障故障估計結果的準確性對仿真參數的變化不敏感。

受Hajiyev和Soken提出的魯棒自適應卡爾曼濾波器[26]啟發，本文對噪聲協方差乘以一個多維的自適應因子矩陣，使用測量數據實現對噪聲協方差矩陣的自適應，用于系統模型參數未知情況下的衛星姿控系統故障估計。

ATSUKF的系統模型與TSUKF相同，基本濾波器設計也與其相同，通過引入標度矩陣S來實現對量測噪聲協方差的自適應。系統量測方差表示為

Sk+1R

(23)

同時，根據實際測量數據可以得到

(24)

(25)

因此，有

Sk+1R

(26)

進而可以算得

(27)

理想情況下，噪聲方差和系統模型匹配，那么標度矩陣即為單位陣。實際上，通過式(27)計算得到的標度矩陣可能并非一個對角陣，并且對角元素可能小于等于0(數學仿真計算時可能出現這種情況，但實物系統不存在)。為了避免數學仿真計算中這種情況的發生，需要對標度矩陣進行處理：

Sk+1=diag(s1,s2，…，sn)

(28)

式中：

si=max{1,Sk+1,ii}i=1，2,…，n

(29)

在對量測噪聲方差中的R進行自適應處理后，同樣可以對系統噪聲方差陣Qx和Qb求取相應的自適應矩陣。

對于Qx的自適應矩陣Λx，對式(26)進行處理可以得到

(30)

式中：

(31)

從而得到自適應矩陣為

(32)

與標度矩陣S一致，Qx的自適應矩陣Λx同樣是對角線元素≥1的對角矩陣，因此定義

(33)

式中：

(34)

(35)

(36)

從而有

(37)

(38)

同樣對其進行處理，得到

(39)

式中：

(40)

(41)

2 快速機動衛星姿態/故障估計

2.1 衛星姿控系統建模

(42)

式中：

C=E6

φ1=Φ(x)

φ2=Φ(x)Abt(x)

Abt(x)=Cy(θbt)Cx(φbt)Cz(ψbt)

(43)

對于衛星姿控系統而言，當衛星處于姿態穩定狀態下，通過姿態角的小量化假設可以將系統模型當作線性系統處理，進行常規的姿態濾波估計。另外，衛星姿態穩定時，控制器傳輸給執行機構的指令力矩近似為0，執行機構的乘性故障不會對系統姿態造成影響。

然而，當衛星處于姿態機動狀態時，一方面，系統模型經過線性化處理后往往無法準確描述機動過程中的系統動態特性。另一方面，執行機構接收到的控制力矩指令實時變化，此時必須考慮執行機構的乘性故障對衛星姿態造成的影響。

因此，在接下來的衛星姿控系統故障估計過程中，設置衛星姿態始終處于快速機動狀態，在系統具有強非線性的條件下，對比驗證TSUKF算法和ATSUKF算法的估計效果。

2.2 仿真分析

對TSUKF算法和ATSUKF算法進行數值仿真對比，仿真條件與所需的初始值如下。

1) 衛星的轉動慣量矩陣為

J=diag(17,12,10) kg·m2

跟蹤目標姿態角φt為

2) 反作用飛輪最大輸出力矩為0.1N·m，最大角動量為2N·m。

3) 系統仿真周期為0.01s，飛輪控制周期為0.1s，濾波采樣周期為0.05s。

4) 對于非線性的航天器姿控系統(43)，設計控制律如下：

式中：

KD和KP為控制器參數，在仿真時分別取：KD=0.54E3,KP=0.09E3。

2.2.1執行機構故障

Qb=(2×10-5N·m)2E3

仿真結果如圖1所示。由于自適應因子的變化過于劇烈，因此在圖中以對數形式指出其數量級的變化。

Qb=(2×10-4)2E3

Qx=

圖1 飛輪加性故障下ATSUKF與TSUKF仿真結果Fig.1 Additive faults simulation results of reaction wheels by ATSUKF and TSUKF

仿真結果如圖2所示。

圖1(a)、圖1(b)和圖2(a)、圖2(b)分別給出了對于x軸姿態角速度和姿態角的估計結果，可以發現，TSUKF算法和ATSUKF算法對于姿態信息的估計結果區別不大。

圖2 飛輪乘性故障下ATSUKF與TSUKF仿真結果Fig.2 Multiplicative faults simulation results of reaction wheels by ATSUKF and TSUKF

從圖1(c)可以看出，第50 s和第150 s時自適應矩陣中對應x軸的對角線元素發生了巨大的變化。故障信號發生變化之后，其數值的數量級從100增加到了1013。第50 s發生故障和第150 s故障消失時，故障發生了變化，量測數據與估計值不再匹配，通過計算可以得到自適應因子突然變大，增大了方差噪聲矩陣，從而使算法快速收斂。而在其他區段，由于未發生故障或者故障穩定，自適應矩陣不會發生突變，對角線上的自適應因子均為1。而對于乘性故障，故障發生和消失時控制器發給執行機構的指令力矩幅值極小，此時乘性故障對系統的影響過小，因此從圖2(c) 可以發現對于故障發生時間的檢測有一定的延遲。從圖1(d)和圖2(d)可以發現，故障估計結果與自適應因子變化結果可以得出相同的結論，在第50 s和第150 s開始發生變化。

另外，從仿真結果可以看出，自適應矩陣在故障發生變化時自適應因子的突變可以直接用于判斷故障是否發生變化。

從圖1(d)和圖2(d)可以看出，TSUKF算法在卡爾曼濾波參數過小的情況下濾波收斂過慢，無法正常估計出故障幅值。而ATSUKF算法因為自適應矩陣的存在，可以根據量測數據調整噪聲方差矩陣的大小，從而實現對故障的快速估計，效果優于TSUKF算法。

2.2.2 敏感器故障

考慮測量姿態角速度的光纖陀螺發生故障，仿真時間200 s，故障設置為50 ～150 s陀螺發生故障，x軸測量存在0.1 (°)/s的常值偏差。TSUKF算法和ATSUKF算法中的噪聲方差參數均設置為

Qb=(2×10-7(°)/s)2E3

仿真結果如圖3所示。

圖3 ATSUKF與TSUKF估計陀螺故障結果Fig.3 Faults estimation results of gyros by ATSUKF and TSUKF

從數學仿真結果可以看出，與飛輪故障類似，在系統參數設置不準確的情況下，相比TSUKF算法，ATSUKF算法對姿控系統中光纖陀螺角速度測量故障的估計效果更好，能夠更快速地估計出故障的幅值。當故障發生后自適應因子變大，增大方差，加速濾波收斂。從對故障估計的結果可以看出，對于光纖陀螺故障，ATSUKF算法能夠很好地進行估計。

綜合執行機構和敏感器故障的仿真結果可以看出，在統計特性不準確的情況下，通過引入自適應矩陣，ATSUKF算法對故障的估計效果優于TSUKF，算法收斂更快，對故障的跟蹤效果更好。

3 結 論

1) 本文在TSKF中融入UKF思想，首先得到TSUKF算法用于系統故障估計，并在此基礎上考慮系統噪聲統計信息不確定的情況下結合自適應因子提出了ATSUKF算法。

2) 針對先驗信息不足或故障發生變化等系統噪聲協方差無法準確已知的情況，ATSUKF算法使用測量數據和殘差信息來自適應地調整噪聲方差陣，與TSUKF算法相比，ATSUKF算法的估計結果收斂速度更快，避免了傳統TSUKF算法需要不斷調試濾波器參數的麻煩。