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

0 引言

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

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

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

1 INS/GNSS/ODO 組合導(dǎo)航

1.1 狀態(tài)方程

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

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

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

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

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

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

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

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

1.2 量測方程

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

可化簡為

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

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

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

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

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

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

表1 RTK 固定解時(shí)的第一級故障判決表

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

3 實(shí)驗(yàn)仿真與實(shí)驗(yàn)分析

3.1 路測裝置及實(shí)驗(yàn)條件

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

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

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

表3 IMU 性能指標(biāo)

表4 修正參數(shù)

3.2 抗差組合導(dǎo)航的仿實(shí)時(shí)實(shí)驗(yàn)

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

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

圖5 實(shí)驗(yàn)路段

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

3.3 故障檢驗(yàn)系統(tǒng)的半實(shí)物仿真

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

表5 故障條件

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

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

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

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

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

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

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

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

4 結(jié)束語

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

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