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

哈爾濱工業(yè)大學(xué) 衛(wèi)星技術(shù)研究所，哈爾濱 150080

增廣狀態(tài)估計(jì)法常用于故障估計(jì)，該方法將故障視為系統(tǒng)狀態(tài)進(jìn)行增廣，然后進(jìn)行卡爾曼濾波處理。隨著系統(tǒng)及偏差維數(shù)的增加，增廣狀態(tài)估計(jì)法的計(jì)算負(fù)荷急劇增加，計(jì)算速度和估計(jì)精度都會(huì)下降?；谄罘蛛x原理的故障估計(jì)算法同樣源于增廣狀態(tài)估計(jì)的設(shè)計(jì)思路，但能有效避免該狀況的發(fā)生。偏差分離原理的基本思想是：先分別估計(jì)無偏狀態(tài)和偏差，然后利用兩者之間的耦合關(guān)系得到最優(yōu)狀態(tài)估計(jì)結(jié)果，也被稱為二階卡爾曼濾波(Two-Stage Kalman Filter, TSKF)算法。文獻(xiàn)[1]中給出的偏差分離估計(jì)算法是TSKF算法最早的形式。此后，Keller等給出了TSKF標(biāo)準(zhǔn)形式[2-3]，并被廣泛引用。

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

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

TSKF算法最早由Friedland[1]提出時(shí)，并沒有針對(duì)故障估計(jì)的應(yīng)用進(jìn)行描述，但實(shí)際系統(tǒng)模型中的偏差可以準(zhǔn)確地描述控制系統(tǒng)的執(zhí)行機(jī)構(gòu)故障和敏感器故障。因此，從理論上看來，可認(rèn)為該算法一經(jīng)提出就是適用于線性系統(tǒng)故障估計(jì)的算法，即TSKF算法可直接應(yīng)用于線性系統(tǒng)故障估計(jì)。

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

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

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

1 ATSUKF算法

1.1 基本TSUKF算法

考慮如下非線性離散隨機(jī)系統(tǒng)：

(1)

其中：Qx為系統(tǒng)噪聲方差陣；Qb為故障噪聲方差陣；R為量測(cè)噪聲方差陣；δk,j為Kronecker符號(hào)。

根據(jù)UKF算法的一般形式，可知UKF與EKF具有相同的算法結(jié)構(gòu)，只是UKF算法直接使用了系統(tǒng)模型，避免了線性化帶來的誤差。為解決TSEKF算法對(duì)姿態(tài)角測(cè)量故障無法準(zhǔn)確估計(jì)的問題，直接將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階單位矩陣。狀態(tài)與偏差的耦合關(guān)系式為

(19)

(20)

(21)

(22)

λ=α2(n+κ)-n

1.2 自適應(yīng)改進(jìn)

TSUKF算法能夠估計(jì)出衛(wèi)星姿控系統(tǒng)中執(zhí)行機(jī)構(gòu)和敏感器故障的大小，但統(tǒng)計(jì)參數(shù)(Qx、Qb和R)的選取對(duì)故障估計(jì)的精度和速度影響較大，而實(shí)際系統(tǒng)中往往存在系統(tǒng)模型的統(tǒng)計(jì)參數(shù)無法準(zhǔn)確已知的情況。針對(duì)這一情況，在TSUKF算法的基礎(chǔ)上提出ATSUKF算法，在濾波過程中對(duì)以上參數(shù)進(jìn)行自適應(yīng)調(diào)整，保障故障估計(jì)結(jié)果的準(zhǔn)確性對(duì)仿真參數(shù)的變化不敏感。

受Hajiyev和Soken提出的魯棒自適應(yīng)卡爾曼濾波器[26]啟發(fā)，本文對(duì)噪聲協(xié)方差乘以一個(gè)多維的自適應(yīng)因子矩陣，使用測(cè)量數(shù)據(jù)實(shí)現(xiàn)對(duì)噪聲協(xié)方差矩陣的自適應(yīng)，用于系統(tǒng)模型參數(shù)未知情況下的衛(wèi)星姿控系統(tǒng)故障估計(jì)。

