一種結合地形和環境特征的水下導航定位方法

徐曉蘇，湯郡郡，張 濤，岳增陽

（1. 微慣性儀表與先進導航技術教育部重點實驗室，南京210096；2. 東南大學 儀器科學與工程學院，南京 210096）

一種結合地形和環境特征的水下導航定位方法

徐曉蘇1,2，湯郡郡1,2，張 濤1,2，岳增陽1,2

（1. 微慣性儀表與先進導航技術教育部重點實驗室，南京210096；2. 東南大學 儀器科學與工程學院，南京 210096）

為了實現水下潛器長時間高精度導航定位，同時考慮到傳統地形輔助導航系統在先驗地形圖不可得或者是地形變化不明顯的海域（地形不可匹配區域），無法用來修正慣性導航位置誤差的問題，提出了一種結合地形和環境特征的水下導航定位方法。在先驗地形圖可得且地形高程變化明顯的可匹配區域，采用地形輔助導航系統來修正慣導位置誤差，在先驗地形圖不可得或者是地形高程變化不明顯的不可匹配區域，采用基于海洋環境特征的同步定位與構圖算法來修正慣導位置誤差。仿真結果表明，該方法在地形可匹配區域以及地形不可匹配區域得到的航跡都比純慣導得到的軌跡更接近于理想航跡，因此可以用來修正慣導位置誤差。

水下航行器；地形輔助導航系統；同步定位與構圖；慣性導航系統

慣性導航系統不需要任何外來信息，也不向外輻射任何信息，僅依靠自身就能在全天候條件下，在全球范圍內和任何介質環境中進行連續的導航和定位，這種同時具備自主性、隱蔽性和能獲取載體完備運動信息的獨特優點使其成為最受廣泛使用的水下導航定位方法。但是，慣導系統也有著系統誤差隨時間積累的原理性缺陷，為了實現長航時高精度的導航目標，慣性導航系統需要利用外界位置信息對其進行周期的調整和校正。

地形輔助導航（Terrain-Aided Navigation，TAN）是一種利用地形高程特征來進行輔助定位的方法，它具有自主、隱蔽、連續、全天候工作、導航定位誤差不隨時間積累等優點，是水下潛器理想的輔助導航定位手段。然而，地形輔助導航需要有先驗的地形圖，且要求地形高程有明顯的變化。對于一些尚未測繪或者是地形變化過于平緩的海域，傳統的地形輔助導航方法是無法進行的。

基于環境特征的同步定位與構圖（Simultaneous Localization and Mapping，SLAM）概念最早于1986年舉辦的 IEEE機器人與自動化會議上被提出，其基本思路是，將機器人放在一個未知環境的未知位置下，機器人依靠自身攜帶的傳感器感知周圍環境，增量式構建一副與環境相一致的地圖，并同時根據該地圖確定自身所在位置。在先驗地形圖不可得或者是地形變化不明顯的不可匹配區域，如果存在一些明顯的靜止物體，如珊瑚礁、水下建筑、失事船等，則可以將這些顯而易見的物體提取為二維點特征，將這些二維點特征構成一幅可描述當前水下環境的“特征地圖”，同步定位與構圖算法可以在構造這種特征地圖的同時實現對水下潛器的輔助定位。

考慮到同步定位與構圖算法的這種優勢，針對地形輔助導航方法在地形不可匹配區域無法修正慣導隨時間積累的誤差問題，提出了一種結合地形和環境特征的水下導航定位方法，在地形可匹配區域，采用地形輔助慣性導航方法來修正慣導位置誤差，在地形不可匹配區域，采用基于海洋環境特征的同步定位與構圖算法來代替地形輔助導航方法修正慣導系統的位置誤差。仿真結果表明，該方法在地形可匹配區域和不可匹配區域都可以實現高精度的導航定位。

1 結合地形和環境特征的水下導航定位方法原理

