概率法——利用鐳射感測器構建佔據柵格地圖

2018年01月24日 10:08:06 MaxChanger 閱讀數：957 標籤： SLAM柵格地圖Occupancygridmap 更多

個人分類： SLAM

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米。另外，感測器資料是本地座標系的，而機器人要構建的是一個全域性的地圖。最後，機器人會運動，運動也是有噪音的。總結起來就兩個字，噪音。通俗點來講，“不準”。

在本篇文章中，我們利用“概率”這一神奇的數學武器來處理機器人Mapping的問題。

2. 佔據柵格地圖

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

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

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

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

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

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

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

帶入之後，我們得

我們對兩邊取對數得：

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

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

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

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

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

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

3. 利用鐳射感測器構建佔據柵格地圖

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

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

其中，scanAngles是一個的陣列，表示鐳射感測器的N個鐳射發射方向（與機器人朝向的夾角，定值）；ranges是一個K*N的陣列，表示N個時間取樣點鐳射感測器的讀數（距離障礙物的距離）；pose是一個3*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表示解析度，顯然我們有： 。

同理，二維情況下： 。

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

假設機器人的狀態為，鐳射與機器人朝向的夾角為，測量的障礙物的距離為：

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

再計算障礙物在柵格地圖中的位置 ，以及機器人在柵格地圖中的位置。根據這兩個座標可以使用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）問題，機器人領域中的一個重要問題。