小視場星敏感器量測延時濾波算法

錢華明， 王迪， 吳永慧， 黃智開

(哈爾濱工程大學(xué) 自動化學(xué)院， 哈爾濱 150001)

小視場(Narrow Filed of View,NFOV)星敏感器成本較低，并且在極端天氣下更加適用，因此小視場星敏感器的姿態(tài)估計(jì)算法具有重要的研究價值。通過對導(dǎo)航星表進(jìn)行預(yù)處理，導(dǎo)致星圖識別過程中識別時間增加，影響星敏感器輸出信息的時間，造成量測延時問題。同時，光學(xué)系統(tǒng)以及確定姿態(tài)等步驟的繁瑣性，也導(dǎo)致了姿態(tài)敏感器輸出信息的同時帶有延時現(xiàn)象。

延時問題的存在，導(dǎo)致系統(tǒng)模型中存在了不確定性。魯棒濾波算法被證實(shí)是一種處理模型不確定性問題的有效算法[1-3]。文獻(xiàn)[4]討論了針對有界模型不確定的線性時域系統(tǒng)二階魯棒卡爾曼濾波(Robust Kalman Filter，RKF)算法，但該模型不適用于非線性系統(tǒng)。文獻(xiàn)[5]提出魯棒擴(kuò)展卡爾曼濾波(Robust Extended Kalman Filter，REKF)算法，通過預(yù)測-校正形式估計(jì)剛體姿態(tài)，所提出的姿態(tài)估計(jì)系統(tǒng)考慮四元數(shù)剛體模型，其建模時主要考慮的是加性噪聲，但其不適于乘性耦合噪聲的環(huán)境。文獻(xiàn)[6]設(shè)計(jì)了一種RKF算法處理乘性耦合噪聲，但其不能很好地解決量測延時帶來的影響。文獻(xiàn)[7]設(shè)計(jì)了一種RKF算法用于處理狀態(tài)延遲與量測丟失的時變離散系統(tǒng)，通過求取狀態(tài)估計(jì)協(xié)方差的最小上界來計(jì)算需要的參數(shù)。文獻(xiàn)[8]提出了魯棒有界時域?yàn)V波(Robust Finite Horizon Filter，RFHF)算法用于解決確定概率分布情況的延遲系統(tǒng)。但是文獻(xiàn)[7-8]這2種算法只適用于線性系統(tǒng)。文獻(xiàn)[9]提出了基于5階球半徑規(guī)則的容積卡爾曼濾波算法，使其更適用于非線性系統(tǒng)，解決傳感器隨機(jī)延時的問題，然而該算法在建模時沒有考慮乘性耦合噪聲，且與本文所研究的實(shí)際情況并不相符。

因此，針對小視場星敏感器與陀螺儀結(jié)合的模型，本文提出了REKF算法用于處理量測延時因素引起的模型不確定情況。通過狀態(tài)擴(kuò)維理論獲得帶有量測延時的非線性系統(tǒng)模型，并且建立的系統(tǒng)模型包含乘性耦合噪聲。根據(jù)最小方差準(zhǔn)則近似確定估計(jì)誤差方差的最小上界范圍，通過該最小上界選取濾波增益參數(shù)，從而得到量測延時情況下的REKF算法。仿真結(jié)果表明，本文算法所能達(dá)到的估計(jì)精度較原有算法有明顯提高。

1 組合姿態(tài)算法設(shè)計(jì)

1.1 系統(tǒng)模型

1.1.1 陀螺的量測模型

在姿態(tài)估計(jì)系統(tǒng)中，陀螺儀用來測量衛(wèi)星轉(zhuǎn)過角速率的姿態(tài)敏感器。陀螺儀產(chǎn)生的誤差因素有很多，如失準(zhǔn)角誤差、標(biāo)度因素誤差、漂移誤差、時間延遲等。大部分的誤差可以通過校正來補(bǔ)償，而隨機(jī)漂移、噪聲因素等是主要的誤差源，故陀螺量測模型可表示為[10]

(1)

1.1.2 星敏感器的量測模型

用四元數(shù)來表示量測模型為

pb=A(q)po+vs

(2)

1.2 狀態(tài)方程與量測方程

由四元數(shù)軌道動力學(xué)方程可得

(3)

(4)

式中：

(5)

根據(jù)式(2)，選取星敏感器輸出的姿態(tài)四元數(shù)作為量測變量，選取3個參考矢量描述姿態(tài)估計(jì)系統(tǒng)，故系統(tǒng)的量測模型為

(6)

由于星敏感器進(jìn)行姿態(tài)估計(jì)時存在量測延時現(xiàn)象，上述建立的星敏感器量測模型沒有考慮延時的存在，使得輸出結(jié)果并不準(zhǔn)確，筆者考慮一步延時的星敏感器量測模型：

