基于改進Dijkstra算法的移動機器人路徑規(guī)劃

王 旭，劉 毅，李國燕

（天津城建大學(xué) 計算機與信息工程學(xué)院，天津 300384）

移動機器人路徑規(guī)劃問題[1]是智能機器人研究領(lǐng)域中的核心之一，其本質(zhì)是對圖論研究中最短路徑算法問題的引申.近年來國內(nèi)外學(xué)者對于機器人路徑規(guī)劃問題進行過大量的研究，提出了多種移動機器人最短路徑算法，如啟發(fā)式搜索方法、神經(jīng)網(wǎng)絡(luò)方法、遺傳算法[2]、蟻群算法[3-4]、人工蜂群算法[5]等.Dijkstra（迪杰斯特拉）算法[6-8]是一種典型的單源最短路徑算法，用于計算一個節(jié)點到其他所有節(jié)點的最短路徑.它的主要特點是以起始點為中心向外層擴展，直到擴展到終點為止.該算法由于適應(yīng)網(wǎng)絡(luò)拓撲的變化，其性能穩(wěn)定，因而在計算機網(wǎng)絡(luò)拓撲路徑選擇、機器人路徑規(guī)劃以及GIS中應(yīng)用廣泛[9].Ammar等[10]提出松弛Dijkstra算法優(yōu)化機器人路徑規(guī)劃，但是其結(jié)果可能存在較多不必要的路徑轉(zhuǎn)折消耗，甚至出現(xiàn)較大誤差.且傳統(tǒng)算法研究大多以0-1柵格化地圖為基礎(chǔ)，缺乏對于復(fù)雜地形環(huán)境下場景建模的兼容與支持.

近年來智能移動機器人發(fā)展迅速，作業(yè)環(huán)境日趨復(fù)雜，尤其以消防、救災(zāi)為目的的特種機器人對于有限空間及復(fù)雜環(huán)境下的作業(yè)能力要求越來越嚴格.現(xiàn)代智能機器人對復(fù)雜地形的適應(yīng)度研究比較成熟，機械性能優(yōu)良，越野避障能力水平較高.在復(fù)雜的地形環(huán)境下，傳統(tǒng)二值柵格地圖模型并不能滿足機器人路徑規(guī)劃要求.

綜合考慮移動機器人工作場景復(fù)雜程度以及算法的運行效率.本文提出構(gòu)建一種蜂巢型地圖模型對地理信息進行采樣，并對機器人在地圖中需要跨越的不同障礙物進行分析，貼近機器人實際工作場景，在提高算法計算效率的同時保證最優(yōu)路徑，保證消防救災(zāi)等特種機器人工作性能充分發(fā)揮.

1 經(jīng)典Dijkstra算法

1.1 柵格地圖建模

柵格法最早由W.E.Howden于1968年提出，其核心思想是將實際機器人工作環(huán)境抽象成為二值矩陣.算法中通常使用一個二維稀疏矩陣C[N][N]來存儲整個交通網(wǎng)絡(luò)中的數(shù)據(jù)，并以矩陣中的0-1描述機器人能否通過地圖中某區(qū)域.柵格地圖及其二值化處理結(jié)果如圖1、圖2所示.

圖1 柵格化地圖模型示意

圖2 二值地圖模型示意

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

最短路問題是指在一個賦權(quán)圖的兩個指定點s0和t0之間找出一條具有最小權(quán)值的路徑.

Dijkstra算法路徑規(guī)劃基本思路為：為每個節(jié)點j創(chuàng)建一組信息j（dj，pj，fj）；其中dj為地圖中源點s0到當前節(jié)點j的最短路徑長度（某節(jié)點與自身距離為0）；pj為源點s0到節(jié)點j的最短距離中到達節(jié)點j的前一節(jié)點；fj記錄節(jié)點j的位置信息中dj值是否為空.

基本過程如下.

（1）初始化.源點s設(shè)置為：ds=0，ps為空，fs=true.除s外所有其它節(jié)點i（n）：di(n)=∞，pi(n)初始化為空，fi(n)=false，從源點處展開搜索.

（2）計算節(jié)點i（n）中所有fi(n)值為false點的di(n)值：di=min{di，dk+l}.其中：l為節(jié)點 j到節(jié)點 i的直線連接距離；k為源點到節(jié)點j的前一節(jié)點，即點j父節(jié)點.

