改進(jìn)Dijkstra 機(jī)器人路徑規(guī)劃算法研究

陳智康 ，劉 佳 ，2，王丹丹 ，張運(yùn)喜 ，2

（1.天津職業(yè)技術(shù)師范大學(xué)天津市信息傳感與智能控制重點(diǎn)實(shí)驗(yàn)室，天津 300222；2.天津職業(yè)技術(shù)師范大學(xué)自動化與電氣工程學(xué)院，天津 300222；3.青島市技師學(xué)院，青島 266229）

自從1959 年恩格爾伯格發(fā)明了第一臺機(jī)器人并用于生產(chǎn)之后，機(jī)器人這一新事物就逐漸成為科研人員研究的焦點(diǎn)。隨著科學(xué)技術(shù)以及新興技術(shù)在人工智能領(lǐng)域不斷發(fā)展，機(jī)器人的相關(guān)開發(fā)成為其中的一個(gè)小分支。多類型的機(jī)器人出現(xiàn)在科學(xué)行業(yè)中，如類人機(jī)器人、移動機(jī)器人、水下機(jī)器人等。目前相關(guān)領(lǐng)域研究者的研究集于陸地移動機(jī)器人方面，該研究被細(xì)分為3 個(gè)方面：構(gòu)建地圖的機(jī)器人環(huán)境、路徑規(guī)劃以及機(jī)器人尋路導(dǎo)航。20 世紀(jì)60 年代末，斯坦福研究院研制了自主移動機(jī)器人，提出了路徑規(guī)劃問題是移動機(jī)器人的關(guān)鍵問題。移動機(jī)器人路徑規(guī)劃是指根據(jù)某種優(yōu)化標(biāo)準(zhǔn)（勞作代價(jià)小、路徑最少、耗時(shí)最短），在機(jī)器人的工作空間中探求一條從起始位置到目的位置的無碰撞最優(yōu)，也可能是次優(yōu)路徑[1-2]?，F(xiàn)今路徑規(guī)劃算法主要分為傳統(tǒng)路徑規(guī)劃算法和智能路徑規(guī)劃算法2 大類。He 等[3]針對路徑規(guī)劃問題，提出了柵格法用于解決機(jī)器人路徑規(guī)劃問題，此方法對機(jī)器人的運(yùn)行環(huán)境進(jìn)行了柵格表示，簡化了規(guī)劃工作。Sariff 等[4]針對未知環(huán)境的路徑規(guī)劃問題，提出了人工勢場法用于移動機(jī)器人路徑規(guī)劃，雖然收斂速度快，但存在死鎖現(xiàn)象。馬麗莎[5]提出了一種用矩形表示不規(guī)則障礙物的可視圖法，簡化了路徑規(guī)劃中構(gòu)建環(huán)境地圖的工作。文獻(xiàn)[6]提出將路徑規(guī)劃中的路段描述成一系列中途點(diǎn)，能夠根據(jù)規(guī)劃要求完成解空間的搜索，輸出最優(yōu)個(gè)體，但是存在算法編碼較長的問題，導(dǎo)致收斂速度慢，計(jì)算量大。Ammar 等[7]針對移動機(jī)器人在已知環(huán)境中的路徑規(guī)劃問題，提出了一種松弛Dijkstra 算法，但是其結(jié)果中存在多余的路徑折損，有時(shí)出現(xiàn)較大誤差。經(jīng)典的Dijkstra 算法使用在機(jī)器人自主構(gòu)思路徑中所產(chǎn)生的路徑，存在少許冗雜的點(diǎn)，在很多情況下整個(gè)運(yùn)算過程運(yùn)算次數(shù)過多且容易陷入局部最優(yōu)解，所以導(dǎo)致整個(gè)尋路過程會走很多彎路。因此，為了將Dijkstra 算法能夠應(yīng)用于機(jī)器人路徑規(guī)劃且獲得較好的規(guī)劃效果，對Dijkstra 算法進(jìn)行改進(jìn)十分必要。本文將蟻群算法中信息素的計(jì)算方法引入原Dijkstra 算法中，完成算法優(yōu)化。

