王 月， 姚善化

(安徽理工大學(xué) 電氣與信息工程學(xué)院， 安徽 淮南 232000)

隨著各大醫(yī)院逐漸引入自動化藥房系統(tǒng)，傳統(tǒng)的人工取藥工作被取藥機器人所替代，取藥機器人路徑規(guī)劃一直是研究的重點。路徑規(guī)劃算法主要包括Floyd算法[1]、人工勢場法、蟻群算法[2]、RRT算法[3]等。A*算法[4]計算量小、路徑規(guī)劃搜索效率高，一直被廣泛的研究。傳統(tǒng)A*算法在進行路徑規(guī)劃過程中忽略了路徑的搜索效率、安全性、平滑度。針對這些問題，很多學(xué)者對A*算法進行了改進。文獻[5]為了提高路徑搜索效率，引入跳點搜索法，但忽視了一些關(guān)鍵點，導(dǎo)致路徑平滑度不高；文獻[6]針對復(fù)雜路況下的路徑規(guī)劃問題，引入二次路徑規(guī)劃對路徑進行優(yōu)化，提升了路徑規(guī)劃效率，但忽略了與障礙物之間的安全間距問題；文獻[7]引入擴展A*鄰域法來擴大搜索方向，可以尋得一條更優(yōu)的路徑，但計算量增大，降低了A*算法搜索效率；文獻[8]為了減少路徑搜索的空間，對修改后的估計路徑進行加權(quán)處理，但忽略了與障礙物之間的安全間距問題。

針對上述存在的問題，提出一種改進A*算法的藥房機器人路徑規(guī)劃。首先，根據(jù)自動化藥房的實際環(huán)境，搭建帶有起始點、目標點和障礙物信息的柵格地圖；其次，在已搭建好的地圖上使用有向圖法來預(yù)防各取藥機器人間的碰撞問題；再次，對藥房中的藥架(在機器人進行路徑規(guī)劃時，將其當(dāng)做障礙物)進行膨脹處理；最后，為了使路徑搜索效率更高、平滑性更好，改進A*算法的評價函數(shù)結(jié)合幾何法和平滑的圓弧來去除多余節(jié)點，得到光滑路徑。本文所提出的改進A*算法可以優(yōu)化移動機器人在路徑搜索中的平滑度和安全性，同時搜索效率也顯著得到提高。

1 自動化藥房環(huán)境模型建立

采用柵格法搭建自動化藥房的模型如圖1所示。圖1表示一個N*M大小的柵格地圖(其中N=20,M=20)。對搭建好的地圖進行定義：圖中白色柵格為可行駛區(qū)域(代表這里無障礙物)，反之為不可行駛區(qū)域。

圖1 柵格地圖

根據(jù)自動化藥房環(huán)境采用柵格法對機器人運行環(huán)境進行地圖建模。針對多機器人在作業(yè)過程中會產(chǎn)生相互碰撞的問題，采用雙向圖法對道路方向進行約束，規(guī)定貨架間的道路為單行道，每個移動機器人只能在指定的取藥空間內(nèi)活動，且機器人把藥物送到取藥柜臺后只能原路返回，這樣可以避免機器人發(fā)生迎面碰撞的問題。約束機器人只能在規(guī)定的區(qū)域內(nèi)作業(yè)，且作業(yè)完之后只能原路返回，這樣可以避免移動機器人在作業(yè)過程中發(fā)生趕超碰撞、交叉碰撞的問題。對機器人行駛路線做以下規(guī)定：

(1) 每一條邊為單行道，且規(guī)定機器人能雙向行駛(即機器人在規(guī)定的取藥空間內(nèi)活動，并在送完藥之后只能原路返回)；

(2) 各機器人單次只可完成一個任務(wù)；

(3) 各機器人以2 m/s勻速行駛。

2 A*算法

2.1 傳統(tǒng)A*算法

A*算法作為靜態(tài)全局路徑尋優(yōu)的標準算法，主要在于估計成本h(x,y)的選取。A*算法的原理是：自規(guī)劃路徑的起始點開始，根據(jù)節(jié)點的擴展方向，不停地歷遍柵格地圖，旨在找到一條到目標點最優(yōu)的搜索方向[9]。A*算法的評價函數(shù)為

