改進(jìn)柵格地圖的煤礦救援機(jī)器人路徑規(guī)劃研究

路子佩，姜媛媛,2，李宏偉

(1.安徽理工大學(xué) 電氣與信息工程學(xué)院，安徽 淮南 232001;2.安徽理工大學(xué) 環(huán)境友好材料與職業(yè)健康研究院(蕪湖)，安徽 蕪湖 241003)

煤礦事故后的救援工作充滿了挑戰(zhàn)和危險(xiǎn)，考慮到救援人員的安全以及救援任務(wù)的緊迫性，有必要使用煤礦救援機(jī)器人來(lái)執(zhí)行救援任務(wù)[1-2].

煤礦救援機(jī)器人在進(jìn)行路徑規(guī)劃過(guò)程中，算法選擇與地圖構(gòu)建是兩個(gè)基本問(wèn)題.按照機(jī)器人路徑規(guī)劃過(guò)程中對(duì)周圍環(huán)境感知的水平，可以劃分為全局和局部路徑規(guī)劃算法.地圖的構(gòu)建方法主要包括三角網(wǎng)格剖分法[3]和柵格法[4].這些路徑規(guī)劃算法和地圖構(gòu)建方法雖然在某一方面有獨(dú)特優(yōu)勢(shì)，但是也存在著一些局限性，因此學(xué)者們對(duì)此做出了許多改進(jìn)工作，并得到了一定的成果.王小平等[5]提出了改進(jìn)的定角度初始路徑規(guī)劃算法，避免了傳統(tǒng)參數(shù)化求解過(guò)程的無(wú)解和多解情況.婁安東等[6]提出一種將圖搜索與柵格搜索相結(jié)合的二叉樹路徑規(guī)劃算法，該算法通過(guò)逆向搜索優(yōu)化局部路徑，剪枝優(yōu)化二叉樹，從而得到最佳路徑.陶哲等[7]提出一種基于A*算法[8]的蜂巢柵格地圖方法，該算法在障礙物信息描述方面具有較大優(yōu)勢(shì)，規(guī)劃出的路徑長(zhǎng)度相比傳統(tǒng)柵格法要短.上述研究雖然取得了一些優(yōu)勢(shì)，但是還存在著一些不足.

該文結(jié)合前人的研究成果，提出一種改進(jìn)柵格地圖的煤礦救援機(jī)器人路徑規(guī)劃方法.首先構(gòu)建障礙物的改進(jìn)柵格地圖模型；其次，采用Dijkstra算法實(shí)現(xiàn)在地圖模型中的路徑規(guī)劃，驗(yàn)證該地圖模型的可行性；在此基礎(chǔ)之上，運(yùn)用A*算法實(shí)現(xiàn)在改進(jìn)柵格地圖上的運(yùn)行，得到最終的優(yōu)化路徑.

1 改進(jìn)柵格地圖建模

現(xiàn)實(shí)中物體的表面從直觀上看是由許多曲面所構(gòu)成，這些曲面包括三角形、四邊形及多邊形等，由于任意的多邊形網(wǎng)格實(shí)際上也能細(xì)分為三角形，且三角形簡(jiǎn)單而更容易操作，因此本文采用三角網(wǎng)格剖分改進(jìn)柵格地圖.

采用三角剖分將測(cè)試地圖構(gòu)建成三角網(wǎng)格地圖，該地圖可以用單元連接矩陣C及節(jié)點(diǎn)矩陣P表示.設(shè)三角剖分后的地圖中有m個(gè)節(jié)點(diǎn)和k個(gè)單元，則單元連接矩陣可以表示為：

(1)

將第i個(gè)節(jié)點(diǎn)的坐標(biāo)表示為Pi(xi,yi,zi)，則節(jié)點(diǎn)矩陣可以表示為：

(2)

(3)

(4)

節(jié)點(diǎn)連接矩陣T和S的節(jié)點(diǎn)為網(wǎng)格節(jié)點(diǎn)號(hào).T中的第i個(gè)元素ti與S中的第i個(gè)si是相關(guān)聯(lián)的，且表示節(jié)點(diǎn)ti與節(jié)點(diǎn)si連接.由于節(jié)點(diǎn)之間的距離不同，因此將相關(guān)矩陣T與S之間的距離矩陣定義為D：

(5)

其中:di表示節(jié)點(diǎn)關(guān)聯(lián)矩陣T中第i個(gè)節(jié)點(diǎn)ti與S的第i個(gè)節(jié)點(diǎn)與節(jié)點(diǎn)si的距離.文中引入加權(quán)系數(shù)，定義

(6)

其中:hi為節(jié)點(diǎn)si與節(jié)點(diǎn)ti的差;di為節(jié)點(diǎn)si與節(jié)點(diǎn)ti的距離.由距離矩陣及加權(quán)系數(shù)，可以定義權(quán)重矩陣：

(7)

