移動機器人全覆蓋路徑規(guī)劃算法研究

賀利樂，劉小羅，黃天柱，楊劍樂

（1.西安建筑科技大學(xué)機電工程學(xué)院，陜西 西安 710055；2.中建建樂實業(yè)有限公司，陜西 西安 710000）

1 引言

路徑規(guī)劃是移動機器人實現(xiàn)智能化的關(guān)鍵技術(shù)之一，分為點對點路徑規(guī)劃和全覆蓋路徑規(guī)劃。通常所提的路徑規(guī)劃即點對點路徑規(guī)劃是指，機器人在無碰撞條件下，從起點出發(fā)規(guī)劃出一條到達終點的最短路徑，算法首要評價指標(biāo)為路徑長度。但是某些作業(yè)要求并不適于點對點路徑規(guī)劃，如：室內(nèi)清掃機器人、擦窗機器人、草坪修剪機器人等。這些移動機器人路徑規(guī)劃即全覆蓋路徑規(guī)劃，不僅要求尋找一條從起始位置到終點的無碰撞最短路徑，更重要的是要掃描遍歷整個無障礙工作區(qū)域，最終形成一條在工作區(qū)域內(nèi)從起始位置經(jīng)過所有無障礙區(qū)域的連續(xù)路徑[1]，算法首要評價指標(biāo)為面積覆蓋率。全覆蓋和點對點路徑規(guī)劃最大的區(qū)別在于是否需要遍歷整個工作區(qū)域。

文獻[2-3]提出基于生物激勵神經(jīng)網(wǎng)絡(luò)構(gòu)建環(huán)境模型，并根據(jù)神經(jīng)元活性值大小決定機器人運動方向，該算法實時性好，能夠解決動、靜態(tài)環(huán)境下機器人全覆蓋路徑規(guī)劃問題。但存在神經(jīng)網(wǎng)絡(luò)環(huán)境建模需要機器人每移動一個神經(jīng)元位置環(huán)境模型需更新一次，當(dāng)神經(jīng)元數(shù)量多時，計算量太大、算法運行效率不高的問題。文獻[4]利用單元分解法將環(huán)境地圖劃分為不同大小的區(qū)域，區(qū)域內(nèi)部采用螺旋收縮算法實現(xiàn)遍歷，區(qū)域之間通過圖的深度優(yōu)先搜索算法實現(xiàn)全局遍歷排序，最終達到全覆蓋路徑規(guī)劃的目的，但此方法易產(chǎn)生小分區(qū)，同時由于圖的深度優(yōu)先搜索算法固有缺陷，機器人在實現(xiàn)路徑全覆蓋時軌跡重復(fù)率不高。文獻[5-6]基于動態(tài)柵格法的環(huán)境建模，分別提出優(yōu)先級A*算法和置信度方向函數(shù)實現(xiàn)全覆蓋路徑規(guī)劃，但它們只是單一考慮未覆蓋區(qū)域或者機器人轉(zhuǎn)向，同時當(dāng)機器人從死區(qū)逃離時通常不是最優(yōu)路徑。

針對上述所提算法的不足，這里在構(gòu)建動態(tài)柵格地圖的基礎(chǔ)上，綜合考慮柵格屬性、機器人轉(zhuǎn)向、臨近柵格距離、未覆蓋區(qū)域面積大小和逃離死區(qū)最優(yōu)路徑，提出一種基于改進的優(yōu)先級蟻群算法的移動機器人全覆蓋路徑規(guī)劃策略，以期達到降低路經(jīng)重復(fù)率，減少轉(zhuǎn)彎次數(shù)，提高工作效率。

2 環(huán)境建模

