基于魯棒擴展卡爾曼粒子濾波的RAIM算法

彭雅奇,許承東,牛 飛,李 臻,范國超

(1.北京理工大學宇航學院,北京 100081; 2.北京衛星導航中心,北京 100094)

0 引 言

完好性是衛星導航系統的重要服務性能,通過完好性監測可以對故障星進行檢測和識別并向用戶報警,從而提高導航系統的可靠性[1]。由于通過衛星自身的系統配置進行故障檢測時,告警時間較長,通常在15分鐘到幾個小時[2],滿足不了實際應用,而在用戶端進行的接收機自主完好性監測(receiver autonomous integrity monitoring,RAIM)對故障星反應迅速,自主性強,利用接收機內部的冗余度信息實現衛星故障檢測和故障識別,能及時有效地給用戶提供告警信息,而且用戶級完好性實現簡單,投入成本低,因此成為一種有效的完好性監測方法[3-4]。

早期經典的RAIM算法包括Lee[5]提出的偽距比較法、Parkinson[6]提出的最小二乘殘差法和Sturza[7]提出的奇偶矢量法,這3種方法都通過利用當前歷元的冗余觀測量來進行故障診斷,被統稱為快照式算法[8],由于這些算法難以檢測微小誤差,因此一些學者提出利用濾波式RAIM算法,充分利用歷史觀測信息和當前歷元信息來降低測量噪聲水平[9],例如Harsha[10]、沙海[11]等提出的基于卡爾曼濾波(kalman filter,KF)的RAIM算法,得到了比快照式算法更好的監測性能,但這類算法要求系統噪聲服從高斯分布,否則濾波性能就會下降[12-13]。由于系統測量誤差并不完全服從高斯分布,因此近年來將基于非線性、非高斯系統的粒子濾波(particle filter,PF)用于RAIM算法成為研究熱點,但PF算法中普遍存在粒子退化現象,針對這一問題,通常解決的方法有選擇合理的建議密度函數和優化重采樣策略[14]。國外Schwiegelshohn[15]、Tran[16]、Daneshyar[17]等都對PF提出諸多改進,一定程度上改善了粒子退化,國內王爾申等也將PF用于RAIM算法并結合遺傳算法、粒子群優化等提出了相應改進[18-20],提高了RAIM算法在非高斯噪聲下的檢測性能,但同時也增加了算法的復雜度,不利于實際工程應用。

本文在前人的研究基礎上提出基于魯棒擴展卡爾曼粒子濾波(robust extended Kalman particle filter,REKPF)的RAIM算法,用加入魯棒估計的擴展卡爾曼濾波(extended Kalman filter,EKF)來計算粒子的建議密度函數,使先驗分布朝著高似然區域移動,提高PF的估計精度。同時結合累加對數似然比(log-likelihood ratio,LLR)對故障衛星進行檢測,通過實測數據對算法進行仿真,結果表明,基于REKPF的RAIM算法能有效改善粒子退化現象,在偽距存在偏差時,具有更短的告警時間和更高的定位精度。

1 粒子濾波

設系統的狀態方程和觀測方程[21]分別為

xk=f(xk-1,wk-1)

(1)

zk=h(xk,vk)

(2)

式中,xk=[rx,ry,rz,Δtu]T,(rx,ry,rz)為地心地固坐標系下接收機的三維位置坐標,Δtu為接收機鐘差;f為狀態轉移函數;wk為過程噪聲;zk為系統的觀測矩陣;h表示xk和zk之間的非線性關系;vk為觀測噪聲。

偽距觀測方程為

(3)

PF的核心思想是利用有限個隨機采樣粒子的加權和來近似表示狀態變量的后驗概率分布,從而得到狀態的估計值[22]。PF主要步驟如下:

步驟1粒子初始化,k=0。

根據先驗概率密度函數p(x0)抽取隨機樣本構成初始粒子集{x0(i);i=1,2,…,N}～p(x0),其中N為粒子數目,每個粒子的權重初始化為1/N,表示為{ω0(i)=1/N;i=1,2,…,N}。

步驟2當k=1,2…時

步驟2.1重要性采樣

在k歷元時,根據建議密度函數q(xk|xk-1(i),zk)抽取隨機樣本為

{xk/k-1(i);i=1,2,…,N}～q(xk|xk-1(i),zk)

