彭雅奇,許承東,牛 飛,李 臻,范國(guó)超

(1.北京理工大學(xué)宇航學(xué)院,北京 100081; 2.北京衛(wèi)星導(dǎo)航中心,北京 100094)

0 引 言

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

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

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

1 粒子濾波

設(shè)系統(tǒng)的狀態(tài)方程和觀測(cè)方程[21]分別為

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

(1)

zk=h(xk,vk)

(2)

式中,xk=[rx,ry,rz,Δtu]T,(rx,ry,rz)為地心地固坐標(biāo)系下接收機(jī)的三維位置坐標(biāo),Δtu為接收機(jī)鐘差;f為狀態(tài)轉(zhuǎn)移函數(shù);wk為過(guò)程噪聲;zk為系統(tǒng)的觀測(cè)矩陣;h表示xk和zk之間的非線性關(guān)系;vk為觀測(cè)噪聲。

偽距觀測(cè)方程為

(3)

PF的核心思想是利用有限個(gè)隨機(jī)采樣粒子的加權(quán)和來(lái)近似表示狀態(tài)變量的后驗(yàn)概率分布,從而得到狀態(tài)的估計(jì)值[22]。PF主要步驟如下:

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

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

步驟2當(dāng)k=1,2…時(shí)

步驟2.1重要性采樣

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

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

(4)

步驟2.2權(quán)值更新

重新計(jì)算每個(gè)粒子的權(quán)值為

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

(5)

權(quán)值歸一化為

(6)

步驟2.3重采樣

步驟2.4狀態(tài)估計(jì)

計(jì)算當(dāng)前歷元的狀態(tài)估計(jì)值為

(7)

步驟3令k=k+1,轉(zhuǎn)入下一個(gè)歷元計(jì)算。

2 REKPF

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

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

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

步驟2當(dāng)k=1,2…時(shí)

步驟2.1通過(guò)REKF進(jìn)行粒子狀態(tài)估計(jì)

(8)

(9)

(10)

(11)

(12)

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

本文等價(jià)權(quán)的計(jì)算為

(13)

步驟2.2重要性采樣

計(jì)算粒子集的均值和方差為

(14)

(15)

通過(guò)建議密度函數(shù)采樣得到

(16)

步驟2.3權(quán)值更新和重采樣,同PF;

步驟2.4狀態(tài)估計(jì),同PF;

步驟3令k=k+1,轉(zhuǎn)入下一個(gè)歷元計(jì)算。

3 基于REKPF的RAIM算法

3.1 累加LLR檢驗(yàn)統(tǒng)計(jì)量

將LLR檢驗(yàn)定義為各輔助PF和主PF的概率密度函數(shù)之比[18],表達(dá)式為

(17)

量測(cè)量yi到y(tǒng)k的累加LLR可以表示為

(18)

由于系統(tǒng)狀態(tài)估計(jì)的似然函數(shù)可用PF中粒子的歸一化權(quán)值近似表示,故式(18)中的pq(yi|Yi-1)和pM(yi|Yi-1)可表示為

(19)

3.2 故障檢測(cè)

設(shè)當(dāng)前可見(jiàn)星數(shù)目s=7,若其中1顆星出現(xiàn)故障,則需要構(gòu)建s+1個(gè)PF,其中1個(gè)主PF包含所有衛(wèi)星的觀測(cè)量,其余s個(gè)輔助PF包含去除1顆衛(wèi)星后其余衛(wèi)星的觀測(cè)量,最后計(jì)算累加LLR,將所有濾波結(jié)果進(jìn)行一致性檢驗(yàn),如圖2所示。當(dāng)某顆衛(wèi)星出現(xiàn)故障時(shí),不含該故障星的輔助PF的累加LLR與其他輔助PF具有明顯區(qū)別,在一致性檢驗(yàn)中便會(huì)出現(xiàn)告警。

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

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

步驟1k=0

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

(20)

(21)

步驟2當(dāng)k=1,2…時(shí)

步驟2.1每個(gè)粒子的狀態(tài)估計(jì)通過(guò)REKF計(jì)算,先驗(yàn)粒子集由重要性采樣得到。

步驟2.2權(quán)值更新和重采樣

步驟2.3計(jì)算累加LLR

(22)

步驟2.4故障診斷

步驟3令k=k+1,轉(zhuǎn)入下一個(gè)歷元計(jì)算。

4 仿真驗(yàn)證

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

圖3 無(wú)故障時(shí)的故障檢測(cè)檢驗(yàn)統(tǒng)計(jì)量Fig.3 Fault detection test statistics under normal condition

由圖3可以看出,在衛(wèi)星無(wú)故障情況下,基于PF和REKPF的RAIM檢驗(yàn)統(tǒng)計(jì)量都比較小,遠(yuǎn)遠(yuǎn)小于檢測(cè)門限200,說(shuō)明在這段仿真時(shí)間內(nèi)未檢測(cè)到故障發(fā)生。

圖4 無(wú)故障時(shí)累加對(duì)數(shù)似然比Fig.4 Cumulative LLR under normal condition

圖4中,各衛(wèi)星的累加LLR也比較小,7顆衛(wèi)星的累加LLR大小變化不明顯,沒(méi)有明顯的某顆衛(wèi)星累加LLR大大超過(guò)其他的衛(wèi)星值,表明沒(méi)有故障衛(wèi)星。

4.1 對(duì)階躍類型偽距偏差的檢測(cè)效果

為了驗(yàn)證本文算法對(duì)階躍類型偽距偏差的檢測(cè)能力,對(duì)18號(hào)衛(wèi)星從200 s開(kāi)始人為加入30 m偽距偏差作為階躍類型的故障,仿真結(jié)果如圖5～圖7所示。

圖5 有故障時(shí)用于故障檢測(cè)的檢驗(yàn)統(tǒng)計(jì)量Fig.5 Fault detection test statistics under fault condition

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

圖6 階躍故障時(shí)累加對(duì)數(shù)似然比Fig.6 Cumulative LLR under step fault condition

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

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

由圖7可以看出,當(dāng)偽距存在偏差時(shí),PF算法的估計(jì)誤差有明顯增加,而REKPF算法受偽距偏差影響較小,這表明即使故障未被隔離,REKPF算法仍然能夠準(zhǔn)確估計(jì)接收機(jī)的狀態(tài)。

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

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

4.2 對(duì)緩變類型偽距偏差的檢測(cè)效果

為了進(jìn)一步驗(yàn)證本文算法對(duì)緩變類型偽距偏差的檢測(cè)能力,對(duì)18號(hào)衛(wèi)星的偽距從200 s開(kāi)始人為加入,由0 m開(kāi)始以0.1 m/s的速率緩慢增長(zhǎng)的偽距偏差,仿真結(jié)果如圖8和圖9所示。

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

由圖8可以看出,從300s左右開(kāi)始,累加LLR值開(kāi)始出現(xiàn)明顯差別,采用REKPF的RAIM算法對(duì)于緩變偽距偏差的響應(yīng)速度明顯快于采用PF的RAIM算法,說(shuō)明本文算法對(duì)于緩變偏差的檢測(cè)效果要優(yōu)于PF算法,同樣誤警概率也較低。

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

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

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

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

5 結(jié) 論

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