這里我們通過Python編程+matplotlib數(shù)據(jù)可視化來實現(xiàn)路徑規(guī)劃算法，這里我們主要實現(xiàn)A Star算法、D Star算法、Dijkstra算法、RRT算法在2D空間下3D空間下的實現(xiàn)。

A Star算法的設(shè)計與實現(xiàn)

Astar潛在地搜索圖中一個很大的區(qū)域。和Dijkstra一樣，Astar能用于搜索最短路徑。和BFS一樣，Astar能用啟發(fā)式函數(shù)引導(dǎo)它自己。在簡單的情況中，它和BFS一樣快。

程序入口部分我們指定起始點和目標(biāo)點，通過調(diào)用定義的Astar類來進(jìn)行路徑錄規(guī)劃，最后通過plot進(jìn)行可視化繪制顯示，如圖所示。

類的初始化內(nèi)容如下，主要是傳入參數(shù)以plot點坐標(biāo)和算法類型。這里以dict的方式存儲，plot通過關(guān)鍵字進(jìn)行索引找尋數(shù)據(jù)，如圖所示。

通過A Star算法搜索路徑點并加入顯示，如圖所示。

最終路徑求解如下，如圖所示。

在A Star算法的3D空間路徑搜索部分，我們添加全部所有的方位點Direction，這里對所有的求解方位，如圖所示。

其余部分內(nèi)容和2D A Star求解一樣，這是增加了求解實現(xiàn)描述顯示，求解效果如下，如圖所示。

D Star算法的設(shè)計與實現(xiàn)

D Star算法對在移動環(huán)境中的尋路也比較高效，向當(dāng)前節(jié)點遷移時，可以只考察最近路線上的結(jié)點以及相鄰節(jié)點的變化狀況，包括機器人尋路等結(jié)果。

這里我們依舊是指定起始點和目標(biāo)點，通過調(diào)用DStar類的方式實現(xiàn)算法的驗證和分析，如圖所示。

類的構(gòu)造函數(shù)部分，調(diào)用Plotting類實現(xiàn)圖表的圖表的初始化構(gòu)造，并聲明相關(guān)閾值變量存儲區(qū)，如圖所示。

這部分是算法的實現(xiàn)核心，主要是“貪心策略”迭代找尋更優(yōu)的求解。如果發(fā)現(xiàn)比當(dāng)前更短的路徑，則進(jìn)行迭代，這里可能向前迭代，也可能向后迭代。D Star算法核心實現(xiàn)如圖所示

Dstar算法對2D空間的求解如圖所示：

D Star算法對3D空間的求解如如圖所示：

Dijkstra算法的設(shè)計與實現(xiàn)

Dijkstra算法也可以算是用貪心思路進(jìn)行的，首先把從起點到每個節(jié)點之間的一段距離都保存留下來并尋找一個到v的，之后松弛一下再重新尋找到v的，所謂的放松方式就是，先遍歷一下把剛才發(fā)現(xiàn)的相距比較近的一點作為中轉(zhuǎn)站會不會更近，如果還更近就再調(diào)節(jié)一段距離，這樣當(dāng)把所有的節(jié)點都找遍了以后，就保存并留下了從起點到其他每個節(jié)點之間的最短距離。

和前兩個一樣，在指定起始點和目標(biāo)點之后，調(diào)用定義的Dijkstra類實現(xiàn)路徑的搜索規(guī)劃，最終通過plot類進(jìn)行可視化顯示。圖可函數(shù)如圖所示。

Dijkstra算法較為較為簡單，這是依據(jù)數(shù)據(jù)結(jié)構(gòu)的基本構(gòu)造進(jìn)行實現(xiàn)，核心代碼如如圖所示。

Dijkstra算法2D路徑規(guī)劃如如圖所示。

Dijkstra算法3D路徑規(guī)劃效果如圖所示：

RRT算法的設(shè)計與實現(xiàn)

RRT（快速尋找隨機樹）是一個很普通的辦法，無所謂任何機器人種類、無所謂自由度是多少、也無所謂約束有多繁復(fù)，都可以使用。

并且它的基本原理非常簡潔，這是其在機器人應(yīng)用領(lǐng)域受歡迎的主要因素之一。但是它的缺陷也非常突出，它得到的路通常質(zhì)量都不會非常好，例如可能具有棱角，不平滑，通常也遠(yuǎn)離最優(yōu)路線。

RRT算法是基于抽樣路徑規(guī)劃，它在3D空間下的路徑規(guī)劃效果較好。核心功能函數(shù)如圖所示。

RRT算法在3D空間下規(guī)劃效果如圖所示。

編輯：黃飛