首页 > 其他分享 >膨胀地图(一)

膨胀地图(一)

时间:2024-07-23 11:20:03浏览次数:11  
标签:障碍物 遍历 cost index 地图 cell inflation 膨胀

前面看过了静态地图与障碍物地图,但是对于路径规划而言,这两个地图是不够的,还需要第三张地图,也就是膨胀地图。

膨胀地图的意思还是比较好理解的,就是将地图的障碍物进行膨胀。为什么要这么操作呢,主要是考虑路径规划时,对于静态地图而言,不太好进行路径规划,因为一个栅格点虽然它可能是空闲的,但是如果它离障碍物很近的话,实际上这个点机器人是不能到达的,所以我们需要对地图按照机器人半径进行膨胀,这样子剩下的点才是真正可以运动的区域。

在move_base中,膨胀地图的内容不算多,只有简单的几个函数,这里简单看一下。

1、computeCaches

这个函数是一个预处理函数,所以我们将其放在最前面来看。这个函数主要执行的功能是预计算障碍物周围点到障碍物的距离以及代价值关系,后续会根据查表法来获取具体数据:

void InflationLayer::computeCaches()
{ //Case 1:如果栅格的膨胀半径是0,那就直接返回,啥也不用干了。
  if (cell_inflation_radius_ == 0)
    return;

  // based on the inflation radius... compute distance and cost caches
  //Case 2:如果栅格的膨胀半径不等于缓存的栅格膨胀半径,那么: 基于膨胀半径...计算距离和成本缓存
  if (cell_inflation_radius_ != cached_cell_inflation_radius_)
  {
    deleteKernels();

    cached_costs_ = new unsigned char*[cell_inflation_radius_ + 2]; //重新分配内存,建立cached_costs_
    cached_distances_ = new double*[cell_inflation_radius_ + 2];    //重新分配内存,建立cached_distances_
    //建立两层循环,从0到栅格膨胀半径+1,逐步填充代价和距离
    for (unsigned int i = 0; i <= cell_inflation_radius_ + 1; ++i)
    {
      cached_costs_[i] = new unsigned char[cell_inflation_radius_ + 2];
      cached_distances_[i] = new double[cell_inflation_radius_ + 2];
      for (unsigned int j = 0; j <= cell_inflation_radius_ + 1; ++j)
      {//hypot函数是一个C++标准库函数,它计算两个浮点数的平方和的平方根,即计算直角三角形的斜边长度。
        cached_distances_[i][j] = hypot(i, j);
      }
    }
    //更新缓存的膨胀半径
    cached_cell_inflation_radius_ = cell_inflation_radius_;
  }
  //计算代价
  for (unsigned int i = 0; i <= cell_inflation_radius_ + 1; ++i)
  {
    for (unsigned int j = 0; j <= cell_inflation_radius_ + 1; ++j)
    {
      cached_costs_[i][j] = computeCost(cached_distances_[i][j]);
    }
  }
}

这里的cell_inflation_radius_就是机器人的半径,当机器人的半径不为0时,需要计算一个膨胀表cached_costs_,它的作用类似于下图:

computeCost这个函数本身是计算一个点的代价值的,它的具体实现如下:

/** @brief  Given a distance, compute a cost.
   * @param  distance The distance from an obstacle in cells
   * @return A cost value for the distance */
  //首先进行距离的判断,如果距离为0,则返回代价为致命障碍物的代价值LETHAL_OBSTACLE;
  //再继续往下判断,距离分辨率是否小于内切半径,是的话,则返回代价为机器人内切圆膨胀障碍物的代价值INSCRIBED_INFLATED_OBSTACLE;
  //否的话,则代价按照欧几里德距离递减,欧几里德距离为距离分辨率,定义factor因子为指数函数,最后计算出cost值;
  virtual inline unsigned char computeCost(double distance) const
  {
    unsigned char cost = 0;
    if (distance == 0)
      cost = LETHAL_OBSTACLE;
    else if (distance * resolution_ <= inscribed_radius_)
      cost = INSCRIBED_INFLATED_OBSTACLE;
    else
    {
      // make sure cost falls off by Euclidean distance
      double euclidean_distance = distance * resolution_;
      double factor = exp(-1.0 * weight_ * (euclidean_distance - inscribed_radius_));
      cost = (unsigned char)((INSCRIBED_INFLATED_OBSTACLE - 1) * factor);
    }
    return cost;
  }

