一種結(jié)合地形和環(huán)境特征的水下導(dǎo)航定位方法

徐曉蘇，湯郡郡，張 濤，岳增陽(yáng)

（1. 微慣性儀表與先進(jìn)導(dǎo)航技術(shù)教育部重點(diǎn)實(shí)驗(yàn)室，南京210096；2. 東南大學(xué) 儀器科學(xué)與工程學(xué)院，南京 210096）

一種結(jié)合地形和環(huán)境特征的水下導(dǎo)航定位方法

徐曉蘇1,2，湯郡郡1,2，張 濤1,2，岳增陽(yáng)1,2

（1. 微慣性儀表與先進(jìn)導(dǎo)航技術(shù)教育部重點(diǎn)實(shí)驗(yàn)室，南京210096；2. 東南大學(xué) 儀器科學(xué)與工程學(xué)院，南京 210096）

為了實(shí)現(xiàn)水下潛器長(zhǎng)時(shí)間高精度導(dǎo)航定位，同時(shí)考慮到傳統(tǒng)地形輔助導(dǎo)航系統(tǒng)在先驗(yàn)地形圖不可得或者是地形變化不明顯的海域（地形不可匹配區(qū)域），無(wú)法用來(lái)修正慣性導(dǎo)航位置誤差的問(wèn)題，提出了一種結(jié)合地形和環(huán)境特征的水下導(dǎo)航定位方法。在先驗(yàn)地形圖可得且地形高程變化明顯的可匹配區(qū)域，采用地形輔助導(dǎo)航系統(tǒng)來(lái)修正慣導(dǎo)位置誤差，在先驗(yàn)地形圖不可得或者是地形高程變化不明顯的不可匹配區(qū)域，采用基于海洋環(huán)境特征的同步定位與構(gòu)圖算法來(lái)修正慣導(dǎo)位置誤差。仿真結(jié)果表明，該方法在地形可匹配區(qū)域以及地形不可匹配區(qū)域得到的航跡都比純慣導(dǎo)得到的軌跡更接近于理想航跡，因此可以用來(lái)修正慣導(dǎo)位置誤差。

水下航行器；地形輔助導(dǎo)航系統(tǒng)；同步定位與構(gòu)圖；慣性導(dǎo)航系統(tǒng)

慣性導(dǎo)航系統(tǒng)不需要任何外來(lái)信息，也不向外輻射任何信息，僅依靠自身就能在全天候條件下，在全球范圍內(nèi)和任何介質(zhì)環(huán)境中進(jìn)行連續(xù)的導(dǎo)航和定位，這種同時(shí)具備自主性、隱蔽性和能獲取載體完備運(yùn)動(dòng)信息的獨(dú)特優(yōu)點(diǎn)使其成為最受廣泛使用的水下導(dǎo)航定位方法。但是，慣導(dǎo)系統(tǒng)也有著系統(tǒng)誤差隨時(shí)間積累的原理性缺陷，為了實(shí)現(xiàn)長(zhǎng)航時(shí)高精度的導(dǎo)航目標(biāo)，慣性導(dǎo)航系統(tǒng)需要利用外界位置信息對(duì)其進(jìn)行周期的調(diào)整和校正。

地形輔助導(dǎo)航（Terrain-Aided Navigation，TAN）是一種利用地形高程特征來(lái)進(jìn)行輔助定位的方法，它具有自主、隱蔽、連續(xù)、全天候工作、導(dǎo)航定位誤差不隨時(shí)間積累等優(yōu)點(diǎn)，是水下潛器理想的輔助導(dǎo)航定位手段。然而，地形輔助導(dǎo)航需要有先驗(yàn)的地形圖，且要求地形高程有明顯的變化。對(duì)于一些尚未測(cè)繪或者是地形變化過(guò)于平緩的海域，傳統(tǒng)的地形輔助導(dǎo)航方法是無(wú)法進(jìn)行的。

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

