SLAM 介紹以及淺析

1、Simultaneous Localization and Mapping即時(shí)定位與地圖構(gòu)建即時(shí)定位與地圖構(gòu)建 SLAM指的是機(jī)器人在自身位置不指的是機(jī)器人在自身位置不確定的條件下確定的條件下,在完全未知環(huán)境中在完全未知環(huán)境中創(chuàng)建地圖創(chuàng)建地圖,同時(shí)利用地圖進(jìn)行自主同時(shí)利用地圖進(jìn)行自主定位和導(dǎo)航。定位和導(dǎo)航。 SLAM問題可以描述為問題可以描述為: 機(jī)器機(jī)器人在未知環(huán)境中從一個(gè)未知位置開人在未知環(huán)境中從一個(gè)未知位置開始移動(dòng)始移動(dòng),在移動(dòng)過程中根據(jù)位置估在移動(dòng)過程中根據(jù)位置估計(jì)和傳感器數(shù)據(jù)進(jìn)行自身定位計(jì)和傳感器數(shù)據(jù)進(jìn)行自身定位,同同時(shí)建造增量式地圖。時(shí)建造增量式地圖。SLAM（Simultaneo

2、us Localization And Mapping ）中文稱中文稱 “同步定位及建圖同步定位及建圖” 是目前在機(jī)器人定位方面的熱門研究課題。到目是目前在機(jī)器人定位方面的熱門研究課題。到目前為止，也取得了不少進(jìn)展。前為止，也取得了不少進(jìn)展。定位定位(localization): 機(jī)器人必須知道自己在機(jī)器人必須知道自己在 環(huán)境中位置。環(huán)境中位置。建圖建圖(mapping): 機(jī)器人必須記錄環(huán)境中特機(jī)器人必須記錄環(huán)境中特 征的位置（如果知道自己的位置）征的位置（如果知道自己的位置）SLAM： 機(jī)器人在定位的同時(shí)建立環(huán)境地圖機(jī)器人在定位的同時(shí)建立環(huán)境地圖其基本原理是運(yùn)過概率統(tǒng)計(jì)的方法，通過多特征其

3、基本原理是運(yùn)過概率統(tǒng)計(jì)的方法，通過多特征匹配來達(dá)到定位和減少定位誤差的。匹配來達(dá)到定位和減少定位誤差的。 基本的基本的SLAM應(yīng)用的應(yīng)用的kalman filter。 Sk表示傳感器測(cè)試獲取數(shù)據(jù)，表示傳感器測(cè)試獲取數(shù)據(jù)，Mk-1表示第表示第K-1時(shí)刻的局部地圖，時(shí)刻的局部地圖，Rk表示表示K時(shí)刻機(jī)器人的位姿。時(shí)刻機(jī)器人的位姿。SLAM問題包括四個(gè)基本方面問題包括四個(gè)基本方面1） 如何進(jìn)行環(huán)境描述，即環(huán)境地如何進(jìn)行環(huán)境描述，即環(huán)境地圖的表示方法；圖的表示方法；2） 怎樣獲得環(huán)境信息，機(jī)器人在怎樣獲得環(huán)境信息，機(jī)器人在環(huán)境中漫游并記錄傳感器的感知數(shù)環(huán)境中漫游并記錄傳感器的感知數(shù)據(jù)，這涉及到機(jī)器人的

4、定位與環(huán)境據(jù)，這涉及到機(jī)器人的定位與環(huán)境特征提取問題；特征提取問題；3） 怎樣表示獲得的環(huán)境信息，并怎樣表示獲得的環(huán)境信息，并根據(jù)環(huán)境信息更新地圖，這需要解根據(jù)環(huán)境信息更新地圖，這需要解決對(duì)不確定信息的描述和處理方法；決對(duì)不確定信息的描述和處理方法；4） 發(fā)展穩(wěn)定、可靠的發(fā)展穩(wěn)定、可靠的SLAM方法。方法。該領(lǐng)域所涉及的關(guān)鍵性問題可以歸結(jié)為：該領(lǐng)域所涉及的關(guān)鍵性問題可以歸結(jié)為：(1) 地圖的表示方式地圖的表示方式 (大致可分為大致可分為3類類:柵格表示、幾何特征表示和拓?fù)鋱D表示柵格表示、幾何特征表示和拓?fù)鋱D表示)(2) 不確定性信息處理方法不確定性信息處理方法-不確定性信息處理必須解決以下問題

