上篇文章講解了如何在ROS中發(fā)布柵格地圖，以及如何向柵格地圖賦值．

這篇文章來講講如何將激光雷達(dá)的數(shù)據(jù)構(gòu)建成柵格地圖．

雷達(dá)的數(shù)據(jù)點所在位置表示為占用，從雷達(dá)開始到這點之間的區(qū)域表示為空閑．

1 GMapping簡介

GMapping是ROS中navigation導(dǎo)航包集中推薦的二維建圖算法包，由于其實現(xiàn)時間早，所以各種書中的demo使用的SLAM基本都是GMapping，同時GMapping網(wǎng)上的教程也是最多的．

GMapping是基于粒子濾波算法實現(xiàn)的SLAM，通過里程計數(shù)據(jù)獲取粒子群的先驗位姿，再通過雷達(dá)數(shù)據(jù)與地圖的匹配程度對所有粒子進(jìn)行打分，通過分?jǐn)?shù)高的粒子群來近似機器人的真實位姿．

GMapping的具體實現(xiàn)是在open_gmapping包里，后來又在ROS中做了個封裝包slam_gmapping．gmapping在ROS中的wiki地址為?http://wiki.ros.org/gmapping

2 代碼

open_gmapping的代碼比較復(fù)雜，比較亂． csdn博主 白茶-清歡 對 open_gmapping 與 slam_gmapping 兩個包進(jìn)行了重寫，整理了代碼使得代碼結(jié)構(gòu)更加清晰，同時添加了注釋，還增加了激光雷達(dá)數(shù)據(jù)的畸變校正功能．

本篇文章的代碼實現(xiàn)是參考于 csdn博主 白茶-清歡 注釋簡化之后的GMapping．其地址為 csdn 白茶-清歡:?https://blog.csdn.net/zhao_ke_xue/article/details/109712355

2.1 獲取代碼

代碼已經(jīng)提交在github上了，如果不知道github的地址的朋友, 請在我的公眾號:?從零開始搭激光SLAM?中回復(fù)?開源地址?獲得。

推薦使用 git clone 的方式進(jìn)行下載, 因為代碼是正處于更新狀態(tài)的, git clone 下載的代碼可以使用 git pull 很方便地進(jìn)行更新.

本篇文章對應(yīng)的代碼為 Creating-2D-laser-slam-from-scratch/lesson4/src/gmapping/ 與 Creating-2D-laser-slam-from-scratch/lesson4/include/gmapping/

2.2 回調(diào)函數(shù)

這個函數(shù)先進(jìn)行角度值的cos與sin的計算，然后調(diào)用PublishMap()計算地圖并發(fā)布出去．

// 回調(diào)函數(shù) 進(jìn)行數(shù)據(jù)處理void GMapping::ScanCallback(const sensor_msgs::LaserScan::ConstPtr &scan_msg){ static ros::Time last_map_update(0, 0); //存儲上一次地圖更新的時間 if (!got_first_scan_) //如果是第一次接收scan { // 將雷達(dá)各個角度的sin與cos值保存下來，以節(jié)約計算量 CreateCache(scan_msg); got_first_scan_ = true; //改變第一幀的標(biāo)志位 } start_time_ = std::chrono::steady_clock::now(); // 計算當(dāng)前雷達(dá)數(shù)據(jù)對應(yīng)的柵格地圖并發(fā)布出去 PublishMap(scan_msg); end_time_ = std::chrono::steady_clock::now(); time_used_ = std::chrono::duration_cast<:chrono::duration>>(end_time_ - start_time_); std::cout << "\n轉(zhuǎn)換一次地圖用時: " << time_used_.count() << " 秒。" << std::endl;}

2.3 PublishMap()

這里的ScanMatcherMap為GMapping中存儲地圖的數(shù)據(jù)類型，先聲明了一個ScanMatcherMap的對象gmapping_map_，然后通過ComputeMap()為gmapping_map_賦值，然后再將gmapping_map_中存儲的值賦值到ros的柵格地圖的數(shù)據(jù)類型中．

