李 旦，劉 明，胡華峰

(湖北航天技術(shù)研究院總體設(shè)計(jì)所，武漢 430040)

0 引言

基于慣性導(dǎo)航的多源信息融合組合導(dǎo)航技術(shù)已廣泛應(yīng)用于海、陸、空、天等各領(lǐng)域的組合導(dǎo)航系統(tǒng)，全球?qū)Ш叫l(wèi)星系統(tǒng)(Global Navigation Satellite System，GNSS)、星敏感器、測(cè)距儀、測(cè)速儀、里程計(jì)、氣壓計(jì)、激光雷達(dá)、視覺導(dǎo)航、聲學(xué)傳感器等輔助導(dǎo)航設(shè)備的發(fā)展和應(yīng)用，對(duì)提升組合導(dǎo)航性能和精度起到了關(guān)鍵性的作用[1-3]。

對(duì)于陸地車輛而言，慣導(dǎo)/GNSS/里程計(jì)/氣壓計(jì)組合導(dǎo)航系統(tǒng)是目前比較成熟的方案，它能夠提供精確的位置、姿態(tài)和速度信息，可以提高車輛的機(jī)動(dòng)性和靈活性[4]。在衛(wèi)星信號(hào)受干擾或無衛(wèi)星信號(hào)可用的環(huán)境下，陸地車輛特別是陸地戰(zhàn)車的自主導(dǎo)航能力對(duì)于提升自身的生成能力和快速反應(yīng)能力具有至關(guān)重要的作用。

慣導(dǎo)/里程計(jì)/氣壓計(jì)自主組合導(dǎo)航方法能提供數(shù)十米級(jí)別的水平位置精度和10 m級(jí)別的高程位置精度，但里程計(jì)的測(cè)量誤差受環(huán)境影響較大[5]，海拔、氣候、溫濕度、路面粗糙程度、載重等都會(huì)影響里程計(jì)測(cè)量的準(zhǔn)確性，從而影響自主組合導(dǎo)航系統(tǒng)的精度。為了提高自主組合導(dǎo)航系統(tǒng)的精度，本文將路網(wǎng)匹配算法與慣性/里程計(jì)組合方法相結(jié)合[6-8]，一方面慣性/里程計(jì)組合可以為路網(wǎng)匹配提供高精度的導(dǎo)航軌跡數(shù)據(jù)，另一方面路網(wǎng)匹配根據(jù)導(dǎo)航軌跡數(shù)據(jù)與事先采集的路網(wǎng)數(shù)據(jù)進(jìn)行對(duì)比匹配，得到高精度的匹配結(jié)果用以修正慣導(dǎo)/里程計(jì)導(dǎo)航誤差，如此往復(fù)迭代修正，能夠進(jìn)一步提高組合導(dǎo)航的精度。

1 路網(wǎng)匹配組合導(dǎo)航

1.1 路網(wǎng)匹配介紹

路網(wǎng)匹配最早與地理信息系統(tǒng)(Geographic Information System，GIS)結(jié)合使用，以提高定位車輛在電子地圖中的位置精度[9-11]。路網(wǎng)匹配作為一種軟件糾錯(cuò)技術(shù)，將導(dǎo)航定位系統(tǒng)采集的位置信息與路網(wǎng)數(shù)據(jù)庫中的位置信息進(jìn)行比較，計(jì)算出導(dǎo)航系統(tǒng)在路網(wǎng)中的匹配位置[12]；由于路網(wǎng)數(shù)據(jù)庫中的位置信息往往精度較高，通過匹配后能夠用來修正車載導(dǎo)航系統(tǒng)的定位誤差，提高車載導(dǎo)航的精度。

點(diǎn)到線路網(wǎng)匹配方法將方向、點(diǎn)到道路距離等因素通過各自加權(quán)系數(shù)進(jìn)行累加，運(yùn)用權(quán)重的策略將點(diǎn)匹配到道路上。然而這種點(diǎn)到線的匹配由于缺乏對(duì)整體軌跡趨勢(shì)的考慮，在復(fù)雜的路網(wǎng)環(huán)境下容易導(dǎo)致誤匹配?，F(xiàn)如今大多數(shù)針對(duì)軌跡數(shù)據(jù)的地圖匹配方法采用基于曲線相似度的全局匹配方法[13]，曲線匹配與點(diǎn)和線的匹配有差異，需要使用距離準(zhǔn)則來定義2條曲線間的相似程度。