这段代码与上面的图片是契合的,对于障碍物的点,其值会为254,而离障碍物距离小于机器人半径的区域则是赋值253,再向外的点的值则是指数型下降。

2、updateCosts

这个函数是膨胀地图中的主要处理函数了,地图从障碍物地图变成膨胀地图就是通过这个函数实现的。首先需要获取一张现有的地图以及地图的尺寸信息:

  unsigned char* master_array = master_grid.getCharMap();//获取地图
  unsigned int size_x = master_grid.getSizeInCellsX(), size_y = master_grid.getSizeInCellsY();//获取网格x,y坐标

在获取到地图后,算法会遍历这张地图,然后将所有的障碍物点都先记录下来:

 std::vector<CellData>& obs_bin = inflation_cells_[0.0];
  for (int j = min_j; j < max_j; j++)
  {
    for (int i = min_i; i < max_i; i++)
    {
      int index = master_grid.getIndex(i, j);    //1)从master_grid地图获取(i,j)对应的索引index
      unsigned char cost = master_array[index];  //2)从master_grid地图获取索引对应的代价cost
      if (cost == LETHAL_OBSTACLE)               //3)先判断代价是否等于致命障碍物对应的代价,如果是,压入
      { //注意这里的obs_bin是一个指针,数据其实是存储在inflation_cells_下
        //std::map<double, std::vector<CellData> > inflation_cells_;
        obs_bin.push_back(CellData(index, i, j, i, j));
      }
    }
  }

注意这里记录的点都被记录在inflation_cells_中了,这是一个std::map<double, std::vector >类型的数据。

既然我们知道了所有的障碍物点了,下一步自然是需要对其进行扩张了,这里的扩张使用的是两个for循环实现的:

//根据膨胀队列,进行膨胀
  //通过上面的操作,现在inflation_queue_里面全部都是obstacle cell,这些cell被打包成CellData类型,包含了这些cell本身的索引,最近障碍物的索引(即它们本身),距离(0)
  std::map<double, std::vector<CellData> >::iterator bin;
  for (bin = inflation_cells_.begin(); bin != inflation_cells_.end(); ++bin)
  { //遍历每一个障碍物点
    //map键值对映射,键是距离最近的障碍物的距离,值是cell
    //second是“键”“值”对中的“值”,即遍历次数为celldata的个数
    for (int i = 0; i < bin->second.size(); ++i)
    {
      // process all cells at distance dist_bin.first
      //在该“double键”下记录迭代到的celldata
      const CellData& cell = bin->second[i];
      //记录该celldata的索引index
      unsigned int index = cell.index_;

      // ignore if already visited  如果已访问过,则忽略
      if (seen_[index])
      {
        continue;
      }

      seen_[index] = true;
      //得到该cell的坐标和离它最近的障碍物的坐标
      unsigned int mx = cell.x_;
      unsigned int my = cell.y_;
      unsigned int sx = cell.src_x_;
      unsigned int sy = cell.src_y_;

      // assign the cost associated with the distance from an obstacle to the cell
      //4)更新膨胀队列中点的cost 值
      //根据该CELL与障碍的距离来分配cost
      unsigned char cost = costLookup(mx, my, sx, sy);
      unsigned char old_cost = master_array[index];  //从master_grid查找指定索引index的代价
      //如果old_cost无信息或者cost≥内接膨胀障碍物,将cost更新到master_array
      if (old_cost == NO_INFORMATION && (inflate_unknown_ ? (cost > FREE_SPACE) : (cost >= INSCRIBED_INFLATED_OBSTACLE)))
        master_array[index] = cost;
      else//否则比较新旧大小,将大的cost更新到master_array
        master_array[index] = std::max(old_cost, cost);

      // attempt to put the neighbors of the current cell onto the inflation list
      //上面首先从inflation_cells_中取出最大distance的cell,再将这个cell的前后左右四个cell都塞进inflation_cells_。
      //由于下面关于enqueue的调用,最后两个参数(sx, sy)没有改变,所以保证了这个索引一定是obstacle cell。
      //由于在 enqueue入口会检查 if (!seen_[index]),这保证了这些cell不会被重复的塞进去。
      //由于这是一个priority queue,因此障碍物的周边点,每次都是离障碍物最远的点被弹出,并被检查,这样保证了这种扩张是朝着离障碍物远的方向进行。
      //尝试将当前单元的邻居(前后左右)放到队列中 
      if (mx > 0)
        enqueue(index - 1, mx - 1, my, sx, sy);
      if (my > 0)
        enqueue(index - size_x, mx, my - 1, sx, sy);
      if (mx < size_x - 1)
        enqueue(index + 1, mx + 1, my, sx, sy);
      if (my < size_y - 1)
        enqueue(index + size_x, mx, my + 1, sx, sy);
    }
  }

  inflation_cells_.clear();

