A*算法(A-star Algorithm)搜索最短路径（含C/C++语言实现代码）

基本介绍

在我们的日常生活中由许多方面都会涉及到 “最短路径” 的搜索问题，比如ROS机器人中根据给定地图进行全局路径规划，或者游戏中NPC的移动计算，线上游戏的的BOT计算等。A*算法作为一种较为高效的算法经常被应用在以上环境中。
在这里插入图片描述

基本原理

A*算法实现的基本原理是将地图虚拟化并划分成小方块（单元格）以便使用二维数组进行保存，然后搜索当前点周围的点，并从中选择一个新的点作为当前点继续搜索，直至搜索至终点。

有关定义和变量的介绍

实际代价G：表示从起点出发移动到地图上当前单元格的移动耗费。例如，我们可以采用从起点开始，经过多少次上下左右移动才移动到指定点作为实际代价G。
预估代价H：表示从当前单元格移动到终点的预估耗费。在实际编程中，我们通常采用曼哈顿距离(Manhattan Distance)作为预估代价H，当然也可以采用欧几里得距离(Euclidean Distance)作为预估代价。
路径总代价F： $F = G + H$ ，表示该单元格点的总耗费。
open列表：记录左右被用来考虑寻找最短路径的单元格，通常采用优先队列(priority queue)数据结构。
close列表：记录已经被淘汰的单元格。一般为与地图对应的布尔二维数组(bool)
父亲点列表pre：记录open列表中元素间的逻辑联系；
单元格代价列表valueF：记录每一个单元格的最小总代价F。

具体搜索过程

首先需要创建一张地图，可以有障碍物但是必须进行标记。
设置路径的起点和终点。
开始搜索路径：
1. 初始化open列表，close列表，pre列表和valueF列表；
2. 将起点加入open列表，然后将其周围四个点（或八个点，由需求决定）加入到open列表。将起点从open列表中移除并移动到close列表；
3. 依次判断周围这四个点是否在close列表中且是否越界，如果不在，以此计算周围点的G，H并更新F，如果对应单元格在valueF中的值为初始化值或较大，那么更新单元格对应valueF值，记录pre值。伪代码如下：
```
if (node_next.F < valueF[node_next.x][node_next.y] || valueF[node_next.x][node_next.y] == 0)
{
    // 保存该节点的父节点
    pre[node_next] = node_current;   //将父亲点添加到pre，建立逻辑联系
    valueF[node_next] = node_next.F; // 修改该节点对应的valF值
    open.add(node_next);             //当前点添加到open列表
}
```
4. 从周围的点中找出F最小的点，获得周围点的集合，然后将这个F最小的点从open列表中移除并移动到close集合中；
5. 跳转第 3 步。
  算法举例说明" />

结束条件

终点单元格被加入open列表并且被作为当前格查询时；
open列表被清空，表示不可能到达终点。

与Dijkstra算法的比较

Dijkstra算法和A*都是最短路径问题的常用算法，下面就对这两种算法的特点进行一下比较。

Dijkstra算法计算源点到其他所有点的最短路径长度，A*关注点到点的最短路径(包括具体路径)。
Dijkstra算法建立在较为抽象的图论层面，A*算法可以更轻松地用在诸如游戏地图寻路中。
Dijkstra算法的实质是广度优先搜索，是一种发散式的搜索，所以空间复杂度和时间复杂度都比较高。对路径上的当前点，A*算法不但记录其到源点的代价，还计算当前点到目标点的期望代价，是一种启发式算法，也可以认为是一种深度优先的算法。
由第一点，当目标点很多时，A*算法会带入大量重复数据和复杂的估价函数，所以如果不要求获得具体路径而只比较路径长度时，Dijkstra算法会成为更好的选择。

参考文章：Dijkstra算法和A*算法的比较

实现代码

#include <algorithm>
#include <cmath>
#include <iostream>
#include <queue>
#include <string>
#include <vector>
#define N 10 // 地图的阶数
using namespace std;

typedef struct NODE
{
    int x, y;    // 节点所在位置
    int F, G, H; // G:从起点开始，沿着产的路径，移动到网格上指定方格的移动耗费。
        // H:从网格上那个方格移动到终点B的预估移动耗费，使用曼哈顿距离。
        // F = G + H
    NODE(int a, int b) { x = a, y = b; }
    // 重载操作符，使优先队列以F值大小为标准维持堆
    bool operator<(const NODE &a) const
    {
        return F == a.F ? G > a.G : F > a.F;
    }
} Node;