(4)

步驟2.2權值更新

重新計算每個粒子的權值為

ωk(i)=ωk-1(i)p(zk|xk(i))

(5)

權值歸一化為

(6)

步驟2.3重采樣

步驟2.4狀態估計

計算當前歷元的狀態估計值為

(7)

步驟3令k=k+1,轉入下一個歷元計算。

2 REKPF

PF中退化現象不可避免,由于粒子權值的方差在迭代中不斷增加,迭代一段時間后,除少數粒子外,其他粒子的權值小到可以忽略不計,使得大量計算資源消耗在處理那些微不足道的粒子上,不僅造成資源浪費,也影響了最終估計結果[23]。

選取合理的建議密度函數可以改善粒子退化,根據最小方差原則,最優建議密度函數應滿足q(xk|xk-1(i),zk)opt=p(xk|xk-1(i),zk)[24],但實際應用中很難從p(xk|xk-1(i),zk)中采樣,文獻[25]利用EKF產生建議分布在角度跟蹤的應用中取得了很好的效果。然而,當觀測數據存在偏差時,通過EKF得到的分布會受到一定的影響,建議密度函數的準確性也會下降。針對這一問題,本文提出在用EKF計算建議密度函數時加入魯棒估計,使得偽距存在偏差時粒子采樣更加準確,REKPF算法主要步驟如下:

步驟1粒子初始化,k=0,同PF。

步驟2當k=1,2…時

步驟2.1通過REKF進行粒子狀態估計

(8)

(9)

(10)

(11)

(12)

圖1 基于IGGⅢ方案的權值處理Fig.1 Weight processing based on IGGⅢ scheme

本文等價權的計算為

(13)

步驟2.2重要性采樣

計算粒子集的均值和方差為

(14)

(15)

通過建議密度函數采樣得到

(16)

步驟2.3權值更新和重采樣,同PF;

步驟2.4狀態估計,同PF;

步驟3令k=k+1,轉入下一個歷元計算。

3 基于REKPF的RAIM算法

3.1 累加LLR檢驗統計量

將LLR檢驗定義為各輔助PF和主PF的概率密度函數之比[18],表達式為

(17)

量測量yi到yk的累加LLR可以表示為

(18)

由于系統狀態估計的似然函數可用PF中粒子的歸一化權值近似表示,故式(18)中的pq(yi|Yi-1)和pM(yi|Yi-1)可表示為

(19)

3.2 故障檢測

設當前可見星數目s=7,若其中1顆星出現故障,則需要構建s+1個PF,其中1個主PF包含所有衛星的觀測量,其余s個輔助PF包含去除1顆衛星后其余衛星的觀測量,最后計算累加LLR,將所有濾波結果進行一致性檢驗,如圖2所示。當某顆衛星出現故障時,不含該故障星的輔助PF的累加LLR與其他輔助PF具有明顯區別,在一致性檢驗中便會出現告警。

圖2 基于REKPF和累加LLR的故障檢測原理框圖Fig.2 Fault detection principle diagram based on REKPF and cumulative LLR

基于REKPF和LLR檢測的RAIM算法流程如下:

步驟1k=0

分別初始化主輔PF的粒子集為

(20)

(21)

步驟2當k=1,2…時

步驟2.1每個粒子的狀態估計通過REKF計算,先驗粒子集由重要性采樣得到。

步驟2.2權值更新和重采樣

步驟2.3計算累加LLR

(22)

步驟2.4故障診斷

步驟3令k=k+1,轉入下一個歷元計算。

4 仿真驗證

本文仿真數據從NASA戈達德空間飛行中心地殼動力學數據信息系統(crustal dynamics data information system,CDDIS)下載,選取武漢市九峰鄉的JFNG跟蹤站在2014年第342天8:00起500s的全球定位系統(global position system,GPS)觀測數據進行故障檢測試驗,這段時間內可以觀測到7顆GPS衛星,編號分別為5,15,18,21,24,26,29。在試驗中,仿真步長設置為1 s,粒子數N=100,累加歷元的窗口長度選為30,故障檢測門限τ=200,無故障時仿真結果如圖3和圖4所示。

圖3 無故障時的故障檢測檢驗統計量Fig.3 Fault detection test statistics under normal condition

