轉自：?占據柵格地圖構建（Occupancy Grid Map） - 知乎

占據柵格地圖構建（Occupancy Grid Map）_「小白學移動機器人」一個專注分享移動機器人相關知識的公眾號！-CSDN博客_構建柵格地圖

用于學習參考用。

懂了占據柵格地圖構建算法，就意味著SLAM問題不再是抽象的理論公式，它變成了浮現在你我腦海里的動態構建過程。這將對我們完整理解各種激光SLAM算法起著十分重要的作用。

1、地圖的分類

移動機器人常見的地圖有三種：尺度地圖、拓撲地圖、語義地圖。

尺度地圖：具有真實的物理尺寸，如柵格地圖、特征地圖、點云地圖；常用于地圖構建、定位、SLAM、小規模路徑規劃。

拓撲地圖：不具備真實的物理尺寸，只表示不同地點的連通關系和距離，如鐵路網，常用于超大規模的機器人路徑規劃。

語義地圖：加標簽的尺度地圖，公認SLAM的未來--SLAM和深度學習的結合，常用于人機交互。

其中對尺度地圖進行補充說明，如下圖所示。

2、占據柵格地圖構建算法

顧名思義，占據柵格地圖構建算法當然構建的是柵格地圖。

2.1、為什么使用占用柵格地圖構建算法構建地圖?

在開始講解之前，我們要明確一些事情。

第一，構建柵格地圖需要使用激光雷達傳感器。

第二，激光雷達傳感器是有噪聲存在的，通俗的說，"不一定準"。

舉個例子，機器人在同一位姿下的不同時刻，通過激光雷達對一個固定的障礙物的探測距離不一致，一幀為5m，一幀為5.1m，我們難道要把5m和5.1m的位置都標記為障礙物？這也就是使用占據柵格地圖構建算法的原因。

2.2、什么是占據柵格地圖構建算法？

為了解決這一問題，我們引入占據柵格地圖（Occupancy Grid Map）的概念。我們將地圖柵格化，對于每一個柵格的狀態要么占用，要么空閑，要么未知（即初始化狀態）。

關于占據柵格地圖構建算法的引出、推導、演化，從下面的圖片得出。（哈哈，圖省事，在word里推導的）

2.3、舉個例子驗證占據柵格地圖構建算法

首先，我們假設 looccu = 0.9，lofree = -0.7。那么，顯而易見，一個柵格狀態的數值越大，就越表示該柵格為占據狀態，相反數值越小，就表示改柵格為空閑狀態。（這也就解決了此前文中提出的激光雷達觀測值"不一定準"的問題）

上圖是用兩次激光掃描數據更新地圖的過程。在結果中，顏色越深越表示柵格是空閑的，顏色越淺越表示是占據的。這里要區分常用的激光SLAM算法中的地圖，只是表述方式的不同，沒有對錯之分。

3、如何通過激光數據構建柵格地圖？

3.1、算法核心依據

通過上述的講解，你是否了抓住算法實現的的重要依據是什么？要是沒有，你就要反思一下自己是否仔細讀了這篇文章？

顯然，整篇文章得出的一個結論就是下圖所示，這里假設lofree和looccu為確定的數值，一般情況下一正一負。

然后，我們通過激光雷達數據柵格進行判斷，如果判定柵格是空閑，就執行上面公式；如果判定柵格是占據，就執行下面的公式。在經過許多幀激光雷達數據的洗禮之后，每一個柵格都存儲了一個值，此時我們可以自己設定閾值與該值比較，來做柵格最終狀態的判定。

3.2、算法輸入數據

激光雷達數據包（每個掃描點包含角度（逆時針為正方向）和距離，每幀激光數據包含若干掃描點，激光雷達數據包包含若干幀激光雷達數據）

機器人位姿數據包（每一個位姿包含世界坐標系下的機器人位置和航向角，初始航向角與世界坐標系X軸正方向重合，逆時針為正方向）

地圖參數（需要構建地圖的高度和寬度，構建地圖的起始點，lofree和looccu的設定值，地圖的分辨率）

假設激光雷達坐標系和機器人坐標系重合

3.3、算法步驟

這里以處理第一幀激光雷達為例，從上向下依次介紹。

（1）讀取一幀激光雷達數據和該幀對應的機器人位姿

//獲取每一幀的激光雷達、機器人位姿數據GeneralLaserScan scan = scans[i];Eigen::Vector3d robotPose = robot_poses[i];

（2）計算該幀機器人位置的柵格序號

//獲取該幀機器人位姿的柵格序號GridIndex robotIndex = ConvertWorld2GridIndex(robotPose(0),robotPose(1));

即從世界坐標系轉入柵格坐標系，每個柵格序號有x，y兩個數字。這里與地圖分辨率有關，比如說：地圖分辨率為0.05，也就是1m用20個柵格表示。

例如：世界坐標系下機器人位置（1,1）對應柵格坐標系的（20,20）。

注意：世界坐標系與像素坐標系區分開來，他們之間的y軸方向相反，其他都一致，地圖的顯示使用的像素坐標系（柵格坐標系）。

（3）遍歷該幀激光雷達數據的所有掃描點執行以下操作

計算每一個激光點擊中柵格在像素坐標系下的柵格序號

//明確這里的世界坐標系world_x，不是真實的世界坐標系，而是像素坐標系，y軸與真實的世界坐標系相反，這樣是laser_y加負號的原因double laser_x = dist * cos(theta + angle);double laser_y = -dist * sin(theta + angle);//得到該激光掃描點，在世界坐標系下（像素坐標系下）的位置double world_x = laser_x + robotPose(0);double world_y = laser_y + robotPose(1);//將該激光掃描點在世界坐標系下的位置，轉化為柵格序號GridIndex mapIndex = ConvertWorld2GridIndex(world_x,world_y);

