機器人的地圖——占據柵格地圖

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）。

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

2、占據柵格地圖

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

 

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

 

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

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

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

 

對於一個點，新來了一個測量值（Measurement，）之后我們需要更新它的狀態。假設測量值來之前，該點的狀態為，我們要更新它為：。這種表達方式類似於條件概率，表示在發生的條件下的狀態。

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

帶入之后，我們得

我們對兩邊取對數得：

這樣，含有測量值的項就只剩下了。我們稱這個比值為測量值的模型（Measurement Model），標記為。測量值的模型只有兩種：和，而且都是定值。

這樣，如果我們用來表示位置的狀態的話，我們的更新規則就進一步簡化成了：。其中和分別表示測量值之后和之前的狀態。

另外，在沒有任何測量值的初始狀態下，一個點的初始狀態。

 

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

例如，假設我們設定，。那么， 一個點狀態的數值越大，就表示越肯定它是Occupied狀態，相反數值越小，就表示越肯定它是Free狀態。

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

3. 利用激光傳感器構建占據柵格地圖

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

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

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

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

考慮一維的情況：

 

圖中是真實世界中的坐標，為離散化了的地圖（柵格地圖）中的坐標，為一格的長度，表示分辨率，顯然我們有：。

ceil向上取整

同理，二維情況下：。

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

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

 

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

 

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

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

完整的Matlab代碼如下：

function myMap = occGridMapping(ranges, scanAngles, pose, param)  resol = param.resol; % the number of grids for 1 meter. myMap = zeros(param.size); % the initial map size in pixels origin = param.origin; % the origin of the map in pixels % Log-odd parameters lo_occ = param.lo_occ; lo_free = param.lo_free; lo_max = param.lo_max; lo_min = param.lo_min; lidarn = size(scanAngles,1); % number of rays per timestamp N = size(ranges,2); % number of timestamp for i = 1:N % for each timestamp theta = pose(3,i); % orientation of robot % coordinate of robot in real world x = pose(1,i); y = pose(2,i); % local coordinates of occupied points in real world local_occs = [ranges(:,i).*cos(scanAngles+theta), -ranges(:,i).*sin(scanAngles+theta)]; % coordinate of robot in metric map grid_rob = ceil(resol * [x; y]); % calc coordinates of occupied and free points in metric map for j=1:lidarn real_occ = local_occs(j,:) + [x, y]; % global coordinate of occ in real world grid_occ = ceil(resol * real_occ); % coordinate of occ in metric map % coordinates of free in metric map (by breshnham's algorithm) [freex, freey] = bresenham(grid_rob(1),grid_rob(2),grid_occ(1),grid_occ(2)); % convert coordinate to offset to array free = sub2ind(size(myMap),freey+origin(2),freex+origin(1)); occ = sub2ind(size(myMap), grid_occ(2)+origin(2), grid_occ(1)+origin(1)); % update metric map myMap(free) = myMap(free) - lo_free; myMap(occ) = myMap(occ) + lo_occ; end end % reassign value if out of range myMap(myMap < lo_min) = lo_min; myMap(myMap > lo_max) = lo_max;

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

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