f(x,y)=g(x,y)+h(x,y)

(1)

路徑搜索對估計成本的選取有較高的要求，為了使選擇的估計函數(shù)實用性和效率高，盡量使選擇的估計代價值和實際代價值保持一致。A*算法的估計成本通常有兩種表達形式：歐式距離和曼哈頓距離。

歐式距離的計算公式：

(2)

曼哈頓距離的計算公式：

h(x,y)Manhattan=|xn-xd|+|yn-yd|

(3)

其中：(xn,yn)為當(dāng)前節(jié)點(x,y)的坐標；(xd,yd)為目標節(jié)點d的坐標。

A*算法在擴展節(jié)點時，會遍歷周圍所有柵格，如圖2所示?？紤]到在實際自動化藥房環(huán)境中，機器人駛向特征為正向行駛(正南、正北、正東、正西)，選擇4個方向的擴展節(jié)點，如圖3所示?；谶@方面的考慮，選擇兩點間的曼哈頓距離作為估計函數(shù)效率更高[10]。

圖2 傳統(tǒng)A*節(jié)點擴展圖 圖3 改進A*節(jié)點擴展圖

2.2 改進A*算法

2.2.1 障礙物膨脹處理

傳統(tǒng)A*算法在進行路徑規(guī)劃時，考慮的質(zhì)點問題是機器人本體，像機器人的長、寬、高等外部特征沒有考慮在內(nèi)。但自動化藥房中存在很多排藥架，藥架上也擺滿了藥物，取藥機器人作為特殊的運動體，假如取藥機器人在前進的過程中與藥架或者其他障礙物沒有保持一定的安全距離，那么取藥機器人在實際運行中難免與藥架等障礙物發(fā)生碰撞，嚴重的話不僅會損壞機器人本體，還會導(dǎo)致藥架上的藥盒跌落[11]。

基于上述分析，主要對柵格地圖中的障礙物(藥架)進行膨脹處理。在對障礙物進行膨脹處理的同時，需要將取藥機器人的外部特征(長、寬、高等)考慮在內(nèi)。通常藥房機器人的高度在1.2 m，寬度在0.3 m左右，兩個藥架之間的距離一般在1.2 m左右，一個藥架的寬度在0.3 m，因此將藥架膨脹半徑設(shè)定為機器人寬度的一半左右，這樣機器人在藥房過道中規(guī)劃出的路徑與障礙物的最小距離大于機器人寬度的一半，能夠保證取藥機器人本體的寬度小于兩個藥架之間可通行的區(qū)域。對障礙物進行膨脹處理旨在幫助取藥機器人安全地穿行各藥架之間。

2.2.2 軌跡優(yōu)化

A*算法規(guī)劃出的路徑不僅要符合機器人運行安全規(guī)范,避免與障礙物發(fā)生碰撞，還要保證路徑轉(zhuǎn)折點較少，平滑性高。若規(guī)劃路徑轉(zhuǎn)折較多意味著機器人在實際運行途中需要多次改變運行方向，不僅會降低機器人取藥效率，而且還會提高機器人在拐彎時藥品滑落的風(fēng)險。因此，在基于對障礙物的膨脹處理基礎(chǔ)之上，提出評價函數(shù)引入安全因子，并在轉(zhuǎn)彎處使用圓弧曲線來代替尖銳點來解決轉(zhuǎn)折點較多、碰撞威脅較大的問題，進一步提高A*算法規(guī)劃路徑的可采用性[12-13]。改進的評價函數(shù)為

(4)

式中：(x,y)為當(dāng)前被搜索節(jié)點；f(x,y)為估計成本；g(x,y)為實際成本；h(x,y)為啟發(fā)函數(shù)，本文使用Manhattan距離；A為當(dāng)前點到目標點的距離；a為起始點到目標點的距離；n為藥架占總柵格的總數(shù)；L為機器人自身所占柵格的數(shù)目；N*M為總柵格數(shù)量；γ為安全因子。

