本文主要是介绍从零开始搭二维激光SLAM --- 基于GMapping的栅格地图的构建,希望对大家解决编程问题提供一定的参考价值,需要的开发者们随着小编来一起学习吧!
上篇文章讲解了如何在ROS中发布栅格地图,以及如何向栅格地图赋值.
这篇文章来讲讲如何将激光雷达的数据构建成栅格地图.
雷达的数据点所在位置表示为占用,从雷达开始到这点之间的区域表示为空闲.
1 GMapping简介
GMapping是ROS中navigation导航包集中推荐的二维建图算法包,由于其实现时间早,所以各种书中的demo使用的SLAM基本都是GMapping,同时GMapping网上的教程也是最多的.
GMapping是基于粒子滤波算法实现的SLAM,通过里程计数据获取粒子群的先验位姿,再通过雷达数据与地图的匹配程度对所有粒子进行打分,通过分数高的粒子群来近似机器人的真实位姿.
GMapping的具体实现是在open_gmapping包里,后来又在ROS中做了个封装包slam_gmapping.gmapping在ROS中的wiki地址为 http://wiki.ros.org/gmapping
2 代码
open_gmapping的代码比较复杂,比较乱. csdn博主 白茶-清欢 对 open_gmapping 与 slam_gmapping 两个包进行了重写,整理了代码使得代码结构更加清晰,同时添加了注释,还增加了激光雷达数据的畸变校正功能.
本篇文章的代码实现是参考于 csdn博主 白茶-清欢 注释简化之后的GMapping.其地址为 csdn 白茶-清欢: https://blog.csdn.net/zhao_ke_xue/article/details/109712355
2.1 获取代码
代码已经提交在github上了,如果不知道github的地址的朋友, 请在我的公众号: 从零开始搭激光SLAM 中回复 开源地址 获得。
推荐使用 git clone 的方式进行下载, 因为代码是正处于更新状态的, git clone 下载的代码可以使用 git pull 很方便地进行更新.
本篇文章对应的代码为 Creating-2D-laser-slam-from-scratch/lesson4/src/gmapping/ 与 Creating-2D-laser-slam-from-scratch/lesson4/include/gmapping/
2.2 回调函数
这个函数先进行角度值的cos与sin的计算,然后调用PublishMap()计算地图并发布出去.
// 回调函数 进行数据处理
void GMapping::ScanCallback(const sensor_msgs::LaserScan::ConstPtr &scan_msg)
{static ros::Time last_map_update(0, 0); //存储上一次地图更新的时间if (!got_first_scan_) //如果是第一次接收scan{// 将雷达各个角度的sin与cos值保存下来,以节约计算量CreateCache(scan_msg);got_first_scan_ = true; //改变第一帧的标志位}start_time_ = std::chrono::steady_clock::now();// 计算当前雷达数据对应的栅格地图并发布出去PublishMap(scan_msg);end_time_ = std::chrono::steady_clock::now();time_used_ = std::chrono::duration_cast<std::chrono::duration<double>>(end_time_ - start_time_);std::cout << "\n转换一次地图用时: " << time_used_.count() << " 秒。" << std::endl;
}
2.3 PublishMap()
这里的ScanMatcherMap为GMapping中存储地图的数据类型,先声明了一个ScanMatcherMap的对象gmapping_map_,然后通过ComputeMap()为gmapping_map_赋值,然后再将gmapping_map_中存储的值赋值到ros的栅格地图的数据类型中.
gmapping认为ros的栅格地图数据只需要有3个值
- -1 代表栅格状态未知
- 0 代表栅格是空闲的,代表可通过区域
- 100 代表栅格是占用的,代表障碍物,不可通过
#define GMAPPING_UNKNOWN (-1)
#define GMAPPING_FREE (0)
#define GMAPPING_OCC (100)// 计算当前雷达数据对应的栅格地图并发布出去
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中存储地图的数据类型ScanMatcherMap gmapping_map_(center, xmin_, ymin_, xmax_, ymax_, resolution_);// 使用当前雷达数据更新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_时才认为是占用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_) // 默认0.25map_.data[MAP_IDX(map_.info.width, x, y)] = GMAPPING_OCC;// 空闲elsemap_.data[MAP_IDX(map_.info.width, x, y)] = GMAPPING_FREE;}}// 添加当前的时间戳map_.header.stamp = ros::Time::now();map_.header.frame_id = scan_msg->header.frame_id;// 发布map和map_metadatamap_publisher_.publish(map_);map_publisher_metadata_.publish(map_.info);
}
2.4 ComputeMap()
这部分代码分为2个部分,第一部分为计算出地图的储存空间,并且计算出从雷达到激光点这条线在gmapping栅格地图中的坐标,以及雷达点的坐标.第二部分为将计算好的直线与点在gmapping地图中进行栅格值的更新.
// 使用当前雷达数据更新GMapping地图中栅格的值
void GMapping::ComputeMap(ScanMatcherMap &map, const sensor_msgs::LaserScan::ConstPtr &scan_msg)
{line_lists_.clear();hit_lists_.clear();// lp为地图坐标系下的激光雷达坐标系的位姿OrientedPoint lp(0, 0, 0.0);// 将位姿lp转换成地图坐标系下的位置IntPoint p0 = map.world2map(lp);// 第一部分// 地图的有效区域(地图坐标系)HierarchicalArray2D<PointAccumulator>::PointSet activeArea;// 通过激光雷达的数据,找出地图的有效区域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为激光雷达的数据点在地图坐标系下的坐标Point phit = lp;phit.x += d * a_cos_[i];phit.y += d * a_sin_[i];IntPoint p1 = map.world2map(phit);// 使用bresenham算法来计算 从激光位置到激光点 要经过的栅格的坐标GridLineTraversalLine line;GridLineTraversal::gridLine(p0, p1, &line);// 将line保存起来以备后用line_lists_.push_back(line);// 计算活动区域的大小for (int i = 0; i < line.num_points - 1; i++){activeArea.insert(map.storage().patchIndexes(line.points[i]));}// 如果d<m_usableRange则需要把击中点也算进去 说明这个值是好的。// 同时如果d==max_use_range_那么说明这个值只用来进行标记空闲区域 不用来进行标记障碍物if (d < max_use_range_){IntPoint cp = map.storage().patchIndexes(p1);activeArea.insert(cp);hit_lists_.push_back(phit);}}// 为activeArea分配内存map.storage().setActiveArea(activeArea, true);map.storage().allocActiveArea();// 第二部分// 在map上更新空闲点for (auto line : line_lists_){// 更新空闲位置for (int k = 0; k < line.num_points - 1; k++){// 未击中,就不记录击中的位置了,所以传入参数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);}
}
代码中的注释已经说明的很清楚了,这里简要说明一下.
第一部分
首先生成了一个activeArea变量,用于记录需要区域的范围.
之后遍历雷达数据点,使用bresemham画线算法生成从激光位置到激光点的连线在栅格地图中的坐标,并将这些点放入activeArea用于计算区域,同时保存下来以备后用.
再判断雷达数据点也就是 phit .判断这个点与预先设定的参数 max_use_range_(雷达数据最大使用距离),如果phit的距离小于max_use_range_则认为是好的数据点,放入hit_lists_中以备后用.
最后,设置区域的大小以及分配内存.
第二部分
第二部分做了两个工作,一个是更新空闲点,代表可通过区域,另一个是更新hit点,也就是激光雷达数据点在地图中的位置,代表着障碍物.
这部分之前是进行了2次for循环,做了很多无用的计算,我进行优化了一下,现在只需要进行一次for循环.
2.5 bresemham画线算法
以下函数位于Creating-2D-laser-slam-from-scratch/lesson4/include/lesson4/gmapping/grid/gridlinetraversal.h文件中.
2.5.1 gridLine()
上述代码调用了gridLine()函数,这个函数调用了gridLineCore()进行直线坐标点的计算,
然后判断了计算出的直线坐标点的起始点是否正确,如果不正确,就把直线的所有点顺序反转一下,
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()
这个函数就是实际bresemham画线算法的实现,代码挺长,但是挺简单的,只是区分了很多情况:斜率是否大于1,以及增长的顺序的方向判断等等,具体的意思已经在代码注释的很清楚了.
如果您想弄清楚理论过程,理解的更透彻一些,可以参看这篇文章 https://www.jianshu.com/p/d63bf63a0e28
网上的教程也很多,我这就不再讲了.
void GridLineTraversal::gridLineCore(IntPoint start, IntPoint end, GridLineTraversalLine *line)
{int dx, dy; // 横纵坐标间距int incr1, incr2; // 增量int d; // int x, y, xend, yend; // 直线增长的首末端点坐标int xdirflag, ydirflag; // 横纵坐标增长方向int cnt = 0; // 直线过点的点的序号dx = abs(end.x - start.x);dy = abs(end.y - start.y);// 斜率绝对值小于等于1的情况,优先增加xif (dy <= dx){d = 2 * dy - dx; // 初始点P_m0值incr1 = 2 * dy; // 情况(1)incr2 = 2 * (dy - dx); // 情况(2)// 将增长起点设置为横坐标小的点处,将 x的增长方向 设置为 向右侧增长if (start.x > end.x){// 起点横坐标比终点横坐标大,ydirflag = -1(负号可以理解为增长方向与直线始终点方向相反)x = end.x; y = end.y;ydirflag = (-1);xend = start.x; // 设置增长终点横坐标}else{x = start.x;y = start.y;ydirflag = 1;xend = end.x;}//加入起点坐标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; // 纵坐标向正方向增长}line->points.push_back(IntPoint(x, y));cnt++;}}// 向 右下 方向增长else{while (x < xend){x++;if (d < 0){d += incr1;}else{y--;d += incr2; // 纵坐标向负方向增长}line->points.push_back(IntPoint(x, y));cnt++;}}}// 斜率绝对值大于1的情况,优先增加yelse{// dy > dx,当斜率k的绝对值|k|>1时,在y方向进行单位步进d = 2 * dx - dy; incr1 = 2 * dx; incr2 = 2 * (dx - dy);// 将增长起点设置为纵坐标小的点处,将 y的增长方向 设置为 向上侧增长if (start.y > end.y){y = end.y; // 取最小的纵坐标作为起点x = end.x;yend = start.y;xdirflag = (-1); }else{y = start.y;x = start.x;yend = end.y;xdirflag = 1;}// 添加起点line->points.push_back(IntPoint(x, y));cnt++;// 向 上右 增长if (((end.x - start.x) * xdirflag) > 0){while (y < yend){y++;if (d < 0){d += incr1;}else{x++;d += incr2; // 横坐标向正方向增长}//添加新的点line->points.push_back(IntPoint(x, y));cnt++;}}// 向 上左 增长else{while (y < yend){y++;if (d < 0){d += incr1;}else{x--;d += incr2; //横坐标向负方向增长}line->points.push_back(IntPoint(x, y));// 记录添加所有点的数目cnt++;}}}line->num_points = cnt;
}
2.6 GMapping中的地图数据类型
我将GMapping中的地图数据类放在了
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的地图相关的文件有3个,array.h,harray2d.h,map.h.
具体的代码实现我就不在这里说明了,代码挺多的,我也没太看懂(哈哈)。
感兴趣的同学可以仔细阅读一下,这里的代码都有注释的,再次感谢白茶清欢小哥的工作.
GMapping的代码比较老了,这块的实现可以使用c++ 11的stl进行更好的实现,这也是我不对这个代码细讲的原因,因为之后会有更好的实现.
2.6.2 地图占用值更新的实现
map.h中有个叫做PointAccumulator的结构体,定义了栅格地图的更新方式以及存储值是如何计算的.其代码如下
//PointAccumulator表示地图中一个cell(栅格)包括的内容
/*
acc: 栅格累计被击中位置
n: 栅格被击中次数
visits:栅格被访问的次数
*/
//PointAccumulator的一个对象,就是一个栅格,gmapping中其他类模板的cell就是这个struct PointAccumulator
{//float类型的pointtypedef point<float> FloatPoint;//构造函数PointAccumulator() : acc(0, 0), n(0), visits(0) {}PointAccumulator(int i) : acc(0, 0), n(0), visits(0) { assert(i == -1); }//计算栅格被击中坐标累计值的平均值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表示该栅格被击中的次数,visits表示该栅格被访问的次数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;}elsevisits++;
}
可以看到,每个格子都有2个值,一个是visits,一个是n.
更新占用: 在更新被击中的栅格时, 也就是激光点所在的栅格, n与visits都会加1,
更新空闲: 在更新未被击中的栅格时,也就是从激光到激光点之间的栅格,只有visits加1,
通过重载了(),获取获取每个格子的占用值,也可以说成是被占用的概率.占用值的计算公式为 n/visists,如果是没被更新过就返回-1.
由于visits大于等于n,所以占用值的范围是 [0,1]的,只有当这个值在大于0.25(默认参数)时,在赋值到ros地图的时候才赋值为100,才认为是真正的占用了.
举个例子说明一下栅格地图的占用值更新的方式.
例如:当雷达扫到人腿时,会对击中点的栅格更新一次占用,这时在ROS地图下这个点将被表示为障碍物.
如果人腿离开了这个位置,在之后的过程中,有4次在这个栅格更新了空闲,就会将ROS地图中这个栅格的状态更新到空闲,也就是没有障碍物.
原因:
第一次更新地图时,这个格子的占用值为 1/1 = 1 > 0.25 ,就会将ROS地图中这个格子设置为100,表示障碍物.
如果之后4次更新地图,这个栅格都被更新空闲了,则这个格子的占用值将变为 1/5 = 0.2 <0.25了,所以就会将ROS地图中对应的栅格设置为0,变成可通过区域了.
3 运行
3.1 launch文件
本篇文章对应的数据包, 请在我的公众号中回复 lesson1 获得,并将launch中的bag_filename更改成您实际的目录名。
<launch><!-- bag的地址与名称 --><arg name="bag_filename" default="/home/lx/bagfiles/lesson1.bag"/><!-- 使用bag的时间戳 --><param name="use_sim_time" value="true" /><!-- 启动节点 --><node name="lesson4_gmapping_node"pkg="lesson4" type="lesson4_gmapping_node" output="screen" /><!-- launch rviz --><node name="rviz" pkg="rviz" type="rviz" required="true"args="-d $(find lesson4)/config/gmapping.rviz" /><!-- play bagfile --><node name="playbag" pkg="rosbag" type="play"args="--clock $(arg bag_filename)" /></launch>
3.2 编译与运行
下载代码后,请放入您自己的工作空间中,通过 catkin_make 进行编译.
由于是新增的包,所以需要通过 rospack profile 命令让ros找到这个新的包.
之后, 使用source命令,添加ros与工作空间的地址到当前终端下,再通过如下命令运行本篇文章对应的程序
roslaunch lesson4 make_gmapping_map.launch
3.3 运行结果
启动之后,会在rviz中显示出如下画面.
地图是不断地根据雷达数据进行更新的,每次生成的地图只是这一帧雷达数据转换后的结果,不会累加之前的雷达数据.
同时会在终端中打印出如下消息.
转换一次地图用时: 0.403898 秒。转换一次地图用时: 0.394922 秒。转换一次地图用时: 0.40791 秒。
3.4 结果分析
通过终端打印出来的信息可以得知,每次计算gmapping地图,再将gmapping地图的值赋值到ros的地图中,再发布出来.这一系列操作大概需要花费0.4秒,这是一个很长的时间了.
可以通过如下命令来看下map这个topic的频率
$ rostopic hz /laser_scan /maptopic 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
可以看到,雷达数据是10hz的,map数据大概是2.5hz.
我们只用了一个线程,也就是雷达回调函数的这个线程.处理一次回调函数需要用时0.4秒,而雷达数据的间隔是0.1秒.
也就是说,当我们正在计算地图的时候,会又有4帧雷达数据到达,当回调函数的缓冲区设置为1时,那这4帧雷达数据将只保留最后一个,其他3帧数据将丢失掉.
这还只是80m * 80m范围的地图,构建更大范围地图的时间将更久.所以,很多SLAM都将地图的生成单独开一个线程,以保证不耽误对实时性要求较高的前端里程计部分.
4 总结与Next
通过使用GMapping中的地图数据格式,以及GMapping中的地图计算方式,我们实现了将激光雷达数据构建成栅格地图的功能,虽然现在的建图是单次的.
希望你通过这篇文章以及代码,知道了如何将激光雷达数据写成栅格地图,知道了栅格地图更新一次的耗时问题.
下一篇文章还是进行单次栅格地图的构建,只不过下次将使用Hector中构建地图的方式,为我们以后做scan-to-map做个基础.
文章将在 公众号: 从零开始搭SLAM 进行同步更新,欢迎大家关注,可以在公众号中添加我的微信,进激光SLAM交流群,大家一起交流SLAM技术。
同时,也希望您将这个公众号推荐给您身边做激光SLAM的人们,大家共同进步。
如果您对我写的文章有什么建议,或者想要看哪方面功能如何实现的,请直接在公众号中回复,我可以收到,并将认真考虑您的建议。
这篇关于从零开始搭二维激光SLAM --- 基于GMapping的栅格地图的构建的文章就介绍到这儿,希望我们推荐的文章对编程师们有所帮助!