NO IMAGE
轉自
作者:丁有爽
連結:https://zhuanlan.zhihu.com/p/21381490
來源:知乎
著作權歸作者所有。商業轉載請聯絡作者獲得授權,非商業轉載請註明出處。

本文的主要目的是簡單介紹移動機器人領域中廣泛應用的技術SLAM(同步定位與地圖繪製)的理論基礎以及應用細節。雖然目前存在很多關於SLAM技術的方方面面的論文,但是對於一個新手來說,仍然需要花費大量的時間去調研與把握SLAM發展的脈絡。本文希望能夠將SLAM技術在保持一些理論基礎的前提下,能夠按照一種簡單易懂的方式呈現出現了。在閱讀完本文後,讀者應該可以在一個移動機器人上實現最簡單使用的SLAM技術。

SLAM可以通過多種方法實現,首先其可以在多種不同的硬體上實現。其次,SLAM更像是一個概念而不是一個演算法。SLAM技術包含了許多步驟,其中的每一個步驟均可以使用不同的演算法實現。在這裡,我們對其中的每一步介紹一種最為常見的方法,至於其他的方法僅作一個簡介。

本文的目的是一種非常簡單實用的方式將SLAM技術呈現出來,如果讀者有一定SLAM技術的技術,可以看出這裡提供了一種基於EKF(擴充套件卡爾曼濾波)的完整的解決方案。需要注意的是,SLAM技術仍然是目前機器人領域的研究熱點之一,仍然有許多問題需要深入研究。

1. 關於SLAM

SLAM是同步定位與地圖構建(Simultaneous Localization And Mapping)的縮寫,最早由Hugh Durrant-Whyte 和 John J.Leonard提出。SLAM主要用於解決移動機器人在未知環境中執行時定位導航與地圖構建的問題。

SLAM通常包括如下幾個部分,特徵提取,資料關聯,狀態估計,狀態更新以及特徵更新等。對於其中每個部分,均存在多種方法。針對每個部分,我們將詳細解釋其中一種方法。在實際使用過程中,讀者可以使用其他的方法代替本文中說明的方法。這裡,我們以室內環境中執行的移動機器人為例進行說明,讀者可以將本文提出的方法應用於其他的環境以及機器人中。

SLAM既可以用於2D運動領域,也可以應用於3D運動領域。這裡,我們將僅討論2D領域內的運動。

2. 機器人平臺

在學習SLAM的過程中,機器人平臺是很重要的,其中,機器人平臺需要可以移動並且至少包含一個測距單元。我們這裡主要討論的是室內輪式機器人,同時主要討論SLAM的演算法實現過程,而並不考慮一些複雜的運動模型如人形機器人。

在選擇機器人平臺時需要考慮的主要因素包括易用性,定位效能以及價格。定位效能主要衡量機器人僅根據自身的運動對自身位置進行估計的能力。機器人的定位精度應該不超過2%,轉向精度不應該超過5%。一般而言,機器人可以在直角座標系中根據自身的運動估計其自身的位置與轉向。

從0開始搭建機器人平臺將會是一個耗時的過程,也是沒有必要的。我們可以選擇一些市場上成熟的機器人開發平臺進行我們的開發。這裡,我們以一個非常簡單的自己開發的機器人開發平臺討論,讀者可以選擇自己的機器人開發平臺。

目前比較常見的測距單元包括鐳射測距、超聲波測距以及影象測距三種。其中,鐳射測距是最為常用的方式。通常鐳射測距單元比較精確、高效並且其輸出不需要太多的處理。其缺點在於價格一般比較昂貴(目前已經有一些價格比較便宜的鐳射測距單元)。鐳射測距單元的另外一個問題是其穿過玻璃平面的問題。另外鐳射測距單元不能夠應用於水下測量。

另外一個常用的測距方式是超聲波測距。超生波測距以及聲波測距等以及在過去得到十分廣泛的應用。相對於鐳射測距單元,其價格比較便宜;但其測量精度較低。鐳射測距單元的發射角僅0.25°,因而,鐳射基本上可以看作直線;相對而言,超聲波的發射角達到了30°,因而,其測量精度較差。但在水下,由於其穿透力較強,因而,是最為常用的測距方式。最為常用的超聲波測距單元是Polaroid超聲波發生器。

