如何实现机器人在未知环境中的定位、建图及移动？

1、前言

隨著計算機技術(shù)的迅速發(fā)展，機器人研究的深入以及人們對機器人需求的擴大，能自主導(dǎo)航與智能移動的機器人成為研究的熱點和重點。

對于已知環(huán)境中的機器人自主定位和已知機器人位置的地圖創(chuàng)建已經(jīng)有了一些實用的解決方法。然而在很多環(huán)境中，機器人不能利用全局定位系統(tǒng)進行定位，而且事先獲取機器人工作環(huán)境的地圖也比較有難度。這時機器人需要在自身位置不確定的條件下，在完全未知環(huán)境中創(chuàng)建地圖,同時利用地圖進行自主定位和導(dǎo)航。而SLAM（同步定位與地圖構(gòu)建）技術(shù)，即是被認為目前實現(xiàn)真正全自主移動機器人的核心和關(guān)鍵技術(shù)。

機器人在未知環(huán)境中從一個未知位置開始移動，在移動過程中根據(jù)位置估計和傳感器數(shù)據(jù)進行自身定位，同時逐步完善和構(gòu)建完整地圖，這就是一個SLAM的過程。在SLAM中，機器人利用自身攜帶的傳感器識別未知環(huán)境中的特征標志，然后根據(jù)機器人與特征標志之間的相對位置和里程計的讀數(shù)估計機器人和特征標志的全局坐標。這種在線的定位與地圖創(chuàng)建需要保持機器人與特征標志之間的詳細信息。近幾年來，SLAM的研究取得了很大的進展，并已應(yīng)用于各種不同的環(huán)境，如：機器人、AR、VR、無人機、自動駕駛等。

2、SLAM的關(guān)鍵性問題

2.1地圖的表示方式

目前，常見的地圖表示方法大致可分為三類：柵格表示、幾何信息表示和拓撲圖表示，每種方法都有自己的優(yōu)缺點。

(1)柵格地圖

柵格地圖即是將整個環(huán)境分為若干相同大小的柵格，對于每個柵格各指出其中是否存在障礙物。

優(yōu)點：

創(chuàng)建和維護容易，盡量的保留了整個環(huán)境的各種信息；

借助于該地圖，可以方便地進行自定位和路徑規(guī)劃；

缺點：

當(dāng)柵格數(shù)量增大時(在大規(guī)模環(huán)境或?qū)Νh(huán)境劃分比較詳細時)，對地圖的維護行為將變得困難，同時定位過程中搜索空間很大，如果沒有較好的簡化算法，實現(xiàn)實時應(yīng)用比較困難。

（2）幾何信息地圖

幾何信息地圖表示法是指機器人收集對環(huán)境的感知信息，從中提取更為抽象的幾何特征，例如線段或曲線，使用這些幾何信息描述環(huán)境。

優(yōu)點：

更為緊湊，且便于位置估計和目標識別；

幾何方法利用卡爾曼濾波在局部區(qū)域內(nèi)可獲得較高精度，且計算量??；

缺點：

幾何信息的提取需要對感知信息作額外處理，且需要一定數(shù)量的感知數(shù)據(jù)才能得到結(jié)果；

在廣域環(huán)境中難以維持精確的坐標信息；

（3）拓撲地圖

拓撲地圖抽象度高，特別在環(huán)境大而簡單時。這種方法將環(huán)境表示為一張拓撲意義中的圖(graph)，圖中的節(jié)點對應(yīng)于環(huán)境中的一個特征狀態(tài)、地點。如果節(jié)點間存在直接連接的路徑則相當(dāng)于圖中連接節(jié)點的弧。

優(yōu)點：

有利于進一步的路徑和任務(wù)規(guī)劃；

存儲和搜索空間都比較小，計算效率高；

可以使用很多現(xiàn)有成熟、高效的搜索和推理算法；

缺點：

使用時要建立在對拓撲節(jié)點的識別匹配基礎(chǔ)上的，如當(dāng)環(huán)境中存在兩個很相似的地方時，拓撲圖方法將很難確定這是否為同一點；

2.2不確定信息的描述

在環(huán)境完全未知的情況下，機器人想要建圖并行走，必須借助其他傳感器得到信息，如里程計、聲納、激光測距儀、視覺等。由于傳感器自身的限制，感知信息存在不同程度的不確定性，例如激光測距儀的不確定性主要來自距離的測量誤差以及反光鏡旋轉(zhuǎn)和激光散射引起的測量角誤差。