考慮到同步定位與構(gòu)圖算法的這種優(yōu)勢(shì)，針對(duì)地形輔助導(dǎo)航方法在地形不可匹配區(qū)域無(wú)法修正慣導(dǎo)隨時(shí)間積累的誤差問(wèn)題，提出了一種結(jié)合地形和環(huán)境特征的水下導(dǎo)航定位方法，在地形可匹配區(qū)域，采用地形輔助慣性導(dǎo)航方法來(lái)修正慣導(dǎo)位置誤差，在地形不可匹配區(qū)域，采用基于海洋環(huán)境特征的同步定位與構(gòu)圖算法來(lái)代替地形輔助導(dǎo)航方法修正慣導(dǎo)系統(tǒng)的位置誤差。仿真結(jié)果表明，該方法在地形可匹配區(qū)域和不可匹配區(qū)域都可以實(shí)現(xiàn)高精度的導(dǎo)航定位。

1 結(jié)合地形和環(huán)境特征的水下導(dǎo)航定位方法原理

SLAM整個(gè)過(guò)程中沒(méi)有一個(gè)確知的絕對(duì)位置，所以它實(shí)際上是一種相對(duì)定位，然而對(duì)于水下潛器這樣的載體來(lái)說(shuō)，得到相對(duì)位置并不是目的。當(dāng)潛器在地形可匹配區(qū)域航行時(shí)，采用地形輔助導(dǎo)航修正慣導(dǎo)位置誤差的同時(shí)，對(duì)環(huán)境進(jìn)行構(gòu)圖，此時(shí)潛器的絕對(duì)位置是經(jīng)過(guò)修正的，通過(guò)轉(zhuǎn)換得到的特征的絕對(duì)位置精度也相對(duì)較高。當(dāng)潛器從地形可匹配區(qū)域進(jìn)入地形不可匹配區(qū)域時(shí)，可以利用地形輔助導(dǎo)航得到的最后一個(gè)絕對(duì)位置，通過(guò)SLAM的相對(duì)定位以及對(duì)路標(biāo)的實(shí)時(shí)觀測(cè)實(shí)現(xiàn)潛器和路標(biāo)絕對(duì)位置的傳遞。當(dāng)潛器再次進(jìn)入地形可匹配區(qū)域時(shí)，由于慣性導(dǎo)航系統(tǒng)的位置誤差已經(jīng)得到了及時(shí)的修正，潛器初始位置誤差很小，地形匹配算法可以在很小的范圍內(nèi)尋找匹配點(diǎn)，這不僅提高了算法的效率，也同時(shí)提高了導(dǎo)航定位的精度。

2 地形可匹配區(qū)域

在先驗(yàn)地形圖可得且地形變化明顯的可匹配區(qū)域，采用基于ICCP算法的地形輔助導(dǎo)航系統(tǒng)來(lái)修正慣性導(dǎo)航系統(tǒng)的位置誤差。

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

① 由慣性導(dǎo)航系統(tǒng)提供航跡測(cè)量的N個(gè)位置值pi（其中i=1,2,…,N）；實(shí)時(shí)測(cè)深傳感器提供慣導(dǎo)指示航跡序列點(diǎn)pi相應(yīng)的水深值hi（其中i=1,2,…,N），并且從已知參考地圖中抽取出相應(yīng)的等值線Ci（其中i=1,2,…,N）。

③ 假設(shè)pi在相應(yīng)等值線上的最近點(diǎn)為yi，尋找隨機(jī)旋轉(zhuǎn)和平移變換T，使集合之間的距離最小，也即使下面的目標(biāo)函數(shù)最小

⑤ 判斷是否滿足最終的迭代終止條件，如果滿足便退出。此時(shí)得到的匹配點(diǎn)即為最佳匹配點(diǎn)，可用來(lái)修正慣導(dǎo)位置誤差。

2.2 SINS/TAN系統(tǒng)模型的構(gòu)建

