5，SLAM的處理過程

SLAM過程包括許多步驟，該過程的目標是使用環境更新機器人的位置。由於機器人的里程計通常是存在誤差的，我們不能直接依賴於里程計。我們可以用鐳射掃描環境來校正機器人的位置，這是通過從環境中提取特徵並不斷觀察機器人移動來實現的。EKF(擴充套件卡爾曼濾波器)是SLAM過程的核心。EKF負責基於環境特徵對機器人位置進行更新，這些環境特徵通常被稱為LANDMARK，以下我們統稱為地標，關於地標，將在接下來的幾章中與EKF一起解釋。EKF對機器人位置的不確定性以及在環境中所見的地標的不確定性進行了跟蹤。

下圖給出了SLAM過程的概要。(1、鐳射感測器對環境特徵（地標）進行提取->資料融合給EKF演算法；)

當里程計發生變化時，由於機器人的移動的有一個不確定的誤差，因此新的機器人位置將基於里程計的更新資料經過EKF處理進行更新。地標將從機器人所處的新的環境中被提取出來，然後機器人試圖將這些地標與之前看到的地標聯絡起來。重新觀察的地標被用來更新機器人在EKF中的位置。以前未見過的地標被新增到EKF中作為新的觀測依據，以便以後可以重新觀測。所有這些步驟將在接下來的章節中以非常實用的方式解釋我們的ER1機器人是如何實現的。需要注意的是，在這些步驟中的任何時刻EKF都將對機器人當前的位置進行估計。

下面的圖表將試圖更詳細地解釋這個過程:機器人由三角形表示。星星代表地標。機器人最初用感測器測量地標的位置(用閃電錶示的感測器測量)。

圖2

-------------------------------------------------------------------------------------------

機器人根據編碼器等算出來的里程計資料，認為他自己在如下圖3的位置

圖3

--------------------------------------------------------------------------------------

同步地，機器人再次使用鐳射感測器測量地標，算出機器人在圖4的位置，但發現這個位置與機器人計步法認為它們應該在的位置不匹配。因此，機器人並不在它根據編碼器里程計測算出的圖3的位置。

圖4

-----------------------------------------------------------------------------------------------------------------------------

由於相比計步法里程計位置，機器人更相信鐳射感測器，所以現在它利用地標所獲得的資訊來確定自己的位置點線三角形(機器人最初認為自己的位置由虛線三角形表示)。

圖5

-------------------------------------------------------------------------------------------------------------------------------------------

事實上，機器人卻在這圖6實線三角形。鐳射感測器也並不完美，因此可以確切的說，機器人無法精確地知道它在哪裡。然而，這個鐳射的感測器估計比僅僅依靠編碼器計步法要好。點線三角形表示機器人通過鐳射感測器認為的位置;這個虛線三角形是計步法里程計告認為的位置;然而EKF演算法卻不相信他們任何一個是精確地位置，於是他算出了最後一個實線三角形的位置。

圖6

6，鐳射資料
        SLAM過程的第一步是獲取機器人周圍環境的資料。當我們選擇使用鐳射掃描器時，我們就可以得到鐳射資料。我們使用的SICK鐳射掃描器可以輸出一個角度100°或180°測量範圍。垂直解析度為0.25°、0.5°或1.0°,意味著鐳射測量的區域寬度是0.25°,寬0.5°或1.0°。典型的鐳射掃描器輸出如下:
2.98,2.99,3.00,2.98,2.99,3.49,3.50,....,2.20,8.17,2.21
鐳射掃描器的輸出以米表示從右到左的範圍。
如果由於某種原因，鐳射掃描器不能測出特定角度的確切長度，它將返回一個高值，我們使用8.1作為閾值來判斷該值是否為錯誤的。但是一些鐳射掃描器可以配置超過8.1米的測量範圍，這裡只需要配置為8.1m。最後需要注意的是鐳射掃描器資料輸出非常快，使用串列埠可以達到11HZ的頻率。
與鐳射掃描器介面的程式碼見附錄B:SICK LMS200 interface code.
7，里程計資料
        SLAM的一個重要方面是里程計資料。里程計資料的目的是提供一個機器人的大致位置，通過機器人車輪的運動來測量，作為在擴充套件卡爾曼濾波器中機器人可能在哪裡的初步猜測。使用內建的telnet伺服器很容易從ER1機器人獲得里程數資料。您可以將一個文字字串傳送到特定埠上的telnet伺服器，伺服器將返回應答。
計步測程資料和鐳射資料的難點在於正確的獲得時間，就是控制兩資料同步獲取和資料融合。如果以後檢索里程計資料，那麼某個時刻的鐳射資料將會過時失效。為了確保它們同時有效，我們可以推斷資料。由於運動控制是已知的，所以最容易推斷里程計資料。很難預測鐳射掃描器的測量結果。如果可以控制返回測量值的時間，那麼同時請求鐳射掃描器值和測程資料就比較容易了。與ER1介面的程式碼附錄C: ER1 interface code.

8，環境特徵（地標）
地標是那些很容易被重新觀察並區別於環境的特徵形狀。就是這些地標被機器人用來分析它在哪裡(定位自己)。
類比一下，你的眼睛被蒙起來行走，就能想象這個機器人是如何工作的。如果你被蒙著眼睛在你熟悉的房子裡移動，你可能會伸出手觸控物體或擁抱牆壁，這樣你就不會迷路了。諸如觸控門框這樣的特徵事物的感覺可以幫助你估計你在哪裡。聲吶和鐳射掃描器就是的機器人的觸覺。
以下是來自不同環境的良好的地標:

圖7自由女神像是一個很好的地標，因為它是獨一無二的，可以很容易地從不同的環境或地點看到，如陸地、海洋和空氣。

圖8碼頭上的木柱可能是水下航行器的地標。
正如你所看到的，機器人所使用的地標型別往往取決於機器人所處的環境。
地標應該被可重新探測，例如從不同的位置，從不同的角度都能夠被探測出。地標應該足夠獨特，這樣就可以從一個時間幀到另一個時間幀被很容易地探測而不會混淆。換句話說，如果你在稍後的時間重新觀察兩個地標，你應該很容易就能確定哪個是我們之前見過的那個地標。如果兩個地標前後探測起來非常相近，這就比較尷尬了。
一個機器人在環境中給定識別的地標應該足夠多，否則沒有足夠的可見地標的話，機器人可能不得不花更長時間，這樣機器人可能會迷路。
如果你決定某物是一個地標它應該是靜止的。使用一個人作為地標是一個壞主意。這個標準的原因很簡單。如果這個地標並不總是在同一個地方，那麼機器人怎麼知道這個地標在哪裡呢?
怎麼利用它來定位呢，關於合適的地標，重點特點如下:
•地標很容易被重新探測。
•不同的地標應該相互區別。
•環境中的地標應該很豐富。
•地標應該是特定固定靜止的。

圖9在我們的機器人使用的室內環境中，有許多直線和清晰的角。這些都可以用作地標。

7.特徵提取

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

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

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

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

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

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

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

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

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