如上圖所示，感知信息的不確定性必然導(dǎo)致所構(gòu)建的環(huán)境模型也不可能是完全精確的，同樣，當(dāng)依靠模型和感知進行決策時也帶有不確定性，即不確定性具有傳遞性。

對不確定性進行度量的方法主要有概率度量、信任度量、可能性度量、模糊度量和證據(jù)理論等。目前，在AMR地圖構(gòu)建中使用較多的是概率度量和模糊度量。概率度量主要存在兩種形式：

(1)以均值、方差和協(xié)方差等概率特征來描述不確定信息。這種度量方法的優(yōu)點是均值等概率特征具有明確的幾何意義，缺點是概率特征的離散計算公式還沒有確定的形式；

(2)以概率模型來描述不確定信息，主要采用Bayes法則與Markov假設(shè)。這種度量方法的優(yōu)點是以隨機概率模型描述機器人的位姿和環(huán)境信息，魯棒性非常好，缺點是概率模型的計算量非常大而且必須事先知道模型的先驗概率，給實際應(yīng)用造成了困難。

2.3定位與環(huán)境特征提取

移動機器人自定位與環(huán)境建模問題是緊密相關(guān)的。環(huán)境模型的準確性依賴于定位精度，而定位的實現(xiàn)又離不開環(huán)境模型。在未知環(huán)境中，機器人沒有什么參照物，只能依靠自己并不十分準確的傳感器來獲取外界信息，如同一個盲人在一個陌生環(huán)境中摸索的情況。這種情況下，定位是比較困難的。有地圖的定位和有定位的地圖創(chuàng)建都是容易解決的，但無地圖的定位和未解決定位的地圖創(chuàng)建如同"雞--蛋"問題，無從下手。

已有的研究中對這類問題的解決方法可分為兩類：

（1）利用自身攜帶的多種內(nèi)部傳感器（包括里程儀、羅盤、加速度計等），通過多種傳感信息的融合減少定位的誤差，使用的融合算法多為基于卡爾曼濾波的方法。這類方法由于沒有參考外部信息，在長時間的漫游后誤差的積累會比較大。

（2）在依靠內(nèi)部傳感器估計自身運動的同時，使用外部傳感器（如激光測距儀、視覺等）感知環(huán)境，對獲得的信息進行分析提取環(huán)境特征并保存，在下一步通過對環(huán)境特征的比較對自身位置進行校正。但這種方法依賴于能夠取得環(huán)境特征。

環(huán)境特征提取的方法有：

1）.Hough transform是一類基于灰度圖檢測直線和其他曲線的方法。該方法需要一簇能被搜索的預(yù)先準備的特定曲線，并根據(jù)顯示的灰度圖中一簇曲線產(chǎn)生曲線參數(shù)。

2）.Clustering分析是一種數(shù)據(jù)探測工具，對于未分類樣例是有效的，同時，它的目標就是把所針對對象分組成自然類別或基于相似性或距離的簇類。在被提取對象類別未知的情況中，簇技術(shù)是一類比HoughTransform更有效的技術(shù)。簇類應(yīng)是以“凝聚”為中心，而不是支離破碎的、不相交的。而環(huán)境特征有時是很難提取出的，例如：環(huán)境特征不夠明顯時或者傳感器信息比較少，難以從一次感知信息中獲得環(huán)境特征。

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

數(shù)據(jù)關(guān)聯(lián)是對兩個特征標志進行匹配，確定它們是否對應(yīng)環(huán)境中的同一物體。SLAM中的數(shù)據(jù)關(guān)聯(lián)主要需要完成三個任務(wù)：

（1）新特征標志的檢測；

（2）特征標志的匹配；

（3）地圖之間的匹配；

雖然在目標跟蹤、傳感融合等領(lǐng)域，數(shù)據(jù)關(guān)聯(lián)已經(jīng)得到較好的解決，但是這些方法的計算量大，不能滿足SLAM的實時性要求。實現(xiàn)m個標志與擁有n個標志的地圖之間的數(shù)據(jù)關(guān)聯(lián)的復(fù)雜度與m之間呈指數(shù)關(guān)系，假設(shè)每個觀測到的標志i有 個可能的匹配，那么對于m個標志需要在指數(shù)空間 = 中搜索正確的匹配。數(shù)據(jù)關(guān)聯(lián)的搜索空間與環(huán)境的復(fù)雜程度以及機器人的定位誤差有關(guān)，環(huán)境的復(fù)雜程度的增加會使m增大，而誤差的增大會使Ni 增大。