因此，節(jié)點(diǎn)關(guān)聯(lián)矩陣T、S和權(quán)重矩陣W可以構(gòu)成稀疏矩陣表示的賦權(quán)無(wú)向連通圖矩陣：

G=sparse(T,S,W).

(8)

而矩陣中基于三角形網(wǎng)格的映射矩陣map可以表示為節(jié)點(diǎn)矩陣和無(wú)向連通圖矩陣組成的關(guān)聯(lián)矩陣：

map=(P,G).

(9)

2 改進(jìn)柵格地圖路徑規(guī)劃算法

2.1 Dijkstra算法

Dijkstra算法是一種尋找從一個(gè)源節(jié)點(diǎn)(即起點(diǎn))到圖構(gòu)造中任何其他節(jié)點(diǎn)最短路徑的技術(shù).它采用了貪心的思想搜索全局，求取最優(yōu)解.Dijkstra算法的主要步驟為：

Step1：運(yùn)行開始，K中只包括初始點(diǎn)，設(shè)初始點(diǎn)為v1，即K={v1}，v1的距離為0.U中包含除v1之外的其余頂點(diǎn)，即U={v2,v3,...,vi}.若v1與U中頂點(diǎn)vi是鄰接點(diǎn)，則〈vi，v1〉正常有權(quán)值，表示能通過(guò)，若vi不是v1的鄰接點(diǎn)，則〈vi，v1〉的權(quán)值為0，表示不能通過(guò)；

Step2：從U中選取一個(gè)距離v1最小的頂點(diǎn)s，把s加入K中(該選定的距離就是v1到s的最短路徑長(zhǎng)度)；

Step3：以s為新考慮的中間點(diǎn)，修改U中各頂點(diǎn)的距離；若從源點(diǎn)v1到頂點(diǎn)vi的距離(經(jīng)過(guò)頂點(diǎn)s)比原來(lái)距離(不經(jīng)過(guò)頂點(diǎn)s)短，則修改頂點(diǎn)vi的距離值，修改后的經(jīng)過(guò)頂點(diǎn)s的最短距離則為該邊的權(quán)值；

Step4：重復(fù)步驟Step2和Step3直到所有頂點(diǎn)都包含在K中.

2.2 A*算法

A*算法是在Dijkstra算法的基礎(chǔ)上加入啟發(fā)式函數(shù)，利用啟發(fā)信息尋找最優(yōu)路徑.A*算法需要在地圖中搜索節(jié)點(diǎn)，并設(shè)定適合的啟發(fā)函數(shù)進(jìn)行指導(dǎo).通過(guò)評(píng)價(jià)各個(gè)節(jié)點(diǎn)的代價(jià)值，獲取下一需要拓展的最佳節(jié)點(diǎn)，直至到達(dá)最終目標(biāo)點(diǎn)位置.其公式為：

f(n)=g(n)+h(n)，

(10)

其中，g(n)表示從起始點(diǎn)到當(dāng)前節(jié)點(diǎn)n的最小工作代價(jià)，h(n)表示當(dāng)前節(jié)點(diǎn)n到終點(diǎn)的最小估計(jì)代價(jià).當(dāng)h(n)為0時(shí)，A*算法也就變成了Dijkstra算法.A*算法優(yōu)點(diǎn)在于對(duì)環(huán)境反應(yīng)迅速，是一種直接的路徑搜索算法，因此被廣泛應(yīng)用于路徑規(guī)劃問(wèn)題.

3 實(shí)驗(yàn)與分析

整體實(shí)驗(yàn)步驟如下：

Step 1：建立虛擬井下復(fù)雜地形的柵格地圖及改進(jìn)柵格地圖模型，并對(duì)程序進(jìn)行初始化；

Step 2：在模型中選定搜索路徑的起始點(diǎn)和終點(diǎn)，確定路徑規(guī)劃方向；

Step 3：將Dijkstra算法分別運(yùn)用到柵格地圖及改進(jìn)柵格地圖中，對(duì)網(wǎng)格中的節(jié)點(diǎn)進(jìn)行搜索，分別計(jì)算工作代價(jià)，進(jìn)行比較并得出結(jié)論；

Step 4：將A*算法運(yùn)用到改進(jìn)柵格地圖中，計(jì)算工作代價(jià)，與原算法進(jìn)行比較得出結(jié)論；

Step 5：得出最終實(shí)驗(yàn)結(jié)果.

該文采用Matlab R2018b軟件進(jìn)行仿真，工作環(huán)境為70×70的二維地圖模型，并隨機(jī)分布障礙物，用“O”號(hào)表示.在該環(huán)境之上分別建立柵格地圖及改進(jìn)柵格地圖模型，如圖1所示.

圖1 建立地圖模型

首先，采用Dijkstra算法分別在柵格地圖和改進(jìn)柵格地圖上進(jìn)行路徑規(guī)劃，結(jié)果如圖2所示.

圖2 Dijkstra算法在兩種地圖上的路徑規(guī)劃

