基于新型柵格啟發(fā)式算法的礦井機器人路徑規(guī)劃

王鶴， 陳靜， 滕瑛瑤

(河南工程學(xué)院 機械工程學(xué)院， 河南 鄭州 451191)

0 引言

為了避免煤礦次生災(zāi)害對被困礦工及救護人員造成嚴重傷害，進一步提高救援工作效率和成功率，機器人路徑規(guī)劃問題已成為礦井類機器人研究的核心內(nèi)容之一[1-2]。為了使機器人可以準確找到事故發(fā)生地點，并確認安全路徑營救被困礦工，尋找一條距離最短、耗時最少的路徑非常重要[3-4]。

在研究機器人路徑規(guī)劃[5-7]時，通常將其運動區(qū)域抽象簡化成若干個形狀規(guī)則、大小相同的柵格，以柵格的特征信息描述實際地形。柵格法[8-11]對障礙物的適應(yīng)能力較強，大大降低了工作環(huán)境建模的復(fù)雜性。研究者通常選擇正方形作為柵格形狀，在正方形柵格化的工作地圖中，當機器人位于某個柵格時，沿水平或豎直方向和沿對角線方向移動。在實際路徑規(guī)劃中，當遇到障礙物時，若沿對角線方向移動，易與障礙物發(fā)生碰撞；機器人在繞障和平穩(wěn)性等方面的能力較差[12-13]；礦難發(fā)生后在實時探測過程中每步消耗的時間無法唯一確定，這對救援造成重大影響。針對上述問題，本文提出了以正六邊形柵格化的工作地圖為基礎(chǔ)，結(jié)合改進的啟發(fā)式路徑搜索算法對多個并行移動的機器人進行路徑規(guī)劃的方法，能夠有效減少井下多個機器人并行工作的路徑長度及探測救援時間，為礦難發(fā)生后救援工作的開展爭取了寶貴時間。

1 2種地圖模型對比分析

將機器人的工作空間分別進行正方形和正六邊形柵格化，如圖1所示。圖中圓圈為機器人，白色柵格為自由柵格，黑色柵格為靜態(tài)障礙物。在正六邊形柵格(即蜂窩柵格)化的工作地圖中，機器人可移動的方向最多有6個，如圖1(b)所示，即使相鄰柵格有障礙物，沿其余任一方向移動時，也不會出現(xiàn)在正方形柵格中沿對角線方向移動可能與障礙物發(fā)生碰撞的情況，安全性更高。

(a) 正方形

(b) 正六邊形

假設(shè)在同一工作環(huán)境下的正方形和正六邊形地圖中，單個機器人移動1個柵格位置的距離相同，對其運動性能做比較。

(1) 繞障轉(zhuǎn)角。在遇到靜態(tài)障礙物時，機器人在傳統(tǒng)的正方形柵格地圖中移動時，轉(zhuǎn)角為90°，而在正六邊形柵格地圖中的轉(zhuǎn)角僅為60°，如圖2所示?？梢姡斢龅届o態(tài)障礙物需要轉(zhuǎn)彎時，在正六邊形柵格地圖中轉(zhuǎn)角較小，能夠提高機器人運動的安全性[7-8]。為提高運動平穩(wěn)性，在路徑規(guī)劃過程中，不僅要減小轉(zhuǎn)角，也要盡可能減少機器人轉(zhuǎn)向次數(shù)。

(a) 正方形柵格

(b) 正六邊形柵格

(3) 最優(yōu)路徑。圖3為同一工作空間的正方形和正六邊形柵格地圖。最優(yōu)路徑以付出代價來衡量，付出代價越少，則路徑最優(yōu)。對于單個機器人路徑規(guī)劃來說，其路徑越短，耗時也越短。因此單個機器人路徑規(guī)劃目標就是其路徑長度代價最小。設(shè)機器人移動1個柵格的路徑長度代價為w，那么1條路徑的長度代價為nw(n為自然數(shù))。對于正方形柵格地圖，1條路徑的長度代價即起點與終點之間的曼哈頓距離[14-15]。在正六邊形柵格中，從起點開始沿靠近終點且與水平相交60°方向移動，直至移動到與終點同一行的柵格，將該柵格與起點之間的距離定義為LS，該柵格和終點之間的距離定義為LD。在正六邊形柵格地圖中，將上述2個距離之和L=LS+LD定義為變形曼哈頓距離。

