A星算法

如何移动到指定位置首先，移动机器人需要有一个地图，同时知道自己现在在哪儿，同时要知道指定位置在地图的坐标，中途哪儿有障碍物。这个问题就是位置环境中的自主定位与建图，也就是SLAM。

引子

我们讨论一个移动机器人遇到问题：如何移动到指定位置

• 首先，移动机器人需要有一个地图，同时知道自己现在在哪儿，同时要知道指定位置在地图的坐标，中途哪儿有障碍物。这个问题就是位置环境中的自主定位与建图，也就是SLAM。

• 其次，我们假设目前是在A点，目标位置是B点。需要求得从A点到B点最短的路径。这个问题就是路径规划。

• 再次，已知地图中最短的路径，我们需要用路径求得实际的运动参数，这里称之为轨迹。这个问题就是路径解析。

• 最后，有了轨迹，根据轨迹的运动参数驱动移动机器人实际运行。这个问题就是运动控制实现。

这里讨论的是路径规划问题。而A*算法是广为使用的解决这个问题的算法。

基本概念

• 启发式搜索：启发式搜索就是在状态空间中的搜索对每一个搜索的位置进行评估，得到最好的位置，再从这个位置进行搜索直到目标。这样可以省略大量无畏的搜索路径，提到了效率。在启发式搜索中，对位置的估价是十分重要的。采用了不同的估价可以有不同的效果。

• 估价函数：从当前节点移动到目标节点的预估费用；这个估计就是启发式的。在寻路问题和迷宫问题中，我们通常用曼哈顿（manhattan）估价函数（下文有介绍）预估费用。

• start：路径规划的起始点，也是机器人当前位置或初始位置A

• goal：路径规划的终点，也是机器人想要到达的位置B

• g_score：当前点到沿着start点A产生的路径到A点的移动耗费

• h_score：不考虑不可通过区域，当前点到goal点B的理论移动耗费

• f_score：g_score+h_score，通常也写为F=G+H

• 开启列表openset：寻路过程中的待检索节点列表

• 关闭列表closeset：不需要再次检索的节点列表

• 追溯表comaFrom：存储父子节点关系的列表，用于追溯生成路径。

算法解析

如图1，绿色点为start设为A，红色点为goal设为B，蓝色点为不可通过的障碍物，黑色点为自由区域。目标是规划从A到B的路径。

图1

开始搜索

• 搜索的从A点开始，首先将A点加入开启列表，此时取开启列表中的最小值，初始阶段开启列表中只有A一个节点，因此将A点从开启列表中取出，将A点加入关闭列表。

• 取出A点的相邻点，将相邻点加入开启列表。如图2所示，此时A点即为相邻点的父节点。图中箭头指向父节点。将相邻点与A点加入追溯表中。

  

  图2

计算耗费评分

对相邻点，一次计算每一点的g_score，h_score，最后得到f_score。如图3，节点的右下角为g_score值，左下角为h_score值，右上角为f_score。

图3

选最小值，再次搜索

• 选出开启列表中的F值最小的节点，将此节点设为当前节点，移出开启列表，同时加入关闭列表。如图4所示。

• 取出当前点的相邻点，当相邻点为关闭点或者墙时，不操作。此外，查看相邻点是否在开启列表中，如不在开启列表中将相邻点加入开启列表。如相邻点已经在开启列表中，则需要进行G值判定

  

  图4

G值判定

• 对于相邻点在开启列表中的，计算相邻点的G值，计算按照当前路径的G值与原开启列表中的G值大小。如果当前路径G值小于原开启列表G值，则相邻点以当前点为父节点，将相邻点与当前点加入追溯表中。同时更新此相邻点的H值。如果当前路径G值大于等于原开启列表G值，则相邻点按照原开启列表中的节点关系，H值不变。因为图示中，当前点G值比原开启列表G值大，因此节点关系按照原父子关系和F值。

计算耗费评分，选最小值

• 此时计算开启列表中F值最小的点，将此节点设为当前节点，并列最小F值的按添加开启列表顺序，以最新添加为佳。

  

  图5

重复搜索判定工作

• 直到当goal点B加入开启列表中，则搜索完成。此时事实上生成的路径并一定是最佳路径，而是最快计算出的路径。若判定标准改为当goal点B加入关闭列表中搜索完成，则得出路径是最佳路径，但此时计算量较前者大。

• 当没有找到goal点，同时开启列表已空，则搜索不到路径。结束搜索。

  

  图6

生成路径

• 由goal点B向上逐级追溯父节点，追溯至起点A，此时各节点组成的路径即使A*算法生成的最优路径。

  

  图7

算法实现

1. 初始化参数

   • 起始点start

   • 终点goal

   • h_score

   • g_socre

   • f_score

   • 开启列表openset

   • 关闭列表closeset

   • 追溯表comeFrom

2. 程序主体

   • 寻找开启列表openset中F值最小的节点，设为当前点current

   • 开启列表openset中移出当前点current

   • 关闭列表openset中加入当前点current

   • 对当前点的每一个相邻点neighbor

   • 当目标点goal在开启列表中，则结束程序，此时有路径生成，此时由goal节点开始逐级追溯上一级父节点，直到追溯到开始节点start，此时各节点即为路径。

   • 当开启列表为空，则结束程序，此时没有路径

   • 如果它不可通过或者已经在关闭列表中，略过。否则：

   • 如果它不在开启列表中，加入开启列表中

   • 如果在开启列表中，G值判定，若此路径G值比之前路径小，则此相邻点的父节点为当前点，同时更新G与F值。反之，则保持原来的节点关系与G、F值。

   • 将起始点start加入开启列表openset

   • 重复一下工作

伪代码

// a* 伪代码
function A*(start, goal)     //初始化关闭列表，已判定过的节点，进关闭列表。    
closedSet := {}     // 初始化开始列表，待判定的节点加入开始列表。     // 初始openset中仅包括start点。    
 openSet := {start}     // 对每一个节点都只有唯一的一个父节点，用cameFrom集合保存节点的子父关系。              //cameFrom（节点）得到父节点。     
cameFrom := the empty map    // gScore估值集合     
gScore := map with default value of Infinity     gScore[start] := 0      // fScore估值集合     
fScore := map with default value of Infinity     
fScore[start] := heuristic_cost_estimate(start, goal)    
while openSet is not empty                     //取出F值最小的节点设为当前点         current := the node in openSet having the lowest fScore[] value                    //当前点为目标点，跳出循环返回路径         
if current = goal             
return reconstruct_path(cameFrom, current)         
openSet.Remove(current)         
closedSet.Add(current)        
for each neighbor of current            
if neighbor in closedSet               
   continue        // 忽略关闭列表中的节点             // tentative_gScore作为新路径的gScore             
   tentative_gScore := gScore[current] + dist_between(current, neighbor)          
  if neighbor not in openSet                    
    openSet.Add(neighbor)            
  else if tentative_gScore >= gScore[neighbor]  //新gScore>=原gScore，则按照原路径               
     continue                     // 否则选择gScore较小的新路径，并更新G值与F值。同时更新节点的父子关系。             
cameFrom[neighbor] := current            
  gScore[neighbor] := tentative_gScore             
 fScore[neighbor] := gScore[neighbor] + heuristic_cost_estimate(neighbor, goal)     return failure    //从caomeFrom中从goal点追溯到start点，取得路径节点。
function reconstruct_path(cameFrom, current)     
total_path := [current]   
    while current in cameFrom.Keys:         current := cameFrom[current]         total_path.append(current)    
    return total_path