賀利樂,劉小羅,黃天柱,楊劍樂
(1.西安建筑科技大學(xué)機電工程學(xué)院,陜西 西安 710055;2.中建建樂實業(yè)有限公司,陜西 西安 710000)
路徑規(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ù),提高工作效率。
環(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)點。
沿邊學(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)境輪廓的目的。
經(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ù)遍歷。
基于環(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ī)劃研究的重點。
文獻[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ū)。
對于全覆蓋工作要求而言,機器人在動態(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)路徑。
改進優(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é)束。
機器人全覆蓋路徑規(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ī)劃算法可行越好。
為說明所提算法的可行性和有效性,在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ù),提高了機器人工作效率,降低了能耗。
針對常見移動機器人全覆蓋路徑規(guī)劃算法的不足,提出一種改進優(yōu)先級蟻群算法。該算法在傳統(tǒng)優(yōu)先級啟發(fā)規(guī)則的基礎(chǔ)上進行了改進,在機器人狀態(tài)陷入死區(qū)時,進一步考慮了死區(qū)點到臨時搜索點之間存在障礙的情況。通過仿真實驗表明,改進后的方法在保證面積覆蓋率為100%的同時能降低死鎖次數(shù)和軌跡重復(fù)率,方法可行有效,為移動機器人全覆蓋路徑規(guī)劃提供了參考。