環(huán)境建模是移動機器人全遍歷路徑規(guī)劃的前提和基礎(chǔ)，常用的環(huán)境建模方法主要有：柵格法、單元分解法、拓?fù)鋱D法、MAKLINK 圖論法[7-8]、神經(jīng)網(wǎng)絡(luò)等。這里是在沿邊學(xué)習(xí)的基礎(chǔ)上，采用柵格法進行二維環(huán)境建模，方法具有簡單可靠，易于實現(xiàn)和維護的優(yōu)點。

2.1 沿邊學(xué)習(xí)

沿邊學(xué)習(xí)是移動機器人沿環(huán)境邊界或緊靠環(huán)境邊界的障礙物分別進行水平和垂直運動，通過機器人本體左右側(cè)的測距傳感器檢測環(huán)境或障礙物邊界與機器人的相對距離，實現(xiàn)對工作環(huán)境輪廓學(xué)習(xí)的過程，為后期柵格法環(huán)境建模奠定基礎(chǔ)。對于復(fù)雜環(huán)境，移動機器人可能需要穿越環(huán)境內(nèi)部，才能達到完全學(xué)習(xí)整個工作環(huán)境輪廓的目的。

2.2 動態(tài)柵格法的環(huán)境建模

經(jīng)過環(huán)境輪廓學(xué)習(xí)獲得環(huán)境地圖后，以固定大小將其劃分成若干柵格[9]。柵格根據(jù)是否含障礙和是否被覆蓋，分為障礙柵格、已覆蓋柵格和未覆蓋柵格。為了便于信息存儲和計算處理，每個柵格狀態(tài)由（x，y，f）描述，（x，y）表示柵格在地圖中的位置，f為柵格的屬性值，根據(jù)式（1）給每個柵格置屬性值f。

圖1 柵格地圖Fig.1 Grid Map

動態(tài)柵格是指柵格的屬性值f是變化的。比如，為增加未覆蓋柵格對機器人的“吸引”，避免機器人反復(fù)擦拭同一柵格，約定柵格每被覆蓋一次，其屬性值遞減-1。同時，機器人在工作過程中，只更新其已覆蓋過柵格的屬性值，而不需要類似基于生物激勵的神經(jīng)網(wǎng)絡(luò)環(huán)境模型根據(jù)分流方程[2]對所有神經(jīng)元或者柵格進行迭代計算，極大地減少了計算量，提高了機器人的工作效率。假設(shè)機器人柵格地圖，如圖1 所示。

為便于研究和仿真實驗，作如下假設(shè)：

（1）機器人在運動過程中，障礙物和環(huán)境始終保持不變，即機器人是在靜態(tài)環(huán)境下工作，且障礙物為規(guī)則形狀。

（2）環(huán)境和障礙物邊界已根據(jù)機器人的實際物理尺寸進行“膨化”處理，故障礙物邊界為安全區(qū)域且機器人以質(zhì)點表示。同時，在不存在障礙物和環(huán)境邊界時，機器人可向鄰域8 個方向的柵格運動，如圖1 所示。

（3）機器人覆蓋一個柵格后即認(rèn)為其已按要求完成工作，比如清掃或者擦拭工作。當(dāng)再次覆蓋同一柵格時，屬于重復(fù)遍歷。

3 全覆蓋路徑規(guī)劃算法

基于環(huán)境模型機器人根據(jù)既定優(yōu)先級規(guī)則不斷選擇擬移動?xùn)鸥?，?dāng)機器人陷入死區(qū)時，采用逃逸算法規(guī)劃出迅速逃離死區(qū)的路徑，并繼續(xù)既定任務(wù)，最終達到全覆蓋的目的。在動態(tài)柵格法環(huán)境建模基礎(chǔ)上，優(yōu)先級規(guī)則和逃逸算法直接關(guān)系到機器人的面積覆蓋率和軌跡重復(fù)率，是全覆蓋路徑規(guī)劃研究的重點。

3.1 改進的優(yōu)先級啟發(fā)規(guī)則

