(武漢大學衛(wèi)星導航定位技術(shù)研究中心，武漢 430079)

0 引言

在嵌入式組合導航系統(tǒng)中，傳感器短時間的故障對系統(tǒng)穩(wěn)定性有很大影響.自適應卡爾曼濾波可在全球衛(wèi)星導航系統(tǒng)(GNSS)定位精度突變的情況下保證定位的精度[1].而該算法是在假定只有GNSS 粗差的情況下使用的.GNSS/慣性導航系統(tǒng)(INS)組合導航中，傳統(tǒng)的χ2檢驗可以判斷出系統(tǒng)是否存在故障，不過其不能區(qū)分出故障所在的傳感器[2].基于神經(jīng)網(wǎng)絡的χ2檢驗方法和利用粒子濾波的故障檢測方法可辨別出故障傳感器[3-4].由于其計算開銷較大，并不適用于嵌入式導航系統(tǒng).

在所有故障中，GNSS 故障對組合導航的性能影響最為顯著，且該故障可分為軟故障和硬故障[5-6].當GNSS 整體的可觀測性較差時，檢測靈敏度較低的殘差卡方檢驗將在GNSS 軟故障下偏離真實值[7].任旭陽[8]提出基于貫序概率比檢驗法(SPRT)的故障檢測算法對軟故障的檢出具有較好的效果，但該算法并不能對故障進行及時地隔離及糾正處理.張浩等[9]提出的雙狀態(tài)卡方故障檢測系統(tǒng)對具有高精度慣性測量單元(IMU)系統(tǒng)的GNSS 軟故障具有較好的效果，該方法并不適用于低成本IMU 組合導航系統(tǒng)，并且雙狀態(tài)同時面臨著INS、里程計(ODO)等故障污染的風險.

隧道、林蔭道等復雜環(huán)境下，實時動態(tài)(RTK)定位結(jié)果無法長時間保持在固定解，因此本文針對這一問題實現(xiàn)了基于多GNSS 狀態(tài)的INS/GNSS/ODO抗差組合導航算法，提高了系統(tǒng)對GNSS 狀態(tài)的容錯性.同時，在此基礎(chǔ)上提出了一種兩級故障檢測方法，提高了系統(tǒng)對各傳感器故障的容錯性.其中，第一級檢測使用了基于解析冗余[10]的殘差卡方檢驗法，可檢驗出ODO、INS 和部分GNSS 故障，并保證了第二級檢驗的狀態(tài)傳播過程不受污染.第二級檢測使用了改進的雙狀態(tài)傳播卡方檢驗算法，可用于進一步檢出GNSS 軟故障.

1 INS/GNSS/ODO 組合導航

1.1 狀態(tài)方程

INS/GNSS/ODO 松組合導航系統(tǒng)選取十六維向量作為系統(tǒng)狀態(tài)向量，

式中：δr為位置誤差；δv為速度誤差；ψ為姿態(tài)誤差；bg為陀螺儀零偏；ba為加速度計零偏；δk為里程計標度因子誤差.

系統(tǒng)狀態(tài)方程可表示為

式中：F(t)為狀態(tài)轉(zhuǎn)移矩陣；G(t)為系統(tǒng)噪聲驅(qū)動矩陣；w(t)為系統(tǒng)噪聲.該系統(tǒng)是以GNSS、ODO 輔助INS 的形式運行的.

在式(2)中，狀態(tài)轉(zhuǎn)移矩陣推導自INS 的誤差方程，常用的有ψ 角誤差模型[11]：

式中：f為加速度計輸出的比力；δg為重力誤差；投影坐標系b系為載體坐標系；投影坐標系c系為計算坐標系；p系為平臺坐標系；ψ 為c系到p系間的誤差角.

根據(jù)機械編排誤差模型和MEMS 的一階馬爾可夫模型可得到離散的狀態(tài)轉(zhuǎn)移矩陣，如式(6)～(8)所示[18]：

