一文教你用python编写Dijkstra算法进行机器人路径规划

avatar 2021年12月5日10:19:37 评论 57

前言

为了机器人在寻路的过程中避障并且找到最短距离,我们需要使用一些算法进行路径规划(Path Planning),常用的算法有Djikstra算法、A*算法等等,在github上有一个非常好的项目叫做PythonRobotics,其中给出了源代码,参考代码,可以对Djikstra算法有更深的了解。

一、算法原理

如图所示,Dijkstra算法要解决的是一个有向权重图中最短路径的寻找问题,图中红色节点1代表起始节点,蓝色节点6代表目标结点。箭头上的数字代表两个结点中的的距离,也就是模型中所谓的代价(cost)。

贪心算法需要设立两个集合,open_set(开集)和closed_set(闭集),然后根据以下程序进行操作:

  • 把初始结点放入到open_set中;
  • 把open_set中代价最小的节点取出来放入到closed_set中,并且作为当前节点;
  • 把与当前节点相邻的节点放入到open_set中,如果代价更小更新代价
  • 重复2-3过程,直到找到终点。

注意open_set中的代价是可变的,而closed_set中的代价已经是最小的代价了,这也是为什么叫做open和close的原因。

至于为什么closed_set中的代价是最小的,是因为我们使用了贪心算法,既然已经把节点加入到了close中,那么初始点到close节点中的距离就比到open中的距离小了,无论如何也不可能找到比它更小的了。

二、程序代码

三、运行结果

四、 A*算法:Djikstra算法的改进

Dijkstra算法实际上是贪心搜索算法,算法复杂度为O( n 2 n^2 n2),为了减少无效搜索的次数,我们可以增加一个启发式函数(heuristic),比如搜索点到终点目标的距离,在选择open_set元素的时候,我们将cost变成cost+heuristic,就可以给出搜索的方向性,这样就可以减少南辕北辙的情况。我们可以run一下PythonRobotics中的Astar代码,得到以下结果:

总结

文章来源于互联网:一文教你用python编写Dijkstra算法进行机器人路径规划

avatar

发表评论

:?: :razz: :sad: :evil: :!: :smile: :oops: :grin: :eek: :shock: :???: :cool: :lol: :mad: :twisted: :roll: :wink: :idea: :arrow: :neutral: :cry: :mrgreen: