激光 2D SLAM 學習

激光 2D SLAM

本文主要介紹的是基於激光雷達的 2D SLAM，以及我最近看的粒子濾波、GMapping、Cartographer 等內容。

什麼是 SLAM

圖 1. cartographer 建的地圖

全稱 simultaneous localization and mapping，顧名思義，就是在機器人構建周圍環境的地圖的同時，定位它在地圖中的位置。

算法的輸入一般是：機器人對周圍環境的觀測信息 z (激光雷達的掃描點、攝像頭的圖像等) 和 機器人的控制信息 u (運動過程中的前進轉向等信息，一般對其積分後用里程計 odometry 來表示)；

算法的輸出是：機器人的運動軌跡 x 和 周圍的地圖 m 。

而我們要做的就是在給定 z 和 u 的條件下估計出 x 和 m：

p(x,m|z,u)$$p(x,m|z,u)$$

這裏我覺得可以稍微說一下爲什麼要用一個條件概率以及爲什麼可以用一個條件概率來表示我們的問題，因爲在一開始接觸機器人的時候我自己就有這樣的疑惑。

首先爲什麼要用一個條件概率而不是一個封閉式的函數？因爲我們無法對整個系統精確建模，或者說精確建模的代價太高，比如說傳感器自身的測量誤差（一般由廠家提供）、傳感器與環境交互時的誤差（激光雷達遇強光或黑色物體時的測量誤差、攝像頭在黑暗條件下的圖像不清晰）等等，這都使我們很難用一個封閉式的函數求得一個解析解，所以我們只能用概率形式來表示在存在各種誤差的情況下，使用 z 和 u 來估計 x 和 m 的分佈。並且在 2d SLAM 中， x 就是一個 [x,y,$[x,y,$ \theta]$]$ 的三維隨機變量， 如果 m 是柵格地圖， m 就是一個 r∗c$r\ast c$ 維的隨機變量，其中 r$r$ 是地圖的寬度， c$c$ 是地圖的長度，所以我們可以用一個條件概率來解決我們的問題。實際上，在與傳感器相關的問題中，很多問題都可以通過構建一個這樣的條件概率、後驗概率來求解。

難點

我們現在已經知道了 SLAM 問題應該如何表示了，那麼還有什麼難點在其中呢？難點就在於 x 和 m 之間是相互依賴的，也就是說：

1. 要想建圖，我們需要知道機器人在各個時刻的精確軌跡信息
2. 要想獲取精確的軌跡信息，我們需要藉助精確的地圖來定位

這也就是雞生蛋問題。那這如何解決呢？對我們之前的 SLAM 後驗分解一下看看：

圖 2. 條件概率

其中建圖那一項根據馬爾科夫性質省略了 u0:t−1$u_{0:t-1}$ ，因爲在已知軌跡和觀測信息的情況下，控制信息對於我們的建圖而言是沒有影響的。這樣分解的實際意義就是，先根據測量信息和控制信息計算出機器人的軌跡，然後根據軌跡和測量信息建圖。其實目前大部分 SLAM 算法都按照這個思路來實現的，並且主要關注於對運動軌跡的估計和優化上，因爲有了精確的軌跡，建圖就成了自然而然的事了。

地圖的種類

在繼續介紹 SLAM 的求解之前，我們先來介紹一下 SLAM 構建的地圖的種類。

我所知道的 SLAM 構建的地圖一般有兩類：volumetric map 和 feature map

圖 3. 地圖種類

其中 feature map 我沒有接觸過，我理解的就是在測量信息中提取特徵，將這些特徵設置爲 landmark ，並標示這些 landmark 的位置。這個地圖的好處是比較適合動態的場景，因爲當 landmark 移動到另一處時，我們只需要改變這個 landmark 的座標就可以了。

雖然我接觸的一直是 volumetric map ，但我還真不知道這該怎麼翻譯，不過在 2d 中， volumetric map 就是 grid map，也就是柵格地圖， 3d 中就是 voxel map，也就是體素地圖。因爲本文主要講的是 2d SLAM ，所以我還是主要講一下柵格地圖。

圖 4. 柵格地圖