ATSUKF的系統(tǒng)模型與TSUKF相同，基本濾波器設(shè)計(jì)也與其相同，通過引入標(biāo)度矩陣S來實(shí)現(xiàn)對(duì)量測(cè)噪聲協(xié)方差的自適應(yīng)。系統(tǒng)量測(cè)方差表示為

Sk+1R

(23)

同時(shí)，根據(jù)實(shí)際測(cè)量數(shù)據(jù)可以得到

(24)

(25)

因此，有

Sk+1R

(26)

進(jìn)而可以算得

(27)

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

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

(28)

式中：

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

(29)

在對(duì)量測(cè)噪聲方差中的R進(jìn)行自適應(yīng)處理后，同樣可以對(duì)系統(tǒng)噪聲方差陣Qx和Qb求取相應(yīng)的自適應(yīng)矩陣。

對(duì)于Qx的自適應(yīng)矩陣Λx，對(duì)式(26)進(jìn)行處理可以得到

(30)

式中：

(31)

從而得到自適應(yīng)矩陣為

(32)

與標(biāo)度矩陣S一致，Qx的自適應(yīng)矩陣Λx同樣是對(duì)角線元素≥1的對(duì)角矩陣，因此定義

(33)

式中：

(34)

(35)

(36)

從而有

(37)

(38)

同樣對(duì)其進(jìn)行處理，得到

(39)

式中：

(40)

(41)

2 快速機(jī)動(dòng)衛(wèi)星姿態(tài)/故障估計(jì)

2.1 衛(wèi)星姿控系統(tǒng)建模

(42)

式中：

C=E6

φ1=Φ(x)

φ2=Φ(x)Abt(x)

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

(43)

對(duì)于衛(wèi)星姿控系統(tǒng)而言，當(dāng)衛(wèi)星處于姿態(tài)穩(wěn)定狀態(tài)下，通過姿態(tài)角的小量化假設(shè)可以將系統(tǒng)模型當(dāng)作線性系統(tǒng)處理，進(jìn)行常規(guī)的姿態(tài)濾波估計(jì)。另外，衛(wèi)星姿態(tài)穩(wěn)定時(shí)，控制器傳輸給執(zhí)行機(jī)構(gòu)的指令力矩近似為0，執(zhí)行機(jī)構(gòu)的乘性故障不會(huì)對(duì)系統(tǒng)姿態(tài)造成影響。

然而，當(dāng)衛(wèi)星處于姿態(tài)機(jī)動(dòng)狀態(tài)時(shí)，一方面，系統(tǒng)模型經(jīng)過線性化處理后往往無法準(zhǔn)確描述機(jī)動(dòng)過程中的系統(tǒng)動(dòng)態(tài)特性。另一方面，執(zhí)行機(jī)構(gòu)接收到的控制力矩指令實(shí)時(shí)變化，此時(shí)必須考慮執(zhí)行機(jī)構(gòu)的乘性故障對(duì)衛(wèi)星姿態(tài)造成的影響。

因此，在接下來的衛(wèi)星姿控系統(tǒng)故障估計(jì)過程中，設(shè)置衛(wèi)星姿態(tài)始終處于快速機(jī)動(dòng)狀態(tài)，在系統(tǒng)具有強(qiáng)非線性的條件下，對(duì)比驗(yàn)證TSUKF算法和ATSUKF算法的估計(jì)效果。

2.2 仿真分析

對(duì)TSUKF算法和ATSUKF算法進(jìn)行數(shù)值仿真對(duì)比，仿真條件與所需的初始值如下。

1) 衛(wèi)星的轉(zhuǎn)動(dòng)慣量矩陣為

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

跟蹤目標(biāo)姿態(tài)角φt為

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

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

