46.在ROS中实现global planner（2）

news2024/9/30 23:32:08

前文实现了一个global planner的模板，并且可以工作，本文将实现astar算法，为后续完成一个astar global planner做准备

1. AStar简介

1.1 AStar

Astar算法是一种图形搜索算法,常用于寻路。Astar算法原理网上可以找到很多，简单的说就是，从起点开始，向外发散，再去其中每个点到终端的估计距离最短的，继续循环上次步骤，直到到达目标点。

1.2 启发函数

估算距离(f)=距离起点距离(G)+距离终点的距离(H)

显然G是已知的，

第一次从起点开始，G当然是0，
向外发散也就是上下左右，距离起点当然是1,也就是其父节点的G+1

H 是距离目标点的距离，我们就是要规划路径，怎么找到距离目标有多远，其实这个距离是估计理想距离，当没有障碍物的时的距离，也就是直线距离

这里的直线距离又有两种方式表示

曼哈顿距离
$x + y$
欧式距离
$\sqrt{x^2 + y^2}$

显然网格计算适合使用曼哈顿距离的，其计算消耗要小很多

2. 实现过程

2.1 数据结构

上面简单提到实现过程，下面我们先定义数据结构，我们需要保存当前已经搜索的节点，同时需要找到最小的f值，然后在该节点进行继续搜索和添加

节点定义


class Grid {
 public:
  Point parent_point_;
  Point point_;
  float g_;
  float h_;  // f = g + h
};

节点定义比较简单，也就是当前点坐标，父节点坐标，g，h值

open list
需要保存当前已经搜索点的列表，由于下次搜索有需要搜有f最小值，我们定义一个有限队列，这样我们取top就可以得到最小f的节点

struct greater {
bool operator()(const Grid& g1, const Grid& g2) const {
    float f1 = g1.h_ + g1.g_;
    float f2 = g2.h_ + g2.g_;

    return f1 > f2 || (f1 == f2 && g1.g_ < g2.g_);
}
};
std::priority_queue<Grid, std::vector<Grid>, greater> open_list_;

2.2 邻域

邻域定义较简单，定义为相对该点的偏移即可

  std::vector<Point> neighbors_;

  // 四邻域
  neighbors_.emplace_back(-1, 0);
  neighbors_.emplace_back(1, 0);
  neighbors_.emplace_back(0, -1);
  neighbors_.emplace_back(0, 1);

  // 八领域再加上下面
  neighbors_.emplace_back(-1, -1);
  neighbors_.emplace_back(1, 1);
  neighbors_.emplace_back(1, -1);
  neighbors_.emplace_back(-1, 1);

2.3 搜索实现

2.3.1 搜索过程

简单概括就是搜索过程就是不断最小的f值的节点的邻域，直到到达终点

伪代码如下

open_list.push(start);

while(!open_list_.empty()) {
    // 取最前面的也就是最小的f节点
    Grid grid = open_list.top();
    open_list.pop();

    // 直到当前搜索点 为终点，终止循环
    if (grid.point == end.point) {
      return true;
    }

    // 循环这个节点的邻居V节点， 分别计算g h， 同时把这些节点添加到open_list
    for (neighbor:neighbors) {
        Grid current;
        current.g_ = grid.g_ + 1
        current.h_ = calc_h(grid, neighbor, end); // 计算邻域的h
        current.parent_point_ = grid.point;  // 更新父节点

        if (!(current in open_list)) {
            // 如果该点已经不在open list中则添加
            open_list.push(current);
        else {
            // 如果该点已经存在open list中 则根据V计算结果确认是否需要更新
            float f = current.g_ + current.h_;
            open_list[current.point].g_ = current.g_ ;
            open_list[current.point].h_ = current.h_ ;
            open_list[current.point].parent_point_ = grid.point;  // 更新父节点
        }
    }
}

2.3.2 得到路径

grid结构可以看出来，其实相当于一个链表结构，找到路径后，只需要从end循环即可得到路径

bool GetPathFromGrid(const Point& start_point, const Point& end_point, std::vector<Point>& path) {
  path.clear();

  path.push_back(end_point);

  int start_index;
  bool ret = Point2Index(start_point, start_index);
  if (!ret) {
    return false;
  }

  int index;
  Point point = end_point;
  ret = Point2Index(point, index);
  if (!ret) {
    return false;
  }

  while (index != start_index) {
    point = all_grid_[index].parent_point_;
    path.push_back(point);

    Point2Index(point, index);
  }

  return true;
}

3. 测试验证

3.1 输入

为了方便我们直接读取png图，这样我们直接编辑图就可以直接用于测试,

   // 使用opencv直接读取png图片
   cv::Mat mat = cv::imread("../map/map_demo.png", cv::IMREAD_GRAYSCALE);
    // 为了保持习惯 我们反转下， 值255认为障碍物(读取的图片255是白色)
   cv::threshold(mat, mat, 128, 255, CV_THRESH_BINARY_INV);

3.2 显示

为了方便我们观察过程，我们设计一个函数用于显示规划和过程，为了简便我们使用opencv窗口

void Display(const cv::Mat& map_data,    // 传入grid map
             cv::Point begin,           // 起点
             cv::Point end,                // 终点
             const std::vector<cv::Point>& path,  // 输出的路径
             const std::vector<cv::Point>& close_list  // 已经完成搜索的点
             );