基于圖像聲納SLAM算法在AUV組合導(dǎo)航中的應(yīng)用

劉 明, 張 嚴(yán), 徐德民

?

基于圖像聲納SLAM算法在AUV組合導(dǎo)航中的應(yīng)用

劉 明1, 張 嚴(yán)2, 徐德民1

(1. 西北工業(yè)大學(xué) 航海學(xué)院, 陜 西西安, 710072; 2. 中國船舶重工集團(tuán)公司第705研究所 昆明分部, 云南 昆明, 650118)

為了在有限的體積、功耗及成本范圍內(nèi)提高自主水下航行器(AUV)遠(yuǎn)程、深海導(dǎo)航定位精度, 提出了將基于圖像聲納的同時定位與地圖創(chuàng)建(SLAM)方法應(yīng)用于AUV水下組合導(dǎo)航系統(tǒng), 利用圖像聲納獲取AUV與地形特征點(diǎn)之間的距離和3D方位數(shù)據(jù), 結(jié)合捷聯(lián)慣性導(dǎo)航系統(tǒng)(SINS)得到的導(dǎo)航數(shù)據(jù), 通過擴(kuò)展卡爾曼濾波(EFK)方法對載體狀態(tài)和地圖狀態(tài)進(jìn)行連續(xù)并行估計和量測, 將得到的誤差估計反饋回SINS進(jìn)行修正, 可抑制其隨航行時間和距離增加的姿態(tài)、速度和位置誤差。此外, 在地形特征點(diǎn)向量中加入聲學(xué)尺寸元素, 可提高特征識別的準(zhǔn)確性。仿真結(jié)果表明, 在持續(xù)觀測到有效的地形特征點(diǎn)條件下, 慣導(dǎo)誤差得到了較好抑制, 特別是在AUV返程或往復(fù)巡航過程中, 重復(fù)觀測到同一地標(biāo)時, 可大幅提高水下組合導(dǎo)航的定位精度。

自主水下航行器; 同時定位與地圖創(chuàng)建; 圖像聲納; 捷聯(lián)慣性導(dǎo)航系統(tǒng)

0 引言

精確水下導(dǎo)航是遠(yuǎn)程自主水下航行器 (autonomous underwater vehicles, AUV)有效應(yīng)用和成功回收的關(guān)鍵技術(shù)之一, 要求導(dǎo)航系統(tǒng)必須在有限的體積、功耗和成本范圍內(nèi)長時間保持較高的位置、速度及姿態(tài)精度。但由于惡劣的水下環(huán)境無法接收GPS信號, 導(dǎo)航誤差隨著航行時間和距離的增加而發(fā)散的問題長期無法有效解決。

各國科學(xué)家對此展開了積極探索并取得了卓有成效的進(jìn)展, 其中很重要的一個突破來自機(jī)器人導(dǎo)航領(lǐng)域的同時定位與地圖創(chuàng)建(simultaneous localization and mapping, SLAM)技術(shù)。SLAM的出現(xiàn)使得在未知區(qū)域的地形輔助導(dǎo)航成為可能, 其基本思路是, 當(dāng)機(jī)器人被放置在一個未知的位置和陌生的環(huán)境中時, 在不斷構(gòu)造與周圍環(huán)境具有相合性(consistent)的增量式地圖的同時確定自身在地圖中的位置[1]。由于地形輔助導(dǎo)航具有與GPS一樣誤差不隨時間增長的優(yōu)良特性, 可以同慣性導(dǎo)航系統(tǒng)構(gòu)成很好的組合。這一特性為AUV水下長航程或者長航時自主式導(dǎo)航過程中抑制慣性系統(tǒng)誤差的發(fā)散提供了新的思路。研究基于SLAM方法的遠(yuǎn)程AUV組合導(dǎo)航技術(shù), 對于抑制慣性導(dǎo)航系統(tǒng)隨時間和距離增長的誤差, 提高導(dǎo)航精度具有實際意義。

SLAM技術(shù)在過去的十幾年中取得了很多實用性的成果[2], 如基于攝像機(jī)的SLAM算法在無人飛機(jī)上已獲得成功應(yīng)用[3], 悉尼大學(xué)水下機(jī)器人Oberon在近海測試結(jié)果證明SLAM算法生成的估計是連續(xù)一致的[4], 麻省理工學(xué)院2000年利用真實海洋聲納進(jìn)行SLAM算法測試證明其誤差增長比INS＋DVL慢得多, 而對于室外,特別是海底的非結(jié)構(gòu)化未知環(huán)境中采用這一技術(shù)目前還處于試驗階段。

1 SLAM在AUV組合導(dǎo)航中的應(yīng)用

1.1 地形傳感器性能