5、：不確定性信息處理必須解決以下問題： 在地圖和位置的表示中，如何描述運(yùn)動(dòng)和感知信息的不確定性? 在迭代過程中，如何處理舊信息與新信息的關(guān)系，連續(xù)更新地圖與位置? 如何依據(jù)不確定的信息進(jìn)行決策?(3) 數(shù)據(jù)的關(guān)聯(lián)；數(shù)據(jù)的關(guān)聯(lián)； (為了獲得全局的環(huán)境地圖和實(shí)現(xiàn)定位，還需要將不同時(shí)間、不同地 點(diǎn)的感知信息進(jìn)行匹配和聯(lián)合，存在局部數(shù)據(jù)之間的關(guān)聯(lián)問題，也存在局部數(shù)據(jù)與全局?jǐn)?shù)據(jù)的關(guān)聯(lián)與匹配問題)(4) 自定位；自定位； (移動(dòng)機(jī)器人的定位按照有無環(huán)境地圖可以分為基于地圖的定位和無地圖的定位)(5) 探索規(guī)劃探索規(guī)劃 (主要目的是提高地圖創(chuàng)建的效率，使機(jī)器人在較短的時(shí)間內(nèi)感知范圍覆蓋盡可能大的區(qū)域，在這方面

6、的研究成果較少)環(huán)境的描述環(huán)境的描述-地圖地圖 目前各國研究者已經(jīng)提出了多種表示法，大致可分為三類：柵格表示、幾何信息表示和拓?fù)鋱D表示，每種方法都有自己的優(yōu)缺點(diǎn)。 柵格地圖表示法即將整個(gè)環(huán)境分為若干相同大小的柵格，對(duì)于每個(gè)柵格各指出其中是否存在障礙物。這種方法最早由Elfes和Moravec提出，而后Elfes進(jìn)行了進(jìn)一步的研究。優(yōu)點(diǎn)是(1)創(chuàng)建和維護(hù)容易，(2)盡量保留了整個(gè)環(huán)境的各種信息，(3)借助于該地圖，可以方便地進(jìn)行自定位和路徑規(guī)劃。缺點(diǎn)在于：當(dāng)柵格數(shù)量增大時(shí)(在大規(guī)模環(huán)境或?qū)Νh(huán)境劃分比較詳細(xì)時(shí))，對(duì)地圖的維護(hù)行為將變得困難，同時(shí)定位過程中搜索空間很大，如果沒有較好的簡化算法，實(shí)現(xiàn)實(shí)

7、時(shí)應(yīng)用比較困難。 幾何信息地圖表示法是指機(jī)器人收集對(duì)環(huán)境的感知信息，從中提取更為抽象的幾何特征，例如線段或曲線，使用這些幾何信息描述環(huán)境。該表示法更為緊湊，且便于位置估計(jì)和目標(biāo)識(shí)別。幾何方法利用卡爾曼濾波在局部區(qū)域內(nèi)可獲得較高精度，且計(jì)算量小，但在廣域環(huán)境中卻難以維持精確的坐標(biāo)信息。幾何信息的提取需要對(duì)感知信息作額外處理，且需要一定數(shù)量的感知數(shù)據(jù)才能得到結(jié)果。 拓?fù)涞貓D抽象度高，特別在環(huán)境大而簡單時(shí)。這種方法將環(huán)境表示為一張拓?fù)湟饬x中的圖(graph)，圖中的節(jié)點(diǎn)對(duì)應(yīng)于環(huán)境中的一個(gè)特征狀態(tài)、地點(diǎn)。如果節(jié)點(diǎn)間存在直接連接的路徑則相當(dāng)于圖中連接節(jié)點(diǎn)的弧。優(yōu)點(diǎn)是：(1)有利于進(jìn)一步的路徑和任務(wù)規(guī)劃，