式中：RM為子午圈曲率半徑；RN為酉卯圈曲率半徑；g為當?shù)刂亓?；h為地理高度；?t為時間更新的時間間隔；?k/k?1為離散形式的狀態(tài)轉(zhuǎn)移矩陣；i系為地心赤道慣性系；e系為地心地固系；n系為導航系.

1.2 量測方程

1.3 基于多GNSS 狀態(tài)的抗差濾波

當林蔭道、隧道等復雜場景下，GNSS 信號容易受到影響，RTK 解在固定解、浮點解、抗差解和無有效解四種狀態(tài)下變化.GNSS 量測不確定協(xié)方差矩陣

式中：Ξ()函數(shù)將向量變換為對角矩陣，同時非對角線上元素全為0 的方陣；σ2為具體GNSS 狀態(tài)下的通過參數(shù)估計法獲取的三軸上協(xié)方差向量.

由于R1的不連續(xù)性，在R1從低定位精度解狀態(tài)變?yōu)榈投ㄎ痪冉鉅顟B(tài)時，系統(tǒng)狀態(tài)量可靠性不高，系統(tǒng)檢出量測粗差或故障的能力差.同時GNSS 量測中存在較多的粗差，故可根據(jù)式(23)～(32)所表示的抗差濾波算法對組合時的量測不確定性進行調(diào)整[13].

式中：Qk為系統(tǒng)狀態(tài)動態(tài)變化中噪聲的協(xié)方差矩陣；Ση為任意參量η的協(xié)方差矩陣；為預測殘差，又稱為新息[14]；為量測不確定協(xié)防差矩陣R1,k的參數(shù)估計值；N 表示參數(shù)估計的窗口大小；βk為抗差因子；在貫序卡爾曼濾波中，下標1 特指GNSS的量測更新過程.

由于模型中錯誤地判斷GNSS 固定解將對系統(tǒng)的穩(wěn)定性造成的影響極大，在濾波前可使用GNSS 位置精度因子(PDOP)對可疑值進行剔除.

式中：pdop為測得的PDOP 值；CBAD為給定的閾值常量.

抗差濾波算法可以降低GNSS 粗差的影響，但是在假定其僅有GNSS 異常下進行的，當ODO 異常或機械編排結(jié)果異常時，抗差濾波反而會降低組合結(jié)果的精度.同時當GNSS 在一段時間內(nèi)出現(xiàn)偏差時，抗差的效果并不如故障檢出后將GNSS 屏蔽的效果好.

2 故障檢測系統(tǒng)

2.1 傳統(tǒng)的預測殘差卡方故障檢驗法

χ2故障檢測是一種通過判斷n維高斯分布的隨機向量的均值是否與假設一致來判斷系統(tǒng)是否故障的方法.根據(jù)隨機向量選取的不同，可分為基于狀態(tài)向量的χ2故障檢測和基于殘差的χ2

根據(jù)式(25)和式(29)，可構(gòu)造評價函數(shù)

2.2 改進的雙狀態(tài)傳播卡方故障檢驗法

如圖1 所示，雙狀態(tài)傳播χ2故障檢測器由雙狀態(tài)傳播器和卡方檢驗器組成.其中雙狀態(tài)傳播器又由兩個相互獨立的狀態(tài)傳播器及其控制開關(guān)組成，狀態(tài)傳播過程是從標準卡爾曼濾波中獨立出來的.

圖1 雙狀態(tài)傳播卡方故障檢測器

傳統(tǒng)的雙狀態(tài)傳播χ2故障檢測法一般用于具有高性能IMU 的系統(tǒng)中.由于消費級MEMS 的精度較差，狀態(tài)傳播的誤差呈指數(shù)增長，導致故障檢出率比傳統(tǒng)的殘差χ2故障檢測法差.針對這一問題，如圖2所示，本文在狀態(tài)傳播器后增加了ODO 量測更新單元，使狀態(tài)遞推的誤差在一定時間內(nèi)呈線性增長趨勢.