從當前機器人位姿的柵格序號到該激光掃描點的柵格序號劃線，找出所有空閑的柵格序號

//從機器人的柵格序號到該激光掃描點的柵格序號劃線//目的：找到兩點之間途徑的空閑柵格，將柵格序號存入std::vector<GridIndex>中std::vector<GridIndex> freeIndex = TraceLine(robotIndex.x,robotIndex.y,mapIndex.x,mapIndex.y);

遍歷所有空閑的柵格更新空閑柵格狀態

data += mapParams.log_free;//log_free=-1，data將變小

更新該激光掃描點擊中的柵格狀態

data += mapParams.log_occ;//log_occ=2，data將變大

（4）結束

4、占據柵格地圖構建算法c++實現

這里分享核心代碼，具體算法功能包，可在公眾號：小白學移動機器人，發送：柵格地圖，即可獲得。

本算法功能包，給出了詳細的中文注釋，是提升c++編程技能的閱讀良品。

//占據柵格地圖構建算法 //輸入激光雷達數據和機器人位姿數據 //目的：通過遍歷所有幀數據，為pMap[]中的每個穿過的空閑柵格或者擊中柵格賦新值，中間有個計算方法，也就是占用柵格地圖構建的理論實現 void OccupanyMapping(std::vector<GeneralLaserScan>& scans,std::vector<Eigen::Vector3d>& robot_poses) {std::cout <<"Scans Size:"<<scans.size()<<std::endl;std::cout <<"Poses Size:"<<robot_poses.size()<<std::endl;//遍歷所有幀激光雷達數據for(int i = 0; i < scans.size();i++){//獲取每一幀的激光雷達、機器人位姿數據GeneralLaserScan scan = scans[i];Eigen::Vector3d robotPose = robot_poses[i];//獲取該幀機器人位姿的柵格序號GridIndex robotIndex = ConvertWorld2GridIndex(robotPose(0),robotPose(1));//判斷該幀機器人位姿的柵格序號，是否在自己設定的柵格地圖范圍內if(isValidGridIndex(robotIndex) == false) continue;//遍歷該幀激光雷達數據所有掃描點for(int id = 0; id < scan.range_readings.size();id++){//取出該激光雷達掃描點的距離和角度double dist = scan.range_readings[id];double angle = scan.angle_readings[id];//剔除異常數據，跳過該次循環，不作處理if(std::isinf(dist) || std::isnan(dist)) continue;//機器人航向角，機器人x軸與世界坐標系x軸夾角double theta = robotPose(2);//在旋轉過后（與世界坐標系（像素坐標系下）平行）的激光雷達坐標系下的坐標x,y//該開始一直不理解這個為啥laser_y要加一個負號//明確激光雷達數據的角度逆時針變化//明確機器人航向角與世界坐標系x軸呈逆時針變化//明確這里的世界坐標系world_x，不是真實的世界坐標系，而是像素坐標系，y軸與真實的世界坐標系相反，這樣是laser_y加負號的原因double laser_x = dist * cos(theta + angle);double laser_y = -dist * sin(theta + angle);//得到該激光掃描點，在世界坐標系下（像素坐標系下）的位置double world_x = laser_x + robotPose(0);double world_y = laser_y + robotPose(1);//將該激光掃描點在世界坐標系下的位置，轉化為柵格序號GridIndex mapIndex = ConvertWorld2GridIndex(world_x,world_y);//判斷該激光掃描點的柵格序號，是否在自己設定的柵格地圖900x900范圍內，如果不在則跳過if(isValidGridIndex(mapIndex) == false)continue;//從機器人的柵格序號到該激光掃描點的柵格序號劃線//目的：找到兩點之間途徑的空閑柵格，將柵格序號存入std::vector<GridIndex>中std::vector<GridIndex> freeIndex = TraceLine(robotIndex.x,robotIndex.y,mapIndex.x,mapIndex.y);//遍歷該掃描激光點通過的所有空閑柵格for(int k = 0; k < freeIndex.size();k++){GridIndex tmpIndex = freeIndex[k];//將空閑柵格的柵格序號，轉化到數組序號,該數組用于存儲每一個柵格的數據int linearIndex = GridIndexToLinearIndex(tmpIndex);//取出該柵格代表的數據int data = pMap[linearIndex];//根據柵格空閑規則，執行data += mapParams.log_free;if(data > 0)//默認data=50data += mapParams.log_free;//log_free=-1，data將變小elsedata = 0;//給該空閑柵格賦新值，最小為0pMap[linearIndex] = data;}//更新該激光掃描點集中的柵格，int tmpIndex = GridIndexToLinearIndex(mapIndex);int data = pMap[tmpIndex];//根據柵格擊中規則，執行data += mapParams.log_occ;if(data < 100)//默認data=50data += mapParams.log_occ;//log_occ=2，data將變大elsedata = 100;//給擊中的柵格賦新值，最大100pMap[tmpIndex] = data;//到這里，對一個位姿下的一個激光掃描數據經過的空閑柵格和擊中柵格的pMap進行了重新賦值}//到這里，對一個位姿下的一幀激光掃描數據經過的空閑柵格和擊中柵格進行了重新賦值}//到這里，對所有幀激光掃描數據經過的空閑柵格和擊中柵格進行了重新賦值 } 與50位技術專家面對面20年技術見證，附贈技術全景圖

總結

以上是生活随笔為你收集整理的机器人学习--栅格地图（occupancy grid map）构建（部分代码解析）的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。