A*算法

A* 算法的最大时间消耗在于将节点插入open_set中，如果我们用优先队列优化，底层实际是二叉堆，因此时间复杂度是O(log(n))。所以我们的最大优化方式有两个，一个是优化open_set的数据结构，而另外一个就是尽可能的减少在寻路过程中加入到open_set中的点。

因此可行的解决的方式有：

  1. open_set数据结构的优化可以使用优先队列实现，如下面示例代码所示
  2. 判断是否将点插入open_set的依据是F值，而如果在寻路的过程中，如果两点中间存在大阻挡，将会使得将很多无用的点塞入open_set中，因此常见的一种做法是进行分块处理，在可绕过阻挡块处设置寻路点。

以下图为例，不同块之间除了黑点之外都是阻挡（黑点是桥梁），因此可以把黑点直接设置为寻路点，这样A -> C，只要分成三个 A 寻路即可，避免了“撞墙”，大大提升了效率
image.png
如果图形是没有阻挡的，实际上使用分块的方式依然可以提升整体的性能，所以还是有必要提前划分某些寻路点。
*当然要注意，这种分块的形式，得到的一般是一个近似的最短路径，在实际项目工程中是可以接受的

  1. 第三种方式就是使用更快的寻路方式了，可以使用JPS寻路算法
  1. import math
  2. import heapq
  4. # f = g + h
  5. class Node():
  6.     Weight = 10  # 这里的权重可以根据不同的地形修改，比如沙漠，那么值应该大些，平原值可以小些
  7.     D = 10
  8.     DD = 14
  9.     def __init__(self, x, y, g=0, h=0):
  10.         self.x = x  # 坐标x
  11.         self.y = y  # 坐标y
  12.         self.g = g  # 当前点到起始点的代价,dijkstra算法
  13.         self.h = h  # 当前点到终点的代价,最佳优先搜索算法
  14.         self.father = None
  16.     def calAddG(self, current_node):
  17.         if abs(self.x - current_node.x) == 1 and abs(self.y - current_node.y) == 1:
  18.             return DD * Node.Weight  # 斜线距离,代价为14
  19.         else:
  20.             return D * Node.Weight  # 直线距离,代价为10
  22.     def setG(self, val):
  23.         self.g = val
  25.     def calH(self, node):  # 曼哈顿距离,也可以使用欧氏距离
  26.         dx = abs(self.x - node.x)
  27.         dy = abs(self.y - node.y)
  28.         # H的计算主要有三种
  29.         # 1. 曼哈顿距离，适用于只有直线的情况下：(dx + dy) * D
  30.         # 2. 对角距离：适用于直线+45度角的情况：(dx + dy) + (DD - 2*D) * min(dx, dy)
  31.         # 3. 欧几里得距离：适用于任意角度，但是还有sqrt操作，相对较慢
  32.         # D * sqrt(dx * dx + dy * dy)
  33.         return D * sqrt(dx * dx + dy * dy)
  35.     def setH(self, val):
  36.         self.h = val
  38.     def setFather(self, node):
  39.         self.father = node
  41.     def __lt__(self, other):
  42.         return id(self) < id(other)
  44.     def __le__(self, other):
  45.         return id(self) >= id(other)
  48. class AStar():
  50.     def __init__(self, graph):
  51.         self.graph = graph
  52.         self.max_row = len(graph)
  53.         self.max_col = len(graph[0])
  55.     def isValid(self, node):
  56.         row = node.x
  57.         col = node.y
  58.         return 0 <= row < self.max_row and 0 <= col < self.max_col and self.graph[row][col] != "#"
  60.     def search(self, start_node, end_node):
  61.         if not (self.isValid(start_node) and self.isValid(end_node)):  # 检查节点的合法性
  62.             return []
  63.         start_node.calH(end_node)
  64.         start_node.setG(0)
  65.         path_list = []  # 最后的路径列表
  66.         open_list = []  # 当前可选择的点(f, node)
  67.         close_list = []  # 已经选择过的点(x,y)
  68.         heapq.heappush(open_list, (start_node.g + start_node.h, start_node))
  69.         while True:
  70.             _val, current_node = heapq.heappop(open_list)  # 从优先队列中弹出最小值
  71.             close_list.append((current_node.x, current_node.y))
  72.             self.searchNear(current_node, end_node, open_list, close_list)
  73.             open_node = self.getFromOpenList(end_node, open_list)
  74.             if open_node:  # 最后的结束节点已在open_list中
  75.                 while True:
  76.                     path_list.append((open_node.x, open_node.y))
  77.                     if open_node.father != None:
  78.                         open_node = open_node.father
  79.                     else:
  80.                         break
  81.             elif len(open_list) == 0:
  82.                 break
  83.         return path_list
  85.     def searchNear(self, current_node, end_node, open_list, close_list):
  86.         """
  87.         搜索节点周围的点
  88.         按照八个方位搜索
  89.         (x-1,y-1)(x-1,y)(x-1,y+1)
  90.         (x  ,y-1)(x  ,y)(x  ,y+1)
  91.         (x+1,y-1)(x+1,y)(x+1,y+1)
  92.         """
  93.         pos_list = [(-1, -1), (-1, 0), (-1, 1), (0, -1), (0, 1), (1, -1), (1, 0), (1, 1)]
  94.         for pos_x, pos_y in pos_list:
  95.             self.searchOneNode(current_node, end_node, Node(current_node.x + pos_x, current_node.y + pos_y), close_list,
  96.                                open_list)
  98.     def searchOneNode(self, current_node, end_node, node, close_list, open_list):
  99.         """
  100.         搜索一个节点
  101.         """
  102.         row = node.x
  103.         col = node.y
  104.         if self.graph[row][col] == "#":  # 是障碍点
  105.             return
  106.         if (node.x, node.y) in close_list:  # 已遍历过
  107.             return
  108.         open_node = self.getFromOpenList(node, open_list)
  109.         tempG = current_node.g + node.calAddG(current_node)
  110.         tempH = node.calH(end_node)
  111.         if not open_node:  # 当前node不在open_list中,计算其G,H存入open_list中
  112.             node.setG(tempG)
  113.             node.setH(tempH)
  114.             node.setFather(current_node)
  115.             heapq.heappush(open_list, (node.g + node.h, node))
  116.         else:
  117.             if (open_node.g > tempG):
  118.                 # need heapify
  119.                 open_node.setG(tempG)
  120.                 open_node.setH(tempH)
  121.                 open_node.setFather(current_node)
  122.                 heapq.heapify(open_list) # 重新调整
  124.     def getFromOpenList(self, node, open_list):  # 如果不在open_list中则返回False,亦可作为判断用
  125.         if not open_list:
  126.             return None
  127.         for _val, open_node in open_list:
  128.             if open_node.x == node.x and open_node.y == node.y:
  129.                 return open_node
  130.         return None
  133. if __name__ == '__main__':
  134.     graph = [list("####################"),
  135.              list("#*****#************#"),
  136.              list("#*****#*****#*####*#"),
  137.              list("#*########*##******#"),
  138.              list("#*****#*****######*#"),
  139.              list("#*****#####*#******#"),
  140.              list("####**#*****#*######"),
  141.              list("#*****#**#**#**#***#"),
  142.              list("#**#*****#**#****#*#"),
  143.              list("####################")]
  145.     print('graph: ')
  147.     for g in graph:
  148.         print(g)
  150.     star = AStar(graph)
  151.     path_list = star.search(Node(1, 1), Node(8, 18))
  153.     for x, y in path_list:
  154.         graph[x][y] = "O"
  155.     print('after A* graph: ')
  157.     for g in graph:
  158.         print(g)
  1. #include <limits.h>
  2. #include <memory>
  3. #include <vector>
  4. #include <queue>
  5. #include <math.h>
  6. #include <unordered_map>
  7. #include <iostream>
  9. using namespace std;
  11. class aStar
  12. {
  13. public:
  14.     using Map = vector<vector<int>>;
  15.     vector<pair<int, int>> search(const Map&, int, int, int, int);
  16. private:
  17.     bool _isValid(const Map&, int, int);
  18.     int _calH(int, int, int, int);
  19.     int _calG(const Map&, int, int, int);
  20.     static int D; // 直线运动的代价
  21.     static int DD; // 斜线运动的代价
  22.     static vector<pair<int, int>> path;
  23.     int _maxX; // 当前地图X的最大值
  24.     int _maxY; // 当前地图Y的最大值
  26.     struct Node
  27.     {
  28.         int x; // x轴位置
  29.         int y; // y轴位置
  30.         int g; // g值
  31.         int h; // h值
  32.         Node* parent; // 父节点
  33.         bool close; // 是否已经在close中，true是，false否
  34.         Node(int x, int y): x(x), y(y), close(false), parent(nullptr){}
  35.     };
  36.     struct cmp
  37.     {
  38.         bool operator()(const Node* node1, const Node* node2)
  39.         {
  40.             return node1->g + node1->h > node2->g + node2->h;
  41.         }
  42.     };
  43. };
  45. int aStar::D = 10;
  46. int aStar::DD = 14;
  47. vector<pair<int, int>> aStar::path{
  48.     /*
  49.     搜索节点周围的点
  50.     按照八个方位搜索
  51.     (x-1,y-1)(x-1,y)(x-1,y+1)
  52.     (x  ,y-1)(x  ,y)(x  ,y+1)
  53.     (x+1,y-1)(x+1,y)(x+1,y+1)
  54.     */
  55.     {-1, 0}, {1, 0}, {0, -1}, {0, 1}, {-1, -1}, {-1, 1}, {1, -1}, {1, 1} // 前四个方向是直线，后四个是斜线
  56. };
  58. bool aStar::_isValid(const Map& map, int x, int y)
  59. {
  60.     return 0 <= x < _maxX && 0 <= y < _maxY && map[x][y] > -1;
  61. }
  63. int aStar::_calH(int current_x, int current_y, int end_x, int end_y)
  64. {
  65.     int dx = abs(current_x - end_x);
  66.     int dy = abs(current_y - end_y);
  67.     /*
  68.     三种计算方式：
  69.     1. 曼哈顿距离，适合只能直线运动的场景：(dx + dy) * D
  70.     2. 对角距离，适合直线+45度角运动的场景：(dx + dy) + (DD - 2*D) * min(dx, dy)
  71.     3. 欧几里得距离：适合任意角度运行，有sqrt操作，相对较慢，开方可以使用别的方式优化：D * sqrt(dx * dx + dy * dy)
  72.     */
  73.     return D * sqrt(dx * dx + dy * dy);
  74. }
  76. int aStar::_calG(const Map& map, int dest_x, int dest_y, int weight)
  77. {
  78.     /*
  79.     如果是直线运动，则结果会是 D * 目标坐标点的地形权重，比如沙漠权重高点，平地权重就低点，即优先走平地
  80.     */
  81.     return weight * map[dest_x][dest_y]; 
  82. }
  84. vector<pair<int, int>> aStar::search(const Map& map, int start_x, int start_y, int end_x, int end_y)
  85. {
  86.     _maxX = map.size();
  87.     _maxY = map[0].size();
  88.     if (!_isValid(map, start_x, start_y) or !_isValid(map, end_x, end_y)) // 起始/终点坐标异常
  89.     {
  90.         return {};
  91.     }
  92.     unordered_map<int, Node*> nodeMap;
  93.     Node* startNode = new Node(start_x, start_y);
  94.     nodeMap[startNode->x + _maxX * startNode->y] = startNode; // x + maxX * y 这种方式可以保证x和y计算出来的值是唯一的
  95.     startNode->g = 0;
  96.     startNode->h = _calH(startNode->x, startNode->y, end_x, end_y);
  97.     priority_queue<Node*, vector<Node*>, cmp> Que; // 使用 priority_queue 会有重复插入的问题，因此需要使用close字段进行判断，和dijkstra算法处理相似
  98.     Que.emplace(startNode);
  99.     vector<pair<int, int>> ret;
  100.     while (!Que.empty())
  101.     {
  102.         Node* parentNode = Que.top();
  103.         Que.pop();
  104.         if (parentNode->close) continue; // 过滤重复
  105.         parentNode->close = true;
  106.         if (parentNode->x == end_x && parentNode->y == end_y)
  107.         {
  108.             while (parentNode->parent != nullptr)
  109.             {
  110.                 ret.emplace_back(parentNode->x, parentNode->y);
  111.                 parentNode = parentNode->parent;
  112.             }
  113.             ret.emplace_back(parentNode->x, parentNode->y);
  114.             // 内存释放，在实际项目工程中可以预先创建一定数量的node节点，然后每次申请节点只返回其中未被使用的节点，用完之后清除状态数据（x\y\close等），不用释放，这样是空间换时间
  115.             for (auto iter = nodeMap.begin(); iter != nodeMap.end(); ++iter) delete iter->second;
  116.             return ret;
  117.         }
  118.         for (int i = 0; i < path.size(); ++i)
  119.         {
  120.             int new_x = path[i].first + parentNode->x;
  121.             int new_y = path[i].second + parentNode->y;
  122.             if (!_isValid(map, new_x, new_y)) continue;
  123.             int idx = new_x + _maxX * new_y;
  124.             if (nodeMap.find(idx) != nodeMap.end()) // 已经遍历过
  125.             {
  126.                 Node* childNode = nodeMap[idx];
  127.                 if (childNode->close) continue;
  128.                 int new_g = _calG(map, new_x, new_y, i < 4 ? D : DD); // 直线 or 斜线，对于重复的节点，这里就会由于父节点位置的不同产生不同的值
  129.                 int new_h = _calH(new_x, new_y, end_x, end_y);
  130.                 if (new_g + new_h < childNode->g + childNode->h) // 有更小的权重，这里会造成重复插入，上面有过滤，因此无妨
  131.                 {
  132.                     childNode->g = new_g;
  133.                     childNode->h = new_h;
  134.                     childNode->parent = parentNode;
  135.                     Que.emplace(childNode);
  136.                 }
  137.             }
  138.             else
  139.             {
  140.                 Node* childNode = new Node(new_x, new_y);
  141.                 nodeMap[idx] = childNode;
  142.                 int new_g = _calG(map, new_x, new_y, i < 4 ? D : DD); // 直线 or 斜线
  143.                 int new_h = _calH(new_x, new_y, end_x, end_y);
  144.                 childNode->g = new_g;
  145.                 childNode->h = new_h;
  146.                 childNode->parent = parentNode;
  147.                 Que.emplace(childNode);
  148.             }
  149.         }
  150.     }
  151.     return {}; // 没有找到路径
  152. }
  155. int main()
  156. {
  157.     // vector<vector<int>> map{ // -1表示阻挡,其他数值表示当前点的权重值,值越大代表地形越"难走",优先级越低
  158.     //     {1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1},
  159.     //     {1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1},
  160.     //     {1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 1, 1, 1, 1, 1, 1},
  161.     //     {1, 0, 1, 1, 1, 1, 1, 1, 1, 1, 0, 1, 1, 0, 0, 0, 0, 0, 0, 1},
  162.     //     {1, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 0, 1},
  163.     //     {1, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 0, 1, 0, 0, 0, 0, 0, 0, 1},
  164.     //     {1, 1, 1, 1, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0, 1, 1, 1, 1, 1, 1},
  165.     //     {1, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 1, 0, 0, 1, 0, 0, 0, 1},
  166.     //     {1, 0, 0, 1, 0, 0, 0, 0 ,0, 1, 0 ,0, 1, 0, 0, 0, 0, 1, 0, 1},
  167.     //     {1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1}
  168.     // };
  169.     vector<vector<int>> map{ // -1表示阻挡,其他数值表示当前点的权重值,值越大代表地形越"难走",优先级越低
  170.         {-1,    -1,     -1,     -1,     -1,     -1,     -1,     -1,     -1,     -1},
  171.         {-1,    0,      5,      0,      0,      0,      1,      0,      -1,     -1},
  172.         {-1,    0,      10,      0,      0,      0,      1,      0,      -1,     -1},
  173.         {-1,    10,      -1,     -1,     -1,     -1,     -1,     -1,     0,      -1},
  174.         {-1,    0,      0,      0,      0,      0,      1,      0,      -1,     -1},
  175.         {-1,    0,      0,      0,      0,      0,      1,      1,      0,      -1},
  176.         {-1,    -1,     -1,     -1,     -1,     -1,     -1,     -1,     -1,     -1}
  177.     };
  178.     aStar a;
  179.     auto path = a.search(map, 1, 1, 5, 8);
  180.     for (int i = path.size()-1; i >= 0; --i)
  181.     {
  182.         cout << "(" << path[i].first << ", " << path[i].second << ")";
  183.         if (i != 0) cout << " => ";
  184.     }
  185.     cout << endl;
  186. }