陸用定位定向系統自主導航技術

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

(北京自動化控制設備研究所，北京 100074)

0 引言

近年來，隨著對平臺機動作戰和自主精確打擊能力需求的不斷提高，實現武器無固定陣地的無依托隨機發射，要求陸用定位定向系統在長時間內實時、自主、準確地確定系統的位置、航向及姿態信息，并降低部隊作戰保障要求。

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

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

1 陸用旋轉調制導航

慣導系統旋轉調制是獲得高精度導航的一項重要技術，旋轉機構帶動慣性測量單元(Inertial Measurement Unit，IMU)周期性地在地理坐標系中轉動，自動抵消常值漂移產生的導航誤差[2]。旋轉調制慣導系統相當于在捷聯慣導系統的基礎上增加了旋轉機構和測角裝置，導航解算采用捷聯慣導算法，計算出載體的速度和位置信息；載體姿態信息利用IMU姿態角和測角裝置實時測量IMU相對載體轉動的角度進行求取[3]。

旋轉調制慣導系統慣性導航解算方法、慣性器件的常值漂移沒有改變，誤差方程與捷聯慣導系統一致[4]，表示如下：

(1)

(2)

(3)

(4)

由于慣性器件存在隨機誤差，旋轉調制導航在長時間導航時依然會存在殘余誤差，累積一定的航向誤差。由于旋轉調制慣導系統陀螺漂移可進行調制，根據羅經效應誤差傳遞規律為：

(5)

因此，陸用定位定向系統一般采用繞航向軸周期旋轉的單軸旋轉調制方案，在旋轉調制過程中水平漂移會抵消，利用羅經效應原理通過慣性/里程計自主組合進行航向誤差的在線估計與補償。

2 定向誤差自主抑制

2.1 定向誤差自主抑制機理

慣導系統在導航狀態下，航向誤差在慣性導航中會引起水平測量軸偏離水平軸，引起水平方向隨時間積累的速度誤差，即羅經效應。里程計測量值通過姿態矩陣投影導致的地理系速度誤差與航向誤差和行駛速度成正比，同時慣性導航由于航向誤差的存在也會引起與行駛速度成正比的速度誤差，所以慣性/里程計組合無法通過載體機動激勵航向誤差，只能利用羅經效應原理估計航向角誤差。由于載體機動產生的慣性速度誤差會對分離羅經效應產生不利影響，須推導適合動態情況下采用羅經效應的誤差方程。

慣性/里程計組合導航系統采用速度組合方式，里程計的速度測量值通過姿態矩陣分解到導航坐標系后，作為基準信息觀測慣導系統的速度誤差，獲得卡爾曼濾波的觀測值。利用閉環卡爾曼濾波技術進行誤差估計和校正，特別是航向誤差的校正可逐步提高利用里程計測量值觀測慣性導航速度的精度，從而提高濾波估計和校正的精度，有效減小慣導系統的誤差積累[5]。

2.2 適應羅經效應的誤差方程

2.2.1 坐標系定義

導航坐標系采用當地地理北天東坐標系，載體坐標系(oxbybzb)的oxb指向載體前進方向，oyb垂直載體縱軸向上，ozb由右手規則給出。導航坐標系是分別繞其3個軸依次旋轉載體航向角、俯仰角和滾動角后形成的坐標系。由于實際導航坐標系為計算地理坐標系，與實際地理坐標系之間存在誤差，常用3個失準角表示：北向失準角φN、航向失準角φU和東向失準角φE[6-7]。

2.2.2 慣導誤差方程

慣導/里程計組合導航系統中，通過里程計航位推算獲得的速度和位置精度優于慣性導航解算結果。為了簡化慣導/里程計組合的誤差方程，慣性導航計算過程中均采用航位推算結果計算載體速度引起的導航坐標系的轉動，因此慣導姿態誤差方程為：

(6)

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

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

(7)

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

(8)

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

2.2.3 航位推算誤差方程

(9)

2.3 濾波器設計

通過對慣導/里程計組合導航系統的性能和誤差源的分析，忽略高度方向的影響，建立系統狀態變量：

(10)

系統狀態方程為：

(11)

系統的量測方程為：

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

(12)

2.4 試驗驗證

為了驗證算法的可行性，采用某光纖定位定向系統進行了長航時車載試驗驗證，定位定向系統單軸旋轉尋北精度優于1′、陀螺零偏精度優于0.01(°)/h，車載試驗結束后試驗車保持不動進行再次對準，所得航向作為長航時導航的航向基準。3次車長航時導航試驗結果如表1所示。

表1 長航時導航航向精度統計

從表1中可以看出，導航過程通過單軸旋轉調制慣性/里程計組合，長航時導航航向誤差可以得到有效抑制，與對比結果相比控制在1.5′以內，實現了航向誤差不隨導航時間發散。

3 慣性/航位推算/地圖匹配導航

3.1 地圖匹配原理

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

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

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

3.2 空間索引建立