gmapping認(rèn)為ros的柵格地圖數(shù)據(jù)只需要有3個值

• -1 代表柵格狀態(tài)未知

• 0 代表柵格是空閑的，代表可通過區(qū)域

• 100 代表柵格是占用的，代表障礙物，不可通過

#define GMAPPING_UNKNOWN (-1)#define GMAPPING_FREE (0)#define GMAPPING_OCC (100)// 計算當(dāng)前雷達(dá)數(shù)據(jù)對應(yīng)的柵格地圖并發(fā)布出去void GMapping::PublishMap(const sensor_msgs::LaserScan::ConstPtr &scan_msg){ // 地圖的中點 Point center; center.x = (xmin_ + xmax_) / 2.0; center.y = (ymin_ + ymax_) / 2.0; // ScanMatcherMap為GMapping中存儲地圖的數(shù)據(jù)類型 ScanMatcherMap gmapping_map_(center, xmin_, ymin_, xmax_, ymax_, resolution_); // 使用當(dāng)前雷達(dá)數(shù)據(jù)更新GMapping地圖中柵格的值 ComputeMap(gmapping_map_, scan_msg); // 將gmapping_map_中的存儲的柵格值 賦值到 ros的map中 for (int x = 0; x < gmapping_map_.getMapSizeX(); x++) { for (int y = 0; y < gmapping_map_.getMapSizeY(); y++) { IntPoint p(x, y); // 獲取這點柵格的值，只有大于occ_thresh_時才認(rèn)為是占用 double occ = gmapping_map_.cell(p); // 未知 if (occ < 0) map_.data[MAP_IDX(map_.info.width, x, y)] = GMAPPING_UNKNOWN; // 占用 else if (occ > occ_thresh_) // 默認(rèn)0.25 map_.data[MAP_IDX(map_.info.width, x, y)] = GMAPPING_OCC; // 空閑 else map_.data[MAP_IDX(map_.info.width, x, y)] = GMAPPING_FREE; } } // 添加當(dāng)前的時間戳 map_.header.stamp = ros::Time::now(); map_.header.frame_id = scan_msg->header.frame_id; // 發(fā)布map和map_metadata map_publisher_.publish(map_); map_publisher_metadata_.publish(map_.info);}

2.4 ComputeMap()

這部分代碼分為2個部分，第一部分為計算出地圖的儲存空間，并且計算出從雷達(dá)到激光點這條線在gmapping柵格地圖中的坐標(biāo)，以及雷達(dá)點的坐標(biāo)．第二部分為將計算好的直線與點在gmapping地圖中進(jìn)行柵格值的更新．

