蘭曉偉,許承東,趙 靖

(1. 北京理工大學(xué)宇航學(xué)院,北京 100081;2. 中國交通通信信息中心,北京 100011)

1 引言

完好性是導(dǎo)航系統(tǒng)四種關(guān)鍵服務(wù)性能(精度、連續(xù)性、完好性、可用性)之一,用于提供對(duì)導(dǎo)航系統(tǒng)所提供信息正確性的置信度的測量,也包括系統(tǒng)在無法用于導(dǎo)航時(shí)向用戶發(fā)出告警[1]。完好性監(jiān)測是確保導(dǎo)航系統(tǒng)滿足完好性風(fēng)險(xiǎn)需求的重要手段,與導(dǎo)航用戶的生命財(cái)產(chǎn)安全息息相關(guān)。現(xiàn)有的完好性監(jiān)測技術(shù)包含故障檢測和完好性風(fēng)險(xiǎn)評(píng)估兩方面,前者通過構(gòu)造檢驗(yàn)統(tǒng)計(jì)量與檢測閾值判斷有無故障發(fā)生,后者提供保護(hù)級(jí)限定用戶定位誤差的安全邊界[2]。

接收機(jī)自主完好性監(jiān)測(Receiver Autonomous Integrity Monitoring,RAIM)是目前針對(duì)全球?qū)Ш叫l(wèi)星系統(tǒng)(Global Navigation Satellite System,GNSS)應(yīng)用最廣泛的完好性監(jiān)測手段。眾多學(xué)者針對(duì)RAIM算法展開了廣泛而深入的研究,如雙星座RAIM算法[3]、模糊聚類RAIM算法[4]和高級(jí)RAIM算法(Advanced RAIM,ARAIM)[5]等。但僅依靠GNSS測量信息,難以滿足完好性需求更為嚴(yán)苛的場景。

GNSS與慣性導(dǎo)航系統(tǒng)(Inertial Navigation System,INS)具有優(yōu)勢互補(bǔ)的特點(diǎn),二者的組合能夠顯著改善導(dǎo)航精度,同時(shí)也為增強(qiáng)導(dǎo)航系統(tǒng)的完好性提供了可能。傳統(tǒng)的GNSS/INS組合導(dǎo)航系統(tǒng)基于卡爾曼濾波器實(shí)現(xiàn)。濾波器的遞推特性對(duì)于完好性監(jiān)測算法的設(shè)計(jì)造成了較大困難?，F(xiàn)有的基于濾波器實(shí)現(xiàn)的GNSS/INS完好性監(jiān)測算法大多只考慮故障檢測,雖提高了對(duì)于衛(wèi)星故障的檢測效果,但忽略了保護(hù)級(jí)的計(jì)算[6],[7]。

近年來,因子圖優(yōu)化(Factor Graph Optimization,FGO)作為一種新型導(dǎo)航估計(jì)算法逐漸受到關(guān)注。研究表明,相較于擴(kuò)展卡爾曼濾波器,FGO使得GNSS/INS組合導(dǎo)航系統(tǒng)能夠獲得更高的導(dǎo)航精度[8]。此外,FGO將GNSS/INS組合導(dǎo)航的非線性數(shù)據(jù)融合問題轉(zhuǎn)化為非線性最小二乘問題,其求解過程本質(zhì)為線性最小二乘的多次迭代,該特點(diǎn)也為故障檢測和保護(hù)級(jí)計(jì)算提供了便利。

因此,本文通過引入因子圖優(yōu)化將GNSS/INS組合導(dǎo)航的非線性數(shù)據(jù)融合問題轉(zhuǎn)化為迭代的線性最小二乘問題,并基于最小二乘殘差構(gòu)建檢驗(yàn)統(tǒng)計(jì)量實(shí)現(xiàn)故障檢測,通過定義斜率尋找最壞故障情況計(jì)算保護(hù)級(jí),實(shí)現(xiàn)完整的GNSS/INS組合導(dǎo)航完好性監(jiān)測。最后通過動(dòng)態(tài)仿真數(shù)據(jù)對(duì)所提算法的有效性進(jìn)行了驗(yàn)證。

