宋永杰,孟祥印,2,翟守才,馮一凡
(1.西南交通大學(xué)機(jī)械工程學(xué)院,四川 成都 610031;2.軌道交通運(yùn)維技術(shù)與裝備四川省重點(diǎn)實(shí)驗(yàn)室,四川 成都 610031)
隨著智能制造業(yè)的崛起,自動導(dǎo)引運(yùn)輸車(AGV)以其控制便捷、定位精準(zhǔn)、持續(xù)工作時間長的特點(diǎn),在物料運(yùn)輸環(huán)節(jié)中發(fā)揮著愈發(fā)重要的作用[1]。傳統(tǒng)AGV通過在地面鋪設(shè)磁條、色帶等方式來人為規(guī)劃出固定的路徑,施工復(fù)雜且難以升級擴(kuò)展[2],無法滿足其在無人環(huán)境下自主靈活運(yùn)行的要求。而路徑規(guī)劃算法[3]旨在空間中自動搜尋出連通起始點(diǎn)與目標(biāo)點(diǎn)的無碰撞路徑,是AGV運(yùn)動智能化的基礎(chǔ),吸引了國內(nèi)眾多學(xué)者對其進(jìn)行研究。
常見的路徑規(guī)劃算法包括人工勢場法、A*算法、神經(jīng)網(wǎng)絡(luò)法、遺傳算法、蟻群算法[4-8]等。這些算法雖然能在簡單地圖上規(guī)劃出較優(yōu)路徑,但存在著計(jì)算復(fù)雜度高或易陷入局部最小值的缺點(diǎn),而且未能很好地將車輛實(shí)際運(yùn)行狀態(tài)納入考慮,規(guī)劃出的路徑不一定適用于現(xiàn)實(shí)環(huán)境下AGV的尋跡。
快速隨機(jī)搜索樹(Rapidly-Exploring Random Tree,RRT)算法[9]是一種基于隨機(jī)采樣的算法。該算法不需要對原始地圖進(jìn)行預(yù)處理建模,相較于其他算法搜索速度快,且具有概率完備性[10],在眾多移動機(jī)器人路徑規(guī)劃算法中優(yōu)勢突出。而傳統(tǒng)RRT算法的缺點(diǎn)也很明顯,采樣過于均勻?qū)е铝怂惴ㄊ諗克俣嚷?,所得路徑偏離最優(yōu)解且重復(fù)性差[11]。
針對傳統(tǒng)RRT 算法的不足,文獻(xiàn)[12]提出雙向生成隨機(jī)樹(Bidirectional RRT,Bi-RRT)算法,將原有算法的單向搜索改為雙向搜索,提高了路徑收斂速度,但生成路徑仍過于隨機(jī)。文獻(xiàn)[13]則提出RRT*算法,以降低運(yùn)行效率為代價,獲取趨近于概率最優(yōu)解的路徑。文獻(xiàn)[14]中添加了動態(tài)步長特性,使得算法兼?zhèn)渎窂酱_定性和避障能力。文獻(xiàn)[15]提出混沌搜索策和局部引導(dǎo)新節(jié)點(diǎn)生成策略對局部最小問題進(jìn)行優(yōu)化。除了對算法本身進(jìn)行改進(jìn),RRT 算法與其他算法的融合也得到了研究。如在文獻(xiàn)[16]中,人工勢場法的合力引導(dǎo)機(jī)制被應(yīng)用到RRT算法的樹節(jié)點(diǎn)擴(kuò)展過程中,大大降低了路徑生成隨機(jī)性,卻增加了陷入局部最小值的風(fēng)險。文獻(xiàn)[17]則在RRT算法基礎(chǔ)上,加入了車輛轉(zhuǎn)向約束和目標(biāo)偏向約束對其改進(jìn),并利用三次B樣條曲線對路徑平滑處理。
為彌補(bǔ)RRT算法中隨機(jī)采樣過于均勻的缺陷,提出雙重目標(biāo)偏向策略進(jìn)行改進(jìn),即在生成隨機(jī)樣點(diǎn)過程中采用基于概率p的目標(biāo)偏向策略,同時引入A*算法中代價評估的思想,啟發(fā)式地選擇最近點(diǎn)。在此策略引導(dǎo)下,分別以起點(diǎn)和終點(diǎn)為根節(jié)點(diǎn)的兩棵隨機(jī)樹均向著對方生長。還在兩棵樹的擴(kuò)展過程中加入直接連通判斷,推進(jìn)兩棵樹相連的過程。在提高路徑生成效率的同時,結(jié)合AGV實(shí)際運(yùn)行狀態(tài),對路徑的擴(kuò)展引入安全距離判斷,提高了生成路徑的質(zhì)量。最后,從終點(diǎn)開始遍歷各點(diǎn),在保證安全距離的前提下,裁剪路徑上的冗余點(diǎn),簡化出一條實(shí)際可執(zhí)行的全局路徑。
Bi-RRT算法繼承了傳統(tǒng)RRT算法的所有優(yōu)點(diǎn),適用于非完整約束下AGV 的全局路徑規(guī)劃[18]。二維地圖由狀態(tài)空間Z表示,包含互補(bǔ)的無障礙空間Zfree和障礙空間Zobs。起點(diǎn)Pstart與終點(diǎn)Pterm均屬于無障礙空間Zfree。T1與T2分別代表從起點(diǎn)Pstart和終點(diǎn)Pterm開始構(gòu)建的隨機(jī)樹。兩棵隨機(jī)樹按給定步長增量式地向未知空間延伸,直至相連。
Bi-RRT算法隨機(jī)樹的擴(kuò)展過程,如圖1所示,步驟如下:
圖1 Bi-RRT算法擴(kuò)展過程Fig.1 Extension Process of Bi-RRT Algorithm
(1)初始化:在二進(jìn)制地圖上,給定處于無障礙空間Zfree的起點(diǎn)Pstart與終點(diǎn)Pterm,分別作為隨機(jī)樹T1與T2的根節(jié)點(diǎn)。并設(shè)置擴(kuò)展步長ε。
(2)隨機(jī)樹生長:在狀態(tài)空間Z中生成隨機(jī)樣點(diǎn)Psample,選取T1上到隨機(jī)點(diǎn)Psample歐式距離最短的點(diǎn)為最近點(diǎn)P1near。然后以P1near為基點(diǎn),朝著Psample方向按照給定步長ε擴(kuò)展出新節(jié)點(diǎn)P1new。若P1near與P1new之間的連線進(jìn)行通過障礙檢測,即不經(jīng)過障礙空間Zobs,則P1new成為隨機(jī)樹T1上的新節(jié)點(diǎn)。否則將舍棄P1new,重新生成Psample進(jìn)行擴(kuò)展。
(3)相連判斷:檢測新節(jié)點(diǎn)P1new與隨機(jī)樹T2上的各點(diǎn)的最短歐式距離是否小于步長ε,且連線是否通過障礙檢測。若都成立,則兩棵隨機(jī)樹連通,隨機(jī)樹停止生長。否則,按(2)中同樣方式擴(kuò)展隨機(jī)樹T2。循環(huán)直至兩棵樹相連,若循環(huán)次數(shù)超出設(shè)定上限N,則判定為路徑搜索失敗。
(4)路徑生成:當(dāng)隨機(jī)樹T1與T2相連,采取由新節(jié)點(diǎn)向最近點(diǎn)不斷回溯的迭代方法,找出路徑點(diǎn),生成連通Pstart與Pterm的路徑Path。
Bi-RRT算法偽代碼,如算法1所示。
算法1 Bi-RRT算法偽代碼
Bi-RRT Algorithm
1.Z=Zfree∪Zobs;{Pstart,Pterm}∈Zfree;
2.{T1,T2}←InitializeRRTree(Pstart,Pterm,ε);
3.whilei<Ndo
4.Psample←RandPoint(Z)
5.P1near←ChoosePnearest(T1,Psample);
6.P1new←GeneratePnew(Psample,P1near);
7.if PathFree(P1new,P1near,Z) then
8.T1←T1∪{P1new};
9.if CheckDistance(P1new,T2,ε) then
10.return Path=GetPath(T1,T2);break;
11.else
12.P2near←ChoosePnearest(T2,Psample);
13.P2new←GeneratePnew(Psample,P2near);
14.if PathFree(P2new,P2near,Z) then
15.T2←T2∪{P2new};
16.if CheckDistance(P2new,T1,ε) then
17.return Path=GetPath(T1,T2);break;
18.end;end;end;end
19.i=i+1;
20.end
A*算法是一種基于二維柵格地圖的直接搜索算法,能夠規(guī)劃出最短路徑,但計(jì)算繁瑣且耗時長[19]。傳統(tǒng)A*算法的核心思想是,通過一個估價函數(shù),從起點(diǎn)開始計(jì)算柵格點(diǎn)到臨近各個不同柵格點(diǎn)的代價值,選擇代價值最小的點(diǎn)向外擴(kuò)散,直至到達(dá)終點(diǎn)。因?yàn)槁窂缴厦總€點(diǎn)的代價值最小,因此生成路徑的總代價值是最小的,路徑為最優(yōu)。A*算法的代價估計(jì)函數(shù)f(n)可表示為式(1)。
式中:f(n)—地圖上任意節(jié)點(diǎn)n的估價函數(shù);g(n)—從起始點(diǎn)到節(jié)點(diǎn)n的實(shí)際代價;h(n)—節(jié)點(diǎn)n到終點(diǎn)的估計(jì)代價。在傳統(tǒng)A*算法中,兩點(diǎn)間的移動代價為其歐式距離。
由Bi-RRT算法的基本原理可知,隨機(jī)樹的優(yōu)劣主要由三個要素所決定,即隨機(jī)樣點(diǎn)的生成、最近點(diǎn)的定義和隨機(jī)樹的擴(kuò)展。對此,首先提出雙重目標(biāo)偏向策略對算法進(jìn)行改進(jìn),啟發(fā)式地生成隨機(jī)樣點(diǎn)和最近點(diǎn)。同時,在隨機(jī)樹的擴(kuò)展過程中加入安全距離判斷以及直接連通判斷來改善算法。最后,對生成路徑后處理,去除冗余節(jié)點(diǎn)。
3.1.1 隨機(jī)樣點(diǎn)的選取
隨機(jī)樹上的節(jié)點(diǎn)均根據(jù)隨機(jī)樣點(diǎn)Psample擴(kuò)展而來,因此Psample的選取在路徑生成過程中起著關(guān)鍵性的作用。在傳統(tǒng)Bi-RRT算法中,Psample在地圖上完全隨機(jī)產(chǎn)生,導(dǎo)致生成的路徑往往偏離最優(yōu)解,而且重復(fù)度低,實(shí)用性不強(qiáng)。
引入基于概率p的目標(biāo)偏向策略,在選取點(diǎn)Psample時,按隨機(jī)概率p與設(shè)定參數(shù)pth的比較結(jié)果,來決定Psample為完全隨機(jī)點(diǎn)Prand或目標(biāo)點(diǎn)Pterm?;诟怕蕄的目標(biāo)偏向算法偽代碼,如算法2所示,對算法1中第4行作出改進(jìn)。
算法2 目標(biāo)偏向算法偽代碼
Improved_RandPoint(pth,Pterm,Z) Algorithm
1.p←Rand(1);
2.if 0<p<pththen
3.Psample←RandPoint(Z);
4.else ifpth<=p<1 then
5.Psample←Pterm;
6.end
7.returnPsample
經(jīng)過多次試驗(yàn)來觀察路徑的收斂情況,發(fā)現(xiàn)當(dāng)pth過低時,生成路徑與傳統(tǒng)Bi-RRT算法相差不大,隨機(jī)性高;而當(dāng)pth太大時,算法易陷入局部最小值,難以在達(dá)到給定迭代上限次數(shù)前到達(dá)目標(biāo)點(diǎn)。因此文中選定pth=0.5,使得路徑在生成過程中既偏向目標(biāo),又不會陷入局部最小值。
3.1.2 啟發(fā)式選取最近點(diǎn)Pnear
最近點(diǎn)Pnear作為新節(jié)點(diǎn)Pnew的父節(jié)點(diǎn),對隨機(jī)樹的生長方向也有著巨大影響。在算法1中,選取隨機(jī)樹T上各點(diǎn)Pn(xn,yn)與樣點(diǎn)Psample(xsa,ysa)之間歐式距離Dns最短的點(diǎn)作為最近點(diǎn)Pnear,導(dǎo)致隨機(jī)樹T的生長對隨機(jī)樣點(diǎn)Psample的依賴嚴(yán)重。歐式距離Dns的算法,如式(2)所示。
以基于概率p的目標(biāo)偏向算法取得隨機(jī)樣點(diǎn)Psample后,引入A*算法中代價評估的思想,啟發(fā)式地選取隨機(jī)樹上代價值最小的點(diǎn)作為最近點(diǎn)Pnear,有利于路徑進(jìn)一步朝著目標(biāo)擴(kuò)展。
仍采用式(1)中函數(shù)f(n)作為隨機(jī)樹T上各點(diǎn)的估價函數(shù),而改進(jìn)后的代價估計(jì)函數(shù),如式(3)所示。
式中:Dns—隨機(jī)樹T上各點(diǎn)Pn(xn,yn)到Psample(xsa,ysa)的實(shí)際代價,即Dns=g(n);Dnt—點(diǎn)Pn(xn,yn)到終點(diǎn)Pterm(xt,yt)的估計(jì)代價。
相較于傳統(tǒng)Bi-RRT算法中遍歷隨機(jī)樹選取最近點(diǎn)的方式,此算法在沒有增加陷入局部最小值風(fēng)險的同時提高了路徑規(guī)劃的目標(biāo)偏向性?;谒惴?第5行,改進(jìn)后算法的偽代碼,如算法3所示。
算法3 啟發(fā)式選取最近點(diǎn)算法偽代碼
Improved_ChoosePnearest(Psample,Pterm,T) Algorithm
1.Pn∈T,n∈[1,length(T)];
2.g(n)←GetDistance(Pn,Psample);
3.h(n)←GetDistance(Pn,Pterm);
4.f(n)=g(n)+h(n);
5.Pnear←min(f(n));
6.returnPnear;
3.2.1 安全距離判斷
在傳統(tǒng)Bi-RRT算法中,擴(kuò)展路徑常出現(xiàn)貼合障礙物邊緣的現(xiàn)象,使得實(shí)際應(yīng)用中存在著AGV與障礙物碰撞的安全隱患,易造成財產(chǎn)損失。白色區(qū)域?yàn)闊o障礙空間Zfree,代表可行駛區(qū)域;黑色區(qū)域?yàn)檎系K空間Zobs,代表障礙物所在范圍,如圖2所示。
圖2 傳統(tǒng)Bi-RRT算法路徑圖Fig.2 Path Fraph of Traditional Bi-RRT Algorithm
藍(lán)色與綠色兩段曲線分別代表從起點(diǎn)和終點(diǎn)出發(fā)的隨機(jī)樹T1、T2。紅色線條表示兩棵隨機(jī)樹連通后所得的路徑Path。而從圖中黃色橢圓區(qū)域可看出,路徑Path與障礙空間Zobs有著貼合或相近部位,十分不利于現(xiàn)實(shí)情形下AGV的尋跡。
針對上述問題,在隨機(jī)樹的擴(kuò)展中加入安全距離判斷機(jī)制,使規(guī)劃出的路徑符合AGV行車安全距離要求。
線段Ln1、Ln2關(guān)于Pnear與Pnew之間的連線Ln對稱,距離均為Width,如圖3所示。同時對三條線段進(jìn)行障礙碰撞檢測,若都通過檢測,則Pnew才算選取成功。若其中任意一條線段與障礙物相接觸,則生成的Pnew失效??紤]AGV的車身寬度為WAGV,在AGV安全行駛過程中,還需保證側(cè)邊與障礙物之間的安全距離為Wsafe,則Ln1、Ln2與Ln的距離Width如式(4)所示。
圖3 安全距離判斷示意圖Fig.3 Safety Distance Judgment Diagram
安全距離判斷偽代碼,如算法4所示:
算法4 安全距離判斷算法偽代碼
CheckWidth(P1,P2,Z) Algorithm
1.[P1a,P1b]=P1+(null(P2-P1))*Width;
2.[P2a,P2b]=P2+(null(P1-P2))*Width;
3.if PathFree(P1a,P2a,Z)&&PathFree(P1b,P2b,Z) then
4.return True;
5.else
6.return False;
7.end
3.2.2 直接連通判斷
圖2中,以人類視角可判斷出隨機(jī)樹T1上的點(diǎn)P1與T2上的點(diǎn)P2能在無障礙空間Zfree內(nèi)直接相連。但在圖像上,P1與P2之間的存在著許多無意義的曲折線段。而且由于算法中Psample有一定幾率在全地圖隨機(jī)產(chǎn)生,這種多余的線段實(shí)際上比兩點(diǎn)間顯示的更多,對計(jì)算資源和規(guī)劃時間造成了不必要的浪費(fèi)。為了減少這類浪費(fèi),加入直接連通判斷機(jī)制對算法進(jìn)行改進(jìn)。若隨機(jī)樹T1或T2上的新節(jié)點(diǎn)Pnew可以與另一棵樹上任意節(jié)點(diǎn)無障礙連通,且通過安全距離判斷,便提前終止隨機(jī)樹的擴(kuò)展。具體過程的偽代碼,如算法5所示,在算法1的第9和16行之前改進(jìn):
算法5 直接連通判斷算法偽代碼
CheckConnection(P1new,T2,Z) Algorithm
1.Path=[];
2.for n=1:size(T2)
3.if PathFree(P1new,Pn,Z)&&CheckWidth(P1new,Pn,Z)then
4.return Path=GetPath(P1new,T2);break;
5.end;
6.end
圖2 中紅色線段代表傳統(tǒng)Bi-RRT 算法所生成的路徑,由多段短小的線段拼接而成,細(xì)碎曲折,十分不利于AGV 的平穩(wěn)運(yùn)行。采用遍歷的方法對生成的原始路徑進(jìn)行后處理,使最終路徑簡潔干凈,可降低AGV 在尋跡過程中的能量消耗和物理磨損。
從終點(diǎn)開始,以倒序的方式遍歷路徑上各個節(jié)點(diǎn),尋找可與起點(diǎn)直接連通的點(diǎn)Pj,用線段連接起點(diǎn)與該點(diǎn)。然后,重新從終點(diǎn)開始遍歷可以與點(diǎn)Pj直接連通的節(jié)點(diǎn),直至連通終點(diǎn)。最終路徑將只保留起點(diǎn)、終點(diǎn)和必要轉(zhuǎn)折點(diǎn),并且各相鄰點(diǎn)之間的連線通過安全距離判斷,大大降低了路徑的曲折程度。生成最終簡潔路徑的偽代碼,如算法6所示。
算法6 路徑后處理算法偽代碼
Post-processing(Path,Z) Algorithm
1.Pstart=Path(1);Pterm=Path(end);
2.TidyPath=[Pstart];
3.forj=size(Path):-1:1
4.if CheckWidth(Pstart,Pj,Z) then
5.TidyPath=[TidyPath;Pj];
6.Pstart=Pj;
7.ifPstart!=Ptermthen
8.j=size(Path);end
9.end;end
10.return TidyPath;
為了驗(yàn)證改進(jìn)Bi-RRT 算法的有效性和優(yōu)越性,在以2.6 GHz CPU為處理器、內(nèi)存8G的PC機(jī)上,基于Matlab 2016b編程,分別對RRT算法、Bi-RRT算法以及改進(jìn)Bi-RRT算法進(jìn)行仿真實(shí)驗(yàn),通過對比來驗(yàn)證改進(jìn)算法的優(yōu)勢。
根據(jù)AGV現(xiàn)實(shí)工作環(huán)境,繪制兩張復(fù)雜度不同的二維地圖,并在算法中直接將地圖導(dǎo)入為二進(jìn)制圖像,手動設(shè)置起點(diǎn)和終點(diǎn)坐標(biāo)。地圖中黑色區(qū)域表示障礙物所在位置,白色區(qū)域表示AGV可到達(dá)空間。藍(lán)色星狀點(diǎn)表示起點(diǎn),綠色三角點(diǎn)表示終點(diǎn)。根據(jù)AGV尺寸與環(huán)境對比數(shù)據(jù),設(shè)定步長和安全距離值。
傳統(tǒng)RRT算法、Bi-RRT算法、改進(jìn)Bi-RRT的直接連通算法以及路徑后處理算法在簡單和復(fù)雜地圖中的仿真結(jié)果,如圖4、圖5所示。從圖4(a)和5(a)中可以看出,傳統(tǒng)RRT算法由于缺乏目標(biāo)導(dǎo)向性,需要在全地圖中進(jìn)行隨機(jī)采集樣點(diǎn),即使在障礙物較少的簡單地圖中,隨機(jī)樹也會在目標(biāo)點(diǎn)附近徘徊,生成路徑曲折且效率極低。傳統(tǒng)Bi-RRT 算法在RRT 算法的基礎(chǔ)上,添加了從終點(diǎn)生長的隨機(jī)樹,增強(qiáng)了路徑生成的效率,但仍產(chǎn)生了大量的隨機(jī)節(jié)點(diǎn),且同樣存在路徑與障礙物貼近的現(xiàn)象。如在圖4(b)與圖5(b)中,傳統(tǒng)Bi-RRT 算法中兩棵隨機(jī)樹相連的周圍空白區(qū)域存在許多無意義的樹節(jié)點(diǎn),最終生成的紅色路徑與多處障礙物貼合。
圖4 簡單地圖仿真結(jié)果Fig.4 Simulation Results in a Simple Map
圖5 復(fù)雜地圖仿真結(jié)果Fig.5 Simulation Results in a Complex Map
如圖4(c)與圖5(c)所示,改進(jìn)Bi-RRT算法加入直接連通判斷機(jī)制后,大大減少了隨機(jī)樹節(jié)點(diǎn)擴(kuò)展過程中路徑連通前的計(jì)算資源與時間損耗。尤其在簡單地圖中,從起點(diǎn)出發(fā)的隨機(jī)樹在雙重目標(biāo)偏向策略的引導(dǎo)下,繞過障礙物,便能直接連通到終點(diǎn),節(jié)省了計(jì)算時間。在復(fù)雜地圖中也明顯簡化了兩棵隨機(jī)樹的相遇過程。而且改進(jìn)算法有效規(guī)避了路徑與障礙物貼合的現(xiàn)象發(fā)生。圖4(d)與圖5(d)則展示了改進(jìn)Bi-RRT算法對所得路徑進(jìn)行后處理的效果。效果顯示,處理后路徑更平直,轉(zhuǎn)彎節(jié)點(diǎn)更少,滿足安全距離判斷。
不同算法在兩種復(fù)雜程度地圖中仿真50次后的實(shí)驗(yàn)數(shù)據(jù),如表1、表2所示。根據(jù)表1數(shù)據(jù),改進(jìn)Bi-RRT算法在簡單地圖中的平均路徑生成時間低于傳統(tǒng)RRT算法的十分之一,約為傳統(tǒng)Bi-RRT算法的三分之一。表2則顯示出,對于復(fù)雜地圖,改進(jìn)Bi-RRT算法的平均路徑生成時間約是傳統(tǒng)RRT的三分之一,傳統(tǒng)Bi-RRT算法的一半。改進(jìn)Bi-RRT算法生成的路徑節(jié)點(diǎn)均為RRT算法與Bi-RRT 算法的一半。而耗時極短的路徑后處理取得的效果十分顯著,大大刪減了路徑節(jié)點(diǎn),獲得平直的最終路徑,更利于實(shí)際應(yīng)用中AGV的運(yùn)行。
表1 簡單地圖中仿真數(shù)據(jù)對比Tab.1 Comparison of Simulation Data in a Simple Map
基于傳統(tǒng)Bi-RRT算法提出的AGV全局路徑規(guī)劃改進(jìn)算法,將雙重目標(biāo)偏向策略應(yīng)用于隨機(jī)樣點(diǎn)和最近點(diǎn)的選取,在隨機(jī)樹的擴(kuò)展過程中加入直接連通判斷與安全距離判斷機(jī)制,最后對生成路徑進(jìn)行后處理,給出了算法偽代碼來描述實(shí)現(xiàn)過程,并對算法性能進(jìn)行仿真驗(yàn)證。實(shí)驗(yàn)表明,改進(jìn)Bi-RRT算法相比于傳統(tǒng)Bi-RRT算法,能在二維地圖上更高效快速地規(guī)劃出一條概率更優(yōu)且有利于AGV 實(shí)際執(zhí)行的簡潔路徑,構(gòu)建了智能生產(chǎn)線中AGV 自主運(yùn)行的基礎(chǔ)。后續(xù)將對該算法進(jìn)行深入改進(jìn),實(shí)現(xiàn)AGV在行駛過程中的實(shí)時避障功能。