文獻[5]所提算法將啟發(fā)式規(guī)則定義為上柵格、下柵格、左柵格優(yōu)先次序，規(guī)則簡單可行，但機器人頻繁轉(zhuǎn)彎，能耗較大。文獻[6]所提算法考慮機器人轉(zhuǎn)向，利用方向函數(shù)策略使得規(guī)劃路徑盡可能保持直行，減少能耗，但這類似于機器人隨機運動方法，機器人易陷入死鎖狀態(tài)。

針對傳統(tǒng)算法中存在的問題，在考慮柵格屬性和機器人轉(zhuǎn)向的基礎(chǔ)上，引入鄰域柵格距離項和未覆蓋區(qū)域面積大小項，提出如式（2）優(yōu)先級啟發(fā)規(guī)則，供機器人在非死區(qū)狀態(tài)下選擇下一移動?xùn)鸥裎恢谩?/p>

式中：fj—柵格j的屬性值，柵格j屬于機器人當(dāng)前柵格i的鄰域柵格。Gcmax—機器人在當(dāng)前柵格時未覆蓋的柵格總數(shù)；Gc—擬移動?xùn)鸥穹较蛞粋?cè)未覆蓋柵格數(shù)量與原運動方向上的未覆蓋柵格數(shù)量之和。當(dāng)擬移動?xùn)鸥竦姆较蚺c原方向一致時，如果除移動方向外不存在未覆蓋柵格，則規(guī)定如果只有一側(cè)存在未覆蓋柵格，Gc為該側(cè)所求Gc減0.5；如果兩側(cè)均存在未覆蓋柵格，Gc為最少一側(cè)所求Gc加0.5。該項的設(shè)置是確保機器人先遍歷未覆蓋柵格數(shù)量最少一側(cè)后轉(zhuǎn)而遍歷另一側(cè)，以期降低機器人的路徑重復(fù)率。wij—當(dāng)前柵格i與擬移動?xùn)鸥駄之間的距離。當(dāng)擬移動?xùn)鸥裨诋?dāng)前柵格的整數(shù)倍方向時取wij＝1.414；當(dāng)擬移動?xùn)鸥裨诋?dāng)前柵格的整數(shù)倍方向時取wij＝1。sj—轉(zhuǎn)向函數(shù)是關(guān)于機器人前一柵格rp、當(dāng)前柵格rc與擬移動?xùn)鸥駌j之間方向角差值△θj的函數(shù)。下式為機器人方向角差值△θj。

式中：（xpp，ypp）、（xpc，ypc）和（xpj，ypj）—前一柵格rp、當(dāng)前柵格rc與擬移動?xùn)鸥駌j的位置坐標(biāo)，方向角計算簡圖，如圖2 所示。θj—擬移動?xùn)鸥駌j和當(dāng)前柵格rc的角度差。θc—當(dāng)前柵格rc和前一柵格rp的角度差。△θj∈［0，π］，由于每次只能運動到當(dāng)前柵格的鄰域，所以△θj常取

圖2 方向角計算簡圖Fig.2 Simplified Diagram for Calculating the Direction Angle

c為未覆蓋區(qū)域系數(shù)，表示原運動方向兩側(cè)中未覆蓋柵格數(shù)量最少一側(cè)對機器人選擇下一柵格的影響程度，取c=0.5。

d為轉(zhuǎn)向系數(shù)，表示轉(zhuǎn)向?qū)C器人選擇下一柵格的影響程度，取d=0.5。

當(dāng)機器人處于非死區(qū)狀態(tài)時，根據(jù)式（2）求出機器人鄰域柵格中該項最大者作為下一步實際移動位置，然后重復(fù)此過程，直至機器人陷入死區(qū)。

3.2 機器人逃離死區(qū)

