基于交互式多模型的車載INS/OD/GPS的定位方法

徐 博，陳 崇，王連釗

（哈爾濱工程大學(xué) 智能科學(xué)與工程學(xué)院，哈爾濱 150001）

鑒于傳感器的互補(bǔ)特性，衛(wèi)星導(dǎo)航和慣性導(dǎo)航數(shù)據(jù)融合是導(dǎo)航定位系統(tǒng)的常用方案。對(duì)具有高斯噪聲的線性狀態(tài)空間模型，卡爾曼濾波在最小均方誤差方面是最優(yōu)的[1]，因此廣泛地應(yīng)用于衛(wèi)星導(dǎo)航和慣性導(dǎo)航的數(shù)據(jù)融合中。實(shí)際工程應(yīng)用中，衛(wèi)星定位存在跳點(diǎn)、低速運(yùn)動(dòng)定位精度偏低等問題，衛(wèi)星定位離群點(diǎn)的存在會(huì)破壞高斯噪聲框架下的卡爾曼濾波器，降低卡爾曼濾波器的性能，影響INS/GPS 組合導(dǎo)航的定位精度。組合導(dǎo)航故障檢測(cè)方法大致可分為三類[2]：基于模型的組合導(dǎo)航故障檢測(cè)算法、基于信號(hào)處理的組合導(dǎo)航故障檢測(cè)算法以及基于機(jī)器學(xué)習(xí)的組合導(dǎo)航故障檢測(cè)算法。通過建立濾波器的殘差及殘差協(xié)方差，檢測(cè)殘差是否服從分布假設(shè)的基于模型的故障檢測(cè)方法應(yīng)用較為廣泛[3]。

多模型算法常被視作一種有效解決結(jié)構(gòu)和參數(shù)不確定或參數(shù)變化系統(tǒng)狀態(tài)估計(jì)問題的方法。交互式多模型方法多用于目標(biāo)跟蹤中運(yùn)動(dòng)模型的切換，組合導(dǎo)航量測(cè)多模型的切換以及模型參數(shù)的辨識(shí)[4-6]。在故障檢測(cè)中，當(dāng)涉及系統(tǒng)結(jié)構(gòu)變化時(shí)，可分別建立正常工作和與之對(duì)應(yīng)的故障模型，應(yīng)用交互式多模型方法對(duì)故障進(jìn)行檢測(cè)與隔離[7]。當(dāng)GPS 故障量測(cè)失效時(shí)，可以借助運(yùn)動(dòng)模型進(jìn)行模型交互，對(duì)定位系統(tǒng)進(jìn)行輔助[8]。對(duì)于GPS 未知故障，在貝葉斯推理的框架下，利用β分布與二項(xiàng)分布互為共軛先驗(yàn)的特性或者利用馬爾可夫過程設(shè)置轉(zhuǎn)移概率矩陣可實(shí)現(xiàn)對(duì)故障的識(shí)別[9]，轉(zhuǎn)移概率矩陣的更新是多模型算法實(shí)現(xiàn)自適應(yīng)估計(jì)的基礎(chǔ)，因此利用交互式多模型算法從原理上可以實(shí)現(xiàn)對(duì)故障的識(shí)別。

交互式多模型算法傳統(tǒng)應(yīng)用于模型參數(shù)存在不確定性。多濾波器并行工作的條件下，為了保證濾波的效果，要求將各濾波器的獨(dú)立參數(shù)組成模型集，并且模型集要涵蓋參數(shù)范圍。無論是目標(biāo)跟蹤領(lǐng)域中運(yùn)動(dòng)模型的切換，還是組合導(dǎo)航中針對(duì)量測(cè)噪聲統(tǒng)計(jì)特性發(fā)生變化時(shí)量測(cè)模型的切換，這些方法在實(shí)際應(yīng)用過程中，很難保證模型集覆蓋所有可能情況，受到模型集完備性和準(zhǔn)確性的制約。本文提出基于交互式多模型的車載INS/OD/GPS 的定位方法是在INS/GPS 的基礎(chǔ)上引入里程計(jì)速度量測(cè)，通過INS/GPS、INS/OD兩種組合導(dǎo)航模型進(jìn)行交互，相互輔助對(duì)比，用兩系統(tǒng)模型概率對(duì)比檢測(cè)并修正INS/GPS 的量測(cè)跳變故障。

1 INS/GPS、INS/OD 系統(tǒng)模型描述