1 經(jīng)典Dijkstra 算法

1.1 可視圖建模

可視圖地圖是構(gòu)建移動機(jī)器人環(huán)境地圖的一種方法，這種算法將環(huán)境中的障礙物抽象為幾何圖形，可以理解為圖中所看到的是事物的俯視圖，在平面上能夠反映障礙物所占的面積。在路徑規(guī)劃領(lǐng)域，考慮求解方便，可把移動的任何機(jī)器人抽象成一個(gè)點(diǎn)，暫時(shí)計(jì)算這些障礙物的大小，這時(shí)障礙物等效成一個(gè)任意形狀的圖形。后期分別把地圖中的出發(fā)點(diǎn)、障礙物以及所要到達(dá)目的地通過線段或是曲線連接起來，每條連接線不能從障礙物上面穿過且不能和邊界有接觸，這樣得到一張環(huán)境地圖，這張圖被稱為可視圖。該可視圖中所有的線段端點(diǎn)是可見的，從起始點(diǎn)到目標(biāo)點(diǎn)可以通過線段到達(dá)。利用可視圖建立機(jī)器人路徑規(guī)劃的環(huán)境地圖和建立的柵格圖相比，可視圖建立的環(huán)境地圖更接近于真實(shí)的環(huán)境實(shí)景。柵格圖的優(yōu)點(diǎn)是能夠在環(huán)境中放置很多障礙物，增加機(jī)器人路徑規(guī)劃的難度，用來檢驗(yàn)算法的抗干擾能力，但柵格圖在很多情況下不能表達(dá)復(fù)雜的障礙物信息。可視圖的原理比較簡單，且比較容易實(shí)現(xiàn)，在大部分應(yīng)用場景中，涉及靜態(tài)的全局路徑規(guī)劃一般會使用可視圖進(jìn)行環(huán)境建模[8]。

雖然可視圖比較好實(shí)現(xiàn)，如果障礙物增加得很多并達(dá)到一定程度的話，或者說是在障礙物外形輪廓十分復(fù)雜時(shí)，在可視圖中會出現(xiàn)很多的連線，這就增大了算法的工作量，影響真正的路徑規(guī)劃結(jié)果[9]。構(gòu)建復(fù)雜的環(huán)境地圖時(shí)，先不要使用可視圖法，可以在簡化環(huán)境地圖形狀后再進(jìn)行路徑規(guī)劃，這樣就能夠節(jié)省很多規(guī)劃上所耗費(fèi)的不必要的計(jì)算。環(huán)境可視圖如圖1所示。

圖1 環(huán)境可視圖

1.2 傳統(tǒng)Dijkstra算法步驟

Dijkstra 算法是一種經(jīng)典的求解最短路徑的算法，用于計(jì)算一個(gè)節(jié)點(diǎn)去往其他各個(gè)不相關(guān)節(jié)點(diǎn)的最小移動代價(jià)[10]。本思想是把在圖中無論如何出現(xiàn)的所有節(jié)點(diǎn)分離為2 組，第1 組包含被準(zhǔn)確認(rèn)定是最短路徑的節(jié)點(diǎn)，第2 組存放待檢查的不確定的節(jié)點(diǎn)。根據(jù)最小移動代價(jià)逐漸增大的順序，逐個(gè)將第2 組需要檢查的節(jié)點(diǎn)加入到第1 組中，一直到從起始點(diǎn)出發(fā)可以到達(dá)的所有節(jié)點(diǎn)都包含在第1 組中。Dijkstra 算法運(yùn)行的主要特征是以機(jī)器人抽象的出發(fā)點(diǎn)為中心向外層層延伸，直到延伸到整個(gè)區(qū)域的末端為止。Dijkstra 算法一般能夠在最終得到最優(yōu)的路徑，但是這個(gè)路徑偶爾會出現(xiàn)冗余拐點(diǎn)，由于其遍歷節(jié)點(diǎn)多，所以有時(shí)算法的效率不高[11]。Dijkstra 算法流程如圖2 所示。