yk=(Ι-Γk)zk+Γkzk-1

(7)

(8)

故根據(jù)式(4)、式(6)、式(7)可以得出關(guān)于量測延時及乘性噪聲的非線性離散系統(tǒng)：

(9)

式中：Cik為有恰當(dāng)維數(shù)的已知矩陣;ξik為均值為零、方差為1的噪聲;vk為零均值高斯白噪聲。由于實(shí)際的量測輸出yk與zk、zk-1兩個時刻的量測量均相關(guān)，需要利用狀態(tài)擴(kuò)維理論，使其得到相同時刻量測量的表達(dá)式。

(10)

式中：

(11)

式中：σ1、σ2為非負(fù)標(biāo)量。

2 魯棒擴(kuò)展卡爾曼濾波算法

針對帶有量測延時及乘性噪聲的離散系統(tǒng)式(9)，經(jīng)過狀態(tài)擴(kuò)維理論，獲得一個相對簡單的模型式(10)，提出改進(jìn)的REKF算法，并且計(jì)算預(yù)測方差，給出估計(jì)誤差方差的最小上界。

一步狀態(tài)預(yù)測

(12)

狀態(tài)更新

(13)

根據(jù)以上分析，對于帶有乘性噪聲及量測延時的不確定性系統(tǒng)，就是設(shè)計(jì)式(10)的REKF算法的問題。由于實(shí)際的誤差方差難以求出，只能通過確定濾波增益參數(shù)來確保誤差方差的最小上界，即

(14)

2.1 估計(jì)誤差方差

在REKF算法中，通過選取一些模型不確定參數(shù)來表示其對系統(tǒng)的影響。與EKF算法類似，需要對狀態(tài)函數(shù)與量測函數(shù)進(jìn)行泰勒展開，主要區(qū)別在于根據(jù)上界范圍進(jìn)行濾波器的具體設(shè)計(jì)。通過這樣的計(jì)算，使得求得的精度、效果等相較于EKF算法更優(yōu)良。

一步狀態(tài)預(yù)測誤差可寫為

(15)

(16)

(17)

將式(16)、式 (17)代入式(15)可得

(18)

由于ηik服從高斯分布，不同時刻之間是互相獨(dú)立的，其相關(guān)性為0。而ηik與wk是互相獨(dú)立的高斯白噪聲，均值為零。故一步預(yù)測誤差方差矩陣可表示為

BkβkLk)T]+E[g(Xk,ηk)gT(Xk,ηk)]+

(19)

式中：

(20)

(21)

式(19)可簡化表示為

(22)

已知k+1時刻的狀態(tài)估計(jì)誤差與真實(shí)值和估計(jì)值有關(guān)，可以表示為

(23)

將式(10)代入式(23)中，可以得到

(24)

同式(16)相同，對量測函數(shù)泰勒展開，保留高階項(xiàng)，并代入式(24)得

(25)

將式(25)代入式(24)中，所以有

Kk+1γk+1s(Xk+1,ξk+1)-Kk+1γk+1Vk+1-

(26)

將式(26)代到式(14)中，得到濾波誤差方差矩陣，表示為

(27)

由式(22)、式(27)可以得到方差矩陣Pk+1|k、Pk+1，因?yàn)樗⒌南到y(tǒng)模型考慮了乘性噪聲及量測延時等不確定性因素，其中包括未知矩陣如βk、αk+1，所以并不能實(shí)際求出協(xié)方差的具體值。根據(jù)最小方差準(zhǔn)則的要求，求取預(yù)測誤差方差Pk+1|k和濾波更新誤差方差Pk+1的最小上界來近似替代。

2.2 算法設(shè)計(jì)

根據(jù)文獻(xiàn)[14]中的2條引理(引理6.1、引理6.2)構(gòu)造上界范圍，并且可以求得濾波更新誤差方差矩陣Pk+1的最小上界。

(28)

根據(jù)式(28)可以得到

(29)

E[g(Xk,ηk)gT(Xk,ηk)]≤

(30)

(Fk+BkβkLk)Pk(Fk+BkβkLk)T≤

(31)

將式(29)～式 (31)代入到式(22)中，可以求出一步預(yù)測誤差方差矩陣的上界：

(32)

E(s(Xk+1,ξk+1)sT(Xk+1,ξk+1))=

(33)

將式(33)代入式(27)，得到濾波誤差方差矩陣為

(34)

同理，根據(jù)二階矩原理，假設(shè)存在正數(shù)ε2，同樣能求解得到

(35)

由式(15)可得到

(36)

假設(shè)存在正數(shù)ε3，同樣能求解得到

(37)

所以有

(38)

(39)

(40)

所以，k+1時刻的更新后狀態(tài)誤差的方差矩陣為

(41)