2 因子圖優(yōu)化GNSS/INS組合導(dǎo)航算法

本文采用的GNSS/INS組合導(dǎo)航框架[8]如圖1所示?；镜慕馑懔鞒虨?首先姿態(tài)航向參考系統(tǒng)結(jié)合地磁強(qiáng)度和角速度信息解算得到載體的姿態(tài)矩陣;其次通過姿態(tài)矩陣將本體坐標(biāo)系的加速度轉(zhuǎn)換至導(dǎo)航坐標(biāo)系,并通過積分器獲得導(dǎo)航坐標(biāo)系下載體的速度增量;最后結(jié)合載體的速度增量以及所有可見衛(wèi)星的偽距和偽距率測量值構(gòu)建因子圖優(yōu)化模型進(jìn)行導(dǎo)航解算,獲得當(dāng)前時(shí)刻載體位置、速度估計(jì)值。

圖1 GNSS/INS組合導(dǎo)航解算框架[8]

2.1 GNSS/INS組合導(dǎo)航因子圖模型

圖1使用的因子圖模型如圖2所示。圖中的圓圈表示變量節(jié)點(diǎn),代表某時(shí)刻需要估計(jì)的狀態(tài)變量;黑色方框表示因子節(jié)點(diǎn),代表對(duì)于狀態(tài)變量的觀測。

圖2 GNSS/INS組合導(dǎo)航因子圖模型

圖3 組合導(dǎo)航完好性監(jiān)測算法仿真流程

在因子圖優(yōu)化中,所有傳感器的測量均以因子函數(shù)fj表示。將與因子函數(shù)fj相關(guān)聯(lián)的狀態(tài)變量記為xj,基于因子圖優(yōu)化的組合導(dǎo)航解算過程實(shí)際上為求解如下的最大后驗(yàn)估計(jì)問題

(1)

當(dāng)所有傳感器的測量噪聲均為高斯白噪聲時(shí),各因子函數(shù)具有如下形式:

(2)

2.2 各類因子及其觀測函數(shù)

2.2.1 先驗(yàn)因子

在GNSS/INS組合導(dǎo)航中,先驗(yàn)信息通常為初始狀態(tài)的估計(jì)值,其觀測函數(shù)和觀測值分別為

(3)

2.2.2 INS因子

在GNSS/INS組合導(dǎo)航因子圖中,INS因子描述了前后兩個(gè)時(shí)刻狀態(tài)變量之間的關(guān)聯(lián)。其對(duì)應(yīng)的觀測值和觀測函數(shù)為

(4)

本文中,導(dǎo)航坐標(biāo)系選取為東北天(ENU)坐標(biāo)系,狀態(tài)變量選取為8維向量,其構(gòu)成為

(5)

式中,pk=[ek,nk,uk]T表示載體在ENU系下的位置,vk=[ve,k,vn,k,vu,k]T表示載體在ENU系下的速度,δtk表示接收機(jī)鐘差,δfk表示接收機(jī)頻漂。

式(5)中的F為狀態(tài)轉(zhuǎn)移矩陣,B為控制矩陣,其元素構(gòu)成分別為

(6)

式中,I為單位矩陣,ΔT為計(jì)算周期,τf為頻漂對(duì)應(yīng)的時(shí)間常數(shù)。

式(5)中的δvk為一個(gè)計(jì)算周期內(nèi)根據(jù)加速度計(jì)測量值積分得到的速度增量,假設(shè)ΔT內(nèi)共有m個(gè)加速度計(jì)采樣值,則δvk的表達(dá)式為

(7)

(8)

2.2.3 GNSS因子

本文中采用的GNSS觀測量為偽距和偽距率,假設(shè)k時(shí)刻共有l(wèi)k顆可見衛(wèi)星,則GNSS因子對(duì)應(yīng)的測量值z(mì)k,GNSS為

(9)

偽距觀測量對(duì)應(yīng)的觀測函數(shù)為

(10)

偽距率觀測值對(duì)應(yīng)的觀測函數(shù)為

(11)

進(jìn)而,GNSS因子對(duì)應(yīng)的觀測函數(shù)為

hk,GNSS(xk)=[hρ,1,…,hρ,lk,hρ,1,…,hρ,lk]T+εk,GNSS