（3）選取下一個節(jié)點：對于所有fi(n)值為false的點，將其di(n)值進行排序，選擇di(n)值最小的點i作為當前節(jié)點，并設(shè)置pi=k，fi=true.

（4）當終點t滿足ft=true時，算法結(jié)束，并沿pt逆序標出最短路徑，否則轉(zhuǎn)入（2）繼續(xù)循環(huán).

在柵格地圖模型中，由于某一節(jié)點最多存在八個相鄰子節(jié)點且距離不等，因此算法迭代過程中只能通過計算找出到距源點距離最小的節(jié)點作為當前節(jié)點才能繼續(xù)擴展.由算法步驟可知，循環(huán)次數(shù)與節(jié)點數(shù)量成正比，計算時間將成倍增長.

在傳統(tǒng)Dijkstra算法搜索過程中，每次迭代都需從Open表中找出距源點最近的節(jié)點作為當前節(jié)點進行擴展，并將其從Open移入Close表中.由于Open表中全部元素源點距離排序過程時間消耗較大，導(dǎo)致Dijkstra算法在路徑規(guī)劃研究中的計算效率較低.

2 改進Dijkstra算法

改進算法引入一種攜帶地形參數(shù)的蜂窩地圖模型取代傳統(tǒng)0-1柵格法地圖模型，并簡化迭代過程中的節(jié)點擴展機制，直接剪除傳統(tǒng)Dijkstra算法在Open表中排序檢索最近節(jié)點的過程，提高計算效率.

2.1 蜂窩地圖建模

與傳統(tǒng)柵格地圖不同，蜂窩地圖模型將原有地圖劃分為若干共臨邊正六邊形網(wǎng)格區(qū)域，每個網(wǎng)格作為一個采樣節(jié)點對地圖中的相應(yīng)位置地形數(shù)據(jù)進行采樣.蜂窩模型初始化過程中，任意相鄰節(jié)點間距離相等，即某節(jié)點最多存在六個等距相鄰子節(jié)點，如圖3所示.

圖3 蜂窩地圖建模示意

在Dijkstra算法每次迭代過程中，由于各節(jié)點所有相鄰節(jié)點距離均相等，因此改進算法直接跳過在Open表中對節(jié)點距離進行排序的計算過程.

2.2 地形影響

智能移動機器人在室外環(huán)境下作業(yè)時，地形條件很難達到理想通行情況.且對于大多數(shù)戶外及特種機器人來說，路徑規(guī)劃算法難以考慮其部分涉水、攀爬及越障能力，造成機器人性能上的浪費.

復(fù)雜環(huán)境下移動機器人作業(yè)地形如圖4所示.暖色區(qū)域（如紅色、橙色）表示在地圖中為凸起的山坡丘陵等地形，冷色區(qū)域（如藍色）表示地圖中的凹陷地形區(qū)域.移動機器人在較為復(fù)雜的溝壑、丘陵、泥濘等地形條件下的安全行進速度不同，因此路線選擇將會受到環(huán)境因素制約.以避開所有減速區(qū)域只選擇平坦路面為基礎(chǔ)的傳統(tǒng)算法并不適用于實際情況.改進算法中引入地形參數(shù)，可將量化后的路況信息數(shù)據(jù)寫入地圖模型，準確模擬真實環(huán)境下的機器人工作場景，并對機器人的越野能力、能否跨越障礙、越障耗時以及越障行為是否有利做出分析并選擇最短路徑.

圖4 復(fù)雜地形示意

處理帶有地形參數(shù)的地圖模型時，將整個地圖看成一張平整但質(zhì)地不均的宣紙，如果在源點處對宣紙進行浸水操作，由于水分子在宣紙上的擴散速率與宣紙各點密度成反比，最先擴散至終點的水分子運動路徑為最優(yōu)路徑.

改進算法中的地圖設(shè)計參考了宣紙浸水模型中對紙密度的定義：平坦路面對應(yīng)宣紙中可正常濕水部分；大量消耗時間或難以通過的地形區(qū)域作為宣紙中不易濕水部分.從而更加有效的對應(yīng)移動機器人通過平坦、山地、泥濘、涉水以及翻越障礙物等實際作業(yè)場景.

Dijkstra算法以層層擴展的形式展開搜索，只有當某層處理結(jié)束時才會擴展到下一層節(jié)點.因此改進算法將地形參數(shù)統(tǒng)一于擴展層次順序中，即：地形參數(shù)控制某些節(jié)點在對應(yīng)層擴展中暫時不擴展子節(jié)點并且將自己保留到下一層擴展Open表中某位置，直到該節(jié)點擴展滯后程度接近地形環(huán)境對機器人行進的時間消耗.