(a) 正方形柵格

(b) 正六邊形柵格

假設(shè)將機器人分別置于起點柵格S1和S2，令其分別移動到終點柵格D1和D2，2種地圖中起點和對應(yīng)終點之間的直行距離相同。建立路徑網(wǎng)絡(luò)集合N={N1,N2}，其中Ni={Si,Di}，i=1,2。機器人在2種地圖中的路徑長度代價見表1。

表1 機器人路徑長度代價Table 1 Path length cost of robot

由表1可知，就單個機器人來說，正六邊形柵格地圖下的路徑長度代價小于正方形柵格地圖的路徑長度代價，從單個機器人的路徑規(guī)劃來看，正六邊形柵格地圖更有利于獲得最短路徑。

通過3個性能方面的分析比較，得出正六邊形柵格比傳統(tǒng)正方形柵格更適合于機器人工作環(huán)境的建模。

2 多個協(xié)同操作的機器人啟發(fā)式路徑搜索算法

本文主要研究多個協(xié)同操作的機器人并行移動的路徑規(guī)劃問題，在正六邊形柵格地圖建模的基礎(chǔ)上，采用改進的啟發(fā)式路徑搜索算法對多個機器人的路徑進行優(yōu)化。

多個協(xié)同操作的機器人路徑規(guī)劃目標是在避免機器人之間碰撞及機器人與障礙物碰撞的前提條件下，最小化每個機器人的路徑長度(即路徑上的柵格數(shù)量)和消耗時間。假設(shè)有m個機器人，建立路徑網(wǎng)絡(luò)集合N={N1,N2,…,Ni}，i=1,2,…,m，路徑長度由該路徑上所用柵格總數(shù)乘以w來表示，則有

(1)

式中：(xit,yit),(xjt,yjt)分別為第i個和第j個機器人在t時刻的柵格位置，j=1,2,…,m；(xit+1,yit+1),(xjt+1,yjt+1)分別為第i個和第j個機器人在t+1時刻的柵格位置；g(Ni)為第i個機器人路徑上的柵格數(shù)量。

采用改進的啟發(fā)式估計函數(shù)來規(guī)劃多個協(xié)同操作的機器人路徑，該函數(shù)決定了當前機器人所在位置所有相鄰柵格中哪一個即將被機器人遍歷。依據(jù)機器人已經(jīng)遍歷的柵格數(shù)Ztn和候選柵格與該機器人目標柵格之間的變形曼哈頓距離L，該啟發(fā)式估計函數(shù)可評估出相鄰柵格的適應(yīng)度值Gf，函數(shù)表達式為

Gf=Ztn+L

(2)

多個協(xié)同操作的機器人啟發(fā)式路徑搜索算法流程如圖4所示。

圖4 多個協(xié)同操作的機器人啟發(fā)式路徑搜索算法流程Fig.4 Flow of heuristic path search algorithm of multiple collaborative robots

(1) 設(shè)置參數(shù)，建立正六邊形柵格化的二維環(huán)境地圖，m個機器人的路徑網(wǎng)絡(luò)集合N={N1,N2,…,Ni}，各個機器人起點、終點及所有障礙物坐標，并初始化路徑上遍歷的柵格數(shù)Ztn=0和機器人路徑上遍歷的柵格集合為?。

(2) 為當前機器人Bi隨機生成2條路徑R1,R2。

(3) 比較2條路徑上的障礙物總數(shù)，選擇障礙物總數(shù)少的路徑。

(4) 將起點柵格初始化為當前柵格。

(5) 判斷是否遇到障礙物。若沒有遇到，則按先前隨機生成的路徑R1和R2繼續(xù)移動；若遇到，則判斷是靜態(tài)障礙物還是動態(tài)障礙物。

