陸用定位定向系統(tǒng)自主導(dǎo)航技術(shù)

劉 輝, 劉 暢, 李殿茜, 王 琦

(北京自動(dòng)化控制設(shè)備研究所，北京 100074)

0 引言

近年來(lái)，隨著對(duì)平臺(tái)機(jī)動(dòng)作戰(zhàn)和自主精確打擊能力需求的不斷提高，實(shí)現(xiàn)武器無(wú)固定陣地的無(wú)依托隨機(jī)發(fā)射，要求陸用定位定向系統(tǒng)在長(zhǎng)時(shí)間內(nèi)實(shí)時(shí)、自主、準(zhǔn)確地確定系統(tǒng)的位置、航向及姿態(tài)信息，并降低部隊(duì)作戰(zhàn)保障要求。

陸用定位定向系統(tǒng)中的慣性與里程信息、地標(biāo)信息、零速停車信息、衛(wèi)星信息的組合導(dǎo)航技術(shù)已經(jīng)成熟并實(shí)現(xiàn)工程化應(yīng)用，但仍存在一些不足：慣性/里程計(jì)組合導(dǎo)航在大范圍、長(zhǎng)時(shí)間工作時(shí)，其遞推計(jì)算的載體位置會(huì)產(chǎn)生累積的定位誤差(與行駛里程、里程計(jì)刻度系數(shù)誤差和航向誤差成正比)；地標(biāo)修正所需的地標(biāo)點(diǎn)需事先通過(guò)測(cè)繪確定，另外，復(fù)雜的戰(zhàn)區(qū)環(huán)境或工程建設(shè)可能導(dǎo)致地標(biāo)點(diǎn)消失而無(wú)法使用；零速修正需要間斷停車，不利于武器作戰(zhàn)平臺(tái)的快速機(jī)動(dòng)；衛(wèi)星導(dǎo)航易受戰(zhàn)場(chǎng)電子干擾、地形等外部環(huán)境的影響[1]。

本文提出了一種基于旋轉(zhuǎn)調(diào)制的慣性/里程計(jì)/地圖匹配組合導(dǎo)航方案，通過(guò)旋轉(zhuǎn)調(diào)制導(dǎo)航抵消慣性器件誤差的影響，利用航位推算隔離載體加速度和速度對(duì)羅經(jīng)效應(yīng)的影響，使航向誤差完全可觀，提升實(shí)時(shí)估計(jì)與修正精度，最后通過(guò)引入地圖匹配技術(shù)進(jìn)行自主定位，解決了陸用定位精度與行駛里程相關(guān)的問(wèn)題。

1 陸用旋轉(zhuǎn)調(diào)制導(dǎo)航

慣導(dǎo)系統(tǒng)旋轉(zhuǎn)調(diào)制是獲得高精度導(dǎo)航的一項(xiàng)重要技術(shù)，旋轉(zhuǎn)機(jī)構(gòu)帶動(dòng)慣性測(cè)量單元(Inertial Measurement Unit，IMU)周期性地在地理坐標(biāo)系中轉(zhuǎn)動(dòng)，自動(dòng)抵消常值漂移產(chǎn)生的導(dǎo)航誤差[2]。旋轉(zhuǎn)調(diào)制慣導(dǎo)系統(tǒng)相當(dāng)于在捷聯(lián)慣導(dǎo)系統(tǒng)的基礎(chǔ)上增加了旋轉(zhuǎn)機(jī)構(gòu)和測(cè)角裝置，導(dǎo)航解算采用捷聯(lián)慣導(dǎo)算法，計(jì)算出載體的速度和位置信息；載體姿態(tài)信息利用IMU姿態(tài)角和測(cè)角裝置實(shí)時(shí)測(cè)量IMU相對(duì)載體轉(zhuǎn)動(dòng)的角度進(jìn)行求取[3]。