第三種常用的測距方式是通過視覺進行測距。傳統上來說,通過視覺進行測距需要大量的計算,並且測量結果容易隨著光線變化而發生變化。如果機器人執行在光線較暗的房間內,那麼視覺測距方法基本上不能使用。但最近幾年,已經存在一些解決上述問題的方法。一般而言,視覺測距一般使用雙目視覺或者三目視覺方法進行測距。使用視覺方法進行測距,機器人可以更好的像人類一樣進行思考。另外,通過視覺方法可以獲得相對於鐳射測距和超聲波測距更多的資訊。但更過的資訊也就意味著更高的處理代價,但隨著演算法的進步和計算能力的提高,上述資訊處理的問題正在慢慢得到解決。

這裡,我們使用鐳射測距方法進行距離測量。其可以很容易實現較高的測量精度並且很容易應用於SLAM中。

3. SLAM的一般過程

SLAM通常包含幾個過程,這些過程的最終目的是更新機器人的位置估計資訊。由於通過機器人運動估計得到的機器人位置資訊通常具有較大的誤差,因而,我們不能單純的依靠機器人運動估計機器人位置資訊。在使用機器人運動方程得到機器人位置估計後,我們可以使用測距單元得到的周圍環境資訊更正機器人的位置。上述更正過程一般通過提取環境特徵,然後在機器人運動後重新觀測特徵的位置實現。SLAM的核心是EKF。EKF用於結合上述資訊估計機器人準確位置。上述選取的特徵一般稱作地標。EKF將持續不斷的對上述機器人位置和周圍環境中地標位置進行估計。SLAM的一般過程如下圖所示:

當機器人運動時,其位置將會發生變化。此時,根據機器人位置感測器的觀測,提取得到觀測資訊中的特徵點,然後機器人通過EKF將目前觀測到特徵點的位置、機器人運動距離、機器人運動前觀測到特徵點的位置相互結合,對機器人當前位置和當前環境資訊進行估計。

下圖是估計的詳細過程。

上圖中三角形表示機器人,星號表示路標;機器人首先使用測距單元測量地標相對於機器人的距離和角度。

然後進行開始進行運動,並且到達一個新的位置,機器人根據其運動方程預測其現在所處於的新的位置。

在新的位置,機器人通過測距單元重新測量各個地標相對於機器人的距離和角度,測量得到的距離和角度與上述預測結果可能並不一致,因而,上述預測值可能並不是機器人準確位置。

在機器人看來,通過感測器獲得的資訊相對於通過運動方程得到的資訊更為準確,因而,機器人將通過感測器的資料更新對機器人位置的預測值,如上圖中實線三角形所示(虛線為第一步中通過運動資訊預測的機器人位置)。

經過上述結合直軸,我們重新估計得到的新的機器人位置如上圖實線三角形所示,但由於測距單元精度有限,因而,此時,機器人可能實際處於上圖點狀三角形位置,但此時估計結果相對於初始預測結果已經有明顯的改善。

4. 測距單元

SLAM的第一步需要通過測距單元獲取機器人周圍環境的資訊。這裡,我們以鐳射測距單元為例。以一個常見的鐳射測距單元為例,其測量範圍可到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

鐳射測距單元的輸出表示機器人距最近障礙物的距離。如果由於某些原因,鐳射測距單元無法測量某個特定角度上的安全範圍,那麼其將返回一個最大值,這裡以8.1為例,測距單元返回資料超過8.1即意味著鐳射測距單元在該角度上發生測量錯誤。需要注意的是,鐳射測距單元可以以很高的頻率對周圍環境進行測量,其可以實現10-100Hz的全周掃描。

5. 機器人自身運動模型

SLAM的另外一個很重要的資料來源是機器人通過自身運動估計得到的自身位置資訊。機器人自身位置資料通過對機器人輪胎執行圈數的估計可以得到機器人自身位置的一個估計,其可以被看作EKF的初始估計資料。

