基于因子圖導(dǎo)航的偽距故障檢測與自適應(yīng)隔離方法

孫克誠，曾慶化，王守一，劉建業(yè)，黃 艷

（南京航空航天大學(xué) 導(dǎo)航研究中心，南京 210016）

可靠的導(dǎo)航和定位能力是無人系統(tǒng)的主要屬性。無人系統(tǒng)的導(dǎo)航狀態(tài)通常是通過融合自慣性導(dǎo)航系統(tǒng)（Inertial Navigation System,INS）和全球?qū)Ш叫l(wèi)星系統(tǒng)（Global Navigation Satellite System,GNSS）測量的多傳感器信息進(jìn)行估計的[1]。由于外界干擾或震蕩的環(huán)境，傳感器容易出現(xiàn)誤差增大甚至故障的情況。震蕩環(huán)境會使得慣性導(dǎo)航系統(tǒng)解算錯誤，誤差突然增加，甚至出現(xiàn)解算故障[2,3]。全球?qū)Ш叫l(wèi)星系統(tǒng)信號更容易受到電磁干擾，在城市環(huán)境、隧道及其他未知惡劣環(huán)境下，全球?qū)Ш叫l(wèi)星系統(tǒng)的定位精度較差，這些區(qū)域的GNSS 信號接收受到干擾，甚至接收不到信號[4,5]。因此，有效的故障檢測和隔離方法以及在未知干擾下的魯棒導(dǎo)航對安全至關(guān)重要。

考慮到復(fù)雜環(huán)境的影響下傳感器的不可預(yù)測性能變化，傳統(tǒng)的濾波算法難以滿足魯棒性的要求。而因子圖的優(yōu)勢是其出色的即插即用能力，以靈活的結(jié)構(gòu)[6,7]處理異步和即插即用問題。1981年，Tanner[8]引入了一個二部圖描述低密度奇偶校驗碼的奇偶校驗矩陣，稱為“Tanner 圖”。文獻(xiàn)[9]基于Tanner 圖、Wiberg圖和其他模型提出了因子圖概念和和積算法。文獻(xiàn)[10]詳細(xì)地將卡爾曼濾波、最小二乘法與基于因子圖的和積算法結(jié)合，并通過因子圖模型描述了估計算法。文獻(xiàn)[11]提出了一種基于因子圖的慣性/視覺信息融合方法，將信息融合問題表示為因子圖模型。文獻(xiàn)[12]使用增量平滑和映射優(yōu)化了全局因子圖中的關(guān)鍵幀。文獻(xiàn)[13]提出了一種改進(jìn)的因子圖多傳感器融合導(dǎo)航算法，實現(xiàn)了無人機多源導(dǎo)航系統(tǒng)方法。文獻(xiàn)[14]提出了基于因子圖的AUV 多傳感器組合導(dǎo)航算法，有效實現(xiàn)慣性導(dǎo)航系統(tǒng)與不同導(dǎo)航傳感器的非等間隔融合。文獻(xiàn)[15]通過考慮GNSS 偽距誤差模型和環(huán)境條件，評估了窗口大小對因子圖優(yōu)化性能的影響。文獻(xiàn)[5]設(shè)計了一個動態(tài)權(quán)重函數(shù)調(diào)整每個因子的權(quán)重，從而提高了因子圖的導(dǎo)航性能和魯棒性。這些方法在保持圖理論即插即用功能的同時，提高了導(dǎo)航性能。因子圖的主要特點是抽象子導(dǎo)航系統(tǒng)，實現(xiàn)快速集成、重構(gòu)和濾波的算法。有效地檢測和隔離算法可以幫助快速定位故障傳感器，并且隔離掉可能污染整個系統(tǒng)的故障量測。因子圖與故障檢測和隔離方法相結(jié)合可以靈活地實現(xiàn)可靠的導(dǎo)航。