8、(2)存儲(chǔ)和搜索空間都比較小，計(jì)算效率高，(3)可以使用很多現(xiàn)有成熟、高效的搜索和推理算法。缺點(diǎn)在于對(duì)拓?fù)鋱D的使用是建立在對(duì)拓?fù)涔?jié)點(diǎn)的識(shí)別匹配基礎(chǔ)上的，如當(dāng)環(huán)境中存在兩個(gè)很相似的地方時(shí)，拓?fù)鋱D方法將很難確定這是否為同一點(diǎn)。 SLA M中定位與環(huán)境特征提取中定位與環(huán)境特征提取移動(dòng)機(jī)器人自定位與環(huán)境建模問題是緊密相關(guān)的。環(huán)境模型的準(zhǔn)確性依賴于定位精度，而定位的實(shí)現(xiàn)又離不開環(huán)境模型。在未知環(huán)境中，機(jī)器人沒有什么參照物，只能依靠自己并不十分準(zhǔn)確的傳感器來獲取外界信息，如同一個(gè)盲人在一個(gè)陌生環(huán)境中摸索的情況。這種情況下，定位是比較困難的。有地圖的定位和有定位的地圖創(chuàng)建都是容易解決的，但無地圖的定位和未解

9、決定位的地圖創(chuàng)建如同雞-蛋問題，無從下手。已有的研究中對(duì)這類問題的解決方法可分為兩類：一類利用自身攜帶的多種內(nèi)部傳感器(包括里程儀、羅盤、加速度計(jì)等)，通過多種傳感信息的融合減少定位的誤差，使用的融合算法多為基于卡爾曼濾波的方法。這類方法由于沒有參考外部信息，在長時(shí)間的漫游后誤差的積累會(huì)比較大。另一類方法在依靠內(nèi)部傳感器估計(jì)自身運(yùn)動(dòng)的同時(shí)，使用外部傳感器(如激光測(cè)距儀、視覺等)感知環(huán)境，對(duì)獲得的信息進(jìn)行分析提取環(huán)境特征并保存，在下一步通過對(duì)環(huán)境特征的比較對(duì)自身位置進(jìn)行校正。但這種方法依賴于能夠取得環(huán)境特征。環(huán)境特征提取的方法有：(1)Hough transform是一類基于灰度圖探察直線和其他

10、曲線的方法。該方法需要一簇能被搜索的預(yù)準(zhǔn)備的特定曲線，并根據(jù)顯示的灰度圖中一簇曲線產(chǎn)生曲線參數(shù)。(2)Clustering分析是一種數(shù)據(jù)探測(cè)工具，對(duì)于未分類樣例是有效的，同時(shí)，它的目標(biāo)就是把所針對(duì)對(duì)象分組成自然類別或基于相似性或距離的簇類。在被提取對(duì)象類別未知的情況中，簇技術(shù)是一類比Hough Transform更有效的技術(shù)。簇類應(yīng)是以凝聚為中心，而不是支離破碎的、不相交的。而環(huán)境特征有時(shí)是很難提取出的，例如：環(huán)境特征不夠明顯時(shí)；傳感器信息比較少，難以從一次感知信息中獲得環(huán)境特征。 不確定信息的描述和處理方法不確定信息的描述和處理方法 在未知環(huán)境中，環(huán)境信息的不確定性尤為明顯。研究人員已經(jīng)提出