// 使用當(dāng)前雷達(dá)數(shù)據(jù)更新GMapping地圖中柵格的值void GMapping::ComputeMap(ScanMatcherMap &map, const sensor_msgs::LaserScan::ConstPtr &scan_msg){ line_lists_.clear(); hit_lists_.clear(); // lp為地圖坐標(biāo)系下的激光雷達(dá)坐標(biāo)系的位姿 OrientedPoint lp(0, 0, 0.0); // 將位姿lp轉(zhuǎn)換成地圖坐標(biāo)系下的位置 IntPoint p0 = map.world2map(lp); // 第一部分 // 地圖的有效區(qū)域(地圖坐標(biāo)系) HierarchicalArray2D::PointSet activeArea; // 通過激光雷達(dá)的數(shù)據(jù)，找出地圖的有效區(qū)域 for (unsigned int i = 0; i < scan_msg->ranges.size(); i++) { // 排除錯誤的激光點 double d = scan_msg->ranges[i]; if (d > max_range_ || d == 0.0 || !std::isfinite(d)) continue; if (d > max_use_range_) d = max_use_range_; // p1為激光雷達(dá)的數(shù)據(jù)點在地圖坐標(biāo)系下的坐標(biāo) Point phit = lp; phit.x += d * a_cos_[i]; phit.y += d * a_sin_[i]; IntPoint p1 = map.world2map(phit); // 使用bresenham算法來計算 從激光位置到激光點 要經(jīng)過的柵格的坐標(biāo) GridLineTraversalLine line; GridLineTraversal::gridLine(p0, p1, &line); // 將line保存起來以備后用 line_lists_.push_back(line); // 計算活動區(qū)域的大小 for (int i = 0; i < line.num_points - 1; i++) { activeArea.insert(map.storage().patchIndexes(line.points[i])); } // 如果d // 同時如果d==max_use_range_那么說明這個值只用來進(jìn)行標(biāo)記空閑區(qū)域　不用來進(jìn)行標(biāo)記障礙物 if (d < max_use_range_) { IntPoint cp = map.storage().patchIndexes(p1); activeArea.insert(cp); hit_lists_.push_back(phit); } } // 為activeArea分配內(nèi)存 map.storage().setActiveArea(activeArea, true); map.storage().allocActiveArea(); // 第二部分 // 在map上更新空閑點 for (auto line : line_lists_) { // 更新空閑位置 for (int k = 0; k < line.num_points - 1; k++) { // 未擊中，就不記錄擊中的位置了，所以傳入?yún)?shù)Point(0,0) map.cell(line.points[k]).update(false, Point(0, 0)); } } // 在map上添加hit點 for (auto hit : hit_lists_) { IntPoint p1 = map.world2map(hit); map.cell(p1).update(true, hit); }}

代碼中的注釋已經(jīng)說明的很清楚了，這里簡要說明一下．

第一部分

首先生成了一個activeArea變量，用于記錄需要區(qū)域的范圍．

之后遍歷雷達(dá)數(shù)據(jù)點，使用bresemham畫線算法生成從激光位置到激光點的連線在柵格地圖中的坐標(biāo)，并將這些點放入activeArea用于計算區(qū)域，同時保存下來以備后用．

再判斷雷達(dá)數(shù)據(jù)點也就是 phit ．判斷這個點與預(yù)先設(shè)定的參數(shù) max_use_range_(雷達(dá)數(shù)據(jù)最大使用距離)，如果phit的距離小于max_use_range_則認(rèn)為是好的數(shù)據(jù)點，放入hit_lists_中以備后用．

最后，設(shè)置區(qū)域的大小以及分配內(nèi)存．

第二部分

第二部分做了兩個工作，一個是更新空閑點，代表可通過區(qū)域，另一個是更新hit點，也就是激光雷達(dá)數(shù)據(jù)點在地圖中的位置，代表著障礙物．

這部分之前是進(jìn)行了2次for循環(huán)，做了很多無用的計算，我進(jìn)行優(yōu)化了一下，現(xiàn)在只需要進(jìn)行一次for循環(huán)．

2.5 bresemham畫線算法

以下函數(shù)位于Creating-2D-laser-slam-from-scratch/lesson4/include/lesson4/gmapping/grid/gridlinetraversal.h文件中．

2.5.1 gridLine()

上述代碼調(diào)用了gridLine()函數(shù)，這個函數(shù)調(diào)用了gridLineCore()進(jìn)行直線坐標(biāo)點的計算，

然后判斷了計算出的直線坐標(biāo)點的起始點是否正確，如果不正確，就把直線的所有點順序反轉(zhuǎn)一下，

void GridLineTraversal::gridLine(IntPoint start, IntPoint end, GridLineTraversalLine *line){ int i, j; int half; IntPoint v; gridLineCore(start, end, line); if (start.x != line->points[0].x || start.y != line->points[0].y) { half = line->num_points / 2; for (i = 0, j = line->num_points - 1; i < half; i++, j--) { v = line->points[i]; line->points[i] = line->points[j]; line->points[j] = v; } }}

2.5.2 gridLineCore()