另外一個需要注意的是需要保證機器人自身位置資料與測距單後設資料的同步性。為了保證其同步性,一般採用插值的方法對資料進行前處理。由於機器人的運動規律是連續的,因而,一般對機器人自身位置資料進行插值。相對而言,由於測距單後設資料的不連續性,因而其插值基本上是不可以實現的。

6. 特徵(地標)

地標是環境中易於觀測和區分的特徵。一般使用這些特徵確定機器人位置。我們可以通過下面的方法想象上述工作過程,假設在一個陌生的房間內,閉上眼睛,那麼此時我們如何確定自身的位置呢?通常而言,我們將在環境中不斷走動,通過觸控物體或者牆壁確定自身位置。上述如被觸控的物體以及牆壁等都可以被看做用於估計自身位置的地標。下面是一些典型的地標。

可以看出,通常而言,對於不同的環境,一般我們選擇不同的地標。

一般而言,地標需要滿足下面的條件:

1. 地標應該可以從不同的位置和角度觀察得到;

2. 地標應該是獨一無二的,從而可以很容易的將底邊從其他物體中分辨出來

3. 地標不應該過少,從而導致機器人需要花費額外的代價尋找地標;

4. 地標應該是靜止的,因而,我們最好不要使用一個人作為地標

舉例來說,室內環境中的地標,我們可以選擇為牆壁與地面之間的連線,以及牆角等。如下圖所示:

7.特徵提取

根據上述步驟6確定完需要提取的特徵後,我們接下來需要從測距單元獲得的資訊中準確的提取出我們需要的特徵。特徵提取的方法有很多種,其主要取決於需要提取特徵以及測距單元的型別。

這裡,我們將以如何從鐳射雷達得到的資訊提取有效特徵為例進行說明。這裡,我們使用兩種典型的特徵提取方法,Spike方法和RANSAC方法。

Spike方法使用極值尋找特徵。通過尋找測距單元返回資料中相鄰資料差距超過一定範圍的點作為特徵點。通過這種方法,當測距單元發射的光束從牆壁上反射回來時,測距單元返回的數值為某些值;而當發射光束碰到其他物體並反射回來時,此時測距單元將返回另外一些數值;兩者將具有較大的差別。如下圖所示:

圖中紅點為根據Spike方法提取到的特徵。

Spike方法也可以通過下面的步驟實現,對於相鄰的三個點A,B,C,分別計算(A-B)與(C-B),然後將兩者相加,如果結果超過一定範圍,則表示提取到一個特徵。

採用Spike方法提取環境特徵,需要保證相鄰兩個鐳射束照射的物體距離機器人距離之間具有較大的變化,因而,其並不能夠適用於光滑環境中的特徵提取。

RANSAC(隨機取樣方法)也可以被用於從鐳射測距單元返回資料中提取系統特徵。其中測距單元返回資料中的直線將被提取為路標。在室內環境中,由於廣泛存在牆壁等,因而,在測距單元返回的資料中將存在大量的直線。

RANSAC首先採用隨機取樣的方法提取測距單元返回資料中的一部分,然後採用最小二乘逼近方法來對上述資料進行擬合。進行資料擬合後,RANSAC方法將會檢查測距單後設資料在擬合曲線周圍的分佈情況。如果分佈情況滿足我們的標準,那麼我們就認為機器人看到了一條直線。

在使用EKF估計機器人位置和環境地圖時,EKF需要將地標按照距離機器人當前的位置和方位表示出來。我們可以很容易的使用三角幾何的方法將提取到的直線轉變為固定的特徵點,如下圖所示:

前面我們提到了特徵提取的兩種不同的方法,Spike方法和RANSAC方法,兩者均可以用於室內環境中特徵的提出。相比較而言,Spike方法較為簡單,並且對室內環境中的活動物體不具有魯棒性。RANSAC方法通過提取直線的方法提取環境中的特徵,相對較為複雜,但對室內活動的物體具有更好的適應性。

8. 資料關聯

資料關聯是將不同時刻位置感測器提取到的地標資訊進行關聯的過程,也成為重觀察過程。

