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

2019-03-05 12:01:12錢華明王迪吳永慧黃智開

北京航空航天大學學報 2019年2期

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

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

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

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

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

1 組合姿態算法設計

1.1 系統模型

1.1.1 陀螺的量測模型

在姿態估計系統中，陀螺儀用來測量衛星轉過角速率的姿態敏感器。陀螺儀產生的誤差因素有很多，如失準角誤差、標度因素誤差、漂移誤差、時間延遲等。大部分的誤差可以通過校正來補償，而隨機漂移、噪聲因素等是主要的誤差源，故陀螺量測模型可表示為[10]

(1)

1.1.2 星敏感器的量測模型

用四元數來表示量測模型為

pb=A(q)po+vs

(2)

1.2 狀態方程與量測方程

由四元數軌道動力學方程可得

(3)

(4)

式中：

(5)

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

(6)

由于星敏感器進行姿態估計時存在量測延時現象，上述建立的星敏感器量測模型沒有考慮延時的存在，使得輸出結果并不準確，筆者考慮一步延時的星敏感器量測模型：

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

(7)

(8)

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

(9)

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

(10)

式中：

(11)

式中：σ1、σ2為非負標量。

2 魯棒擴展卡爾曼濾波算法

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

一步狀態預測

(12)

狀態更新

(13)

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

(14)

2.1 估計誤差方差

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

一步狀態預測誤差可寫為

(15)

(16)

(17)

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

(18)

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

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

(19)

式中：

(20)

(21)

式(19)可簡化表示為

(22)

已知k+1時刻的狀態估計誤差與真實值和估計值有關，可以表示為

(23)

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

(24)

同式(16)相同，對量測函數泰勒展開，保留高階項，并代入式(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，因為所建立的系統模型考慮了乘性噪聲及量測延時等不確定性因素，其中包括未知矩陣如βk、αk+1，所以并不能實際求出協方差的具體值。根據最小方差準則的要求，求取預測誤差方差Pk+1|k和濾波更新誤差方差Pk+1的最小上界來近似替代。

2.2 算法設計

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

(28)

根據式(28)可以得到

(29)

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

(30)

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

(31)

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

(32)

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

(33)

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

(34)

同理，根據二階矩原理，假設存在正數ε2，同樣能求解得到

(35)

由式(15)可得到

(36)

假設存在正數ε3，同樣能求解得到

(37)

所以有

(38)

(39)

(40)

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

(41)

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

(42)

(43)

用數學歸納法證明Σk+1|k、Σk+1是Pk+1|k、Pk+1的上界范圍，即

(44)

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

P1|0≤Σ1|0

(45)

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

(46)

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

P1≤Σ1

(47)

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

(48)

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

(49)

(50)

(51)

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

3 仿真實驗與分析

3.1 仿真環境

3.2 仿真分析

1) 情況1

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

2) 情況2

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

從圖3和圖4可以看出，REKF算法處理帶有延時及乘性噪聲的非線性系統的效果要明顯優于RFHF算法、RKF算法及AEKF算法，這是因為建立了帶有乘性噪聲和延時的誤差模型來表示這種情況，而AEKF算法并不適用帶有延時的系統，RKF算法只能滿足乘性噪聲項所帶來的干擾，但沒有考慮延時問題。所以，當出現延時時，RKF算法并不能保證系統精度，甚至會帶來濾波發散。而RFHF算法只考慮了量測延時和量測丟失，沒有考慮乘性噪聲的影響，因此，在具有乘性噪聲的實驗環境下不能達到最理想的狀態。