2.3 改進Dijkstra算法設(shè)計

在改進算法運行過程中，只需按順序遍歷Open表中節(jié)點，即利用廣度優(yōu)先方法對節(jié)點進行擴展即可.且改進算法設(shè)置源點集與終點集，支持區(qū)域目標或多區(qū)域多目標的路徑規(guī)劃.最優(yōu)路徑結(jié)果通過標號法將相應(yīng)父節(jié)點與子節(jié)點連接后產(chǎn)生.改進算法運行步驟如下：

（1）給出源點集S及終點集T，并初始化Open表、Close表；

（2）將源點集S中所有節(jié)點加入Open表；

（3）取Open表中首個元素s1；

（4）選擇與s1相鄰且不在Close表中的所有節(jié)點s（n），將 s1標記為 s（n）的父節(jié)點；

（5）分析點s1處地形情況，判斷是否應(yīng)將點s1由Open表移入Close表中；

（6）將 s（n）加入 Open 表中；

（7）重復(fù)上述過程（3）到（6），直到任意 t∈T 進入Open表搜索停止；

（8）標記終點t父節(jié)點p1為路徑點并不斷重復(fù)此過程直到有p（n）∈S被標記為路徑點時，路徑標記完成；

（9）算法結(jié)束，記錄數(shù)據(jù).

改進算法運行流程如圖5所示.

圖5 改進算法流程

3 實驗結(jié)果

改進Dijkstra算法采用蜂窩模型結(jié)構(gòu)地圖，剪除了運行過程中在Open表中的排序操作，對復(fù)雜地形下的障礙物區(qū)域進行分析，實現(xiàn)標記最優(yōu)路徑的同時提高了計算效率.

3.1 程序設(shè)計

為證明本文算法的有效性，在Windows 7系統(tǒng)下的Visual Studio 2013開發(fā)平臺，通過MFC程序?qū)鹘y(tǒng)Dijkstra算法與改進算法進行對比.程序流程如圖6所示.

圖6 程序流程

傳統(tǒng)算法在無地形參數(shù)影響下的路徑規(guī)劃程序演示界面示意如圖7所示.

圖7 傳統(tǒng)算法無地形分析示意

在不考慮地形參數(shù)影響下的路徑規(guī)劃示意圖中，左側(cè)方形區(qū)域為顯示區(qū)，其中深色色塊為障礙物區(qū)域，白色區(qū)域為平坦路面，紅色線條為標記最優(yōu)路徑.

程序同時支持傳統(tǒng)Dijkstra算法與改進算法的路徑規(guī)劃模擬仿真，并記錄相應(yīng)數(shù)據(jù).

圖8為改進算法在引入地形參數(shù)后對機器人路徑進行規(guī)劃的仿真程序界面，其中顯示區(qū)域中白色為平坦路面，深色塊區(qū)為地形條件較為復(fù)雜、機器人需低速通過的區(qū)域.且深色區(qū)塊顏色越深表示機器人通過該區(qū)域時的難度越高，即時間消耗越大.

圖8 改進算法程序仿真示意

3.2 結(jié)果分析

程序?qū)鹘y(tǒng)DIjkstra算法、改進算法進行運行測試，并將結(jié)果與A*進行對比分析.算法仿真程序運行環(huán)境均為windows7操作系統(tǒng)、Intel Core i5 CPU、4G內(nèi)存，在不同類型的障礙物比例情況下，幾種算法仿真結(jié)果見表1.

表1 算法運行速度對比

結(jié)果表明，在地形條件非常復(fù)雜、障礙物比例較大的情況下，A*算法處理節(jié)點數(shù)明顯增加，啟發(fā)式搜索優(yōu)勢減小.改進算法在復(fù)雜環(huán)境中搜索效率明顯較高.

4 結(jié)語

對于機器人路徑規(guī)劃，本文提出一種蜂窩形地圖模型對地圖進行建模，并參考模擬宣紙浸水模型改進Dijkstra算法，引入地形參數(shù)信息分析，使移動機器人在復(fù)雜地形、危險地形以及多區(qū)域多目標地形條件下快速選擇最優(yōu)路徑.對于搶險、消防等特種機器人路徑規(guī)劃問題的研究發(fā)展，具有一定實際意義.