選取“東-北-天”（ENU）地理坐標(biāo)系為導(dǎo)航坐標(biāo)系，記為n系；將導(dǎo)航解算的計(jì)算導(dǎo)航參考坐標(biāo)系記為n′系；以為原點(diǎn)，選取“右-前-上”（RFU）坐標(biāo)系為載體坐標(biāo)系記為b系；以里程計(jì)為原點(diǎn)，選取“右-前-上”（RFU）坐標(biāo)系為里程計(jì)理想測(cè)量坐標(biāo)系為m系。

INS/GPS 和INS/OD 系統(tǒng)狀態(tài)方程和量測(cè)方程為：

為了保證INS/GPS 和INS/OD 濾波輸出的交互，兩系統(tǒng)選擇相同的誤差狀態(tài)向量：

誤差定義為計(jì)算導(dǎo)航系n′相對(duì)導(dǎo)航坐標(biāo)系n的偏差，其中φx、φy、φz代表計(jì)算導(dǎo)航系相對(duì)導(dǎo)航系在俯仰、橫滾、航向軸上的失準(zhǔn)角；代表東向、北向、天向速度誤差；Lδ、δλ、hδ代表緯度、經(jīng)度、高度誤差；?x、?y、?z為加速度計(jì)零偏；εx、εy、εz為陀螺常值漂移。假設(shè)系統(tǒng)噪聲W，量測(cè)噪聲V為高斯白噪聲，其中兩模型量測(cè)來源不同，設(shè)置噪聲形式如式(3)所示。wL、wλ、wh分別對(duì)應(yīng)GPS 緯度、經(jīng)度、高度量測(cè)的高斯白噪聲。wE、wN、Uw分別對(duì)應(yīng)里程計(jì)量測(cè)在導(dǎo)航系下東向、北向、天向速度誤差。

在INS/OD 組合中，里程計(jì)提供速度量測(cè)；在INS/GPS 組合中，GPS 提供位置量測(cè)，考慮載體系下桿臂誤差δlb的各系統(tǒng)量測(cè)，并對(duì)桿臂誤差進(jìn)行補(bǔ)償可得INS/GPS 和INS/OD 兩系統(tǒng)的量測(cè)方程及對(duì)應(yīng)的量測(cè)矩陣：

2 交互式多模型的車載INS/OD/GPS 的定位方法

2.1 INS/OD/GPS 交互式多模型算法

INS/GPS 組合導(dǎo)航系統(tǒng)設(shè)置為模型1，INS/OD 組合導(dǎo)航系統(tǒng)設(shè)置為模型2。交互式多模型方法的每個(gè)濾波器都有獨(dú)立參數(shù)，通過似然函數(shù)為各個(gè)模型分配模型概率，并以此為權(quán)重對(duì)各個(gè)模型的數(shù)據(jù)進(jìn)行融合。但是對(duì)于擁有兩個(gè)實(shí)際量測(cè)和一致估計(jì)狀態(tài)的濾波器系統(tǒng)，模型概率可以反映出模型接近當(dāng)前時(shí)刻真實(shí)狀態(tài)的程度。

鑒于 INS/GPS 組合導(dǎo)航的精度水平要高于INS/OD 組合的精度水平，在GPS 量測(cè)無故障時(shí)，INS/GPS 組合導(dǎo)航模型會(huì)一直保持較高的概率。一旦GPS 量測(cè)發(fā)生跳變，INS/GPS 組合導(dǎo)航模型同INS/OD組合模型間的精度領(lǐng)先優(yōu)勢(shì)會(huì)減弱，而導(dǎo)致模型概率的降低，以此來識(shí)別GPS 量測(cè)故障，并在量測(cè)跳變故障發(fā)生時(shí)刻進(jìn)行狀態(tài)更新，對(duì)狀態(tài)估計(jì)進(jìn)行修整后，調(diào)整模型概率進(jìn)行數(shù)據(jù)融合輸出?？傮w上基于交互式多模型的車載INS/OD/GPS 定位算法可分為：輸入交互、模型概率更新及故障檢測(cè)、濾波更新和交互輸出四個(gè)環(huán)節(jié)，算法結(jié)構(gòu)如圖1所示。

圖1 車載INS/OD/GPS 定位算法結(jié)構(gòu)圖Fig.1 Algorithm structure of INS/OD/GPS vehicle positioning method

輸入交互過程如式(6)所示。

進(jìn)一步通過預(yù)測(cè)歸一化常數(shù)和模型似然函數(shù)可以得到當(dāng)前時(shí)刻模型概率歸一化常數(shù)cj以及當(dāng)前時(shí)刻模型概率。模型概率更新如式(8)所示。