在安全關(guān)鍵應(yīng)用程序（Safety-Critical Applications,SCA）中，故障檢測和隔離（Fault Detection and Isolation,FDI）是一個重要方面，可以防止用戶因?qū)Ш较到y(tǒng)中的傳感器故障而面臨潛在風(fēng)險[16]。文獻(xiàn)[17]提出了一種新的故障檢測算法，其中卡爾曼濾波器被多模型算法取代。文獻(xiàn)[18]基于殘差卡方檢驗方法設(shè)計了一種容錯卡爾曼濾波器方案，包括三個子濾波器和一個主卡爾曼濾波器。文獻(xiàn)[19]設(shè)計了基于觀測器的魯棒傳感器故障診斷濾波器，可保證量測殘差對于故障及增廣擾動具有魯棒性，殘差對于故障及擾動的魯棒性能指標(biāo)均小于給定值1.18，具有良好的魯棒性。文獻(xiàn)[20]通過在容錯組合導(dǎo)航系統(tǒng)中加入神經(jīng)網(wǎng)絡(luò)，提出了一種基于深度神經(jīng)網(wǎng)絡(luò)的智能主動容錯系統(tǒng)。文獻(xiàn)[21]分析了殘差卡方的檢測性能，并提出了兩個合適的閾值計算每個測量的權(quán)重因子。文獻(xiàn)[22]提出了一種基于AR 量測建模的組合導(dǎo)航系統(tǒng)漸變故障雙閾值檢測方法，避免由于傳統(tǒng)殘差c2檢測方法因引入觀測污染數(shù)據(jù),對漸變故障不敏感的問題。文獻(xiàn)[2]研究了方差偏移異常值模型，該模型根據(jù)方差中相關(guān)偏移的大小排除或包含測量值，以檢測原始偽距數(shù)據(jù)中的故障。文獻(xiàn)[23]提出了一種傳感器驗證方法，該方法將基于殘差的測試統(tǒng)計數(shù)據(jù)與Kalman-Schmidt 濾波器的部分更新公式相結(jié)合，為傳感器模型驗證提供可靠的方法，以保護(hù)導(dǎo)航系統(tǒng)的完好性。然而，靈活的圖優(yōu)化結(jié)構(gòu)所涉及的故障模式較為多樣，尤其是INS節(jié)點故障或GNSS 節(jié)點偽距故障的問題，目前與之適應(yīng)的有效故障檢測和隔離方法還有待進(jìn)一步深入研究。

眾所周知，GNSS 的量測容易受到電磁環(huán)境和遮擋的干擾，偽距中可能包含故障誤差。在復(fù)雜的道路環(huán)境和發(fā)動機受到干擾的情況下，載體容易發(fā)生振動。INS 是自主系統(tǒng)的核心傳感器，復(fù)雜的環(huán)境變化可能導(dǎo)致其不穩(wěn)定或電子元件損壞，故障如果沒有被及時診斷將會導(dǎo)致嚴(yán)重事故。為了解決復(fù)雜環(huán)境下導(dǎo)航設(shè)備的故障問題，提出了一種基于因子圖導(dǎo)航的偽距故障檢測與自適應(yīng)隔離方法(Pseudo-range Fault Detection and Adaptive Isolation,PFDAI)，基于因子圖導(dǎo)航的 PFDAI 方法采用衛(wèi)星偽距故障檢測(Pseudo-range Fault Detection,PFD)、自適應(yīng)隔離和即插即用因子圖框架，與傳統(tǒng)的FDI 算法相比，該方法可以具體到某個衛(wèi)星存在偽距誤差問題或INS 故障，并在此檢測結(jié)果的基礎(chǔ)上，自適應(yīng)隔離故障因子節(jié)點，該方法在故障下具有更好的導(dǎo)航定位效果。針對衛(wèi)星偽距中存在的部分軟/硬故障，本文所提方法可以輔助導(dǎo)航系統(tǒng)獲得更好的故障檢測和隔離效果。

1 基于因子圖導(dǎo)航的故障檢測與隔離系統(tǒng)方案

