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

前文实现了一个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_listfor (neighbor:neighbors) {Grid current;current.g_ = grid.g_ + 1current.h_ = calc_h(grid, neighbor, end); // 计算邻域的hcurrent.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 mapcv::Point begin,           // 起点cv::Point end,                // 终点const std::vector<cv::Point>& path,  // 输出的路径const std::vector<cv::Point>& close_list  // 已经完成搜索的点);

4. 测试

• 输入地图

• 测试结果