基于改進(jìn)A*算法的藥房機(jī)器人路徑規(guī)劃

王 月， 姚善化

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

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

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

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

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

圖1 柵格地圖

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

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

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

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

2 A*算法

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

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

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

(1)

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

歐式距離的計(jì)算公式：

(2)

曼哈頓距離的計(jì)算公式：

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

(3)

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

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

圖2 傳統(tǒng)A*節(jié)點(diǎn)擴(kuò)展圖 圖3 改進(jìn)A*節(jié)點(diǎn)擴(kuò)展圖

2.2 改進(jìn)A*算法

2.2.1 障礙物膨脹處理

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

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

2.2.2 軌跡優(yōu)化

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

(4)

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

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

(5)

如果S△等于零時(shí)，說(shuō)明C、D、E三點(diǎn)共線，那么節(jié)點(diǎn)D為冗余點(diǎn)，需剔除，路徑由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ù)此原理判斷。其實(shí)只要三條角平分線中的任意一條與線段CE相交，則代表節(jié)點(diǎn)C與節(jié)點(diǎn)E之間有障礙物，路徑仍然是由C到D再到E(不改變其路徑)；反之，如果三條角平分線和線段CE都沒(méi)有相交，則代表節(jié)點(diǎn)C與節(jié)點(diǎn)E之間并無(wú)障礙物，連接節(jié)點(diǎn)C與節(jié)點(diǎn)E，去除冗余節(jié)點(diǎn)D，路徑由C到D再到E直接變?yōu)橛蒀到E[14]。

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

3 仿真實(shí)驗(yàn)與分析

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

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

表2 實(shí)驗(yàn)結(jié)果對(duì)比

(a)標(biāo)準(zhǔn)A*算法 (b)改進(jìn)A*算法圖6 基于簡(jiǎn)單地圖的仿真實(shí)驗(yàn)結(jié)果

(a)標(biāo)準(zhǔn)A*算法 (b)改進(jìn)A*算法圖7 基于復(fù)雜地圖的仿真實(shí)現(xiàn)結(jié)果

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

4 結(jié) 語(yǔ)

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