2.2.1 狀態(tài)方程

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

式中，

2.2.2 觀測(cè)方程

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

式中，Vk為觀測(cè)噪聲, Hk如下：

2.2.3 誤差修正

將慣性導(dǎo)航系統(tǒng)輸出的位置信息與ICCP匹配得到的位置信息的差值作為觀測(cè)量，進(jìn)行卡爾曼濾波，并將濾波得到的位置誤差量反饋到主慣性導(dǎo)航系統(tǒng)就可以實(shí)現(xiàn)對(duì)主慣性導(dǎo)航系統(tǒng)位置信息的校正。

3 地形不可匹配區(qū)域

在先驗(yàn)地形圖不可得或者是地形變化不明顯的不可匹配區(qū)域，采用基于擴(kuò)展卡爾曼濾波的同步定位與構(gòu)圖算法來(lái)修正慣性導(dǎo)航系統(tǒng)的位置誤差。

3.1 系統(tǒng)狀態(tài)向量和協(xié)方差矩陣

水下潛器的狀態(tài)用它在地理坐標(biāo)系下緯度方向的位置分量xv、經(jīng)度方向的位置分量yv以及航向φv表示，其估計(jì)均值和方差分別定義為

設(shè)潛器觀測(cè)到的特征均為靜止的2維點(diǎn)特征，并用這些點(diǎn)特征在同一個(gè)坐標(biāo)系下的位置來(lái)形成地圖。

該向量維數(shù)隨著新特征的加入而擴(kuò)充，其協(xié)方差為

系統(tǒng)狀態(tài)向量為

系統(tǒng)狀態(tài)協(xié)方差為

式中，Pvv表示潛器位姿估計(jì)的不確定性，Pvm表示潛器位姿與環(huán)境特征估計(jì)之間的相關(guān)性，Pmm中對(duì)角線元素表示每個(gè)環(huán)境特征位置估計(jì)的不確定性，非對(duì)角線元素表示某一特征與其他特征估計(jì)間的相關(guān)性。

3.2 水下航行器EKF-SLAM算法的具體執(zhí)行過(guò)程

EKF-SLAM算法的執(zhí)行過(guò)程大體可以分為三個(gè)階段：預(yù)測(cè)、觀測(cè)和更新/擴(kuò)充。首先將潛器姿態(tài)和地圖特征存儲(chǔ)在一個(gè)獨(dú)立的狀態(tài)向量中，然后通過(guò)先觀測(cè)再觀測(cè)的迭代地推過(guò)程來(lái)估計(jì)系統(tǒng)狀態(tài)，實(shí)現(xiàn)對(duì)潛器的定位和特征地圖的創(chuàng)建。

3.2.1 預(yù)測(cè)階段

圖1 潛器姿態(tài)變換示意圖Fig.1 Attitude transformation of vehicle

3.2.2 觀測(cè)階段

設(shè)當(dāng)前時(shí)刻聲納測(cè)量得到一個(gè)特征，經(jīng)過(guò)數(shù)據(jù)關(guān)聯(lián)，它與存儲(chǔ)在地圖中的估計(jì)值為的特征關(guān)聯(lián)成功，觀測(cè)值和觀測(cè)協(xié)方差分別為

式中，r表示載體坐標(biāo)系下特征相對(duì)于潛器的距離，θ表示載體坐標(biāo)系下特征相對(duì)于潛器的方位。

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

而：

對(duì)于特征數(shù)量很多的海底環(huán)境而言，SLAM算法中的雅可比矩陣中大部分項(xiàng)都是 0，這使得新息協(xié)方差矩陣以及濾波增益的計(jì)算成為可能。

3.2.3 更新/擴(kuò)充階段

對(duì)于關(guān)聯(lián)正確的特征，可以對(duì)潛器狀態(tài)和地圖進(jìn)行更新，更新如下：

