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

类的构造函数部分，调用Plotting类实现图表的图表的初始化构造，并声明相关阈值变量存储区，如图所示。

这部分是算法的实现核心，主要是“贪心策略”迭代找寻更优的求解。如果发现比当前更短的路径，则进行迭代，这里可能向前迭代，也可能向后迭代。D Star算法核心实现如图所示

Dstar算法对2D空间的求解如图所示：

D Star算法对3D空间的求解如如图所示：

Dijkstra算法的设计与实现

Dijkstra算法也可以算是用贪心思路进行的，首先把从起点到每个节点之间的一段距离都保存留下来并寻找一个到v的，之后松弛一下再重新寻找到v的，所谓的放松方式就是，先遍历一下把刚才发现的相距比较近的一点作为中转站会不会更近，如果还更近就再调节一段距离，这样当把所有的节点都找遍了以后，就保存并留下了从起点到其他每个节点之间的最短距离。

和前两个一样，在指定起始点和目标点之后，调用定义的Dijkstra类实现路径的搜索规划，最终通过plot类进行可视化显示。图可函数如图所示。

Dijkstra算法较为较为简单，这是依据数据结构的基本构造进行实现，核心代码如如图所示。

Dijkstra算法2D路径规划如如图所示。

Dijkstra算法3D路径规划效果如图所示：

RRT算法的设计与实现

RRT（快速寻找随机树）是一个很普通的办法，无所谓任何机器人种类、无所谓自由度是多少、也无所谓约束有多繁复，都可以使用。并且它的基本原理非常简洁，这是其在机器人应用领域受欢迎的主要因素之一。但是它的缺陷也非常突出，它得到的路通常质量都不会非常好，例如可能具有棱角，不平滑，通常也远离最优路线。

RRT算法是基于抽样路径规划，它在3D空间下的路径规划效果较好。核心功能函数如图所示。

RRT算法在3D空间下规划效果如图所示。