承德网站建设公司,手机如何制作软件app,服装网络营销是什么,来个网站2021能用的前文实现了一个global planner的模板#xff0c;并且可以工作#xff0c;本文将实现astar算法#xff0c;为后续完成一个astar global planner做准备
1. AStar简介
1.1 AStar
Astar算法是一种图形搜索算法,常用于寻路。Astar算法原理网上可以找到很多#xff0c;简单的说…前文实现了一个global planner的模板并且可以工作本文将实现astar算法为后续完成一个astar global planner做准备
1. AStar简介
1.1 AStar
Astar算法是一种图形搜索算法,常用于寻路。Astar算法原理网上可以找到很多简单的说就是从起点开始向外发散再去其中每个点到终端的估计距离最短的继续循环上次步骤直到到达目标点。
1.2 启发函数
估算距离(f)距离起点距离(G)距离终点的距离(H)
显然G是已知的
第一次从起点开始G当然是0向外发散也就是上下左右距离起点当然是1,也就是其父节点的G1
H 是距离目标点的距离我们就是要规划路径怎么找到距离目标有多远其实这个距离是估计理想距离当没有障碍物的时的距离也就是直线距离
这里的直线距离又有两种方式表示
曼哈顿距离 xyxy xy欧式距离 x2y2\sqrt{x^2 y^2} x2y2 显然网格计算适合使用曼哈顿距离的其计算消耗要小很多 2. 实现过程
2.1 数据结构
上面简单提到实现过程下面我们先定义数据结构 我们需要保存当前已经搜索的节点同时需要找到最小的f值然后在该节点进行继续搜索和添加
节点定义 class Grid {public:Point parent_point_;Point point_;float g_;float h_; // f g h
};节点定义比较简单也就是当前点坐标父节点坐标gh值
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_queueGrid, std::vectorGrid, greater open_list_;2.2 邻域
邻域定义较简单定义为相对该点的偏移即可 std::vectorPoint 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::vectorPoint 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::vectorcv::Point path, // 输出的路径const std::vectorcv::Point close_list // 已经完成搜索的点);4. 测试 输入地图 测试结果