SLAM整個過程中沒有一個確知的絕對位置，所以它實際上是一種相對定位，然而對于水下潛器這樣的載體來說，得到相對位置并不是目的。當潛器在地形可匹配區域航行時，采用地形輔助導航修正慣導位置誤差的同時，對環境進行構圖，此時潛器的絕對位置是經過修正的，通過轉換得到的特征的絕對位置精度也相對較高。當潛器從地形可匹配區域進入地形不可匹配區域時，可以利用地形輔助導航得到的最后一個絕對位置，通過SLAM的相對定位以及對路標的實時觀測實現潛器和路標絕對位置的傳遞。當潛器再次進入地形可匹配區域時，由于慣性導航系統的位置誤差已經得到了及時的修正，潛器初始位置誤差很小，地形匹配算法可以在很小的范圍內尋找匹配點，這不僅提高了算法的效率，也同時提高了導航定位的精度。

2 地形可匹配區域

在先驗地形圖可得且地形變化明顯的可匹配區域，采用基于ICCP算法的地形輔助導航系統來修正慣性導航系統的位置誤差。

2.1 基于ICCP算法的地形匹配流程

① 由慣性導航系統提供航跡測量的N個位置值pi（其中i=1,2,…,N）；實時測深傳感器提供慣導指示航跡序列點pi相應的水深值hi（其中i=1,2,…,N），并且從已知參考地圖中抽取出相應的等值線Ci（其中i=1,2,…,N）。

③ 假設pi在相應等值線上的最近點為yi，尋找隨機旋轉和平移變換T，使集合之間的距離最小，也即使下面的目標函數最小

⑤ 判斷是否滿足最終的迭代終止條件，如果滿足便退出。此時得到的匹配點即為最佳匹配點，可用來修正慣導位置誤差。

2.2 SINS/TAN系統模型的構建

2.2.1 狀態方程

SINS/TAN系統的狀態方程就是SINS系統的狀態方程，采用間接法估計 S INS系統狀態的誤差。選取東北天坐標系為導航坐標系，載體坐標系x軸沿潛器橫軸指向右舷，y軸沿潛器縱軸指向前，z軸垂直于x軸和y軸所確定的平面構成右手坐標系。選擇位置誤差、速度誤差、姿態誤差、陀螺漂移和加速度計偏置作為狀態量，有：

式中，

2.2.2 觀測方程

系統量測量由SINS輸出的位置信息LSINS、λSINS減去ICCP算法得到的位置信息LTAN、λTAN形成。

式中，Vk為觀測噪聲, Hk如下：

2.2.3 誤差修正

將慣性導航系統輸出的位置信息與ICCP匹配得到的位置信息的差值作為觀測量，進行卡爾曼濾波，并將濾波得到的位置誤差量反饋到主慣性導航系統就可以實現對主慣性導航系統位置信息的校正。

3 地形不可匹配區域

在先驗地形圖不可得或者是地形變化不明顯的不可匹配區域，采用基于擴展卡爾曼濾波的同步定位與構圖算法來修正慣性導航系統的位置誤差。

3.1 系統狀態向量和協方差矩陣

水下潛器的狀態用它在地理坐標系下緯度方向的位置分量xv、經度方向的位置分量yv以及航向φv表示，其估計均值和方差分別定義為

設潛器觀測到的特征均為靜止的2維點特征，并用這些點特征在同一個坐標系下的位置來形成地圖。

該向量維數隨著新特征的加入而擴充，其協方差為

系統狀態向量為

系統狀態協方差為

式中，Pvv表示潛器位姿估計的不確定性，Pvm表示潛器位姿與環境特征估計之間的相關性，Pmm中對角線元素表示每個環境特征位置估計的不確定性，非對角線元素表示某一特征與其他特征估計間的相關性。

3.2 水下航行器EKF-SLAM算法的具體執行過程

EKF-SLAM算法的執行過程大體可以分為三個階段：預測、觀測和更新/擴充。首先將潛器姿態和地圖特征存儲在一個獨立的狀態向量中，然后通過先觀測再觀測的迭代地推過程來估計系統狀態，實現對潛器的定位和特征地圖的創建。

3.2.1 預測階段

圖1 潛器姿態變換示意圖Fig.1 Attitude transformation of vehicle

3.2.2 觀測階段

設當前時刻聲納測量得到一個特征，經過數據關聯，它與存儲在地圖中的估計值為的特征關聯成功，觀測值和觀測協方差分別為

式中，r表示載體坐標系下特征相對于潛器的距離，θ表示載體坐標系下特征相對于潛器的方位。

