聶永芳 潘 爽 孫東平 施建禮

1. 海軍潛艇學院戰(zhàn)略導彈與水中兵器系，青島 266071 2. 清華大學自動化系，北京 100084

自主式水下機器人(Autonomous Underwater Vehicle，AUV)在過去20年中得到迅速發(fā)展，逐步進入主流的海洋探測工具行列，由于海洋環(huán)境的特殊性，常用于陸地和空中的一些傳感器(例如GPS)不能在水中使用；海洋環(huán)境本身的不定性和非結構化，使聲納等距離傳感器的分辨率較低，所以，實現AUV的精確導航仍是一項艱難的任務。近年來，研究者們開始對AUV運用自身攜帶的掃描成像聲納,配合平臺傳感器(慣性測量器件(Inertial Measurement Units，IMU)、羅盤等)和多普勒測速儀(Doppler Velocity Log，DVL)等在航行過程中自主地探索、識別周邊環(huán)境同時在線地實現自身定位等問題進行研究。上述導航定位方法就是機器人領域研究熱點—同時定位與地圖構建方法(Simultaneous Localization and Mapping，SLAM)。SLAM理論被研究者們一致認為是移動機器人在未知環(huán)境中實現真正自主導航的主要途徑。目前，SLAM理論大多應用在室內、室外和空中環(huán)境，應用于水下環(huán)境的并不多。

以往對水下潛器導航技術進行綜述的文獻[1-3]中，大多包括了對現有的水下導航技術(船位推算導航、慣性導航、水下聲學導航、視覺導航和地球物理導航方法)的全部綜述，SLAM技術只作為其中的一小部分，敘述簡略。文獻[4-5]則是對SLAM理論進行了總體論述，并沒有指明具體的應用環(huán)境和應用對象。文獻[6]雖然是針對水下SLAM的綜述，但只介紹了AUV-SLAM的幾種常用方法，其他方面并沒涉及，到目前為止，尚沒有文獻對AUV-SLAM問題進行全面綜述。近幾年隨著水下傳感器技術的進步，應用于水下SLAM問題的研究也有了新進展，因此，本文主要對SLAM導航方法在AUV上的應用進行綜述，主要內容包括國內外的研究現狀、水下SLAM的主要實現方法和存在的難點，并探討了AUV-SLAM未來的發(fā)展趨勢。

1 AUV-SLAM的研究現狀

水下機器人的導航問題是許多國家的研究熱點，但研究用SLAM方法進行AUV導航定位研究的國家并不多，國外的研究機構主要集中在美國、西班牙、澳大利亞及英國等國家。

美國的MIT學院是以John Leonard為首的海洋機器人(Marine Robotics Group)研究團隊，他們利用第三代Odyssey AUVs，并與MIT的水聲學團隊和海格蘭特學院合作，主要側重于水下SLAM的應用研究。他們提出了唯距離SLAM[7]，常時間SLAM[8]和遞增平滑與構圖(Increamental Smooth and Mapping)[9]和視覺SLAM[10-11]等眾多理論。Leonard首次將EKF-SLAM運用到基于特征的水下目標識別中，另外，該團隊也注重SLAM理論在多水下機器人協作定位和船艙檢測方面的研究。

澳大利亞悉尼大學的菲爾德機器人研究中心是世界上著名的水下SLAM研究機構。Hugh Durrant-Whyte是SLAM理論的奠基人，他的團隊利用Oberon AUV，主要開展對經典的EKF-SLAM理論的研究。在包含人造路標(位置已知)的游泳池內測試了基于幾何投影濾波器(Geometrical Projection Filter，GPF)的SLAM算法[12]。

西班牙的Girona大學計算機視覺和機器人組主要側重于對水下結構性環(huán)境中SLAM方法的研究。西班牙Zaragoza大學的José Neira教授，是聯合相容分支定界算法(Joint Compatibility Branch and Bound，JCBB)的首次提出者[13]。他的學生David Ribas用Ictineu AUV在人造結構性環(huán)境中使用機械掃描成像聲納(Mechanical Scanned Imaging Sonar，MSIS)獲取數據進行導航定位研究，還使用立體照相機用EKF-SLAM算法對海底地形進行了實地勘探[14]。David Ribas給出了著名的西班牙廢棄碼頭的數據集，為后來許多研究水下結構性SLAM算法的學者提供了非常大的幫助。他們合作出版了水下SLAM方面的著作[15]。