基于FDI 和因子圖的導(dǎo)航系統(tǒng)方案如圖1所示。因子圖中的各類傳感器因子節(jié)點可以提供位置、速度、姿態(tài)測量，在復(fù)雜環(huán)境中，由于傳感器故障，測量的可用性仍然取決于是否有有效的故障檢測和隔離算法。在此基礎(chǔ)上，為了保證組合導(dǎo)航的可靠性和精度，結(jié)合故障檢測和圖優(yōu)化的優(yōu)點，提出了一種改進(jìn)的偽距故障檢測方法。與以INS 測量值作為參考值檢測其他導(dǎo)航系統(tǒng)是否存在故障的傳統(tǒng)FDI 方法不同，該方法基于GNSS 原始偽距信息和INS 量測檢測故障，可以檢測并定位具體故障衛(wèi)星偽距量測，考慮到系統(tǒng)內(nèi)各子導(dǎo)航系統(tǒng)同時發(fā)生故障的概率較小，若故障檢測主濾波器判斷系統(tǒng)存在故障，而子濾波器判斷偽距量測均無問題的情況下，則可以推斷為INS 量測故障，此后可以結(jié)合衛(wèi)星接收機自身完好性算法研究，較好保證導(dǎo)航性能。

圖1 基于因子圖的故障檢測和隔離方案Fig.1 Fault detection and isolation scheme based on factor graph

本文在有效檢測偽距故障的基礎(chǔ)上，進(jìn)一步提出了自適應(yīng)隔離方法。在檢測到傳感器故障后，算法能夠自適應(yīng)隔離故障傳感器的測量信息，保證隔離后定位精度。因子圖的即插即用特性有利于根據(jù)檢測結(jié)果自適應(yīng)隔離故障傳感器的測量信息。當(dāng)傳感器無故障時，因子圖算法將傳感器測量作為因子節(jié)點添加到因子圖框架中。當(dāng)傳感器故障時，因子圖算法將通過不添加因子節(jié)點或者添加低權(quán)重因子節(jié)點從而隔離或降低傳感器故障量測的影響，所提算法能夠在干擾環(huán)境下自適應(yīng)地融合所有可用的信息提高導(dǎo)航精度。

2 衛(wèi)星偽距故障檢測方法

傳感器故障下有效的故障檢測與隔離方法對導(dǎo)航系統(tǒng)的魯棒性至關(guān)重要。由于周圍環(huán)境的影響，一些傳感器可能會在短時間內(nèi)處于非活動狀態(tài)或出現(xiàn)故障。本文目標(biāo)是結(jié)合所有可用信息判斷是否存在傳感器故障，基于故障檢測結(jié)果自適應(yīng)隔離節(jié)點以提高導(dǎo)航精度。

根據(jù)圖2中的多解分離法（MultipleSolution Separation,MSS）檢測算法，對每顆衛(wèi)星的偽距信息進(jìn)行分析和判斷。首先，該檢測方法需要一個主濾波器處理所有可見衛(wèi)星的偽距數(shù)據(jù)，主濾波器與傳統(tǒng)卡方檢測方法近似，區(qū)別在于傳統(tǒng)卡方校驗采用位置作為檢測依據(jù)，偽距檢測方法將所有衛(wèi)星的偽距與慣性導(dǎo)航推算的偽距作為量測值，導(dǎo)航系統(tǒng)位置、速度、姿態(tài)和偽距、偽距率為狀態(tài)值進(jìn)行故障檢測。主濾波器一直保持工作，用于檢測導(dǎo)航系統(tǒng)中的突變故障和緩變故障。此外，在主濾波器檢測到故障后，代表整體系統(tǒng)存在故障，則啟用N個子濾波器，在依次移除一顆可見衛(wèi)星量測后，剩余所有其他衛(wèi)星的偽距測量值作為判斷依據(jù)進(jìn)入每個子濾波器。如果扣除一顆衛(wèi)星偽距量測的每個子濾波器依然全部判斷存在故障，這意味著不止一顆衛(wèi)星的偽距存在故障，則每個濾波器再減去一顆衛(wèi)星的偽距測量值，并將其輸入子-子濾波器進(jìn)行故障判斷，以此類推。當(dāng)所有子濾波器判斷存在故障，則判定INS量測基準(zhǔn)存在故障。偽距故障檢測方法的濾波器結(jié)構(gòu)圖如圖2所示。

