佔據柵格地圖（Occupancy Grid Map）

01-29

寫在前面：這篇文章是Coursera上的課程（Robotics: Estimation and Learning），權當筆記，激光感測器的數據可以在課程內下載。這一周的內容比較簡單，但十分實用。

在這片文章中，我們將會介紹：

機器人世界的幾種地圖；
佔據柵格地圖的表示方法與更新方法；
利用激光感測器數據構建佔據柵格地圖。

1. 機器人地圖的分類

地圖有很多種表示方式，例如，用經緯度標識地方的世界地圖，城市的地鐵圖，校園指引圖。

第一種我們稱為尺度地圖（Metric Map），每一個地點都可以用坐標來表示，比如北京在東經116°23′17，北緯39°54′27；第二種我們稱為拓撲地圖（Topological Map），每一個地點用一個點來表示，用邊來連接相鄰的點，即圖論中的圖（Graph），比如從地鐵路線圖中我們知道地鐵紅磡站與旺角東站和尖東站相連；第三種我們稱為語義地圖（Semantic Map），其中每一個地點和道路都會用標籤的集合來表示，例如，有人問我中山大學教學樓E棟在哪裡，我會說在圖書館正門右手邊靠近圖書館的一側。

在機器人領域，尺度地圖常用於定位於地圖構建（Mapping）、定位（Localization）和同時定位與地圖構建（Simultaneous Localization And Mapping，SLAM），拓撲地圖常用於路徑規劃（Path Planning），而語義地圖常用於人機交互（Human Robot Interaction）。

這節課我們將介紹如何用機器人感測器數據繪製尺度地圖。這有什麼難點呢？首先也是最重要的一點，感測器數據有噪音。用激光感測器檢測前方障礙物距離機器人多遠，不可能檢測到一個準確的數值。如果準確值是 $sqrt{2}$ 米，有時會測出1.42米，有時甚至1.35米。另外，感測器數據是本地坐標系的，而機器人要構建的是一個全局的地圖。最後，機器人會運動，運動也是有噪音的。總結起來就兩個字，噪音。通俗點來講，「不準」。

在專欄的第一篇文章中，我們詳細提到了如何對噪音進行建模，用的是概率分布（高斯分布）。在這篇文章中，我們同樣利用「概率」這一神奇的數學武器來處理機器人Mapping的問題。

2. 佔據柵格地圖

我們首先來介紹機器人Mapping用到的的感測器，它叫做激光感測器（Laser Sensor），如下圖所示：

激光感測器會向固定的方向發射激光束，發射出的激光遇到障礙物會被反射，這樣就能得到激光從發射到收到的時間差，乘以速度除以二就得到了感測器到該方向上最近障礙物的距離。

這樣看來，似乎利用激光感測器，機器人能夠很好地完成Mapping這一任務。但是我們前面提到了，感測器數據是有噪音的。例如，假如我們在此時檢測到距離障礙物4米，下一時刻檢測到距離障礙物4.1米，我們是不是應該把4米和4.1米的地方都標記為障礙物？又或者怎麼辦呢？

為了解決這一問題，我們引入佔據柵格地圖（Occupancy Grid Map）的概念。

我們首先來解釋這裡的佔據率（Occupancy）指的是什麼。在通常的尺度地圖中，對於一個點，它要麼有（Occupied狀態，下面用1來表示）障礙物，要麼沒有（Free狀態，下面用0來表示）障礙物（旁白：那麼問題來了，薛定諤狀態呢？）。在佔據柵格地圖中，對於一個點，我們用 $p(s=1)$ 來表示它是Free狀態的概率，用 $p(s=0)$ 來表示它是Occupied狀態的概率，當然兩者的和為 $1$ 。兩個值太多了，我們引入兩者的比值來作為點的狀態： $Odd(s) = frac{p(s=1)}{p(s=0)}$ 。

對於一個點，新來了一個測量值（Measurement， $zsim{0,1}$ ）之後我們需要更新它的狀態。假設測量值來之前，該點的狀態為 $Odd(s)$ ，我們要更新它為： $Odd(s|z) = frac{p(s=1|z)}{p(s=0|z)}$ 。這種表達方式類似於條件概率，表示在 $z$ 發生的條件下 $sn$ 的狀態。

根據貝葉斯公式，我們有：

$p(s=1|z) = frac{p(z|s=1)p(s=1)}{p(z)}np(s=0|z) = frac{p(z|s=0)p(s=0)}{p(z)}$

帶入之後，我們得

$begin{align}nOdd(s|z) &= frac{p(s=1|z)}{p(s=0|z)}n&= frac{p(z|s=1)p(s=1)/p(z)}{p(z|s=0)p(s=0)/p(z)}n&=frac{p(z|s=1)}{p(z|s=0)}Odd(s)nend{align}$

我們對兩邊取對數得：

$log Odd(s|z) = logfrac{p(z|s=1)}{p(z|s=0)}+log Odd(s)$

這樣，含有測量值的項就只剩下了 $logfrac{p(z|s=1)}{p(z|s=0)}$ 。我們稱這個比值為測量值的模型（Measurement Model），標記為 $lomeas$ 。測量值的模型只有兩種： $lofree = logfrac{p(z=0|s=1)}{p(z=0|s=0)}$ 和 $looccu = logfrac{p(z=1|s=1)}{p(z=1|s=0)}$ ，而且都是定值。

這樣，如果我們用 $log Odd(s)$ 來表示位置 $s$ 的狀態 $S$ 的話，我們的更新規則就進一步簡化成了： $S^+ = S^- + lomeas$ 。其中 $S^+$ 和 $S^-$ 分別表示測量值之後和之前 $s$ 的狀態。

