基于地圖匹配的車載慣導行進間精對準算法

李 欣，趙海波，馬士國

（1.海軍研究院空中所， 上海 200436；2.中國人民解放軍 91445 部隊， 大連 116043）

0 引言

自主導航性和高機動性是保證車載武器系統生存和作戰能力的重要因素。自主導航性是指載車具備快速自主定位定向能力，甚至是行進中定位定向的能力，為車載武器提供參考基準，以期實現 “停車就打”，甚至 “邊走邊打”［1］。為此，車載武器系統往往以高精度慣導為核心構造組合導航系統。高機動性是指載車必須能夠快速進入陣地和撤離陣地，而自主導航性又是高機動性的前提，為了提高機動性，導航系統必須具有快速進入工作狀態的能力。

為縮減慣導系統初始化所需時間，文獻［2］提出了一種基于預置路標點的車載導航系統行進間對準算法。裝載該算法的導航系統啟動后，只需要進行簡單的粗對準后即允許車輛進入行駛狀態。車輛路過路標點時認為輸入觸發信息號給導航系統即可完成系統的精對準，且對準精度高于靜基座自對準精度。該算法［2］大幅度降低了導航系統的準備時間，但增加了對預置路標點的依賴。目前，數字地圖、捷聯慣導和里程儀的組合導航逐漸成為軍用車輛自主導航發展的重要方向［3］。但是，對于慣導系統，進入工作狀態前必須進行初始對準，而精確性和快速性是慣導系統自對準的兩項重要指標［3］，所以研究車輛的行進間精度準具有重要意義［4-9］。

本文提出了采用慣導和數字地圖匹配技術獲取精確的位置信息代替預置路標信息進行行進間精對準的方法。首先研究了基于捷聯慣導的地圖匹配算法，對文獻［3］中提出的基于移動相關的最小二乘地圖匹配算法進行了改進，然后研究利用地圖匹配結果進行行進間精對準的算法，并通過仿真驗證了對準算法的有效性。

1 地圖匹配算法

1.1 基于移動相關的最小二乘地圖匹配算法

文獻［3］針對傳統地圖匹配算法存在的計算量大、精度不夠、易出現誤匹配等，以及共同存在的無法消除定位點沿道路方向縱向誤差的問題，依據車輛的航位推算軌跡與行駛的真實軌跡相似性原理，同時考慮到地圖的數據格式和形式以及編程的可實現、計算的快速性，綜合設計了一個充分利用路網信息和行車歷史數據的基于慣導的地圖匹配算法。該算法是一個部分與整體相結合的匹配算法，分4步進行地圖匹配，即：1）預處理；2）路段搜索；3）曲線投影；4）定位修正。文獻［3］中曲線投影部分的核心思想是：根據相似性原理和最小二乘原理，擬合出SINS軌跡點（或航位推算點）與對應數字地圖點的變換參數平移變換向量T、尺度因子K、旋轉變換角α，并依此進行匹配投影。

1.2 匹配算法分析與改進

文獻［3］中的實驗結果給出了匹配的極限精度（所謂極限精度是指同次實驗處理得到的地圖數據與定位數據間的匹配精度），與實際應用仍有差距。使用相同路段不同時間數據進行實驗，可得如圖1所示誤差曲線。由圖1可知，X方向與Y方向的匹配誤差差異較大，X方向匹配后定位誤差在0.8m～5.1m，而Y方向匹配后定位誤差在0附近波動。因此，考慮對X方向和Y方向分別使用不同尺度因子。

圖1 原始方法匹配誤差Fig.1 Matching error of original method

根據相似性原理，SINS軌跡與地圖軌跡之間存在拉伸、旋轉和平移三個維度上的變換。因此，為了更好地匹配精度，這里設尺度變換陣為

式（1）中，k1和k2分別為X方向與Y方向的變換拉伸尺度因子。

則相似變換后DR定位點為

式（2）中，α為軌跡的旋轉角度，x0和y0分別為X方向與Y方向的平移參數。

目標函數為

根據最小二乘原理擬合出拉伸、旋轉和平移參數，使得f的值最小。所以，分別求f對k1、k2、α、x0和y0的偏導數，并令其等于0，即

上述方程不好求得解析解，故可求數值解。對變換參數方程組采用擬Newton法［10］進行迭代求解，該方法不用求Jacobi矩陣，也不用對矩陣求逆。

1.3 實驗驗證

為了驗證該改進匹配算法的有效性，本文進行了跑車實驗，實驗條件與文獻［3］中基本一致。跑車路線為某環山公路，選用的某型激光SINS陀螺漂移為0.01（°）/h，加速度計零偏為5×10-5g（g為重力加速度），輸出頻率為50Hz，數字地圖精度在米級［11］。使用定位誤差優于0.1m的DGPS（差分GPS）作為定位參考標準，這里取3000個SINS采樣點進行處理演示，匹配結果如圖2所示，匹配誤差如圖3所示。由圖2、圖3可知，X方向匹配誤差在±3m以內，而Y方向匹配誤差依然在0附近波動。經過統計，X方向匹配誤差均值從原方法的約3.0m減小到目前的-0.1m，兩種方法的匹配誤差方差大致相當。因此，根據統計結果可知，改進后的匹配算法比改進前的匹配精度更高。