故障檢測(cè)主要針對(duì)GPS 量測(cè)。INS/GPS 組合導(dǎo)航引入故障量測(cè)后，INS/GPS 組合導(dǎo)航模型與INS/OD組合導(dǎo)航模型間的模型概率會(huì)產(chǎn)生較為明顯的差異性變化。設(shè)置故障參數(shù)β，故障閾值α（對(duì)于GPS 信號(hào)質(zhì)量較高的條件下可將α設(shè)置為0.9，相反在信號(hào)接收較差的條件下相應(yīng)的降低α取值），β=1 代表檢測(cè)量測(cè)故障發(fā)生，故障判斷如式(9)所示。

濾波更新過程會(huì)受到故障參數(shù)β的影響，針對(duì)GPS 量測(cè)跳變，將量測(cè)剔除只進(jìn)行狀態(tài)更新，以保證定位精度。反之，量測(cè)更新、狀態(tài)更新同時(shí)進(jìn)行，濾波更新過程如式(10)所示。

交互輸出便是基于模型概率對(duì)每個(gè)濾波器的估計(jì)結(jié)果加權(quán)合并，得到總的狀態(tài)估計(jì)和協(xié)方差矩陣，如式(11)所示。

2.2 跑車實(shí)驗(yàn)

基于交互式多模型的車載INS/OD/GPS 定位方法的性能通過陸地車輛道路試驗(yàn)來進(jìn)行檢驗(yàn)。傳感器數(shù)據(jù)是在實(shí)驗(yàn)期間收集的，實(shí)驗(yàn)用車為 FORD TRANSIT，搭載慣性系統(tǒng)的陀螺儀常漂為0.01°/h，加速度計(jì)零偏為50 μg，RTK 定位精度為0.05 m。試驗(yàn)用車及車上捷聯(lián)慣性導(dǎo)航系統(tǒng)安裝如圖2所示。

圖2 試驗(yàn)用車及捷聯(lián)慣組圖Fig.2 Vehicle and strapdown inertial navigation unit

試驗(yàn)選擇在哈爾濱江北新區(qū)濱水大道進(jìn)行，試驗(yàn)期間進(jìn)行了典型的駕駛操作，如在紅綠燈處轉(zhuǎn)彎和停車，試驗(yàn)路徑衛(wèi)星圖如圖3所示。準(zhǔn)確可靠的PHINS系統(tǒng)被用作定量比較的參考，該參考系統(tǒng)的定位精度為0.01 m，參考軌跡如圖4所示。通過與基準(zhǔn)的對(duì)比，試驗(yàn)過程出現(xiàn)GPS 量測(cè)故障的現(xiàn)象如圖5所示。

圖3 試驗(yàn)路徑衛(wèi)星圖Fig.3 Satellite map of vehicle experiment path

圖4 車載試驗(yàn)運(yùn)動(dòng)軌跡Fig.4 Vehicle experimental trajectory

圖5 GPS 經(jīng)緯度量測(cè)對(duì)比圖Fig.5 Comparison chart of GPS longitude and latitude

在IMM-INS/OD/GPS 定位方法中，INS/GPS 組合模型初始概率設(shè)置為1，INS/OD 組合模型初值設(shè)置為0。在INS/GPS 組合系統(tǒng)為模型1，INS/OD 組合系統(tǒng)為模型2 的設(shè)置條件下，一步轉(zhuǎn)移概率p11=0.9，p12=0.1，p21=0.2，p22=0.8，故障閾值α=0.9，量測(cè)陣方差RGPS=diag[（0.5m）2,（0.5m）2,（0.5m）2]，RINS=diag[（0.01m/s）2,（0.01m/s）2,（0.01m/s）2]。圖6-7分別為在GPS 量測(cè)發(fā)生跳變故障時(shí)，INS/GPS 與INS/OD/GPS-IMM定位方法下的姿態(tài)誤差、速度誤差、位置誤差和行駛軌跡。

圖6 INS/GPS 組合導(dǎo)航誤差圖Fig.6 Error chart of INS/GPS integrated navigation

圖7 INS/OD/GPS 交互式多模型定位方法誤差圖Fig.7 Error chart of INS/OD/GPS IMM positioning method

圖8-9 給出了當(dāng)GPS 量測(cè)出現(xiàn)離群點(diǎn)時(shí)，INS/GPS與INS/OD 組合導(dǎo)航模型的概率變化。在GPS 量測(cè)存在跳變時(shí)，INS/GPS 組合導(dǎo)航同基于交互式多模型的車載INS/OD/GPS 的定位方法定位誤差對(duì)比如圖10所示。表1又列舉了在不同初始模型概率和一步轉(zhuǎn)移概率下實(shí)驗(yàn)參數(shù)，圖10和圖11給出了表1中不同條件下的概率演變情況。