圖2 偽距故障檢測檢測濾波器示意圖Fig.2 Structurediagram of thepseudo-range fault detection

對于上述每個子濾波器，基于相應(yīng)的故障算法構(gòu)造統(tǒng)計模型。在時間k，對于主濾波器和子濾波器，主濾波器F00使用N個可用的衛(wèi)星偽距測量計算故障檢測校驗值的主要濾波估計。在主濾波器中檢測到故障后，子濾波器故障檢測校驗估計值由N個子濾波器（表示為{F0n}n?[1,N]）計算。子濾波器F0n根據(jù)除第nth顆衛(wèi)星偽距外的所有衛(wèi)星偽距測量值檢測故障。如果第nth顆衛(wèi)星出現(xiàn)故障，除第nth濾波器外，其他濾波器的檢測值都會受到故障衛(wèi)星測量的污染，導(dǎo)致檢測出故障，而第nth濾波器檢測不出故障。因此對比主濾波器和子濾波器的檢測校驗結(jié)果，則可以判斷哪顆星的偽距存在故障，表示為：

位置、速度、姿態(tài)、接收機時鐘偏置誤差和接收機時鐘誤差變化率在故障檢測中都非常重要。本文關(guān)注11維狀態(tài)值，其協(xié)方差矩陣為）。

通過應(yīng)用以下假設(shè)檢驗檢測故障：

當(dāng)無故障時，服從高斯概率分布，它的檢測特性幅值，服從瑞利分布。其中D0n是一個檢測門限，根據(jù)內(nèi)曼-皮爾遜準(zhǔn)則，即：

其中z為觀測值，L(z)為似然比，D0n稱為似然比門限，似然比L(z)在H0為真的條件下的概率密度為p(L/H0)，則由：

就可求解出D0n，D0n滿足如下要求：

其中當(dāng)限定誤警率Pf=a時，則由式(5)解出的門限D(zhuǎn)0n可使漏檢率?D0n/H0達(dá)到最小，因而門限D(zhuǎn)0n可由誤警率Pf確定。

3 因子圖融合與自適應(yīng)隔離方法

當(dāng)前組合導(dǎo)航中，信息融合一般采用集中式濾波或聯(lián)邦濾波。當(dāng)部分傳感器出現(xiàn)故障時，必須重新構(gòu)造系統(tǒng)和濾波器架構(gòu)，這將影響導(dǎo)航系統(tǒng)的準(zhǔn)確性和及時性。因此，導(dǎo)航系統(tǒng)的彈性濾波結(jié)構(gòu)具有良好的研究前景，將有助于在干擾環(huán)境下實現(xiàn)導(dǎo)航系統(tǒng)的故障和隔離。

根據(jù)無人系統(tǒng)的實際特點，構(gòu)建基于因子圖的組合導(dǎo)航框架，如圖3所示。系統(tǒng)中的信息源是INS和GNSS。將導(dǎo)航系統(tǒng)的狀態(tài)和量測方程轉(zhuǎn)化為兩類因子節(jié)點。在圖3中，紫色圓點表示變量節(jié)點，實心圓表示因子節(jié)點。xk表示系統(tǒng)的導(dǎo)航狀態(tài)，p表示先驗因子節(jié)點量測信息；u表示來自INS的測量信息，該信息與前后兩個時間點的導(dǎo)航狀態(tài)有關(guān)。m表示來自GNSS的測量信息。如果某個子導(dǎo)航系統(tǒng)因干擾而發(fā)生故障，則可以實時從系統(tǒng)中去除相應(yīng)的因子。當(dāng)某顆衛(wèi)星偽距故障時，將該偽距量測隔離后，基于其他偽距量測形成GNSS因子節(jié)點。當(dāng)慣性系統(tǒng)故障時，在因子圖框架中隔離INS因子節(jié)點后，GNSS量測信息轉(zhuǎn)化為主線上因子節(jié)點，導(dǎo)航系統(tǒng)實現(xiàn)依靠衛(wèi)星量測的低頻實時穩(wěn)定工作。故障排除后，可以將該導(dǎo)航系統(tǒng)因子重新添加到因子圖導(dǎo)航濾波器中。

