基于栅格地图的粒子群算法_基于GMapping的栅格地图的构建
上篇文章講解了如何在ROS中發(fā)布柵格地圖，以及如何向柵格地圖賦值．
這篇文章來(lái)講講如何將激光雷達(dá)的數(shù)據(jù)構(gòu)建成柵格地圖．
雷達(dá)的數(shù)據(jù)點(diǎn)所在位置表示為占用，從雷達(dá)開(kāi)始到這點(diǎn)之間的區(qū)域表示為空閑．
1 GMapping簡(jiǎn)介
GMapping是ROS中navigation導(dǎo)航包集中推薦的二維建圖算法包，由于其實(shí)現(xiàn)時(shí)間早，所以各種書中的demo使用的SLAM基本都是GMapping，同時(shí)GMapping網(wǎng)上的教程也是最多的．
GMapping是基于粒子濾波算法實(shí)現(xiàn)的SLAM，通過(guò)里程計(jì)數(shù)據(jù)獲取粒子群的先驗(yàn)位姿，再通過(guò)雷達(dá)數(shù)據(jù)與地圖的匹配程度對(duì)所有粒子進(jìn)行打分，通過(guò)分?jǐn)?shù)高的粒子群來(lái)近似機(jī)器人的真實(shí)位姿．
GMapping的具體實(shí)現(xiàn)是在open_gmapping包里，后來(lái)又在ROS中做了個(gè)封裝包slam_gmapping．gmapping在ROS中的wiki地址為?http://wiki.ros.org/gmapping
2 代碼
open_gmapping的代碼比較復(fù)雜，比較亂． csdn博主 白茶-清歡 對(duì) open_gmapping 與 slam_gmapping 兩個(gè)包進(jìn)行了重寫，整理了代碼使得代碼結(jié)構(gòu)更加清晰，同時(shí)添加了注釋，還增加了激光雷達(dá)數(shù)據(jù)的畸變校正功能．
本篇文章的代碼實(shí)現(xiàn)是參考于 csdn博主 白茶-清歡 注釋簡(jiǎn)化之后的GMapping．其地址為 csdn 白茶-清歡:?https://blog.csdn.net/zhao_ke_xue/article/details/109712355
2.1 獲取代碼
代碼已經(jīng)提交在github上了，如果不知道github的地址的朋友, 請(qǐng)?jiān)谖业墓娞?hào):?從零開(kāi)始搭激光SLAM?中回復(fù)?開(kāi)源地址?獲得。
推薦使用 git clone 的方式進(jìn)行下載, 因?yàn)榇a是正處于更新?tīng)顟B(tài)的, git clone 下載的代碼可以使用 git pull 很方便地進(jìn)行更新.
本篇文章對(duì)應(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ù)
這個(gè)函數(shù)先進(jìn)行角度值的cos與sin的計(jì)算，然后調(diào)用PublishMap()計(jì)算地圖并發(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); //存儲(chǔ)上一次地圖更新的時(shí)間 if (!got_first_scan_) //如果是第一次接收scan { // 將雷達(dá)各個(gè)角度的sin與cos值保存下來(lái)，以節(jié)約計(jì)算量 CreateCache(scan_msg); got_first_scan_ = true; //改變第一幀的標(biāo)志位 } start_time_ = std::chrono::steady_clock::now(); // 計(jì)算當(dāng)前雷達(dá)數(shù)據(jù)對(duì)應(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)換一次地圖用時(shí): " << time_used_.count() << " 秒。" << std::endl;}2.3 PublishMap()
這里的ScanMatcherMap為GMapping中存儲(chǔ)地圖的數(shù)據(jù)類型，先聲明了一個(gè)ScanMatcherMap的對(duì)象gmapping_map_，然后通過(guò)ComputeMap()為gmapping_map_賦值，然后再將gmapping_map_中存儲(chǔ)的值賦值到ros的柵格地圖的數(shù)據(jù)類型中．
gmapping認(rèn)為ros的柵格地圖數(shù)據(jù)只需要有3個(gè)值
-1 代表柵格狀態(tài)未知
0 代表柵格是空閑的，代表可通過(guò)區(qū)域
100 代表柵格是占用的，代表障礙物，不可通過(guò)
2.4 ComputeMap()
這部分代碼分為2個(gè)部分，第一部分為計(jì)算出地圖的儲(chǔ)存空間，并且計(jì)算出從雷達(dá)到激光點(diǎn)這條線在gmapping柵格地圖中的坐標(biāo)，以及雷達(dá)點(diǎn)的坐標(biāo)．第二部分為將計(jì)算好的直線與點(diǎn)在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; // 通過(guò)激光雷達(dá)的數(shù)據(jù)，找出地圖的有效區(qū)域 for (unsigned int i = 0; i < scan_msg->ranges.size(); i++) { // 排除錯(cuò)誤的激光點(diǎn) 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ù)點(diǎn)在地圖坐標(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算法來(lái)計(jì)算 從激光位置到激光點(diǎn) 要經(jīng)過(guò)的柵格的坐標(biāo) GridLineTraversalLine line; GridLineTraversal::gridLine(p0, p1, &line); // 將line保存起來(lái)以備后用 line_lists_.push_back(line); // 計(jì)算活動(dòng)區(qū)域的大小 for (int i = 0; i < line.num_points - 1; i++) { activeArea.insert(map.storage().patchIndexes(line.points[i])); } // 如果d // 同時(shí)如果d==max_use_range_那么說(shuō)明這個(gè)值只用來(lái)進(jìn)行標(biāo)記空閑區(qū)域 不用來(lái)進(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上更新空閑點(diǎn) 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點(diǎn) for (auto hit : hit_lists_) { IntPoint p1 = map.world2map(hit); map.cell(p1).update(true, hit); }}代碼中的注釋已經(jīng)說(shuō)明的很清楚了，這里簡(jiǎn)要說(shuō)明一下．
第一部分
首先生成了一個(gè)activeArea變量，用于記錄需要區(qū)域的范圍．
之后遍歷雷達(dá)數(shù)據(jù)點(diǎn)，使用bresemham畫線算法生成從激光位置到激光點(diǎn)的連線在柵格地圖中的坐標(biāo)，并將這些點(diǎn)放入activeArea用于計(jì)算區(qū)域，同時(shí)保存下來(lái)以備后用．
再判斷雷達(dá)數(shù)據(jù)點(diǎn)也就是 phit ．判斷這個(gè)點(diǎn)與預(yù)先設(shè)定的參數(shù) max_use_range_(雷達(dá)數(shù)據(jù)最大使用距離)，如果phit的距離小于max_use_range_則認(rèn)為是好的數(shù)據(jù)點(diǎn)，放入hit_lists_中以備后用．
最后，設(shè)置區(qū)域的大小以及分配內(nèi)存．
第二部分
第二部分做了兩個(gè)工作，一個(gè)是更新空閑點(diǎn)，代表可通過(guò)區(qū)域，另一個(gè)是更新hit點(diǎn)，也就是激光雷達(dá)數(shù)據(jù)點(diǎn)在地圖中的位置，代表著障礙物．
這部分之前是進(jìn)行了2次for循環(huán)，做了很多無(wú)用的計(jì)算，我進(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ù)，這個(gè)函數(shù)調(diào)用了gridLineCore()進(jìn)行直線坐標(biāo)點(diǎn)的計(jì)算，
然后判斷了計(jì)算出的直線坐標(biāo)點(diǎn)的起始點(diǎn)是否正確，如果不正確，就把直線的所有點(diǎn)順序反轉(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()
這個(gè)函數(shù)就是實(shí)際bresemham畫線算法的實(shí)現(xiàn)，代碼挺長(zhǎng)，但是挺簡(jiǎn)單的，只是區(qū)分了很多情況：斜率是否大于1，以及增長(zhǎng)的順序的方向判斷等等，具體的意思已經(jīng)在代碼注釋的很清楚了.
如果您想弄清楚理論過(guò)程，理解的更透徹一些，可以參看這篇文章?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; // 直線增長(zhǎng)的首末端點(diǎn)坐標(biāo) int xdirflag, ydirflag; // 橫縱坐標(biāo)增長(zhǎng)方向 int cnt = 0; // 直線過(guò)點(diǎn)的點(diǎn)的序號(hào) dx = abs(end.x - start.x); dy = abs(end.y - start.y); // 斜率絕對(duì)值小于等于1的情況，優(yōu)先增加x if (dy <= dx) { d = 2 * dy - dx; // 初始點(diǎn)P_m0值 incr1 = 2 * dy; // 情況(1) incr2 = 2 * (dy - dx); // 情況(2) // 將增長(zhǎng)起點(diǎn)設(shè)置為橫坐標(biāo)小的點(diǎn)處，將 x的增長(zhǎng)方向 設(shè)置為 向右側(cè)增長(zhǎng) if (start.x > end.x) { // 起點(diǎn)橫坐標(biāo)比終點(diǎn)橫坐標(biāo)大，ydirflag = -1(負(fù)號(hào)可以理解為增長(zhǎng)方向與直線始終點(diǎn)方向相反) x = end.x; y = end.y; ydirflag = (-1); xend = start.x; // 設(shè)置增長(zhǎng)終點(diǎn)橫坐標(biāo) } else { x = start.x; y = start.y; ydirflag = 1; xend = end.x; } //加入起點(diǎn)坐標(biāo) line->points.push_back(IntPoint(x, y)); cnt++; // 向 右上 方向增長(zhǎng) if (((end.y - start.y) * ydirflag) > 0) { while (x < xend) { x++; if (d < 0) { d += incr1; } else { y++; d += incr2; // 縱坐標(biāo)向正方向增長(zhǎng) } line->points.push_back(IntPoint(x, y)); cnt++; } } // 向 右下 方向增長(zhǎng) else { // ... 省略 } } // 斜率絕對(duì)值大于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 文件夾下，以及依賴的點(diǎn)的類文件放在了
Creating-2D-laser-slam-from-scratch/lesson4/include/lesson4/gmapping/utils 文件夾下.
2.6.1 地圖類
GMapping的地圖相關(guān)的文件有3個(gè)，array.h，harray2d.h，map.h．
具體的代碼實(shí)現(xiàn)我就不在這里說(shuō)明了，代碼挺多的，我也沒(méi)太看懂(哈哈)。
感興趣的同學(xué)可以仔細(xì)閱讀一下，這里的代碼都有注釋的，再次感謝白茶清歡小哥的工作．
GMapping的代碼比較老了，這塊的實(shí)現(xiàn)可以使用c++ 11的stl進(jìn)行更好的實(shí)現(xiàn)，這也是我不對(duì)這個(gè)代碼細(xì)講的原因，因?yàn)橹髸?huì)有更好的實(shí)現(xiàn)．
2.6.2 地圖占用值更新的實(shí)現(xiàn)
map.h中有個(gè)叫做PointAccumulator的結(jié)構(gòu)體，定義了柵格地圖的更新方式以及存儲(chǔ)值是如何計(jì)算的．其代碼如下
//PointAccumulator表示地圖中一個(gè)cell(柵格)包括的內(nèi)容/*acc：柵格累計(jì)被擊中位置n：柵格被擊中次數(shù)visits：柵格被訪問(wèn)的次數(shù)*///PointAccumulator的一個(gè)對(duì)象，就是一個(gè)柵格，gmapping中其他類模板的cell就是這個(gè)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); } //計(jì)算柵格被擊中坐標(biāo)累計(jì)值的平均值 inline Point mean() const { return 1. / n * Point(acc.x, acc.y); } //返回該柵格被占用的概率，范圍是 -1(沒(méi)有訪問(wèn)過(guò)) 、[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)); //該柵格被擊中的位置累計(jì)，最后取累計(jì)值的均值 FloatPoint acc; //n表示該柵格被擊中的次數(shù)，visits表示該柵格被訪問(wèn)的次數(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++;}可以看到，每個(gè)格子都有2個(gè)值，一個(gè)是visits，一個(gè)是n．
更新占用: 在更新被擊中的柵格時(shí), 也就是激光點(diǎn)所在的柵格, n與visits都會(huì)加1，
更新空閑: 在更新未被擊中的柵格時(shí)，也就是從激光到激光點(diǎn)之間的柵格，只有visits加1,
通過(guò)重載了()，獲取獲取每個(gè)格子的占用值，也可以說(shuō)成是被占用的概率．占用值的計(jì)算公式為 n/visists，如果是沒(méi)被更新過(guò)就返回-1．
由于visits大于等于n，所以占用值的范圍是 [0,1]的，只有當(dāng)這個(gè)值在大于0.25(默認(rèn)參數(shù))時(shí)，在賦值到ros地圖的時(shí)候才賦值為100，才認(rèn)為是真正的占用了．
舉個(gè)例子說(shuō)明一下柵格地圖的占用值更新的方式．
例如：當(dāng)雷達(dá)掃到人腿時(shí)，會(huì)對(duì)擊中點(diǎn)的柵格更新一次占用，這時(shí)在ROS地圖下這個(gè)點(diǎn)將被表示為障礙物．
如果人腿離開(kāi)了這個(gè)位置，在之后的過(guò)程中，有4次在這個(gè)柵格更新了空閑，就會(huì)將ROS地圖中這個(gè)柵格的狀態(tài)更新到空閑，也就是沒(méi)有障礙物．
原因：
第一次更新地圖時(shí)，這個(gè)格子的占用值為 1/1 = 1 > 0.25 ，就會(huì)將ROS地圖中這個(gè)格子設(shè)置為100，表示障礙物．
如果之后4次更新地圖，這個(gè)柵格都被更新空閑了，則這個(gè)格子的占用值將變?yōu)?1/5 = 0.2 <0.25了，所以就會(huì)將ROS地圖中對(duì)應(yīng)的柵格設(shè)置為0，變成可通過(guò)區(qū)域了．
3 運(yùn)行
3.1 launch文件
本篇文章對(duì)應(yīng)的數(shù)據(jù)包, 請(qǐng)?jiān)谖业墓娞?hào)中回復(fù)?lesson1?獲得，并將launch中的bag_filename更改成您實(shí)際的目錄名。
<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 編譯與運(yùn)行
下載代碼后，請(qǐng)放入您自己的工作空間中，通過(guò) catkin_make 進(jìn)行編譯．
由于是新增的包，所以需要通過(guò) rospack profile 命令讓ros找到這個(gè)新的包．
之后, 使用source命令，添加ros與工作空間的地址到當(dāng)前終端下，再通過(guò)如下命令運(yùn)行本篇文章對(duì)應(yīng)的程序
roslaunch?lesson4?make_gmapping_map.launch3.3 運(yùn)行結(jié)果
啟動(dòng)之后，會(huì)在rviz中顯示出如下畫面．
地圖是不斷地根據(jù)雷達(dá)數(shù)據(jù)進(jìn)行更新的，每次生成的地圖只是這一幀雷達(dá)數(shù)據(jù)轉(zhuǎn)換后的結(jié)果，不會(huì)累加之前的雷達(dá)數(shù)據(jù)．
同時(shí)會(huì)在終端中打印出如下消息．
3.4 結(jié)果分析
通過(guò)終端打印出來(lái)的信息可以得知，每次計(jì)算gmapping地圖，再將gmapping地圖的值賦值到ros的地圖中，再發(fā)布出來(lái)．這一系列操作大概需要花費(fèi)0.4秒，這是一個(gè)很長(zhǎng)的時(shí)間了．
可以通過(guò)如下命令來(lái)看下map這個(gè)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．
我們只用了一個(gè)線程，也就是雷達(dá)回調(diào)函數(shù)的這個(gè)線程．處理一次回調(diào)函數(shù)需要用時(shí)0.4秒，而雷達(dá)數(shù)據(jù)的間隔是0.1秒．
也就是說(shuō)，當(dāng)我們正在計(jì)算地圖的時(shí)候，會(huì)又有4幀雷達(dá)數(shù)據(jù)到達(dá)，當(dāng)回調(diào)函數(shù)的緩沖區(qū)設(shè)置為1時(shí)，那這4幀雷達(dá)數(shù)據(jù)將只保留最后一個(gè)，其他3幀數(shù)據(jù)將丟失掉．
這還只是80m * 80m范圍的地圖，構(gòu)建更大范圍地圖的時(shí)間將更久．所以，很多SLAM都將地圖的生成單獨(dú)開(kāi)一個(gè)線程，以保證不耽誤對(duì)實(shí)時(shí)性要求較高的前端里程計(jì)部分．
4 總結(jié)與Next
通過(guò)使用GMapping中的地圖數(shù)據(jù)格式，以及GMapping中的地圖計(jì)算方式，我們實(shí)現(xiàn)了將激光雷達(dá)數(shù)據(jù)構(gòu)建成柵格地圖的功能，雖然現(xiàn)在的建圖是單次的．
希望你通過(guò)這篇文章以及代碼，知道了如何將激光雷達(dá)數(shù)據(jù)寫成柵格地圖，知道了柵格地圖更新一次的耗時(shí)問(wèn)題．
下一篇文章還是進(jìn)行單次柵格地圖的構(gòu)建，只不過(guò)下次將使用Hector中構(gòu)建地圖的方式，為我們以后做scan-to-map做個(gè)基礎(chǔ)．
其他原創(chuàng)文章可以戳：柵格地圖的構(gòu)建基于PL-ICP的激光雷達(dá)里程計(jì)從零開(kāi)始搭二維激光SLAM --- 基于PL-ICP的幀間匹配
文章將在?CSDN: 李太白lx?與?知乎：李想?進(jìn)行同步更新，可以更方便的看代碼，歡迎大家關(guān)注。
同時(shí)，也希望您將這個(gè)公眾號(hào)推薦給您身邊做激光SLAM的人們，可以在公眾號(hào)中添加我的微信，進(jìn)激光SLAM交流群，大家一起交流SLAM技術(shù)。
如果您對(duì)我寫的文章有什么建議，或者想要看哪方面功能如何實(shí)現(xiàn)的，請(qǐng)直接在公眾號(hào)中回復(fù)，我可以收到，并將認(rèn)真考慮您的建議。
【您的在看與轉(zhuǎn)發(fā)，是對(duì)我莫大鼓勵(lì)】
總結(jié)
以上是生活随笔為你收集整理的基于栅格地图的粒子群算法_基于GMapping的栅格地图的构建的全部?jī)?nèi)容，希望文章能夠幫你解決所遇到的問(wèn)題。
- 上一篇： c#后台如何导出excel到本地_C#后
- 下一篇： 24有几种封装尺寸_Y6T16 光模块尺