2.5累積誤差

SLAM中的誤差主要來自三個方面：

（1） 觀測誤差；

（2） 里程計的誤差；

（3） 錯誤的數(shù)據(jù)關(guān)聯(lián)帶來的誤差；

當(dāng)機器人在已知地圖的環(huán)境中進行定位時，機器人可以通過觀測位置已知的特征標志對里程計的誤差進行補償，每一次觀測使機器人的位置誤差趨向于觀測誤差與特征標志的位置誤差之和。

然而在SLAM中，由于機器人的位置和環(huán)境中的特征標志的位置都是未知的，觀測信息不能有效糾正里程計的誤差，機器人的位置誤差隨著機器人的運動距離而增大。而機器人的位置誤差的增大將導(dǎo)致錯誤的數(shù)據(jù)關(guān)聯(lián)，從而增大特征標志的位置誤差：反過來，特征標志的誤差又將增大機器人的位置誤差。因此，機器人的位置誤差與特征標志的位置誤差密切相關(guān)。它們之間的相互影響使機器人和特征標志的位置估計產(chǎn)生累計誤差，難以保證地圖的一致性。

3、SLAM的實現(xiàn)方法

目前SLAM方法大致可分為兩類：

（1）基于概率模型的方法：基于卡爾曼濾波的完全SLAM、壓縮濾波、FastSLAM等

（2）非概率模型方法：SM-SLAM、掃描匹配、數(shù)據(jù)融合(dataassociation)、基于模糊邏輯等。

3.1基于卡爾曼濾波器的實現(xiàn)方法

從統(tǒng)計學(xué)的觀點看，SLAM是一個濾波問題，也就是根據(jù)系統(tǒng)的初始狀態(tài)和從0到t時刻的觀測信息與控制信息（里程計的讀數(shù)）估計系統(tǒng)的當(dāng)前狀態(tài)。在SLAM中，系統(tǒng)的狀態(tài)，由機器人的位姿r和地圖信息m組成（包含各特征標志的位置信息）。假設(shè)系統(tǒng)的運動模型和觀測模型是帶高斯噪聲的線性模型，系統(tǒng)的狀態(tài) 服從高斯分布，那SLAM可以采用卡爾曼濾波器來實現(xiàn)?；诳柭鼮V波器的SLAM 包括系統(tǒng)狀態(tài)預(yù)測和更新兩步，同時還需要進行地圖信息的管理，如：新特征標志的加入與特征標志的刪除等。

卡爾曼濾波器假設(shè)系統(tǒng)是線性系統(tǒng)，但是實際中機器人的運動模型與觀測模型是非線性的。因此通常采用擴展卡爾曼濾波器（Extended Kalman Filter），擴展卡爾曼濾波器通過一階泰勒展開來近似表示非線性模型。另一種適用于非線性模型的卡爾曼濾波器是UKF（Unscented Kalman Filter），UKF采用條件高斯分布來近似后驗概率分布，與EKF相比，UKF的線性化精度更高，而且不需要計算雅可比矩陣。

卡爾曼濾波器已經(jīng)成為實現(xiàn)SLAM的基本方法。其協(xié)方差矩陣包含了機器人的位置和地圖的不確定信息。當(dāng)機器人連續(xù)地觀測環(huán)境中的特征標志時，協(xié)方差矩陣的任何子矩陣的行列式呈單調(diào)遞減。從理論上講，當(dāng)觀測次數(shù)趨向于無窮時，每個特征標志的協(xié)方差只與機器人起始位置的協(xié)方差有關(guān)?？柭鼮V波器的時間復(fù)雜度是O( )，由于每一時刻機器人只能觀測到少數(shù)的幾個特征標志，基于卡爾曼濾波器的SLAM的時間復(fù)雜度可以優(yōu)化為O( )，n表示地圖中的特征標志數(shù)。

3.2局部子地圖法

局部子地圖法從空間的角度將SLAM分解為一些較小的子問題。子地圖法中主要需要考慮以下幾個問題：

（1）如何劃分子地圖；

（2）如何表示子地圖間的相互關(guān)系；