旋轉(zhuǎn)調(diào)制慣導(dǎo)系統(tǒng)慣性導(dǎo)航解算方法、慣性器件的常值漂移沒(méi)有改變，誤差方程與捷聯(lián)慣導(dǎo)系統(tǒng)一致[4]，表示如下：

(1)

(2)

(3)

(4)

由于慣性器件存在隨機(jī)誤差，旋轉(zhuǎn)調(diào)制導(dǎo)航在長(zhǎng)時(shí)間導(dǎo)航時(shí)依然會(huì)存在殘余誤差，累積一定的航向誤差。由于旋轉(zhuǎn)調(diào)制慣導(dǎo)系統(tǒng)陀螺漂移可進(jìn)行調(diào)制，根據(jù)羅經(jīng)效應(yīng)誤差傳遞規(guī)律為：

(5)

因此，陸用定位定向系統(tǒng)一般采用繞航向軸周期旋轉(zhuǎn)的單軸旋轉(zhuǎn)調(diào)制方案，在旋轉(zhuǎn)調(diào)制過(guò)程中水平漂移會(huì)抵消，利用羅經(jīng)效應(yīng)原理通過(guò)慣性/里程計(jì)自主組合進(jìn)行航向誤差的在線估計(jì)與補(bǔ)償。

2 定向誤差自主抑制

2.1 定向誤差自主抑制機(jī)理

慣導(dǎo)系統(tǒng)在導(dǎo)航狀態(tài)下，航向誤差在慣性導(dǎo)航中會(huì)引起水平測(cè)量軸偏離水平軸，引起水平方向隨時(shí)間積累的速度誤差，即羅經(jīng)效應(yīng)。里程計(jì)測(cè)量值通過(guò)姿態(tài)矩陣投影導(dǎo)致的地理系速度誤差與航向誤差和行駛速度成正比，同時(shí)慣性導(dǎo)航由于航向誤差的存在也會(huì)引起與行駛速度成正比的速度誤差，所以慣性/里程計(jì)組合無(wú)法通過(guò)載體機(jī)動(dòng)激勵(lì)航向誤差，只能利用羅經(jīng)效應(yīng)原理估計(jì)航向角誤差。由于載體機(jī)動(dòng)產(chǎn)生的慣性速度誤差會(huì)對(duì)分離羅經(jīng)效應(yīng)產(chǎn)生不利影響，須推導(dǎo)適合動(dòng)態(tài)情況下采用羅經(jīng)效應(yīng)的誤差方程。

慣性/里程計(jì)組合導(dǎo)航系統(tǒng)采用速度組合方式，里程計(jì)的速度測(cè)量值通過(guò)姿態(tài)矩陣分解到導(dǎo)航坐標(biāo)系后，作為基準(zhǔn)信息觀測(cè)慣導(dǎo)系統(tǒng)的速度誤差，獲得卡爾曼濾波的觀測(cè)值。利用閉環(huán)卡爾曼濾波技術(shù)進(jìn)行誤差估計(jì)和校正，特別是航向誤差的校正可逐步提高利用里程計(jì)測(cè)量值觀測(cè)慣性導(dǎo)航速度的精度，從而提高濾波估計(jì)和校正的精度，有效減小慣導(dǎo)系統(tǒng)的誤差積累[5]。

2.2 適應(yīng)羅經(jīng)效應(yīng)的誤差方程

2.2.1 坐標(biāo)系定義

導(dǎo)航坐標(biāo)系采用當(dāng)?shù)氐乩肀碧鞏|坐標(biāo)系，載體坐標(biāo)系(oxbybzb)的oxb指向載體前進(jìn)方向，oyb垂直載體縱軸向上，ozb由右手規(guī)則給出。導(dǎo)航坐標(biāo)系是分別繞其3個(gè)軸依次旋轉(zhuǎn)載體航向角、俯仰角和滾動(dòng)角后形成的坐標(biāo)系。由于實(shí)際導(dǎo)航坐標(biāo)系為計(jì)算地理坐標(biāo)系，與實(shí)際地理坐標(biāo)系之間存在誤差，常用3個(gè)失準(zhǔn)角表示：北向失準(zhǔn)角φN、航向失準(zhǔn)角φU和東向失準(zhǔn)角φE[6-7]。