中國開展AUV-SLAM研究的高校和研究所也寥寥無幾，主要有哈爾濱工程大學、華中科技大學和中國海洋大學等。

表1列出了國內外的主要研究機構、主要研究方向和所用的AUV。

表1 研究AUV-SLAM技術的主要國家和組織機構

2 應用在AUV導航上的主要SLAM方法

SLAM方法大致可分為2類：1)基于貝葉斯估計理論，基礎是貝葉斯法則和馬爾科夫假設的概率SLAM。目前應用于AUV上的這類SLAM主要是基于擴展卡爾曼濾波器(Extended Kalman Filter，EKF)的EKF-SLAM和基于粒子濾波器(Particle Filter，PF)的FastSLAM；2)應用在AUV上的是非概率SLAM，主要是graph-SLAM。

另一種分類是根據對環(huán)境地圖的描述[1]，分為基于特征的SLAM和基于視覺的SLAM。在基于特征的SLAM中，特征由外部傳感器測得，AUV每運動一步，都會把新的位姿和新測得的特征加入到狀態(tài)向量中。非結構的自然水下環(huán)境不適合基于特征的SLAM，海底的山峰和底谷這些特征在測量時對視角比較敏感，較難辨識和可靠的建模。而基于視覺的SLAM，AUV每運動一步，并不提取任何特征，只是把當前看到的視圖和上一次看到的視圖作比較。狀態(tài)向量中可以包括一步或多步的位姿。圖1(b)中，在位姿P3時，用V3和V2對比來發(fā)現視角的變化。

圖1 a) 基于特征的SLAM； b)基于視覺的SLAM

2.1 EKF-SLAM

由于AUV的運動模型與觀測模型是非線性的，因此常采用基于EKF的EKF-SLAM。EKF-SLAM的計算分為4個過程[16]：預測、觀測更新、數據關聯和狀態(tài)擴維，其具體的運算過程不在本文贅述，有興趣的讀者可參閱文獻[16]。

文獻[15]論述了EKF-SLAM理論在水下人造環(huán)境(港口、船塢、海事平臺及大壩等)的應用，依靠機械掃描成像聲納數據和Hough變換理論來提取線特征，產生了一幅基于特征的2D地圖。數據相關使用的是典型的最近鄰法則，并引入了局部地圖的概念。文獻[17]論述了EKF-SLAM理論在非結構的淺水中利用前向聲納提取人造點特征，使用GPS提供真實數據，使用DVL和FOG進行航位推算。在淺水區(qū)域，圖像聲納會受到由地面折射、水面折射和反射引起的噪聲的嚴重影響，這些影響會使計算能力有限的AUV得不到魯棒的圖像特征和數據相關。當AUV提取到魯棒的圖像特征時，EKF-SLAM可以有效地糾正由航位推算帶來的誤差。文獻[18]給出了一種選擇性子地圖連接SLAM，論述了EKF-SLAM理論在非結構環(huán)境中利用側掃聲納提取3D的點特征，2個子地圖之間的數據相關使用了JCBB算法，并用來自REMUS-100 AUV上獲得的真實數據進行實驗證明，選擇性的子地圖SLAM可以降低地圖的計算代價并保證在大尺度下的構圖一致性，且每個子地圖所包括的路標數在10～15個之間時所需的計算代價最少，當2個子地圖之間共享的路標數為子地圖路標數的50%時，可將2個子地圖進行合并。

表2列出了3種比較典型的用不同傳感器測量路標時使用EKF-SLAM為AUV導航定位的情況。

表2 典型的EKF-SLAM導航在AUV中的應用

由表2可以看出，EKF-SLAM通常用在結構性的環(huán)境中。在非結構的環(huán)境中，必須有顯著的點特征或人工加入點特征才能使用。EKF-SLAM繼承了EKF算法的優(yōu)缺點，算法原理簡單，易于實現，但由于系統(tǒng)非線性化引起的誤差可能會使算法不收斂，且計算代價大。

