轉載請說明出處：http://blog.csdn.net/zhubaohua_bupt/article/details/72923373

區域性地圖的繪製

二維佔據柵格地圖(柵格地圖)廣泛應用於移動機器人導航領域中，比如路徑規劃、實時避障。

柵格地圖的繪製通常有兩種方式，一種是通話雷達掃描，另一種是通過相機獲取場景三維點雲，然後再繪製柵格地圖．

本篇部落格講述的是一種基於深度相機的建圖方法.深度相機可以輸出相機的原圖和深度資訊，即可以實時的獲取場景的三維點雲．

我們只利用深度資訊就可以建立區域性柵格地圖．建立地圖的步驟如下：

圖1  區域性柵格地圖繪製流程

下面通過對一個場景建立區域性地圖的過程，解釋每一步驟並給出結果：

下圖為我們要建立的柵格地圖場景，這個例子中，我們獲取以地面為起點，0-0.8米的點雲來畫柵格地圖。

圖2 本例中畫取柵格地圖的場景

第一步：初始化區域性地圖

首先，初始化區域性地圖需要設定地圖尺寸，柵格大小（每一柵格所代表的實際距離）；

其次，先把整個影象設定成黑色，代表未知區域，按距離(z)從近到遠掃描，掃描的範圍決定著我們畫的地圖的大小，掃描原理如下圖：

圖3表示初始化地圖過程中，正在掃描的地圖，掃描到的區域置灰，代表相機能看到的範圍。

圖 3  初始化區域性地圖的過程

最後，掃描完成，初始化的地圖如圖4所示

圖4  初始化後的區域性地圖

第二步 ：獲取一定高度範圍內的三維點

對於移動機器人而言，只有一定高度範圍內的障礙才對其構成威脅，所以在建圖時，我們只考慮這個範圍內的三維點。這個例子中，我們獲取以地面為起點，0-0.8米的點雲來畫柵格地圖。

第三步：投影形成二維離散障礙圖

獲取離散的障礙柵格圖是為了方便下一步的掃描，分為兩個小步驟：

1 投影統計，統之前我們在一定高度範圍內提取的點雲在初始化地圖上的投影。圖5中紅色部分代表空間中離散三維點往二維柵格的投影．

本文用畫素點值來統計每個柵格被障礙投影的次數。請注意，圖5中紅色三維點的投影超過我們初始化的柵格地圖的原因是，我們在初始化地圖時，

人為的控制地圖最遠距離maxZ，即我們只畫距離相機在maxZ之內的障礙，但實際中，肯定有超過這個範圍的點雲，除非把maxZ設的很大。

因此，就造成圖5中超出範圍的現象。

圖5 障礙投影統計圖

2 用閾值判別每個柵格是否是障礙

在1中，我們畫素點值來統計每個柵格被障礙投影的次數，當次數大於某個閾值時，就認為對應柵格被障礙佔據。

表現在圖５中，就是，紅色越亮，就越可能是障礙。經過此步，就形成了離散的障礙佔據圖，如圖6，綠色代表被障礙佔據的柵格。

圖 6 離散的障礙佔據圖

第四步：從相機中心掃描放射掃描，形成區域性柵格地圖

這一步是掃描形成柵格圖，核心在於掃描。由於上一步我們獲取的是離散的柵格障礙圖，而且柵格地圖最終的樣子應該是是以相機為中心，

放射掃描第一次遇到障礙以後的位置都是不確定的。因此，本步驟就是以相機光心為起點，放射掃描所有角度，

每一次掃描，都把光心和第一次遇到障礙之間的柵格畫成白色，代表可通行區域，另外，遇到障礙就開始下一步的掃描，這樣第一次遇到的障礙身後的區域都是灰色。

 圖8 和 圖9是從左至右掃描時的兩個時刻。

注：我用的掃描是方法是

圖７ 掃描方法

圖8  掃描過程圖1

圖9  掃描過程圖2

圖10 是掃描形成的障礙佔據柵格地圖。