本文以Imagenex 技術(shù)公司837型“Delta T”民用多波束圖像聲納作為地形傳感器, 其基本性能指標(biāo)如下[5]: 頻率260 kHz, 掃描范圍在120°(水平)× 20°(垂直), 單波束水平寬度為3°, 距離分辨率是0.2%(為距離), 探測距離為5~100 m, 輸出接口為10 M以太網(wǎng)。

該傳感器輸出的數(shù)據(jù)比較復(fù)雜, 主要用于圖像顯示, 本文直接采集其各個波束輸出的原始數(shù)據(jù), 利用特征提取方法實時獲得滿足要求的地圖特征點(diǎn)相對于AUV的方位和距離數(shù)據(jù)。

1.2 SLAM基本原理

SLAM方法并不需要象地形匹配導(dǎo)航中那樣事先存儲地圖信息, 而是通過避碰聲納實時構(gòu)造一個增量式地圖, 并行估計AUV在這個地圖中的位置, 從而形成一個真正自持的自主式導(dǎo)航系統(tǒng), 系統(tǒng)框圖如圖1所示。

圖1 系統(tǒng)框圖

文獻(xiàn)[6]已證明SINS、DVL和SLAM系統(tǒng)利用聯(lián)邦卡爾曼濾波進(jìn)行有效融合, 可使水下航行器的水平速度誤差、姿態(tài)角誤差、航向角誤差和位置誤差都在較短時間內(nèi)以較高精度達(dá)到穩(wěn)定。

SLAM方法中, 當(dāng)?shù)匦蝹鞲衅鳈z測到一個有效的地形特征時, SLAM估計器將該特征點(diǎn)位置增加到地理坐標(biāo)系下的地圖數(shù)據(jù)庫中, 然后通過連續(xù)的量測并行估計AUV和地圖的狀態(tài), 得到的誤差估計值反饋回INS進(jìn)行修正, 使其保持高于純慣導(dǎo)的姿態(tài)、速度和位置精度。然而問題是隨著地標(biāo)數(shù)目的增加, 地圖狀態(tài)變量維數(shù)以地標(biāo)數(shù)目的平方關(guān)系遞增, 算法很快發(fā)散, 突破來自Durrant-Whyte給出的SLAM問題的結(jié)構(gòu)描述和收斂性分析結(jié)果:如果把地圖創(chuàng)建和定位估計同時進(jìn)行, 算法是收斂的。以往試圖消除的地標(biāo)之間的關(guān)聯(lián)度其實是解決問題的關(guān)鍵, 而且關(guān)聯(lián)度越大, 收斂性越好[1]。

1.3 數(shù)據(jù)融合算法模型

1.3.1 AUV運(yùn)動模型

式中:為橫滾角;為俯仰角;為航向角。

1.3.2 SLAM算法框架

SLAM算法基本框架是利用卡爾曼濾波器作為狀態(tài)估計器, 在給出AUV運(yùn)動模型和AUV與特征點(diǎn)之間的相對位置量測之后, 估計出地圖的結(jié)構(gòu)以及AUV在地圖中的位置、速度及方位[7]。

算法以AUV及地圖的上述變量的誤差作為狀態(tài)變量

每當(dāng)有新地標(biāo)進(jìn)入則地圖狀態(tài)變量更新

每個特征點(diǎn)向量包括該點(diǎn)在導(dǎo)航坐標(biāo)系下的位置和聲學(xué)尺寸,為當(dāng)前地圖中的特征點(diǎn)數(shù)目。

連續(xù)時間的誤差模型為

式中:f ,為來自INS的加速度及轉(zhuǎn)動速率; f ,為對應(yīng)的誤差值。

線性化后SLAM誤差離散時間狀態(tài)方程為[7]

其中，()為慣導(dǎo)系統(tǒng)白噪聲, 其噪聲強(qiáng)度為()。

噪聲強(qiáng)度

1.3.3 地形特征點(diǎn)方位/距離/高度量測

量測方程為非線性模型[7]

式中，()為量測噪聲向量。

由于該方程為載體和第個地形特征向量的函數(shù), 因此方程

式中:()為第次量測；v()是方差為()的白噪聲。

第個地形特征點(diǎn)地理系中的位置為

式中:,,分別為地形傳感器量測得到的特征點(diǎn)相對距離, 方位角及高度角。

因此, 式(16)可寫為

此處

1.3.4 量測誤差方程

以該誤差作為量測狀態(tài)向量建立量測方程

式中:()為量測的雅可比矩陣;()是強(qiáng)度為()的量測噪聲。

量測方程包含傳感器坐標(biāo)系到地理坐標(biāo)系、直角系到極坐標(biāo)系的轉(zhuǎn)換, 經(jīng)過線性化處理后得到觀測矩陣和噪聲輸入矩陣為

