基于地圖匹配的車載慣導(dǎo)行進間精對準(zhǔn)算法

李 欣，趙海波，馬士國

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

0 引言

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

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

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

1 地圖匹配算法

1.1 基于移動相關(guān)的最小二乘地圖匹配算法

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

1.2 匹配算法分析與改進

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

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

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

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

則相似變換后DR定位點為

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

目標(biāo)函數(shù)為

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

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

1.3 實驗驗證

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

圖2 匹配結(jié)果Fig.2 Results of matching

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

2 慣導(dǎo)行進間精對準(zhǔn)算法

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

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

圖4 單個路標(biāo)點對準(zhǔn)示意圖Fig.4 Diagram of alignment based on single landmark point

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

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

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

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

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

由式（12）可知，以N個地圖匹配位置點P′作為路標(biāo)點進行航向誤差角估計的平均值與以N個無誤差真實位置點Pi作為路標(biāo)點進行航向誤差角估計的平均值是相當(dāng)?shù)模@就奠定了以地圖匹配結(jié)果輔助航向精對準(zhǔn)的理論基礎(chǔ)。

綜上，有誤差估計公式

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

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

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

1）在出發(fā)點，快速解析式粗對準(zhǔn)；

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

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

4）修正初始時刻的姿態(tài)陣、里程儀刻度系數(shù)；

5）重新解算，再次估計和修正，對準(zhǔn)結(jié)束。

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

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

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

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

表1 仿真參數(shù)設(shè)置Table 1 Setting of simulation parameters

圖8 四組航向精對準(zhǔn)結(jié)果對比曲線Fig.8 Comparison of four cases for fine alignment

表2 參數(shù)估計值Table 2 Estimation for parameters

3 結(jié)論

本文提出了一種改進的地圖匹配方法，通過兩水平方向分別使用不同尺度因子的慣導(dǎo)和地圖匹配算法能夠改善系統(tǒng)的定位精度，進而提出了利用地圖匹配定位結(jié)果實現(xiàn)的行進間迭代精對準(zhǔn)算法。該算法能夠獲得與預(yù)置路標(biāo)點行進間精對準(zhǔn)方式精度相當(dāng)?shù)膶?zhǔn)結(jié)果，從而在不降低精度的前提下擺脫對預(yù)置路標(biāo)點的依賴，有效改善了系統(tǒng)的機動性。