分別創(chuàng)建 2 個(gè)表，為：START 與 FINISH。START表中存放環(huán)境中已經(jīng)生成存在但是未經(jīng)計(jì)算的節(jié)點(diǎn)，F(xiàn)INISH 表中存放所有經(jīng)過計(jì)算考察的節(jié)點(diǎn)。

（1）觀測尋找在環(huán)境路網(wǎng)中離起始源點(diǎn)近而且是沒有被計(jì)算過的點(diǎn)，把滿足調(diào)節(jié)的點(diǎn)放在START 數(shù)組中等待檢查。

（2）從START 表中找出距離起始點(diǎn)最小的點(diǎn)，然后把該點(diǎn)放進(jìn)FINISH 表中。

（3）遍歷去尋找考察當(dāng)前此點(diǎn)的子節(jié)點(diǎn)，計(jì)算出起始源點(diǎn)到這些子節(jié)點(diǎn)的距離值，將值存入數(shù)組中用來排序，將子節(jié)點(diǎn)放在START 表中。

（4）重復(fù)（2）和（3），直到把 START 表中的點(diǎn)全部清空，或者找到真正要去的目標(biāo)點(diǎn)。

1.3 Dijkstra算法的思想

（1）將環(huán)境可視圖中出現(xiàn)的節(jié)點(diǎn)或是頂點(diǎn)分別加入2 個(gè)組，其中一組存放經(jīng)過求解驗(yàn)證為最短路徑所經(jīng)過的頂點(diǎn)，該集合用FINISH 表示，剛開始時(shí)START表中只有1 個(gè)起始點(diǎn)，在后面的運(yùn)算過程中每得到1條最短路徑，就把相關(guān)節(jié)點(diǎn)加入到集合FINISH 中。如果全部的頂點(diǎn)都加入到FINISH 后，此算法運(yùn)算完成。另一組存放未經(jīng)過考察且還未被認(rèn)定為最短路徑的頂點(diǎn)，用START 表示。

（2）在將頂點(diǎn)逐漸加入到FINISH 表的過程中，在運(yùn)行算法期間總保持從出發(fā)點(diǎn)到FINISH 表中各個(gè)點(diǎn)的最短路徑的長度不超過出發(fā)點(diǎn)到START 表中任何頂點(diǎn)的最短路徑長度。

典型的Dijkstra 算法在尋找最優(yōu)路徑的準(zhǔn)則上較為單一，以某一個(gè)點(diǎn)為中心向外層層擴(kuò)展就類似于畫圓，隨著搜索半徑的逐漸增大，最后總能找到想要的目標(biāo)點(diǎn)，但同時(shí)最優(yōu)路徑會出現(xiàn)一些無關(guān)的冗余點(diǎn)，這樣在一定程度上給實(shí)際的移動機(jī)器人造成尋找路徑的困難[12]。如果能夠?qū)?jīng)典算法的計(jì)算準(zhǔn)則進(jìn)行優(yōu)化，達(dá)到減少路徑中拐點(diǎn)的目的，就可以減少一大部分機(jī)器人尋路所耗費(fèi)的時(shí)間。在大面積環(huán)境中使用Dijkstra 算法時(shí)，由于每次都要從START 列表中挑選離原始點(diǎn)最近的點(diǎn)，這樣導(dǎo)致規(guī)劃效率不高，在排序過程消耗很多不必要的工作時(shí)間[13-14]。

2 改進(jìn)Dijkstra 算法

