基于過(guò)Python+matplotlib數(shù)據(jù)可視化路徑規(guī)劃算法實(shí)現(xiàn)

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

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

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

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

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

通過(guò)A Star算法搜索路徑點(diǎn)并加入顯示，如圖所示。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

編輯：黃飛