礦用機器人局部路徑優(yōu)化算法研究

武 達，王然風，付 翔，梁 毅

(太原理工大學，山西 太原 030024)

科學技術(shù)的發(fā)展使得煤礦搜救機器人的研究越來越深入。機器人代替人工探測井下復雜的環(huán)境，完成搜索和救援任務(wù)，減少了勞力，大大提高了工作效率。但是煤礦井下復雜，當發(fā)生礦難時，為了使機器人能夠達到指定地點展開作業(yè)，要求機器人必須具備一定的避障和路徑規(guī)劃能力[1，2]。

目前有關(guān)煤礦井下機器人路徑規(guī)劃的研究可歸納為以下幾類。文獻[3]提出了基于梯度-坐標輪換法的煤礦搜救機器人最優(yōu)路徑規(guī)劃算法，雖然可以規(guī)劃出從起點到目標點的最佳路徑，但是沒有考慮到機器人需要轉(zhuǎn)彎的問題，容易與障礙物發(fā)生摩擦[4]。文獻[5]提出了蟻群算法優(yōu)化的Dijkstra算法煤礦機器人路徑規(guī)劃方法，雖然可以在滿足收斂速度條件和安全距離的前提下找到最優(yōu)路徑，但是Dijkstra算法遍歷節(jié)點多，導致算法計算量大，效率低。文獻[6]提出了改進人工勢場法來完成局部機器人避障，來達到局部路徑規(guī)劃的目的。文獻7通過改進A*算法來使得機器人尋求最優(yōu)路徑，方法是改進了傳統(tǒng)A*算法的評價函數(shù)。

為了解決上述問題，本文針對傳統(tǒng)的A*算法進行改進，設(shè)計了基于JPS-A*的算法，該方法主要是針對傳統(tǒng)的A*算法在尋找路徑的過程中對大量無用的節(jié)點進行計算浪費內(nèi)存導致算法效率低的問題進行了優(yōu)化，改進后的算法刪除了不必要的節(jié)點，對篩選出的跳點直接進行操作，大大提高了算法尋找最優(yōu)路徑的效率，達到了局部路徑優(yōu)化的目的[7，8]。

1 礦用機器人控制器總體方案設(shè)計

在整個礦用機器人的設(shè)計當中，控制器是核心，機器人的所有功能都是基于控制器展開的，本文設(shè)計的礦用機器人具有障礙物檢測、自動避障導航等功能，根據(jù)上述所說，本套控制器的硬件部分包括：電源模塊、微控制器模塊、障礙物檢測模塊、電機驅(qū)動模塊、CAN通信模塊等。由于采用的是模塊化編程，所以各功能相對獨立，方便二次開發(fā)與改造升級。

2 控制系統(tǒng)硬件和軟件設(shè)計

2.1 微控制器選擇

主控芯片是由TI公司推出的TMS320C28x系列32位浮點DSP處理器，型號是TMS320f28335，它的工作頻率最高是150MHz，內(nèi)置有256k×16bit的閃存和34k×16bit的SRAM，帶有USB、CAN、串口等豐富的外設(shè)，具有價格低廉，運算速度塊、開法周期短等優(yōu)點。

2.2 障礙物檢測模塊

2.2.1 超聲波傳感器

本文選用的超聲波傳感器型號是HC-SR04，它的檢測范圍是2～400cm，精度控制在3cm左右。它的基本工作原理是：采用IO口TRIG觸發(fā)測距，給最少是10μs的高電平信號，然后模塊會自動發(fā)送8個頻率為40kHz的方波，然后檢測是否有信號返回，一旦系統(tǒng)檢測有信號返回，ECHO引腳就會輸出一個高電平，然后這個信號會返回給主控制器，高電平的持續(xù)時間是超聲波從發(fā)射到返回的時間，把這個時間記為Δt，然后可以算出機器人距離障礙物的距離X。計算公式如下：

X=c×Δt/2

(1)

式中，c為聲波速度，取331.4m/s(空氣溫度為0℃)。

2.2.2 紅外光電傳感器

紅外光電傳感器又稱為光電開關(guān)，電信號輸入后轉(zhuǎn)換為光信號輸出，照射到障礙物后反射回來，接收器通過接收到的紅外線的強弱和有無來進行探測。本文選擇的光電開關(guān)型號是E18-D80NK，它的測量距離是3～80cm，具有測量距離遠、抗干擾信號能力強、易于安裝等優(yōu)點。

這個紅外開關(guān)的工作原理是：在正常狀態(tài)下，光電開關(guān)連接的IO口輸出為高電平，一旦檢測到障礙物時間，即輸出低電平，將相關(guān)IO口設(shè)置為下降沿觸發(fā)中斷，一旦發(fā)生中斷，信號便會傳到主控制器，通過程序判斷障礙物的方向，從而采取相應的措施進行避障。

2.3 外界環(huán)境信息采集模塊

