现象描述

使用激光雷达传感器感知障碍物，建立动态地图，实时的在地图上添加和清除障碍物。有时，清除障碍物机制会不符合预期：
当障碍物出现在机器人面前时，local_costmap会出现障碍物，障碍物离开后，某些障碍物在地图上不消失，或者会残留障碍物的某些点。

注意：
当采用国产激光雷达时，由于分辨率低，才会出现本文所描述的现象，当采用hokuyo激光雷达时，可以适当忽略此问题。

原因及解决

1 激光雷达数据格式原因

1.1 原因分析

当物体消失后，若该方向上的探测距离大于激光雷达量程，则返回数据为大于max_range的值。例如，量程位30m的HOKUYO激光雷达，超量程后会返回65.33m；仿真环境中，量程为3.5m的激光雷达，超量程后的返回数据位inf；某些激光雷达，超量程后返回数据可能为0；具体情况，需要打开激光雷达后，测试其数据方可确定。
而move_base中对不同的激光雷达的超量程数据，没有很好的兼容处理，导致超量程数据没有正常使用，这就是问题根源。

1.2 解决措施

1.2.1 在配置文件中使能雷达数据无穷有效的功能

在costmap_common_params.yaml文件中设置：

obstacle_layer:...scan:{...inf_is_valid: true,...}

1.2.2 在程序中做点云数据处理修改