圖3 基于因子圖的組合導(dǎo)航示意圖Fig.3 Schematicdiagram of integrated navigation framework based on factor graph

在圖3設(shè)計的方案中，使用東北天坐標(biāo)作為導(dǎo)航坐標(biāo)系。有三種狀態(tài)變量，包括位置、速度和姿態(tài)。因子圖的變量節(jié)點是維度系統(tǒng)狀態(tài)，如式(6)所示。

式(6)中，9個變量是慣性導(dǎo)航系統(tǒng)的導(dǎo)航輸出參數(shù)。其中jE、jN、jU包括平臺失準(zhǔn)角；δvE、δvN、δvU是速度誤差；δL、δl、δh是緯度、經(jīng)度和高度誤差。

因子節(jié)點的量測模型與各傳感器相關(guān)，這里不再一一列舉。根據(jù)每個子系統(tǒng)的狀態(tài)和測量方程，使用標(biāo)準(zhǔn)的Levenberg-Marquardt 優(yōu)化器計算因子圖融合信息?；谝蜃訄D論和綜合導(dǎo)航數(shù)學(xué)模型，融合方法如下所示。

每個因子代表一個最小化的誤差函數(shù)。對于高斯噪聲分布，表示測量模型的因子定義為：

式中hi(.)是預(yù)測傳感器測量的非線性測量函數(shù)，zi是實際測量。

通過分析因子圖導(dǎo)航系統(tǒng)的原理，系統(tǒng)狀態(tài)估計函數(shù)是概率分布函數(shù)（PDF）的最大后驗概率。在此基礎(chǔ)上，我們基于故障檢測校驗結(jié)果添加權(quán)重因子，函數(shù)如下所示：

其中，hi(Xi)是觀測函數(shù)，zi是實際測量值，Si是協(xié)方差矩陣。為權(quán)重函數(shù)，根據(jù)故障檢測結(jié)果，隔離部分嚴(yán)重故障因子，調(diào)整融合權(quán)重，以削弱故障對導(dǎo)航系統(tǒng)的影響。

式(9)中，l1和l2為判斷自適應(yīng)隔離的上下門限，根據(jù)上一節(jié)的理論分析，由誤警率Pf決定。選定誤警率Pf=10-9，計算故障完全隔離檢驗門限l2=44.84；選定誤警率Pf=10-3，計算故障自適應(yīng)融合門限l1=16.27。

采用一階泰勒展開對非線性觀測方程hi(Xi)進(jìn)行線性化，通過將觀測方程引入最大后驗概率估計，得到

式中Hi是雅可比矩陣，表示點Xi0處hi()的偏微分，Di是狀態(tài)更新向量，為使得式(10)右側(cè)取最小值的解。

當(dāng)傳感器出現(xiàn)故障時，傳統(tǒng)的聯(lián)邦濾波算法需要重構(gòu)主濾波器，以防止故障傳感器影響主濾波器。而因子圖法只需要不連接故障傳感器的因子節(jié)點即可實現(xiàn)故障隔離。

4 實驗與分析

為了驗證所提FDI 算法的有效性，基于復(fù)雜故障環(huán)境下仿真實驗和實際跑車數(shù)據(jù)實驗驗證所提方法在存在慣性量測或者衛(wèi)星導(dǎo)航系統(tǒng)偽距量測異常的情況下，能正常檢測出偽距故障并提升導(dǎo)航定位性能。