礦用機器人要進行躲避障礙物，進行路徑規(guī)劃，首先要了解周圍的信息，本文采用普通消費級視頻攝像頭來得到周圍環(huán)境的圖像，它的圖像分辨率是640×480，視頻每秒傳輸?shù)膸瑪?shù)為30，通過CAN通訊接口傳遞到機器人CPU進行處理，使得機器人知道起始點、障礙物的位置以及終點。

3 路徑規(guī)劃算法設(shè)計

3.1 A*尋路算法

A*算法是靜態(tài)網(wǎng)絡(luò)中求解最短路徑的直接搜索算法，它將Dijkstra算法和BFS算法的搜索策略結(jié)合在一起，在保證尋得路徑最優(yōu)的前提下，采用啟發(fā)式搜索。A*算法通過評估函數(shù)來確定路徑搜索方向，它的評估函數(shù)表示為：

f(m)=g(m)+h(m)

(2)

式中，f(m)表示從起始點到達目的點的評價函數(shù)；g(m)表示從起始點到達目的點的實際代價；h(m)表示從起始點到達目標點的估計代價。本文啟發(fā)式函數(shù)h(m)采用歐幾里得來測量兩點的估計代價：

對于評估函數(shù)f(m)，當h(m)=0時，原式退化成Dijkstra算法，在搜索路徑時，需要計算大量的節(jié)點；當g(m)=0時，原式變成計算從起始點到目標點的BFS算法，雖然計算速度塊，但容易進入死胡同，不便于得到最優(yōu)解。A*算法朝著目的地點的方向進行搜索，對搜索方向上的點進行計算，當達到某一個點時，該點周圍的點會被加入到OpenList中，A*算法會選擇具有最小估價值的點作為下一個拓展節(jié)點，同時該點會被加入ClosedList，然后不斷循環(huán)這個過程，直到將目的地點加入OpenList。

但是傳統(tǒng)的A*算法存在著問題，雖然可以尋找到最優(yōu)路徑，然而尋路過程中，產(chǎn)生了大量不必操作的節(jié)點，算法要對這些節(jié)點進行不斷的訪問和計算，當起始點到目的地的路徑過于長時，A*算法會呈現(xiàn)出計算量大、大量占用計算機內(nèi)存、效率低下等問題。

3.2 跳點搜索算法

跳點搜索算法(jump point search，JPS)是一種搜索跳點的策略，跳點就是在搜索過程中產(chǎn)生的一些節(jié)點，這些節(jié)點可以直接跳躍。尋路過程節(jié)點的擴展過程如圖1所示，X是起點，Y是終點，從X到Y(jié)的路徑有：X→b2→b3→Y，X→d2→d3→Y，X→b1→a2→a3→a4→b4→Y，X→c2→c3→Y，在A*算法尋路的過程中，算法會對路徑中經(jīng)過的節(jié)點進行評估，選取代價值最小的節(jié)點。由以上給出的路徑，發(fā)現(xiàn)除了節(jié)點c2和c3，對其它節(jié)點的評估是沒有必要的，由圖1明顯得出路徑X→c2→c3→Y永遠是最短的。跳點搜索算法就是濾除掉路徑上不必要的節(jié)點，而只對某點節(jié)點進行評估，而這些被評估的節(jié)點被稱為跳點。圖1中的X和Y節(jié)點是兩個跳點，這兩個跳點中間的節(jié)點將不會被擴展，這也就大大降低了在尋路過程中對不必要節(jié)點的評估，減少了計算量和內(nèi)存消耗，增加了尋找最優(yōu)路徑的效率。

圖1 尋路過程節(jié)點的擴展過程

3.2.1 跳點的篩選

對A*算法的改進主要是針對跳點的篩選，結(jié)合參考文獻[9]提出的幾條修剪不必要節(jié)點的規(guī)則，本文根據(jù)現(xiàn)場條件做出了修改，對以下兩種情況做出討論。

1)節(jié)點周圍不存在障礙物。水平方向擴展如圖2所示，S(x)是X的父節(jié)點，Z是圖上的任意節(jié)點，要規(guī)劃一條從S(x)到Z的路徑，對所有情況進行分析可知，一種是經(jīng)過X向右擴展到達Z節(jié)點，另一種是不經(jīng)過X節(jié)點，進而由S(x)向上下擴展到達Z節(jié)點。明顯可以看出，在所有路徑中經(jīng)過X節(jié)點的路徑是最短的，但是A*算法經(jīng)常訪問S(x)→X周圍的節(jié)點，計算量大且沒有意義，所以要篩選掉圖中的灰色柵格進而提高算法的運算速度。由圖2中看出，在達到目標節(jié)點Z之前，S(x)→X擴展方向上的鄰節(jié)點都可以由S(x)直接到達，若是經(jīng)過X再達到這些鄰節(jié)點，路徑就會變得復雜，所以這些節(jié)點被認為是擴展過程中的不必要節(jié)點。因此本文給出在如圖2所示情況下的篩選節(jié)點條件：

