基于FGO的GNSS/INS組合導航完好性監(jiān)測算法

蘭曉偉,許承東,趙 靖

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

1 引言

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

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

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

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

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

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

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

圖1 GNSS/INS組合導航解算框架[8]

2.1 GNSS/INS組合導航因子圖模型

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

圖2 GNSS/INS組合導航因子圖模型

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

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

(1)

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

(2)

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

2.2.1 先驗因子

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

(3)

2.2.2 INS因子

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

(4)

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

(5)

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

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

(6)

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

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

(7)

(8)

2.2.3 GNSS因子

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

(9)

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

(10)

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

(11)

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

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

(12)

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

(13)

2.3 因子圖優(yōu)化

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

(14)

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

δ

(15)

(16)

式(15)的解為

δ=H*TδZ*

(17)

(18)

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

X0=X0+δ

(19)

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

3 完好性監(jiān)測

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

3.1 故障檢測

將最后一次迭代所對應(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)計特性為

(23)

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

(24)

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

(25)

3.2 保護級計算

保護級用于評估故障檢測算法的完好性風險。以垂向為例,垂向完好性風險PI,v定義為

(26)

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

將式(26)等號左側(cè)替換為垂向完好性風險需求Ireq,v,等號右側(cè)的VAL替換為VPL即為垂向保護級的計算公式

(27)

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

(28)

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

(29)

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

μv,k=-TXSb*

(30)

δ的估計誤差方差陣為Σ=,當前時刻垂向定位誤差的標準差為

(31)

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

(32)

(33)

(34)

綜上,垂向保護級的最終表達式

(35)

4 仿真分析

4.1 仿真條件

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

圖4 載體飛行軌跡

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

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

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

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

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

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

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

4.2 故障檢測

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

圖5 PRN9偽距故障時故障檢測性能

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

圖6 兩種算法偽距故障檢測率對比

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

圖7 PRN9偽距率故障時故障檢測性能

4.3 保護級

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

圖8 無故障時垂向保護級

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

圖9 漏檢時的檢驗統(tǒng)計量與閾值之比

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

圖10 漏檢時的垂向定位誤差與垂向保護級

5 結(jié)論

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