轉(zhuǎn)自：?占據(jù)柵格地圖構(gòu)建（Occupancy Grid Map） - 知乎

占據(jù)柵格地圖構(gòu)建（Occupancy Grid Map）_「小白學(xué)移動機器人」一個專注分享移動機器人相關(guān)知識的公眾號！-CSDN博客_構(gòu)建柵格地圖

用于學(xué)習(xí)參考用。

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

1、地圖的分類

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

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

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

語義地圖：加標(biāo)簽的尺度地圖，公認(rèn)SLAM的未來--SLAM和深度學(xué)習(xí)的結(jié)合，常用于人機交互。

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

2、占據(jù)柵格地圖構(gòu)建算法

顧名思義，占據(jù)柵格地圖構(gòu)建算法當(dāng)然構(gòu)建的是柵格地圖。

2.1、為什么使用占用柵格地圖構(gòu)建算法構(gòu)建地圖?

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

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

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

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

2.2、什么是占據(jù)柵格地圖構(gòu)建算法？

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

關(guān)于占據(jù)柵格地圖構(gòu)建算法的引出、推導(dǎo)、演化，從下面的圖片得出。（哈哈，圖省事，在word里推導(dǎo)的）

2.3、舉個例子驗證占據(jù)柵格地圖構(gòu)建算法

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

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

3、如何通過激光數(shù)據(jù)構(gòu)建柵格地圖？

3.1、算法核心依據(jù)

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

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

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

3.2、算法輸入數(shù)據(jù)

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

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

地圖參數(shù)（需要構(gòu)建地圖的高度和寬度，構(gòu)建地圖的起始點，lofree和looccu的設(shè)定值，地圖的分辨率）

假設(shè)激光雷達坐標(biāo)系和機器人坐標(biāo)系重合

3.3、算法步驟

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

（1）讀取一幀激光雷達數(shù)據(jù)和該幀對應(yīng)的機器人位姿

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

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

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

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

例如：世界坐標(biāo)系下機器人位置（1,1）對應(yīng)柵格坐標(biāo)系的（20,20）。

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

（3）遍歷該幀激光雷達數(shù)據(jù)的所有掃描點執(zhí)行以下操作

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

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

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

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

遍歷所有空閑的柵格更新空閑柵格狀態(tài)

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

更新該激光掃描點擊中的柵格狀態(tài)

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

（4）結(jié)束

4、占據(jù)柵格地圖構(gòu)建算法c++實現(xiàn)

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

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

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

總結(jié)

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

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯，歡迎將生活随笔推薦給好友。