11、了多種用來處理不確定性的度量方法，如模糊度量、概率度量、信任度量、可能性度量、證據(jù)理論度量等等。目前在SLAM中使用較多的主要是模糊度量和概率度量的方法。 以概率描述信息的不確定性，其優(yōu)點(diǎn)在于：適應(yīng)不確定模型，對(duì)于性能差的傳感器也適用，在實(shí)際應(yīng)用中魯棒性較好，在很多實(shí)現(xiàn)中得到應(yīng)用。缺點(diǎn)是計(jì)算量較大，算法建立在一定假設(shè)條件上，而假設(shè)的合理性有爭議。因此，有些學(xué)者認(rèn)為使用模糊度的描述方法更適合于該任務(wù)。他們認(rèn)為模糊邏輯(fuzzy logic)提供了一個(gè)關(guān)于處理不確定信息更自然的框架，是一個(gè)更為魯棒且有效的工具(尤其對(duì)于聲納感知過程引起的不確定性)。同時(shí)，在對(duì)多種不確定性信息進(jìn)行建模和融合時(shí)有更多

12、的操作符供選擇。 幾種典型幾種典型SLAM方法方法 目前SLAM方法大致可分為兩類：一類為基于概率模型的方法，另一類為非概率模型方法。許多基于卡爾曼濾波的SLAM方法如完全SLAM、壓縮濾波、FastSLAM就屬于概率模型方法。非概率模型方法有SM-SLAM、掃描匹配、數(shù)據(jù)融合(data association)、基于模糊邏輯等。 在一個(gè)未知的室內(nèi)結(jié)構(gòu)化環(huán)境中提出了基于柵格表示的局部模型與基于幾何信息表示的全局模型相結(jié)合的環(huán)境建模方式。環(huán)境特征的提取采用了Hough transform與Clustering相結(jié)合的方法。感知數(shù)據(jù)的融合采用了擴(kuò)展卡爾曼濾波方式。 提出的算法不需任何人為設(shè)定的參照

13、物(如路標(biāo)、燈塔)并且可適用于非多邊形及動(dòng)態(tài)環(huán)境，同時(shí)和基于SLAM的卡爾曼濾波策略比較，其計(jì)算復(fù)雜度較小、與路標(biāo)數(shù)量、環(huán)境大小無關(guān)。環(huán)境建模策略由3部分構(gòu)成：(1)收集環(huán)境信息；(2)掃描注冊(cè)；(3)構(gòu)建柵格地圖。在掃描注冊(cè)中綜合使用了幾何模式匹配與掃描匹配。環(huán)境特征提取采用Hough transform技術(shù)。采用HIMM (Histogramic In-Motion Mapping)方法來構(gòu)建柵格地圖。在定位方面，融合使用地圖匹配與基于Petri網(wǎng)的離散事件控制策略。 有以下優(yōu)勢(shì)：(1)在無任何人工路標(biāo)的情況下，能創(chuàng)建準(zhǔn)確地圖；(2)擴(kuò)展了掃描匹配的應(yīng)用范圍，使其能應(yīng)用于非多邊形的環(huán)境；(

14、3)提高了幾何模式匹配在多邊形環(huán)境中的精確度；(4)每次掃描注冊(cè)精度都將與以前所有局部區(qū)域掃描結(jié)果進(jìn)行比較，以保證全局地圖的準(zhǔn)確性；(5)計(jì)算復(fù)雜度較小。離散事件控制策略的運(yùn)用能更好的進(jìn)行位置估測(cè)與對(duì)定位錯(cuò)誤的處理。 提出了使用SM(Set Membership)方法，解決同種多機(jī)器人SLAM問題。在地圖融合方面，把2D地圖分解為兩個(gè)1D地圖來進(jìn)行，進(jìn)一步降低計(jì)算復(fù)雜度。在SLAM中使用SP-Model(Symmetries and Perturbations Model)，對(duì)任何幾何元素它提供了一個(gè)統(tǒng)一的表示方式，并避免了參數(shù)過多的問題。說明：機(jī)器人在定位誤差隨著機(jī)器人的移動(dòng)而增加。說明：機(jī)