（3）如何將子地圖的信息傳遞給全局地圖以及能否保證全局地圖的一致性；

最簡單局部子地圖方法是不考慮各子地圖之間的相互關(guān)系，將全局地圖劃分為包括固定特征標志數(shù)的獨立子地圖，在各子地圖中分別實現(xiàn)SLAM，這種方法的時間復(fù)雜度為O(1)。但是，由于丟失了表示不同子地圖之間相關(guān)關(guān)系的有用信息，這種方法不能保證地圖的全局一致性。

對此，Leonard 等人提出了DSM（DecoupledStochastic Mapping）方法，DSM中各子地圖分別保存自己的機器人位置估計，當(dāng)機器人從一個子地圖A進入另一個子地圖B時，采用基于EKF的方法來將子地圖A中的信息傳送給子地圖B；B.Williams等人提出了一種基于CLSF（ConstrainedLocal Submap Filter）的SLAM方法，CLSF在地圖中創(chuàng)建全局坐標已知的子地圖，機器人前進過程中只利用觀測信息更新機器人和局部子地圖中的特征標志的位置，并且按一定的時間間隔把局部子地圖信息傳送給全局地圖。

雖然實驗表明這兩種算法具有很好的性能，但是沒有從理論上證明它們能夠保持地圖的一致性。J.Guivant等人提出了一種沒有任何信息丟失的SLAM優(yōu)化算法CEKF(CompressedExtended Kalman Filter)。

CEKF將已經(jīng)觀測到的特征標志分為A與B部分，A表示與機器人當(dāng)前位置相鄰的區(qū)域，被稱為活動子地圖。當(dāng)機器人在活動子地圖A中運動時，利用觀測信息實時更新機器人的位置與子地圖A，并采用遞歸的方法記錄觀測信息對子地圖B的影響；當(dāng)機器人離開活動子地圖A時，將觀測信息無損失地傳送給子地圖B，一次性地實現(xiàn)子地圖B的更新，同時創(chuàng)建新的活動子地圖。

該方法的計算時間由兩部分組成：活動子地圖中的SLAM，其時間復(fù)雜度為O( )， 是活動子地圖A中特征標志的數(shù)目；子地圖B的更新，其時間復(fù)雜度為O( ) ， 是地圖B中特征標志的數(shù)目。當(dāng)子地圖合并的時間間隔較大時，CEKF能有效減少SLAM的計算量。

3.3去相關(guān)法

降低SLAM復(fù)雜度的另一種方法是將表示相關(guān)關(guān)系的協(xié)方差矩陣中一些取值較小的元素忽略掉，使其變?yōu)橐粋€稀疏矩陣。然而這也會因信息的丟失而使地圖失去一致性。但是，如果能改變協(xié)方差矩陣的表示方式，使其中的很多的元素接近于零或等于零，那么就可以將其安全地忽略了?；跀U展信息濾波器EIF（ExtendedInformation Filter）的SLAM就是出于這一思想。

EIF EKF的基于信息的表達形式，它們的區(qū)別在于表示信息的形式不一樣。EIF采用協(xié)方差矩陣的逆矩陣來表征SLAM中的不確定信息，并稱之為信息矩陣。兩個不相關(guān)的信息矩陣的融合可以簡單地表示為兩個矩陣相加。信息矩陣中每個非對角線上的元素表示機器人與特征標志之間或特征標志與特征標志之間的一種約束關(guān)系，這些約束關(guān)系可以通過系統(tǒng)狀態(tài)的信關(guān)系進行局部更新。這種局部更新使得信息矩陣近似于稀疏矩陣，對其進行稀疏化產(chǎn)生的誤差很小。

根據(jù)這一點，S.Thrun等人提出了一種基于稀疏信息濾波器SEIF(Sparse Extended InformationFilter)的SLAM方法，并證明利用稀疏的信息矩陣實現(xiàn)SLAM的時間復(fù)雜度是O(1)。雖然EIF可以有效降低SLAM的時間復(fù)雜度，但是在地圖信息的表示和管理方面還存在一些問題。首先，在常數(shù)時間內(nèi)只能近似算得系統(tǒng)狀態(tài)的均值；其次，在基于EIF 的SLAM 方法中，特征標志的增刪不方便。

3.4分解法（FastSLAM）