對于全覆蓋工作要求而言，機器人在動態(tài)柵格法構(gòu)建的環(huán)境地圖里面運動過程中常常會陷入死區(qū)。機器人陷入死區(qū)是指它的周邊相鄰柵格是邊界、障礙物柵格和已覆蓋柵格的組合，不存在未覆蓋的柵格，只有從死區(qū)逃離出來，才能繼續(xù)全覆蓋任務(wù)。而逃離死區(qū)的路徑，影響全覆蓋的路徑重復(fù)率[6]。當(dāng)機器人利用優(yōu)先級啟發(fā)規(guī)則陷入死區(qū)時，采用蟻群算法能夠快速規(guī)劃出最優(yōu)逃離路徑，很好地解決了此問題。

3.2.1 搜索臨時目標(biāo)點

當(dāng)機器人陷入死區(qū)時，在利用蟻群算法逃離死區(qū)之前，必須確立一個臨時搜索點，作為機器人逃離目標(biāo)點，它要求柵格未被覆蓋且距機器人當(dāng)前位置距離最短。同時，目標(biāo)點的選取影響機器人的重復(fù)率的高低。在充分考慮障礙物的基礎(chǔ)上，選取離機器人實際距離最近的點作為臨時目標(biāo)點，從而極大降低了重復(fù)率。

假設(shè)機器人當(dāng)前位置為Pr（xc，yc），待選柵格位置為Pi（xi，yi），i=1，…，m，m為滿足條件的待選目標(biāo)點的總數(shù)。當(dāng)機器人與待選點之間不存在障礙物時，機器人位置與待選柵格之間的距離di，如式（3）所示。

當(dāng)機器人與待選點之前存在障礙物時，可將障礙物位置描述為Qri=｛A（x1i，y1i），B（x1i，y2i），C（x2i，y1i），D（x2i，y2i）｝。首先將距離機器人當(dāng)前位置最近且到直線PrPi垂直距離最短的障礙物頂點作為過渡點，比存在障礙物時點B即為過渡點，如圖3 所示。然后根據(jù)式（4）求距離di。

式中：θc—機器人當(dāng)前位置、過渡點和坐標(biāo)軸之間的夾角；θi—待選柵格、過渡點和坐標(biāo)軸之間的夾角。

圖3 存在障礙物時機器人位置與待選柵格距離Fig.3 The Distance Between the Robot Position and the Grid to be Selected in the Presence of Obstacles

在求得所有待選柵格與機器人位置距離di后，比較得到di距離最小柵格并記其位置坐標(biāo)為po（xo，yo），此柵格即作為機器人逃離死區(qū)時的臨時搜索點。

3.2.2 蟻群算法

當(dāng)確定臨時搜索點后，機器人逃離死區(qū)的過程轉(zhuǎn)化為點對點路徑規(guī)劃問題。文章采用蟻群算法能夠快速規(guī)劃出最優(yōu)逃離路徑，較好地解決了此問題。

蟻群算法是在模擬自然界螞蟻群體覓食行為的基礎(chǔ)上提出的智能優(yōu)化算法，具有全局尋優(yōu)能力強和復(fù)雜程度低等特點。傳統(tǒng)的蟻群算法中，螞蟻k在t時刻由位置i選擇下一位置j時，傾向于選擇短路徑且信息素濃度高的作為移動方向，忽略了路徑規(guī)劃全局尋優(yōu)性的要求，為此在狀態(tài)轉(zhuǎn)移規(guī)則中引入能表征擬移動位置與目標(biāo)位置距離項，使搜索更具導(dǎo)向性。螞蟻在狀態(tài)轉(zhuǎn)移時首先產(chǎn)生一個隨機數(shù)0≤q≤1，如果q≤q0則按照狀態(tài)轉(zhuǎn)移規(guī)則式（5）選取下一位置，否則按照轉(zhuǎn)盤賭式（6）選擇。

式中：S—按照轉(zhuǎn)盤賭式（6）選擇的下一位置。