4) 對(duì)于非線性的航天器姿控系統(tǒng)(43)，設(shè)計(jì)控制律如下：

式中：

KD和KP為控制器參數(shù)，在仿真時(shí)分別?。篕D=0.54E3,KP=0.09E3。

2.2.1執(zhí)行機(jī)構(gòu)故障

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

仿真結(jié)果如圖1所示。由于自適應(yīng)因子的變化過于劇烈，因此在圖中以對(duì)數(shù)形式指出其數(shù)量級(jí)的變化。

Qb=(2×10-4)2E3

Qx=

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

仿真結(jié)果如圖2所示。

圖1(a)、圖1(b)和圖2(a)、圖2(b)分別給出了對(duì)于x軸姿態(tài)角速度和姿態(tài)角的估計(jì)結(jié)果，可以發(fā)現(xiàn)，TSUKF算法和ATSUKF算法對(duì)于姿態(tài)信息的估計(jì)結(jié)果區(qū)別不大。

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

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

另外，從仿真結(jié)果可以看出，自適應(yīng)矩陣在故障發(fā)生變化時(shí)自適應(yīng)因子的突變可以直接用于判斷故障是否發(fā)生變化。

從圖1(d)和圖2(d)可以看出，TSUKF算法在卡爾曼濾波參數(shù)過小的情況下濾波收斂過慢，無法正常估計(jì)出故障幅值。而ATSUKF算法因?yàn)樽赃m應(yīng)矩陣的存在，可以根據(jù)量測(cè)數(shù)據(jù)調(diào)整噪聲方差矩陣的大小，從而實(shí)現(xiàn)對(duì)故障的快速估計(jì)，效果優(yōu)于TSUKF算法。

2.2.2 敏感器故障

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

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

仿真結(jié)果如圖3所示。

圖3 ATSUKF與TSUKF估計(jì)陀螺故障結(jié)果Fig.3 Faults estimation results of gyros by ATSUKF and TSUKF

從數(shù)學(xué)仿真結(jié)果可以看出，與飛輪故障類似，在系統(tǒng)參數(shù)設(shè)置不準(zhǔn)確的情況下，相比TSUKF算法，ATSUKF算法對(duì)姿控系統(tǒng)中光纖陀螺角速度測(cè)量故障的估計(jì)效果更好，能夠更快速地估計(jì)出故障的幅值。當(dāng)故障發(fā)生后自適應(yīng)因子變大，增大方差，加速濾波收斂。從對(duì)故障估計(jì)的結(jié)果可以看出，對(duì)于光纖陀螺故障，ATSUKF算法能夠很好地進(jìn)行估計(jì)。

綜合執(zhí)行機(jī)構(gòu)和敏感器故障的仿真結(jié)果可以看出，在統(tǒng)計(jì)特性不準(zhǔn)確的情況下，通過引入自適應(yīng)矩陣，ATSUKF算法對(duì)故障的估計(jì)效果優(yōu)于TSUKF，算法收斂更快，對(duì)故障的跟蹤效果更好。

3 結(jié) 論

1) 本文在TSKF中融入U(xiǎn)KF思想，首先得到TSUKF算法用于系統(tǒng)故障估計(jì)，并在此基礎(chǔ)上考慮系統(tǒng)噪聲統(tǒng)計(jì)信息不確定的情況下結(jié)合自適應(yīng)因子提出了ATSUKF算法。

2) 針對(duì)先驗(yàn)信息不足或故障發(fā)生變化等系統(tǒng)噪聲協(xié)方差無法準(zhǔn)確已知的情況，ATSUKF算法使用測(cè)量數(shù)據(jù)和殘差信息來自適應(yīng)地調(diào)整噪聲方差陣，與TSUKF算法相比，ATSUKF算法的估計(jì)結(jié)果收斂速度更快，避免了傳統(tǒng)TSUKF算法需要不斷調(diào)試濾波器參數(shù)的麻煩。