其实上述这里实现的是一个对可能点的遍历,但是比较有意思的是一般这种遍历采用的都是while居多,但是这里使用的是一个对于map容器的for循环。

它的主体思想是这样子的:


1.将所有障碍物点都存储到map容器内
2.遍历map容器,取出其中每一个障碍物点,遍历其周围四邻近点,计算四邻近点到最近障碍物的距离以及其代价值,并存储到map容器内
3.遍历这些离障碍物较近的点,遍历其周围四邻近点,找到下一组比当前点更远离障碍物的点,计算其代价值并存储
4.循环步骤3直到剩下的点离障碍物距离都超出阈值,跳出for循环。


注意这里for循环为什么能够实现这个功能,跟上面的数据类型有很大关系:std::map<double, std::vector >的数据类型中第一位数据为点到障碍物的距离,初始时,map下存储的点都是障碍物的点,所以其键值都为0,然后遍历这些点的过程中会产生一系列四邻近点,这些点到障碍物的距离都为1个栅格的值,因此map下会产生一个新的键值以及一组新的地图点信息,这样子就实现了inflation_cells_的扩张以及inflation_cells_的循环遍历。而当所有接近障碍物的点都遍历完成后,将不会产生新的键值,则inflation_cells_的遍历也就结束了。

另外这里面还有一个比较重要的东西是:

// ignore if already visited  如果已访问过,则忽略
      if (seen_[index])
      {
        continue;
      }

      seen_[index] = true;

对于每一个点都只遍历一次,这样做的方式是为了防止重复遍历导致的数据异常,因为对于上述的遍历过程而言,前一时刻遍历过的点其离障碍物的距离值一定是比下一个循环遍历时离障碍物的距离值更近的(以障碍物为起点向外扩张)。

3、enqueue

这是膨胀地图中另外一个主要的函数,它的作用是扩散,即从障碍物点向外逐渐扩散直到所有离障碍物较近的点都被遍历完。

/**
 * @brief  Given an index of a cell in the costmap, place it into a list pending for obstacle inflation
 * @param  grid The costmap
 * @param  index The index of the cell
 * @param  mx The x coordinate of the cell (can be computed from the index, but saves time to store it)
 * @param  my The y coordinate of the cell (can be computed from the index, but saves time to store it)
 * @param  src_x The x index of the obstacle point inflation started at
 * @param  src_y The y index of the obstacle point inflation started at
 */
inline void InflationLayer::enqueue(unsigned int index, unsigned int mx, unsigned int my,
                                    unsigned int src_x, unsigned int src_y)
{
  if (!seen_[index])
  {
    // we compute our distance table one cell further than the inflation radius dictates so we can make the check below
    //找它和最近的障碍物的距离,如果超过了阈值,则直接返回
    //如果不超过阈值,就把它也加入inflation_cells_数组里
    double distance = distanceLookup(mx, my, src_x, src_y);

    // we only want to put the cell in the list if it is within the inflation radius of the obstacle point
    if (distance > cell_inflation_radius_)
      return;

    // push the cell data onto the inflation list and mark
    inflation_cells_[distance].push_back(CellData(index, mx, my, src_x, src_y));
  }
}

在updateCosts中,对于每一个点,会调用enqueue函数遍历其周围的四个点,在enqueue函数中,这个点如果已经被遍历过一次则跳过,否则会根据这个点离最近的障碍物的距离是否小于cell_inflation_radius_来决定是否需要对其进行updateCosts的计算,对于需要的点会被push进inflation_cells_,也就是我们第二个函数中两层for循环执行的点。

注意这里的cell_inflation_radius_与第一部分中计算时使用的inscribed_radius_是两个参数,这个参数应该会比inscribed_radius_要大一点。如果两个设置成一样的话应该是类似第一部分图中只保留254与253参数的格子,消除所有指数下降部分。

