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

1、前言

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

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

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

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

2.1地圖的表示方式

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

(1)柵格地圖

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

優(yōu)點(diǎn)：

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

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

缺點(diǎn)：

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

（2）幾何信息地圖

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

優(yōu)點(diǎn)：

更為緊湊，且便于位置估計(jì)和目標(biāo)識別；

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

缺點(diǎn)：

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

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

（3）拓?fù)涞貓D

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

優(yōu)點(diǎn)：

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

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

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

缺點(diǎn)：

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

2.2不確定信息的描述

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

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

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

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

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

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

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

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

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

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

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

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

2）.Clustering分析是一種數(shù)據(jù)探測工具，對于未分類樣例是有效的，同時，它的目標(biāo)就是把所針對對象分組成自然類別或基于相似性或距離的簇類。在被提取對象類別未知的情況中，簇技術(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)是對兩個特征標(biāo)志進(jìn)行匹配，確定它們是否對應(yīng)環(huán)境中的同一物體。SLAM中的數(shù)據(jù)關(guān)聯(lián)主要需要完成三個任務(wù)：

（1）新特征標(biāo)志的檢測；

（2）特征標(biāo)志的匹配；

（3）地圖之間的匹配；

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

2.5累積誤差

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

（1） 觀測誤差；

（2） 里程計(jì)的誤差；

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

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

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

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

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

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

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

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

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

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

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

3.2局部子地圖法

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

（1）如何劃分子地圖；

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

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

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

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

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

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

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

3.3去相關(guān)法

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

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

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

3.4分解法（FastSLAM）

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

機(jī)器人自主移動面臨的幾大問題