4.1 仿真試驗

根據(jù)自主車輛的實際路徑要求，設(shè)計了一條800 秒的軌跡。如圖4所示，起始點為118.79oE、31.94oN、5 m。三個方向的初始速度為0 m/s，起始點三個方向的初始姿態(tài)角為0 °。

圖4 車輛航跡圖Fig.4 The path of vehicles

慣性導(dǎo)航系統(tǒng)的誤差參數(shù)見表1，衛(wèi)星導(dǎo)航系統(tǒng)的誤差參數(shù)見表2，導(dǎo)航系統(tǒng)的故障模擬設(shè)置見表3。

表1 慣性導(dǎo)航誤差Tab.1 Inertial navigation error

表2 衛(wèi)星導(dǎo)航系統(tǒng)誤差Tab.2 GNSS errors

表3 各實驗方案添加故障Tab.3 Add faults to each experimental scheme

階躍故障和緩變故障是全球?qū)Ш叫l(wèi)星系統(tǒng)偽距量測的典型故障類型。如表3所示，本文將各種故障（階躍故障、緩變故障、單衛(wèi)星故障、多衛(wèi)星故障、大故障、小故障）添加到GNSS 偽距中。本文的實驗驗證在偽距量測中分別添加了振幅1000 m 的大階躍故障、振幅50 m 的小階躍故障和斜率為20 m/s 的大緩變故障、斜率為5 m/s 的小緩變故障，同時不同衛(wèi)星偽距故障也有重合階段，以測試所提算法的有效性。

仿真試驗1：圖5-6和表4比較了卡方檢驗法、偽距檢測法和所提方法在GNSS 階躍故障情況下的性能。

圖5 多衛(wèi)星階躍故障下故障檢測函數(shù)圖Fig.5 The test statistic of fault detect obtained by simulation under the multi-satellites step fault

在圖5中，SV-1 和SV-2 中由階躍故障引起的主濾波器故障檢測函數(shù)值圖看出，由于所提方法和偽距檢測法的故障檢測原理相同，檢驗效果基本相同，曲線也基本重合，這三種方法都可以成功地檢測多顆衛(wèi)星中的大階躍故障，而對小階躍故障，傳統(tǒng)依靠位置的方法難以檢驗，而對偽距檢測法和所提方法，都可以正常的檢測出故障，同時兩顆衛(wèi)星同時出現(xiàn)故障階段校驗函數(shù)也顯著增加。

在圖6中，對于大階躍故障(200～250 s)，無論是單衛(wèi)星故障還是多衛(wèi)星故障，各個方法都可以成功檢測到，依靠位置的卡方檢測隔離掉衛(wèi)星位置量測后會出現(xiàn)發(fā)散的情況，所提方法和偽距檢測在隔離故障衛(wèi)星偽距后，正常衛(wèi)星數(shù)據(jù)可以像往常一樣用所提方法校正慣性數(shù)據(jù)；而對于小階躍故障(400～450 s)，所提方法可以自適應(yīng)調(diào)節(jié)因子的權(quán)重，因此相較于偽距檢測法，所提算法有著更好的精度。表4的誤差RMS值同樣證明了該方法具有更好的精度，針對故障階段，所提PFDAI 方法相較于卡方檢測法和偽距檢測法三維位置精度分別提升了45%和35%。

圖6 多衛(wèi)星階躍故障下融合位置誤差Fig.6 The fusion position errors obtained by simulation under the multi-satellites step fault

表4 多衛(wèi)星階躍故障下融合狀態(tài)誤差RMS 值Tab.4 RMS of navigation errors obtained by simulation under the multi-satellites step fault