由圖3可以看出,在衛星無故障情況下,基于PF和REKPF的RAIM檢驗統計量都比較小,遠遠小于檢測門限200,說明在這段仿真時間內未檢測到故障發生。

圖4 無故障時累加對數似然比Fig.4 Cumulative LLR under normal condition

圖4中,各衛星的累加LLR也比較小,7顆衛星的累加LLR大小變化不明顯,沒有明顯的某顆衛星累加LLR大大超過其他的衛星值,表明沒有故障衛星。

4.1 對階躍類型偽距偏差的檢測效果

為了驗證本文算法對階躍類型偽距偏差的檢測能力,對18號衛星從200 s開始人為加入30 m偽距偏差作為階躍類型的故障,仿真結果如圖5～圖7所示。

圖5 有故障時用于故障檢測的檢驗統計量Fig.5 Fault detection test statistics under fault condition

由圖5可以看出,在t<200 s時,累加LLR值較小,滿足Tr<τ,即認為該階段未發生衛星故障;在200 s后,18號星的累加LLR值明顯增加,采用PF的RAIM算法在215 s告警,而采用REKPF的RAIM算法在207 s觸發告警,有效縮短了延遲時間。

圖6 階躍故障時累加對數似然比Fig.6 Cumulative LLR under step fault condition

圖6中,REKPF算法的累加LLR跳變值高于PF算法的,提高了故障檢測的靈敏度;再者,PF算法其他星的累加LLR也有相應增加,而REKPF的其他星累加LLR則非常穩定,降低了誤警概率。

圖7 階躍故障時誤差分析Fig.7 Error analysis under step fault condition

由圖7可以看出,當偽距存在偏差時,PF算法的估計誤差有明顯增加,而REKPF算法受偽距偏差影響較小,這表明即使故障未被隔離,REKPF算法仍然能夠準確估計接收機的狀態。

表1 不同階躍偽距偏差下性能對比Table 1 Performance comparison of different step pseudo-range bias

可以看出,在不同偽距偏差下,基于REKPF的RAIM算法對于故障檢測的延遲時間都小于基于PF的RAIM算法,由于采用了更加合理的建議密度函數,大大提高了用于檢測的有效粒子數,同時加入了魯棒估計,使得RMSE值基本不受影響,提高了系統穩定性。

4.2 對緩變類型偽距偏差的檢測效果

為了進一步驗證本文算法對緩變類型偽距偏差的檢測能力,對18號衛星的偽距從200 s開始人為加入,由0 m開始以0.1 m/s的速率緩慢增長的偽距偏差,仿真結果如圖8和圖9所示。

圖8 緩變故障時累加LLRFig.8 Cumulative LLR under slowly growing fault condition

由圖8可以看出,從300s左右開始,累加LLR值開始出現明顯差別,采用REKPF的RAIM算法對于緩變偽距偏差的響應速度明顯快于采用PF的RAIM算法,說明本文算法對于緩變偏差的檢測效果要優于PF算法,同樣誤警概率也較低。

圖9 緩變故障時誤差分析Fig.9 Error analysis under slowly growing fault condition

由圖9可以看出,隨著緩變偽距偏差的積累,PF算法的定位誤差會隨之增長,而應用REKPF算法的定位誤差僅受到微小影響,當誤差達到一定程度后,對應故障星的權因子會逐漸降為0,定位誤差即恢復正常水平,不受故障星的干擾,說明本文算法具有一定的魯棒性。

在不同偽距變化速率下,仿真結果如表2所示。

表2 不同緩變速率偽距偏差下性能對比Table 2 Performance comparison of different slowly growing pseudo-range bias

5 結 論

為減小PF中退化現象的影響,提高基于PF的RAIM算法對偽距偏差的檢測能力,本文利用EKF計算PF的建議密度函數,并加入了基于IGGⅢ方案的魯棒估計,結合累加LLR檢測方法,建立基于REKPF的RAIM算法。并分別對階躍類型和緩變類型偽距偏差進行驗證,仿真結果表明:本文算法有效粒子數可達90%左右,顯著高于PF算法,較大程度改善了粒子退化;在兩種類型偽距偏差下,本文RAIM算法對于故障衛星的檢測延遲時間都得到了有效縮短,而且誤警概率較低;同時在定位精度方面,受故障衛星的影響較小,具有更穩健的估計精度。