这里我们通过Python编程+matplotlib数据可视化来实现路径规划算法,这里我们主要实现A Star算法、D Star算法、Dijkstra算法、RRT算法在2D空间下3D空间下的实现。
A Star算法的设计与实现
Astar潜在地搜索图中一个很大的区域。和Dijkstra一样,Astar能用于搜索最短路径。和BFS一样,Astar能用启发式函数引导它自己。在简单的情况中,它和BFS一样快。
程序入口部分我们指定起始点和目标点,通过调用定义的Astar类来进行路径录规划,最后通过plot进行可视化绘制显示,如图所示。
类的初始化内容如下,主要是传入参数以plot点坐标和算法类型。这里以dict的方式存储,plot通过关键字进行索引找寻数据,如图所示。
通过A Star算法搜索路径点并加入显示,如图所示。
最终路径求解如下,如图所示。
在A Star算法的3D空间路径搜索部分,我们添加全部所有的方位点Direction,这里对所有的求解方位,如图所示。
其余部分内容和2D A Star求解一样,这是增加了求解实现描述显示,求解效果如下,如图所示。
D Star算法的设计与实现
D Star算法对在移动环境中的寻路也比较高效,向当前节点迁移时,可以只考察最近路线上的结点以及相邻节点的变化状况,包括机器人寻路等结果。
这里我们依旧是指定起始点和目标点,通过调用DStar类的方式实现算法的验证和分析,如图所示。
点击聊聊路径规划算法(六)——路径规划算法实现 - 古月居可查看全文