這里我們通過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空間下規劃效果如圖所示。

編輯：黃飛