算法简介

A星搜索算法(A* search algorithm),简称A星算法,是一种启发式搜索算法,用于求解图中两个节点之间的最短路径及其长度。常用于游戏中NPC的移动计算和求地图中两点的实际最短路径。A星算法是Dijkstra算法的升级版,但Dijkstra算法属于盲目搜索,每次都需要访问到所有结点的路径并找出最短的一条路径,然后以该最短路径为基础更新到其他结点的路径,不断重复直到找到所有的最短路径结点。当图中的结点很多时,如果采用Dijkstra算法寻找最短路径,产生的计算量将会非常大,而采用A星算法产生的计算量将会远小于Dijkstra算法。

以到达某个目的地为例,广度优先搜索和Dijkstra算法每次都是把周围的路都走完,再选择最短的一条路走,直到到达目的地。虽然能找到最短的一条路,但需要花很多时间。而深度优先搜索是一条路走到底,走不通再退回来,一条条地试。虽然能到达目的地,但可能不是最短的一条路。而A*算法的搜索过程类似于人的搜索方法,比如我们要到目的地,我们每走一段路就会看看当前位置的方向是否偏离目的地,这样就能够保证我们始终朝着目标方向走,不会走冤枉路,从而少走很多路程。

因此A*算法引入了一个启发函数h(n),通过计算当前节点到目标节点的估算距离来保证搜索的方向不会偏离目标节点。在实际运用中,每条路径的地形可能不同,因此每条路径都有不同的权值,即经过该路径的代价。因此A星算法的公式为:f(n) = g(n) + h(n),g(n)为当前节点到起点的实际代价,h(n)为当前节点到目标节点的估算距离代价,f(n)为经过可能经过该节点的最短路径的估算代价。每次选择下一个最短路径经过的节点时,就从待选节点中选择一个估价f最小的一个节点。

因此,一个好的启发函数h(n)很大程度上决定了A*算法的效率。只要h(n)不大于当前节点到目标节点的实际距离,就一定能够找出最短路径。而且h(n)越小,需要计算的节点数就越多,A星算法的效率就越低。常见的启发式函数有欧几里得距离,曼哈顿距离和切比雪夫距离等,但最常用的是曼哈顿距离。即两个点(x1, x2)和(x2, x3)之间的距离为|x1-x2|+|y1-y2|,就像横着和竖着穿越街区一样。只能如果h(n)为0,A星算法就退化成了Dijkstra算法,只需要计算出当前节点到起点的实际距离即可,此时需要计算的节点数最多。

算法步骤

在上图中,绿色方块为起点,红色方块为终点,蓝色方块为障碍物,有红色圆心的方块为最短路径上的节点,把它们连起来就是一条起点到终点的一条最短路径。A*算法需要两个列表集合:open表和close表。open表用于存放待处理的节点,close表用于存放已经处理过的节点。此外,还需要为每个节点指定一个父节点,用于找到终点时进行回溯得到起点到终点的最短路径。

以上图为例,设往上下左右横着竖着走的代价为10,往右上右下左下左上四个角斜着走的代价为14,因为正方形中斜边长为直角边长的根号2倍约为1.414,这里为了计算方便去1.4倍。每一个正方形左下方的值为代价G,即该节点到起点的实际代价;右下方的值为估价H,即通过曼哈顿距离计算的每个节点到终点的估算代价H;左上方的值为可能经过该节点的最短路径的估价F。

下面我们开始手动模拟下A*算法的搜索过程,首先从起点开始,把起点加入open表,同时检测起点周围的8个相邻节点。每次节点检测分三种情况:

1、如果相邻节点是不可到达的,比如障碍物,则忽略该节点。

2、如果相邻节点可到达,但已经在open表中,就计算当前节点的实际代价G加上到这个节点的实际代价,是否小于该相邻节点的实际代价G。如果是就说明找到了一条更优的路径,因为节点代价H不边,代价G越小代价F就越小。于是就更新该相邻节点的代价G和代价F并重新参与代价F的排序,并更改它的父节点为当前节点。如果不是,则不做任何操作。

3、如果相邻节点可到达,且不在open表和close表中,则该相邻节点为新结点,把当前节点设置为该相邻节点的父节点,并把该相邻节点加入到open表中。

从上图可以看出,起点的周围8个相邻节点都属于第三种情况,因此把这8个结点都加入到open表中。这样一轮节点扩展就完成了,把起点移除open表同时把起点加入到close表中,即起点不再检测和参与代价f的排序。接着从这8个待检测结点选择一个代价F最小的节点,即起点右边代价f为40的节点,然后以该节点为当前节点进行下一轮节点扩展。

