引子
我们讨论一个移动机器人遇到问题:如何移动到指定位置
- 首先,移动机器人需要有一个地图,同时知道自己现在在哪儿,同时要知道指定位置在地图的坐标,中途哪儿有障碍物。这个问题就是位置环境中的自主定位与建图,也就是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的路径。
开始搜索
- 搜索的从A点开始,首先将A点加入开启列表,此时取开启列表中的最小值,初始阶段开启列表中只有A一个节点,因此将A点从开启列表中取出,将A点加入关闭列表。
-
取出A点的相邻点,将相邻点加入开启列表。如图2所示,此时A点即为相邻点的父节点。图中箭头指向父节点。将相邻点与A点加入追溯表中。
计算耗费评分
对相邻点,一次计算每一点的g_score,h_score,最后得到f_score。如图3,节点的右下角为g_score值,左下角为h_score值,右上角为f_score。
选最小值,再次搜索
- 选出开启列表中的F值最小的节点,将此节点设为当前节点,移出开启列表,同时加入关闭列表。如图4所示。
-
取出当前点的相邻点,当相邻点为关闭点或者墙时,不操作。此外,查看相邻点是否在开启列表中,如不在开启列表中将相邻点加入开启列表。如相邻点已经在开启列表中,则需要进行G值判定
G值判定
- 对于相邻点在开启列表中的,计算相邻点的G值,计算按照当前路径的G值与原开启列表中的G值大小。如果当前路径G值小于原开启列表G值,则相邻点以当前点为父节点,将相邻点与当前点加入追溯表中。同时更新此相邻点的H值。如果当前路径G值大于等于原开启列表G值,则相邻点按照原开启列表中的节点关系,H值不变。因为图示中,当前点G值比原开启列表G值大,因此节点关系按照原父子关系和F值。
计算耗费评分,选最小值
-
此时计算开启列表中F值最小的点,将此节点设为当前节点,并列最小F值的按添加开启列表顺序,以最新添加为佳。
重复搜索判定工作
- 直到当goal点B加入开启列表中,则搜索完成。此时事实上生成的路径并一定是最佳路径,而是最快计算出的路径。若判定标准改为当goal点B加入关闭列表中搜索完成,则得出路径是最佳路径,但此时计算量较前者大。
-
当没有找到goal点,同时开启列表已空,则搜索不到路径。结束搜索。
生成路径
-
由goal点B向上逐级追溯父节点,追溯至起点A,此时各节点组成的路径即使A*算法生成的最优路径。
算法实现
- 初始化参数
- 起始点start
- 终点goal
- h_score
- g_socre
- f_score
- 开启列表openset
- 关闭列表closeset
- 追溯表comeFrom
- 程序主体
- 将起始点start加入开启列表openset
- 重复一下工作
- 寻找开启列表openset中F值最小的节点,设为当前点current
- 开启列表openset中移出当前点current
- 关闭列表openset中加入当前点current
- 对当前点的每一个相邻点neighbor
- 如果它不可通过或者已经在关闭列表中,略过。否则:
- 如果它不在开启列表中,加入开启列表中
- 如果在开启列表中,G值判定,若此路径G值比之前路径小,则此相邻点的父节点为当前点,同时更新G与F值。反之,则保持原来的节点关系与G、F值。
- 当目标点goal在开启列表中,则结束程序,此时有路径生成,此时由goal节点开始逐级追溯上一级父节点,直到追溯到开始节点start,此时各节点即为路径。
- 当开启列表为空,则结束程序,此时没有路径
伪代码
// 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]
continue //新gScore>=原gScore,则按照原路径
// 否则选择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