這個函數(shù)就是實際bresemham畫線算法的實現(xiàn)，代碼挺長，但是挺簡單的，只是區(qū)分了很多情況：斜率是否大于1，以及增長的順序的方向判斷等等，具體的意思已經(jīng)在代碼注釋的很清楚了.

如果您想弄清楚理論過程，理解的更透徹一些，可以參看這篇文章?https://www.jianshu.com/p/d63bf63a0e28

網(wǎng)上的教程也很多，我這就不再講了．

void GridLineTraversal::gridLineCore(IntPoint start, IntPoint end, GridLineTraversalLine *line){ int dx, dy; // 橫縱坐標(biāo)間距 int incr1, incr2; // 增量 int d; // int x, y, xend, yend; // 直線增長的首末端點坐標(biāo) int xdirflag, ydirflag; // 橫縱坐標(biāo)增長方向 int cnt = 0; // 直線過點的點的序號 dx = abs(end.x - start.x); dy = abs(end.y - start.y); // 斜率絕對值小于等于1的情況，優(yōu)先增加x if (dy <= dx) { d = 2 * dy - dx; // 初始點P_m0值 incr1 = 2 * dy; // 情況(1) incr2 = 2 * (dy - dx); // 情況(2) // 將增長起點設(shè)置為橫坐標(biāo)小的點處，將 x的增長方向 設(shè)置為 向右側(cè)增長 if (start.x > end.x) { // 起點橫坐標(biāo)比終點橫坐標(biāo)大，ydirflag = -1(負(fù)號可以理解為增長方向與直線始終點方向相反) x = end.x; y = end.y; ydirflag = (-1); xend = start.x; // 設(shè)置增長終點橫坐標(biāo) } else { x = start.x; y = start.y; ydirflag = 1; xend = end.x; } //加入起點坐標(biāo) line->points.push_back(IntPoint(x, y)); cnt++; // 向 右上 方向增長 if (((end.y - start.y) * ydirflag) > 0) { while (x < xend) { x++; if (d < 0) { d += incr1; } else { y++; d += incr2; // 縱坐標(biāo)向正方向增長 } line->points.push_back(IntPoint(x, y)); cnt++; } } // 向 右下 方向增長 else { // ... 省略 } } // 斜率絕對值大于1的情況，優(yōu)先增加y else {????????//?...?省略 } line->num_points = cnt;}

2.6 GMapping中的地圖數(shù)據(jù)類型

我將GMapping中的地圖數(shù)據(jù)類放在了
Creating-2D-laser-slam-from-scratch/lesson4/include/lesson4/gmapping/grid 文件夾下，以及依賴的點的類文件放在了
Creating-2D-laser-slam-from-scratch/lesson4/include/lesson4/gmapping/utils 文件夾下.

2.6.1 地圖類

GMapping的地圖相關(guān)的文件有3個，array.h，harray2d.h，map.h．

具體的代碼實現(xiàn)我就不在這里說明了，代碼挺多的，我也沒太看懂(哈哈)。

感興趣的同學(xué)可以仔細(xì)閱讀一下，這里的代碼都有注釋的，再次感謝白茶清歡小哥的工作．

GMapping的代碼比較老了，這塊的實現(xiàn)可以使用c++ 11的stl進(jìn)行更好的實現(xiàn)，這也是我不對這個代碼細(xì)講的原因，因為之后會有更好的實現(xiàn)．

2.6.2 地圖占用值更新的實現(xiàn)

map.h中有個叫做PointAccumulator的結(jié)構(gòu)體，定義了柵格地圖的更新方式以及存儲值是如何計算的．其代碼如下

//PointAccumulator表示地圖中一個cell(柵格)包括的內(nèi)容/*acc：柵格累計被擊中位置n：柵格被擊中次數(shù)visits：柵格被訪問的次數(shù)*///PointAccumulator的一個對象，就是一個柵格，gmapping中其他類模板的cell就是這個struct PointAccumulator{ //float類型的point typedef point<float> FloatPoint; //構(gòu)造函數(shù) PointAccumulator() : acc(0, 0), n(0), visits(0) {} PointAccumulator(int i) : acc(0, 0), n(0), visits(0) { assert(i == -1); } //計算柵格被擊中坐標(biāo)累計值的平均值 inline Point mean() const { return 1. / n * Point(acc.x, acc.y); } //返回該柵格被占用的概率，范圍是 -1(沒有訪問過) 、[0,1] inline operator double() const { return visits ? (double)n * 1 / (double)visits : -1; } //更新該柵格成員變量 inline void update(bool value, const Point &p = Point(0, 0)); //該柵格被擊中的位置累計，最后取累計值的均值 FloatPoint acc; //n表示該柵格被擊中的次數(shù)，visits表示該柵格被訪問的次數(shù) int n, visits;};//更新該柵格成員變量，value表示該柵格是否被擊中，擊中n++，未擊中僅visits++;void PointAccumulator::update(bool value, const Point &p){ if (value) { acc.x += static_cast<float>(p.x); acc.y += static_cast<float>(p.y); n++; visits += 1; } else visits++;}

可以看到，每個格子都有2個值，一個是visits，一個是n．

更新占用: 在更新被擊中的柵格時, 也就是激光點所在的柵格, n與visits都會加1，

更新空閑: 在更新未被擊中的柵格時，也就是從激光到激光點之間的柵格，只有visits加1,

通過重載了()，獲取獲取每個格子的占用值，也可以說成是被占用的概率．占用值的計算公式為 n/visists，如果是沒被更新過就返回-1．

由于visits大于等于n，所以占用值的范圍是 [0,1]的，只有當(dāng)這個值在大于0.25(默認(rèn)參數(shù))時，在賦值到ros地圖的時候才賦值為100，才認(rèn)為是真正的占用了．

舉個例子說明一下柵格地圖的占用值更新的方式．

例如：當(dāng)雷達(dá)掃到人腿時，會對擊中點的柵格更新一次占用，這時在ROS地圖下這個點將被表示為障礙物．

如果人腿離開了這個位置，在之后的過程中，有4次在這個柵格更新了空閑，就會將ROS地圖中這個柵格的狀態(tài)更新到空閑，也就是沒有障礙物．

原因：

第一次更新地圖時，這個格子的占用值為 1/1 = 1 > 0.25 ，就會將ROS地圖中這個格子設(shè)置為100，表示障礙物．

如果之后4次更新地圖，這個柵格都被更新空閑了，則這個格子的占用值將變?yōu)?1/5 = 0.2 <0.25了，所以就會將ROS地圖中對應(yīng)的柵格設(shè)置為0，變成可通過區(qū)域了．