现在发现代价f为40的当前节点周围有三个是障碍物,忽略它们。剩下的相邻节点都已经在open表中,分别计算当前节点的实际代价G加上到这些相邻节点的实际代价,是否小于这些相邻节点的实际代价G。经过计算发现明显不是,因为两点之间线段最短,斜着走肯定比走直角边的代价更小,因此不做任何操作。

现在把当前节点移除open表并加入到close表,接着重现遍历open表找到一个代价f最小的节点,这时又两个代价f为54的节点。我们优先选择代价h最小的一个即离终点最近的一个节点,如果代价h也一样,就选择新发现的节点。新发现和后发现的不重要,最后的最短路径长度是一样的,只是路径节点不一样,所以A*算法可能会找到几条最短路径。然后以代价f为54的节点为当前节点进行下一轮节点扩展,不断重复上述过程,直到找到终点为止。

代码实现

  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
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
using System;
using System.Collections.Generic;

namespace AStar
{
    // 每个节点的坐标
    public class Location
    {
        private int x;
        private int y;

        public int X { get => x; set => x = value; }
        public int Y { get => y; set => y = value; }

        public Location(int x, int y)
        {
            this.x = x;
            this.y = y;
        }
    }

    // 节点的定义
    public class Node
    {
        // 节点坐标
        private Location location;
        // 节点到起点的实际代价
        private int costG;
        // 节点到终点的估算代价
        private int costH;
        // 经过该结点的最短路径的估算代价
        private int costF;
        // 节点的父结点,默认值为null表示没有指定父节点。
        private Node father;
        // 节点是否已被访问,默认值为false表示没有被访问。
        private bool isVisted;

        public Location Location { get => location; set => location = value; }
        public int CostG { get => costG; set => costG = value; }
        public int CostH { get => costH; set => costH = value; }
        public int CostF { get => costF; set => costF = value; }
        public Node Father { get => father; set => father = value; }
        public bool IsVisted { get => isVisted; set => isVisted = value; }

        // 初始化节点
        public Node(Location location, int costG, int costH)
        {
            this.location = location;
            this.costG = costG;
            this.costH = costH;
            this.costF = costG + costH;
        }

        // 节点向周围八个方向走一步的代价
        public int StepCost(Location location)
        {
            int dx = location.X - this.location.X;
            int dy = location.Y - this.location.Y;
            switch (dx, dy)
            {
                // 往上走
                case (0, 1):
                // 往下走
                case (0, -1):
                // 往左走
                case (-1, 0):
                // 往右走
                case (1, 0):
                    {
                        // 往上下左右走每一步代价为10
                        return 10;
                    }
                // 往右上方走
                case (1, 1):
                // 往右下方走
                case (1, -1):
                // 往左下方走
                case (-1, -1):
                // 往左上方走
                case (-1, 1):
                    {
                        // 往四个方向斜着走每一步代价为14,对于一个正方形节点斜边约为直角边的1.4倍。
                        return 14;
                    }
                // 往其他方向走代价默认为0
                default:
                    return 0;
            }
        }
    }

    // 八个方向,以当前节点为基准,上北下南左西右东
    public enum Direction
    {
        North,
        NorthEast,
        East,
        SouthEast,
        South,
        SouthWest,
        West,
        NorthWest
    }

    // 定义当前地图
    public class Map
    {
        // 地图长度
        private int length;
        // 地图宽度
        private int width;
        // 障碍矩阵,默认为fasle表示没有障碍。
        private bool[,] obstacles;

        public int Length { get => length; set => length = value; }
        public int Width { get => width; set => width = value; }
        public bool[,] Obstacles { get => obstacles; set => obstacles = value; }

        // 地图构造函数
        public Map(int length, int width, List<Location> obstacleList)
        {
            this.Length = length;
            this.Width = width;
            this.Obstacles = new bool[length, width];
            // 初始化地图中的障碍结点
            InitObstacles(obstacleList);
        }

        // 设置障碍函数
        private void InitObstacles(List<Location> obstacleList)
        {
            foreach (Location o in obstacleList)
            {
                Obstacles[o.X, o.Y] = true;
            }
        }