近年來，數(shù)學(xué)學(xué)科得到快速發(fā)展，涌現(xiàn)出了多種判別曲線相似度的方法，使用較廣泛的是相似性函數(shù)判別方法以及特征值判別方法[14]。其中Hausdorff距離主要用于計(jì)算點(diǎn)集之間的相似度，F(xiàn)rechet距離用于計(jì)算曲線之間的相似度。Frechet方法選取Frechet距離作為路段的權(quán)重，構(gòu)建網(wǎng)絡(luò)圖，計(jì)算最短路徑，從而獲得最佳的匹配路段，但是Frechet距離是考慮距離集合中的最大值，因此很容易受到異常值的影響。

本文面向的對(duì)象主要是陸地戰(zhàn)車等軍事化裝備，它們行駛的路網(wǎng)與城市車輛道路路網(wǎng)不同，城市道路路網(wǎng)交匯點(diǎn)和分節(jié)點(diǎn)多，而且路網(wǎng)縱橫交錯(cuò)，用來描述路網(wǎng)的數(shù)據(jù)庫表現(xiàn)形式復(fù)雜[15-17]；而陸地戰(zhàn)車所需要覆蓋的路網(wǎng)較為單一，主要圍繞營(yíng)區(qū)或訓(xùn)練區(qū)域，路網(wǎng)數(shù)量和復(fù)雜程度有所降低。因此本文在路網(wǎng)匹配算法的基礎(chǔ)上，根據(jù)實(shí)際使用情況，應(yīng)用路網(wǎng)匹配算法與慣性組合導(dǎo)航歷史數(shù)據(jù)相結(jié)合，考慮車輛行駛軌跡曲線的整體變化趨勢(shì)，基于全局匹配的思想提出了慣性/里程計(jì)/路網(wǎng)匹配組合導(dǎo)航算法，解決了陸地戰(zhàn)車自主導(dǎo)航相關(guān)技術(shù)問題。

1.2 改進(jìn)匹配方案

首先，為了實(shí)現(xiàn)高精度匹配定位的可靠性，對(duì)路網(wǎng)數(shù)據(jù)庫進(jìn)行優(yōu)化處理，即將路網(wǎng)數(shù)據(jù)處理成10m等間隔的點(diǎn)集合，并在此基礎(chǔ)上進(jìn)行后續(xù)路網(wǎng)匹配。

路網(wǎng)匹配以慣性/里程計(jì)組合導(dǎo)航歷史軌跡數(shù)據(jù)作為輸入，構(gòu)建導(dǎo)航數(shù)據(jù)緩沖區(qū)，按照匹配準(zhǔn)則獲取匹配路段及匹配點(diǎn)。整個(gè)匹配過程及步驟如下：

步驟1：獲取慣性/里程計(jì)組合導(dǎo)航定位點(diǎn)的位置信息，計(jì)算慣性定位行駛里程，若小于最短里程計(jì)要求(例如3km)，則不進(jìn)行匹配；若滿足最短里程要求，則開始匹配。

步驟2：首先在慣性/里程計(jì)組合導(dǎo)航定位點(diǎn)誤差范圍(例如100m)內(nèi)搜尋待匹配的路網(wǎng)，從路網(wǎng)中第一條道路開始，遍歷該條道路中所有點(diǎn)到導(dǎo)航定位點(diǎn)的距離，取距離最小值，若距離最小值大于誤差范圍(100m)，則該條道路不在待選擇范圍內(nèi)；若距離最小值小于誤差范圍，則該條道路為待選擇道路。遍歷路網(wǎng)中的所有道路，列出所有待選擇道路

Rds={Rd1,Rd2,Rd3，…，Rdn}

(1)

步驟3：針對(duì)一條待選擇道路，將其中與定位點(diǎn)距離小于誤差范圍的點(diǎn)構(gòu)成一個(gè)點(diǎn)集

