一、A* 算法的由来及应用背景

广度优先搜索（BFS）

为了更好的理解A*算法，我们首先从广度优先（Breadth First）算法讲起。

正如其名称所示，广度优先搜索以广度做为优先级进行搜索。

从起点开始，首先遍历起点周围邻近的点，然后再遍历已经遍历过的点邻近的点，逐步的向外扩散，直到找到终点。

这种算法就像洪水（Flood fill）一样向外扩张，算法的过程如下图所示：

在上面这幅动态图中，算法遍历了图中所有的点，这通常没有必要。对于有明确终点的问题来说，一旦到达终点便可以提前终止算法，下面这幅图对比了这种情况：

在执行算法的过程中，每个点需要记录达到该点的前一个点的位置 – 可以称之为父节点。这样做之后，一旦到达终点，便可以从终点开始，反过来顺着父节点的顺序找到起点，由此就构成了一条路径。

Dijkstra算法

Dijkstra算法是由计算机科学家Edsger W. Dijkstra在1956年提出的。

Dijkstra算法用来寻找图形中节点之间的最短路径。

考虑这样一种场景，在一些情况下，图形中相邻节点之间的移动代价并不相等。例如，游戏中的一幅图，既有平地也有山脉，那么游戏中的角色在平地和山脉中移动的速度通常是不相等的。

在Dijkstra算法中，需要计算每一个节点距离起点的总移动代价。同时，还需要一个优先队列结构。对于所有待遍历的节点，放入优先队列中会按照代价进行排序。

在算法运行的过程中，每次都从优先队列中选出代价最小的作为下一个遍历的节点。直到到达终点为止。

下面对比了不考虑节点移动代价差异的广度优先搜索与考虑移动代价的Dijkstra算法的运算结果：

当图形为网格图，并且每个节点之间的移动代价是相等的，那么Dijkstra算法将和广度优先算法变得一样。

最佳优先搜索

在一些情况下，如果我们可以预先计算出每个节点到终点的距离，则我们可以利用这个信息更快的到达终点。

其原理也很简单。与Dijkstra算法类似，我们也使用一个优先队列，但此时以每个节点到达终点的距离作为优先级，每次始终选取到终点移动代价最小（离终点最近）的节点作为下一个遍历的节点。这种算法称之为最佳优先（Best First）算法。

这样做可以大大加快路径的搜索速度，如下图所示：

但这种算法会不会有什么缺点呢？答案是肯定的。

因为，如果起点和终点之间存在障碍物，则最佳优先算法找到的很可能不是最短路径，下图描述了这种情况。

A*算法

对比了上面几种算法，最后终于可以讲解本文的重点：A*算法了。

下面的描述我们将看到，A*算法实际上是综合上面这些算法的特点于一身的。

A*算法通过下面这个函数来计算每个节点的优先级。

$$f(n)=g(n)+h(n)$$

其中：

• $f(n)$ 是节点 $n$ 的综合优先级。当我们选择下一个要遍历的节点时，我们总会选取综合优先级最高（值最小）的节点。
• $g(n)$ 是节点 $n$ 距离起点的代价。（在加入优先队列时，是按照离源点更近的点加入队列，即更小的 g 加入队列，而 f 是在队列中，下一次扩展的节点）
• $h(n)$ 是节点 $n$ 距离终点的预计代价，这也就是 $A^{\ast }$ 算法的启发函数。关于启发函数我们在下面详细讲解。

A*算法在运算过程中，每次从优先队列中选取值最小（优先级最高）的节点作为下一个待遍历的节点。

启发函数

上面已经提到，启发函数会影响A*算法的行为。

• 在极端情况下，当启发函数h(n)始终为0，则将由g(n)决定节点的优先级，此时算法就退化成了Dijkstra算法。
• 如果h(n)始终小于等于节点n到终点的代价，则A*算法保证一定能够找到最短路径。但是当h(n)的值越小，算法将遍历越多的节点，也就导致算法越慢。
• 如果h(n)完全等于节点n到终点的代价，则A*算法将找到最佳路径，并且速度很快。可惜的是，并非所有场景下都能做到这一点。因为在没有达到终点之前，我们很难确切算出距离终点还有多远。
• 如果h(n)的值比节点n到终点的代价要大，则A*算法不能保证找到最短路径，不过此时会很快。

