李 欣，趙海波，馬士國

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

0 引言

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

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

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

1 地圖匹配算法

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

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

1.2 匹配算法分析與改進(jìn)

文獻(xiàn)［3］中的實(shí)驗(yàn)結(jié)果給出了匹配的極限精度（所謂極限精度是指同次實(shí)驗(yàn)處理得到的地圖數(shù)據(jù)與定位數(shù)據(jù)間的匹配精度），與實(shí)際應(yīng)用仍有差距。使用相同路段不同時(shí)間數(shù)據(jù)進(jìn)行實(shí)驗(yàn)，可得如圖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)和平移三個(gè)維度上的變換。因此，為了更好地匹配精度，這里設(shè)尺度變換陣為

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

則相似變換后DR定位點(diǎn)為

式（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］進(jìn)行迭代求解，該方法不用求Jacobi矩陣，也不用對矩陣求逆。

1.3 實(shí)驗(yàn)驗(yàn)證

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

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

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

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

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

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

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

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

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

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

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

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

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

綜上，有誤差估計(jì)公式

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

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

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

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

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

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

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

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

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

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

圖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ù)估計(jì)值Table 2 Estimation for parameters

3 結(jié)論

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