如果障礙物是靜態(tài)障礙物，則在當前機器人Bi所處柵格創(chuàng)建節(jié)點，并計算當前節(jié)點柵格所有相鄰柵格(障礙物除外)與當前機器人Bi的終點柵格Di之間的變形曼哈頓距離L及對應(yīng)路徑上的轉(zhuǎn)向次數(shù)，由此選擇Gf最小且轉(zhuǎn)向次數(shù)最少的相鄰柵格作為下一步移動的位置。如果障礙物是動態(tài)障礙物，即其他機器人Bj，分別在Bi和Bj所在柵格創(chuàng)建1個節(jié)點，并計算節(jié)點柵格所有相鄰柵格(Bj占據(jù)的柵格除外)與各自的終點柵格Di或Dj之間的變形曼哈頓距離Li和Lj及對應(yīng)路徑上的轉(zhuǎn)向次數(shù)。如果Li

(6) 移動到下一個柵格，并判斷是否到達終點，如果是則停止尋址；否則返回步驟(5)，直至到達終點為止。

(7) 停止尋址，輸出最優(yōu)路徑。

3 仿真實驗及結(jié)果分析

為了驗證基于正六邊形柵格地圖的多機器人啟發(fā)式路徑搜索算法的可行性和有效性，采用Matlab對不同數(shù)量機器人的路徑規(guī)劃進行仿真。為了使機器人在2種環(huán)境地圖中的路徑具有可比性，將地圖中所有靜態(tài)障礙物看作圓形，在2種地圖中，每個障礙物的位置和半徑都相同，任意機器人的起點和終點位置對應(yīng)相同。

機器人在2種地圖中移動1個柵格的距離相同，同一工作空間分別被分割成30×30個正方形柵格和30×35個正六邊形柵格。3個機器人并行移動時的路徑如圖5所示。仿真收斂曲線如圖6所示。不同數(shù)量機器人并行移動的路徑規(guī)劃仿真結(jié)果見表2。

由圖5與圖6可知，正六邊形柵格地圖在路徑總長及算法的運行時間上均比正方形柵格地圖減少了10%以上，且有效避免了機器人與靜態(tài)障礙物之間及機器人之間發(fā)生碰撞，提高了機器人的安全性。另外，在算法中引入了轉(zhuǎn)向次數(shù)，明顯提高了機器人運動的平穩(wěn)性。

由表2可知，隨著機器人數(shù)量增多，改進的啟發(fā)式路徑搜索算法對正六邊形柵格地圖下的機器人路徑和算法運行時間的優(yōu)化作用更加明顯。因此，正六邊形柵格地圖與改進的啟發(fā)式路徑搜索算法的結(jié)合，可實現(xiàn)對多個并行移動機器人路徑的優(yōu)化，且效果顯著。

(a) 正方形柵格地圖下的路徑

(b) 正六邊形柵格地圖下的路徑

圖6 機器人路徑仿真收斂曲線Fig.6 Convergence curves of robot path simulation

表2 不同數(shù)量機器人并行移動的路徑規(guī)劃仿真結(jié)果Table 2 Simulation results of path planning for parallel mobile of different number of robots

4 結(jié)論

(1) 從繞障轉(zhuǎn)角、繞障能力及最優(yōu)路徑3個方面，對單個機器人在正方形和正六邊形柵格地圖中進行比較分析。在正六邊形柵格地圖中，遇到靜態(tài)障礙物轉(zhuǎn)彎時，轉(zhuǎn)角和繞行時間都減小，提高了運動的安全性、平穩(wěn)性及繞障能力，且最優(yōu)路徑長度有所減小。

(2) 正六邊形柵格地圖與改進的啟發(fā)式路徑搜索算法的結(jié)合，實現(xiàn)了對多個并行移動的礦井機器人路徑的優(yōu)化，達到了各機器人路徑最短的優(yōu)化目標，同時避免了機器人之間及機器人與靜態(tài)障礙物之間發(fā)生碰撞。

(3) 仿真結(jié)果表明，正六邊形柵格地圖在路徑總長及算法運行時間上均比正方形柵格地圖減少了10%以上，且有效避免了機器人與靜態(tài)障礙物之間及機器人之間發(fā)生碰撞，提高了機器人的安全性；隨著機器人數(shù)量增多，改進的啟發(fā)式路徑搜索算法對正六邊形柵格地圖下的機器人路徑和算法運行時間的優(yōu)化作用更加明顯。