仿真試驗2：圖7-8 和表5比較了卡方檢驗法、偽距檢測法和所提方法在GNSS 緩變故障情況下的性能。在圖7中，SV-1 和SV-2 中斜坡故障導(dǎo)致的檢測統(tǒng)計圖顯示，不論是大緩變故障還是小緩變故障，所提出的方法和偽距檢測法都可以更快地檢測緩變故障。在圖8中，在大緩變故障時，卡方檢測隔離衛(wèi)星位置后會出現(xiàn)短時的發(fā)散，而所提方法和偽距檢測法都可以隔離掉故障偽距，效果近似；而在小緩變故障下，所提方法較偽距檢測法有著更好的自適應(yīng)效果，在多星偽距緩變故障中具有良好的檢測和自適應(yīng)隔離效果。RMS 表同樣顯示了本文方法的優(yōu)越性，針對故障階段，所提PFDAI 方法相較于卡方檢測法和偽距檢測法三維位置精度分別提升了29%和32%，如表5所示。

表5 多衛(wèi)星緩變故障下融合狀態(tài)誤差RMS值Tab.5 RMSof navigationerrorsobtainedby simulation under themulti-satellites rampfault

圖7 多衛(wèi)星緩變故障下故障檢測函數(shù)圖Fig.7 The test statistic of fault detect obtained by simulation under the multi-satellites ramp fault

圖8 多衛(wèi)星緩變故障下融合位置誤差Fig.8 The fusion position errors obtained by simulation under the multi-satellitesrampfault

仿真試驗3：圖9-10比較了卡方檢驗法和所提方法在INS階躍故障情況下的性能，由于INS本身存在累積誤差，小故障與緩變故障情況難以檢測，但能通過融合進(jìn)行修正，因此，本文僅考慮較大的INS階躍故障，該情況下所提方法無需考慮自適應(yīng)因子并且性能與偽距檢測方法近似，所以不再單獨對比。

圖9 慣導(dǎo)階躍故障下故障檢測函數(shù)圖Fig.9 Thetest statisticof fault detect obtained by simulation under INSstepfault

在圖9中，傳統(tǒng)的卡方檢測方法將INS量測作為參考值，不能定位并隔離故障慣性傳感器，因此其一直判斷系統(tǒng)故障，無法恢復(fù)；而本文所提方法可以通過主子濾波器對比判斷INS故障，隔離后可以正常檢測系統(tǒng)故障和恢復(fù)。在圖10中，由于傳統(tǒng)卡方檢測方法無法定位并隔離慣性系統(tǒng)故障，因此系統(tǒng)誤差累積增大，最終發(fā)散；而本文方法可以正常隔離INS 故障，達(dá)到誤差收斂的效果。

圖10 慣導(dǎo)階躍故障下融合位置誤差Fig.10 Thefusion position errorsobtained by simulation under theINSstepfault

4.2 實際采集數(shù)據(jù)集實驗

本文利用開放數(shù)據(jù)進(jìn)行實驗測試，采用香港理工大學(xué)提供的城市采集參考數(shù)據(jù)。該數(shù)據(jù)集通過香港理工大學(xué)車輛平臺采集，車輛平臺包括一個帶有INS/GNSS的導(dǎo)航系統(tǒng)，以及來自SPAN-CPT系統(tǒng)的精確地面參考數(shù)據(jù)。車輛路徑如圖11所示，該環(huán)境為半城市峽谷環(huán)境，有部分地方有高樓遮擋造成偽距檢測故障，部分低矮房屋也會造成偽距誤差小范圍波動，適合檢測所提算法的效果。

圖11 城市環(huán)境下車載實驗平臺軌跡Fig.11 The path of vehicleplatform under theurban environment

捷聯(lián)慣性導(dǎo)航系統(tǒng)提供陀螺儀和加速度計數(shù)據(jù)，以計算地面位置、速度和方向信息。原始GNSS RINEX數(shù)據(jù)包括偽距、多普勒頻移、星歷數(shù)據(jù)。INS 測量由Xsens Mti-10 提供，頻率為100 Hz 和200 Hz，原始GNSS 測量由u-blox M8T 收集，頻率為1 Hz。由于香港的高層建筑，GNSS 信號容易受到反射或遮擋，導(dǎo)致非視線（NLOS）重新定位、多徑效應(yīng)或故障。與電磁干擾環(huán)境類似，傳感器性能不穩(wěn)定，容易發(fā)生故障。

