INS/GNSS/ODO 嵌入式系統(tǒng)的容錯(cuò)技術(shù)研究

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

0 引言

在嵌入式組合導(dǎo)航系統(tǒng)中，傳感器短時(shí)間的故障對(duì)系統(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)方法和利用粒子濾波的故障檢測(cè)方法可辨別出故障傳感器[3-4].由于其計(jì)算開銷較大，并不適用于嵌入式導(dǎo)航系統(tǒng).

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

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

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

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

1.2 量測(cè)方程

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

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

式中：Ξ()函數(shù)將向量變換為對(duì)角矩陣，同時(shí)非對(duì)角線上元素全為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)檢出量測(cè)粗差或故障的能力差.同時(shí)GNSS 量測(cè)中存在較多的粗差，故可根據(jù)式(23)～(32)所表示的抗差濾波算法對(duì)組合時(shí)的量測(cè)不確定性進(jìn)行調(diào)整[13].

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

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

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

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

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

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

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

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

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

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

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

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

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

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

可化簡(jiǎn)為

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

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

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

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

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

在故障處理方面，當(dāng)GNSS 故障出現(xiàn)時(shí)，系統(tǒng)屏蔽GNSS 量測(cè)更新.當(dāng)ODO 出現(xiàn)故障時(shí)，系統(tǒng)屏蔽包含改進(jìn)的雙狀態(tài)傳播χ2故障檢測(cè)器在內(nèi)的ODO 量測(cè)更新，同時(shí)不對(duì)ODO 標(biāo)度因子進(jìn)行反饋和修改.當(dāng)INS 發(fā)生故障時(shí)，系統(tǒng)將對(duì)改進(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í)的第一級(jí)故障判決表

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

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

3.1 路測(cè)裝置及實(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)境下可獲取厘米級(jí)定位精度信息.

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

為了提高在復(fù)雜場(chǎng)景下的魯棒性，系統(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ù)對(duì)組合導(dǎo)航原始觀測(cè)數(shù)據(jù)進(jìn)行修正.

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

表4 修正參數(shù)

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

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

如圖5 所示全路段經(jīng)過四次隧道，路段中存在多處林蔭道，路段中存在的3 處實(shí)為浮點(diǎn)解或抗差解被錯(cuò)誤判為固定解的GNSS 結(jié)果.水平位置誤差如圖6所示，x軸中，黃色點(diǎn)表示浮點(diǎn)解，占總測(cè)試時(shí)長(zhǎng)的40.6%；粉色表示抗差解，占總測(cè)試時(shí)長(zhǎng)的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í)測(cè)中同時(shí)可遇到多種故障的情形有限，本節(jié)選用另一組全路段固定解的數(shù)據(jù)進(jìn)行半實(shí)物仿真[15].測(cè)試路段平均車速為30km/h.分階段施加ODO 故障、GNSS 硬故障、GNSS 軟故障和INS故障.如表5 所示，仿真過程分為兩組，第一組在RTK 固定解下進(jìn)行，第二組在含噪聲定位解下進(jìn)行.其中含噪聲定位解在RTK 固定解的基礎(chǔ)上各軸添加1m 高斯白噪聲來(lái)模擬.

表5 故障條件

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

圖7 RTK 固定解下的卡方故障檢測(cè)

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

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

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

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

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

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

4 結(jié)束語(yǔ)

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

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