2.2 基于粒子濾波器的FastSLAM

FastSLAM的基礎是PF[19]，PF的核心思想是利用一系列隨機抽取的樣本和樣本的權重表示狀態(tài)的后驗概率分布，當樣本的個數足夠多時，通過這樣的隨機抽樣，可以很好地近似真實的后驗分布。

FastSLAM算法把聯合SLAM狀態(tài)分成運動部分和條件地圖部分以縮小采樣空間，機器人的位姿用不同權值的粒子表示，地圖用獨立的高斯分布解析表示，機器人位姿狀態(tài)的遞歸估計采用PF方法，地圖狀態(tài)的遞歸估計采用EKF方法。而且在給定機器人位姿的條件下，路標之間是相互獨立的。

FastSLAM算法的一般包括4個步驟： 1)對每一個粒子，從提議分布中采樣； 2)計算重要性權重； 3)進行將權重低的粒子由權重高的粒子來代替的重采樣過程； 4)對每一個粒子，在已知機器人位姿的條件下通過測量模型對觀測的路標進行EKF更新。

FastSLAM1.0算法和2.0算法的區(qū)別在于所選的提議分布模型不同。前者用運動模型當作提議分布進行采樣，而后者的提議分布包括了當前的觀測。

2.0算法的重采樣過程會帶來“粒子耗盡”問題。文獻[20]對2.0算法進行了改進，采用單個粒子最大似然算法和改進的“反面證據”相結合的數據關聯方法；采用基于等級的重采樣方法克服粒子退化問題。利用C-Range AUV真實數據驗證了改進FastSLAM算法比標準FastSLAM算法有著更好的準確性和一致性。文獻[21]利用AUV DEPTHX在墨西哥的薩卡通灰?guī)r洞進行了試驗，驗證了FastSLAM方法的有效性。

FastSLAM算法用樹結構表示路標位置的不確定性，并在此基礎上完成采樣集合的更新處理，最終算法復雜度相比于EKF-SLAM顯著降低。其缺點是存在著粒子“蛻化”問題，而使用重采樣又不可避免地帶來樣本枯竭問題，同時用于生成粒子的建議分布函數即重要性密度函數難以找到一個通用的選擇標準。

2.3 Graph-SLAM

Graph-SLAM處理數據的方式和濾波的方法不同，它不是在線糾正位姿，而是把所有數據記下來，進行離線處理。在Graph-SLAM中，機器人的位姿是一個節(jié)點或頂點，位姿之間的關系構成邊。所以Graph-SLAM問題分解成2個任務[22]：

1)構建圖

機器人位姿當作頂點，位姿間關系當作邊，這一步常常被稱為前端，往往是傳感器信息的堆積。

圖2 基于圖優(yōu)化的 SLAM 框架

2)優(yōu)化圖

調整機器人位姿頂點盡量滿足邊的約束，這一步稱為后端。

圖3 GraphSLAM的模型

Graph-SLAM的目的就是建立好圖以后，采用非線性最優(yōu)算法求解機器人位姿，使得誤差平方函數最小。

文獻[23]提出了一種新的NAPS(Negative and Positive Scoring)數據算法，實驗證明該算法比JCBB匹配性能好。在使用Graph-SLAM時為避免不斷增加的圖形帶來的計算復雜度，每隔一定的時間周期對前面產生的節(jié)點進行邊緣化處理，只保留1個節(jié)點。如圖4所示，當AUV往前運動時，節(jié)點和觀測的特征在每一次迭代后加入到圖形中，形成稠密的路徑，這種稠密的路徑很大。因此，邊緣化一部分節(jié)點最終形成綠色的綜合觀測作為Graph-SLAM的輸入。時間周期與AUV的運動速度和觀測到的特征之間的距離有關。文獻[24]使用前視的雙頻辨識聲納提取圖像數據，并利用NDT(Normal Distribution Transform,NDT)算法對數據進行成對注冊，該算法可以給出緊湊的掃描圖像而無需獲得點與點之間的對應關系。

