博客轉載自：https://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 離散的障礙占據圖

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

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

繪制二維障礙柵格地圖的一種方法1（局部地圖的繪制）