圖8 INS/OD/GPS 組合導(dǎo)航模型概率圖Fig.8 Probability of INS/OD/GPS integrated navigation model

圖9 模型概率放大圖Fig.9 Enlarged view of model probability chart

圖10 模型概率演變對(duì)比1Fig.10 Comparison of model probability evolution 1

圖11 模型概率演變對(duì)比2Fig.11 Comparison of model probability evolution 2

表1 模型概率演變?cè)囼?yàn)參數(shù)Tab.1 Parameters in model probability evolution test

圖10和圖11對(duì)比了相同初始模型概率，不同模型概率轉(zhuǎn)移矩陣下的模型概率演變情況。無后驗(yàn)概率更新下的模型概率變化反映的是人為主觀先驗(yàn)信息，先驗(yàn)信息的描述通過設(shè)置模型概率轉(zhuǎn)移矩陣 [pij]2×2實(shí)現(xiàn)。a 組和c 組無后驗(yàn)概率更新下INS/GPS 組合系統(tǒng)的模型概率長(zhǎng)期保持在1，反映了主觀先驗(yàn)認(rèn)定INS/GPS 組合系統(tǒng)精度優(yōu)于INS/OD 組合系統(tǒng)。c 組和d 組無后驗(yàn)概率更新下INS/GPS 組合與INS/OD 組合系統(tǒng)模型概率相等，說明對(duì)于模型精度的先驗(yàn)信息掌握為零，即認(rèn)定兩模型精度相同。后驗(yàn)概率更新下的模型概率演變反映了引入量測(cè)新息下兩組合系統(tǒng)模型的精度，此時(shí)實(shí)際模型的精度判斷是在主觀先驗(yàn)的基礎(chǔ)上通過引入量測(cè)新息下的后驗(yàn)概率進(jìn)行模型交互概率更新確定的，所以可以對(duì)量測(cè)故障進(jìn)行識(shí)別。通過圖10和圖11對(duì)比可以發(fā)現(xiàn)，模型概率初值對(duì)模型概率演變過程的作用可以忽略，量測(cè)新息下的后驗(yàn)概率更新決定了模型概率的演變趨勢(shì)，模型概率轉(zhuǎn)移矩陣作為先驗(yàn)新息的表達(dá)載體不能改變模型概率演變的趨勢(shì)，但是會(huì)影響模型單點(diǎn)概率的大小。

通過圖8-9 可以看出對(duì)于導(dǎo)航精度一直領(lǐng)先的INS/GPS 組合導(dǎo)航模型，在導(dǎo)航過程中存在模型概率由1 下降至0.4 甚至0 的突變點(diǎn)，對(duì)比圖6中INS/GPS組合導(dǎo)航故障檢測(cè)點(diǎn)可以看出，INS/OD 組合導(dǎo)航模型為GPS 量測(cè)故障的判斷提供了參考。圖12和表2通過試驗(yàn)全程?hào)|向、北向定位誤差的對(duì)比驗(yàn)證了基于交互式多模型的車載INS/OD/GPS 的定位方法在里程計(jì)量測(cè)輔助條件下，當(dāng)GPS 量測(cè)出現(xiàn)離群點(diǎn)時(shí)，系統(tǒng)平均定位精度比INS/GPS 單一組合導(dǎo)航模式的定位精度提升60%以上，達(dá)到厘米量級(jí)。

圖12 定位誤差對(duì)比圖Fig.12 Comparison chart of positioning error

表2 東向、北向定位誤差均值Tab.2 Mean value of east and north positioning error

3 結(jié) 論

基于交互式多模型的車載INS/OD/GPS 的定位方法主要討論了針對(duì)GPS 量測(cè)信息出現(xiàn)跳變導(dǎo)致濾波器濾波效果降低，定位精度下降的情況。通過在INS/GPS 組合導(dǎo)航的基礎(chǔ)上引入里程計(jì)量測(cè)，分別建立INS/GPS，INS/OD 兩個(gè)組合導(dǎo)航模型并進(jìn)行交互。當(dāng)穩(wěn)定的模型概率分布發(fā)生變化時(shí)，可以認(rèn)定GPS 量測(cè)出現(xiàn)故障，在故障發(fā)生后對(duì)故障量測(cè)進(jìn)行剔除，更新模型概率進(jìn)行融合。試驗(yàn)結(jié)果表明，引入里程計(jì)量測(cè)可以為GPS 量測(cè)跳變故障提供依據(jù)，基于交互式多模型的車載INS/OD/GPS 的定位方法可以對(duì)GPS 離群故障進(jìn)行有效識(shí)別，并實(shí)現(xiàn)系統(tǒng)定位精度的提升。