柵格地圖主要就是把地圖離散化爲一個個的單元(cell)，每個單元佔據 r∗r$r\ast r$ 個像素，所以 r$r$ 的大小就代表了地圖的分辨率，位置在 (xi,yi)$(x_i,y_i)$ 的 cell 的 index 爲 i=xi∗y+yi$i=x_i\ast y+y_i$ 。

Assumption：

1. 每個 cell 有兩種狀態：free、occupied
2. 每個 cell 之間的狀態相互獨立

當 cell 爲 free 時，該 cell 的灰度值就設爲 255，也就是白色，當 cell 爲 occupied 時，該 cell 的灰度值就設爲 0 ，也就是黑色。初始時我們不知道 cell 具體的狀態，我們就將其設爲 100 ，也就是灰色。

接下來我們設 mi$m_i$ 爲第 i$i$ 個 cell 是否occupied 的隨機變量，那麼 p(mi=1)$p(m_i=1)$ 或 p(mi)$p(m_i)$ 就表示第 i$i$ 個 cell 爲 occupied 的概率，所以 p(mi)=1$p(m_i)=1$ 就表示第 i$i$ 個 cell 爲 occupied ，p(mi)=0$p(m_i)=0$ 就表示第 i$i$ 個 cell 爲 free ，如下圖 5 ：

圖 5. assumption 1

根據 assumption 2 ，我們很容易能得到下式：

p(m)=∏ip(mi)$$p(m)=\prod _{i}p(m_i)$$

圖 6. assumption 2

有了這些基礎概念，我們接下來就可以介紹各種具體的 SLAM 算法了。

已知軌跡的條件下建圖

這裏我們先介紹一下如何在已知軌跡的條件下建圖。也就是在給定傳感器數據 z1:t$z_{1:t}$ 和軌跡信息 x1:t$x_{1:t}$ 的條件下估計 t$t$ 時刻的地圖 m$m$ ，即 p(m|z1:t,x1:t)$p(m|z_{1:t},x_{1:t})$ 。上節我們說過，各個 cell 之間的狀態是相互獨立的，所以我們能夠得到：

p(m|z1:t,x1:t)=∏ip(mi|z1:t,x1:t)$$p(m|z_{1:t},x_{1:t})=\prod _{i}p(m_i|z_{1:t},x_{1:t})$$

也就是我們單獨的爲每個 cell 計算 p(mi|z1:t,x1:t)$p(m_i|z_{1:t},x_{1:t})$ 即可。這裏介紹一種最簡單的方法——基於頻率的方法——幫助我們快速理解這一計算過程。

首先定義各種用到的各種變量：

mit$m_t^i$ ：第 i$i$ 個 cell 在 t$t$ 時刻是否被 occupied

vit$v_t^i$ ：第 i$i$ 個 cell 被觀測的次數，也就是被激光掃描到的次數

bit$b_t^i$ ：觀測到第 i$i$ 個 cell 被 occupied 的次數

p(mit)$p(m_t^i)$ 的計算如下所示

---

每當接收到一次觀測數據時，計算