式中：j∈allowedk—螞蟻k下一步允許轉(zhuǎn)移的位置；τij—邊（i，j）上的信息素濃度；ηij—一個啟發(fā)信息，通常γjg—允許轉(zhuǎn)移位置到目標(biāo)位置之間的歐式距離；α、β、γ—信息素濃度、啟發(fā)信息和目標(biāo)位置信息在螞蟻選擇路徑中的相對重要程度。

對于信息素更新而言，為有效避免螞蟻收斂到同一路徑，螞蟻從城市i向城市j移動時需要局部信息素更新，規(guī)則如式（7）所示。

式中：μ—信息素?fù)]發(fā)系數(shù)，0＜μ＜1；τ0—一小正常數(shù)。

在所有螞蟻完成一次迭代后進行全局信息素更新，根據(jù)最大最小螞蟻系統(tǒng)原理，只增強當(dāng)前迭代最優(yōu)路徑上的信息素[10]，同時為避免出現(xiàn)停滯現(xiàn)象，每次迭代信息素濃度設(shè)置上下限τmin≤τij（t）≤τmax。全局信息素更新規(guī)則，如式（8）所示。

式中：ρ—信息素?fù)]發(fā)系數(shù)，0＜ρ＜1；Lib—當(dāng)前迭代最優(yōu)路徑。

3.3 改進算法具體步驟

改進優(yōu)先級蟻群算法移動流程圖，如圖4 所示。

圖4 改進優(yōu)先級蟻群算法流程圖Fig.4 The Flow Chart of Improved Priority Rule with Ant Colony Algorithm

具體步驟描述如下：

（1）環(huán)境建模。利用機器人本體上的傳感器對環(huán)境進行沿邊學(xué)習(xí)得到環(huán)境輪廓，對其進行等大小網(wǎng)格劃分，并根據(jù)各網(wǎng)格的狀態(tài)置屬性值，最終形成柵格地圖。

（2）路徑選擇。機器人根據(jù)改進的優(yōu)先級規(guī)則式（2）尋找下一可行柵格，并修改已覆蓋柵格的屬性值，如此循環(huán)直到機器人當(dāng)前位置鄰域柵格不存在未覆蓋柵格。

（3）陷入死區(qū)。遍歷柵格地圖，如果存在屬性值為1 的柵格，則轉(zhuǎn)（4）；否則轉(zhuǎn)（6）。

（4）臨時目標(biāo)點。當(dāng)機器人與待選點之間不存在障礙物時，根據(jù)式（3）求它們之間的距離；當(dāng)機器人與待選點之前存在障礙物時，根據(jù)式（4）求它們之間的距離；最終選擇距離最短柵格作為臨時搜索目標(biāo)點。

（5）逃離死區(qū)。蟻群算法狀態(tài)轉(zhuǎn)移規(guī)則加入距離目標(biāo)點項，然后機器人利用其規(guī)劃出最優(yōu)逃離死區(qū)路線，并轉(zhuǎn)步驟2 繼續(xù)進行路徑全覆蓋工作。

（6）結(jié)束搜索。當(dāng)柵格地圖中不存在屬性值為1 的柵格時，表示全覆蓋工作已經(jīng)完成，算法結(jié)束。

4 仿真驗證

4.1 路徑規(guī)劃算法性能指標(biāo)

機器人全覆蓋路徑規(guī)劃算法常用性能評價指標(biāo)為轉(zhuǎn)彎總數(shù)、軌跡重復(fù)率和面積覆蓋率。令SΩ表示機器人待覆蓋區(qū)域總面積，Sd表示機器人已覆蓋（單次）區(qū)域總面積，Sg表示機器人完成全覆蓋任務(wù)后經(jīng)過區(qū)域總面積，T表示機器人轉(zhuǎn)彎總次數(shù)。

轉(zhuǎn)彎總次數(shù)T是指機器人運動方向改變45°的次數(shù)。

面積覆蓋率pc是指機器人完成任務(wù)后單次覆蓋區(qū)域總面積Sd與機器人待覆蓋區(qū)域總面積SΩ的比值：