Pts={Pt1,Pt2,Pt3，…，Ptn}

(2)

步驟4：從點(diǎn)集中第一個(gè)點(diǎn)開始，根據(jù)慣導(dǎo)的歷史軌跡數(shù)據(jù)(例如數(shù)據(jù)長(zhǎng)度3000m，同樣需要對(duì)歷史軌跡數(shù)據(jù)進(jìn)行優(yōu)化處理得到10m等間隔點(diǎn)集)與路網(wǎng)數(shù)據(jù)進(jìn)行比較，構(gòu)造慣導(dǎo)數(shù)據(jù)與路網(wǎng)數(shù)據(jù)的方位角差值函數(shù)和距離差值函數(shù)。方位角計(jì)算方法如式(3)所示。

(3)

其中，Δλ表示前后2個(gè)點(diǎn)的經(jīng)度之差，ΔL表示前后2個(gè)點(diǎn)的緯度之差。慣導(dǎo)點(diǎn)與路網(wǎng)點(diǎn)的距離計(jì)算方法如下

(4)

步驟5：構(gòu)造匹配準(zhǔn)則，方位角差值函數(shù)值小于閾值，并且同時(shí)距離差值函數(shù)值也小于閾值。

步驟6：對(duì)路網(wǎng)數(shù)據(jù)庫中每條道路的點(diǎn)集Pts按照匹配準(zhǔn)則進(jìn)行遍歷，得到滿足匹配準(zhǔn)則的匹配點(diǎn)，一般情況下滿足條件的匹配點(diǎn)不止一個(gè)，選擇距離差值函數(shù)最小的點(diǎn)作為匹配點(diǎn)。如果不存在滿足條件的匹配點(diǎn)，則匹配結(jié)果為空。

步驟7：匹配結(jié)束，匹配完成后根據(jù)路網(wǎng)匹配的結(jié)果修正慣導(dǎo)位置誤差，以慣導(dǎo)位置與匹配點(diǎn)位置之差作為卡爾曼濾波器的量測(cè)輸入值。

步驟8：進(jìn)行卡爾曼濾波，使用濾波估計(jì)值校正慣導(dǎo)誤差，并開始下一次匹配。

2 慣性/里程計(jì)/路網(wǎng)匹配組合導(dǎo)航

組合導(dǎo)航濾波器狀態(tài)向量選取慣導(dǎo)姿態(tài)誤差、速度誤差、位置誤差、陀螺誤差、加速度計(jì)誤差共15維向量，即

(5)

根據(jù)慣性導(dǎo)航誤差方程得到狀態(tài)轉(zhuǎn)移矩陣。量測(cè)值的選取根據(jù)慣導(dǎo)位置與路網(wǎng)匹配位置來確定，即以慣導(dǎo)位置與路網(wǎng)匹配位置之差作為卡爾曼濾波器的量測(cè)輸入

Zk=δP=δPSINS-δPMAP
=H(t)X+v(t)

(6)

式中v(t)表示量測(cè)噪聲，且有

(7)

組合導(dǎo)航采用反饋校正的方式，通過濾波估計(jì)出姿態(tài)誤差、速度誤差、位置誤差及器件誤差后實(shí)時(shí)反饋修正慣導(dǎo)的姿態(tài)、速度、位置及器件參數(shù)，其結(jié)構(gòu)原理如圖 1所示。

圖1 慣性/里程計(jì)/路網(wǎng)匹配組合導(dǎo)航結(jié)構(gòu)Fig.1 Flow chart of SINS/odometer/road network matching integrated navigation

3 路網(wǎng)匹配試驗(yàn)驗(yàn)證

首先對(duì)高精度定位定向系統(tǒng)(Position and Orientation System, POS)采集的道路軌跡數(shù)據(jù)進(jìn)行優(yōu)化處理，得到等間隔的路網(wǎng)數(shù)據(jù)庫，比較路網(wǎng)中前后點(diǎn)距離間隔如圖 2所示。從圖2中可以看出，路網(wǎng)數(shù)據(jù)庫中各個(gè)點(diǎn)等間隔均勻分布。