舉例來說,對於我們人類來說,假設我們在一個房間內看到了一把椅子,現在我們離開房間,過一段時間後,再次回到房間,如果我們再次看到了椅子,那麼我們可以認為這把椅子很有可能就是我們之前看到的椅子。但是,如果我們假設房價內有兩把完全一樣的椅子,重複上述過程,當我們再次來到房間後,我們可能無法區分我們看到的兩把椅子。但我們可以猜測,此時比如說左邊的椅子仍然是之前看到的左邊的椅子,而右邊的椅子仍然是之前看到的右邊的椅子。

在實際應用中進行資料關聯時,我們可能遇到下面的問題:

1. 我們可能上一次看到了某個地標,但下一次卻沒有看到;

2.我們可能這次看到了地標,但之後卻再也看不到這個地標;

3.我們可能錯誤的將現在看到的某個地標與之前看到的某個地標進行關聯;

根據我們選擇路標時的標準,我們可以很容易的排除上面第1和第2個問題。但對於第三個問題,如果發生了,將會對我們的導航以及地圖繪製造成嚴重的問題。

現在我們將討論解決上面第三個問題的方法。假設我們現在已經到了每時每刻採集處理得到的路標的方位資訊,並將其其中的特徵儲存在一資料庫中。資料庫初始階段是空的,首先我們建立的第一條規則是,除非該特徵已經出現了N次,否則我們並不將其加入資料庫。當得到一副新的感測器資訊後,我們進行下面的計算:

1.得到一副新的感測器資訊後,首先利用上面的特徵提取方法提取特徵;

2.將提取到的特徵與資料庫中已經出現N次的並且距離最近的特徵關聯起來;

3.通過驗證環節驗證上面的關聯過程是正確的,如果驗證通過,則表明我們再次看到了某個物體,因而其出現次數 1,否則表明我們看到了一個新的特徵,在資料庫中新建一個特徵,並將其記作1.

上述特徵關聯方法被稱作距離最近方法。上面最簡單的計算距離的方法是歐式距離的計算,其他距離計算包括馬氏距離等,雖然效果更好,但計算結果更為複雜。

驗證環節通過利用EKF執行過程中,觀測誤差的有界性進行判斷。因而,我們可以通過判斷一個路標是否在現存路標的誤差允許範圍內來判斷其是否為新的路標。路標區域可以通過圖形繪製或者定義一個橢圓誤差。

通過設定一個常數\lambda,最新觀測到的路標可以通過下面的公式與之前觀測到的路標相互關聯。

其中,v_i為觀測新息,S_i為特徵的協方差矩陣。

9. 擴充套件卡爾曼濾波器(EKF)

在SLAM中,我們一般使用擴充套件卡爾曼濾波器基於機器人運動資訊與感測器測量特徵點資訊估計機器人的狀態。這裡,我們將詳細討論將其應用於SLAM中的具體步驟。

在得到路標點的位置和方位,並且將路標點進行關聯後,SLAM的過程分為如下三個部分:

1. 基於機器人運動資訊更新機器人當前狀態;

2. 基於路標資訊更新估計狀態;

3. 在當前狀態中增加新的狀態;

第一步相對來說較為簡單,其僅僅需要將當前控制器的輸出疊加到上一時刻的狀態上。舉例來說,假設機器人目前位於(x,y,\theta),當前運動資訊為(dx,dy,d\theta),那麼第一部的機器人的當前狀態為(x dx,y dy,\theta d\theta).

在第二步中,我們需要考慮路標資訊,基於當前機器人的狀態,我們可以估計路標應該處於的位置。這個估計位置與測量位置有所差別,這個差別被稱作新息。新息即為機器人估計根據機器人狀態估計資訊與實際資訊之間的差別。此時,根據上述新息,每個特徵點的方法同時被更新。

在第三步中,觀測到的新的路標被加入到狀態中,即當前環境的地圖。

下面我們對SLAM中常用的一些定義進行說明。

1.系統狀態X