圖12和13均采用數(shù)據(jù)集UrbanNav HK 2020 分析了所提方法的性能。圖10顯示了城市峽谷環(huán)境中受影響的GNSS 信號引起的檢測統(tǒng)計圖。在圖12中132 s～150 s 左右，由于高樓的遮擋，系統(tǒng)判斷偽距出現(xiàn)故障，對偽距誤差較大的衛(wèi)星隔離后，進(jìn)行自適應(yīng)融合，而在270 s～299 s 左右，低矮樓房也造成了衛(wèi)星定位精度略差，此時可以相應(yīng)降低自適應(yīng)權(quán)重，可以提升整體的定位精度。

圖12 香港城市環(huán)境下故障檢測值Fig.12 The test statistic of fault detect obtained by the dataset UrbanNav-HK 2020

從圖13可以看出，不論是在大偽距故障(132 s～150 s)情況下，還是小偽距故障(270 s～299 s)情況下，所提算法的紅色曲線相較于傳統(tǒng)依賴位置進(jìn)行卡方檢測法還是依賴偽距進(jìn)行檢測隔離的方法都可以自適應(yīng)地調(diào)整，適當(dāng)?shù)亟档虶NSS 因子的權(quán)重，提高短時間內(nèi)的定位精度。

圖13 香港城市環(huán)境下融合位置誤差Fig.13 The fusion position errors obtained by the dataset Ur-banNav-HK 2020

從表6可以看出，提出PFDAI 方法在故障期間的水平位置精度和高度精度相較于傳統(tǒng)卡方檢測法提高了14.35%和12.05%。

表6 香港城市環(huán)境下融合狀態(tài)誤差RMS 值Tab.6 RMS of navigation errors obtained by the dataset Ur-banNav-HK 2019 under the satellite fault

5 結(jié) 論

對于因子圖的即插即用框架，有效的故障檢測和隔離方法以及在未知干擾下的魯棒導(dǎo)航對安全至關(guān)重要。為了提高復(fù)雜環(huán)境下組合導(dǎo)航系統(tǒng)的故障檢測和魯棒性能，提出了一種基于因子圖導(dǎo)航的偽距故障檢測與自適應(yīng)隔離方法。它提供了衛(wèi)星偽距故障定位的能力，并在檢測結(jié)果的基礎(chǔ)上進(jìn)行自適應(yīng)隔離，提升故障下系統(tǒng)定位精度。本研究得出的結(jié)論可總結(jié)如下：

（1）基于因子圖的組合導(dǎo)航算法能夠有效地解決導(dǎo)航信息融合中的傳感器異步問題，實現(xiàn)多傳感器的靈活配置，因此得到了廣泛的應(yīng)用。

（2）基于INS 和GNSS 偽距測量，設(shè)計了一種基于因子圖組合導(dǎo)航的偽距故障檢測方法，并在檢測結(jié)果的基礎(chǔ)上自適應(yīng)隔離故障量測，提升故障下的導(dǎo)航定位效果。

（3）仿真和基于實測數(shù)據(jù)的試驗結(jié)果表明，在GNSS 偽距階躍故障、緩變故障、單衛(wèi)星故障、多衛(wèi)星故障、大小故障下的故障檢測與隔離性能得以改進(jìn)。仿真試驗顯示不同故障下所提方法相較于卡方檢測法和偽距檢測法三維位置精度均提升29%以上，基于跑車公開數(shù)據(jù)集數(shù)據(jù)的試驗結(jié)果表明，與傳統(tǒng)卡方的故障檢驗與隔離方法相比，本文提出的故障檢測和自適應(yīng)隔離方法的導(dǎo)航水平位置精度和高度精度均提高12%以上。