        // 以当前节点为基准,判断某个方向上位置的相邻结点能否到达
        public bool isWalkable(Node currentNode, Location nextLocation)
        {
            // 如果相邻结点在地图之外,则不可到达
            if (nextLocation.X < 0 || nextLocation.X > Length - 1 || nextLocation.Y < 0 || nextLocation.Y > Width - 1)
            {
                return false;
            }

            // 如果相邻结点是障碍,则不可到达
            if (Obstacles[nextLocation.X, nextLocation.Y])
            {
                return false;
            }

            // 如果当前节点右边是一个障碍,且相邻结点在障碍的上方或下方,则该节点不可直接穿墙角到该相邻结点。
            if (currentNode.Location.X + 1 >= 0 && currentNode.Location.X + 1 <= Length - 1 && Obstacles[currentNode.Location.X + 1, currentNode.Location.Y] && currentNode.Location.X + 1 == nextLocation.X)
            {
                return false;
            }

            // 如果当前节点左边是一个障碍,且相邻结点在障碍的上方或下方,则该节点不可直接穿墙角到该相邻结点。
            if (currentNode.Location.X - 1 >= 0 && currentNode.Location.X - 1 <= Length - 1 && Obstacles[currentNode.Location.X - 1, currentNode.Location.Y] && currentNode.Location.X - 1 == nextLocation.X)
            {
                return false;
            }

            // 如果当前节点上方是一个障碍,且相邻结点在障碍的左边或右边,则该节点不可直接穿墙角到该相邻结点。
            if (currentNode.Location.Y + 1 >= 0 && currentNode.Location.Y + 1 <= Width - 1 && Obstacles[currentNode.Location.X, currentNode.Location.Y + 1] && currentNode.Location.Y + 1 == nextLocation.Y)
            {
                return false;
            }

            // 如果当前节点下方是一个障碍,且相邻结点在障碍的左边或右边,则该节点不可直接穿墙角到该相邻结点。
            if (currentNode.Location.Y - 1 >= 0 && currentNode.Location.Y - 1 <= Width - 1 && Obstacles[currentNode.Location.X, currentNode.Location.Y - 1] && currentNode.Location.Y - 1 == nextLocation.Y)
            {
                return false;
            }

            return true;
        }
    }

    // A*算法类
    public class AStar
    {
        private Map map;
        // 起始节点
        private Location source;
        private Node startNode;
        // 终点节点
        private Location destination;
        private Node endNode;
        // 最小代价F节点
        private Node minCostFNode;
        // 存放Direction类型的八个方向。
        private List<Direction> directionList;
        // open表用于存放待处理的节点
        private List<Node> openList;
        // close表用于存放已经处理过的节点
        private List<Node> closeList;
        // 最短路径
        private List<Node> route;

        public List<Node> OpenList { get => openList; set => openList = value; }
        public List<Node> CloseList { get => closeList; set => closeList = value; }
        public Node StartNode { get => startNode; set => startNode = value; }

        // A*类构造函数,参数为:地图,起点坐标,终点坐标
        public AStar(Map map, Location source, Location destination)
        {
            this.map = map;
            this.source = source;
            this.destination = destination;
            this.startNode = new Node(source, 0, (Math.Abs(source.X - destination.X) + Math.Abs(source.Y - destination.Y)) * 10);
            // 终点的代价G默认为无穷大
            this.endNode = new Node(destination, int.MaxValue, 0);
            this.openList = new List<Node>();
            // 将起点加入open表等待处理
            this.closeList = new List<Node>();
            openList.Add(startNode);
            this.route = new List<Node>();
            this.directionList = new List<Direction> { Direction.North, Direction.NorthEast, Direction.East, Direction.SouthEast, Direction.South, Direction.SouthWest, Direction.West, Direction.NorthWest };
        }

