改進柵格地圖的煤礦救援機器人路徑規劃研究

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

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

煤礦事故后的救援工作充滿了挑戰和危險，考慮到救援人員的安全以及救援任務的緊迫性，有必要使用煤礦救援機器人來執行救援任務[1-2].

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

該文結合前人的研究成果，提出一種改進柵格地圖的煤礦救援機器人路徑規劃方法.首先構建障礙物的改進柵格地圖模型；其次，采用Dijkstra算法實現在地圖模型中的路徑規劃，驗證該地圖模型的可行性；在此基礎之上，運用A*算法實現在改進柵格地圖上的運行，得到最終的優化路徑.

1 改進柵格地圖建模

現實中物體的表面從直觀上看是由許多曲面所構成，這些曲面包括三角形、四邊形及多邊形等，由于任意的多邊形網格實際上也能細分為三角形，且三角形簡單而更容易操作，因此本文采用三角網格剖分改進柵格地圖.

采用三角剖分將測試地圖構建成三角網格地圖，該地圖可以用單元連接矩陣C及節點矩陣P表示.設三角剖分后的地圖中有m個節點和k個單元，則單元連接矩陣可以表示為：

(1)

將第i個節點的坐標表示為Pi(xi,yi,zi)，則節點矩陣可以表示為：

(2)

(3)

(4)

節點連接矩陣T和S的節點為網格節點號.T中的第i個元素ti與S中的第i個si是相關聯的，且表示節點ti與節點si連接.由于節點之間的距離不同，因此將相關矩陣T與S之間的距離矩陣定義為D：

(5)

其中:di表示節點關聯矩陣T中第i個節點ti與S的第i個節點與節點si的距離.文中引入加權系數，定義

(6)

其中:hi為節點si與節點ti的差;di為節點si與節點ti的距離.由距離矩陣及加權系數，可以定義權重矩陣：

(7)

因此，節點關聯矩陣T、S和權重矩陣W可以構成稀疏矩陣表示的賦權無向連通圖矩陣：

G=sparse(T,S,W).

(8)

而矩陣中基于三角形網格的映射矩陣map可以表示為節點矩陣和無向連通圖矩陣組成的關聯矩陣：

map=(P,G).

(9)

2 改進柵格地圖路徑規劃算法

2.1 Dijkstra算法

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

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

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

Step3：以s為新考慮的中間點，修改U中各頂點的距離；若從源點v1到頂點vi的距離(經過頂點s)比原來距離(不經過頂點s)短，則修改頂點vi的距離值，修改后的經過頂點s的最短距離則為該邊的權值；

Step4：重復步驟Step2和Step3直到所有頂點都包含在K中.

2.2 A*算法

A*算法是在Dijkstra算法的基礎上加入啟發式函數，利用啟發信息尋找最優路徑.A*算法需要在地圖中搜索節點，并設定適合的啟發函數進行指導.通過評價各個節點的代價值，獲取下一需要拓展的最佳節點，直至到達最終目標點位置.其公式為：

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

(10)

其中，g(n)表示從起始點到當前節點n的最小工作代價，h(n)表示當前節點n到終點的最小估計代價.當h(n)為0時，A*算法也就變成了Dijkstra算法.A*算法優點在于對環境反應迅速，是一種直接的路徑搜索算法，因此被廣泛應用于路徑規劃問題.

3 實驗與分析

整體實驗步驟如下：

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

Step 2：在模型中選定搜索路徑的起始點和終點，確定路徑規劃方向；

Step 3：將Dijkstra算法分別運用到柵格地圖及改進柵格地圖中，對網格中的節點進行搜索，分別計算工作代價，進行比較并得出結論；

Step 4：將A*算法運用到改進柵格地圖中，計算工作代價，與原算法進行比較得出結論；

Step 5：得出最終實驗結果.

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

圖1 建立地圖模型

首先，采用Dijkstra算法分別在柵格地圖和改進柵格地圖上進行路徑規劃，結果如圖2所示.

圖2 Dijkstra算法在兩種地圖上的路徑規劃

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

改變機器人路徑規劃的起點與終點位置，重復進行實驗來驗證上述結論.隨機選取其中的4組實驗數據作比較，結果如表1所列，可以看出機器人在改進后的地圖模型上規劃的路線長度相比柵格地圖減少了近四分之一.

表1 兩種地圖模型上規劃路線的長度比較

考慮到Dijkstra算法的完全泛化特點，其計算精度很高，能夠避免局部最優陷阱，100%求解最優路徑.由于需要對所有節點進行檢查，導致計算速度大大降低，因此，為了優化路徑規劃的時間及長度，進一步驗證該改進地圖的可行性及正確性，運用A*算法實現在改進柵格地圖上的路徑規劃，實驗結果如圖3所示.

圖3 兩種算法路徑規劃結果

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

表2 兩種算法路徑規劃時間結果比較

最終結果表明，A*算法能有效運用到改進柵格地圖中，獲得最優路徑，充分考慮兩點之間路徑的可通過性，有效降低了運行時間，提高了對最優路徑的搜索能力，驗證了改進柵格地圖的可行性及正確性.

4 結論

針對Dijkstra算法在柵格地圖上的路徑規劃過程中存在規劃路線長、工作代價大等問題，提出一種改進柵格地圖的煤礦救援機器人路徑規劃方法.采用三角網格地圖與柵格地圖相結合的地圖構建方法，綜合考慮障礙物信息，實現了對機器人規劃路線的優化.針對上述兩種地圖中出現的遍歷時間過長的問題，采用A*算法進一步解決，很大程度上減小了工作代價.實驗結果表明改進的柵格地圖能有效處理復雜的煤礦井下環境，減少路徑規劃長度及時間，針對各種算法都具有一定的效果.