笔试算法题（48）：简介 - A*搜索算法（A Star Search Algorithm）

 1 function A*(start,goal)
 2      closedset := the empty set
 3                 // closedset存储已经访问过的节点
 4      openset := {start}
 5                 //openset存储已经探测到但还未访问的节点，初始化时放入start点
 6      came_from := the empty map
 7                 //came_from存储最短路径
 8  
 9      g_score[start] := 0
10                 //g_score[u]存储当前当u点到start点的最小代价
11      h_score[start] := heuristic_cost_estimate(start, goal)
12                 //h_score[u]存储当前u点到goal点的最小代价
13      f_score[start] := g_score[start] + h_score[start]
14                 //f_score[u]存储当前u点在整个状态空间中的代价
15 
16      while openset is not empty
17          current := the node in openset having the lowest f_score[] value
18                 //从当前openset中选取具有最小f_score的节点current
19          if current = goal
20              return reconstruct_path(came_from, came_from[goal])
21                 //判定如果是goal点，则结束
22          remove current from openset
23          add current to closedset
24                 //判定如果不是goal点，则将current点从openset中移到closedset
25          for each neighbor in neighbor_nodes(current)
26                 //遍历current直接相连的节点neighbor
27                 //如果neighbor已经处理过一次（在closedset中），则跳过
28              if neighbor in closedset
29                  continue
30              tentative_g_score := g_score[current] + dist_between(current,neighbor)
31                 //利用当前current点的g_score重新计算neighbor的g_score
32              if neighbor not in openset
33                 //如果neighbor是新探测到的节点（没有在openset中），创建新节点
34                  add neighbor to openset
35                  h_score[neighbor] := heuristic_cost_estimate(neighbor, goal)
36                  tentative_is_better := true
37              else if tentative_g_score < g_score[neighbor]
38                 //如果neighbor已经探测到，并且当前g_socre[current]有更小的代价
39                  tentative_is_better := true
40              else
41                  tentative_is_better := false
42                  //更新neighbor的代价
43              if tentative_is_better = true
44                  came_from[neighbor] := current
45                  g_score[neighbor] := tentative_g_score
46                  f_score[neighbor] := g_score[neighbor] + h_score[neighbor]
47  
48      return failure
49  
50  function reconstruct_path(came_from, current_node)
51      if came_from[current_node] is set
52          p := reconstruct_path(came_from, came_from[current_node])
53          return (p + current_node)
54      else
55          return current_node