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

现象描述

使用激光雷达传感器感知障碍物，建立动态地图，实时的在地图上添加和清除障碍物。有时，清除障碍物机制会不符合预期：
当障碍物出现在机器人面前时，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 later
  double 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 > 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 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 > 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 >::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 height
      if (wpz > max_obstacle_height_)
      {
        // we know we want the vector's z value to be max_z
        t = std::max(0.0, std::min(t, (max_obstacle_height_ - 0.01 - oz) / c));
      }
      // and we can only raytrace down to the floor
      else if (wpz < origin_z_)
      {
        // we know we want the vector's z value to be 0.0
        t = std::min(t, (origin_z_ - oz) / c);
      }

      // the minimum value to raytrace from is the origin
      if (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 map
      if (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_);
  }
}