圖2 改進的雙狀態(tài)傳播器

為了理論驗證改進算法的有效性及適用范圍，以單軸線性軟故障為例，當故障發(fā)生時GNSS 立即屏蔽.并假設線性軟故障增長率為a，各軸的狀態(tài)遞推誤差增長率均為b(b

可化簡為

2.3 兩級故障檢測系統(tǒng)模型

上述的兩種方法可以用于檢測系統(tǒng)是否故障，但嚴格意義上并不能鎖定故障的出現(xiàn)位置.另一方面，雙狀態(tài)傳播卡方故障檢測法雖然能夠處理GNSS 軟故障，但其同時面臨著ODO 故障或INS 故障污染的風險.相較于不使用該方法，當ODO 故障或INS 故障發(fā)生時，系統(tǒng)將變得更不穩(wěn)定[17].

針對上訴兩個問題，本文提出了一種兩級故障檢測系統(tǒng)，如圖3 所示.在第一級檢測中，系統(tǒng)分別計算出三組新息及卡方值，并在故障檢測判決器中通過基于解析冗余的故障檢測方法來初步鎖定故障位置.當系統(tǒng)認為第一級檢測并不足以檢測出GNSS 軟故障時，系統(tǒng)將通過改進的雙狀態(tài)傳播卡方故障檢測法進行進一步判斷.

圖3 基于解析冗余的故障檢測系統(tǒng)模型

由式(37)可知，GNSS 量測的精度遠大于INS/ODO 航跡推算的位置精度，第二級檢測將被跳過，故可根據(jù)RTK 定位是否為固定解設計如表1和表2 所示.

在故障處理方面，當GNSS 故障出現(xiàn)時，系統(tǒng)屏蔽GNSS 量測更新.當ODO 出現(xiàn)故障時，系統(tǒng)屏蔽包含改進的雙狀態(tài)傳播χ2故障檢測器在內(nèi)的ODO 量測更新，同時不對ODO 標度因子進行反饋和修改.當INS 發(fā)生故障時，系統(tǒng)將對改進的雙狀態(tài)傳播χ2器重置及屏蔽處理，防止狀態(tài)傳播器內(nèi)的狀態(tài)受到污染，同時適當調(diào)節(jié)Q 陣的自適應組合導航算法避免組合導航系統(tǒng)發(fā)散.

表1 RTK 固定解時的第一級故障判決表

表2 含噪定位解時的第一級故障判決表

3 實驗仿真與實驗分析

3.1 路測裝置及實驗條件

圖4 為GN310 嵌入式系統(tǒng)，車載實驗的核心裝置為自主研制的一款具有通信導航一體化功能的低成本嵌入式系統(tǒng)—GN310.該系統(tǒng)通過4G通信從千尋獲取RTK 差分改正數(shù)并在內(nèi)部GNSS芯片中進行單頻RTK 計算，在開闊環(huán)境下可獲取厘米級定位精度信息.

圖4 GN310 嵌入式系統(tǒng)

為了提高在復雜場景下的魯棒性，系統(tǒng)內(nèi)還集成了IMU、磁力計等器件，其中IMU 的性能如表3 所示.系統(tǒng)通過DB9 接口接入分辨率為2000P/R 的編碼式里程計即可運行GNSS/INS/ODO組合導航算法，并實時輸出高精度強魯棒性位置速度等信息.實驗使用的參考系統(tǒng)為裝有光纖陀螺、雙頻RTK 的GNSS/INS 緊組合系統(tǒng)NovAtel CPT6.實驗過程中使用表4 中參數(shù)對組合導航原始觀測數(shù)據(jù)進行修正.

表3 IMU 性能指標

表4 修正參數(shù)

3.2 抗差組合導航的仿實時實驗

實驗時長為1 000s，INS 運算頻率100Hz，ODO 量測更新頻率10Hz，GNSS 量測更新頻率均為1Hz，平均車速為40km/h.

如圖5 所示全路段經(jīng)過四次隧道，路段中存在多處林蔭道，路段中存在的3 處實為浮點解或抗差解被錯誤判為固定解的GNSS 結(jié)果.水平位置誤差如圖6所示，x軸中，黃色點表示浮點解，占總測試時長的40.6%；粉色表示抗差解，占總測試時長的3.1%.標準卡爾曼濾波下，水平位置均方根誤差(RMSE)為0.67m；多GNSS 狀態(tài)抗差濾波下，水平位置RMSE為0.42m，定位效果提升39.7%.

圖5 實驗路段

圖6 水平位置誤差與時間的關(guān)系

3.3 故障檢驗系統(tǒng)的半實物仿真

考慮到實測中同時可遇到多種故障的情形有限，本節(jié)選用另一組全路段固定解的數(shù)據(jù)進行半實物仿真[15].測試路段平均車速為30km/h.分階段施加ODO 故障、GNSS 硬故障、GNSS 軟故障和INS故障.如表5 所示，仿真過程分為兩組，第一組在RTK 固定解下進行，第二組在含噪聲定位解下進行.其中含噪聲定位解在RTK 固定解的基礎(chǔ)上各軸添加1m 高斯白噪聲來模擬.

表5 故障條件

在RTK 固定解的情況下，故障檢測值如圖7所示，故障屏蔽效果如圖8 所示，其中硬故障值與真值的差在故障區(qū)間內(nèi)為常向量，而軟故障值與真值的差在故障區(qū)間內(nèi)為遞增向量.根據(jù)表5 的判決條件，表中的四種故障皆能有效地辨別出.在GNSS 漸變故障時，改進雙狀態(tài)傳播卡方檢測算法與殘差卡方檢測結(jié)果接近，故嵌入式系統(tǒng)中可跳過雙狀態(tài)傳播卡方檢測算法的計算，實現(xiàn)降低計算量的目標.

圖7 RTK 固定解下的卡方故障檢測

圖8 RTK 固定解下故障屏蔽效果

在單點定位解時，標準和改進的雙狀態(tài)傳播器的窗口長度都為10s.如圖9 所示，第一層故障檢測能檢出ODO 故障.圖10 是根據(jù)先驗已知的故障時間來屏蔽GNSS 量測更新，由于低成本嵌入式組合導航系統(tǒng)的IMU 性能較差，故標準的雙狀態(tài)傳播卡方檢測效果較差.

圖9 含噪聲定位解下的卡方故障檢測

圖10 含噪聲定位解下根據(jù)時間屏蔽的卡方結(jié)果

在根據(jù)實際的檢測閾值處理故障時，如圖11所示，在GNSS 軟故障發(fā)生時，由于故障初期殘差卡方檢測法未能檢測出該故障，組合導航結(jié)果被拉偏.而改進的雙卡方檢測法在故障發(fā)生后的第15s 時檢測出并修復了故障，從而保證了系統(tǒng)的穩(wěn)定性和可靠性.

圖11 含噪聲定位解下的卡方故障檢測

4 結(jié)束語

本文提出了基于INS/GNSS/ODO 抗差組合導航算法的兩極故障檢測方法，第一級檢測使用了基于解析冗余的殘差卡方檢測法，第二級檢測使用了改進的雙狀態(tài)傳播卡方檢測算法.相比于傳統(tǒng)故障檢測方法，該容錯方法能夠有效地處理ODO、INS 故障和GNSS 軟故障，提高了系統(tǒng)的導航性能和魯棒性.該算法除了能夠用于檢測硬件故障，同時還能夠在車輛輪胎空轉(zhuǎn)和側(cè)滑、ODO 接口接觸檢測、GNSS 防欺騙等方面起到積極作用，具有良好的工程應用意義.

致謝：西北工業(yè)大學自動化學院嚴恭敏老師和武漢大學衛(wèi)星導航定位技術(shù)研究中心牛小驥團隊的討論.