在經(jīng)典的Dijkstra 算法中，用來判斷最優(yōu)路徑點(diǎn)的方法就是從源點(diǎn)找到最短歐式距離的點(diǎn)，作為合適的最短路徑所經(jīng)過的點(diǎn)，這樣所找出的路徑往往會有一些冗余的點(diǎn)。優(yōu)化就是在尋找節(jié)點(diǎn)的過程中做仔細(xì)篩選，遵循原經(jīng)典算法尋優(yōu)準(zhǔn)則有時(shí)會陷入局部最優(yōu)，本文所做的改進(jìn)是在原Dijkstra 算法的基礎(chǔ)上，增加了一部分尋優(yōu)準(zhǔn)則函數(shù)，在原本判斷為最優(yōu)路徑點(diǎn)之中利用相關(guān)路徑的信息素或是密集程度做排除，在確定放入FINISH 表中之前做好刪減，這樣在最后就可以從終點(diǎn)到起點(diǎn)再回溯到起點(diǎn)。

2.1 環(huán)境地圖構(gòu)建

在真實(shí)平面環(huán)境中，建立用于機(jī)器人路徑規(guī)劃的環(huán)境地圖模型一般有2 種選擇：柵格法和可視圖法。柵格法就是將障礙物模擬成小方格的集合，相當(dāng)于將場景的所有事物進(jìn)行二值化替代，障礙物為1，非障礙物為0?？梢晥D法就是將環(huán)境中的障礙物抽象成俯視圖，連接起點(diǎn)和障礙物各個(gè)頂點(diǎn)，再加上和周圍邊界的連接生成帶鏈接線的環(huán)境地圖，這樣做出地圖之后能夠?yàn)檫\(yùn)行尋路算法減少很多工作量。帶鏈接線可視圖如圖3 所示。從圖3 中可以看出，機(jī)器人所走的無碰撞最優(yōu)路徑需要依靠算法實(shí)現(xiàn)[15]。

圖3 帶鏈接線可視圖

2.2 Dijkstra算法改進(jìn)步驟

經(jīng)典的Dijkstra 算法得到的路徑存在冗余雜點(diǎn)，本文所做的工作就是減少在原最短路徑上存在的冗余雜點(diǎn)[16-18]。在原來算法基礎(chǔ)上加上類似于蟻群算法中的信息素概念的判別因子，相關(guān)步驟加入了帶信息素因子的算式并進(jìn)行判別，為原先的算法添加了一些計(jì)算公式。

2.2.1 信息素因子

本文所添加的信息素因子類似于蟻群算法中的信息素濃度。蟻群算法是由Dorigo M 等在20 世紀(jì)90年代構(gòu)思出的一種屬于生物啟發(fā)式智能優(yōu)化算法，它來自人們對螞蟻尋找路徑方法的研究[19]。人們在觀測螞蟻搜索食物時(shí)候發(fā)現(xiàn)，螞蟻在尋找食物時(shí)，總在路過的道路上留下一種被叫做信息素的生物性物質(zhì)，信息素可以在原來的地點(diǎn)保留一段時(shí)間，使得在附近區(qū)域內(nèi)的其他螞蟻能夠發(fā)現(xiàn)該信息素的存留，后面過來的螞蟻在選擇要走的路時(shí)，會挑選信息素比較大的路徑，每個(gè)螞蟻經(jīng)過路徑都會留下自身的信息素，隨著信息素不斷增強(qiáng)，之后最優(yōu)的路徑就會逐漸顯現(xiàn)出來[20]。信息素尋路示意圖如圖4 所示。

圖4 信息素尋路示意圖

假設(shè)機(jī)器人從 P（i）到 P（i+2），P（i）、P（i+1）、P（i+2）3 個(gè)點(diǎn)是經(jīng)過經(jīng)典Dijkstra 算法運(yùn)算得到的3個(gè)機(jī)器人經(jīng)過的最短路徑點(diǎn)。這3 個(gè)點(diǎn)會被放在FINISH 表中，算法的運(yùn)行結(jié)果會認(rèn)為經(jīng)過這3 個(gè)點(diǎn)是最優(yōu)的路徑。但顯然直接從 P（i）到 P（i+2）比先從 P（i）到P（i+1）再到P（i+2）機(jī)器人行走的移動代價(jià)要小很多，也就是說從 P（i）到 P（i+2）留下的信息素會比較多。本文所做的改進(jìn)是添加了信息因子作為判斷準(zhǔn)則，用來做進(jìn)一步篩選。