隨著聲納對(duì)海底環(huán)境的探測(cè)，必然會(huì)掃描到一些新的特征，當(dāng)某個(gè)特征第一次被觀測(cè)時(shí)，必須通過(guò)一個(gè)初始化過(guò)程將特征加入到地圖中去，使地圖可以隨著潛器探索區(qū)域的增大而不斷擴(kuò)展。對(duì)新特征的初始化方法如下：

首先，將新觀測(cè)值z(mì)和觀測(cè)協(xié)方差R擴(kuò)充到系統(tǒng)狀態(tài)向量和狀態(tài)協(xié)方差中，測(cè)量是在載體坐標(biāo)系下獲得的。擴(kuò)充后的系統(tǒng)狀態(tài)和狀態(tài)協(xié)方差如下所示：

新的觀測(cè)值z(mì)與潛器當(dāng)前姿態(tài)的關(guān)系如圖2所示。

圖2 新的特征觀測(cè)與航行器關(guān)系圖Fig.2 Relationship between new feature and vehicle

由于雅可比矩陣大部分項(xiàng)為 0，變換僅影響到協(xié)方差矩陣對(duì)角線上的矩陣塊和新特征所在的行和列上與地圖其它狀態(tài)的互相關(guān)項(xiàng)，因此擴(kuò)充后的狀態(tài)協(xié)方差矩陣可以如下表示：

4 仿真實(shí)驗(yàn)

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

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

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

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

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

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

對(duì)地形1和地形2在經(jīng)緯度方向的最大誤差和平均誤差進(jìn)行統(tǒng)計(jì)，統(tǒng)計(jì)結(jié)果如表1所示。

表1 兩塊匹配誤差統(tǒng)計(jì)結(jié)果Tab.1 Matching errors of two terrains

若要求潛器經(jīng)緯度方向定位精度小于一個(gè)網(wǎng)格，也即139 m，則從表1可以簡(jiǎn)單判定地形1為可匹配地形，地形2為不可匹配地形。

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

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

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

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

5 結(jié) 論

綜上所述，算法不僅可以在地形可匹配區(qū)域，采用ICCP算法實(shí)現(xiàn)高精度的匹配定位，也可以在地形不可匹配區(qū)域，采用SLAM算法實(shí)現(xiàn)精確的定位，這對(duì)于實(shí)現(xiàn)水下潛器長(zhǎng)時(shí)間高精度導(dǎo)航具有很重要的意義。

（References）：

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

[2] 徐曉蘇, 吳劍飛, 徐勝保, 等. 基于仿射修正技術(shù)的水下地形 ICCP匹配算法[J]. 中國(guó)慣性技術(shù)學(xué)報(bào), 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ǎo)航方法研究[D]. 哈爾濱: 哈爾濱工程大學(xué), 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] 王文晶. 基于重力和環(huán)境特征的水下導(dǎo)航定位方法研究[D]. 哈爾濱: 哈爾濱工程大學(xué), 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] 李佩娟, 徐曉蘇, 張濤. 信息融合技術(shù)在水下組合導(dǎo)航系統(tǒng)中的應(yīng)用[J]. 中國(guó)慣性技術(shù)學(xué)報(bào), 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] 王向磊, 蘇牡丹, 劉培根, 等. 利用改進(jìn)的ICCP算法輔助導(dǎo)航[J]. 測(cè)繪科學(xué), 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

國(guó)家自然科學(xué)基金項(xiàng)目（51175082，61473085，51375088）、微慣性儀表與先進(jìn)導(dǎo)航技術(shù)教育部重點(diǎn)實(shí)驗(yàn)室基金，優(yōu)秀青年教師教學(xué)科研資助計(jì)劃（2242015R30031）

徐曉蘇（1961—），男，博士生導(dǎo)師，從事測(cè)控技術(shù)與導(dǎo)航定位領(lǐng)域的研究。E-mail：xxs@seu.edu.cn

1005-6734(2015)05-0590-07

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