        // A*算法具体的递归执行函数
        public List<Node> DoAStar(Node currentNode, List<Node> openList, List<Node> closeList)
        {
            // 每次寻找相邻节点时,先把open表中所有节点的IsVisted标志位置为true,表示它们已经被访问过。
            foreach (Node node in openList)
            {
                node.IsVisted = true;
            }

            // 开始遍历当前节点周围的八个方向寻找相邻节点
            foreach (Direction direction in directionList)
            {
                // 获取某个方向上的相邻节点
                Node nextNode = GetAdjacentLocation(currentNode, direction);
                // 判断该节点位置是否能够到达,不能到达直接跳过本次循环,继续寻找下一个方向
                if (!map.isWalkable(currentNode, nextNode.Location))
                {
                    continue;
                }

                // 如果相邻节点的估价H为0,表示该相邻节点是终点
                if (nextNode.CostH == 0)
                {
                    // 把终点的父节点指向当前节点
                    nextNode.Father = currentNode;
                    // 沿着终点的父节点回溯得到最短路径
                    while (nextNode != null)
                    {
                        // 由于是回溯,因此需要依次将节点插入到route的第一个位置
                        route.Insert(0, nextNode);
                        nextNode = nextNode.Father;
                    }
                    // 返回最短路径,A*算法结束
                    return route;
                }

                // 如果相邻节点已经在open表,即已经被访问过,就判断当前节点的代价G加上到该相邻节点的代价G,是否小于该相邻节点之前的代价G。
                // 如果是,就更新该相邻节点的代价G和代价F,已经更改它的父节点为当前节点。如果不是,则不做任何操作。
                if (isOpened(nextNode.Location) && nextNode.Father != null && nextNode.CostG < nextNode.Father.CostG + nextNode.Father.StepCost(nextNode.Location))
                {
                    // 如果是就说明找到了一条经过该相邻节点的更短路径,把该相邻节点的父节点改为当前节点。因为该相邻节点的代价H是不会变,代价G变小了,因此代价F也就更小了。
                    nextNode.Father = currentNode;
                }

                // 如果相邻节点不在open表和close表中,就说明该相邻节点是一个新节点,把它的父节点指向当前节点并加入到open表中。
                if (!isOpened(nextNode.Location) && !isClosed(nextNode.Location))
                {
                    nextNode.Father = currentNode;
                    // 每一轮代价F排序先从新加入的节点开始,这里的nextNode只是一个“哨兵”,具体值不重要。
                    minCostFNode = nextNode;
                    openList.Add(nextNode);
                }
            }

            // 当周围八个方向上的相邻结点都已经处理过了,就把该当前节点移除open表加入到close表中,表示该节点不再处理及参与代价F的排序。
            openList.Remove(currentNode);
            closeList.Add(currentNode);

            // 从open表中获取一个最小代价F节点,作为最短路径上的下一个节点。
            minCostFNode = GetMinCostFNode();
            // 如果最小代价F节点为空,即open表为空,A*算法搜索失败,没有找到最短路径。
            if (minCostFNode == null)
            {
                return null;
            }
            // 把最小代价F节点的父节点指向当前节点,确保最短路径上的结点不“断链”。
            minCostFNode.Father = currentNode;

            // 把最小代价F节点作为下一轮递归执行的开始结点
            return DoAStar(minCostFNode, openList, closeList);
        }

        // 显示最短路径
        public void ShowRoute()
        {
            bool flag = false;
            for (int i = 0; i < map.Width; i++)
            {
                for (int j = 0; j < map.Length; j++)
                {
                    // 显示障碍物
                    if (map.Obstacles[j, i])
                    {
                        Console.Write("■ ");
                    }
                    // 显示起点和终点
                    else if ((j == source.X && i == source.Y) || (j == destination.X && i == destination.Y))
                    {
                        Console.Write("◆ ");
                    }
                    else
                    {
                        flag = false;
                        foreach (Node node in route)
                        {
                            // 显示最短路径上的节点
                            if (j == node.Location.X && i == node.Location.Y)
                            {
                                flag = true;
                                Console.Write("▲ ");
                            }
                        }
                        // 显示其他可到达的节点
                        if (flag == false)
                        {
                            Console.Write("□ ");
                        }
                    }
                }

                Console.WriteLine();
            }
        }

        // 获取最小代价F节点
        public Node GetMinCostFNode()
        {
            // 如果open表为空,则最小代价F节点为空
            if (openList.Count == 0)
            {
                return null;
            }

            // 如果open表中的所有节点都已经被访问过,则重新遍历open表寻找最小代价F节点。
            if (isAllVisited())
            {
                minCostFNode = openList[0];
                foreach (Node node in openList)
                {
                    if (node.CostF < minCostFNode.CostF)
                    {
                        minCostFNode = node;
                    }
                }
            }

            // 如果有新加入的节点和代价F更新的节点,就从这些节点中找最小代价节点,基准元素就是刚才的“哨兵”。
            foreach (Node node in openList)
            {
                if (node.CostF < minCostFNode.CostF && !node.IsVisted)
                {
                    minCostFNode = node;
                }
            }

            return minCostFNode;
        }

        // 判断open表中的所有节点是否已经被访问过
        public bool isAllVisited()
        {
            foreach (Node node in openList)
            {
                if (!node.IsVisted)
                {
                    return false;
                }
            }

            return true;
        }