1.3.5 Kalman濾波估計

其中

1.3.6 誤差修正

地圖位置修正

上述公式遞推執(zhí)行構(gòu)成了一個完整的SLAM/INS估計的閉環(huán)運(yùn)算過程。

1.4 數(shù)據(jù)關(guān)聯(lián)

數(shù)據(jù)關(guān)聯(lián)是指不同時刻對環(huán)境的觀測是否來源于同一特征。對于擴(kuò)展卡爾曼濾波器(extended Kalman filter, EKF)模型來說, 數(shù)據(jù)關(guān)聯(lián)不準(zhǔn)確將導(dǎo)致算法無法收斂。數(shù)據(jù)關(guān)聯(lián)目的是找出時刻的量測與地圖中地形特征點(diǎn)之間的對應(yīng)關(guān)系, 將量測與地形特征點(diǎn)之間進(jìn)行準(zhǔn)確的關(guān)聯(lián)是構(gòu)造實時地圖的關(guān)鍵步驟, 直接決定SLAM算法的成敗。

文獻(xiàn)[8]采用正則化新息方差(normalised innovation square, NIS)進(jìn)行關(guān)聯(lián)計算

假設(shè)新息及其協(xié)方差均滿足正態(tài)分布, 則為2分布。在預(yù)先確定的置信度水平下, 對2用查表法得到關(guān)聯(lián)閥值, 每次量測計算得到的小于該閥值, 則該次新息可與特征點(diǎn)關(guān)聯(lián)。

2 算法數(shù)學(xué)仿真結(jié)果

數(shù)學(xué)仿真以837型“Delta T”多波束成像聲納作為地形傳感器對方位和距離進(jìn)行量測, 各傳感器參數(shù)按民用級別設(shè)定如下[5]。

仿真邊界條件: 假設(shè)AUV在水下貼近海底200 m高度范圍內(nèi)航行, 根據(jù)國外常見的AUV參數(shù)假設(shè)平均航速≤10 kn, 航行時間2 000 s, DVL對INS漂移的補(bǔ)償不予考慮, 僅考慮INS和聲納的量測誤差, 噪聲水平見表1, 可關(guān)聯(lián)的地形特征點(diǎn)分26個和54個2種情況進(jìn)行仿真。

表1 仿真參數(shù)

Table 1 Simulation parameters

開始時刻假定地圖中有5個利用GPS確定的地形特征點(diǎn), 之后GPS信號無效, INS誤差和地圖誤差通過新量測到的地形特征點(diǎn)各自獨(dú)立并行估計, 隨著時間推移, INS誤差不斷增長, 導(dǎo)致地圖誤差隨之增大, 但在具有SLAM相關(guān)性的閉環(huán)作用下, INS誤差被有效抑制, 進(jìn)而消除了地圖中因INS導(dǎo)致的誤差項。每觀測到一個特征點(diǎn), 地圖估計方差單調(diào)下降, 反過來抑制了INS誤差增長, 實現(xiàn)了提高INS定位和測姿精度的目標(biāo)。數(shù)學(xué)仿真得到的軌跡見圖2。

觀測新地標(biāo)時, SLAM反饋回路可起到類似GPS誤差修正的作用, 特別是在回旋路徑上反復(fù)觀測到同一地標(biāo)時, 效果更加明顯。圖3為放大的軌跡圖, 修正的部分效果加入SLAM反饋前后東向和北向速度誤差對比見圖4和圖5。從圖中可知, 誤差增長得到有效控制, 這一特性對于提高返程階段或在同一海域反復(fù)巡航的AUV導(dǎo)航精度有一定意義。

圖2 AUV運(yùn)動軌跡圖

圖3 SLAM回路效果放大圖

圖4 東向速度誤差

圖5 北向速度誤差

從26個特征點(diǎn)和54個特征點(diǎn)這2種情況下速度誤差曲線可見, 在數(shù)學(xué)仿真理想的邊界條件下, 地形特征點(diǎn)的數(shù)量對于導(dǎo)航經(jīng)度的影響主要表現(xiàn)在噪聲強(qiáng)度上, 對于絕對定位精度沒有數(shù)量級的提高。

3 結(jié)束語

837型“Delta T”多波束成像聲納作為地形傳感器, 可生成3D聲納圖像, 地形信息含量更為豐富, 且輸出頻率較高。本文針對這一特點(diǎn), 在提高地形特征點(diǎn)密度前提下仿真了AUV高速(10 kn)航行時速度誤差估計的效果, 結(jié)果表明, 使用該型聲納可使AUV在更大的航速范圍內(nèi)使用SLAM算法進(jìn)行組合導(dǎo)航。同時本文在地形特征向量中加入地標(biāo)聲學(xué)尺寸元素, 有利于AUV返航過程中再次觀測到同一地標(biāo)時更加準(zhǔn)確快速的作出判斷, 數(shù)學(xué)仿真表明, 回旋過程導(dǎo)航精度得到了一定程度的提高。