信息素因子計(jì)算公式為

式中：（xs，ys）為起始點(diǎn)坐標(biāo)；（xt，yt）為終點(diǎn)坐標(biāo)；（xi，yi）為路徑中第i 個(gè)節(jié)點(diǎn)；cmn為信息素因子。

信息素因子計(jì)算公式的分子是點(diǎn)與點(diǎn)之間移動歐式距離的疊加，其分母是起點(diǎn)到終點(diǎn)的曼哈頓距離的疊加。該因子越小說明經(jīng)過這幾個(gè)節(jié)點(diǎn)的路徑越接近最優(yōu)路徑。信息素因子運(yùn)用示意圖如圖5 所示。

在圖5 中運(yùn)用上述公式計(jì)算，假若從P（i）到P′（i+1）再到P（i+2）計(jì)算出信息素因子為C1，從P（i）到 P（i+1）再到 P（i+2）計(jì)算出的信息素因子為C2。如果C1＜C2，那么C2 所代表的路徑除了和C1 所代表路徑的共有的點(diǎn)外，其他的點(diǎn)被刪除，不再存放在FINISH 表中。通過這種方法將經(jīng)典Dijkstra 算法得到的路徑點(diǎn)中冗余的點(diǎn)刪除，剩下的點(diǎn)便可存放在FINISH 表中。

2.2.2 改進(jìn)Dijkstra算法設(shè)計(jì)

在上述經(jīng)典Dijkstra 算法步驟3 中，如果得到的新節(jié)點(diǎn)數(shù)≥2，先不放入FINISH 表中，計(jì)算經(jīng)過這些節(jié)點(diǎn)且有相同移動效果路徑的信息素因子的值，將信息素因子較大的值所代表的路徑上的節(jié)點(diǎn)（除了共有節(jié)點(diǎn)外）刪除后，剩下的節(jié)點(diǎn)再加入FINISH 表中。在對新節(jié)點(diǎn)進(jìn)行排序之后，不斷運(yùn)用這種準(zhǔn)則進(jìn)行判斷，直到完成對所有節(jié)點(diǎn)的遍歷。最后得到的FINISH表中，從終點(diǎn)依次按照節(jié)點(diǎn)序號遞減到到達(dá)起點(diǎn)就得到一條最短路徑。在原始的Dijkstra 算法中，每次得到2 個(gè)以上新節(jié)點(diǎn)時(shí)，運(yùn)用信息素因子判斷準(zhǔn)則，判斷這些新得到的最短路徑節(jié)點(diǎn)是否可以存到FINISH 表中。這樣在FINISH 表中存放的節(jié)點(diǎn)可以減少很大一部分冗余[21-22]。

蟻群算法本身就是一種在圖中尋找優(yōu)化路徑的概率性算法。其求解模式可以將問題求解的快速性、全局優(yōu)化特征以及有限時(shí)間內(nèi)答案的合理性相結(jié)合。正反饋式的信息傳遞和積累保證了蟻群算法尋優(yōu)的快速性。而算法的早熟性收斂又能夠通過其分布式的計(jì)算來避免。同時(shí)，具有貪婪啟發(fā)式搜索特征的蟻群系統(tǒng)又能在搜索過程中盡早找到可以接受的解去解決相應(yīng)問題。本文的信息素因子來源于蟻群算法，通過將其加入經(jīng)典Dijkstra 算法，對Dijkstra 算法中的路徑節(jié)點(diǎn)進(jìn)行尋優(yōu)，加快了算法的運(yùn)算速度。經(jīng)過改進(jìn)后的算法能夠根據(jù)解的分布情況自適應(yīng)地進(jìn)行信息素更新，從而動態(tài)地調(diào)整了各路徑上的信息素強(qiáng)度，增加了解空間的多樣性，增加了在全局規(guī)劃上的搜索能力，有效規(guī)避了局部收斂和早熟現(xiàn)象。因此，通過使用蟻群算法優(yōu)化經(jīng)典Dijkstra 算法，能夠獲得較好的規(guī)劃效果，方法切實(shí)可行。