如果在一個掃描周期內聲納觀測到了m( m＞1)個特征，則觀測向量和觀測方差可以寫成：

而：

對于特征數量很多的海底環境而言，SLAM算法中的雅可比矩陣中大部分項都是 0，這使得新息協方差矩陣以及濾波增益的計算成為可能。

3.2.3 更新/擴充階段

對于關聯正確的特征，可以對潛器狀態和地圖進行更新，更新如下：

隨著聲納對海底環境的探測，必然會掃描到一些新的特征，當某個特征第一次被觀測時，必須通過一個初始化過程將特征加入到地圖中去，使地圖可以隨著潛器探索區域的增大而不斷擴展。對新特征的初始化方法如下：

首先，將新觀測值z和觀測協方差R擴充到系統狀態向量和狀態協方差中，測量是在載體坐標系下獲得的。擴充后的系統狀態和狀態協方差如下所示：

新的觀測值z與潛器當前姿態的關系如圖2所示。

圖2 新的特征觀測與航行器關系圖Fig.2 Relationship between new feature and vehicle

由于雅可比矩陣大部分項為 0，變換僅影響到協方差矩陣對角線上的矩陣塊和新特征所在的行和列上與地圖其它狀態的互相關項，因此擴充后的狀態協方差矩陣可以如下表示：

4 仿真實驗

為了驗證算法的有效性，本文選取了2塊網格（網格間距為0.00125°，也即139 m）大小一致的地形進行了算法仿真。地形 1起點為（東經 38°，北緯120.0375°），如圖 3(a)所示；地形 2起點為（東經38.075°，北緯120°），如圖3(b)所示。

設定慣導初始位置誤差為：經度方向1個網格，緯度方向2個網格，陀螺漂移0.02 (°)/h，加速度計偏置為0.0005 m/s2。測深傳感器采樣時間為60 s，在兩塊地形上分別采樣10個點后進行ICCP算法匹配，匹配結果如圖4(a)、圖4(b)所示。

圖3(a) 地形1Fig.3(a) Terrain 1

圖3(b) 地形2Fig.3(b) Terrain 2

圖4(a) 地形1匹配結果Fig.4(a) Matching result of terrain 1

圖4(b) 地形2匹配結果Fig.4(b) Matching result of terrain 2

對地形1和地形2在經緯度方向的最大誤差和平均誤差進行統計，統計結果如表1所示。

表1 兩塊匹配誤差統計結果Tab.1 Matching errors of two terrains

若要求潛器經緯度方向定位精度小于一個網格，也即139 m，則從表1可以簡單判定地形1為可匹配地形，地形2為不可匹配地形。

在地形2上對同步定位與構圖算法的有效性進行仿真驗證。分別為水下潛器設置20 m × 20 m、300 m × 200 m的運動空間，設定聲納的掃描范圍為20 m，慣性儀表參數設置同上。如圖5(a)、圖5(b)所示分別為潛器在小、大兩個運動空間上以不同的航行路徑所得到的仿真結果圖。

圖5(a) 20 m × 20 m仿真結果Fig.5(a) Simulation result of 20 m × 20 m

如圖5(a)、圖5(b)中，“-o”線所示為純慣導指示航跡，“-”線所示為潛器理想航跡，“-口”線所示SLAM算法得到的航跡。從圖中可以明顯地看到，當純慣導得到的航跡與潛器理想航跡存在較大偏差時，SLAM算法得到的航跡很接近于潛器的理想航跡。

圖5(b) 300 m × 200 m仿真結果Fig.5(b) Simulation result of 300 m × 200 m

5 結 論

綜上所述，算法不僅可以在地形可匹配區域，采用ICCP算法實現高精度的匹配定位，也可以在地形不可匹配區域，采用SLAM算法實現精確的定位，這對于實現水下潛器長時間高精度導航具有很重要的意義。

（References）：

[1] Bryson M. Active airborne localisation and exploration in unknown environments using inertial SLAM[C]//IEEE Aerospace Conference. 2006: 229-241.

[2] 徐曉蘇, 吳劍飛, 徐勝保, 等. 基于仿射修正技術的水下地形 ICCP匹配算法[J]. 中國慣性技術學報, 2014, 22(3): 362-367. Xu Xiao-su, Wu Jian-fei, Xu Sheng-bao, et al. ICCP algorithm for underwater terrain matching navigation based on affine correction[J]. Journal of Chinese Inertial Technology, 2014, 22(3): 362-367.

