王 旭，劉 毅，李國(guó)燕

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

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

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

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

1 經(jīng)典Dijkstra算法

1.1 柵格地圖建模

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

圖1 柵格化地圖模型示意

圖2 二值地圖模型示意

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

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

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

基本過程如下.

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

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

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

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

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

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

2 改進(jìn)Dijkstra算法

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

2.1 蜂窩地圖建模

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

圖3 蜂窩地圖建模示意

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

2.2 地形影響

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

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

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

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

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

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

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

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

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

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

（3）取Open表中首個(gè)元素s1；

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

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

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

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

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

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

改進(jìn)算法運(yùn)行流程如圖5所示.

圖5 改進(jìn)算法流程

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

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

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

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

圖6 程序流程

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

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

在不考慮地形參數(shù)影響下的路徑規(guī)劃示意圖中，左側(cè)方形區(qū)域?yàn)轱@示區(qū)，其中深色色塊為障礙物區(qū)域，白色區(qū)域?yàn)槠教孤访?，紅色線條為標(biāo)記最優(yōu)路徑.

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

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

圖8 改進(jìn)算法程序仿真示意

3.2 結(jié)果分析

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

表1 算法運(yùn)行速度對(duì)比

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

4 結(jié)語

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