3 運行

3.1 launch文件

本篇文章對應(yīng)的數(shù)據(jù)包, 請在我的公眾號中回復(fù)?lesson1?獲得，并將launch中的bag_filename更改成您實際的目錄名。

<launch> <arg name="bag_filename" default="/home/lx/bagfiles/lesson1.bag"/> <param name="use_sim_time" value="true" /> <node name="lesson4_gmapping_node" pkg="lesson4" type="lesson4_gmapping_node" output="screen" /> <node name="rviz" pkg="rviz" type="rviz" required="true" args="-d $(find lesson4)/config/gmapping.rviz" /> <node name="playbag" pkg="rosbag" type="play" args="--clock $(arg bag_filename)" />launch>

3.2 編譯與運行

下載代碼后，請放入您自己的工作空間中，通過 catkin_make 進(jìn)行編譯．

由于是新增的包，所以需要通過 rospack profile 命令讓ros找到這個新的包．

之后, 使用source命令，添加ros與工作空間的地址到當(dāng)前終端下，再通過如下命令運行本篇文章對應(yīng)的程序

roslaunch?lesson4?make_gmapping_map.launch

3.3 運行結(jié)果

啟動之后，會在rviz中顯示出如下畫面．

地圖是不斷地根據(jù)雷達(dá)數(shù)據(jù)進(jìn)行更新的，每次生成的地圖只是這一幀雷達(dá)數(shù)據(jù)轉(zhuǎn)換后的結(jié)果，不會累加之前的雷達(dá)數(shù)據(jù)．
同時會在終端中打印出如下消息．