空間索引是指依據空間對象的位置和形狀或空間對象之間的某種空間關系，按一定的順序排列的一種數據結構。作為一種輔助性的空間數據結構，空間索引介于空間操作和空間對象之間，它通過篩選作用，大量與特定空間操作無關的空間對象被排除，從而提高了空間操作的速度和效率[13]。

對于地圖匹配而言，快速搜索出車輛附近一定范圍內的路段，作為匹配計算的道路信息輸入，直接影響匹配效率。因此，需對數字矢量地圖數據進行預處理，以建立從地圖道路數據庫中快速搜索路段信息的空間索引。具體做法是建立地圖網格索引和建立路段間連通性空間拓撲關系。

3.2.1 建立地圖網格索引

空間填充曲線可以將高維空間中沒有良好順序的數據映射到一維空間，經過這種編碼方式，空間上相鄰的對象會鄰接存儲在一塊，可以減少I/O時間，提高內存中數據處理效率。如圖2所示，將地圖自左而右、自下而上按固定步長劃分成N×N個網格區域，每個網格都給以唯一的編號。完成地圖網格劃及編碼分后，分別判斷各條路段的外接矩形框與各個網格區域的位置關系，并將路段號與網格編碼對應存儲，方便查詢。

圖2 地圖網格劃分及Hilbert編碼Fig.2 Map grid division and Hilbert coding

利用車輛定位點搜索待匹配路段時，首先利用公式求取車輛所在網格的空間坐標，然后根據網格維數N和空間坐標，通過一系列位運算求取定位點所在網格的Hilbert編碼H，進而找到該網格內的路段。

(13)

式中,(X1,Y1)和(X2,Y2)為整個地圖區域邊界的左下角頂點和右上角頂點坐標；(x0,y0)為定位點坐標；(xp,yp)為定位點所在網格的空間坐標；[ ]為向下取整。

3.2.2 建立連通性拓撲關系

對于無拓撲關系的矢量地圖數據，為了快速地確定候選路段，提高確定匹配路段效率，研究路段之間的連通性十分重要。由于車速是有限的，在一定時間范圍內，車輛離開上一匹配路段后，只能行駛在與之相連的路段，而不可能在其他路段上行駛。

分析數字矢量地圖道路數據存儲結構，通過比較各路段起止節點的坐標值，認為具有公共節點的兩路段存在連通關系，如圖3所示。建立路段間的連通性拓撲關系表(見表2)，確立了路段節點和其相連通路段之間的索引關系。

圖3 連通性拓撲關系示意圖Fig.3 Diagram of connectivity topology

連通路段編號連通路段數目路段1的起點51路段1的終點2,3,43路段2的起點1,3,43………………

當車輛離開上一時刻的匹配路段時，利用路段的編號及路段端點，從拓撲關系表中快速找到對應的連通路段作為候選路段進行匹配，進而極大地節省了尋找候選路段的搜索時間，并且可以減少誤匹配現象的出現。

3.3 地圖匹配算法設計

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

3.3.1 候選路段的確定

根據車輛行駛狀態的不同，候選路段的選取準則也不同，可分為下面3個狀態：

1)初始狀態，初始時刻沒有歷史信息可以利用，此時，取和定位點為幾何中心的搜索框相交的路段作為候選路段。

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

3)更新狀態，若車輛離開上一時刻的匹配路段，則查詢拓撲關系表，找到和上一路段相連通的所有路段作為候選路段，匹配結果將是對上一時刻匹配路段的更新。

車輛與路段的位置關系，可利用式(14)判斷，當滿足0<η<1且d<ε(ε為節點圓半徑)時，認為車輛沒有離開該路段[16]。

(14)

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

3.3.2 匹配路段的確定

設某一時刻定位點到候選路段的距離為di，車輛行駛航向與路段方向夾角為Δθi，則定義匹配度fi，有

(15)

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

3.3.3 匹配點的確定

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

(16)

3.4 試驗驗證

采用某型號定位定向系統進行車載試驗，試驗結果如下。其中，車輛以慣性/航位推算組合位置信息作為地圖匹配輸入，數字矢量地圖文件事先裝載到系統導航計算機中。

如圖4、圖5所示，以道路地圖為背景圖，將衛星導航、慣性/航位推算導航以及慣性/航位推算/地圖匹配導航三種導航方式得到的行車軌跡進行比較。慣性/航位推算導航以及慣性/航位推算/地圖匹配導航定位誤差統計結果如圖6所示。其中，將衛星定位結果作為參考基準。

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

(a)

(b)

(c)

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

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

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

4 結論

1)利用旋轉調制導航技術，抵消慣性器件誤差的影響，從而利用羅經效應原理，通過慣性/里程計組合抑制航向誤差隨導航時間的漂移，可有效降低定向精度受導航時間、器件常值誤差的影響。

2)通過建立數字矢量地圖空間索引，并在此基礎上設計地圖匹配算法，以高精度數字矢量地圖為模板，實現了慣性/航位推算/地圖匹配導航，可有效解決定位精度隨行駛里程發散的問題。

3)通過車載試驗驗證了本文方法的可行性和準確性，為長航時大作戰半徑情況下的高精度定位、定向提供了解決途徑。