前文45.在ROS中实现global planner（1）
实现了一个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 是距离目标点的距离，我们就是要规划路径，怎么找到距离目标有多远，其实这个距离是估计理想距离，当没有障碍物的时的距离，也就是直线距离
这里的直线距离又有两种方式表示
- 曼哈顿距离
- 欧式距离
显然网格计算适合使用曼哈顿距离的，其计算消耗要小很多
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 // 已经完成搜索的点
);
4. 测试
-
输入地图
image.png -
测试结果
61a441eaef0c4759aa68f90467b164d0.gif