轉(zhuǎn)換一次地圖用時:?0.403898?秒。轉(zhuǎn)換一次地圖用時:?0.394922?秒。轉(zhuǎn)換一次地圖用時:?0.40791?秒。

3.4 結(jié)果分析

通過終端打印出來的信息可以得知，每次計算gmapping地圖，再將gmapping地圖的值賦值到ros的地圖中，再發(fā)布出來．這一系列操作大概需要花費0.4秒，這是一個很長的時間了．

可以通過如下命令來看下map這個topic的頻率

$ rostopic hz /laser_scan /map topic rate min_delta max_delta std_dev window===============================================================/laser_scan 9.987 0.09058 0.1108 0.003163 50 /map??????????2.613???0.1914??????0.4549??????0.06057????50????

可以看到，雷達(dá)數(shù)據(jù)是10hz的，map數(shù)據(jù)大概是2.5hz．

我們只用了一個線程，也就是雷達(dá)回調(diào)函數(shù)的這個線程．處理一次回調(diào)函數(shù)需要用時0.4秒，而雷達(dá)數(shù)據(jù)的間隔是0.1秒．

也就是說，當(dāng)我們正在計算地圖的時候，會又有4幀雷達(dá)數(shù)據(jù)到達(dá)，當(dāng)回調(diào)函數(shù)的緩沖區(qū)設(shè)置為1時，那這4幀雷達(dá)數(shù)據(jù)將只保留最后一個，其他3幀數(shù)據(jù)將丟失掉．

這還只是80m * 80m范圍的地圖，構(gòu)建更大范圍地圖的時間將更久．所以，很多SLAM都將地圖的生成單獨開一個線程，以保證不耽誤對實時性要求較高的前端里程計部分．

4 總結(jié)與Next

通過使用GMapping中的地圖數(shù)據(jù)格式，以及GMapping中的地圖計算方式，我們實現(xiàn)了將激光雷達(dá)數(shù)據(jù)構(gòu)建成柵格地圖的功能，雖然現(xiàn)在的建圖是單次的．

希望你通過這篇文章以及代碼，知道了如何將激光雷達(dá)數(shù)據(jù)寫成柵格地圖，知道了柵格地圖更新一次的耗時問題．

下一篇文章還是進(jìn)行單次柵格地圖的構(gòu)建，只不過下次將使用Hector中構(gòu)建地圖的方式，為我們以后做scan-to-map做個基礎(chǔ)．

---

其他原創(chuàng)文章可以戳：柵格地圖的構(gòu)建基于PL-ICP的激光雷達(dá)里程計從零開始搭二維激光SLAM --- 基于PL-ICP的幀間匹配

---

文章將在?CSDN: 李太白lx?與?知乎：李想?進(jìn)行同步更新，可以更方便的看代碼，歡迎大家關(guān)注。

同時，也希望您將這個公眾號推薦給您身邊做激光SLAM的人們，可以在公眾號中添加我的微信，進(jìn)激光SLAM交流群，大家一起交流SLAM技術(shù)。

如果您對我寫的文章有什么建議，或者想要看哪方面功能如何實現(xiàn)的，請直接在公眾號中回復(fù)，我可以收到，并將認(rèn)真考慮您的建議。

【您的在看與轉(zhuǎn)發(fā)，是對我莫大鼓勵】

總結(jié)

以上是生活随笔為你收集整理的基于栅格地图的粒子群算法_基于GMapping的栅格地图的构建的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

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