圖4 Graph-SLAM計算時的數據綜合測量

基于圖優(yōu)化的方法被認為太耗費時間，無法滿足SLAM的實時性要求，但隨著高效求解方法的出現，基于圖優(yōu)化的方法重新得到重視．在經歷一個復興后，其成為當前SLAM研究的一個熱點。

上述3種算法，根據能處理的路標數量、可達區(qū)域、計算需求和靈活度等特點[6]，總結在表3中。表中m表示特征數量，N表示采樣的粒子數。

表3 3種AUV-SLAM算法對比

3 AUV-SLAM存在的難點問題

目前，理論上比較成熟的水下SLAM方法主要限定在結構化的人工環(huán)境或小尺度的淺海中，若要擴大航程或實現更大尺度自然水下環(huán)境中的實時導航定位，首先要解決的是隨著特征數量的增加造成的地圖過大、計算復雜問題；其次，要提高SLAM運行時的魯棒性，觀測特征和地圖中的已有特征必須要有正確的數據關聯。這2個問題也是普遍存在于整個SLAM研究領域中的2大難點。

3.1 計算復雜度

子地圖方法是目前機器人領域學者們解決大尺度SLAM計算復雜度問題的主要思路[25]。分離隨機地圖構建(Decoupled Stochastic Mapping，DSM)方法是一種高效的大尺度SLAM解決方法[26]，它將環(huán)境分成很多個有重疊區(qū)域的子地圖，降低傳統(tǒng)隨機地圖構建算法的計算壓力。DSM方法雖然可以極大降低SLAM算法的計算復雜度，卻切斷了地圖間的關聯關系，因此獲得的地圖都有可能不一致。Williams等人提出的約束局部子地圖濾波器[27]提出構建相對于局部參考坐標系的隨機地圖，并且確保局部地圖在統(tǒng)計上是完全獨立的。由于大部分更新運算都在各局部子地圖內進行，因此能改善計算復雜度問題，但不能保證全局一致性。Atlas[28]在計算時間上有所改善，但在本質上是一種逼近方法，通過犧牲估計精度換取計算復雜度的降低。條件獨立局部地圖方法[29]通過分享連續(xù)子地圖間環(huán)境特征及機器人狀態(tài)等信息建立最終全局地圖，而且最終地圖中不會引入不必要的近似計算。但該方法需要根據先驗知識對每一幅新的子地圖進行初始化。

3.2 數據關聯

在基于特征的SLAM算法中，數據關聯是一個最為關鍵也最為困難的問題，特別是在水下復雜多變的環(huán)境中，可靠的數據關聯顯得尤為重要。數據關聯是對2個環(huán)境特征的檢驗進行匹配，確定它們是否對應環(huán)境中的同一個特征。水下SLAM問題中，常用的數據關聯算法有獨立兼容最近鄰算法(Individual Compatibility Nearest Neighbor，ICNN)、聯合相容分支定界算法(Joint Compatibility Branch and Bound，JCBB)和多假設跟蹤算法(Multi-Hypothesis Tracker，MHT)。

3.2.1 ICNN算法

ICNN算法[30]是一種具有固定記憶并能工作于多回波環(huán)境的關聯方法，該方法將特征預測位置和有效回波間的統(tǒng)計距離即馬氏距離作為度量對象，在落入檢驗門之內的觀測值中“唯一性”選取與被觀測目標的預測位置間統(tǒng)計距離最小的觀測值作為關聯對象。

ICNN算法概念簡單，計算復雜度低，許多文獻中都采用該方法。當AUV位姿不確定性相對于特征間距足夠小或是存在較少雜波時，ICNN算法可以表現出較好的工作性能，AUV位姿估計的精度也會較高，在工程上有廣泛的應用。但是當AUV處在復雜多變的水下環(huán)境中，ICNN的性能急劇下降，不正確的特征匹配會使整個SLAM算法失敗。所以ICNN僅適用于特征稀疏和結構性的環(huán)境中。

3.2.2 JCBB算法

