前沿探索算法

概述

在SLAM 建图时，地图数据中有未知区域（NO_INFORMATION=255），障碍物（LETHAL_OBSTACLE=254），内切膨胀层（INSCRIBED_INFLATED_OBSTACLE=253），最大非障碍物（MAX_NON_OBSTACLE=252），自由空间（FREE_SPACE=0）。所谓的前沿指的就是邻接未知区域和自由空间之间的区域。前沿探索算法的目标就是不断地找到这些区域，然后让机器人到达这些区域进行更大范围的构建地图。下面我们讲讲如何寻找这些前沿。

什么是前沿

前沿就是邻接未知区域和自由空间且8连通的一组点。我们先看看前沿的结构体定义：

struct Frontier {
   
   
  std::uint32_t size;  // 点的个数
  double min_distance; // 距离机器人的最短距离
  double cost; // 前往该前沿的代价，代价越低越优先
  geometry_msgs::msg::Point initial; // 聚类前沿的起始点
  geometry_msgs::msg::Point centroid; // 前沿所有点的质心
  geometry_msgs::msg::Point middle; // 距离机器人的最短距离的点
  std::vector<geometry_msgs::msg::Point> points; // 前沿中的所有点
};

如何判断一个点是不是前沿点

先给出代码，结合代码进行说明。

bool FrontierSearch::isNewFrontierCell(unsigned int idx,
                                       const std::vector<bool>& frontier_flag)
{
   
   
  // check that cell is unknown and not already marked as frontier
  if (map_[idx] != NO_INFORMATION || frontier_flag[idx]) {
   
   
    return false;
  }

  // frontier cells should have at least one cell in 4-connected neighbourhood
  // that is free
  for (unsigned int nbr : nhood4(idx, *costmap_)) {
   
   
    if (map_[nbr] == FREE_SPACE) {
   
   
      return true;
    }
  }

  return false;
}

1. map_中存放的是代价值，如果当前点不在未知区域（NO_INFORMATION），直接判定false.
2. 如果该点之前已经标记为前沿，直接判定false.
3. 1 2 都不满足的情况下，如果该点的四邻域内存在自由区域，那么该点就是前沿。否则，判定为false.

如何寻找前沿

采用广度优先搜索算法进行寻找。同样还是结合代码进行说明。（为了代码清晰，采用伪代码）

td::vector<Frontier> searchFrom