M.Montemerlo 等人提出了一種基于粒子濾波器(ParticleFilter) FastSLAM 方法。FastSLAM 將SLAM分解為機器人定位和特征標志的位置估計兩個過程。 粒子濾波器中的每個粒子代表機器人的一條可能運動路徑，利用觀測信息計算每個粒子的權(quán)重，以評價每條路徑的好壞。對于每個粒子來說，機器人的運動路徑是確定的，因此特征標志之間相互獨立，特征標志的觀測信息只與機器人的位姿有關(guān)，每個粒子可以采用n個卡爾曼濾波器分別估計地圖中n個特征的位置。假設(shè)需要k個粒子實現(xiàn)SLAM、FastSLAM，總共有kn個卡爾曼濾波器。FastSLAM的時間復(fù)雜度為O(kn)，通過利用樹型的數(shù)據(jù)結(jié)構(gòu)進行優(yōu)化，其時間復(fù)雜度可以達到O(klog n)。Fast2SLAM方法的另一個主要優(yōu)點是通過采用粒子濾波器估計機器人的位姿，可以很好地表示機器人的非線性、非高斯運動模型。

3.5基于多機器人協(xié)作的SLAM

一些研究者對基于多機器人協(xié)作的同時定位與地圖創(chuàng)建CSLAM（CooperativeSimultaneous Localizationand Mapping）進行了探討和研究。 與單機器人相比，通過機器人之間的相互協(xié)調(diào)與合作以及信息共享，CSLAM可以提高地圖創(chuàng)建的效率和提高定位與地圖的精度。

CSLAM按照地圖的存儲與處理方式的不同可以分為兩大類型：集中式CSLAM和分布式CSLAM。

在集中式CSLAM中，存在一個中央處理模塊，每個機器人分別在自己所在的局部地圖中進行定位與地圖創(chuàng)建，然后利用無線通信裝置將在局部地圖中獲得的信息傳送給中央模塊。這種方法通過子地圖的匹配，可以充分利用子地圖間的冗余信息提高定位與地圖創(chuàng)建的精度。但是，當(dāng)機器人數(shù)量增加時中央模塊的計算量會顯著增大，而且集中式的信息傳遞需要很大的帶寬；系統(tǒng)的可靠性也比較低，一旦中央模塊出現(xiàn)故障,整個系統(tǒng)都會陷入癱瘓狀態(tài)。

在分布式CSLAM中，不存在中央模塊，每個機器人都擁有自己的全局地圖，在每一時刻機器人把來自其他相鄰機器人的信息和自己的觀測信息融合到自己的全局地圖中，然后以點對點的方式將新的信息傳送給其他機器人。每個機器人只能獲得與其相鄰的機器人的位置信息，不知道整個系統(tǒng)的拓撲結(jié)構(gòu)。這種方法與分布式的信息融合十分相似，可以利用信息濾波器來實現(xiàn)。由于兩個不相關(guān)信息矩陣的信息融合可以通過兩個矩陣的相加而實現(xiàn)，所以利用信息濾波器實現(xiàn)分布式CSLAM可以避免復(fù)雜的計算。

4、研究方向與發(fā)展趨勢

綜上所述，近幾年來機器人領(lǐng)域的研究者對SLAM進行了大量的研究，特別是在降低計算復(fù)雜度、提高魯棒性等方面取得了很大的進展。隨著研究的深入，以下的幾方面成為了當(dāng)前SLAM的研究熱點方向。

（1）擴展SLAM的應(yīng)用環(huán)境：將目前局限二維靜態(tài)環(huán)境中的研究與應(yīng)用擴展到與現(xiàn)實中的環(huán)境切合的動態(tài)的三維環(huán)境；

（2）深入研究基于多機器人協(xié)作的SLAM，提高其應(yīng)用水平；

（3）研究更有效的SLAM實現(xiàn)方法，將人工智能、智能控制等領(lǐng)域的方法引入到SLAM中，開發(fā)更有效的SLAM算法.；

（4）研究更好的地圖表達方式，特別是復(fù)雜地形和大環(huán)境中的地圖表達方式；

（5）研究更好的將視覺處理與其他傳感器結(jié)合，提高環(huán)境特征提取的精度，減少誤差，提高定位和構(gòu)圖的精確性。

相關(guān)內(nèi)容推薦：

基于ROS的激光雷達建圖及定位數(shù)據(jù)獲取步驟

機器人自主移動面臨的幾大問題