2.2.2 慣導(dǎo)誤差方程

慣導(dǎo)/里程計(jì)組合導(dǎo)航系統(tǒng)中，通過(guò)里程計(jì)航位推算獲得的速度和位置精度優(yōu)于慣性導(dǎo)航解算結(jié)果。為了簡(jiǎn)化慣導(dǎo)/里程計(jì)組合的誤差方程，慣性導(dǎo)航計(jì)算過(guò)程中均采用航位推算結(jié)果計(jì)算載體速度引起的導(dǎo)航坐標(biāo)系的轉(zhuǎn)動(dòng)，因此慣導(dǎo)姿態(tài)誤差方程為：

(6)

為了獲得可用的慣導(dǎo)速度誤差方程，忽略垂向速度，慣導(dǎo)速度采用以下公式描述：

VN=VX-VZφU+ΔVX
VE=VZ+VXφU+ΔVZ

(7)

式(7)通過(guò)微分和積分變化，可以獲得以下速度誤差方程：

(8)

可以看出,以上水平加速度對(duì)速度誤差的影響可忽略不計(jì)。

2.2.3 航位推算誤差方程

(9)

2.3 濾波器設(shè)計(jì)

通過(guò)對(duì)慣導(dǎo)/里程計(jì)組合導(dǎo)航系統(tǒng)的性能和誤差源的分析，忽略高度方向的影響，建立系統(tǒng)狀態(tài)變量：

(10)

系統(tǒng)狀態(tài)方程為：

(11)

系統(tǒng)的量測(cè)方程為：

Z(t)=H(t)X(t)+V(t)

(12)

2.4 試驗(yàn)驗(yàn)證

為了驗(yàn)證算法的可行性，采用某光纖定位定向系統(tǒng)進(jìn)行了長(zhǎng)航時(shí)車載試驗(yàn)驗(yàn)證，定位定向系統(tǒng)單軸旋轉(zhuǎn)尋北精度優(yōu)于1′、陀螺零偏精度優(yōu)于0.01(°)/h，車載試驗(yàn)結(jié)束后試驗(yàn)車保持不動(dòng)進(jìn)行再次對(duì)準(zhǔn)，所得航向作為長(zhǎng)航時(shí)導(dǎo)航的航向基準(zhǔn)。3次車長(zhǎng)航時(shí)導(dǎo)航試驗(yàn)結(jié)果如表1所示。

表1 長(zhǎng)航時(shí)導(dǎo)航航向精度統(tǒng)計(jì)

從表1中可以看出，導(dǎo)航過(guò)程通過(guò)單軸旋轉(zhuǎn)調(diào)制慣性/里程計(jì)組合，長(zhǎng)航時(shí)導(dǎo)航航向誤差可以得到有效抑制，與對(duì)比結(jié)果相比控制在1.5′以內(nèi)，實(shí)現(xiàn)了航向誤差不隨導(dǎo)航時(shí)間發(fā)散。

3 慣性/航位推算/地圖匹配導(dǎo)航

3.1 地圖匹配原理

地圖匹配是一種基于軟件技術(shù)的定位誤差修正技術(shù)，依靠精確的數(shù)字地圖和完善的地圖匹配算法實(shí)現(xiàn)道路信息與車輛定位信息之間的匹配[11]。將慣導(dǎo)系統(tǒng)獲取的車輛位置或行駛軌跡，與車載數(shù)字地圖的道路數(shù)據(jù)進(jìn)行比較，經(jīng)過(guò)地圖匹配后，找到車輛所在的道路，并且確定車輛在道路上的具體位置。不難看出，整個(gè)地圖匹配可分為2個(gè)相對(duì)獨(dú)立的過(guò)程：1)尋找車輛當(dāng)前行駛的道路；2)確定車輛在當(dāng)前道路上的具體位置[12]。