(bit,vit)={(bit−1+1,vit−1+1)(bit−1,vit−1+1)ifoccupiediffree$$(b_t^i,v_t^i)=\{\begin{array}{cc}(b_{t-1}^i+1,v_{t-1}^i+1)& \mathrm{i}\mathrm{f}\mathrm{o}\mathrm{c}\mathrm{c}\mathrm{u}\mathrm{p}\mathrm{i}\mathrm{e}\mathrm{d}\\ (b_{t-1}^i,v_{t-1}^i+1)& \mathrm{i}\mathrm{f}\mathrm{f}\mathrm{r}\mathrm{e}\mathrm{e}\end{array}$$

p(mit)=bitvit$$p(m_t^i)=\frac{b_t^i}{v_t^i}$$

---

基於頻率的方法比較簡單粗暴，缺點是對 cell 的初始狀態考慮不足。

《Probabilistic Robotics》 C9.2 介紹了更通用的算法來計算 p(mi|z1:t,x1:t)$p(m_i|z_{1:t},x_{1:t})$ ，這裏就先不介紹了，書上說的也比較明白。

總之，我們需要知道的就是，在已知運動軌跡的條件下進行建圖是比較簡單的， SLAM 真正的難點應該是估計機器人的運動軌跡，也可以說是估計某時刻的位置。

已知地圖的條件下預測軌跡

這一節介紹一下如何在已知地圖的條件下預測機器人的運動軌跡，也就是 p(x1:t|z1:t,u0:t−1)$p(x_{1:t}|z_{1:t},u_{0:t-1})$ ，也許你會有個疑問：不是說先預測軌跡，纔有地圖的麼，怎麼這裏能用地圖信息來預測軌跡呢？而且這裏的條件概率裏也沒有以地圖信息作爲條件啊。

這裏我們將要求解的式子稍微改變一下，改爲求

p(xt|xt−1,mt−1,zt,ut−1)$$p(x_t|x_{t-1},m_{t-1},z_t,u_{t-1})$$

也就是在已知 t−1$t-1$ 時刻的機器人位置、地圖和控制信息，以及 t$t$ 時刻的觀測信息的條件下，估計 t$t$ 時刻的機器人位置。其實這個式子跟之前那個是一樣的，只不過一個估計的是軌跡，一個估計的是下一時刻的位置，實際上大多數 SLAM 都是迭代式的估計下一時刻的位置來進行建圖的。接下來我就介紹一下粒子濾波是如何應用到機器人的位置估計的。

首先我們需要的其實就是能夠得到上式的概率密度函數，然後找到使該概率密度函數取最大值時的 xt$x_t$ ，但是這個概率密度函數很難直接求解，於是人們就想到了用蒙特卡洛的方法通過樣本來近似這個分佈，進而求出 xt$x_t$ 。這也就是基於蒙特卡洛的定位算法。

這裏先通過圖示來大致介紹一下蒙特卡洛方法是如何幫助機器人定位的。

圖 8. 蒙特卡洛定位

蒙特卡洛方法其實就是通過樣本來近似概率分佈。如圖 8.a ，在沒有任何觀測數據的時候，機器人完全不知道自己在地圖中的什麼位置，因此此時它的位置在地圖中應該是一個均勻分佈，那我們就隨機在地圖中選取 100 個樣本，各個樣本的位置如圖 8.a 的下圖所示。

但好在我們的機器人有“一雙眼睛”，可以看到自己目前是在一扇門的前面，那麼我們就可以對每個樣本計算從該位置看到的門與機器人“眼睛”看到的門的匹配程度了，匹配程度越高，我們也就爲該樣本賦予更高的權重，如圖 8.b 的下圖所示。

然後我們根據每個樣本的權重對所有樣本進行有放回的重新採樣，直觀上來說，我們應該能想到在三個門附近的樣本數會更多一些，其他地方的樣本數會比之前均勻分佈的要少一些，也就是如圖 8.c 的下圖所示。不過這裏的圖 8.c 是機器人往右移動了一段距離後的圖像，因此你會看到下圖中三個門附近的樣本都往右平移了一段距離。

現在機器人又看到自己在一個門的前面了，跟之前的操作一樣，我們還是對每個樣本計算它在地圖中的匹配程度，匹配程度越高，我們就賦予該樣本更高的權重，然後根據權重重新採樣，不斷重複這一過程。移動一段距離之後這些樣本就能大致收斂到一處了，機器人也就能大概知道自己在地圖中的位置了。

圖 9 就是一個採用蒙特卡洛定位的真實例子，圖中的黑點是一個樣本，也就是一個位置估計，在運動到圖 c 的位置時，位置估計就比較準確了。

圖 9. 蒙特卡洛定位實例

這裏給出蒙特卡洛定位的算法：

圖 10. 蒙特卡洛定位算法

這裏的 χ$\chi$ 就是所有的樣本集，算法背後的數學原理如圖 11 所示。

圖 11. 貝葉斯分解

其中 measurement_model 就是計算圖 11 中 observation likelihood，算法中沒有計算 normalizer 的部分，這是因爲每個樣本的權重會標準化爲 Σwi=1$\mathrm{\Sigma }{w}_i=1$ ，而 normalizer 與 xt$x_t$ 無關，所以在標準化時會被抵消掉，不需要計算。

參考資料

Probabilistic Robotics. S.Thrun

grid-maps：http://ais.informatik.uni-freiburg.de/teaching/ws15/mapping/pdf/slam10-gridmaps.pdf

RBPF：http://webdiis.unizar.es/~neira/5007439/stachniss-rbpfmapping.pdf