軌跡重復(fù)率pr是指機器人多次覆蓋的區(qū)域面積Sg-Sd與待覆蓋區(qū)域總面積SΩ的比值：

綜合機器人運動過程的轉(zhuǎn)彎數(shù)、面積覆蓋率和軌跡重復(fù)率等指標(biāo)全面評價全覆蓋路徑規(guī)劃算法。若轉(zhuǎn)彎總數(shù)越低、面積覆蓋率越高、軌跡重復(fù)率越低，則機器人工作效率越高，路徑規(guī)劃算法可行越好。

4.2 仿真分析

為說明所提算法的可行性和有效性，在10*10 的環(huán)境地圖下，分別采用文獻[5-6]和所提算法進行全覆蓋路徑規(guī)劃仿真實驗。不同算法全覆蓋路徑規(guī)劃仿真結(jié)果，如圖5 所示。其中標(biāo)識點S表示機器人的初始位置，標(biāo)識點F表示機器人的終止位置。文獻[5]基于優(yōu)先級A*算法的路徑規(guī)劃仿真結(jié)果，如圖5（a）所示。文獻[6]基于方向信度函數(shù)算法的路徑規(guī)劃仿真結(jié)果，如圖5（b）所示。所提算法的路徑規(guī)劃算法，如圖5（c）所示。不同路徑規(guī)劃算法性能指標(biāo)，如表1 所示。

圖5 不同算法全覆蓋路徑規(guī)劃仿真結(jié)果Fig.5 The Simulation Results of Different Complete-Coverage Path Planning Algorithms

表1 不同算法性能對比Tab.1 Performances of Different Algorithms

通過圖5 和表1 可知，雖然不同算法均可實現(xiàn)面積全覆蓋，但機器人的轉(zhuǎn)彎次數(shù)和軌跡重復(fù)率不盡相同。文獻[5]的路徑規(guī)劃算法由于強調(diào)環(huán)境地圖的左側(cè)始終保持已覆蓋狀態(tài)，使轉(zhuǎn)彎總數(shù)和軌跡重復(fù)率較高；文獻[6]的路徑規(guī)劃算法利用方向信度函數(shù)盡可能使機器人保持直線運動以降低能耗；使轉(zhuǎn)彎總數(shù)和軌跡重復(fù)率較文獻[5]所下降；綜合考慮機器人轉(zhuǎn)彎能耗和未覆蓋區(qū)域的基礎(chǔ)上，提出新的優(yōu)先級啟發(fā)規(guī)則，同時當(dāng)機器人陷入死區(qū)時，利用蟻群算法能夠找到最優(yōu)路徑。比較文獻[5-6]，本算法不僅陷入死鎖次數(shù)較少，而且在軌跡重復(fù)方面分別降低了81.80%和66.64%；在轉(zhuǎn)彎總數(shù)方面，本算法比文獻[5]降低了26.83%，而與文獻[6]相差不大。仿真實驗結(jié)果表明，所提算法在保證面積全覆蓋的基礎(chǔ)上，更加有效地降低了軌跡重復(fù)率，減少了轉(zhuǎn)彎次數(shù)，提高了機器人工作效率，降低了能耗。

5 結(jié)論

針對常見移動機器人全覆蓋路徑規(guī)劃算法的不足，提出一種改進優(yōu)先級蟻群算法。該算法在傳統(tǒng)優(yōu)先級啟發(fā)規(guī)則的基礎(chǔ)上進行了改進，在機器人狀態(tài)陷入死區(qū)時，進一步考慮了死區(qū)點到臨時搜索點之間存在障礙的情況。通過仿真實驗表明，改進后的方法在保證面積覆蓋率為100%的同時能降低死鎖次數(shù)和軌跡重復(fù)率，方法可行有效，為移動機器人全覆蓋路徑規(guī)劃提供了參考。