至此膨胀地图基本就看完了,这个部分在没看之前感觉比较高大上,但是实际看完发现东西其实不多。总结来说,就是预先计算一个查表,表中记录一组到达障碍物不同距离的点的代价值。然后遍历每一个障碍物点,这些障碍物点同步向外扩散,求其周围点到障碍物的距离,根据查表法求周围栅格的代价值。直到所有点都扩散到离障碍物较远的距离后,一张膨胀地图就算是计算完成了。

标签:障碍物,遍历,cost,index,地图,cell,inflation,膨胀
From: https://www.cnblogs.com/Gaowaly/p/18317887

相关文章

  • 障碍物地图(三)写一张障碍物地图
    花了不少时间看完了障碍物地图的大致思路,这里简单根据前面的思路来写一个简易版的障碍物地图。1.订阅一张地图首先,我们需要一张静态地图作为原始数据,这个我们可以订阅当前的map来获取:voidmap_test1::MapCallback(constnav_msgs::OccupancyGrid::ConstPtr&map){map_or......
  • 障碍物地图(二)
    上一篇大致看完了障碍物地图的初始化内容以及对于传感器数据的处理,我们知道在该部分算法维护了一个ObservationBuffer,其中存储了一段时间内的点云数据。每次新的数据进来后,还会根据设定的时间参数observation_keep_time抛弃比较久远的障碍物点云。但是在看的过程中,我们也产生了一......
  • 障碍物地图
    前面我们看完了栅格地图,知道了地图的基本数据结构,今天进一步的看一下障碍物地图。障碍物地图的存在更多是用于局部路径规划中所使用,因为大部分时候全局地图都是比较大的,那么很难保证其始终是一成不变的,所以如果我们只是按照全局地图进行路径规划,很可能会出现原有的地图中没有障碍......
  • 栅格地图
    在ROS中,地图是非常基本的元素,特别对于2D激光SLAM而言,栅格地图可以说是必不可少的元素。机器人在需要前往目标点时,需要在栅格地图中找到一条合适的路径从当前点到达目标点,这部分内容在move_base中有了详细的接口,可以直接调用并返回路径。但是作为一名工程师,不仅要知其然更要知其所......
  • openlayers在地图上使用Overlay渲染图标无法操作地图问题
    ol对于在地图上渲染图标,并且图标可以随着地图的缩放层级自适应,跟随地图移动,ol是提供了一个很好用的方法的---overlay代码如下://定义标注对象letlable_div=document.createElement('div')lable_div.className=[s.labelDiv]letoffsetY=0let_classname=''_classn......
  • 全地图商家采集工具
    1.引言地图信息采集软件在商业分析、城市规划和交通管理等领域发挥着重要作用。通过实时采集地图上的商户信息,该软件能够帮助用户快速获取所需的地理数据和商业信息。2.系统功能2.1地图数据采集多平台支持:软件支持百度地图、高德地图、腾讯地图等主流电子地图服务。......
  • 全地图商家采集工具
    1.引言地图信息采集软件在商业分析、城市规划和交通管理等领域发挥着重要作用。通过实时采集地图上的商户信息,该软件能够帮助用户快速获取所需的地理数据和商业信息。2.系统功能2.1地图数据采集多平台支持:软件支持百度地图、高德地图、腾讯地图等主流电子地图服务。实......
  • 【宏观经济学】第30章 货币增长与通货膨胀
    第30章货币增长与通货膨胀古典通货膨胀理论在长期中,物价总水平会调整到使货币需求等于货币供给的水平  货币数量论解释物价水平如何决定以及为什么它一直在变化经济中可得到的货币量决定了货币的价值,而且货币量增长是通货膨胀的主要原因  古典二分法所有经济变量分......
  • 注意!使用高德地图授权但还未缴费的用户,账号将被强制关闭
    这回高德是来真的了,以前三大图商的催收策略是不停地进行电话轰炸,限流、关闭账号、法律起诉等都是停留在口头的告知,并未真正地行动。现在是来真的了,前面的文章我也提到过,高德在今年7月将会出行新的政策,现在已经在其平台公布,且限流、关闭账号、等一些列催......
  • GIS地图可视化怎么做?这款免费工具帮你轻松搞定
    GIS地图可视化怎么做?山海鲸可视化这款免费可视化工具帮你轻松搞定。从三维GIS地图可视化需求出发,山海鲸可视化提供了强大的GIS场景编辑功能,包括支持添加倾斜摄影和地形编辑。无论是复杂的地形调整还是细致的倾斜摄影添加,这款工具都能轻松实现。山海鲸可视化是一款非常易用的软件......