(12)

GNSS因子對(duì)應(yīng)的誤差方差陣Σk,GNSS為

(13)

2.3 因子圖優(yōu)化

通過對(duì)式(1)右側(cè)各項(xiàng)取負(fù)對(duì)數(shù)可將式(1)所描述的最大后驗(yàn)估計(jì)問題轉(zhuǎn)換為非線性最小二乘問題,即

(14)

通過一階泰勒展開可將上式進(jìn)一步轉(zhuǎn)化為線性最小二乘問題

δ

(15)

(16)

式(15)的解為

δ=H*TδZ*

(17)

(18)

通過高斯-牛頓法多次迭代可獲得式(14)的優(yōu)化解

X0=X0+δ

(19)

當(dāng)δ足夠小時(shí),認(rèn)為迭代過程已經(jīng)收斂,可將此時(shí)的X0視作式(14)的解。

3 完好性監(jiān)測

通過第2節(jié)所描述的因子圖優(yōu)化算法將GNSS/INS組合導(dǎo)航問題轉(zhuǎn)化為線性最小二乘問題。本節(jié)將在線性最小二乘的基礎(chǔ)上進(jìn)行故障檢測和保護(hù)級(jí)計(jì)算。

3.1 故障檢測

將最后一次迭代所對(duì)應(yīng)的式(15)轉(zhuǎn)換為如下形式

δZ*=H*δX+ε*+b*

(20)

式中,ε*～N(0,I)為歸一化后的測量誤差,b*為歸一化后的故障向量

(21)

最小二乘殘差定義為

r=δZ*-H*δ=(I-H*S)(ε*+b*)

(22)

最小二乘殘差平方和的統(tǒng)計(jì)特性為

(23)

λ2=b*T(I-H*S)b*

(24)

根據(jù)連續(xù)性風(fēng)險(xiǎn)需求Creq以及式(23)描述的統(tǒng)計(jì)特性可確定故障檢測門限值TFGO

(25)

3.2 保護(hù)級(jí)計(jì)算

保護(hù)級(jí)用于評(píng)估故障檢測算法的完好性風(fēng)險(xiǎn)。以垂向?yàn)槔?垂向完好性風(fēng)險(xiǎn)PI,v定義為

(26)

式中,δuk為當(dāng)前時(shí)刻垂向定位誤差,VAL為給定的垂向告警門限;NF為無故障模式,PNF=(1-Psat)N為無故障模式先驗(yàn)概率,N為可見衛(wèi)星數(shù),Psat為衛(wèi)星先驗(yàn)故障概率;F表示故障模式,PF=1-PNF為故障模式先驗(yàn)概率。

將式(26)等號(hào)左側(cè)替換為垂向完好性風(fēng)險(xiǎn)需求Ireq,v,等號(hào)右側(cè)的VAL替換為VPL即為垂向保護(hù)級(jí)的計(jì)算公式

(27)

對(duì)于式(27),一種保守的解法是將Ireq,v平均分配,分別計(jì)算無故障模式和故障模式下的保護(hù)級(jí),并取其最大值作為最終的保護(hù)級(jí)[2],即

(28)

式中,σv,k為當(dāng)前時(shí)刻垂向定位誤差的標(biāo)準(zhǔn)差;kNF和kF分別為無故障模式和故障模式對(duì)應(yīng)的系數(shù),其表達(dá)式為

(29)

式中,Q-1為標(biāo)準(zhǔn)正態(tài)分布概率累積分布函數(shù)的逆函數(shù)。

μv,k=-TXSb*

(30)

δ的估計(jì)誤差方差陣為Σ=,當(dāng)前時(shí)刻垂向定位誤差的標(biāo)準(zhǔn)差為

(31)

在完好性監(jiān)測算法中,斜率定義為未知故障引起的均值漂移與非中心化參數(shù)的比值,即

(32)

(33)

(34)

綜上,垂向保護(hù)級(jí)的最終表達(dá)式

(35)

4 仿真分析

4.1 仿真條件