由圖2(a)可知，使用Dijkstra算法雖然能夠讓機(jī)器人在柵格地圖上完成從起始點(diǎn)到終點(diǎn)的路徑規(guī)劃，但是它僅僅考慮了如何避開障礙物，而忽略了路徑規(guī)劃過(guò)程中搜索的區(qū)域及路徑的長(zhǎng)短問(wèn)題，因此路徑規(guī)劃的時(shí)間較長(zhǎng)，規(guī)劃出的路徑存在冗余，導(dǎo)致機(jī)器人工作代價(jià)很大.由圖2(b)可知,將Dijkstra算法運(yùn)用在改進(jìn)柵格地圖上進(jìn)行路徑規(guī)劃時(shí)，雖然機(jī)器人也對(duì)全局進(jìn)行了搜索，但是在行進(jìn)過(guò)程中，綜合考慮了三角網(wǎng)格和柵格共同檢測(cè)出的障礙物信息，使得機(jī)器人規(guī)劃出來(lái)的路線相比于柵格地圖上規(guī)劃出的路線減少了很多.

改變機(jī)器人路徑規(guī)劃的起點(diǎn)與終點(diǎn)位置，重復(fù)進(jìn)行實(shí)驗(yàn)來(lái)驗(yàn)證上述結(jié)論.隨機(jī)選取其中的4組實(shí)驗(yàn)數(shù)據(jù)作比較，結(jié)果如表1所列，可以看出機(jī)器人在改進(jìn)后的地圖模型上規(guī)劃的路線長(zhǎng)度相比柵格地圖減少了近四分之一.

表1 兩種地圖模型上規(guī)劃路線的長(zhǎng)度比較

考慮到Dijkstra算法的完全泛化特點(diǎn)，其計(jì)算精度很高，能夠避免局部最優(yōu)陷阱，100%求解最優(yōu)路徑.由于需要對(duì)所有節(jié)點(diǎn)進(jìn)行檢查，導(dǎo)致計(jì)算速度大大降低，因此，為了優(yōu)化路徑規(guī)劃的時(shí)間及長(zhǎng)度，進(jìn)一步驗(yàn)證該改進(jìn)地圖的可行性及正確性，運(yùn)用A*算法實(shí)現(xiàn)在改進(jìn)柵格地圖上的路徑規(guī)劃，實(shí)驗(yàn)結(jié)果如圖3所示.

圖3 兩種算法路徑規(guī)劃結(jié)果

通過(guò)圖3兩種路徑規(guī)劃算法的結(jié)果圖來(lái)看，Dijkstra算法遍歷了地形中從起點(diǎn)到終點(diǎn)的所有節(jié)點(diǎn)，在不考慮規(guī)劃時(shí)間的情況下得到了一條最優(yōu)路徑.與此相比，A*算法則考慮了工作代價(jià)，在得到最優(yōu)路徑的情況下大大縮短了規(guī)劃時(shí)間，說(shuō)明在綜合考慮路徑規(guī)劃時(shí)間和工作代價(jià)的情況下，A*算法規(guī)劃出來(lái)的路徑更加符合實(shí)際需要.改變機(jī)器人路徑規(guī)劃起點(diǎn)與終點(diǎn)位置，重復(fù)進(jìn)行實(shí)驗(yàn)來(lái)驗(yàn)證結(jié)論，并隨機(jī)選擇4組實(shí)驗(yàn)數(shù)據(jù)進(jìn)行對(duì)比，如表2所列.

表2 兩種算法路徑規(guī)劃時(shí)間結(jié)果比較

最終結(jié)果表明，A*算法能有效運(yùn)用到改進(jìn)柵格地圖中，獲得最優(yōu)路徑，充分考慮兩點(diǎn)之間路徑的可通過(guò)性，有效降低了運(yùn)行時(shí)間，提高了對(duì)最優(yōu)路徑的搜索能力，驗(yàn)證了改進(jìn)柵格地圖的可行性及正確性.

4 結(jié)論

針對(duì)Dijkstra算法在柵格地圖上的路徑規(guī)劃過(guò)程中存在規(guī)劃路線長(zhǎng)、工作代價(jià)大等問(wèn)題，提出一種改進(jìn)柵格地圖的煤礦救援機(jī)器人路徑規(guī)劃方法.采用三角網(wǎng)格地圖與柵格地圖相結(jié)合的地圖構(gòu)建方法，綜合考慮障礙物信息，實(shí)現(xiàn)了對(duì)機(jī)器人規(guī)劃路線的優(yōu)化.針對(duì)上述兩種地圖中出現(xiàn)的遍歷時(shí)間過(guò)長(zhǎng)的問(wèn)題，采用A*算法進(jìn)一步解決，很大程度上減小了工作代價(jià).實(shí)驗(yàn)結(jié)果表明改進(jìn)的柵格地圖能有效處理復(fù)雜的煤礦井下環(huán)境，減少路徑規(guī)劃長(zhǎng)度及時(shí)間，針對(duì)各種算法都具有一定的效果.