圖2 路網(wǎng)數(shù)據(jù)庫間隔距離Fig.2 Distance of road network points

本文的路網(wǎng)匹配驗(yàn)證分2個(gè)步驟進(jìn)行，首先僅根據(jù)慣性/里程計(jì)組合導(dǎo)航的結(jié)果進(jìn)行路網(wǎng)匹配，匹配的結(jié)果不參與后續(xù)組合導(dǎo)航解算，即開環(huán)驗(yàn)證，主要目的是比較在組合導(dǎo)航基礎(chǔ)上路網(wǎng)匹配的精度；第二步是根據(jù)組合導(dǎo)航的結(jié)果進(jìn)行路網(wǎng)匹配，并將匹配結(jié)果進(jìn)行反饋用于修正慣導(dǎo)誤差，通過不斷迭代和閉環(huán)修正減少慣導(dǎo)誤差。

3.1 開環(huán)驗(yàn)證

選取一組定位導(dǎo)航試驗(yàn)數(shù)據(jù)，首先根據(jù)GNSS數(shù)據(jù)生成路網(wǎng)數(shù)據(jù)庫，然后進(jìn)行慣性/里程計(jì)組合導(dǎo)航解算，并得到組合結(jié)果。根據(jù)組合導(dǎo)航結(jié)果進(jìn)行路網(wǎng)匹配，以采集的GNSS位置信息作為參考基準(zhǔn)，比較組合導(dǎo)航結(jié)果與路網(wǎng)匹配結(jié)果，統(tǒng)計(jì)組合導(dǎo)航誤差與路網(wǎng)匹配誤差，組合導(dǎo)航軌跡及匹配誤差如圖 3所示。

(a)組合導(dǎo)航軌跡

(b)導(dǎo)航誤差及匹配誤差

(c)誤差直方圖圖3 組合導(dǎo)航及路網(wǎng)匹配誤差示意圖(開環(huán))Fig.3 Error of integrated navigation and road network matching (open loop revise)

根據(jù)組合導(dǎo)航結(jié)果及開環(huán)匹配結(jié)果，導(dǎo)航軌跡包含東西向及南北向路線，組合導(dǎo)航結(jié)果最大誤差約40m，經(jīng)過路網(wǎng)匹配后，根據(jù)導(dǎo)航點(diǎn)與匹配的誤差曲線可以看出，匹配過程識(shí)別出了導(dǎo)航誤差，從而匹配點(diǎn)與參考點(diǎn)誤差大部分在10m左右。根據(jù)誤差直方圖3(c)可以看出，經(jīng)過路網(wǎng)匹配后導(dǎo)航誤差分布明顯減小，相比于慣性/里程計(jì)組合導(dǎo)航結(jié)果，匹配效果顯著。

3.2 閉環(huán)驗(yàn)證

閉環(huán)驗(yàn)證采用與開環(huán)驗(yàn)證同樣的試驗(yàn)數(shù)據(jù)，匹配開始后根據(jù)匹配結(jié)果修正慣導(dǎo)誤差及慣性器件誤差，作為下一次慣導(dǎo)計(jì)算的初值，通過不斷地迭代修正，得到全程的導(dǎo)航定位誤差，試驗(yàn)結(jié)果如圖 4所示。

(a)組合導(dǎo)航軌跡

(b)導(dǎo)航誤差及匹配誤差

(c)誤差直方圖圖4 組合導(dǎo)航及路網(wǎng)匹配誤差示意圖(閉環(huán))Fig.4 Errors of integrated navigation and road network matching(closed loop revise)