另外，在沒有任何測量值的初始狀態下，一個點的初始狀態 $S_{init} = log Odd(s) = log frac{ p(s=1) }{ p(s=0) } = logfrac{0.5}{0.5} = 0$ 。

經過這樣的建模，更新一個點的狀態就只需要做簡單的加減法了。這，就是數學的魅力。

例如，假設我們設定 $looccu = 0.9$ ， $lofree = -0.7$ 。那麼，一個點狀態的數值越大，就表示越肯定它是Occupied狀態，相反數值越小，就表示越肯定它是Free狀態。

上圖就展示了用兩個激光感測器的數據更新地圖的過程。在結果中，一個點顏色越深表示越肯定它是Free的，顏色越淺表示越肯定它是Occupied的。

3. 利用激光感測器構建佔據柵格地圖

前面講到通常用激光感測器數據來構佔據柵格地圖，這一節我們將詳細介紹其中的實現細節。具體來說，我們需要編寫函數：

function myMap = occGridMapping(ranges, scanAngles, pose, param)n

其中，scanAngles是一個 $Ntimes1$ 的數組，表示激光感測器的 $N$ 個激光發射方向（與機器人朝向的夾角，定值）；ranges是一個 $Ktimes N$ 的數組，表示 $N$ 個時間採樣點激光感測器的讀數（距離障礙物的距離）；pose是一個 $3times N$ 的數組，表示 $N$ 個時間採樣點機器人的位置和朝向信息（前兩維為位置，第三維為朝向角度）；param是一些傳入的參數，param.origin是機器人的起點，param.lo_occ和param.lo_free分別是第二節中的 $looccu$ 和 $lofree$ ，param.max和param.min表示位置狀態的閾值（超過則置為閾值邊界），param.resol表示地圖的解析度，即實際地圖中一米所表示的格點數目，param.size表示地圖的大小。

首先，我們解決如何將真實世界中的坐標轉化為柵格地圖中的坐標。

考慮一維的情況：

圖中 $x$ 是真實世界中的坐標， $i$ 為離散化了的地圖（柵格地圖）中的坐標， $r$ 為一格的長度， $1/r$ 表示解析度，顯然我們有： $i = ceil(x/r)$ 。

同理，二維情況下： $(i,j) = (ceil(x/r), ceil(y/r))$ 。

其次，我們來計算每一條激光所檢測出的障礙物和非障礙物在柵格地圖中的位置。

假設機器人的狀態為 $(x,y,theta)$ ，激光與機器人朝向的夾角為 $alpha$ ，測量的障礙物的距離為 $d$ （途中未標明 $alpha$ ，不好意思）：

計算障礙物所在點的實際位置：

$x_o = dcos(theta+alpha) + x$

$y_o = dsin(theta+alpha)+y$

再計算障礙物在柵格地圖中的位置 $(i_o,j_o)$ ，以及機器人在柵格地圖中的位置 $(i,j)$ 。根據這兩個坐標可以使用Bresenham演算法來計算非障礙物格點的集合。

最後，利用第二節中的結論，我們使用簡單的加減法不斷更新格點的狀態即可。

完整的Matlab代碼如下：

function myMap = occGridMapping(ranges, scanAngles, pose, param)nnresol = param.resol; % the number of grids for 1 meter.nmyMap = zeros(param.size); % the initial map size in pixelsnorigin = param.origin; % the origin of the map in pixelsnn% Log-odd parameters nlo_occ = param.lo_occ;nlo_free = param.lo_free;nlo_max = param.lo_max;nlo_min = param.lo_min;nnlidarn = size(scanAngles,1); % number of rays per timestampnN = size(ranges,2); % number of timestampnnfor i = 1:N % for each timestampn theta = pose(3,i); % orientation of robotn % coordinate of robot in real worldn x = pose(1,i);n y = pose(2,i);nn % local coordinates of occupied points in real worldn local_occs = [ranges(:,i).*cos(scanAngles+theta), -ranges(:,i).*sin(scanAngles+theta)];nn % coordinate of robot in metric mapn grid_rob = ceil(resol * [x; y]);nn % calc coordinates of occupied and free points in metric mapn for j=1:lidarnn real_occ = local_occs(j,:) + [x, y]; % global coordinate of occ in real worldn grid_occ = ceil(resol * real_occ); % coordinate of occ in metric mapnn % coordinates of free in metric map (by breshnhams algorithm)n [freex, freey] = bresenham(grid_rob(1),grid_rob(2),grid_occ(1),grid_occ(2));nn % convert coordinate to offset to arrayn free = sub2ind(size(myMap),freey+origin(2),freex+origin(1));n occ = sub2ind(size(myMap), grid_occ(2)+origin(2), grid_occ(1)+origin(1));nn % update metric mapn myMap(free) = myMap(free) - lo_free;n myMap(occ) = myMap(occ) + lo_occ;n endnendnn% reassign value if out of rangenmyMap(myMap < lo_min) = lo_min;nmyMap(myMap > lo_max) = lo_max;n

使用課程給的數據，我們最終畫出下面這樣的佔據柵格地圖（用灰度圖顯示出來　的）：

結語：地圖構建（Mapping）是機器人領域的一個重要問題，本文介紹了佔據柵格地圖的表示方法和利用激光感測器構建佔據柵格地圖的方法。但是，我們不難發現，使用的數據中機器人的位置和朝向是給定的。然而在實際的應用中，機器人不僅需要為未知環境構建地圖，還要在未知環境中定位（Localization）以完成其他任務。我們發現，Mapping和Localization是相輔相成、不可分割的兩部分，這就是同時定位與地圖構建（SLAM）問題，機器人領域中的一個重要問題。