慣性/里程計(jì)組合導(dǎo)航雖然可以提高導(dǎo)航精度，但在大范圍、長(zhǎng)時(shí)間工作時(shí)，其遞推計(jì)算的載體位置會(huì)產(chǎn)生累積的定位誤差。而地圖匹配技術(shù)需要其他導(dǎo)航定位方式為其提供待匹配的定位點(diǎn)，才可以結(jié)合高精度數(shù)字地圖，進(jìn)而得到較高精度的位置信息。因此，利用地圖匹配導(dǎo)航技術(shù)能夠與慣性導(dǎo)航形成良好的互補(bǔ)，加上里程計(jì)輔助，從而形成慣性/航位推算/地圖匹配導(dǎo)航，如圖1所示。

圖1 慣性/航位推算/地圖匹配導(dǎo)航原理圖Fig.1 Principle diagram of INS/DR/Map-Matching navigation

3.2 空間索引建立

空間索引是指依據(jù)空間對(duì)象的位置和形狀或空間對(duì)象之間的某種空間關(guān)系，按一定的順序排列的一種數(shù)據(jù)結(jié)構(gòu)。作為一種輔助性的空間數(shù)據(jù)結(jié)構(gòu)，空間索引介于空間操作和空間對(duì)象之間，它通過(guò)篩選作用，大量與特定空間操作無(wú)關(guān)的空間對(duì)象被排除，從而提高了空間操作的速度和效率[13]。

對(duì)于地圖匹配而言，快速搜索出車輛附近一定范圍內(nèi)的路段，作為匹配計(jì)算的道路信息輸入，直接影響匹配效率。因此，需對(duì)數(shù)字矢量地圖數(shù)據(jù)進(jìn)行預(yù)處理，以建立從地圖道路數(shù)據(jù)庫(kù)中快速搜索路段信息的空間索引。具體做法是建立地圖網(wǎng)格索引和建立路段間連通性空間拓?fù)潢P(guān)系。

3.2.1 建立地圖網(wǎng)格索引

空間填充曲線可以將高維空間中沒(méi)有良好順序的數(shù)據(jù)映射到一維空間，經(jīng)過(guò)這種編碼方式，空間上相鄰的對(duì)象會(huì)鄰接存儲(chǔ)在一塊，可以減少I(mǎi)/O時(shí)間，提高內(nèi)存中數(shù)據(jù)處理效率。如圖2所示，將地圖自左而右、自下而上按固定步長(zhǎng)劃分成N×N個(gè)網(wǎng)格區(qū)域，每個(gè)網(wǎng)格都給以唯一的編號(hào)。完成地圖網(wǎng)格劃及編碼分后，分別判斷各條路段的外接矩形框與各個(gè)網(wǎng)格區(qū)域的位置關(guān)系，并將路段號(hào)與網(wǎng)格編碼對(duì)應(yīng)存儲(chǔ)，方便查詢。

圖2 地圖網(wǎng)格劃分及Hilbert編碼Fig.2 Map grid division and Hilbert coding

利用車輛定位點(diǎn)搜索待匹配路段時(shí)，首先利用公式求取車輛所在網(wǎng)格的空間坐標(biāo)，然后根據(jù)網(wǎng)格維數(shù)N和空間坐標(biāo)，通過(guò)一系列位運(yùn)算求取定位點(diǎn)所在網(wǎng)格的Hilbert編碼H，進(jìn)而找到該網(wǎng)格內(nèi)的路段。

(13)

式中,(X1,Y1)和(X2,Y2)為整個(gè)地圖區(qū)域邊界的左下角頂點(diǎn)和右上角頂點(diǎn)坐標(biāo)；(x0,y0)為定位點(diǎn)坐標(biāo)；(xp,yp)為定位點(diǎn)所在網(wǎng)格的空間坐標(biāo)；[ ]為向下取整。

