这里我们通过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类的方式实现算法的验证和分析，如图所示。

点击聊聊路径规划算法（六）——路径规划算法实现 - 古月居可查看全文