根據(jù)閉環(huán)試驗(yàn)驗(yàn)證結(jié)果，經(jīng)過路網(wǎng)匹配對(duì)慣性導(dǎo)航結(jié)果的迭代修正后，慣性/里程計(jì)/路網(wǎng)匹配組合導(dǎo)航結(jié)果定位誤差明顯減小，大部分誤差控制在10m左右。對(duì)比開環(huán)試驗(yàn)驗(yàn)證與閉環(huán)試驗(yàn)驗(yàn)證的誤差直方圖，將路網(wǎng)匹配與慣性組合導(dǎo)航相結(jié)合，能大幅度提高組合導(dǎo)航系統(tǒng)的精度。并且路網(wǎng)匹配與慣性組合導(dǎo)航相輔相成，路網(wǎng)匹配能夠提供高精度的位置信息，用于修正慣性組合導(dǎo)航結(jié)果；而慣性組合導(dǎo)航通過匹配修正后能夠減少誤差，同樣為路網(wǎng)匹配提供高精度的待匹配點(diǎn)位置信息，降低匹配誤差。

慣性器件誤差估計(jì)結(jié)果如圖 5所示。從估計(jì)曲線可以看出，經(jīng)過慣性/里程計(jì)/路網(wǎng)匹配組合導(dǎo)航后加表零偏誤差與陀螺零偏誤差估計(jì)穩(wěn)定，間接表明了組合導(dǎo)航的性能。

圖5 慣性器件誤差估計(jì)曲線Fig.5 Estimation curve of sensor of IMU

為了進(jìn)一步驗(yàn)證路網(wǎng)匹配的精度，本文選取一組長(zhǎng)時(shí)間的試驗(yàn)數(shù)據(jù)，并且人為增大慣性/里程計(jì)組合導(dǎo)航的誤差，得到路網(wǎng)匹配修正前后組合導(dǎo)航位置誤差曲線如圖 6所示。

根據(jù)路網(wǎng)匹配的試驗(yàn)結(jié)果，路網(wǎng)匹配修正前，慣性/里程計(jì)組合導(dǎo)航誤差較大且不斷發(fā)散，已無法提供準(zhǔn)確的導(dǎo)航信息;經(jīng)過路網(wǎng)匹配修正后，組合導(dǎo)航誤差明顯減小，且位置誤差保持穩(wěn)定，不存在發(fā)散趨勢(shì)，進(jìn)一步證明了路網(wǎng)匹配與慣性組合導(dǎo)航結(jié)合的優(yōu)勢(shì)。

(a)匹配前：慣性/里程計(jì)組合誤差

(b)匹配后：慣性/里程計(jì)/路網(wǎng)匹配組合誤差圖6 路網(wǎng)匹配結(jié)果對(duì)比Fig.6 Relatively result of map matching

3.3 試驗(yàn)小結(jié)

本節(jié)試驗(yàn)過程中先后通過開環(huán)驗(yàn)證試驗(yàn)和閉環(huán)驗(yàn)證試驗(yàn)驗(yàn)證了路網(wǎng)匹配的精度，從不同側(cè)面證明了路網(wǎng)匹配的精度及路網(wǎng)匹配與慣性組合導(dǎo)航相結(jié)合的優(yōu)勢(shì)，通過對(duì)比試驗(yàn)與不同試驗(yàn)數(shù)據(jù)的驗(yàn)證試驗(yàn)，證明了慣性/里程計(jì)/路網(wǎng)匹配組合導(dǎo)航的精度與性能。

4 結(jié)論

本文針對(duì)慣性/里程計(jì)組合導(dǎo)航定位精度易受環(huán)境影響的情況，提出了采用路網(wǎng)匹配方法與組合導(dǎo)航相結(jié)合，以提高導(dǎo)航定位精度的方法。首先對(duì)路網(wǎng)數(shù)據(jù)庫進(jìn)行優(yōu)化處理，得到等間隔的路網(wǎng)點(diǎn)集；路網(wǎng)匹配采用組合導(dǎo)航的歷史軌跡數(shù)據(jù)進(jìn)行全局匹配，同時(shí)對(duì)歷史軌跡數(shù)據(jù)進(jìn)行等間隔處理，提高了匹配的精度與可靠度；經(jīng)過匹配修正后，組合導(dǎo)航能夠通過不斷迭代修正定位誤差，從而提高精度。通過試驗(yàn)驗(yàn)證表明，慣性/里程計(jì)/路網(wǎng)匹配組合導(dǎo)航的定位精度有大幅度提升，且可靠性高，是陸地車載自主導(dǎo)航系統(tǒng)的一種有效導(dǎo)航方式。