3.2.2 建立連通性拓?fù)潢P(guān)系

對(duì)于無(wú)拓?fù)潢P(guān)系的矢量地圖數(shù)據(jù)，為了快速地確定候選路段，提高確定匹配路段效率，研究路段之間的連通性十分重要。由于車速是有限的，在一定時(shí)間范圍內(nèi)，車輛離開(kāi)上一匹配路段后，只能行駛在與之相連的路段，而不可能在其他路段上行駛。

分析數(shù)字矢量地圖道路數(shù)據(jù)存儲(chǔ)結(jié)構(gòu)，通過(guò)比較各路段起止節(jié)點(diǎn)的坐標(biāo)值，認(rèn)為具有公共節(jié)點(diǎn)的兩路段存在連通關(guān)系，如圖3所示。建立路段間的連通性拓?fù)潢P(guān)系表(見(jiàn)表2)，確立了路段節(jié)點(diǎn)和其相連通路段之間的索引關(guān)系。

圖3 連通性拓?fù)潢P(guān)系示意圖Fig.3 Diagram of connectivity topology

連通路段編號(hào)連通路段數(shù)目路段1的起點(diǎn)51路段1的終點(diǎn)2,3,43路段2的起點(diǎn)1,3,43………………

當(dāng)車輛離開(kāi)上一時(shí)刻的匹配路段時(shí)，利用路段的編號(hào)及路段端點(diǎn)，從拓?fù)潢P(guān)系表中快速找到對(duì)應(yīng)的連通路段作為候選路段進(jìn)行匹配，進(jìn)而極大地節(jié)省了尋找候選路段的搜索時(shí)間，并且可以減少誤匹配現(xiàn)象的出現(xiàn)。

3.3 地圖匹配算法設(shè)計(jì)

地圖匹配算法可以看作是一個(gè)模式識(shí)別的過(guò)程。一個(gè)完整的地圖匹配算法一般包括3個(gè)大的步驟：1)確定誤差區(qū)域，找出車輛附近的所有待匹配的候選路段；2)從所有候選路段中確定車輛當(dāng)前所在路段，即匹配路段；3)確定車輛在當(dāng)前路段上的具體位置，即匹配點(diǎn)[14]。

3.3.1 候選路段的確定

根據(jù)車輛行駛狀態(tài)的不同，候選路段的選取準(zhǔn)則也不同，可分為下面3個(gè)狀態(tài)：

1)初始狀態(tài)，初始時(shí)刻沒(méi)有歷史信息可以利用，此時(shí)，取和定位點(diǎn)為幾何中心的搜索框相交的路段作為候選路段。

2)跟蹤狀態(tài)，若此刻車輛沒(méi)有離開(kāi)上一時(shí)刻確定的匹配路段，則該路段即為此刻車輛的候選路段也就是匹配路段。相較于上一時(shí)刻，匹配路段沒(méi)有變化。

3)更新?tīng)顟B(tài)，若車輛離開(kāi)上一時(shí)刻的匹配路段，則查詢拓?fù)潢P(guān)系表，找到和上一路段相連通的所有路段作為候選路段，匹配結(jié)果將是對(duì)上一時(shí)刻匹配路段的更新。

車輛與路段的位置關(guān)系，可利用式(14)判斷，當(dāng)滿足0<η<1且d<ε(ε為節(jié)點(diǎn)圓半徑)時(shí)，認(rèn)為車輛沒(méi)有離開(kāi)該路段[16]。

(14)

式中,(xo,yo)為慣導(dǎo)輸出車輛的定位坐標(biāo)； (x1,y1)和(x2,y2)為某一路段的起始節(jié)點(diǎn)和終止節(jié)點(diǎn)的坐標(biāo)。

3.3.2 匹配路段的確定