在完成對A*算法評價函數(shù)改進的基礎(chǔ)之上，為了使得到的路徑更加平滑，使用幾何法來剔除路徑冗余點與不必要的轉(zhuǎn)折點，結(jié)合平滑的圓弧來改進轉(zhuǎn)彎時的尖銳點，對路徑進行優(yōu)化處理。文獻[14]通過計算任意兩點斜率是否相等來判斷路徑中三點共線的問題，但忽略了實際情況中斜率可能不存在，或者為0等特殊情況。本文使用三階行列式面積法解決路徑中三點共線的問題。假設(shè)路徑上的某一節(jié)點坐標(x1,y1)以D表示，節(jié)點D的前一節(jié)點坐標(x0,y0)以C表示，節(jié)點D的后一節(jié)點坐標(x2,y2)以E表示。則其所圍成的三角形面積為

(5)

如果S△等于零時，說明C、D、E三點共線，那么節(jié)點D為冗余點，需剔除，路徑由C到D再到E直接變?yōu)橛蒀到E。

(6)

(7)

(8)

(9)

其中：μ1、μ2、μ3、μ4均為叉積的系數(shù)。如果μ1·μ2<0，且μ3·μ4<0，如圖4所示，則表示角平分線FI與線段CE相交；如圖5所示，則表示線段CE與角平分線不相交。其余兩條角平分線與線段CE是否相交也可根據(jù)此原理判斷。其實只要三條角平分線中的任意一條與線段CE相交，則代表節(jié)點C與節(jié)點E之間有障礙物，路徑仍然是由C到D再到E(不改變其路徑)；反之，如果三條角平分線和線段CE都沒有相交，則代表節(jié)點C與節(jié)點E之間并無障礙物，連接節(jié)點C與節(jié)點E，去除冗余節(jié)點D，路徑由C到D再到E直接變?yōu)橛蒀到E[14]。

圖4 節(jié)點之間存在障礙物 圖5 節(jié)點之間不存在障礙物

3 仿真實驗與分析

在柵格地圖中為取藥機器人設(shè)置3組不同起始點與目標點的坐標，且隨機產(chǎn)生3個取藥任務(wù)(任務(wù)信息如表1所示)，每個任務(wù)由一臺機器人完成，一旦確定目標點將會開始路徑規(guī)劃。從3個方面分別對標準A*算法和改進后的A*算法進行評價：一是計算時間；二是優(yōu)化路徑長度；三是路徑的平滑度。實驗分為兩組，分別基于簡單地圖和復(fù)雜地圖進行。實驗結(jié)果如表2所示,仿真圖如圖6和圖7所示。

表1 機器人任務(wù)信息

表2 實驗結(jié)果對比

(a)標準A*算法 (b)改進A*算法圖6 基于簡單地圖的仿真實驗結(jié)果

(a)標準A*算法 (b)改進A*算法圖7 基于復(fù)雜地圖的仿真實現(xiàn)結(jié)果

圖6和圖7中A1、B1代表的是第一個機器人的起始點與目標點，A2、B2、A3、B3分別代表的是第二個、第三個機器人的起始點與目標點。不考慮各取藥機器人任務(wù)下達等時間問題。從圖6和圖7中可以明顯看出，改進后的A*算法所規(guī)劃的路徑比標準A*算法所規(guī)劃出的路徑能夠與障礙物保持安全距離，路徑更加平滑，在優(yōu)化路徑的同時，也能節(jié)約規(guī)劃的時間。

4 結(jié) 語

為解決自動化藥房中取藥機器人路徑規(guī)劃問題，提出一種對障礙物進行膨脹處理的改進A*算法，該算法可以有效地優(yōu)化機器人路徑規(guī)劃，提高機器人工作效率。為驗證算法的有效性，在構(gòu)建柵格地圖的基礎(chǔ)上，設(shè)計了兩組具有不同起始點、目標點和障礙物的仿真實驗。經(jīng)仿真驗證，改進的A*算法可以增強路徑搜索靈活性和安全性，提高其搜索效率，在實際自動化藥房路徑規(guī)劃系統(tǒng)中具有一定的現(xiàn)實意義。