在另外一个极端情况下，如果h(n)相较于g(n)大很多，则此时只有h(n)产生效果，这也就变成了最佳优先搜索。

由上面这些信息我们可以知道，通过调节启发函数我们可以控制算法的速度和精确度。因为在一些情况，我们可能未必需要最短路径，而是希望能够尽快找到一个路径即可。这也是A*算法比较灵活的地方。

对于网格形式的图，有以下这些启发函数可以使用：

• 如果图形中只允许朝上下左右四个方向移动，则可以使用曼哈顿距离（Manhattan distance）。
• 如果图形中允许朝八个方向移动，则可以使用对角距离。
• 如果图形中允许朝任何方向移动，则可以使用欧几里得距离（Euclidean distance）。

关于距离

曼哈顿距离

如果图形中只允许朝上下左右四个方向移动，则启发函数可以使用曼哈顿距离，它的计算方法如下图所示：

对角距离

如果图形中允许斜着朝邻近的节点移动，则启发函数可以使用对角距离。它的计算方法如下：

欧几里得距离

如果图形中允许朝任意方向移动，则可以使用欧几里得距离。

欧几里得距离是指两个节点之间的直线距离，因此其计算方法也是我们比较熟悉的：

/*
A* 应用场景:
起点→终点的最短距离
状态空间 >> 1e10 
启发函数减小搜索空间

A*算法:
while(q.size())
    t ← 优先队列的队头  小根堆
        当终点第一次出队时 break;
        从起点到当前点的真实距离 d_real
        从当前点到终点的估计距离 d_estimate
        选择一个估计距离最小的点 min(d_estimate)
    for j in ne[t]:
        将邻边入队

A*算法条件:
估计距离<=真实距离(并且估计函数尽可能的等于真实距离)
d[state] + f[state] = 起点到state的真实距离 + state到终点的估计距离=估计距离
                                                                       ^
d[state] + g[state] = 起点到state的真实距离 + state到终点的真实距离=真实距离

178. 第K短路

179. 八数码

利用 A* 求走迷宫的最短路

#include <iostream>
#include <vector>
#include <queue>
#include <cmath>
#include <bits/stdc++.h>

using namespace std;

struct Node {
    int x, y;
    int g, h;
    int parent_x, parent_y;

    // g:从起点到当前点的真实距离 h:从当前点到终点的估计距离
    Node(int x, int y, int g, int h, int parent_x, int parent_y) : x(x), y(y), g(g), h(h), parent_x(parent_x), parent_y(parent_y) {}

    int f() const {
        return g + h;
    }
};

struct Compare {
    bool operator()(const Node& lhs, const Node& rhs) const {
        return lhs.f() > rhs.f();
    }
};

int manhattan_distance(int x1, int y1, int x2, int y2) {
    return abs(x1 - x2) + abs(y1 - y2);
}

vector<pair<int, int>> reconstruct_path(const vector<vector<Node>>& came_from, int current_x, int current_y) {
    vector<pair<int, int>> path;
    while (current_x != -1 && current_y != -1) {
        path.emplace_back(current_x, current_y);
        int temp_x = came_from[current_x][current_y].parent_x;
        current_y = came_from[current_x][current_y].parent_y;
        current_x = temp_x;
    }
    reverse(path.begin(), path.end());
    return path;
}

vector<pair<int, int>> astar(const vector<vector<int>>& maze, pair<int, int> start, pair<int, int> goal) {
    priority_queue<Node, vector<Node>, Compare> open_set;
    vector<vector<bool>> closed_set(maze.size(), vector<bool>(maze[0].size(), false));
    vector<vector<Node>> came_from(maze.size(), vector<Node>(maze[0].size(), Node(-1, -1, -1, -1, -1, -1)));

    open_set.push(Node(start.first, start.second, 0, manhattan_distance(start.first, start.second, goal.first, goal.second), -1, -1));

    while (!open_set.empty()) {
        Node current = open_set.top();
        open_set.pop();

        if (current.x == goal.first && current.y == goal.second) {
            return reconstruct_path(came_from, current.x, current.y);
        }

        // 设置当前点为已访问点
        closed_set[current.x][current.y] = true;

        // 上下左右均可移动
        for (int dx = -1; dx <= 1; ++dx) {
            for (int dy = -1; dy <= 1; ++dy) {
                if (dx == 0 && dy == 0 || dx == 1 && dy == 1) continue;

                int new_x = current.x + dx;
                int new_y = current.y + dy;


                // 判断如果超出边界，或者位置已经访问过，或者有障碍物，则跳过
                if (new_x < 0 || new_x >= maze.size() || new_y < 0 || new_y >= maze[0].size() || maze[new_x][new_y] == 1 || closed_set[new_x][new_y]) {
                    continue;
                }

                int tentative_g = current.g + 1;
                int h = manhattan_distance(new_x, new_y, goal.first, goal.second);

                // 满足：源点到当前点的距离 + 1 < 源点到当前点父点的距离，则更新该点的距离（此点此时的路径距离更近了，所以更新）
                if (came_from[new_x][new_y].x == -1 || tentative_g < came_from[new_x][new_y].g) {

                    // 设置当前位置 new_x，new_y 来自于那个节点
                    came_from[new_x][new_y] = Node(current.x, current.y, tentative_g, h, current.x, current.y);

                    // 将这个新点入队列
                    open_set.push(Node(new_x, new_y, tentative_g, h, current.x, current.y));
                }
            }
        }
    }

    return {};  // No path found
}

int main() {
    vector<vector<int>> maze = {
            {0, 0, 0, 0, 1},
            {1, 1, 0, 1, 1},
            {0, 0, 0, 0, 0},
            {1, 0, 1, 1, 0},
            {0, 0, 0, 0, 0}
    };
    pair<int, int> start = {0, 0};
    pair<int, int> goal = {4, 4};

    vector<pair<int, int>> path = astar(maze, start, goal);
    if (!path.empty()) {
        cout << "Found path: ";
        int s = path[0].first, e = path[0].second;
        for (int i = 1; i < path.size(); i ++ ) {
            if (path[i].first == path[i - 1].first) { // 左右运动
                if ((path[i].second - path[i - 1].second) == 1) {
                    printf("right 0 "); // 向右
                } else {
                    printf("left 1 "); // 向左
                }
            } else {
                if ((path[i].first - path[i - 1].first) == 1) {
                    printf("down 3 "); // 向上
                } else {
                    printf("up 2 "); // 向下
                }
            }
        }

        printf("\n");

        for (const auto& point : path) {
            cout << "(" << point.first << ", " << point.second << ") ";
        }
        cout << endl;
    } else {
        cout << "No path found" << endl;
    }

    return 0;
}

双向A*求最短路，并返回路径（不保证对）

#include <bits/stdc++.h>
using namespace std;
#define BiggestLength 10000

const int n = 200;
const int robot_num = 10;
const int berth_num = 10;
const int boat_num = 5;
const int N = 210;
typedef pair<int,int> Point;
const int CargotimeLimitMs = 8;
const int FindtimeLimitMs = 14; // 时间限制 xx ms
const int ConflicttimeLimitMs = 15;

int manhattan_distance(int x1, int y1, int x2, int y2) {
    return abs(x1 - x2) + abs(y1 - y2);
}

struct Node {
    int x, y;
    int g, h, f;
//    Node* parent; // 指向父节点的指针
    int parent_x, parent_y;

    // g:从起点到当前点的真实距离 h:从当前点到终点的估计距离
    Node(int x, int y, int g, int h, int f, int parent_x, int parent_y) : x(x), y(y), g(g), h(h), f(f), parent_x(parent_x), parent_y(parent_y) {}

};

struct Compare {
    bool operator()(const Node& lhs, const Node& rhs) const {
        return lhs.f > rhs.f;
    }
};

vector<Point> reconstruct_path(const vector<vector<Node>>& forwardCame_from, const vector<vector<Node>>& backwarCame_from, int current_x, int current_y,int& length) {
    vector<Point> path;


    int T_x, T_Y;
    T_x = current_x;
    T_Y = current_y;

    while (current_x != -1 && current_y != -1) {
        path.emplace_back(current_x, current_y);
        int temp_x = forwardCame_from[current_x][current_y].parent_x;
        current_y = forwardCame_from[current_x][current_y].parent_y;
        current_x = temp_x;
    }
    reverse(path.begin(), path.end());

    current_x = T_x, current_y = T_Y;
    while (current_x != -1 && current_y != -1) {
        if(current_x != T_x && current_y != T_Y) path.emplace_back(current_x, current_y);
        int temp_x = backwarCame_from[current_x][current_y].parent_x;
        current_y = backwarCame_from[current_x][current_y].parent_y;
        current_x = temp_x;
    }
//    reverse(path.begin(), path.end());


    if (!path.empty()) {
        path.erase(path.begin());
        length = path.size();
    }
    else{
        length = BiggestLength;
    }

    return path;
}

vector<Point> astar(const vector<vector<char>>& maze, Point start, Point goal,int& length) {

    // 创建两个 open 列表
    priority_queue<Node, vector<Node>, Compare> forwardOpenList;
    priority_queue<Node, vector<Node>, Compare> backwardOpenList;

    // 初始化起点和终点的节点
    Node Start(start.first, start.second, -1, manhattan_distance(start.first, start.second, goal.first, goal.second), manhattan_distance(start.first, start.second, goal.first, goal.second), -1, -1);
    Node Goal(goal.first, goal.second, -1, manhattan_distance(start.first, start.second, goal.first, goal.second), manhattan_distance(start.first, start.second, goal.first, goal.second), -1, -1);

    // 将起点和终点加入 open 列表
    forwardOpenList.push(Start);
    backwardOpenList.push(Goal);

    // 创建两个 closed 列表
    vector<vector<bool>> forwardClosed(maze.size(), vector<bool>(maze[0].size(), false));
    vector<vector<bool>> backwardClosed(maze.size(), vector<bool>(maze[0].size(), false));

    vector<vector<Node>> forwardCame_from(maze.size(), vector<Node>(maze[0].size(), Node(-1, -1, -1, -1, -1, -1, -1)));
    vector<vector<Node>> backwarCame_from(maze.size(), vector<Node>(maze[0].size(), Node(-1, -1, -1, -1, -1,  -1, -1)));

    int dx[4] = {0, 0, 1, -1};
    int dy[4] = {1, -1, 0, 0};

    // 开始搜索
    while (!forwardOpenList.empty() && !backwardOpenList.empty()) {

        // 从起点开始搜索
        Node currentForward = forwardOpenList.top();
//        cout << currentForward.parent_x << " " << currentForward.parent_x << " " << endl;
        forwardOpenList.pop();
        forwardClosed[currentForward.x][currentForward.y] = true;

        // 检查是否到达终点或与另一边相遇
        if (backwardClosed[currentForward.x][currentForward.y]) {
            reconstruct_path(forwardCame_from, backwarCame_from, currentForward.x, currentForward.y, length);
        }

        // 从终点开始搜索
        Node currentBackward = backwardOpenList.top();
        backwardOpenList.pop();
        backwardClosed[currentBackward.x][currentBackward.y] = true;

        // 检查是否到达终点或与另一边相遇
        if (forwardClosed[currentBackward.x][currentBackward.y]) {
            reconstruct_path(forwardCame_from, backwarCame_from, currentBackward.x, currentBackward.y, length);
        }

        for (int i = 0; i < 4; ++i) {
            int nextForward_x = currentForward.x + dx[i];
            int nextForward_y = currentForward.y + dy[i];

            int nextBackward_x = currentBackward.x + dx[i];
            int nextBackward_y = currentBackward.y + dy[i];

            // 判断是否可以前进，即没有超出边界，障碍物等情况
            if (nextForward_x >= 0 && nextForward_x < maze.size() && nextForward_y >= 0 && nextForward_y < maze[0].size() && maze[nextForward_x][nextForward_y] != '#' && maze[nextForward_x][nextForward_y] != '*' && !forwardClosed[nextForward_x][nextForward_y]) {

                // 计算代价函数值
                int gNewForward = currentForward.g + 1; // 1 表示两个相邻节点之间的距离为 1
                int hNewForward = manhattan_distance(nextForward_x, nextForward_y, Goal.x, Goal.y);
                int fNewForward = gNewForward + hNewForward;

                // 如果相邻节点不在 open 或 closed 列表中，或者有更低的 f 值，则更新它的 f、g、h 值，并设置父节点
//                if (currentForward.x == Start.x || gNewForward < currentForward.g) {

//                    cout << nextForward_x << " " << nextForward_y  << " ---- ";
                    Node* nextNode = new Node(nextForward_x, nextForward_y, gNewForward, hNewForward, fNewForward, currentForward.x, currentForward.y);

                    forwardCame_from[nextForward_x][nextForward_y] = Node(nextForward_x, nextForward_y, gNewForward, hNewForward, fNewForward, currentForward.x, currentForward.y);
//                    cout << currentForward.x << " " << currentForward.y << " ##### ";
                    forwardOpenList.push(*nextNode);
//                }
            }

            // 判断是否可以前进，即没有超出边界，障碍物等情况
            if (nextBackward_x >= 0 && nextBackward_x < maze.size() && nextBackward_y >= 0 && nextBackward_y < maze[0].size() && maze[nextBackward_x][nextBackward_y] != '#' && maze[nextBackward_x][nextBackward_y] != '*' && !backwardClosed[nextBackward_x][nextBackward_y]) {

                // 计算代价函数值
                int gNewBackward = currentBackward.g + 1; // 1 表示两个相邻节点之间的距离为 1
                int hNewBackward = manhattan_distance(nextBackward_x, nextBackward_y, Start.x, Start.y);
                int fNewBackward = gNewBackward + hNewBackward;

//                // 如果相邻节点不在 open 或 closed 列表中，或者有更低的 f 值，则更新它的 f、g、h 值，并设置父节点
//                if (currentBackward.x == Goal.x || gNewForward < currentBackward.g) {
                Node* nextNode = new  Node(nextBackward_x, nextBackward_y, gNewBackward, hNewBackward, fNewBackward, currentBackward.x, currentBackward.y);

                backwarCame_from[nextBackward_x][nextBackward_y] = Node(nextBackward_x, nextBackward_y, gNewBackward, hNewBackward, fNewBackward, currentBackward.x, currentBackward.y);

//                Node* nextNode = new Node(nextBackward_x, nextBackward_y, gNewBackward, hNewBackward, fNewBackward,
//                                         &currentBackward);
//                    nextNode->parent = &currentBackward;
                    backwardOpenList.push(*nextNode);
//                }
            }
        }
    }
    //路径不存在，设为BiggestLength表示无穷大
    length = BiggestLength;
    return {};  // No closestpath found
}


int main() {
    // 创建地图
    // 创建地图
    vector<vector<char>> grid = {
            {'#', '.', '#', '#', '#'},
            {'#', '#', '#', '.', '#'},
            {'.', '.', '#', '#', '#'},
            {'#', '#', '#', '#', '.'},
            {'#', '#', '#', '#', '#'}
    };

    // 定义起点和终点
    int startRow = 0, startCol = 0;
    int goalRow = 4, goalCol = 4;

    int length;

    astar(grid,Point(startRow,startCol),Point(goalRow, goalCol),length);


    return 0;
}

References

A*算法详解(个人认为最详细,最通俗易懂的一个版本)

路径规划之 A* 算法

A*算法的介绍