為驗(yàn)證本文提出的GNSS/INS組合導(dǎo)航完好性監(jiān)測算法的有效性,選取載體典型運(yùn)動(dòng)軌跡進(jìn)行仿真。仿真環(huán)節(jié)主要包括運(yùn)動(dòng)信息仿真,測量信息仿真和算法仿真三部分:運(yùn)動(dòng)信息仿真用于產(chǎn)生衛(wèi)星位置、衛(wèi)星速度以及載體理想運(yùn)動(dòng)軌跡;測量信息仿真用于產(chǎn)生衛(wèi)星的偽距/偽距率模擬測量信息,載體的姿態(tài)角和比力模擬測量值以及衛(wèi)星的故障信息;算法仿真部分用于實(shí)現(xiàn)所提組合導(dǎo)航算法以及完好性監(jiān)測算法,并對(duì)其性能進(jìn)行驗(yàn)證。仿真流程如圖4所示。

圖4 載體飛行軌跡

載體900s內(nèi)的運(yùn)動(dòng)軌跡包括平飛、爬升、轉(zhuǎn)彎等多個(gè)階段,其飛行軌跡如圖4所示。

本次仿真中GNSS星座選取為GPS,衛(wèi)星偽距和偽距率誤差均建模為白噪聲,慣性傳感器誤差選取為民航飛機(jī)組合導(dǎo)航仿真的典型值,其具體數(shù)值見表1。加速度計(jì)的采樣頻率為50Hz,GPS測量值的采樣頻率為2Hz。

表1 GPS衛(wèi)星與INS測量噪聲仿真參數(shù)

仿真過程中完好性相關(guān)參數(shù)取值如表2所示

表2 完好性相關(guān)參數(shù)取值

此外考慮到隨著歷元數(shù)的增加,因子圖優(yōu)化算法需要占用的計(jì)算資源也隨之增加,本文采用滑動(dòng)窗口的策略避免這一問題,滑動(dòng)窗口的尺寸選取為50個(gè)歷元。

將本文所提算法記為因子圖優(yōu)化完好性監(jiān)測算法(FGO-IM),下文將通過與文獻(xiàn)[2]中基于卡爾曼濾波器實(shí)現(xiàn)的加權(quán)最小二乘完好性監(jiān)測算法(WLS-IM)對(duì)比以展示本文所提算法的性能提升。

4.2 故障檢測

當(dāng)衛(wèi)星PRN9在200-400s內(nèi)注入大小為15m的偽距故障偏差時(shí),FGO-IM和WLS-IM檢驗(yàn)統(tǒng)計(jì)量與檢測閾值的比值如圖5所示。由圖可知,相較于WLS-IM,所提FGO-IM算法對(duì)于同樣大小的偽距故障更為敏感。在給定的故障條件下,FGO-IM的故障檢測率接近100%,而WLS-IM只有在少數(shù)歷元檢驗(yàn)統(tǒng)計(jì)量超過了檢測閾值。但是,與WLS-IM相比,由于FGO-IM利用了過去歷元的測量信息,FGO-IM對(duì)于故障的響應(yīng)具有一定的滯后性。

圖5 PRN9偽距故障時(shí)故障檢測性能

將故障檢測率定義為檢測到故障的歷元數(shù)占故障發(fā)生歷元數(shù)的百分比,則FGO-IM和WLS-IM對(duì)于發(fā)生在PRN9上的不同大小偽距故障的檢測率如圖6所示。FGO-IM和WLS-IM均在偽距故障約為10m時(shí)開始能夠在部分歷元檢測到故障發(fā)生。當(dāng)偽距故障達(dá)到15m以上時(shí),FGO-IM的故障檢測率已經(jīng)達(dá)到100%。但是對(duì)于WLS-IM,只有當(dāng)偽距故障大小超過45m時(shí),其故障檢測率才能達(dá)到100%。因此,相較于WLS-IM,所提FGO-IM較為顯著地提升了對(duì)于偽距故障的檢測性能。

圖6 兩種算法偽距故障檢測率對(duì)比

