劉 明, 張 嚴, 徐德民

?

基于圖像聲納SLAM算法在AUV組合導航中的應用

劉 明1, 張 嚴2, 徐德民1

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

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

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

0 引言

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

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

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

1 SLAM在AUV組合導航中的應用

1.1 地形傳感器性能

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

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

1.2 SLAM基本原理

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

圖1 系統框圖

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

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

1.3 數據融合算法模型

1.3.1 AUV運動模型

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

1.3.2 SLAM算法框架

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

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

每當有新地標進入則地圖狀態(tài)變量更新

每個特征點向量包括該點在導航坐標系下的位置和聲學尺寸,為當前地圖中的特征點數目。

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

式中:f ,為來自INS的加速度及轉動速率; f ,為對應的誤差值。

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

其中，()為慣導系統白噪聲, 其噪聲強度為()。

噪聲強度

1.3.3 地形特征點方位/距離/高度量測

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

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

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

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

第個地形特征點地理系中的位置為

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

因此, 式(16)可寫為

此處

1.3.4 量測誤差方程

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

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

量測方程包含傳感器坐標系到地理坐標系、直角系到極坐標系的轉換, 經過線性化處理后得到觀測矩陣和噪聲輸入矩陣為

1.3.5 Kalman濾波估計

其中

1.3.6 誤差修正

地圖位置修正

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

1.4 數據關聯

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

文獻[8]采用正則化新息方差(normalised innovation square, NIS)進行關聯計算

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

2 算法數學仿真結果

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

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

表1 仿真參數

Table 1 Simulation parameters

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

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

圖2 AUV運動軌跡圖

圖3 SLAM回路效果放大圖

圖4 東向速度誤差

圖5 北向速度誤差

從26個特征點和54個特征點這2種情況下速度誤差曲線可見, 在數學仿真理想的邊界條件下, 地形特征點的數量對于導航經度的影響主要表現在噪聲強度上, 對于絕對定位精度沒有數量級的提高。

3 結束語

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

隨著聲納技術的發(fā)展, 在繪制地圖范圍和地圖特征提取準確度上也已取得進展, 這給提高AUV水下遠程深海組合導航精度展現了光明前景。當然, 利用聲納作為地形傳感器仍然面臨很多問題, 例如海底環(huán)境特征非結構化特征導致制圖困難; 聲納成像速度和AUV航行速度的匹配; 水中動態(tài)障礙物和靜態(tài)障礙物作為地形特征點的甄別和遴選[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] 李佩娟, 徐曉蘇, 張濤. 信息融合技術在水下組合導航系統中的應用[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 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]. 控制工程, 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-), 男, 高工, 博士研究生, 研究領域為水下航行器組合導航.

(責任編輯: 楊力軍)