圖2 匹配結果Fig.2 Results of matching

圖3 匹配誤差分析Fig.3 Analysis of matching error

2 慣導行進間精對準算法

對于慣導系統而言，進入工作狀態前必須進行初始對準。精確性和快速性是慣導系統自對準的兩項重要指標，但是它們之間常常是矛盾的［12］，所以研究車輛粗對準后的行進間精對準具有重要意義。

文獻［13］中論述了利用單個路標點進行航向精對準的方法，其原理示意圖如圖4所示。在行車時間較短、陀螺漂移較小和安裝誤差角通過標定精確補償的情況下，根據起始點和路標點間的矢量水平投影ΔSh和航位推算矢量水平投影Δh，給出了航向誤差角和里程儀刻度系數誤差估計公式

圖4 單個路標點對準示意圖Fig.4 Diagram of alignment based on single landmark point

跑車實驗結果表明，該方法精對準后除去陀螺漂移影響航向誤差角約為3′。但是，該方法存在著顯著問題：首先必須事先測定好路標點，然后跑車時必須經過該路標點，這對戰時的車載武器平臺來說很有可能是做不到的。由上一節的實驗結果可知，經過地圖匹配后能獲得若干圓誤差在3m以內的定位點，這些定位點就是很好的路標點群，所以這里將地圖匹配結果引入輔助精對準。

如圖5所示，Pi為無誤差真實位置點，i為航位推算位置點，P′i或者Pi為對應的地圖匹配位置點，具體是P′i還是Pi是由地圖匹配結果決定的。

圖5 真實位置與誤差位置關系Fig.5 Relationship between error-free positions and errored positions

以P′i為標志點估計出的航向誤差角δψ′比以Pi為路標點估計出的δψ大，以Pi為標志點估計出的航向誤差角δψ″比δψ小。為了表述方便，這里統一以P′i來表示Pi對應的地圖匹配位置點。因此，對于N個地圖匹配位置點P′來說，以P′i估計出的航向誤差角為

式（10）中，εi基本滿足在［-c，c］的對稱均勻分布，c為與匹配結果直接相關的常數，所以有

由式（12）可知，以N個地圖匹配位置點P′作為路標點進行航向誤差角估計的平均值與以N個無誤差真實位置點Pi作為路標點進行航向誤差角估計的平均值是相當的，這就奠定了以地圖匹配結果輔助航向精對準的理論基礎。

綜上，有誤差估計公式

通過式（13）、式（14）估計出的慣導航向角誤差δψ與里程儀刻度系數誤差δKD都是近似的，補償后還會有殘差。因此，考慮迭代估計的方法以提高精度。

首先，以式（13）、式（14）進行δψ與δKD的初步估計，并以此修正初始時刻的航向角ψ與里程儀刻度系數KD。然后，以保存的N個位置點處的角速度、比力和速度信息重新進行捷聯解算與航位推算，以航位推算位置結果再次估計δψ與δKD，并修正ψ與KD。這樣，就完成了精對準過程。

利用地圖匹配的輔助對準方案的整個流程如圖6所示，具體步驟如下：

1）在出發點，快速解析式粗對準；

2）行車6min ～10min，進行地圖匹配，獲得精確位置信息；

3）估計航向誤差角與里程儀刻度系數誤差；

4）修正初始時刻的姿態陣、里程儀刻度系數；

5）重新解算，再次估計和修正，對準結束。

為了驗證精對準算法，本文進行了數值仿真。主要仿真誤差參數設置如表1所示，載車運動軌跡如圖7所示。仿真時間為600s，誤差估計、修正時間約在第500s。仿真時，假設車輛出發前已經完成解析式粗對準。

仿真實驗分成四個對照組：1）單個路標點一次估計，路標點位置精確，零誤差；2）單個路標點一次估計，位置誤差3m；3）路標點群（即地圖匹配輔助）一次估計，位置誤差服從（-3m，3m）的均勻分布；4）路標點群迭代估計，位置誤差同上。仿真結果如圖8所示，參數估計如表2所示。由估計結果可知，地圖匹配輔助單次估計精度與單個精確路標點估計精度基本相當，路標點群迭代估計精度最高。

圖6 地圖匹配精對準流程框圖Fig.6 Flowchart of map-matching fine alignment

圖7 行車路線圖Fig.7 Routine for the vehicle

表1 仿真參數設置Table 1 Setting of simulation parameters

圖8 四組航向精對準結果對比曲線Fig.8 Comparison of four cases for fine alignment

表2 參數估計值Table 2 Estimation for parameters

3 結論

本文提出了一種改進的地圖匹配方法，通過兩水平方向分別使用不同尺度因子的慣導和地圖匹配算法能夠改善系統的定位精度，進而提出了利用地圖匹配定位結果實現的行進間迭代精對準算法。該算法能夠獲得與預置路標點行進間精對準方式精度相當的對準結果，從而在不降低精度的前提下擺脫對預置路標點的依賴，有效改善了系統的機動性。