Neira等人[13]在NN數據關聯的基礎上，將單個觀測、馬氏距離和檢驗門限結合起來，提出了能夠解決一組觀測數據的關聯問題的聯合相容性檢驗(Joint Compatibility Test，JCT)數據關聯方法。其基本思想是對同一時刻獲得的一幀觀測中所有觀測值和全部環(huán)境特征間關聯假設的聯合相容性進行檢驗，從中選取相容性最大的關聯假設作為關聯結果。由于算法在最優(yōu)關聯假設的搜索過程中使用了分支界定(Branch and Bound，BB)法，所以也被稱為聯合相容性分支界定關聯。

JCBB算法通過縱向和橫向遍歷搜索測量—地圖特征空間組成的解釋樹尋找最大的聯合關聯數據集，是一種約束性更強的數據關聯算法，關聯結果正確率很高。但一方面在噪聲或雜波對觀測影響較大時，特征的實際觀測值和估計值間可能存在較大偏差，過為嚴格的關聯假設判斷條件可能會丟棄正確的關聯假設；另一方面其計算復雜度隨環(huán)境特征個數呈指數型增長，因此不適用于大尺度環(huán)境。許多學者一直在ICNN和JCBB的基礎上進行改進，以得到更合適的水下SLAM數據關聯方法。

3.2.3 MHT算法

MHT算法[31]通過一個有限長度的時間滑窗建立多個候選關聯假設，通過假設產生、假設概率計算和假設管理技術實現多目標的跟蹤。MHT的魯棒性強，特別適于大尺度的復雜環(huán)境[5]。但由于該算法中可行關聯假設的個數隨著路標個數和測量個數的增加呈指數增長，由此帶來存儲量和計算量的迅速增長。所以在大規(guī)模關聯問題中，該方法的使用受到了極大的限制，一般用于稀疏和局部子地圖中。

4 發(fā)展趨勢

AUV-SLAM的發(fā)展已經歷了十多年，但目前的研究結果基本局限于人工結構、小尺度的淺海和深海的局部區(qū)域的水下環(huán)境，且大多數為理論成果，工程實現的很少。將來，AUV-SLAM主要以大尺度、自然環(huán)境的實用化智能化發(fā)展為重點，具體表現在：

1)隨著AUV可利用能源的不斷發(fā)展進步，結合圖像處理和模式識別的最新進展，使用雙目立體視覺傳感器獲得更多更準確3D環(huán)境特征的水下Visual-SLAM將成為該領域實現水下精確導航的重點研究方向[25,32]；

2)基于隨機有限集理論(Random Finite Set,RFS)的RFS-SLAM[33]，將各時刻機器人位姿、傳感器觀測信息以及環(huán)境地圖表示為隨機有限集，能更好地描述觀測噪聲、視場、檢測概率及虛警等多種傳感器信息，避免了數據關聯過程，在模型建立上更加準確，能實現機器人位姿和地圖狀態(tài)的同時估計。對復雜多變、噪聲未知的水下環(huán)境非常適用，具有較大的發(fā)展?jié)摿Γ瑢⒊蔀槲磥鞟UV-SLAM問題的一個研究熱點；

3)多個AUV進行的協同導航(Cooperative SLAM，C-SLAM)是未來解決水下中間層區(qū)域AUV導航定位的重要方法。隨著水下通信技術的飛速發(fā)展，多個AUV之間將通過共享傳感器信息和各自的位姿信息，利用SLAM進行協同導航。與單AUV相比，通過AUV之間的相互協調與合作以及信息共享，C-SLAM可以提高地圖創(chuàng)建的效率，提高定位與地圖的精度，使多個AUV都具有誤差有界能力。2015年，MIT的J.J.Leonard團隊首次實現了在低帶寬通信條件下的多AUV C-SLAM算法[34]。針對特征稀疏的大尺度水下環(huán)境的開發(fā)和利用，協同的AUV C-SLAM必將是各國重點發(fā)展的水下導航技術；

4)擴展AUV-SLAM的應用環(huán)境一直是水下機器人導航的研究熱點。將目前局限于2D靜態(tài)水下結構性環(huán)境中的研究與應用擴展到與自然非結構環(huán)境契合的動態(tài)環(huán)境，結合平滑子地圖技術構建復雜水下環(huán)境的精確3D地圖，解決水下檢測任務中的避碰和精確對接任務[35]。