l({S(x)，…，Z|X})≤l({S(x)，X，Z})

(4)

函數(shù)中的l(x)表示路徑的長度，{S(x)，…，Z|X}表示從S(x)為起始點、Z為目標點且不經(jīng)過X的路徑，{S(x)，X，Z}表示這條路徑經(jīng)過X到達目標點Z。同理給出在對角線方向擴展跳點的篩選條件：

l({S(x)，…，Z|X})

(5)

斜對角線方向擴展如圖3所示，篩選掉不必要節(jié)點后，當路徑擴展到X節(jié)點時，X周圍的相鄰節(jié)點定義為X的自然鄰節(jié)點。

圖2 水平方向擴展

圖3 斜對角線方向擴展

2)節(jié)點周圍存在障礙物。節(jié)點周圍存在障礙物時如圖4所示，給出篩選條件：①Z不是X的自然鄰節(jié)點；②l({S(x)，…，Z|X})>l({S(x)，X，Z})滿足篩選條件的X的鄰節(jié)點a3和a1定義為強制鄰節(jié)點，在路徑擴展過程中它們都將被視為跳點。

圖4 節(jié)點周圍存在障礙物

3.2.2 A*算法的改進

改進A*算法的路徑尋路過程如圖5所示，在滿足節(jié)點周圍存在障礙物篩選條件下，刪除不必要節(jié)點后，然后再執(zhí)行A*算法，進而得到最優(yōu)路徑。

圖5 改進后的A*算法尋路圖

在地圖柵格中，S表示起始節(jié)點，Z表示目標節(jié)點，首先將S節(jié)點添加到OpenList，然后沿著水平方向和豎直方向擴展，直到遇到障礙物或者地圖邊緣，然后沿著對角線擴展到下一節(jié)點，然后繼續(xù)沿著水平和豎直方向擴展，不斷重復上述過程，直到在水平方向發(fā)現(xiàn)一個強制節(jié)點，這時候?qū)娭乒?jié)點添加到OpenList，繼續(xù)擴展，直到發(fā)現(xiàn)下一個強制節(jié)點，重復上述過程，直到發(fā)現(xiàn)目標點，停止迭代，將目標點加入到OpenList，得到最終路徑[10-12]。

4 仿真分析與實驗驗證

4.1 環(huán)境搭建

為了更好的驗證改進后的A*算法的合理性，使用MATLAB進行路徑的仿真分析測試。由實際仿真知，當柵格地圖較小時，改進后的A*算法和傳統(tǒng)的A*算法差別不是特別明顯，當柵格地圖為60×60以及更大尺寸時，改進后的A*算法的搜索速度得到了較大程度的提升。

4.2 MATLAB仿真結(jié)果與分析

在15×15的柵格地圖進行的仿真實驗如圖6和圖7所示，其中的黑色部分表示障礙物，藍色代表起始點，紫色代表目標點，灰色的柵格表示算法在尋路過程中曾添加到Openlist的點。由圖6、圖7看出改進后的A*算法生成的路徑和傳統(tǒng)的A*算法基本一致，但是改進后的A*算法尋路過程中操作的節(jié)點數(shù)大大減少。其他尺寸的柵格地圖仿真分析結(jié)果見表1。

圖6 傳統(tǒng)A*算法

圖7 改進后的A*算法

由表1得出，改進后的A*算法較傳統(tǒng)的A*算法在搜索時間和擴展節(jié)點數(shù)量大大減少，從而提高了搜索路徑的效率，降低了對CPU的內(nèi)存消耗，滿足了井下機器人躲避障礙物應用的條件。

表1 優(yōu)化A*算法與傳統(tǒng)A*算法數(shù)據(jù)對比

4.3 實驗驗證

將改進后的A*算法應用到基于TMS320C28x系列32位浮點DSP處理器的移動機器人上，通過機器人掛載的傳感器來獲取外界環(huán)境信息，傳感器的位置如圖8所示。

圖8 試驗樣機

為了驗證改進后的A*算法的時效性，預先在樓道設(shè)計好機器人的起始點以及目標點，然后機器人在已設(shè)定好的路線上進行路徑規(guī)劃試驗，機器人在不同路徑下的尋路長度以及尋路時間見表2，實驗結(jié)果表明結(jié)合JPS算法改進的A*算法較傳統(tǒng)的A*算法尋路時間大大縮短，因而計算速度更快，尋路效率更高。

表2 不同路徑下2種算法尋路時間及路徑長度對比

5 結(jié) 語

礦用機器人在井下開展作業(yè)要進行局部路徑規(guī)劃，本文結(jié)合了JPS算法的優(yōu)勢，改進了傳統(tǒng)的A*算法遍歷節(jié)點多、計算量大、時間久等問題，仿真表明改進后的算法在滿足躲避障礙物的前提下能夠大大提高尋求最優(yōu)路徑的效率，可以滿足實際需求，且優(yōu)化后的效果明顯，對礦用機器人的發(fā)展有指導意義。