隨著聲納技術(shù)的發(fā)展, 在繪制地圖范圍和地圖特征提取準(zhǔn)確度上也已取得進(jìn)展, 這給提高AUV水下遠(yuǎn)程深海組合導(dǎo)航精度展現(xiàn)了光明前景。當(dāng)然, 利用聲納作為地形傳感器仍然面臨很多問題, 例如海底環(huán)境特征非結(jié)構(gòu)化特征導(dǎo)致制圖困難; 聲納成像速度和AUV航行速度的匹配; 水中動態(tài)障礙物和靜態(tài)障礙物作為地形特征點(diǎn)的甄別和遴選[9]等都對SLAM算法成敗起決定性作用。

[1] Durrant-Whyte H, Batley T. Simultaneous Localization and Mapping: Part I[J]. IEEE Robotics & Automation Magazine, 2006, 13(2): 99-110.

[2] Nüchter A, Lingemann K, Hertzberg J. 6D SLAM—3D Mapping Outdoor Environments[J]. Journal of Field Ro- botics, 2007, 24(8/9): 699-722.

[3] Steder B, Grisetti G, Stachniss C, et al. Visual SLAM for Flying Vehicles[J]. IEEE Transactions on Robotics, 2008, 24(5): 1088-1093 .

[4] Majumder S, Dissanayake G, Durrant-Whyte H. Multi- Sensor Data Fusion for underwater Navigation[J]. Jour- nal of Robotics and Autonomous System, 2001, 35(2): 97-108.

[5] Imagenex Technology Corp. Delta T Imaging Manual [EB/OL].[2006-07].http://www.imagenex.com/docs/prod/folders/print/Delta T.html.

[6] 李佩娟, 徐曉蘇, 張濤. 信息融合技術(shù)在水下組合導(dǎo)航系統(tǒng)中的應(yīng)用[J]. 中國慣性技術(shù)學(xué)報, 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 Tech- nology, 2009, 17(3): 344-349.

[7] Kim J, Sukkarieh S. 6DoF SLAM Aided GNSS/INS Navigation in GNSS Denied and Unknown Environ- ments[J]. Journal of Global Positioning Systems, 2005, 4(1-2): 120-128.

[8] Kim J. Autonomous Navigation for Airborne Appli- cations[D]. Sydney:The University of Sydney, 2004.

[9] 蔡自興, 肖正, 于金霞. 動態(tài)環(huán)境中移動機(jī)器人地圖構(gòu)建的研究進(jìn)展[J]. 控制工程, 2007, 14(3): 231-235. Cai Zi-xing, Xiao Zheng, Yu Jin-xia. Advances on Map Building with Mobile Robots in Dynamic Environments [J]. Control Engineering of China, 2007, 14(3): 231-235.

Application of SLAM Algorithm Based on Image Sonar to AUV Integrated Navigation

LIU Ming1, ZHANG Yan2, XU De-min

(1. College of Marine Engineering, Northwestern Polytechnical University, Xi￠an 710072, China; 2. Kunming Branch of the 705 Research Institute, China Shipbuilding Industry Corporation, Kunming 650118, China)

To improve the long-distance or deep-sea navigation and localization precision for an autonomous underwater vehicles(AUV) within limited volume, power and cost, an algorithm of simultaneous localization and mapping(SLAM) based on image sonar is presented for AUV underwater integrated navigation system. According to the data of real-time distance between each feature on map and an AUV, the data of three-dimensional bearing from image sonar and the data of navigation from strapdown inertial navigation system(SINS), the carrier state and map state can be estimated and measured simultaneously and continuously by extended Kalman filter(EKF). The errors are fed back to SINS for correction, hence the accumulated errors of attitude, velocity and position increasing with running distance and time can be effectively inhibited. Furthermore, the accuracy of features identification can be improved by putting acoustics size element into map features vector. Simulation result shows that this method can effectively inhibit errors of inertial navigation by continuously measuring valid features on map. Particularly, the localization accuracy of an underwater integrated navigation is significantly enhanced when same landmarks are observed repeatedly in cruise or return voyage of an AUV.

autonomous underwater vehicles (AUV); simultaneous localization and mapping (SLAM); image sonar; strapdown inertial navigation system (SINS)

TJ630.33; TP24

A

1673-1948(2011)04-0276-06

2011-01-17;

2011-02-17.

劉 明(1973-), 男, 高工, 博士研究生, 研究領(lǐng)域為水下航行器組合導(dǎo)航.

(責(zé)任編輯: 楊力軍)