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

?

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

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

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

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

自主水下航行器; 同時(shí)定位與地圖創(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)長(zhǎng)時(shí)間保持較高的位置、速度及姿態(tài)精度。但由于惡劣的水下環(huán)境無(wú)法接收GPS信號(hào), 導(dǎo)航誤差隨著航行時(shí)間和距離的增加而發(fā)散的問(wèn)題長(zhǎng)期無(wú)法有效解決。

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

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

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

1.1 地形傳感器性能

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

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

1.2 SLAM基本原理

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

圖1 系統(tǒng)框圖

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

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

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

1.3.1 AUV運(yùn)動(dòng)模型

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

1.3.2 SLAM算法框架

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

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

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

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

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

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

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

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

噪聲強(qiáng)度

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

量測(cè)方程為非線性模型[7]

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

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

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

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

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

因此, 式(16)可寫為

此處

1.3.4 量測(cè)誤差方程

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

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

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

1.3.5 Kalman濾波估計(jì)

其中

1.3.6 誤差修正

地圖位置修正

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

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

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

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

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

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

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

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

表1 仿真參數(shù)

Table 1 Simulation parameters

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

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

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

圖3 SLAM回路效果放大圖

圖4 東向速度誤差

圖5 北向速度誤差

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

3 結(jié)束語(yǔ)

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

隨著聲納技術(shù)的發(fā)展, 在繪制地圖范圍和地圖特征提取準(zhǔn)確度上也已取得進(jìn)展, 這給提高AUV水下遠(yuǎn)程深海組合導(dǎo)航精度展現(xiàn)了光明前景。當(dāng)然, 利用聲納作為地形傳感器仍然面臨很多問(wèn)題, 例如海底環(huán)境特征非結(jié)構(gòu)化特征導(dǎo)致制圖困難; 聲納成像速度和AUV航行速度的匹配; 水中動(dòng)態(tài)障礙物和靜態(tài)障礙物作為地形特征點(diǎn)的甄別和遴選[9]等都對(duì)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]. 中國(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 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] 蔡自興, 肖正, 于金霞. 動(dòng)態(tài)環(huán)境中移動(dòng)機(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)域?yàn)樗潞叫衅鹘M合導(dǎo)航.

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