文件：navigation-kinetic-devel/costmap_2d/plugins/obstacle_layer.cpp
函数：ObstacleLayer::laserScanValidInfCallback
修改内容：

  sensor_msgs::LaserScan message = *raw_message;for (size_t i = 0; i < message.ranges.size(); i++){float range = message.ranges[ i ];// changed by kaikai.gao：将判断无穷修改为判断超出范围；// 若是超范围后数据为零，则可以判断小于epsilon；// if (!std::isfinite(range) && range > 0)if (range > message.range_max){message.ranges[ i ] = message.range_max - epsilon;}}

2 激光分辨率不足原因

2.1 原因分析

当设置地图的分辨率位0.01时，地图上的最小颗粒度位1cmX1cm的矩形。当设置局部地图的范围是5mX5m时，局部地图上距离机器人中心最远距离为5*1.414=7.07m（对角线）。最远距离出相邻两颗粒地图的角度为：弧度0.01/7.07,角度0.081。若要求激光可以扫描每一颗粒，则激光360度分辨率位360/0.081=4444，对于1圈4000一下的激光就无法达到。
当局部地图上障碍物消失后，由于某些颗粒处激光扫描不到，导致此处障碍物无法消失，只有机器人移动时，扫描的原点发生变化，才有概率扫描到那个位置，从而达到清扫障碍物的目的。

2.2 解决措施

如何在不增加精光分辨率的情况下，改变这一现象呢，需要修改清除障碍物的机制。
思想：将激光扫描末端点加大，相当于激光变粗了。
文件：navigation-kinetic-devel/costmap_2d/plugins/obstacle_layer.cpp
函数：ObstacleLayer::raytraceFreespace
修改内容：

void VoxelLayer::raytraceFreespace(const Observation& clearing_observation, double* min_x, double* min_y,double* max_x, double* max_y)
{if (clearing_observation.cloud_->points.size() == 0)return;double sensor_x, sensor_y, sensor_z;// 原点double ox = clearing_observation.origin_.x;double oy = clearing_observation.origin_.y;double oz = clearing_observation.origin_.z;if (!worldToMap3DFloat(ox, oy, oz, sensor_x, sensor_y, sensor_z)){ROS_WARN_THROTTLE(1.0,"The origin for the sensor at (%.2f, %.2f, %.2f) is out of map bounds. So, the costmap cannot raytrace for it.",ox, oy, oz);return;}bool publish_clearing_points = (clearing_endpoints_pub_.getNumSubscribers() > 0);if (publish_clearing_points){clearing_endpoints_.points.clear();clearing_endpoints_.points.reserve(clearing_observation.cloud_->points.size());}// 边界// we can pre-compute the enpoints of the map outside of the inner loop... we'll need these laterdouble map_end_x = origin_x_ + getSizeInMetersX();double map_end_y = origin_y_ + getSizeInMetersY();for (unsigned int i = 0; i < clearing_observation.cloud_->points.size(); ++i){// 激光点double wpx = clearing_observation.cloud_->points[i].x;double wpy = clearing_observation.cloud_->points[i].y;double wpz = clearing_observation.cloud_->points[i].z;// kaikai.gao:// 在这里增加激光雷达数据//在检测到的点周围生成点// 方法一：十字交叉点// double inflate_dx = 0.01, inflate_dy = 0.01; //在原来点的位置膨胀的尺度// std::vector< std::pair<double,double> > inflate_pts;// inflate_pts.push_back(std::make_pair(wpx +    0      , wpy +     0     ));// inflate_pts.push_back(std::make_pair(wpx -    0      , wpy - inflate_dy));// inflate_pts.push_back(std::make_pair(wpx - inflate_dx, wpy -     0     ));// inflate_pts.push_back(std::make_pair(wpx +    0      , wpy + inflate_dy));// inflate_pts.push_back(std::make_pair(wpx + inflate_dx, wpy +     0      ));// inflate_pts.push_back(std::make_pair(wpx -    0      , wpy - 2*inflate_dy));// inflate_pts.push_back(std::make_pair(wpx - 2*inflate_dx, wpy -     0     ));// inflate_pts.push_back(std::make_pair(wpx +    0        , wpy + 2*inflate_dy));// inflate_pts.push_back(std::make_pair(wpx + 2*inflate_dx, wpy +     0      ));// inflate_pts.push_back(std::make_pair(wpx -    0        , wpy - 3*inflate_dy));// inflate_pts.push_back(std::make_pair(wpx - 3*inflate_dx, wpy -     0     ));// inflate_pts.push_back(std::make_pair(wpx +    0        , wpy + 3*inflate_dy));// inflate_pts.push_back(std::make_pair(wpx + 3*inflate_dx, wpy +     0      ));  // 方法二：圆弧const double lidar_reslution = 0.25*3.1415926/180.0; // 0.25(实际分辨率)，4*0.25仿真分辨率const int half_div_N = 5;const double dth = lidar_reslution/2.0/(double)half_div_N;// 距离与角度double lx = wpx - ox;double ly = wpy - oy;double l = sqrt(lx*lx + ly*ly);double theta_center = atan2(ly, lx);// 生成角度序列std::vector<double> theta_s;theta_s.push_back(theta_center);double theta_t = 0.0;for (int i = 0; i < half_div_N; i++){theta_t = theta_center + (double)i*dth;theta_s.push_back(theta_t);theta_t = theta_center - (double)i*dth;theta_s.push_back(theta_t);}// 生成点序列std::vector< std::pair<double,double> > inflate_pts;for (int i = 0; i < theta_s.size(); i++){theta_t = theta_s[i];double x = ox + l*cos(theta_t);double y = oy + l*sin(theta_t);inflate_pts.push_back(std::make_pair(x, y));}std::vector< std::pair<double,double> >::iterator inflate_iter;for(inflate_iter = inflate_pts.begin(); inflate_iter != inflate_pts.end(); inflate_iter++){wpx = (*inflate_iter).first;wpy = (*inflate_iter).second;double distance = dist(ox, oy, oz, wpx, wpy, wpz);double scaling_fact = 1.0;scaling_fact = std::max(std::min(scaling_fact, (distance - 2 * resolution_) / distance), 0.0);wpx = scaling_fact * (wpx - ox) + ox;wpy = scaling_fact * (wpy - oy) + oy;wpz = scaling_fact * (wpz - oz) + oz;double a = wpx - ox;double b = wpy - oy;double c = wpz - oz;double t = 1.0;// we can only raytrace to a maximum z heightif (wpz > max_obstacle_height_){// we know we want the vector's z value to be max_zt = std::max(0.0, std::min(t, (max_obstacle_height_ - 0.01 - oz) / c));}// and we can only raytrace down to the floorelse if (wpz < origin_z_){// we know we want the vector's z value to be 0.0t = std::min(t, (origin_z_ - oz) / c);}// the minimum value to raytrace from is the originif (wpx < origin_x_){t = std::min(t, (origin_x_ - ox) / a);}if (wpy < origin_y_){t = std::min(t, (origin_y_ - oy) / b);}// the maximum value to raytrace to is the end of the mapif (wpx > map_end_x){t = std::min(t, (map_end_x - ox) / a);}if (wpy > map_end_y){t = std::min(t, (map_end_y - oy) / b);}wpx = ox + a * t;wpy = oy + b * t;wpz = oz + c * t;double point_x, point_y, point_z;if (worldToMap3DFloat(wpx, wpy, wpz, point_x, point_y, point_z)){unsigned int cell_raytrace_range = cellDistance(clearing_observation.raytrace_range_);// voxel_grid_.markVoxelLine(sensor_x, sensor_y, sensor_z, point_x, point_y, point_z);voxel_grid_.clearVoxelLineInMap(sensor_x, sensor_y, sensor_z, point_x, point_y, point_z, costmap_,unknown_threshold_, mark_threshold_, FREE_SPACE, NO_INFORMATION,cell_raytrace_range);updateRaytraceBounds(ox, oy, wpx, wpy, clearing_observation.raytrace_range_, min_x, min_y, max_x, max_y);if (publish_clearing_points){geometry_msgs::Point32 point;point.x = wpx;point.y = wpy;point.z = wpz;clearing_endpoints_.points.push_back(point);}}}}if (publish_clearing_points){clearing_endpoints_.header.frame_id = global_frame_;clearing_endpoints_.header.stamp = pcl_conversions::fromPCL(clearing_observation.cloud_->header).stamp;clearing_endpoints_.header.seq = clearing_observation.cloud_->header.seq;clearing_endpoints_pub_.publish(clearing_endpoints_);}
}

【移动机器人技术】move_base中障碍物无法清除的解决办法相关推荐

本文主要讲述如何开通自己的博客。若读者不想或已经知道如何开通使用博客，那么就可以跳过。一直以来，想把自己在学习过程中遇到的问题及解决办法共享给志同道合的人，那么如何分享自己的见解呢？有如下方法
本本文主要讲述如何开通自己的博客.若读者不想或已经知道如何开通使用博客,那么就可以跳过. 一直以来,想把自己在学习过程中遇到的问题及解决办法共享给志同道合的人,那么如何分享自己的见解呢?有如下方法, ...
IE中不显示背景颜色解决办法
IE中不显示背景颜色解决办法今天同学计算机上的IE不显示背景颜色,帮忙看了很久还以为是主题影响的,最后发现还是IE的设置问题.虽然不是这么高深的技术,但是希望能帮助遇到同样问题的朋友不走冤枉路. 控 ...
GDI+中发生一般性错误的解决办法 from http://www.cnblogs.com/winzheng/archive/2008/12/23/1360440.html...
GDI+中发生一般性错误的解决办法这个错误经常发生,代码如下: private static byte[] GetBytes (Image image) { ...
redhat enterprise 5 在 VMware 6.5 中中文显示乱码的解决办法
redhat enterprise 5 在 VMware 6.5 中中文显示乱码的解决办法事情的经过是这样的记得以前曾经在VMware 5.5.3 上装过redhat 估计是9吧当初我选的是图形 ...
Jupyter中打印所有结果的解决办法
学习笔记,仅供参考 Jupyter中打印所有结果的解决办法我们都知道,在Jupyter中,除非加上print,每个cell只会显示最后一个输出结果,这里,我们通过加上如下代码,使之打印所有的交互式输 ...
ubuntu18.0.4 不能下载 libgd2-dev（ubuntu 20.04 安装perl 中GD 模块失败的解决办法）
ubuntu18.0.4 不能下载 libgd2-dev 一.错误信息: Unable to locate package libgd2-dev 二.原因没有对应源到 https://packag ...
phpcmsV9 中 phpsso通信失败的解决办法
phpcmsV9 中 phpsso通信失败的解决办法 phpcmsV9 通信失败,很大一定程度上是因为通信地址设置不一致,很多人想象认为通信参数需要设置的必须保持一致才能实现正常的通信,实际检测并非如 ...
RS错误RSV-VAL-0032之项目未在布局中引用的3种解决办法
如下图所示,我用RS新建了一个空白页面,拖入了一个列表,给该列表新建了一个条件样式条件样式如下所示,表达式来自查询1 运行,报错如下图所示原因就是条件样式使用到了查询1中的数据项1但是数据项1在报 ...
项目中遇到的问题及解决办法
项目中遇到的问题及解决办法参考文章: (1)项目中遇到的问题及解决办法 (2)https://www.cnblogs.com/xulibing/p/5940741.html 备忘一下.

【移动机器人技术】move_base中障碍物无法清除的解决办法