根據(jù)式(32)、式 (41)得到了一步預(yù)測方差和濾波更新方差的上界，當(dāng)ε1、ε2、ε3、λ、μ都是正數(shù)時，提出以下2個離散Riccati方程：

(42)

(43)

用數(shù)學(xué)歸納法證明Σk+1|k、Σk+1是Pk+1|k、Pk+1的上界范圍，即

(44)

在初始k=0時刻，能夠輕易求出P0=Σ0>0，根據(jù)不等式(29)、式(30)以及等式(43)可得

P1|0≤Σ1|0

(45)

由式(45)及式(44)中的第一項(xiàng)可以得出

(46)

由式(45)和式(46)，可以推斷出k=0時刻的狀態(tài)誤差方差滿足：

P1≤Σ1

(47)

選取k=n-1時刻，假設(shè)滿足不等式Pn≤Σn，那么當(dāng)k=n時，同式(46)類似，有

(48)

因此，根據(jù)式(44)可以得出

(49)

(50)

(51)

設(shè)計(jì)濾波器的目的是根據(jù)狀態(tài)模型和不同觀測量中的信息計(jì)算狀態(tài)變量的估計(jì)值，達(dá)到狀態(tài)量的估計(jì)值與真實(shí)值誤差最小，精度最高；在濾波設(shè)計(jì)過程中，需要權(quán)衡多種不同信息，充分合理地利用這些信息。REKF的設(shè)計(jì)是基于不確定的系統(tǒng)模型進(jìn)行的，REKF算法的性能在于不確定性模型是否能完整描述實(shí)際系統(tǒng)。為了獲得理想的濾波效果，式(1)給出的不確定性模型需要準(zhǔn)確給出不確定性參數(shù)的大小。

3 仿真實(shí)驗(yàn)與分析

3.1 仿真環(huán)境

3.2 仿真分析

1) 情況1

從圖1和圖2可以看出，當(dāng)假設(shè)系統(tǒng)不存在延時情況時，各濾波算法的性能相差不大，基本都能達(dá)到0.000 1°左右的精度。整體來看，REKF算法和RKF算法要略優(yōu)于AEKF算法和RFHF算法，這是由于AEKF算法和RFHF算法在設(shè)計(jì)時都沒有考慮到乘性噪聲的影響，導(dǎo)致它們在非線性估計(jì)時存在一定誤差，因此濾波精度稍差。

2) 情況2

假設(shè)星敏感器工作時出現(xiàn)延時現(xiàn)象，設(shè)3個方向的矢量延時速率各不相同的，并且滿足伯努利分布：

從圖3和圖4可以看出，REKF算法處理帶有延時及乘性噪聲的非線性系統(tǒng)的效果要明顯優(yōu)于RFHF算法、RKF算法及AEKF算法，這是因?yàn)榻⒘藥в谐诵栽肼暫脱訒r的誤差模型來表示這種情況，而AEKF算法并不適用帶有延時的系統(tǒng)，RKF算法只能滿足乘性噪聲項(xiàng)所帶來的干擾，但沒有考慮延時問題。所以，當(dāng)出現(xiàn)延時時，RKF算法并不能保證系統(tǒng)精度，甚至?xí)頌V波發(fā)散。而RFHF算法只考慮了量測延時和量測丟失，沒有考慮乘性噪聲的影響，因此，在具有乘性噪聲的實(shí)驗(yàn)環(huán)境下不能達(dá)到最理想的狀態(tài)。

圖1 情況1時姿態(tài)角均方根誤差對比Fig.1 Comparison of root mean square error of attitude angle in Case 1

圖2 情況1時姿態(tài)角誤差對比Fig.2 Comparison of attitude angle error in Case 1

圖3 情況2時姿態(tài)角均方根誤差對比Fig.3 Comparison of RMSE of attitude angle in Case 2

圖4 情況2時姿態(tài)角誤差對比Fig.4 Comparison of attitude angle error in case 2

4 結(jié) 論

針對小視場星敏感器姿態(tài)估計(jì)時存在的信息延時問題，本文做了如下研究：

1) 建立帶有延時不確定項(xiàng)的誤差模型，該模型考慮到非線性系統(tǒng)同時存在乘性噪聲及量測模型延時的情況，對REKF濾波算法進(jìn)行改進(jìn)。

2) 在算法設(shè)計(jì)時，根據(jù)最小均方誤差準(zhǔn)則要求，通過求取預(yù)測誤差方差和濾波更新誤差方差的最小上界進(jìn)而確定濾波增益的最優(yōu)值。REKF算法的性能在于不確定性模型來表示實(shí)際系統(tǒng)，從而達(dá)到狀態(tài)估計(jì)誤差最小，精度最高。

3) 仿真結(jié)果表明，REKF濾波算法可以有效解決量測延時問題，提高姿態(tài)估計(jì)的精度。