        // 判断相邻节点是否已经在open表中
        public bool isOpened(Location location)
        {
            foreach (Node node in openList)
            {
                if (node.Location.X == location.X && node.Location.Y == location.Y)
                {
                    return true;
                }
            }

            return false;
        }

        // 判断相邻节点是否已经在close表中
        public bool isClosed(Location location)
        {
            foreach (Node node in closeList)
            {
                if (node.Location.X == location.X && node.Location.Y == location.Y)
                {
                    return true;
                }
            }

            return false;
        }

        // 获取当前节点某个位置的相邻节点
        public Node GetNode(Node currentNode, Location location)
        {
            // 该相邻节点的代价G为当前节点的代价G加上到该相邻节点位置上这一步的代价
            int costG = currentNode.CostG + currentNode.StepCost(location);
            // 采用曼哈顿距离估算该相邻节点到终点的代价H
            int costH = (Math.Abs(location.X - endNode.Location.X) + Math.Abs(location.Y - endNode.Location.Y)) * 10;
            return new Node(location, costG, costH);
        }

        // 获取当前节点某个方向上的相邻节点
        public Node GetAdjacentLocation(Node currentNode, Direction direction)
        {
            switch (direction)
            {
                case Direction.North:
                    {
                        Location locationNorth = new Location(currentNode.Location.X, currentNode.Location.Y + 1);
                        return GetNode(currentNode, locationNorth);
                    }
                case Direction.NorthEast:
                    {
                        Location locationNorthEast = new Location(currentNode.Location.X + 1, currentNode.Location.Y + 1);
                        return GetNode(currentNode, locationNorthEast);
                    }
                case Direction.East:
                    {
                        Location locationEast = new Location(currentNode.Location.X + 1, currentNode.Location.Y);
                        return GetNode(currentNode, locationEast);
                    }
                case Direction.SouthEast:
                    {
                        Location locationSouthEast = new Location(currentNode.Location.X + 1, currentNode.Location.Y - 1);
                        return GetNode(currentNode, locationSouthEast);
                    }
                case Direction.South:
                    {
                        Location locationSouth = new Location(currentNode.Location.X, currentNode.Location.Y - 1);
                        return GetNode(currentNode, locationSouth);
                    }
                case Direction.SouthWest:
                    {
                        Location locationSouthWest = new Location(currentNode.Location.X - 1, currentNode.Location.Y - 1);
                        return GetNode(currentNode, locationSouthWest);
                    }
                case Direction.West:
                    {
                        Location locationWest = new Location(currentNode.Location.X - 1, currentNode.Location.Y);
                        return GetNode(currentNode, locationWest);
                    }
                case Direction.NorthWest:
                    {
                        Location locationNorthWest = new Location(currentNode.Location.X - 1, currentNode.Location.Y + 1);
                        return GetNode(currentNode, locationNorthWest);
                    }
                default:
                    {
                        return currentNode;
                    }
            }
        }
    }

    class Client
    {
        public static void Main(string[] args)
        {
            // 设置障碍
            List<Location> obstacleList = new List<Location>() { new Location(4, 1), new Location(4, 2), new Location(4, 3) };
            // 创建地图
            Map map = new Map(8, 6, obstacleList);
            // 指定地图,起点坐标,终点坐标,开始初始化A*算法对象。
            AStar aStar = new AStar(map, new Location(2, 2), new Location(6, 2));
            // 指定开始节点,open表,close表,开始执行A*算法,并返回最短路径。
            aStar.DoAStar(aStar.StartNode, aStar.OpenList, aStar.CloseList);
            //List<Node> route = aStar.DoAStar(aStar.StartNode, aStar.OpenList, aStar.CloseList);
            //// 输出最短路径节点
            //foreach (Node node in route)
            //{
            //    Console.WriteLine("({0},{1}):{2} ", node.Location.X, node.Location.Y, node.CostG);
            //}
            Console.WriteLine("显示最短路径: ");
            aStar.ShowRoute();
        }
    }
}

// 输出结果:
// 显示最短路径(三角形节点连起来就是最短路径):
// □ □ □ □ □ □ □ □ 
// □ □ □ □ ■ □ □ □ 
// □ □ ◆ ▲ ■ □ ◆ □ 
// □ □ □ ▲ ■ □ ▲ □ 
// □ □ □ ▲ ▲ ▲ □ □ 
// □ □ □ □ □ □ □ □ 

参考资料:A星算法详解(个人认为最详细,最通俗易懂的一个版本)