設(shè)某一時(shí)刻定位點(diǎn)到候選路段的距離為di，車輛行駛航向與路段方向夾角為Δθi，則定義匹配度f(wàn)i，有

(15)

其中，wd、wθ分別表示距離因素和方向因素在匹配度中的權(quán)重系數(shù)，滿足wd+wθ=1。由式(15)可知，定位點(diǎn)到路段的距離越小，車輛行駛航向和路段方向夾角越小，則匹配度越大，說(shuō)明該路段是當(dāng)前車輛所在道路的可能性就越大[15]。因此，在匹配過(guò)程中，求出定位點(diǎn)對(duì)于各個(gè)候選路段的匹配度，取匹配度最大的候選路段為車輛的匹配路段。

3.3.3 匹配點(diǎn)的確定

匹配路段確定后，將車輛定位點(diǎn)向匹配路段上做投影，投影點(diǎn)作為車輛在路段上的匹配點(diǎn)。匹配點(diǎn)(xp,yp)求取如下：

(16)

3.4 試驗(yàn)驗(yàn)證

采用某型號(hào)定位定向系統(tǒng)進(jìn)行車載試驗(yàn)，試驗(yàn)結(jié)果如下。其中，車輛以慣性/航位推算組合位置信息作為地圖匹配輸入，數(shù)字矢量地圖文件事先裝載到系統(tǒng)導(dǎo)航計(jì)算機(jī)中。

如圖4、圖5所示，以道路地圖為背景圖，將衛(wèi)星導(dǎo)航、慣性/航位推算導(dǎo)航以及慣性/航位推算/地圖匹配導(dǎo)航三種導(dǎo)航方式得到的行車軌跡進(jìn)行比較。慣性/航位推算導(dǎo)航以及慣性/航位推算/地圖匹配導(dǎo)航定位誤差統(tǒng)計(jì)結(jié)果如圖6所示。其中，將衛(wèi)星定位結(jié)果作為參考基準(zhǔn)。

圖4 行車路線的匹配結(jié)果整體圖Fig.4 The integral map of route matching result

(a)

(b)

(c)

(d)圖5 匹配結(jié)果局部圖Fig.5 The partial maps of route matching result

通過(guò)圖4、圖5可以看出，本文設(shè)計(jì)的地圖算法除了在車輛正常行駛時(shí)能夠匹配到正確道路上，并且在單、雙路段變換(圖5(a))，轉(zhuǎn)彎(圖5(b)、 (c))，平行雙向路段(圖5(d))等特殊路段具有較好的匹配效果，說(shuō)明該算法具有較高的道路識(shí)別精度和容錯(cuò)能力。

從圖6中可以看出，利用地圖匹配可將經(jīng)度誤差從49m提高到20m，緯度誤差從65m提高到15m。證明慣性/航位推算/地圖匹配導(dǎo)航方式對(duì)慣性定位定向系統(tǒng)位置誤差具有較好的抑制作用。

4 結(jié)論

1)利用旋轉(zhuǎn)調(diào)制導(dǎo)航技術(shù)，抵消慣性器件誤差的影響，從而利用羅經(jīng)效應(yīng)原理，通過(guò)慣性/里程計(jì)組合抑制航向誤差隨導(dǎo)航時(shí)間的漂移，可有效降低定向精度受導(dǎo)航時(shí)間、器件常值誤差的影響。

2)通過(guò)建立數(shù)字矢量地圖空間索引，并在此基礎(chǔ)上設(shè)計(jì)地圖匹配算法，以高精度數(shù)字矢量地圖為模板，實(shí)現(xiàn)了慣性/航位推算/地圖匹配導(dǎo)航，可有效解決定位精度隨行駛里程發(fā)散的問(wèn)題。

3)通過(guò)車載試驗(yàn)驗(yàn)證了本文方法的可行性和準(zhǔn)確性，為長(zhǎng)航時(shí)大作戰(zhàn)半徑情況下的高精度定位、定向提供了解決途徑。