[3] Kim J, Sukkarieh S. Real-time implementation of airborne inertial-SLAM[J]. Robotics and Autonomous Systems, 2007, 55(1): 62-71.

[4] 楊繪弘. 基于ICCP的水下潛器地形輔助導航方法研究[D]. 哈爾濱: 哈爾濱工程大學, 2009. Yang Hong-hui. Research on terrain matching algori- thm based on ICCP for underwater vehicles[D]. Harbin: Harbin Engineering University, 2009.

[5] Zheng Tong, Bian Shao-feng. Study on the algorithm of sea bottom terrain match[C]//Manchester, United Kingdom, 2009: 179-182.

[6] 王文晶. 基于重力和環境特征的水下導航定位方法研究[D]. 哈爾濱: 哈爾濱工程大學, 2009. Wang Wen-jing. Underwater navigation methods based on gravity and environmental features[D]. Harbin: Harbin Engineering University, 2009.

[7] Durrant H F, Bailey T. Simultaneous location and mapping: PartⅡ[J]. IEEE Robotics and Automation Magazine, 2006, 13(3): 108-117.

[8] Ribas D, Ridao P, Tardos J D, et al. Visual SLAM for 3D large-scale seabed acquisition employing underwater vehicles[C]//Proceedings of IEEE/RSJ International Conference on Intelligent Robots and Systems. Nice, France: IEEE Press, 2008: 1011-1016.

[9] 李佩娟, 徐曉蘇, 張濤. 信息融合技術在水下組合導航系統中的應用[J]. 中國慣性技術學報, 2009, 17(3): 344-349. Li Pei-juan, Xu Xiao-su, Zhang Tao. Application of information fusion to integrated navigation system of underwater vehicle[J]. Journal of Chinese Inertial Technology, 2009, 17(3): 344-349.

[10] Deng Z, Ge Y, Guan W, et al. Underwater map-matching aided inertial navigation system based on multigeophysical information[J]. Frontiers of Electrical and Electronic Engineering in China, 2010, 5(4): 496-500.

[11] 王向磊, 蘇牡丹, 劉培根, 等. 利用改進的ICCP算法輔助導航[J]. 測繪科學, 2013, 38(1): 36-39, 50. Wang Xiang-lei, Su Mu-dan, Liu Pei-gen, et al. Application of improved ICCP algorithm in gravity matching aided navigation[J]. Science of Surveying and Mapping, 2013, 38(1): 36-39, 50.

Underwater navigation method based on terrain and environmental features

XU Xiao-su1,2,Tang Jun-jun1,2, ZHANG Tao1,2, YUE Zeng-yang1,2
(1. Key Laboratory of Micro-inertial Instrument and Advanced Navigation Technology, Ministry of Education, Southeast University, Nanjing 210096, China; 2. School of Instrument Science & Engineering, Southeast University, Nanjing 210096, China.)

The main drawback in traditional terrain aided navigation(TAN)/SINS systems is that it becomes useless in some area when without any terrain data or with a featureless terrain field. In order to realize long-distance and high-precision navigation for an underwater vehicle, a new navigation algorithm is presented which combines terrain data with environmental features. For the areas where terrain data is available and terrain is of great change, TAN is used to correct errors of SINS; for the areas where terrain data is not available or terrain is of less change, a feature-based SLAM method is used to correct the error of SINS. Simulation results show that the proposed method can achieve a better positioning precision than that of pure inertial navigation both in matchable areas and unmatchable areas.

underwater vehicle; terrain aided navigation; simultaneous localization and mapping; strapdowm inertial navigation system

U666.1

：A

2015-05-23；

：2015-08-28

國家自然科學基金項目（51175082，61473085，51375088）、微慣性儀表與先進導航技術教育部重點實驗室基金，優秀青年教師教學科研資助計劃（2242015R30031）

徐曉蘇（1961—），男，博士生導師，從事測控技術與導航定位領域的研究。E-mail：xxs@seu.edu.cn

1005-6734(2015)05-0590-07

10.13695/j.cnki.12-1222/o3.2015.05.007