基于同時定位與地圖構(gòu)建的AUV導(dǎo)航定位方法綜述*

聶永芳 潘 爽 孫東平 施建禮

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

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

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

1 AUV-SLAM的研究現(xiàn)狀

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

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

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

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

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

表1列出了國內(nèi)外的主要研究機構(gòu)、主要研究方向和所用的AUV。

表1 研究AUV-SLAM技術(shù)的主要國家和組織機構(gòu)

2 應(yīng)用在AUV導(dǎo)航上的主要SLAM方法

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

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

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

2.1 EKF-SLAM

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

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

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

表2 典型的EKF-SLAM導(dǎo)航在AUV中的應(yīng)用

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

2.2 基于粒子濾波器的FastSLAM

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

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

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

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

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

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

2.3 Graph-SLAM

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

1)構(gòu)建圖

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

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

2)優(yōu)化圖

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

圖3 GraphSLAM的模型

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

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

圖4 Graph-SLAM計算時的數(shù)據(jù)綜合測量

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

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

表3 3種AUV-SLAM算法對比

3 AUV-SLAM存在的難點問題

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

3.1 計算復(fù)雜度

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

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

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

3.2.1 ICNN算法

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

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

3.2.2 JCBB算法

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

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

3.2.3 MHT算法

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

4 發(fā)展趨勢

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

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

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

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

4)擴展AUV-SLAM的應(yīng)用環(huán)境一直是水下機器人導(dǎo)航的研究熱點。將目前局限于2D靜態(tài)水下結(jié)構(gòu)性環(huán)境中的研究與應(yīng)用擴展到與自然非結(jié)構(gòu)環(huán)境契合的動態(tài)環(huán)境，結(jié)合平滑子地圖技術(shù)構(gòu)建復(fù)雜水下環(huán)境的精確3D地圖，解決水下檢測任務(wù)中的避碰和精確對接任務(wù)[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，F(xiàn)rance,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)境中自主式水下機器人同時定位與地圖構(gòu)建算法研究[D].青島，中國海洋大學(xué)，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.