// 定义方向
//const int next_position[8][2] = {{-1, -1}, {-1, 0}, {-1, 1}, {0, -1}, {0, 1}, {1, -1}, {1, 0}, {1, 1}};
const int next_position[4][2] = {{-1, 0}, {0, -1}, {0, 1}, {1, 0}};
priority_queue<Node> open; // 优先队列，就相当于open表
// 棋盘
int map[N][N] = {{0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
                 {0, 1, 0, 0, 0, 0, 0, 1, 0, 0},
                 {0, 0, 1, 1, 0, 0, 0, 1, 0, 0},
                 {0, 0, 0, 1, 0, 0, 0, 1, 0, 0},
                 {0, 0, 0, 0, 0, 0, 1, 0, 0, 0},
                 {0, 0, 1, 1, 0, 1, 0, 0, 0, 0},
                 {0, 0, 1, 0, 1, 0, 1, 0, 0, 0},
                 {0, 0, 1, 0, 0, 0, 0, 1, 0, 0},
                 {0, 1, 0, 1, 0, 0, 0, 0, 0, 0},
                 {0, 0, 0, 0, 0, 0, 0, 0, 0, 0}};
bool close[N][N]; // 访问情况记录，close列表
int valueF[N][N]; // 记录每个节点对应的F值
int pre[N][N][2]; // 存储每个节点的父节点

int Manhattan(int x, int y, int x1, int y1)
{
    return (abs(x - x1) + abs(y - y1)) * 10;
}

bool isValidNode(int x, int y, int xx, int yy)
{
    if (x < 0 || x >= N || y < 0 || y >= N)
        return false; // 判断边界
    if (map[x][y] == 1)
        return false; // 判断障碍物
    // 两节点成对角型且它们的公共相邻节点存在障碍物，在8方向时用
    if (x != xx && y != yy && (map[x][yy] == 1 || map[xx][y] == 1))
        return false;
    return true;
}

void Astar(int x0, int y0, int x1, int y1)
{
    // 起点加入open列表
    Node node(x0, y0);
    node.G = 0;
    node.H = Manhattan(x0, y0, x1, y1);
    node.F = node.G + node.H;
    valueF[x0][y0] = node.F;
    open.push(node);

    while (!open.empty())
    {
        Node node_current = open.top();                   //取优先队列头元素，即周围单元格中代价最小的点
        open.pop();                                       //从open列表中移除
        close[node_current.x][node_current.y] = true;     // 访问该点，加入close列表
        if (node_current.x == x1 && node_current.y == y1) // 到达终点
            break;

        // 遍历node_top周围的4个位置，如果是next_position有8，那么就需要遍历周围8个点
        for (int i = 0; i < 4; i++)
        {
            Node node_next(node_current.x + next_position[i][0], node_current.y + next_position[i][1]); // 创建一个node_top周围的点
            // 该节点坐标合法 且没有被访问
            if (isValidNode(node_next.x, node_next.y, node_current.x, node_current.y) && !close[node_next.x][node_next.y])
            {
                // 计算从起点并经过node_top节点到达该节点所花费的代价
                node_next.G = node_current.G + int(sqrt(pow(next_position[i][0], 2) + pow(next_position[i][1], 2)) * 10);
                // 计算该节点到终点的曼哈顿距离
                node_next.H = Manhattan(node_next.x, node_next.y, x1, y1);
                // 从起点经过node_top和该节点到达终点的估计代价
                node_next.F = node_next.G + node_next.H;

                // node_next.F < valueF[node_next.x][node_next.y] 说明找到了更优的路径，进行更新
                // valueF[node_next.x][node_next.y] == 0 说明该节点还未加入open表中，则加入
                if (node_next.F < valueF[node_next.x][node_next.y] || valueF[node_next.x][node_next.y] == 0)
                {
                    // 保存该节点的父节点
                    pre[node_next.x][node_next.y][0] = node_current.x;
                    pre[node_next.x][node_next.y][1] = node_current.y;
                    valueF[node_next.x][node_next.y] = node_next.F; // 修改该节点对应的valueF值
                    open.push(node_next);
                }
            }
        }
    }
}

void PrintPath(int x1, int y1)
{
    if (pre[x1][y1][0] == -1 || pre[x1][y1][1] == -1)
    {
        cout << "no path to get" << endl;
        return;
    }
    int x = x1, y = y1;
    int a, b;
    while (x != -1 || y != -1)
    {
        map[x][y] = 2; // 将可行路径上的节点赋值为2
        a = pre[x][y][0];
        b = pre[x][y][1];
        x = a;
        y = b;
    }
    // ' '表示未经过的节点， '#'表示障碍物， '@'表示可行节点
    string s[3] = {"  ", " #", " @"};
    for (int i = 0; i < N; i++)
    {
        for (int j = 0; j < N; j++)
            cout << s[map[i][j]];
        cout << endl;
    }
}

int main(int argc, char *argv[])
{
    fill(close[0], close[0] + N * N, false);    // 将visit数组赋初值false
    fill(valueF[0], valueF[0] + N * N, 0);      // 初始化F全为0
    fill(pre[0][0], pre[0][0] + N * N * 2, -1); // 路径同样赋初值-1

    //  // 起点 // 终点
    int x0 = 2, y0 = 4, x1 = 8, y1 = 6;

    // printf("input start: ");
    // scanf("%d%d", &x0, &y0);
    // printf("iinput destination: ");
    // scanf("%d%d", &x1, &y1);

    if (!isValidNode(x0, y0, x0, y0))
    {
        printf("Invalid input.\n");
        return 0;
    }

    Astar(x0, y0, x1, y1); // A*算法
    PrintPath(x1, y1);     // 打印路径

	return 0;
}