15、器人在定位誤差隨著機(jī)器人的移動(dòng)而增加。 說明：機(jī)器人在定位誤差隨著機(jī)器人的移動(dòng)而增加，但是由于有了路標(biāo)的糾正，其誤差相對(duì)就小了很多。說明：機(jī)器人在定位誤差隨著機(jī)器人的移動(dòng)而增加，但是由于有了路標(biāo)的糾正，其誤差相對(duì)就小了很多。說明：通過說明：通過2次對(duì)路標(biāo)的測(cè)定，其定位誤差已經(jīng)大大減少次對(duì)路標(biāo)的測(cè)定，其定位誤差已經(jīng)大大減少 說明：經(jīng)過說明：經(jīng)過3次誤差校正，其定位精度已經(jīng)很好了，但是隨著路標(biāo)位置的丟失，其定位誤差又一次的擴(kuò)大了。次誤差校正，其定位精度已經(jīng)很好了，但是隨著路標(biāo)位置的丟失，其定位誤差又一次的擴(kuò)大了?；诨舴蚵椒ǖ幕诨舴蚵椒ǖ腟LAM的基本過程為：的基本過程為： 1）使用）使用e

16、xtend kalman filter （EKF) 進(jìn)行定位進(jìn)行定位 2）獲取所有的路標(biāo)和機(jī)器人的位置信息作為先決條件）獲取所有的路標(biāo)和機(jī)器人的位置信息作為先決條件 3）使用矢量方程來存儲(chǔ)路標(biāo)和機(jī)器人的位置）使用矢量方程來存儲(chǔ)路標(biāo)和機(jī)器人的位置4）通過循環(huán)迭代來減少每次的誤差量）通過循環(huán)迭代來減少每次的誤差量狀態(tài)估計(jì)是卡爾曼濾波的重要組成部分。一般來說，根據(jù)觀測(cè)數(shù)據(jù)狀態(tài)估計(jì)是卡爾曼濾波的重要組成部分。一般來說，根據(jù)觀測(cè)數(shù)據(jù)對(duì)隨機(jī)量進(jìn)行定量推斷就是估計(jì)問題，特別是對(duì)動(dòng)態(tài)行為的狀態(tài)估對(duì)隨機(jī)量進(jìn)行定量推斷就是估計(jì)問題，特別是對(duì)動(dòng)態(tài)行為的狀態(tài)估計(jì)，它能實(shí)現(xiàn)實(shí)時(shí)運(yùn)行狀態(tài)的估計(jì)和預(yù)測(cè)功能。比如對(duì)飛行器狀態(tài)

17、計(jì)，它能實(shí)現(xiàn)實(shí)時(shí)運(yùn)行狀態(tài)的估計(jì)和預(yù)測(cè)功能。比如對(duì)飛行器狀態(tài)估計(jì)。狀態(tài)估計(jì)對(duì)于了解和控制一個(gè)系統(tǒng)具有重要意義，所應(yīng)用的估計(jì)。狀態(tài)估計(jì)對(duì)于了解和控制一個(gè)系統(tǒng)具有重要意義，所應(yīng)用的方法屬于統(tǒng)計(jì)學(xué)中的估計(jì)理論。最常用的是最小二乘估計(jì)，線性最方法屬于統(tǒng)計(jì)學(xué)中的估計(jì)理論。最常用的是最小二乘估計(jì)，線性最小方差估計(jì)、最小方差估計(jì)、遞推最小二乘估計(jì)等。其他如風(fēng)險(xiǎn)準(zhǔn)小方差估計(jì)、最小方差估計(jì)、遞推最小二乘估計(jì)等。其他如風(fēng)險(xiǎn)準(zhǔn)則的貝葉斯估計(jì)、最大似然估計(jì)、隨機(jī)逼近等方法也都有應(yīng)用。則的貝葉斯估計(jì)、最大似然估計(jì)、隨機(jī)逼近等方法也都有應(yīng)用。 受噪聲干擾的狀態(tài)量是個(gè)隨機(jī)量，不可能測(cè)得精確值，但可對(duì)受噪聲干擾的狀態(tài)量是個(gè)隨機(jī)