參 考 文 獻

[1] Liam Paull, Sajad Saeedi, Mae Seto et al. AUV Navigation and Localization-a Review[J]. IEEE Journal of Oceanic Engineering,2014,39(1):131-149.

[2] P.Miller, J.Farrell, Y.Zhao et al. Autonomous Underwater Vehicle Navigation[J]. IEEE Journal of Oceanic Engineering,2010,35(3):663-678.

[3] L.Stutters,H.liu,C.Tiltman et al. Navigation Technologies for Autonomous Underwater Vehicle[J]. System, man, Cybernetics, Part C:Application and Reviews, IEEE Transactions on,2008，38(4): 581-589.

[4] H.Durrant-Whyte, T.Bailey. Simultaneous Localization and Mapping (SLAM):Part I The Essential Algorithms [J]. IEEE Robotics & Automation Magazine,2006,13(2): 99-110.

[5] T.Baile, H.Durrant-Whyte. Simultaneous Localization and Mapping(SLAM):Part II State of the art[J]. IEEE Robotics & Automation Magazine, 2006,13(3):108-117.

[6] F.Hidalgo， T. Braunl. Review of Underwater SLAM Techniques[C]// Proceedings of the 6th International Conference on Automation, Robotics and Applications. Queenstown, New Zealand,2015:306-311.

[7] P.Newman, J.J.Leonard. Pure Range-only Sub-sea SLAM[C]// IEEE International Conference on Robotics and Automation (ICRA), Taipei, Taiwan ,2003:1921-1926.

[8] P.M.Newman, J.J.Leonard, R.J.Rikoski. Towards Constant-time SLAM on an Autonomous Underwater Vehicle Using Synthetic Aperture Sonar[C]// Proceedings of the 11th International Symposium on Robotics Research, Sienna, Italy , 2003:409-420.

[9] M.Kaess, H.Johannsson, R.Roberts et al. iSAM2: Incremental Smoothing and Mapping Using the Bayes Tree[J]. International Journal of Robotics Research,2012,31(2), 216-235.

[10] R.M.Eustice, H.Singh, J.J.Leonard, et al. Visually Mapping the RMS Titanic: Conservative Covariance Estimates for SLAM Information Filters[J]. International Journal of Robotics Research, 2006,25(12), 1223-1242.

[11] O.Pizarro, R.M.Eustice, H.Singh. Large area 3d Reconstructions from Underwater Optical Surveys[J]. IEEE Journal of Oceanic Engineering, 2009,34(20), 150-169.

[12] P.Newman. On the Structure and Solution of the Simultaneous Localization and Map Building Problem[D]. Australian, University of Sydney, 1999.

[13] J.Neira, J.D.Tardos. Data Association in Stochastic Mapping Using the Joint Compatibility Test[J]. IEEE Transactions on Robotics and Automation, 2001:17(6):890-897.

[14] D.Ribas, P.Ridao, J.D.Tardos et a1．Visual SLAM for 3D Large-Scale Seabed Acquisition Employing Underwater Vehicles[C]//IEEE/RSJ Intemational Conference on Intelligent Robots and Systems，Nice，France,2008：1011-1016.

[15] D. Ribas， P. Ridao，J. Neira. Underwater SLAM for Structured Environments.Using an Imaging Sonar[M]. Springer Tracts in Advanced Robotics, B. Siciliano, O. Khatib, and F. Groen, Eds. Heidelberg, Alemania: Springer Verlag, August 2010, no. 65.

[16] I.Tena. Enhanced Concurrent Mapping and Localization Using Forward-looking Sonar[D].England, Heriot-Watt University, 2001.

[17] ACT.Koh, WS.Wijesoma, SL. Pua et al. Shallow waters SLAM Experiments on Meredith AUV Using Forward Looking Sonar[C]//.IEEE Oceans,Biloxi,MS,2009:1-6.

