在路径规划blog中已经结合具体的室内环境对实现室内路径规划的基本思想进行了介绍,这里再介绍一种常用的路径搜索算法A*,对算法的一些基本原理和实现思路进行分析,后期将进一步将该算法运用到室内路径规划中,加入各种输入参数,为机器人提供路径查询和预测。
A*算法,A*(A-Star)算法是一种静态路网中求解最短路径最有效的直接搜索方法,也是解决许多搜索问题的有效算法。算法中的距离估算值与实际值越接近,最终搜索速度越快。
A*算法基本数学模型如下:
f(n)=g(n)+h(n)
其中g(n)是状态空间从初始化到状态n的实际代价
h(n)是从状态n到目标状态的最佳路径的估计代价
f(n)是从初始状态经由状态n到目标状态的代价估计
我们以d(n)表达状态n到目标状态的距离,那么h(n)的选取大致有如下三种情况,这也是算法的基本思路:
如果h(n)<= d(n)到目标状态的实际距离,这种情况下,搜索的点数多,搜索范围大,效率低。但能得到最优解。
如果h(n)=d(n),即距离估计h(n)等于最短距离,那么搜索将严格沿着最短路径进行, 此时的搜索效率是最高的。
如果 h(n)>d(n),搜索的点数少,搜索范围小,效率高,但不能保证得到最优解。
基于上述思路,采用python实现A*算法核心过程如下:
while True:
_block = raw_input('input block:')
if not _block:
break
_block = _block.split(',')
_block = (int(_block[0]), int(_block[1]))
_2dmap[_block[0]][_block[1]] = 'X'
block_node = Node(None, _block[0], _block[1])
close_list[(block_node.x, block_node.y)] = block_node
详细实现可以参考http://blog.csdn.net/nobugtodebug/article/details/43986597,后续设计将借鉴该实现代码,结合室内环境需求对其进行改进设计。后续blog将向大家详细介绍如何将A*算法应用到室内的路径规划中。
同样关于算法的实际运行展示,大家可以访问http://netsmell.com/post/pathfinding.html,这里可以设置相关参数进行算法模拟展示,如下图所示。