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

作者：IT小白 | 2024-05-02 21:28:15

踩

a*算法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列表
}
1
2
3
4
5
6
7
```
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;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158