18、量，不可能測(cè)得精確值，但可對(duì)它進(jìn)行一系列觀測(cè)，并依據(jù)一組觀測(cè)值，按某種統(tǒng)計(jì)觀點(diǎn)對(duì)它進(jìn)行它進(jìn)行一系列觀測(cè)，并依據(jù)一組觀測(cè)值，按某種統(tǒng)計(jì)觀點(diǎn)對(duì)它進(jìn)行估計(jì)。使估計(jì)值盡可能準(zhǔn)確地接近真實(shí)值，這就是最優(yōu)估計(jì)。真實(shí)估計(jì)。使估計(jì)值盡可能準(zhǔn)確地接近真實(shí)值，這就是最優(yōu)估計(jì)。真實(shí)值與估計(jì)值之差稱為估計(jì)誤差。若估計(jì)值的數(shù)學(xué)期望與真實(shí)值相等，值與估計(jì)值之差稱為估計(jì)誤差。若估計(jì)值的數(shù)學(xué)期望與真實(shí)值相等，這種估計(jì)稱為無偏估計(jì)?？柭岢龅倪f推最優(yōu)估計(jì)理論，采用狀這種估計(jì)稱為無偏估計(jì)?？柭岢龅倪f推最優(yōu)估計(jì)理論，采用狀態(tài)空間描述法，在算法采用遞推形式，卡爾曼濾波能處理多維和非態(tài)空間描述法，在算法采用遞推形式，卡爾曼濾波能

19、處理多維和非平穩(wěn)的隨機(jī)過程。平穩(wěn)的隨機(jī)過程。 卡爾曼濾波理論的提出，克服了威納濾波理論的局限性使其在卡爾曼濾波理論的提出，克服了威納濾波理論的局限性使其在工程上得到了廣泛的應(yīng)用，尤其在控制、制導(dǎo)、導(dǎo)航、通訊等現(xiàn)代工程上得到了廣泛的應(yīng)用，尤其在控制、制導(dǎo)、導(dǎo)航、通訊等現(xiàn)代工程方面。工程方面。 圖圖1：初始位置：初始位置（由于機(jī)器人相對(duì)于路標(biāo)（由于機(jī)器人相對(duì)于路標(biāo)A的位置為估計(jì)值，所以路標(biāo)的位置為估計(jì)值，所以路標(biāo)A用圓圈表示用圓圈表示A的實(shí)際的可能值在圓圈內(nèi)）的實(shí)際的可能值在圓圈內(nèi)）圖圖2：機(jī)器人移動(dòng)到新位置：機(jī)器人移動(dòng)到新位置（在新的位置，機(jī)器人相對(duì)于（在新的位置，機(jī)器人相對(duì)于A的真實(shí)值可能落在

20、圈內(nèi)）的真實(shí)值可能落在圈內(nèi)）圖圖3：路標(biāo)：路標(biāo)C被觀測(cè)到被觀測(cè)到（在新的位置，路標(biāo)（在新的位置，路標(biāo)C被觀測(cè)到，路標(biāo)被觀測(cè)到，路標(biāo)C的相對(duì)于的相對(duì)于A的位置也是一個(gè)估計(jì)值（更大的圈）的位置也是一個(gè)估計(jì)值（更大的圈）圖圖4：路標(biāo)：路標(biāo)B被觀測(cè)到被觀測(cè)到圖圖5：機(jī)器人返回到初始位置：機(jī)器人返回到初始位置（此時(shí)機(jī)器人的位置相對(duì)于沒有移動(dòng)前更加不確定，一個(gè)超大的橢圓表示了其可能的真實(shí)位置值范圍）（此時(shí)機(jī)器人的位置相對(duì)于沒有移動(dòng)前更加不確定，一個(gè)超大的橢圓表示了其可能的真實(shí)位置值范圍）圖圖6：對(duì)：對(duì)A點(diǎn)的重新測(cè)量點(diǎn)的重新測(cè)量（通過對(duì)（通過對(duì)A的重新測(cè)量，圖的重新測(cè)量，圖4中的超大橢圓值被大大的縮小了，其位置真值落入了一個(gè)比較小的范圍內(nèi)