[18] J.Aulinas, X.Lladó, X.Lladó et al. Selective Submap Joining for Underwater Large Scale 6-DOF SLAM[C]//IEEE International Conference on Intelligent Robots & System，2010,6219(1):2552-2557.

[19] M.Montemerlo, S.Thrun, D.Roller et al．FastSLAM 2.0:An Improved Particle Filtering Algorithm for Simultaneous Localization and Mapping that Provably Converges[C]//Proceedings of the International Conference on Artificial Intelligence. Califomia, CA,USA, 2003:115l-1156.

[20] Bo He, Yan Liang, Xiao Feng et al. AUV SLAM and Experiments Using a Mechanical Scanning Forward-Looking Sonar[J].Sensors,2012,12(7):9386-9410.

[21] N.Fairfield, G.Kantor, D.Wettergreen. Towards Particle Filter SLAM with Three Dimensional Evidence Grids in a Flooded Subterranean Environment[C]//IEEE International Conference on Robotics and Automatio. Orlando, Florida 2006:3575-3580.

[22] G.Grisetti, R.Kummerle, C.Stachniss et al. A Tutorial on Graph-based SLAM[J]．IEEE Intelligent Transportation Systems Magazine, 2010,2(4):31-43.

[23] M.F.Fallon, J.Folkesson, H.McClelland et al. Relocating Underwater Features Autonomously Using Sonar-based SLAM[J]. IEEE Journal of Oceanic Engineering, 2013,38(3):500-513.

[24] H. Johannsson, M. Kaess, B. Englot et al. Imaging Sonar-aided Navigation for Autonomous Underwater Harbor Surveillance[C]//IEEE/RSJ International Conference on Intelligent Robots and Systems,2013:4396-4403.

[25] 張書景.大尺度環(huán)境中自主式水下機器人同時定位與地圖構建算法研究[D].青島，中國海洋大學，2014.(Zhang Shujing, Simultaneous Localization and Mapping for AUVs in Large Scale Environments [D].Qingdao,Chinese Marine University.)

[26] J.J.Leonard, H.J.S.Feder. Decoupled Stochastic Mapping for Mobile Robot & Auv Navigation[J]. IEEE Journal of Oceanic Engineering, 2001,26(4): 561-571.

[27] S.B.Williams, G.Dissanayake, H.Durrant-Whyte. An Efficient Approach to the Simultaneous Localization and Mapping Problem[C]//IEEE International Conference on Robotics and Automation (ICRA), Washington, DC, USA 2002:406-411.

[28] M.Bosse, P.Newman, J.J.Leonard, et al. Simultaneous Localization and Map Building in Large-scale Cyclic Environments Using the Atlas Framework[J]// International Journal of Robotics Research, 2004,23(12):1113-1139.

[29] P.Piniés，J.D.Tardós. Large-scale Slam Building Conditionally Independent Local Maps: Application to Monocular Vision[J]. IEEE Transactions on Robotics,2008, 24(5):1094-1106.

[30] Y.Bar-Shalom, T.E.Fortmann. Tracking and Data Association[M].Boston, MA: Academic, 1988.

[31] I.J.Cox, J.J.Leonard. Modeling a Dynamic Environment Using a Bayesian Multiple Hypothesis Approach[J]. Artificial Intelligence, 1994,66(2):311-344.

[32] F. Bonin-Font, A. Cosic, P. Lluis Negre et al. Stereo SLAM for Robust Dense 3D Reconstruction of Underwater Environments[C]// IEEE Oceans, Genoa,2015:1-6.

[33] J.Mullane, BN.Vo, M.D.Adams et al. A Random-Finite-Set Approach to Bayesian SLAM[J]. IEEE Transactions on Robotics,2011,27(2):268-282.

[34] L.Paull, Guoquan Huang, M.Seto et al. Communication-constrained Multi-AUV Cooperative SLAM[C]// IEEE International Conference on Robotics and Automation (ICRA),Seattle, Washington, 2015:26-30.

[35] M.VanMiddlesworth, M.Kaess, F.Hover et al. Mapping 3D Underwater Environments with Smoothed Submaps[J]. Springer Tracts in Advanced Robotics,2015,105:17-30.