基于路網匹配的多源自主組合導航技術研究

李 旦，劉 明，胡華峰

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

0 引言

基于慣性導航的多源信息融合組合導航技術已廣泛應用于海、陸、空、天等各領域的組合導航系統，全球導航衛星系統(Global Navigation Satellite System，GNSS)、星敏感器、測距儀、測速儀、里程計、氣壓計、激光雷達、視覺導航、聲學傳感器等輔助導航設備的發展和應用，對提升組合導航性能和精度起到了關鍵性的作用[1-3]。

對于陸地車輛而言，慣導/GNSS/里程計/氣壓計組合導航系統是目前比較成熟的方案，它能夠提供精確的位置、姿態和速度信息，可以提高車輛的機動性和靈活性[4]。在衛星信號受干擾或無衛星信號可用的環境下，陸地車輛特別是陸地戰車的自主導航能力對于提升自身的生成能力和快速反應能力具有至關重要的作用。

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

1 路網匹配組合導航

1.1 路網匹配介紹

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

點到線路網匹配方法將方向、點到道路距離等因素通過各自加權系數進行累加，運用權重的策略將點匹配到道路上。然而這種點到線的匹配由于缺乏對整體軌跡趨勢的考慮，在復雜的路網環境下容易導致誤匹配。現如今大多數針對軌跡數據的地圖匹配方法采用基于曲線相似度的全局匹配方法[13]，曲線匹配與點和線的匹配有差異，需要使用距離準則來定義2條曲線間的相似程度。

近年來，數學學科得到快速發展，涌現出了多種判別曲線相似度的方法，使用較廣泛的是相似性函數判別方法以及特征值判別方法[14]。其中Hausdorff距離主要用于計算點集之間的相似度，Frechet距離用于計算曲線之間的相似度。Frechet方法選取Frechet距離作為路段的權重，構建網絡圖，計算最短路徑，從而獲得最佳的匹配路段，但是Frechet距離是考慮距離集合中的最大值，因此很容易受到異常值的影響。

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

1.2 改進匹配方案

首先，為了實現高精度匹配定位的可靠性，對路網數據庫進行優化處理，即將路網數據處理成10m等間隔的點集合，并在此基礎上進行后續路網匹配。

路網匹配以慣性/里程計組合導航歷史軌跡數據作為輸入，構建導航數據緩沖區，按照匹配準則獲取匹配路段及匹配點。整個匹配過程及步驟如下：

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

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

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

(1)

步驟3：針對一條待選擇道路，將其中與定位點距離小于誤差范圍的點構成一個點集

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

(2)

步驟4：從點集中第一個點開始，根據慣導的歷史軌跡數據(例如數據長度3000m，同樣需要對歷史軌跡數據進行優化處理得到10m等間隔點集)與路網數據進行比較，構造慣導數據與路網數據的方位角差值函數和距離差值函數。方位角計算方法如式(3)所示。

(3)

其中，Δλ表示前后2個點的經度之差，ΔL表示前后2個點的緯度之差。慣導點與路網點的距離計算方法如下

(4)

步驟5：構造匹配準則，方位角差值函數值小于閾值，并且同時距離差值函數值也小于閾值。

步驟6：對路網數據庫中每條道路的點集Pts按照匹配準則進行遍歷，得到滿足匹配準則的匹配點，一般情況下滿足條件的匹配點不止一個，選擇距離差值函數最小的點作為匹配點。如果不存在滿足條件的匹配點，則匹配結果為空。

步驟7：匹配結束，匹配完成后根據路網匹配的結果修正慣導位置誤差，以慣導位置與匹配點位置之差作為卡爾曼濾波器的量測輸入值。

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

2 慣性/里程計/路網匹配組合導航

組合導航濾波器狀態向量選取慣導姿態誤差、速度誤差、位置誤差、陀螺誤差、加速度計誤差共15維向量，即

(5)

根據慣性導航誤差方程得到狀態轉移矩陣。量測值的選取根據慣導位置與路網匹配位置來確定，即以慣導位置與路網匹配位置之差作為卡爾曼濾波器的量測輸入

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

(6)

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

(7)

組合導航采用反饋校正的方式，通過濾波估計出姿態誤差、速度誤差、位置誤差及器件誤差后實時反饋修正慣導的姿態、速度、位置及器件參數，其結構原理如圖 1所示。

圖1 慣性/里程計/路網匹配組合導航結構Fig.1 Flow chart of SINS/odometer/road network matching integrated navigation

3 路網匹配試驗驗證

首先對高精度定位定向系統(Position and Orientation System, POS)采集的道路軌跡數據進行優化處理，得到等間隔的路網數據庫，比較路網中前后點距離間隔如圖 2所示。從圖2中可以看出，路網數據庫中各個點等間隔均勻分布。

圖2 路網數據庫間隔距離Fig.2 Distance of road network points

本文的路網匹配驗證分2個步驟進行，首先僅根據慣性/里程計組合導航的結果進行路網匹配，匹配的結果不參與后續組合導航解算，即開環驗證，主要目的是比較在組合導航基礎上路網匹配的精度；第二步是根據組合導航的結果進行路網匹配，并將匹配結果進行反饋用于修正慣導誤差，通過不斷迭代和閉環修正減少慣導誤差。

3.1 開環驗證

選取一組定位導航試驗數據，首先根據GNSS數據生成路網數據庫，然后進行慣性/里程計組合導航解算，并得到組合結果。根據組合導航結果進行路網匹配，以采集的GNSS位置信息作為參考基準，比較組合導航結果與路網匹配結果，統計組合導航誤差與路網匹配誤差，組合導航軌跡及匹配誤差如圖 3所示。

(a)組合導航軌跡

(b)導航誤差及匹配誤差

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

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

3.2 閉環驗證

閉環驗證采用與開環驗證同樣的試驗數據，匹配開始后根據匹配結果修正慣導誤差及慣性器件誤差，作為下一次慣導計算的初值，通過不斷地迭代修正，得到全程的導航定位誤差，試驗結果如圖 4所示。

(a)組合導航軌跡

(b)導航誤差及匹配誤差

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

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

慣性器件誤差估計結果如圖 5所示。從估計曲線可以看出，經過慣性/里程計/路網匹配組合導航后加表零偏誤差與陀螺零偏誤差估計穩定，間接表明了組合導航的性能。

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

為了進一步驗證路網匹配的精度，本文選取一組長時間的試驗數據，并且人為增大慣性/里程計組合導航的誤差，得到路網匹配修正前后組合導航位置誤差曲線如圖 6所示。

根據路網匹配的試驗結果，路網匹配修正前，慣性/里程計組合導航誤差較大且不斷發散，已無法提供準確的導航信息;經過路網匹配修正后，組合導航誤差明顯減小，且位置誤差保持穩定，不存在發散趨勢，進一步證明了路網匹配與慣性組合導航結合的優勢。

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

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

3.3 試驗小結

本節試驗過程中先后通過開環驗證試驗和閉環驗證試驗驗證了路網匹配的精度，從不同側面證明了路網匹配的精度及路網匹配與慣性組合導航相結合的優勢，通過對比試驗與不同試驗數據的驗證試驗，證明了慣性/里程計/路網匹配組合導航的精度與性能。

4 結論

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