當(dāng)衛(wèi)星PRN9在200-400s內(nèi)注入大小為1m/s的偽距率故障偏差時(shí),檢驗(yàn)統(tǒng)計(jì)量與檢測閾值的比值如圖7所示。由于WLS-IM在進(jìn)行故障檢測時(shí)未利用偽距率信息,因此其檢驗(yàn)統(tǒng)計(jì)量對(duì)于偽距率故障無任何響應(yīng)。而所提FGO-IM算法綜合利用了偽距和偽距率信息,對(duì)于偽距率故障也具有較好的檢測效果。在給定的偽距率故障條件下,FGO-IM算法的故障檢測率接近100%。

圖7 PRN9偽距率故障時(shí)故障檢測性能

4.3 保護(hù)級(jí)

無故障條件下,WLS-IM和所提FGO-IM算法計(jì)算得到的垂向保護(hù)級(jí)如圖8所示。整體而言,通過FGO-IM計(jì)算得到的垂向保護(hù)級(jí)保持在20m附近,相較于WLS-IM,垂向保護(hù)級(jí)明顯得到降低,意味著相同條件下FGO-IM具有更高的可用性。在仿真過程中,450s后衛(wèi)星PRN18不再可見,由此引起的衛(wèi)星幾何變化會(huì)造成垂向保護(hù)級(jí)的增大。但相較于WLS-IM,由于FGO-IM利用了過去歷元的測量信息進(jìn)行平滑,盡管450s后衛(wèi)星幾何變化同樣引起其垂向保護(hù)級(jí)增大,但增大量并不顯著,即對(duì)其可用性影響較小。此外,由于采用了滑動(dòng)窗口算法,在初始?xì)v元和衛(wèi)星幾何變化的歷元,FGO-IM的垂向保護(hù)級(jí)需要一定時(shí)間收斂至穩(wěn)定值。

圖8 無故障時(shí)垂向保護(hù)級(jí)

保護(hù)級(jí)的重要意義在于提供FGO-IM在未檢測到故障時(shí)載體定位誤差的安全邊界。未檢測到故障分為兩種情況:無故障發(fā)生或發(fā)生了故障卻未被檢測到,即漏檢情況。漏檢情況對(duì)于用戶而言非常危險(xiǎn)。在200-400s向PRN11注入大小為5m的偽距故障,在500-700s向PRN16注入大小為0.4m/s的偽距率故障,用于模擬漏檢情況。該條件下檢驗(yàn)統(tǒng)計(jì)量與檢測閾值之比如圖9所示,此時(shí)某些時(shí)刻的檢驗(yàn)統(tǒng)計(jì)量已經(jīng)非常接近檢測閾值,但由于二者之比始終低于1,未觸發(fā)FGO-IM 的告警。

圖9 漏檢時(shí)的檢驗(yàn)統(tǒng)計(jì)量與閾值之比

在模擬的漏檢情況下,所提算法的垂向定位誤差(VPE)與垂向保護(hù)級(jí)的變化曲線如圖10所示。由圖可知,即使在模擬的極限漏檢情況下,垂向定位誤差絕對(duì)值始終保持在FGO-IM提供的垂向保護(hù)級(jí)之下。這表明所提FGO-IM的保護(hù)級(jí)算法是有效的,能夠提供未檢測到故障情況下用戶定位誤差的安全邊界。

圖10 漏檢時(shí)的垂向定位誤差與垂向保護(hù)級(jí)

5 結(jié)論

針對(duì)GNSS/INS組合導(dǎo)航中GNSS衛(wèi)星的故障風(fēng)險(xiǎn),本文提出一種基于因子圖優(yōu)化的組合導(dǎo)航自主完好性監(jiān)測算法。在構(gòu)建的GNSS/INS因子圖模型基礎(chǔ)上,將導(dǎo)航估計(jì)問題轉(zhuǎn)化為多次迭代的線性最小二乘問題,利用最小二乘殘差構(gòu)建檢驗(yàn)統(tǒng)計(jì)量進(jìn)行故障檢測和保護(hù)級(jí)計(jì)算。對(duì)動(dòng)態(tài)數(shù)據(jù)的仿真結(jié)果表明,相較于傳統(tǒng)算法,所提出的完好性監(jiān)測算法能夠更加有效地檢測GNSS衛(wèi)星的偽距和偽距率故障,且保護(hù)級(jí)在大幅降低的同時(shí)能夠有效包絡(luò)漏檢情況下的定位誤差。