系統狀態以及其協方差矩陣式SLAM過程中最為重要的向量。X包含了機器人的位置(x,y,\theta)以及環境中每個路標的資訊。其格式如下圖所示:

可以看出,系統狀態X為一個(3 2*n)*1的矩陣,即列向量,其中n為路標的個數。其單位一般為米(m)或者毫米(mm)。角度一般選擇為弧度。

2. 協方差矩陣P

兩個變數之間的協方差矩陣描述的是兩個變數之間的相關程度。

這裡,協方差矩陣包含了機器人位置的協方差,路標的協方差以及機器人與路標之間的協方差。下圖為協方差矩陣的一個形式。

左上角第一個單元A描述的是機器人位置的協方差矩陣,其為一個3*3的矩陣。B為路標第一個路標的協方差矩陣,其為2*2的格式。C為最後一個路標的協方差矩陣。E機器人狀態與第一個路標之間的協方差矩陣。D與E互為轉置關係。可以看出,雖然協方差矩陣看起來較為複雜,但實際上其是有跡可循的。

在初始時刻,由於機器人並不知道任何路標的存在,因而P=A。初始化時,一般設定初始協方差矩陣為對角陣,反映的是初始位置的不確定性。雖然初始位置可能是精確的,單如果不包含初始誤差,在之後的計算過程中可能會導致矩陣的奇異性。

3. 卡爾曼增益K

卡爾曼增益描述的是我們在估計系統真實狀態時對觀測值和計算值的信任程度。舉例來說,假設我們通過計算得到機器人應該右移10cm,根據觀測到的地標,我們可能發現機器人移動了5cm,因而此時我們就需要衡量機器人所處的真實位置。卡爾曼增益通過觀測地標以及機器人運動估計的不確定性進行計算。如果測量元件精度相對於機器人運動估計,那麼卡爾曼增益將會較小;反之較大。卡爾曼增益矩陣的形式如下圖所示。

其中,第一列表示系統狀態第一列的新息的增益。第一列前兩行的元素表示位置新息增益,第三行表示角度新息增益。卡爾曼增益矩陣為(3 2n)*2的矩陣,其中n為地標數目。

4. 測量模型的雅可比矩陣H

測量模型的雅可比矩陣與測量模型息息相關。這裡,我們首先討論測量模型的構造。測量模型描述的是如何計算期望的位置和角度資訊,它通過下面的過程計算得到:

其中,\lambda_x表示地標的x方向座標;x表示估計機器人位置;同理,\lambda_y表示地標的y方向座標;y表示機器人的估計位置,\theta為機器人方位角。通過上述公式我們便可以得到地標的位置和方位資訊。其雅可比矩陣為:

雅可比矩陣表示的地標的位置與方位角隨著機器人位置和方位角變化的程度。第一行的第一個元素為地標位置相對於機器人x座標的變化;第二個元素為相對於機器人y座標的變化;最後一項為相對於機器人\theta座標的變化。可以看出,當機器人做純旋轉運動時,地標的位置並不發生變化。第二行表示的地標所處角度相對於機器人座標變化的情況。上面的雅可比矩陣式我們使用EKF時常見的雅可比矩陣。在做SLAM時,我們通常需要一些額外的資訊,如下:

其中第一行僅用作指示,並不是矩陣的一部分。上面意味著前三列為常規雅可比矩陣。對於每一個地標,我們增加兩列,其餘為0,上圖所示即為第二個地標的觀測雅可比矩陣。

5. 預測模型的雅可比矩陣:A

與上述測量模型的雅可比矩陣一樣,預測模型的雅可比矩陣與預測模型緊密相關,因而,這裡我們首先考慮預測模型。預測模型表示的是如何根據機器人上一時刻的位置以及輸出計算下一時刻的機器人的位置,其計算過程如下:

其中,x和y為機器人位置,\theta為機器人角度,\Delta t為時間間隔,q為誤差項,因而,雅可比矩陣表示為:

與上述觀測矩陣計算方式一樣,只不過我們現在增加了一列機器人角度列。由於預測方程僅用於機器人位置的預測,而與地標無關,因而雅可比矩陣相對比較簡單。

