SLAM算法簡(jiǎn)介
發(fā)布日期：2022/1/2 23:57:57 瀏覽量：
本文的主要目的是簡(jiǎn)單介紹移動(dòng)機(jī)器人領(lǐng)域中廣泛應(yīng)用的技術(shù)SLAM(同步定位與地圖繪制)的理論基礎(chǔ)以及應(yīng)用細(xì)節(jié)。雖然目前存在很多關(guān)于SLAM技術(shù)的方方面面的論文，但是對(duì)于一個(gè)新手來(lái)說(shuō)，仍然需要花費(fèi)大量的時(shí)間去調(diào)研與把握SLAM發(fā)展的脈絡(luò)。本文希望能夠?qū)LAM技術(shù)在保持一些理論基礎(chǔ)的前提下，能夠按照一種簡(jiǎn)單易懂的方式呈現(xiàn)出現(xiàn)了。在閱讀完本文后，讀者應(yīng)該可以在一個(gè)移動(dòng)機(jī)器人上實(shí)現(xiàn)最簡(jiǎn)單使用的SLAM技術(shù)。
SLAM可以通過(guò)多種方法實(shí)現(xiàn)，首先其可以在多種不同的硬件上實(shí)現(xiàn)。其次，SLAM更像是一個(gè)概念而不是一個(gè)算法。SLAM技術(shù)包含了許多步驟，其中的每一個(gè)步驟均可以使用不同的算法實(shí)現(xiàn)。在這里，我們對(duì)其中的每一步介紹一種最為常見(jiàn)的方法，至于其他的方法僅作一個(gè)簡(jiǎn)介。
本文的目的是一種非常簡(jiǎn)單實(shí)用的方式將SLAM技術(shù)呈現(xiàn)出來(lái)，如果讀者有一定SLAM技術(shù)的技術(shù)，可以看出這里提供了一種基于EKF（擴(kuò)展卡爾曼濾波）的完整的解決方案。需要注意的是，SLAM技術(shù)仍然是目前機(jī)器人領(lǐng)域的研究熱點(diǎn)之一，仍然有許多問(wèn)題需要深入研究。
1. 關(guān)于SLAM
SLAM是同步定位與地圖構(gòu)建(Simultaneous Localization And Mapping)的縮寫(xiě)，最早由Hugh Durrant-Whyte 和 John J.Leonard提出。SLAM主要用于解決移動(dòng)機(jī)器人在未知環(huán)境中運(yùn)行時(shí)定位導(dǎo)航與地圖構(gòu)建的問(wèn)題。
SLAM通常包括如下幾個(gè)部分，特征提取，數(shù)據(jù)關(guān)聯(lián)，狀態(tài)估計(jì)，狀態(tài)更新以及特征更新等。對(duì)于其中每個(gè)部分，均存在多種方法。針對(duì)每個(gè)部分，我們將詳細(xì)解釋其中一種方法。在實(shí)際使用過(guò)程中，讀者可以使用其他的方法代替本文中說(shuō)明的方法。這里，我們以室內(nèi)環(huán)境中運(yùn)行的移動(dòng)機(jī)器人為例進(jìn)行說(shuō)明，讀者可以將本文提出的方法應(yīng)用于其他的環(huán)境以及機(jī)器人中。
SLAM既可以用于2D運(yùn)動(dòng)領(lǐng)域，也可以應(yīng)用于3D運(yùn)動(dòng)領(lǐng)域。這里，我們將僅討論2D領(lǐng)域內(nèi)的運(yùn)動(dòng)。
2. 機(jī)器人平臺(tái)
在學(xué)習(xí)SLAM的過(guò)程中，機(jī)器人平臺(tái)是很重要的，其中，機(jī)器人平臺(tái)需要可以移動(dòng)并且至少包含一個(gè)測(cè)距單元。我們這里主要討論的是室內(nèi)輪式機(jī)器人，同時(shí)主要討論SLAM的算法實(shí)現(xiàn)過(guò)程，而并不考慮一些復(fù)雜的運(yùn)動(dòng)模型如人形機(jī)器人。
在選擇機(jī)器人平臺(tái)時(shí)需要考慮的主要因素包括易用性，定位性能以及價(jià)格。定位性能主要衡量機(jī)器人僅根據(jù)自身的運(yùn)動(dòng)對(duì)自身位置進(jìn)行估計(jì)的能力。機(jī)器人的定位精度應(yīng)該不超過(guò)2%，轉(zhuǎn)向精度不應(yīng)該超過(guò)5%。一般而言，機(jī)器人可以在直角坐標(biāo)系中根據(jù)自身的運(yùn)動(dòng)估計(jì)其自身的位置與轉(zhuǎn)向。
從0開(kāi)始搭建機(jī)器人平臺(tái)將會(huì)是一個(gè)耗時(shí)的過(guò)程，也是沒(méi)有必要的。我們可以選擇一些市場(chǎng)上成熟的機(jī)器人開(kāi)發(fā)平臺(tái)進(jìn)行我們的開(kāi)發(fā)。這里，我們以一個(gè)非常簡(jiǎn)單的自己開(kāi)發(fā)的機(jī)器人開(kāi)發(fā)平臺(tái)討論，讀者可以選擇自己的機(jī)器人開(kāi)發(fā)平臺(tái)。
目前比較常見(jiàn)的測(cè)距單元包括激光測(cè)距、超聲波測(cè)距以及圖像測(cè)距三種。其中，激光測(cè)距是最為常用的方式。通常激光測(cè)距單元比較精確、高效并且其輸出不需要太多的處理。其缺點(diǎn)在于價(jià)格一般比較昂貴（目前已經(jīng)有一些價(jià)格比較便宜的激光測(cè)距單元）。激光測(cè)距單元的另外一個(gè)問(wèn)題是其穿過(guò)玻璃平面的問(wèn)題。另外激光測(cè)距單元不能夠應(yīng)用于水下測(cè)量。
另外一個(gè)常用的測(cè)距方式是超聲波測(cè)距。超生波測(cè)距以及聲波測(cè)距等以及在過(guò)去得到十分廣泛的應(yīng)用。相對(duì)于激光測(cè)距單元，其價(jià)格比較便宜；但其測(cè)量精度較低。激光測(cè)距單元的發(fā)射角僅0.25°，因而，激光基本上可以看作直線；相對(duì)而言，超聲波的發(fā)射角達(dá)到了30°，因而，其測(cè)量精度較差。但在水下，由于其穿透力較強(qiáng)，因而，是最為常用的測(cè)距方式。最為常用的超聲波測(cè)距單元是Polaroid超聲波發(fā)生器。
第三種常用的測(cè)距方式是通過(guò)視覺(jué)進(jìn)行測(cè)距。傳統(tǒng)上來(lái)說(shuō)，通過(guò)視覺(jué)進(jìn)行測(cè)距需要大量的計(jì)算，并且測(cè)量結(jié)果容易隨著光線變化而發(fā)生變化。如果機(jī)器人運(yùn)行在光線較暗的房間內(nèi)，那么視覺(jué)測(cè)距方法基本上不能使用。但最近幾年，已經(jīng)存在一些解決上述問(wèn)題的方法。一般而言，視覺(jué)測(cè)距一般使用雙目視覺(jué)或者三目視覺(jué)方法進(jìn)行測(cè)距。使用視覺(jué)方法進(jìn)行測(cè)距，機(jī)器人可以更好的像人類(lèi)一樣進(jìn)行思考。另外，通過(guò)視覺(jué)方法可以獲得相對(duì)于激光測(cè)距和超聲波測(cè)距更多的信息。但更過(guò)的信息也就意味著更高的處理代價(jià)，但隨著算法的進(jìn)步和計(jì)算能力的提高，上述信息處理的問(wèn)題正在慢慢得到解決。
這里，我們使用激光測(cè)距方法進(jìn)行距離測(cè)量。其可以很容易實(shí)現(xiàn)較高的測(cè)量精度并且很容易應(yīng)用于SLAM中。
3. SLAM的一般過(guò)程
SLAM通常包含幾個(gè)過(guò)程，這些過(guò)程的最終目的是更新機(jī)器人的位置估計(jì)信息。由于通過(guò)機(jī)器人運(yùn)動(dòng)估計(jì)得到的機(jī)器人位置信息通常具有較大的誤差，因而，我們不能單純的依靠機(jī)器人運(yùn)動(dòng)估計(jì)機(jī)器人位置信息。在使用機(jī)器人運(yùn)動(dòng)方程得到機(jī)器人位置估計(jì)后，我們可以使用測(cè)距單元得到的周?chē)h(huán)境信息更正機(jī)器人的位置。上述更正過(guò)程一般通過(guò)提取環(huán)境特征，然后在機(jī)器人運(yùn)動(dòng)后重新觀測(cè)特征的位置實(shí)現(xiàn)。SLAM的核心是EKF。EKF用于結(jié)合上述信息估計(jì)機(jī)器人準(zhǔn)確位置。上述選取的特征一般稱(chēng)作地標(biāo)。EKF將持續(xù)不斷的對(duì)上述機(jī)器人位置和周?chē)h(huán)境中地標(biāo)位置進(jìn)行估計(jì)。SLAM的一般過(guò)程如下圖所示：
當(dāng)機(jī)器人運(yùn)動(dòng)時(shí)，其位置將會(huì)發(fā)生變化。此時(shí)，根據(jù)機(jī)器人位置傳感器的觀測(cè)，提取得到觀測(cè)信息中的特征點(diǎn)，然后機(jī)器人通過(guò)EKF將目前觀測(cè)到特征點(diǎn)的位置、機(jī)器人運(yùn)動(dòng)距離、機(jī)器人運(yùn)動(dòng)前觀測(cè)到特征點(diǎn)的位置相互結(jié)合，對(duì)機(jī)器人當(dāng)前位置和當(dāng)前環(huán)境信息進(jìn)行估計(jì)。
下圖是估計(jì)的詳細(xì)過(guò)程。
上圖中三角形表示機(jī)器人，星號(hào)表示路標(biāo)；機(jī)器人首先使用測(cè)距單元測(cè)量地標(biāo)相對(duì)于機(jī)器人的距離和角度。
然后進(jìn)行開(kāi)始進(jìn)行運(yùn)動(dòng)，并且到達(dá)一個(gè)新的位置，機(jī)器人根據(jù)其運(yùn)動(dòng)方程預(yù)測(cè)其現(xiàn)在所處于的新的位置。
在新的位置，機(jī)器人通過(guò)測(cè)距單元重新測(cè)量各個(gè)地標(biāo)相對(duì)于機(jī)器人的距離和角度，測(cè)量得到的距離和角度與上述預(yù)測(cè)結(jié)果可能并不一致，因而，上述預(yù)測(cè)值可能并不是機(jī)器人準(zhǔn)確位置。
在機(jī)器人看來(lái)，通過(guò)傳感器獲得的信息相對(duì)于通過(guò)運(yùn)動(dòng)方程得到的信息更為準(zhǔn)確，因而，機(jī)器人將通過(guò)傳感器的數(shù)據(jù)更新對(duì)機(jī)器人位置的預(yù)測(cè)值，如上圖中實(shí)線三角形所示(虛線為第一步中通過(guò)運(yùn)動(dòng)信息預(yù)測(cè)的機(jī)器人位置)。
經(jīng)過(guò)上述結(jié)合直軸，我們重新估計(jì)得到的新的機(jī)器人位置如上圖實(shí)線三角形所示，但由于測(cè)距單元精度有限，因而，此時(shí)，機(jī)器人可能實(shí)際處于上圖點(diǎn)狀三角形位置，但此時(shí)估計(jì)結(jié)果相對(duì)于初始預(yù)測(cè)結(jié)果已經(jīng)有明顯的改善。
4. 測(cè)距單元
SLAM的第一步需要通過(guò)測(cè)距單元獲取機(jī)器人周?chē)h(huán)境的信息。這里，我們以激光測(cè)距單元為例。以一個(gè)常見(jiàn)的激光測(cè)距單元為例，其測(cè)量范圍可到360°，水平分辨率為0.25°，即激光束的角度為0.25°。其輸出如下：
2.98，2.99，3.00，3.01，3.02，3.49，3.50，...，2.20，8.17，2.21
激光測(cè)距單元的輸出表示機(jī)器人距最近障礙物的距離。如果由于某些原因，激光測(cè)距單元無(wú)法測(cè)量某個(gè)特定角度上的安全范圍，那么其將返回一個(gè)最大值，這里以8.1為例，測(cè)距單元返回?cái)?shù)據(jù)超過(guò)8.1即意味著激光測(cè)距單元在該角度上發(fā)生測(cè)量錯(cuò)誤。需要注意的是，激光測(cè)距單元可以以很高的頻率對(duì)周?chē)h(huán)境進(jìn)行測(cè)量，其可以實(shí)現(xiàn)10-100Hz的全周掃描。
5. 機(jī)器人自身運(yùn)動(dòng)模型
SLAM的另外一個(gè)很重要的數(shù)據(jù)來(lái)源是機(jī)器人通過(guò)自身運(yùn)動(dòng)估計(jì)得到的自身位置信息。機(jī)器人自身位置數(shù)據(jù)通過(guò)對(duì)機(jī)器人輪胎運(yùn)行圈數(shù)的估計(jì)可以得到機(jī)器人自身位置的一個(gè)估計(jì)，其可以被看作EKF的初始估計(jì)數(shù)據(jù)。
另外一個(gè)需要注意的是需要保證機(jī)器人自身位置數(shù)據(jù)與測(cè)距單元數(shù)據(jù)的同步性。為了保證其同步性，一般采用插值的方法對(duì)數(shù)據(jù)進(jìn)行前處理。由于機(jī)器人的運(yùn)動(dòng)規(guī)律是連續(xù)的，因而，一般對(duì)機(jī)器人自身位置數(shù)據(jù)進(jìn)行插值。相對(duì)而言，由于測(cè)距單元數(shù)據(jù)的不連續(xù)性，因而其插值基本上是不可以實(shí)現(xiàn)的。
6. 特征（地標(biāo)）
地標(biāo)是環(huán)境中易于觀測(cè)和區(qū)分的特征。一般使用這些特征確定機(jī)器人位置。我們可以通過(guò)下面的方法想象上述工作過(guò)程，假設(shè)在一個(gè)陌生的房間內(nèi)，閉上眼睛，那么此時(shí)我們?nèi)绾未_定自身的位置呢？通常而言，我們將在環(huán)境中不斷走動(dòng)，通過(guò)觸摸物體或者墻壁確定自身位置。上述如被觸摸的物體以及墻壁等都可以被看做用于估計(jì)自身位置的地標(biāo)。下面是一些典型的地標(biāo)。
可以看出，通常而言，對(duì)于不同的環(huán)境，一般我們選擇不同的地標(biāo)。
一般而言，地標(biāo)需要滿(mǎn)足下面的條件：
1. 地標(biāo)應(yīng)該可以從不同的位置和角度觀察得到；
2. 地標(biāo)應(yīng)該是獨(dú)一無(wú)二的，從而可以很容易的將底邊從其他物體中分辨出來(lái)
3. 地標(biāo)不應(yīng)該過(guò)少，從而導(dǎo)致機(jī)器人需要花費(fèi)額外的代價(jià)尋找地標(biāo)；
4. 地標(biāo)應(yīng)該是靜止的，因而，我們最好不要使用一個(gè)人作為地標(biāo)
舉例來(lái)說(shuō)，室內(nèi)環(huán)境中的地標(biāo)，我們可以選擇為墻壁與地面之間的連線，以及墻角等。如下圖所示：
7.特征提取
根據(jù)上述步驟6確定完需要提取的特征后，我們接下來(lái)需要從測(cè)距單元獲得的信息中準(zhǔn)確的提取出我們需要的特征。特征提取的方法有很多種，其主要取決于需要提取特征以及測(cè)距單元的類(lèi)型。
這里，我們將以如何從激光雷達(dá)得到的信息提取有效特征為例進(jìn)行說(shuō)明。這里，我們使用兩種典型的特征提取方法，Spike方法和RANSAC方法。
Spike方法使用極值尋找特征。通過(guò)尋找測(cè)距單元返回?cái)?shù)據(jù)中相鄰數(shù)據(jù)差距超過(guò)一定范圍的點(diǎn)作為特征點(diǎn)。通過(guò)這種方法，當(dāng)測(cè)距單元發(fā)射的光束從墻壁上反射回來(lái)時(shí)，測(cè)距單元返回的數(shù)值為某些值；而當(dāng)發(fā)射光束碰到其他物體并反射回來(lái)時(shí)，此時(shí)測(cè)距單元將返回另外一些數(shù)值；兩者將具有較大的差別。如下圖所示：
圖中紅點(diǎn)為根據(jù)Spike方法提取到的特征。
Spike方法也可以通過(guò)下面的步驟實(shí)現(xiàn)，對(duì)于相鄰的三個(gè)點(diǎn)A,B,C,分別計(jì)算（A-B）與（C-B），然后將兩者相加，如果結(jié)果超過(guò)一定范圍，則表示提取到一個(gè)特征。
采用Spike方法提取環(huán)境特征，需要保證相鄰兩個(gè)激光束照射的物體距離機(jī)器人距離之間具有較大的變化，因而，其并不能夠適用于光滑環(huán)境中的特征提取。
RANSAC（隨機(jī)采樣方法）也可以被用于從激光測(cè)距單元返回?cái)?shù)據(jù)中提取系統(tǒng)特征。其中測(cè)距單元返回?cái)?shù)據(jù)中的直線將被提取為路標(biāo)。在室內(nèi)環(huán)境中，由于廣泛存在墻壁等，因而，在測(cè)距單元返回的數(shù)據(jù)中將存在大量的直線。
RANSAC首先采用隨機(jī)采樣的方法提取測(cè)距單元返回?cái)?shù)據(jù)中的一部分，然后采用最小二乘逼近方法來(lái)對(duì)上述數(shù)據(jù)進(jìn)行擬合。進(jìn)行數(shù)據(jù)擬合后，RANSAC方法將會(huì)檢查測(cè)距單元數(shù)據(jù)在擬合曲線周?chē)姆植记闆r。如果分布情況滿(mǎn)足我們的標(biāo)準(zhǔn)，那么我們就認(rèn)為機(jī)器人看到了一條直線。
在使用EKF估計(jì)機(jī)器人位置和環(huán)境地圖時(shí)，EKF需要將地標(biāo)按照距離機(jī)器人當(dāng)前的位置和方位表示出來(lái)。我們可以很容易的使用三角幾何的方法將提取到的直線轉(zhuǎn)變?yōu)楣潭ǖ奶卣鼽c(diǎn)，如下圖所示：
前面我們提到了特征提取的兩種不同的方法，Spike方法和RANSAC方法，兩者均可以用于室內(nèi)環(huán)境中特征的提出。相比較而言，Spike方法較為簡(jiǎn)單，并且對(duì)室內(nèi)環(huán)境中的活動(dòng)物體不具有魯棒性。RANSAC方法通過(guò)提取直線的方法提取環(huán)境中的特征，相對(duì)較為復(fù)雜，但對(duì)室內(nèi)活動(dòng)的物體具有更好的適應(yīng)性。
8. 數(shù)據(jù)關(guān)聯(lián)
數(shù)據(jù)關(guān)聯(lián)是將不同時(shí)刻位置傳感器提取到的地標(biāo)信息進(jìn)行關(guān)聯(lián)的過(guò)程，也成為重觀察過(guò)程。
舉例來(lái)說(shuō)，對(duì)于我們?nèi)祟?lèi)來(lái)說(shuō)，假設(shè)我們?cè)谝粋€(gè)房間內(nèi)看到了一把椅子，現(xiàn)在我們離開(kāi)房間，過(guò)一段時(shí)間后，再次回到房間，如果我們?cè)俅慰吹搅艘巫樱敲次覀兛梢哉J(rèn)為這把椅子很有可能就是我們之前看到的椅子。但是，如果我們假設(shè)房?jī)r(jià)內(nèi)有兩把完全一樣的椅子，重復(fù)上述過(guò)程，當(dāng)我們?cè)俅蝸?lái)到房間后，我們可能無(wú)法區(qū)分我們看到的兩把椅子。但我們可以猜測(cè)，此時(shí)比如說(shuō)左邊的椅子仍然是之前看到的左邊的椅子，而右邊的椅子仍然是之前看到的右邊的椅子。
在實(shí)際應(yīng)用中進(jìn)行數(shù)據(jù)關(guān)聯(lián)時(shí)，我們可能遇到下面的問(wèn)題：
1. 我們可能上一次看到了某個(gè)地標(biāo)，但下一次卻沒(méi)有看到；
2.我們可能這次看到了地標(biāo)，但之后卻再也看不到這個(gè)地標(biāo)；
3.我們可能錯(cuò)誤的將現(xiàn)在看到的某個(gè)地標(biāo)與之前看到的某個(gè)地標(biāo)進(jìn)行關(guān)聯(lián)；
根據(jù)我們選擇路標(biāo)時(shí)的標(biāo)準(zhǔn)，我們可以很容易的排除上面第1和第2個(gè)問(wèn)題。但對(duì)于第三個(gè)問(wèn)題，如果發(fā)生了，將會(huì)對(duì)我們的導(dǎo)航以及地圖繪制造成嚴(yán)重的問(wèn)題。
現(xiàn)在我們將討論解決上面第三個(gè)問(wèn)題的方法。假設(shè)我們現(xiàn)在已經(jīng)到了每時(shí)每刻采集處理得到的路標(biāo)的方位信息，并將其其中的特征存儲(chǔ)在一數(shù)據(jù)庫(kù)中。數(shù)據(jù)庫(kù)初始階段是空的，首先我們建立的第一條規(guī)則是，除非該特征已經(jīng)出現(xiàn)了N次，否則我們并不將其加入數(shù)據(jù)庫(kù)。當(dāng)?shù)玫揭桓毙碌膫鞲衅餍畔⒑?，我們進(jìn)行下面的計(jì)算：
1.得到一副新的傳感器信息后，首先利用上面的特征提取方法提取特征；
2.將提取到的特征與數(shù)據(jù)庫(kù)中已經(jīng)出現(xiàn)N次的并且距離最近的特征關(guān)聯(lián)起來(lái)；
3.通過(guò)驗(yàn)證環(huán)節(jié)驗(yàn)證上面的關(guān)聯(lián)過(guò)程是正確的，如果驗(yàn)證通過(guò)，則表明我們?cè)俅慰吹搅四硞€(gè)物體，因而其出現(xiàn)次數(shù)+1，否則表明我們看到了一個(gè)新的特征，在數(shù)據(jù)庫(kù)中新建一個(gè)特征，并將其記作1.
上述特征關(guān)聯(lián)方法被稱(chēng)作距離最近方法。上面最簡(jiǎn)單的計(jì)算距離的方法是歐式距離的計(jì)算，其他距離計(jì)算包括馬氏距離等，雖然效果更好，但計(jì)算結(jié)果更為復(fù)雜。
驗(yàn)證環(huán)節(jié)通過(guò)利用EKF執(zhí)行過(guò)程中，觀測(cè)誤差的有界性進(jìn)行判斷。因而，我們可以通過(guò)判斷一個(gè)路標(biāo)是否在現(xiàn)存路標(biāo)的誤差允許范圍內(nèi)來(lái)判斷其是否為新的路標(biāo)。路標(biāo)區(qū)域可以通過(guò)圖形繪制或者定義一個(gè)橢圓誤差。
通過(guò)設(shè)定一個(gè)常數(shù)，最新觀測(cè)到的路標(biāo)可以通過(guò)下面的公式與之前觀測(cè)到的路標(biāo)相互關(guān)聯(lián)。
其中，9. 擴(kuò)展卡爾曼濾波器(EKF)
在SLAM中，我們一般使用擴(kuò)展卡爾曼濾波器基于機(jī)器人運(yùn)動(dòng)信息與傳感器測(cè)量特征點(diǎn)信息估計(jì)機(jī)器人的狀態(tài)。這里，我們將詳細(xì)討論將其應(yīng)用于SLAM中的具體步驟。
在得到路標(biāo)點(diǎn)的位置和方位，并且將路標(biāo)點(diǎn)進(jìn)行關(guān)聯(lián)后，SLAM的過(guò)程分為如下三個(gè)部分：
1. 基于機(jī)器人運(yùn)動(dòng)信息更新機(jī)器人當(dāng)前狀態(tài)；
2. 基于路標(biāo)信息更新估計(jì)狀態(tài)；
3. 在當(dāng)前狀態(tài)中增加新的狀態(tài)；
第一步相對(duì)來(lái)說(shuō)較為簡(jiǎn)單，其僅僅需要將當(dāng)前控制器的輸出疊加到上一時(shí)刻的狀態(tài)上。舉例來(lái)說(shuō)，假設(shè)機(jī)器人目前位于,當(dāng)前運(yùn)動(dòng)信息為
,那么第一部的機(jī)器人的當(dāng)前狀態(tài)為
.
在第二步中，我們需要考慮路標(biāo)信息，基于當(dāng)前機(jī)器人的狀態(tài)，我們可以估計(jì)路標(biāo)應(yīng)該處于的位置。這個(gè)估計(jì)位置與測(cè)量位置有所差別，這個(gè)差別被稱(chēng)作新息。新息即為機(jī)器人估計(jì)根據(jù)機(jī)器人狀態(tài)估計(jì)信息與實(shí)際信息之間的差別。此時(shí)，根據(jù)上述新息，每個(gè)特征點(diǎn)的方法同時(shí)被更新。
在第三步中，觀測(cè)到的新的路標(biāo)被加入到狀態(tài)中，即當(dāng)前環(huán)境的地圖。
下面我們對(duì)SLAM中常用的一些定義進(jìn)行說(shuō)明。
1.系統(tǒng)狀態(tài)X
系統(tǒng)狀態(tài)以及其協(xié)方差矩陣式SLAM過(guò)程中最為重要的向量。X包含了機(jī)器人的位置(x,y,\theta)以及環(huán)境中每個(gè)路標(biāo)的信息。其格式如下圖所示：
可以看出，系統(tǒng)狀態(tài)X為一個(gè)(3+2*n)*1的矩陣，即列向量，其中n為路標(biāo)的個(gè)數(shù)。其單位一般為米(m)或者毫米(mm)。角度一般選擇為弧度。
2. 協(xié)方差矩陣P
兩個(gè)變量之間的協(xié)方差矩陣描述的是兩個(gè)變量之間的相關(guān)程度。
這里，協(xié)方差矩陣包含了機(jī)器人位置的協(xié)方差，路標(biāo)的協(xié)方差以及機(jī)器人與路標(biāo)之間的協(xié)方差。下圖為協(xié)方差矩陣的一個(gè)形式。
左上角第一個(gè)單元A描述的是機(jī)器人位置的協(xié)方差矩陣，其為一個(gè)3*3的矩陣。B為路標(biāo)第一個(gè)路標(biāo)的協(xié)方差矩陣，其為2*2的格式。C為最后一個(gè)路標(biāo)的協(xié)方差矩陣。E機(jī)器人狀態(tài)與第一個(gè)路標(biāo)之間的協(xié)方差矩陣。D與E互為轉(zhuǎn)置關(guān)系?？梢钥闯?，雖然協(xié)方差矩陣看起來(lái)較為復(fù)雜，但實(shí)際上其是有跡可循的。
在初始時(shí)刻，由于機(jī)器人并不知道任何路標(biāo)的存在，因而P=A。初始化時(shí)，一般設(shè)置初始協(xié)方差矩陣為對(duì)角陣，反映的是初始位置的不確定性。雖然初始位置可能是精確的，單如果不包含初始誤差，在之后的計(jì)算過(guò)程中可能會(huì)導(dǎo)致矩陣的奇異性。
3. 卡爾曼增益K
卡爾曼增益描述的是我們?cè)诠烙?jì)系統(tǒng)真實(shí)狀態(tài)時(shí)對(duì)觀測(cè)值和計(jì)算值的信任程度。舉例來(lái)說(shuō)，假設(shè)我們通過(guò)計(jì)算得到機(jī)器人應(yīng)該右移10cm，根據(jù)觀測(cè)到的地標(biāo)，我們可能發(fā)現(xiàn)機(jī)器人移動(dòng)了5cm，因而此時(shí)我們就需要衡量機(jī)器人所處的真實(shí)位置?？柭鲆嫱ㄟ^(guò)觀測(cè)地標(biāo)以及機(jī)器人運(yùn)動(dòng)估計(jì)的不確定性進(jìn)行計(jì)算。如果測(cè)量元件精度相對(duì)于機(jī)器人運(yùn)動(dòng)估計(jì)，那么卡爾曼增益將會(huì)較?。环粗^大。卡爾曼增益矩陣的形式如下圖所示。
其中，第一列表示系統(tǒng)狀態(tài)第一列的新息的增益。第一列前兩行的元素表示位置新息增益，第三行表示角度新息增益。卡爾曼增益矩陣為(3+2n)*2的矩陣，其中n為地標(biāo)數(shù)目。
4. 測(cè)量模型的雅可比矩陣H
測(cè)量模型的雅可比矩陣與測(cè)量模型息息相關(guān)。這里，我們首先討論測(cè)量模型的構(gòu)造。測(cè)量模型描述的是如何計(jì)算期望的位置和角度信息，它通過(guò)下面的過(guò)程計(jì)算得到：
其中，
雅可比矩陣表示的地標(biāo)的位置與方位角隨著機(jī)器人位置和方位角變化的程度。第一行的第一個(gè)元素為地標(biāo)位置相對(duì)于機(jī)器人x坐標(biāo)的變化；第二個(gè)元素為相對(duì)于機(jī)器人y坐標(biāo)的變化；最后一項(xiàng)為相對(duì)于機(jī)器人
其中第一行僅用作指示，并不是矩陣的一部分。上面意味著前三列為常規(guī)雅可比矩陣。對(duì)于每一個(gè)地標(biāo)，我們?cè)黾觾闪校溆酁?，上圖所示即為第二個(gè)地標(biāo)的觀測(cè)雅可比矩陣。
5. 預(yù)測(cè)模型的雅可比矩陣：A
與上述測(cè)量模型的雅可比矩陣一樣，預(yù)測(cè)模型的雅可比矩陣與預(yù)測(cè)模型緊密相關(guān)，因而，這里我們首先考慮預(yù)測(cè)模型。預(yù)測(cè)模型表示的是如何根據(jù)機(jī)器人上一時(shí)刻的位置以及輸出計(jì)算下一時(shí)刻的機(jī)器人的位置，其計(jì)算過(guò)程如下：
其中，x和y為機(jī)器人位置，
與上述觀測(cè)矩陣計(jì)算方式一樣，只不過(guò)我們現(xiàn)在增加了一列機(jī)器人角度列。由于預(yù)測(cè)方程僅用于機(jī)器人位置的預(yù)測(cè)，而與地標(biāo)無(wú)關(guān)，因而雅可比矩陣相對(duì)比較簡(jiǎn)單。
6. SLAM特定的雅可比矩陣和
在SLAM中，還存在兩個(gè)與常規(guī)EKF不同的雅可比矩陣，分別為和
。其中，
與預(yù)測(cè)模型雅可比矩陣基本一致，只不過(guò)，我們并不考慮機(jī)器人角度項(xiàng)，如下：
7. 過(guò)程噪聲Q和W
這里，我們假設(shè)系統(tǒng)中的噪聲為高斯噪聲，其幅值與控制幅值成正比。過(guò)程噪聲的協(xié)方差矩陣為3*3的矩陣，如下所示：
其一般通過(guò)過(guò)程噪聲進(jìn)行自相關(guān)得到：
8. 測(cè)量噪聲R和V
測(cè)量期間同樣也假設(shè)包含高斯噪聲。其協(xié)方差矩陣通過(guò)VRV^T計(jì)算得到，其中V為2*2的單位矩陣。其格式如下：
其中各元素表示測(cè)量期間的精度，精度越低，測(cè)量噪聲越大；
10. SLAM的過(guò)程
1. 根據(jù)機(jī)器人運(yùn)動(dòng)信息預(yù)測(cè)機(jī)器人當(dāng)前位置，使用下面方程實(shí)現(xiàn)：
此時(shí)，我們需要更新預(yù)測(cè)模型的雅可比矩陣以及預(yù)測(cè)誤差向量如下圖：
最后，我們需要計(jì)算當(dāng)前機(jī)器人位置的新息，如下：
其中,現(xiàn)在，我們擁有了機(jī)器人位置的預(yù)測(cè)值以及當(dāng)前機(jī)器人位置估計(jì)的方差，接下來(lái)我們需要更新機(jī)器人的協(xié)方差矩陣，如下：
2. 根據(jù)觀測(cè)到的地標(biāo)信息更新?tīng)顟B(tài)估計(jì)值
由于機(jī)器人運(yùn)動(dòng)模型的誤差，在第一步中我們得到的機(jī)器人位置并不是機(jī)器人真實(shí)的位置，因而，我們需要通過(guò)觀測(cè)值對(duì)上述估計(jì)進(jìn)行修正。我們前面已經(jīng)討論了如何提取以及關(guān)聯(lián)特征，這里不再討論，使用機(jī)器人觀測(cè)到的關(guān)聯(lián)特征的變化，我們可以計(jì)算機(jī)器人的位移。進(jìn)一步的，我們可以更新機(jī)器人位置的估計(jì)值。
這里，我們將根據(jù)當(dāng)前機(jī)器人位置的估計(jì)值以及目前存儲(chǔ)的地標(biāo)位置
利用下面的公式計(jì)算地標(biāo)位置和角度的估計(jì)值：
通過(guò)對(duì)比上面計(jì)算得到的地標(biāo)位置與測(cè)量得到的地標(biāo)位置，我們可以計(jì)算此時(shí)的新息以及此時(shí)測(cè)量模型雅可比矩陣。
根據(jù)前文所述，此時(shí)雅可比矩陣為：
此時(shí)，同樣我們需要更新誤差矩陣R反映當(dāng)前測(cè)量信息的不確定性。測(cè)量誤差矩陣R的初始值可以設(shè)定為位置不確定性為1%,角度不確定性為1°。這里，誤差不應(yīng)該與測(cè)量值成正比。測(cè)量誤差矩陣如下：
根據(jù)上面的計(jì)算，我們可以計(jì)算卡爾曼增益，其可以通過(guò)下面的方式計(jì)算：
卡爾曼增益表示的是如何根據(jù)當(dāng)前估計(jì)值與測(cè)量值更新當(dāng)前的估計(jì)值。其中
被稱(chēng)作信息協(xié)方差S。
最后，我們可以使用上述卡爾曼增益計(jì)算一個(gè)新的狀態(tài)向量。
上式將會(huì)更新當(dāng)前機(jī)器人位置以及各個(gè)地標(biāo)的位置。上述步驟對(duì)每一個(gè)地標(biāo)均重復(fù)進(jìn)行，直至對(duì)所有地標(biāo)完成計(jì)算。
3. 為當(dāng)前系統(tǒng)狀態(tài)增加新的地標(biāo)
這里，我們將在當(dāng)前系統(tǒng)狀態(tài)與協(xié)方差矩陣P中增加新的地標(biāo)，目的是為了能夠匹配更多的地標(biāo)。
首先，我們可以按照下面的形式增加地標(biāo)：
另外，我們需要在協(xié)方差矩陣中增加新的元素，如下圖灰色部分所示：
重復(fù)上述過(guò)程即完成了SLAM的所有過(guò)程。
10. 結(jié)語(yǔ)
上面提到的SLAM僅僅為非?；镜腟LAM計(jì)算過(guò)程。SLAM目前仍然是研究的熱點(diǎn)，其中有許多領(lǐng)域我們并沒(méi)有提到，同樣上述SLAM存在很大的提高余地。舉例來(lái)說(shuō)，機(jī)器人的閉環(huán)問(wèn)題，即如何使機(jī)器人重新回到之前出現(xiàn)的地方。機(jī)器人應(yīng)該能夠識(shí)別出再次出現(xiàn)的地標(biāo)并且使用最先出現(xiàn)的信息更新地標(biāo)信息。另外，機(jī)器人應(yīng)該在機(jī)器人回到一個(gè)已知地方之前更新地標(biāo)信息。
另外，我們也可以將SLAM與網(wǎng)格方法相結(jié)合，將地圖映射為人類(lèi)可以讀懂的格式另外，通過(guò)將SLAM與網(wǎng)格方法相結(jié)合，我們還可以進(jìn)行機(jī)器人的路徑規(guī)劃。
參考文獻(xiàn)：
本文大部分內(nèi)容譯自：
SLAM for Dummies- A Tutorial Approach to Simultaneous Localization and Mapping
業(yè)務(wù)實(shí)施流程 馬上咨詢(xún)： 如果您有業(yè)務(wù)方面的問(wèn)題或者需求，歡迎您咨詢(xún)！我們帶來(lái)的不僅僅是技術(shù)，還有行業(yè)經(jīng)驗(yàn)積累。
QQ: 39764417/308460098 Phone: 13 9800 1 9844 / 135 6887 9550 聯(lián)系人：石先生/雷先生