本篇文章是机器人自动寻路算法实现的第三章。我们要讨论的是一个在一个M×N的格子的房间中，有若干格子里有灰尘，有若干格子里有障碍物，而我们的扫地机器人则是要在不经过障碍物格子的前提下清理掉房间内的灰尘。具体的问题情景请查看人工智能: 自动寻路算法实现(一、广度优先搜索)这篇文章，即我们这个系列的第一篇文章。在前两篇文章里，我们介绍了通过广度优先搜索算法和深度优先算法来实现扫地机器人自动寻路的功能。两种算法都有各自的优点和缺点：对于广度优先搜索算法，程序会找到最优解，但是需要遍历的节点很多。而深度优先搜索则与之相反：遍历的节点很少，但是不一定会找到最优解，而且还有一种极端的情况，就是深度优先搜索在遍历的时候，如果遍历的那个分支是无限大，并且解并不在那个分支中而是在其他的分支中，那么深度优先搜索永远都找不到解。两种算法具体的比较在人工智能: 自动寻路算法实现(二、深度优先搜索)中有详细介绍。在这篇文章中，我们要介绍一种结合了前两种算法优点的算法，A*算法。
A*算法被广泛应用于游戏中的自动寻路功能，说明它作为一个路径规划的算法，确实有着很大的优势。以游戏举例来看，比如在游戏中我们想要找到从一个位置到另一个位置的路径，我们不仅尝试着找到最短距离的路径；我们还想要顾忌到消耗的时间。在一张地图上，穿过一片池塘速度会明显减慢，所以我们想要找到一条可以绕过水路的路径。
本文项目下载地址： https://github.com/tjfy1992/Robot-Path-planning-AStar
正文
算法介绍
首先我们来回顾一下广度优先搜索和深度优先搜索算法。我们从前两篇文章中可以得知，广度优先搜索算法中使用数据结构是队列，而深度优先搜索算法中适用的数据结构是栈。对于一个队列，节点总是先进先出（FIFO），因此对于队列中的第一个节点来说，它的所有直接子节点在队列中都是紧紧跟随在该节点之后，这样程序在运行的时候，对于第一个入队列的节点，就会先遍历完他所有的直接子节点，接着才会去遍历他的每个直接子节点的直接子节点。可以看出遍历的顺序就是一个类似金字塔的形状：第一行有一个头结点，第二行是该头结点的直接子节点，而第三行是直接子节点的直接子节点…每遍历一行都会找出这一行的所有子节点，所以这种算法被称为“广度优先搜索”。
广度优先搜索的节点遍历顺序
而相对地，深度优先搜索就很好理解。对于任何一个节点，都会先去遍历它的第一个子节点的第一个子节点的第一个子节点…后进先出（LIFO）的栈则正好保证了这一点。这种“一条路走到黑”的方式，在它遍历的第一条路径就可能会找到解，但是由于不是横向遍历，路径的长度并不一定是最短，即程序不一定会给出最优解。
深度优先搜索的节点遍历顺序
这两种算法的优缺点都很明显，于是我们需要想出一种能结合两种算法优点的算法。我们可以做出如下处理：对于广度优先搜索算法的队列，如果我们可以想出一种方法，对队列进行排序，把前文中类似“穿过一片沼泽”这样的节点尽量放在最后去遍历，那么我们就可以在相对短的时间内找出一个最优解来。在A*算法中，我们对节点按以下的方式进行排序：
F = G + H
其中，F是我们计算出的权值，F值越大，代表这个节点的收益越小，也就越接近于我们上文提到的“沼泽地”。
G指的是我们从初始节点到达现在的节点的过程中付出的代价。例如我们的机器人每走一格或每清理一个灰尘会耗费1个单位时间，那么机器人做了5个动作之后，我们的G值就是5。
H值是一个相对开放的概念，它指的是从目前状态到目标状态预计要付出的代价。这个值由算法工程师来进行估算，H值被估算的越准确，算法所需要遍历的节点就越少。以我们的扫地机器人举例，假如目前房间内只剩下一个灰尘，而这个灰尘就在机器人的东侧（右侧），那么机器人通过这种算法就可以直接选择先往东走去清理这个灰尘，而不是向其他方向走，避免了“南辕北辙”这种人工智障???的情况出现。
计算出这个值之后，我们就按这个F值在队列中进行排序。本例中源码由Java编写，在Java中有一种数据结构，PriorityQueue，即优先级队列，这种数据结构正好可以用来存放要遍历的节点。
代码
首先是Point类，用于表示坐标系中的点。这个文件与前两个算法中相同。代码如下：
public class Point { private int X; private int Y; public Point(int x, int y){ this.X = x; this.Y = y; } public int getX() { return X; } public void setX(int x) { X = x; } public int getY() { return Y; } public void setY(int y) { Y = y; } public static boolean isSamePoint(Point point1, Point point2){ if(point1.getX() == point2.getX() && point1.getY() == point2.getY()) return true; return false; }}
- 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
接下来是State类。如果把问题的情景（房间、机器人）比作一个系统，那么State类就表示某一时刻系统的状态，也就是我们要遍历的节点。注意这个类里比之前的两个算法多了两个属性，F值和G值。G值就是机器人从其实状态到当前状态所进行的操作次数。F值是G值和H值的和。而H值，在这里并没有列出来，因为我把它视为当前状态下仍未被清理的灰尘数量，也就是灰尘列表的size，通过当前状态的dirtList取size()即可得到，便不再单独设该属性。
import java.util.ArrayList;import java.util.List;public class State { private Point robotLocation; private String operation; private State previousState; private List<Point> dirtList; private int fvalue; private int cost; public Point getRobotLocation() { return robotLocation; } public void setRobotLocation(Point robotLocation) { this.robotLocation = robotLocation; } public String getOperation() { return operation; } public void setOperation(String operation) { this.operation = operation; } public State getPreviousState() { return previousState; } public void setPreviousState(State previousState) { this.previousState = previousState; } public List<Point> getDirtList() { return dirtList; } public void setDirtList(List<Point> dirtList) { this.dirtList = new ArrayList<Point>(); for(Point point : dirtList){ this.dirtList.add(point); } } public int getFvalue() { return fvalue; } public void setFvalue(int fvalue) { this.fvalue = fvalue; } public int getCost() { return cost; } public void setCost(int cost) { this.cost = cost; } public static boolean isSameState(State state1, State state2){ if(!Point.isSamePoint(state1.getRobotLocation(), state2.getRobotLocation())) return false; else if(state1.getDirtList().size() != state2.getDirtList().size()) return false; else{ for(Point point : state1.getDirtList()) { boolean same = false; for(Point point2 : state2.getDirtList()) { if(Point.isSamePoint(point, point2)) same = true; } if(!same) return false; } } return true; }}
- 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
最后是算法的实现类，Robot类。该类使用了一个新的数据结构：PriorityQueue来作为存储节点的open list。PriorityQueue需要我们自定义一个比较器（Comparator）用于在插入元素时将该元素与列表中的元素进行比较，并插入适当的位置，保证PriorityQueue在任何时刻都是有序的。需要注意的一点是我们需要把F值较小的元素排在前面。
import java.util.ArrayList;import java.util.Comparator;import java.util.List;import java.util.PriorityQueue;import java.util.Queue;import java.util.Scanner;public class Robot { public static int colomnNum; public static int rowNum; public static int obstacleNum; public static Queue<State> priorityQueue; public static String[][] map; public static List<Point> dirtList; public static List<State> closeList; public static int cost = 0; public static void main(String[] args) { State initialState = new State(); Scanner sc = new Scanner(System.in); System.out.println("Please Enter Row Number:"); rowNum = sc.nextInt(); System.out.println("Please Enter Colomn Number:"); colomnNum = sc.nextInt(); map = new String[rowNum][colomnNum]; dirtList = new ArrayList<Point>(); closeList = new ArrayList<State>(); sc.nextLine(); for(int i=0; i<rowNum; i++) { System.out.println("Please Enter the Elements in row " + (i + 1) + ":"); String line = sc.nextLine(); for(int j=0; j<colomnNum; j++) { if(line.charAt(j) == '#') { obstacleNum++; } if(line.charAt(j) == '*') { dirtList.add(new Point(i, j)); } if(line.charAt(j) == '@') { initialState.setRobotLocation(new Point(i, j)); } map[i][j] = line.charAt(j) + ""; } } sc.close(); initialState.setDirtList(dirtList); initialState.setCost(0); initialState.setFvalue(0 + dirtList.size()); Comparator<State> cmp = new Comparator<State>() { public int compare(State s1, State s2) { return s1.getFvalue() - s2.getFvalue(); } }; priorityQueue = new PriorityQueue<State>(5, cmp); closeList.add(initialState); priorityQueue.add(initialState); cost++; while(!priorityQueue.isEmpty()){ State state = priorityQueue.poll(); if(isgoal(state)){ output(state); return; } calculate(state); } } public static void calculate(State state){ int x = state.getRobotLocation().getX(); int y = state.getRobotLocation().getY(); if(map[x][y].equals("*") && !isCleared(new Point(x, y), state.getDirtList())){ State newState = new State(); List<Point> newdirtList = new ArrayList<Point>(); for(Point point : state.getDirtList()) { if(point.getX() == x && point.getY() == y) continue; else newdirtList.add(new Point(point.getX(), point.getY())); } newState.setDirtList(newdirtList); newState.setCost(state.getCost() + 1); newState.setFvalue(newState.getCost() + newdirtList.size()); newState.setRobotLocation(new Point(x, y)); newState.setOperation("C"); newState.setPreviousState(state); if(!isDuplicated(newState)){ priorityQueue.add(newState); closeList.add(newState); cost++; } } if(x + 1 < rowNum) { if(!map[x+1][y].equals("#")) { State newState = new State(); newState.setDirtList(state.getDirtList()); newState.setRobotLocation(new Point(x + 1, y)); newState.setOperation("S"); newState.setCost(state.getCost() + 1); newState.setFvalue(newState.getCost() + state.getDirtList().size()); newState.setPreviousState(state); if(!isDuplicated(newState)){ priorityQueue.add(newState); closeList.add(newState); cost++; } } } if(x - 1 >= 0) { if(!map[x-1][y].equals("#")) { State newState = new State(); newState.setDirtList(state.getDirtList()); newState.setRobotLocation(new Point(x - 1, y)); newState.setOperation("N"); newState.setCost(state.getCost() + 1); newState.setFvalue(newState.getCost() + state.getDirtList().size()); newState.setPreviousState(state); if(!isDuplicated(newState)){ priorityQueue.add(newState); closeList.add(newState); cost++; } } } if(y - 1 >= 0) { if(!map[x][y-1].equals("#")) { State newState = new State(); newState.setDirtList(state.getDirtList()); newState.setRobotLocation(new Point(x, y - 1)); newState.setOperation("W"); newState.setCost(state.getCost() + 1); newState.setFvalue(newState.getCost() + state.getDirtList().size()); newState.setPreviousState(state); if(!isDuplicated(newState)){ priorityQueue.add(newState); closeList.add(newState); cost++; } } } if(y + 1 < colomnNum) { if(!map[x][y+1].equals("#")) { State newState = new State(); newState.setDirtList(state.getDirtList()); newState.setRobotLocation(new Point(x, y + 1)); newState.setOperation("E"); newState.setCost(state.getCost() + 1); newState.setFvalue(newState.getCost() + state.getDirtList().size()); newState.setPreviousState(state); if(!isDuplicated(newState)){ priorityQueue.add(newState); closeList.add(newState); cost++; } } } } public static boolean isgoal(State state){ if(state.getDirtList().isEmpty()) return true; return false; } public static void output(State state){ String output = ""; while(state != null){ if(state.getOperation() != null) output = state.getOperation() + "\r\n" + output; state = state.getPreviousState(); } System.out.println(output); System.out.println(cost); } public static boolean isDuplicated(State state){ for(State state2 : closeList){ if(State.isSameState(state, state2)) return true; } return false; } public static boolean isCleared(Point point, List<Point> list){ for(Point p : list){ if(Point.isSamePoint(p, point)) return false; } return true; }}
- 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
测试结果与前两种算法比较
我们使用一个6×6的比较复杂的地图来测试。
地图为
@__*#__**____*__
其中@表示机器人初始位置，*表示灰尘位置，#表示障碍物，_表示空格子。来看结果。
首先是A*算法，注意程序需要运行一段时间才会出结果：
Please Enter Row Number:6Please Enter Colomn Number:6Please Enter the Elements in row 1:@_#*_*Please Enter the Elements in row 2:_*#__*Please Enter the Elements in row 3:#*__#*Please Enter the Elements in row 4:*_#__*Please Enter the Elements in row 5:__#*__Please Enter the Elements in row 6:_*__#*SECSCSWCESSCEENCEESCNNCNCNCNCWWC42824
- 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
可以看到遍历的节点数为42824个。
接下来是广度优先搜索算法，程序同样需要运行一段时间才会出结果：
Please Enter Row Number:6Please Enter Colomn Number:6Please Enter the Elements in row 1:@_#*_*Please Enter the Elements in row 2:_*#__*Please Enter the Elements in row 3:#*__#*Please Enter the Elements in row 4:*_#__*Please Enter the Elements in row 5:__#*__Please Enter the Elements in row 6:_*__#*SECSCSWCSSECEENCEESCNNCNCNCNCWWC56691
- 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
需要遍历的节点是56691个。可以看出以上两种算法都可以得到最优解，而A*算法需要遍历的节点数要比广度优先搜索少14000个左右。
最后是深度优先搜索。
Please Enter Row Number:6Please Enter Colomn Number:6Please Enter the Elements in row 1:@_#*_*Please Enter the Elements in row 2:_*#__*Please Enter the Elements in row 3:#*__#*Please Enter the Elements in row 4:*_#__*Please Enter the Elements in row 5:__#*__Please Enter the Elements in row 6:_*__#*SECSCSSSCNNNEESSCSWWNNNEESESESCNNCSWNWSSWWNNNEENNCSSSSSWWNNWCSSENNNEESSENENCSSWNWSSWWNNNEENNESECSSSWNWSSWWNNNEENNEEC155
- 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
可以看到深度优先搜索的效率特别高，在只遍历了155个节点的情况下得出了解。但是很显然机器人选择的线路比前两种算法长很多。并不是最优解。
结束语
我在文中说过，A*算法中H值的估算的准确度可以影响到算法的效率，即最终遍历到的节点的数量。遍历的节点数量越少，算法的执行速度就越快。本文中仅仅是把H值估算为剩余灰尘的数量，只是为了说明H值的意义，但是这样估算的准确度较低。在实际应用中，我们可以对H值进行更加准确的估算。比如在这个例子中，有一种思路是计算机器人当前位置与所有灰尘格子的距离的方差。但是这可能仍然不是特别准确，具体的思路和实现读者可以自己进行考虑和斟酌。当然这也是A*算法的最大魅力：工程师可以在不同的情况中对算法进行不同的优化。