3 實(shí)驗(yàn)結(jié)果

本文改進(jìn)的Dijkstra 算法采用可視圖法構(gòu)建地圖，將得到的新節(jié)點(diǎn)在加入到FINISH 表之前進(jìn)行篩選，對模擬可視圖的帶有障礙物機(jī)器人的運(yùn)行環(huán)境進(jìn)行路徑規(guī)劃，得到最優(yōu)路徑點(diǎn)的同時(shí)也增加了尋路的速度。

3.1 程序設(shè)計(jì)

為驗(yàn)證本文算法改進(jìn)的有效性，在Windows 7 系統(tǒng)下的Matlab 2014a 開發(fā)平臺上，通過Matlab 程序腳本首先實(shí)現(xiàn)傳統(tǒng)Dijkstra 路徑規(guī)劃算法，然后再實(shí)現(xiàn)改進(jìn)后的算法，最后將實(shí)際運(yùn)行后的2 個(gè)效果圖進(jìn)行對比。程序設(shè)計(jì)流程圖如圖6 所示。

圖6 程序設(shè)計(jì)流程圖

3.2 結(jié)果分析

在程序設(shè)計(jì)中，對經(jīng)典Dijkstra 算法、改進(jìn)后的Dijkstra 算法進(jìn)行運(yùn)行測試，并將得到的運(yùn)行結(jié)果進(jìn)行對比分析。算法仿真程序運(yùn)行環(huán)境為windows 7 的64位操作系統(tǒng)，處理器為IntelCorei5CPU、16G 運(yùn)行內(nèi)存，算法仿真運(yùn)行結(jié)果如圖7 所示。

在圖7 中，斷續(xù)實(shí)心點(diǎn)軌跡代表經(jīng)典Dijkstra 算法路徑規(guī)劃的運(yùn)行結(jié)果，虛線軌跡代表改進(jìn)后Dijkstra算法運(yùn)行結(jié)果。在原始算法規(guī)劃的軌跡中存在一些多余的拐點(diǎn)，在添加信息素因子判斷準(zhǔn)則優(yōu)化后的算法，生成的軌跡比之前的軌跡少了一些多余的拐點(diǎn)。運(yùn)算次數(shù)如圖8 所示。

圖7 仿真運(yùn)行結(jié)果

圖8 運(yùn)算次數(shù)

從圖8 可知，在110 左右的運(yùn)算次數(shù)之后便基本上完成路徑規(guī)劃的任務(wù)，迭代次數(shù)少。在運(yùn)行效率上，改進(jìn)后的Dijkstra 算法能夠?qū)崿F(xiàn)更加快速地搜索，在一定程度上減少了搜索時(shí)間。

4 結(jié) 語

本文針對陸地移動機(jī)器人路徑規(guī)劃問題，提出了一種改進(jìn)Dijkstra 算法，該算法能夠在可視圖地圖上實(shí)現(xiàn)平面路徑規(guī)劃，將蟻群算法中信息素因子加入原Dijkstra 算法的尋優(yōu)準(zhǔn)則中，在一定程度上避免路徑規(guī)劃中出現(xiàn)冗余點(diǎn)，提高機(jī)器人路徑規(guī)劃效率。仿真結(jié)果表明，經(jīng)過改進(jìn)的Dijkstra 算法能夠更高效地完成路徑規(guī)劃任務(wù)。如果將此算法運(yùn)用到現(xiàn)實(shí)中的機(jī)器人路徑規(guī)劃，能夠在一定程度上使機(jī)器人所走的路徑盡可能平滑。在引入了改進(jìn)Dijkstra 算法后，使陸地移動機(jī)器人在獲得周邊環(huán)境信息進(jìn)行地圖建模后能夠順利找到適合移動的最佳路徑。改進(jìn)算法對于家庭、軍事、醫(yī)療等服務(wù)型機(jī)器人路徑規(guī)劃的研究具有一定的現(xiàn)實(shí)意義。