6. SLAM特定的雅可比矩陣J_{xr}J_{z}

在SLAM中,還存在兩個與常規EKF不同的雅可比矩陣,分別為J_{xr}J_{z}。其中,J_{xr}與預測模型雅可比矩陣基本一致,只不過,我們並不考慮機器人角度項,如下:

J_{z}為地標預測模型的雅可比矩陣,但是隻針對位置和角度,如下:

7. 過程噪聲Q和W

這裡,我們假設系統中的噪聲為高斯噪聲,其幅值與控制幅值成正比。過程噪聲的協方差矩陣為3*3的矩陣,如下所示:

其一般通過過程噪聲進行自相關得到:

8. 測量噪聲R和V

測量期間同樣也假設包含高斯噪聲。其協方差矩陣通過VRV^T計算得到,其中V為2*2的單位矩陣。其格式如下:

其中各元素表示測量期間的精度,精度越低,測量噪聲越大;

10. SLAM的過程

1. 根據機器人運動資訊預測機器人當前位置,使用下面方程實現:

此時,我們需要更新預測模型的雅可比矩陣以及預測誤差向量如下圖:

最後,我們需要計算當前機器人位置的新息,如下:

其中,p^{TT}表示P矩陣的前三列。

現在,我們擁有了機器人位置的預測值以及當前機器人位置估計的方差,接下來我們需要更新機器人的協方差矩陣,如下:

2. 根據觀測到的地標資訊更新狀態估計值

由於機器人運動模型的誤差,在第一步中我們得到的機器人位置並不是機器人真實的位置,因而,我們需要通過觀測值對上述估計進行修正。我們前面已經討論瞭如何提取以及關聯特徵,這裡不再討論,使用機器人觀測到的關聯特徵的變化,我們可以計算機器人的位移。進一步的,我們可以更新機器人位置的估計值。

這裡,我們將根據當前機器人位置的估計值(x,y)以及目前儲存的地標位置(\lambda_x,\lambda_y)利用下面的公式計算地標位置和角度的估計值:

通過對比上面計算得到的地標位置與測量得到的地標位置,我們可以計算此時的新息以及此時測量模型雅可比矩陣。

根據前文所述,此時雅可比矩陣為:

此時,同樣我們需要更新誤差矩陣R反映當前測量資訊的不確定性。測量誤差矩陣R的初始值可以設定為位置不確定性為1%,角度不確定性為1°。這裡,誤差不應該與測量值成正比。測量誤差矩陣如下:

根據上面的計算,我們可以計算卡爾曼增益,其可以通過下面的方式計算:

卡爾曼增益表示的是如何根據當前估計值與測量值更新當前的估計值。其中

被稱作資訊協方差S。

最後,我們可以使用上述卡爾曼增益計算一個新的狀態向量。

上式將會更新當前機器人位置以及各個地標的位置。上述步驟對每一個地標均重複進行,直至對所有地標完成計算。

3. 為當前系統狀態增加新的地標

這裡,我們將在當前系統狀態與協方差矩陣P中增加新的地標,目的是為了能夠匹配更多的地標。

首先,我們可以按照下面的形式增加地標:

另外,我們需要在協方差矩陣中增加新的元素,如下圖灰色部分所示:

重複上述過程即完成了SLAM的所有過程。

10. 結語

上面提到的SLAM僅僅為非常基本的SLAM計算過程。SLAM目前仍然是研究的熱點,其中有許多領域我們並沒有提到,同樣上述SLAM存在很大的提高餘地。舉例來說,機器人的閉環問題,即如何使機器人重新回到之前出現的地方。機器人應該能夠識別出再次出現的地標並且使用最先出現的資訊更新地標資訊。另外,機器人應該在機器人回到一個已知地方之前更新地標資訊。

另外,我們也可以將SLAM與網格方法相結合,將地圖對映為人類可以讀懂的格式另外,通過將SLAM與網格方法相結合,我們還可以進行機器人的路徑規劃。

參考文獻:

本文大部分內容譯自:

SLAM for Dummies-A Tutorial Approach to Simultaneous Localization and Mapping