楊小月，李宏偉，秦雨露，姜懿芮，王步云

(1.鄭州大學(xué) 計(jì)算機(jī)與人工智能學(xué)院，河南 鄭州 450001；2.鄭州大學(xué) 地球科學(xué)與技術(shù)學(xué)院，河南 鄭州 450001)

0 引 言

針對(duì)RRT*算法在復(fù)雜環(huán)境中搜索存在的問(wèn)題，Wang X等[3]提出一種自適應(yīng)擴(kuò)展雙向RRT*(AEB-RRT*)算法，該算法能成功用于六自由度弧焊機(jī)器人路徑規(guī)劃問(wèn)題。在雙向快速探測(cè)隨機(jī)樹(shù)BI-RRT算法的基礎(chǔ)上，Wang等[4]提出一種運(yùn)動(dòng)約束雙向快速探測(cè)隨機(jī)樹(shù)(KB-RRT*)算法，通過(guò)引入運(yùn)動(dòng)學(xué)約束，從而避免樹(shù)的不必要增長(zhǎng)。為了實(shí)現(xiàn)機(jī)器人的安全高效導(dǎo)航，Kwon等[5]提出一種基于雙樹(shù)RRT的仿車機(jī)器人軌跡規(guī)劃(CDT-RRT*)算法，在機(jī)器人運(yùn)動(dòng)學(xué)約束下快速產(chǎn)生生長(zhǎng)軌跡，但無(wú)法在極其狹窄和混亂的環(huán)境中產(chǎn)生可行軌跡。針對(duì)蟻群算法在移動(dòng)機(jī)器人路徑規(guī)劃領(lǐng)域存在的易陷入局部最優(yōu)且收斂速度較慢等問(wèn)題，李理等[6]和Luo等[7]通過(guò)加入新的影響因素去獲得信息素初始值，從而得到非均勻分布的信息素，以此來(lái)加快算法收斂速度。Jiao等[8]和江明等[9]則將傳統(tǒng)狀態(tài)轉(zhuǎn)移規(guī)則改為參數(shù)自適應(yīng)轉(zhuǎn)移，以此來(lái)提升蟻群算法的性能。趙天亮等[10]提出一種將A*算法和蟻群進(jìn)行融合的算法，Chen等[11]提出一種多策略融合蟻群算法的系統(tǒng)。這兩種方法均能提高算法的尋優(yōu)能力但結(jié)構(gòu)較為復(fù)雜。

本文首先對(duì)RRT*算法進(jìn)行改進(jìn)，再將改進(jìn)后的RRT*算法與蟻群算法進(jìn)行融合，引入雙向搜索策略，和B樣條曲線平滑策略對(duì)所得路徑進(jìn)一步優(yōu)化，使得最終生成路徑更加光滑，長(zhǎng)度更短，效率更高。

1 算法基礎(chǔ)

1.1 RRT*算法

RRT*算法相較于RRT算法主要進(jìn)行以下兩方面的改進(jìn)，分別是：為Xnew重選父節(jié)點(diǎn)和為搜索樹(shù)重布線的過(guò)程。具體過(guò)程如下[12]。

如圖1(a)所示，該圖為某刻RRT*算法在路徑搜索過(guò)程中的樹(shù)狀結(jié)構(gòu)圖，圖中線段上的數(shù)字代表節(jié)點(diǎn)之間的路徑代價(jià)值。由圖可知，該時(shí)刻算法運(yùn)行過(guò)程中所產(chǎn)生的新節(jié)點(diǎn)Xnew為10號(hào)節(jié)點(diǎn)，在為該節(jié)點(diǎn)進(jìn)行重選父節(jié)點(diǎn)的過(guò)程中，以它為中心在設(shè)定搜索范圍內(nèi)去尋找近鄰節(jié)點(diǎn)，為5、6、9號(hào)節(jié)點(diǎn)。由圖可知，到達(dá)10號(hào)節(jié)點(diǎn)的初始路徑代價(jià)為17；該節(jié)點(diǎn)的近鄰節(jié)點(diǎn)與之連線的路徑代價(jià)依次為15、12、13。若想路徑代價(jià)最小，需將10號(hào)節(jié)點(diǎn)的父節(jié)點(diǎn)由節(jié)點(diǎn)7改為節(jié)點(diǎn)6，重新連線生成的隨機(jī)樹(shù)如圖1(b)所示。

圖1 RRT*重選父節(jié)點(diǎn)過(guò)程

上述過(guò)程結(jié)束之后，再進(jìn)一步對(duì)該隨機(jī)樹(shù)進(jìn)行重布線。如圖2(a)所示，該時(shí)刻算法運(yùn)行過(guò)程中所產(chǎn)生的新節(jié)點(diǎn)Xnew為10號(hào)節(jié)點(diǎn)，近鄰節(jié)點(diǎn)分別為5、7、9號(hào)節(jié)點(diǎn)，由起點(diǎn)Xstart到近鄰節(jié)點(diǎn)連線的路徑代價(jià)依次為10、15、9。若將5號(hào)節(jié)點(diǎn)的父節(jié)點(diǎn)換為10號(hào)節(jié)點(diǎn)，則到達(dá)節(jié)點(diǎn)5的路徑代價(jià)變?yōu)?5，大于原有代價(jià)10，因此不改變其父節(jié)點(diǎn)。同理，按照此方式去處理其它節(jié)點(diǎn)。最終重新連線生成的隨機(jī)樹(shù)如圖2(b)所示。

圖2 RRT*重布線過(guò)程

上述兩個(gè)過(guò)程為RRT*算法在經(jīng)典RRT算法基礎(chǔ)上所做出的重要改進(jìn)。該算法使得隨機(jī)樹(shù)在擴(kuò)展過(guò)程中新產(chǎn)生節(jié)點(diǎn)的路徑代價(jià)盡量小，路徑更優(yōu)[13]。

1.2 蟻群算法

蟻群算法中每只螞蟻選擇路徑的依據(jù)被定義為“信息素濃度”，信息素濃度較高的路徑會(huì)大概率被選擇。同時(shí)，螞蟻會(huì)釋放定量信息素，以此形成正反饋，從而促使大多數(shù)螞蟻個(gè)體集中至一條相對(duì)較優(yōu)的路徑上[14]。該算法的具體過(guò)程如下。

(1)

蟻群算法的信息素更新規(guī)則為

τij(t+1)=(1-ρ)τij(t)

(2)

(3)

(4)

式(2)表示節(jié)點(diǎn)的信息素濃度衰減過(guò)程，其中τij(t) 表示節(jié)點(diǎn)在時(shí)刻t的信息素濃度值，ρ(0<ρ<1) 為信息素濃度衰減系數(shù)。式(3)表示三維環(huán)境中離散點(diǎn)的信息素濃度。式(4)中的length(m) 表示由m只螞蟻搜索的路徑長(zhǎng)度集合。上述搜索路徑過(guò)程不斷迭代即可尋出最優(yōu)路徑。

2 ACO-RRT*算法設(shè)計(jì)

2.1 RRT*算法改進(jìn)

為了適應(yīng)精細(xì)化三維建模環(huán)境和解決地面起伏不平坦等問(wèn)題，本文首先對(duì)傳統(tǒng)RRT*算法進(jìn)行改進(jìn)；并將改進(jìn)后的RRT*算法與蟻群算法融合，設(shè)計(jì)出適用于三維環(huán)境的ACO-RRT*路徑規(guī)劃算法。本文對(duì)RRT*算法改進(jìn)了以下幾個(gè)方面，以提升算法運(yùn)行的效率和性能。

RRT*是一種基于采樣的路徑規(guī)劃算法，但是目前將其用于柵格地圖中進(jìn)行路徑搜索的研究相對(duì)較少。本文針對(duì)三維環(huán)境中路徑起伏不平坦的問(wèn)題，將RRT*算法中的隨機(jī)采樣設(shè)置為正整數(shù)采樣，以此來(lái)適應(yīng)精細(xì)化的柵格建模環(huán)境，保證機(jī)器人以特定的步長(zhǎng)去采樣，計(jì)算公式為

(5)

式中：(x1，y1) 和 (x2，y2) 表示機(jī)器人在路徑選擇過(guò)程中路徑節(jié)點(diǎn)的坐標(biāo)。

傳統(tǒng)的RRT*算法以一定的步長(zhǎng)由Xrand向Xnear的連線方向擴(kuò)展，以此得到新節(jié)點(diǎn)Xnew。但在三維環(huán)境中，地面起伏不平坦和環(huán)境中存在障礙物這種情況是不可避免的。因此在擴(kuò)展新節(jié)點(diǎn)Xnew的過(guò)程中加入高度判斷，即判斷Xrand與Xnear連線中的所有路徑節(jié)點(diǎn)是否高于或低于所對(duì)應(yīng)障礙物的高度。若低于障礙物的高度，即發(fā)生碰撞，會(huì)出現(xiàn)路徑穿過(guò)障礙物的情況，放棄此次生長(zhǎng)，并重新進(jìn)行路徑節(jié)點(diǎn)的選??；否則，代表未發(fā)生碰撞，即該點(diǎn)可被擴(kuò)展為新節(jié)點(diǎn)。

與此同時(shí)，在一定范圍內(nèi)遍歷樹(shù)中Xnew的所有近鄰節(jié)點(diǎn)，將Xnew與近鄰節(jié)點(diǎn)依次連線，進(jìn)行碰撞檢測(cè)，并尋找路徑代價(jià)最小的近鄰節(jié)點(diǎn)。本文節(jié)點(diǎn)之間的路徑代價(jià)計(jì)算方法為三維歐氏距離，公式為

(6)

若發(fā)生碰撞，則將路徑代價(jià)記為無(wú)窮大；否則，判斷其連線是否超過(guò)障礙物高度，若沒(méi)有超過(guò)，則找到路徑代價(jià)最小的近鄰節(jié)點(diǎn)，更新父節(jié)點(diǎn)，并將該點(diǎn)加入到樹(shù)中；若超過(guò)該高度，則搜索新的節(jié)點(diǎn)。

本文改進(jìn)的RRT*算法核心偽代碼如下。

(1)V←{XA};ε←φ;TA,B←φ;Cpath←∞ //初始化各項(xiàng)參數(shù)

(2)fori=1,…,ndo//擴(kuò)展新節(jié)點(diǎn)

(3)Xrand←CeilFree;

(4)Xnear←Near(Xrand,V);

(5)Xnew←Steer(Xnear,Xrand);

(6)ifLine(Xnew,Xnear)

(7)ifCollisionFree(T(Xnear,Xnew))then

(8)V←V∪{Xnew};

(9)Xmin←Xnear;

(10)minCost←Cost(Xnew,G)+Cost(T(Xnear,Xnew));
//記錄當(dāng)前路徑代價(jià)

(11)forj=1,…,mdo

(12)Xnear←Near(Xnew,V);

//遍歷近鄰節(jié)點(diǎn), 尋找最低代價(jià)節(jié)點(diǎn)

(13)Cnew←Cost(Xnear,G)+Cost(T(Xnear,Xnew));
//記錄最新路徑代價(jià)

(14)ifLine(Xnew,Xnear)

//判斷所選新節(jié)點(diǎn)的連線是否與障礙物碰撞

(15)ifCollisionFree(T(Xnear,Xnew))&Cnew

(16)Xmin←Xnear;

(17)V←V∪{Xnew}; //將該節(jié)點(diǎn)加入到樹(shù)中

(18)ε∪(T(Xnear,Xnew));

(19)returnTA,B(G)

2.2 ACO-RRT*算法設(shè)計(jì)

RRT*算法能夠收斂到最優(yōu)路徑，且易與其它算法相結(jié)合；但是它節(jié)點(diǎn)計(jì)算量大，內(nèi)存消耗大。蟻群算法在運(yùn)算過(guò)程中，采用多個(gè)體同時(shí)進(jìn)行的方式，很大程度上改進(jìn)了算法在對(duì)大規(guī)模復(fù)雜問(wèn)題方面的計(jì)算問(wèn)題；但是它易陷入局部最優(yōu)。本文設(shè)計(jì)的ACO-RRT*算法融合了兩種算法的優(yōu)點(diǎn)，對(duì)路徑長(zhǎng)度和運(yùn)行時(shí)間進(jìn)行優(yōu)化。ACO-RRT*算法采用雙向搜索策略，在起點(diǎn)處運(yùn)行改進(jìn)后的RRT*算法，同時(shí)在終點(diǎn)處運(yùn)行蟻群算法，運(yùn)行過(guò)程中，當(dāng)找到第一個(gè)重合的路徑節(jié)點(diǎn)時(shí)，則停止路徑搜索，輸出路徑。上述過(guò)程的步驟如下。

步驟1 初始化算法各項(xiàng)參數(shù)，輸入機(jī)器人三維環(huán)境模型。

步驟2

(1)從起點(diǎn)處開(kāi)始運(yùn)行改進(jìn)后的RRT*算法：在搜索空間內(nèi)取整采樣，產(chǎn)生節(jié)點(diǎn)Xrand；之后進(jìn)行改進(jìn)后的采樣和擴(kuò)展新節(jié)點(diǎn)過(guò)程；再將Xnew與一定范圍內(nèi)的近鄰節(jié)點(diǎn)依次連線，并判斷新選擇的路徑代價(jià)是否比原存儲(chǔ)的路徑代價(jià)小，該判斷如圖3所示。由圖3可得，陰影部分為模擬障礙物，圖中線段上的數(shù)字代表節(jié)點(diǎn)之間的路徑代價(jià)值，Xnew的父節(jié)點(diǎn)為Xparent，X1和X2為指定范圍內(nèi)的近鄰節(jié)點(diǎn)。原存儲(chǔ)路徑代價(jià)為起點(diǎn)Xstart到Xnew的三維歐氏距離，由圖可知為30，記作C0。若選擇X1作為Xnew的新父節(jié)點(diǎn)，由圖可知新的路徑代價(jià)為27，記作Cnew1；若選擇X2作為Xnew的新父節(jié)點(diǎn)，由圖可知新的路徑代價(jià)為24，記作Cnew2。由于Cnew2的值最小，即路徑代價(jià)最低，故更新Xnew的父節(jié)點(diǎn)，同時(shí)進(jìn)行碰撞檢測(cè)。由圖3(a)可知選擇節(jié)點(diǎn)X2的這條路徑發(fā)生碰撞，故將Cnew2記為無(wú)窮大。最終選擇X1作為Xnew的新父節(jié)點(diǎn)，更新該節(jié)點(diǎn)路徑代價(jià)并將X1加入樹(shù)中，重新連線后如圖3(b)所示。之后再判斷Xnew與目標(biāo)點(diǎn)之間的距離是否小于步長(zhǎng)h。若小于，則停止搜索；否則重新進(jìn)行采樣。

圖3 路徑代價(jià)判斷

(2)同時(shí)從終點(diǎn)開(kāi)始運(yùn)行蟻群算法。先為每只螞蟻構(gòu)建解空間，為其概率選擇下一移動(dòng)位置，直至所有螞蟻訪問(wèn)完所有節(jié)點(diǎn)；再將螞蟻途徑路徑的距離和目前迭代過(guò)程中的最短路徑記錄下來(lái)，并更新路徑上的信息素濃度值。若尚未達(dá)到迭代次數(shù)上限，則將每只螞蟻的路徑記錄表清空，并重新進(jìn)行采樣和擴(kuò)展新節(jié)點(diǎn)過(guò)程；否則結(jié)束運(yùn)算。

步驟3 在蟻群算法中每只螞蟻以“逐層”尋找下一轉(zhuǎn)移節(jié)點(diǎn)的方式來(lái)移動(dòng)前進(jìn)，將該方式對(duì)照到三維環(huán)境模型中，可表示為沿著X方向每次前進(jìn)一層到達(dá)下一層所在平面的某一可選路徑節(jié)點(diǎn)，再在下一平面的Y方向和Z方向進(jìn)行路徑點(diǎn)搜索，并將每只螞蟻?zhàn)罱K所選擇的路徑節(jié)點(diǎn)加入到相應(yīng)的螞蟻個(gè)體節(jié)點(diǎn)記錄列表中，以此方式不斷運(yùn)行計(jì)算，直至趨向目標(biāo)點(diǎn)，獲得最終路徑。在RRT*算法子節(jié)點(diǎn)的擴(kuò)展過(guò)程中，將其父節(jié)點(diǎn)鄰近的16個(gè)節(jié)點(diǎn)作為備選的子節(jié)點(diǎn)，即在前后左右兩個(gè)柵格的距離范圍內(nèi)，對(duì)蟻群算法的路徑記錄表進(jìn)行處理。

步驟4 剔除起點(diǎn)和終點(diǎn)，若RRT*算法擴(kuò)展的新節(jié)點(diǎn)與蟻群算法中某個(gè)體的節(jié)點(diǎn)記錄列表產(chǎn)生重合，即當(dāng)?shù)谝粋€(gè)重合的路徑節(jié)點(diǎn)被找到時(shí)，停止路徑搜索，輸出路徑。

ACO-RRT*算法的核心代碼如下。

(1)V←{XA};ε←φ;TA,B←φ;Cpath←∞;G(V,E) //初始化各項(xiàng)參數(shù)

(2)fori=1,…,ndo//擴(kuò)展新節(jié)點(diǎn)

(3)Xnew←Steer(Xnear,Xrand);

(4)ifLine(Xnew,Xnear)

(5)ifCollisionFree(T(Xnear,Xnew))then

(6)V←V∪{Xnew};

(7)forj=1,…,mdo

(8)Xnear←Near(Xnew,V);

(9)ifLine(Xnew,Xnear)

(10)ifCollisionFree(T(Xnear,Xnew))&Cnew

(11)Xmin←Xnear; //更新父節(jié)點(diǎn)

(12)V←V∪{Xnew}; //將該點(diǎn)加入到樹(shù)中

(13)ε∪(T(Xnear,Xnew)); //重布線

(14)fork∈mdo//為螞蟻k構(gòu)建解空間

(15)Jk(rk1)←V- -{rk1};

(16)rk←sk;

(17) add edge (rk,sk) toTourk;

(18)procedureUpdate_pheromones; //更新信息素

(19)Tr,s←T0;

(20)ηr,s←1/Cr,s;

(21)ifTA,B(G)=G(V,E)then//判斷路徑節(jié)點(diǎn)是否重合

(22) coutpath

2.3 B樣條曲線平滑策略

針對(duì)ACO-RRT*算法所獲得的初始路徑，引入三次均勻B樣條曲線平滑策略，對(duì)路徑進(jìn)行平滑優(yōu)化。

由公式

(7)

可知，k=3時(shí)B樣條曲線數(shù)學(xué)表達(dá)式為

(8)

(9)

當(dāng)三次B樣條曲線各節(jié)點(diǎn)矢量間插值為常數(shù)時(shí)，為三次均勻B樣條曲線，第i段三次均勻B樣條曲線數(shù)學(xué)表達(dá)式為[15]

(10)

由式(7)、式(9)、式(10)可得三次均勻B樣條曲線的基函數(shù)數(shù)學(xué)表達(dá)式為

(11)

將式(11)代入式(7)可得

(12)

式(12)為三次均勻B樣條曲線數(shù)學(xué)表達(dá)式。

如圖4所示為本文引入B樣條曲線平滑策略之后的路徑仿真示意圖，圖中Start處為起始點(diǎn)，Goal處為目標(biāo)點(diǎn)，黑色陰影部分代表機(jī)器人搜索空間中的障礙物。兩點(diǎn)之間的這部分曲折實(shí)線段為未平滑前路徑，引入平滑策略之后，路徑中拐點(diǎn)周圍的折線段被曲線所代替，圖中用虛線段表示，以此獲得更短路徑。

圖4 B樣條曲線平滑路徑仿真

2.4 設(shè)計(jì)路徑平滑度函數(shù)

本文設(shè)計(jì)了路徑平滑度函數(shù)Qk來(lái)計(jì)算算法在運(yùn)行過(guò)程中路徑的總體平滑度[16]，該函數(shù)表示為

(13)

式(13)中，D1、D2、D3表示任意3個(gè)相鄰路徑節(jié)點(diǎn)之間的距離，表達(dá)式為

(14)

(15)

(16)

式(3)中所得Qk值越大，則代表相鄰路徑節(jié)點(diǎn)之間所存夾角越多，即路徑越曲折；反之，代表路徑越平滑。

3 仿真實(shí)驗(yàn)結(jié)果與分析

為了驗(yàn)證算法在三維環(huán)境中的有效性，將本文算法進(jìn)行編程，并基于Matlab2018a平臺(tái)進(jìn)行仿真實(shí)驗(yàn)驗(yàn)證。首先將不同三維環(huán)境模型數(shù)據(jù)通過(guò)Matlab中的“l(fā)oad”函數(shù)在ACO-RRT*算法的主程序中進(jìn)行加載，完成不同三維環(huán)境模型的呈現(xiàn)。通過(guò)使用Matlab中的“scatter”和“plot”繪圖函數(shù)，繪制算法在三維環(huán)境中通過(guò)搜索得到的路徑。最后編寫評(píng)估函數(shù)程序來(lái)測(cè)試算法的性能，從路徑長(zhǎng)度、運(yùn)行時(shí)間和路徑平滑度這3個(gè)維度來(lái)分析ACO-RRT*算法的有效性，并與RRT*算法和蟻群算法進(jìn)行對(duì)比研究。

3.1 仿真實(shí)驗(yàn)環(huán)境

為了驗(yàn)證本文算法在不同三維環(huán)境下的適應(yīng)性，選擇了兩種復(fù)雜程度不同的三維環(huán)境模型來(lái)模擬機(jī)器人真實(shí)的工作空間，兩種模型的大小均為21km*21km*2000m。其中每個(gè)模型中的障礙物數(shù)量、形狀及道路寬度都有所差別。

本文分別對(duì)兩種環(huán)境模型進(jìn)行“柵格化”，將機(jī)器人的工作空間沿X、Y、Z方向分解為多個(gè)大小相同的網(wǎng)格，每個(gè)網(wǎng)格即為一個(gè)單元[17]。其中曲面表面為機(jī)器人環(huán)境輪廓，凸出部分和曲面以下部分為機(jī)器人工作空間中的障礙物。將RRT*算法、蟻群算法和ACO-RRT*算法都用于三維環(huán)境模型中，搜索一條從起點(diǎn)到終點(diǎn)避開(kāi)所有障礙物的路徑，并分別設(shè)置不同的起始點(diǎn)和終點(diǎn)坐標(biāo)。仿真環(huán)境描述見(jiàn)表1。

表1 仿真環(huán)境描述

3.2 仿真實(shí)驗(yàn)結(jié)果

不失一般性，蟻群算法中信息素啟發(fā)因子為α∈[0，5]、 期望啟發(fā)因子為β∈[0，5]、 局部信息素?fù)]發(fā)率為ξ∈[0.1，0.99]、 全局信息素?fù)]發(fā)率為ρ∈[0.1，0.99]。 根據(jù)本文所使用的三維環(huán)境模型和融合算法的特性，在經(jīng)過(guò)反復(fù)測(cè)試實(shí)驗(yàn)之后，調(diào)試出相對(duì)適合本文算法的參數(shù)值，其中一些關(guān)鍵參數(shù)設(shè)置見(jiàn)表2。

表2 算法參數(shù)

本文在第一個(gè)三維環(huán)境模型中進(jìn)行機(jī)器人路徑規(guī)劃仿真測(cè)試，3種算法的路徑規(guī)劃結(jié)果示意圖如圖5所示。由圖5(a)可得RRT*算法初期雖然朝著目標(biāo)點(diǎn)方向進(jìn)行搜索，但由于該算法隨機(jī)擴(kuò)展節(jié)點(diǎn)，導(dǎo)致在起點(diǎn)處遇到障礙物時(shí)所生成的路徑較為曲折；后續(xù)則一直朝著目標(biāo)方向順利搜索。整體生成路徑的效果良好，但不夠平滑，拐點(diǎn)較多。由圖5(b)可得蟻群算法生成的路徑整體較為曲折，這是由于啟發(fā)式信息導(dǎo)致螞蟻朝著終點(diǎn)方向搜索，直至遇到環(huán)境中的障礙物時(shí)，才改變搜索方向，因此花費(fèi)較多時(shí)間，收斂速度較慢，整體路徑效果一般。由圖5(c)可得本文算法融合了改進(jìn)的RRT*算法和蟻群算法的優(yōu)點(diǎn)。該算法在尋找初始可行解時(shí)，所需擴(kuò)展節(jié)點(diǎn)更少，減少了整體路徑搜索時(shí)間；不僅能夠很好地避開(kāi)障礙物，且拐點(diǎn)較少；經(jīng)過(guò)路徑簡(jiǎn)化與平滑方法處理后，路徑更光滑，且長(zhǎng)度更短，符合預(yù)期效果。

圖5 環(huán)境1中3種算法路徑規(guī)劃結(jié)果

本文在第二個(gè)三維環(huán)境模型中進(jìn)行機(jī)器人路徑規(guī)劃仿真測(cè)試，3種算法的路徑規(guī)劃結(jié)果示意圖如圖6所示。由圖可知，該模型比第一個(gè)模型更為復(fù)雜，障礙物分布不均，地面凹凸不平，增加了路徑規(guī)劃的難度。由圖6(a)可得RRT*算法早期搜索路徑時(shí)，因避開(kāi)障礙物而浪費(fèi)過(guò)多時(shí)間，整體路徑效果一般。由圖6(b)可得蟻群算法在更為復(fù)雜的環(huán)境中適應(yīng)性能一般，路徑較為曲折，且拐點(diǎn)數(shù)目較多。由圖6(c)可得本文算法在復(fù)雜環(huán)境中生成的路徑較優(yōu)，由于雙向搜索策略的引導(dǎo)，避免了前期路徑搜索的盲目性，快速鎖定終點(diǎn)方向，很大程度上提高了路徑規(guī)劃效率，在避開(kāi)障礙物的同時(shí)又保證生成最短路徑。

圖6 環(huán)境2中3種算法路徑規(guī)劃結(jié)果

3.3 仿真實(shí)驗(yàn)對(duì)比分析

針對(duì)路徑規(guī)劃算法所具有的隨機(jī)特性，本文在兩種不同環(huán)境下分別進(jìn)行10次實(shí)驗(yàn)，并從路徑長(zhǎng)度、運(yùn)行時(shí)間和路徑平滑度這3個(gè)維度對(duì)RRT*算法、蟻群算法和本文設(shè)計(jì)的ACO-RRT*算法進(jìn)行對(duì)比分析。

(1)路徑長(zhǎng)度

在兩種不同三維環(huán)境模型中，3種算法的多組實(shí)驗(yàn)路徑長(zhǎng)度對(duì)比見(jiàn)表3。

表3 3種算法多實(shí)驗(yàn)組路徑長(zhǎng)度對(duì)比

由表3可得，在環(huán)境模型1中，本文設(shè)計(jì)的ACO-RRT*算法與RRT*算法相比，平均路徑長(zhǎng)度減少了15.9%；與蟻群算法相比，平均路徑長(zhǎng)度減少了17.6%。在比環(huán)境模型1更復(fù)雜的環(huán)境模型2中，本文設(shè)計(jì)的ACO-RRT*算法與RRT*算法相比，平均路徑長(zhǎng)度減少了12.7%；與蟻群算法相比，平均路徑長(zhǎng)度減少了18.5%。由此可得，ACO-RRT*算法在復(fù)雜化三維環(huán)境中同樣具備搜索較優(yōu)路徑的能力。根據(jù)方差可進(jìn)一步得出，ACO-RRT*算法搜索路徑的穩(wěn)定性要優(yōu)于另外兩種算法，魯棒性更強(qiáng)。

(2)運(yùn)行時(shí)間

在兩種不同三維環(huán)境模型中，3種算法的多組實(shí)驗(yàn)運(yùn)行時(shí)間平均值對(duì)比如圖7所示。

圖7 3種算法多實(shí)驗(yàn)組運(yùn)行時(shí)間對(duì)比

由圖7可知，在環(huán)境模型1中，本文設(shè)計(jì)的ACO-RRT*算法與RRT*算法相比，平均運(yùn)行時(shí)間減少了16.8%；與蟻群算法相比，平均運(yùn)行時(shí)間減少了22.5%。在環(huán)境模型2中，本文設(shè)計(jì)的ACO-RRT*算法與RRT*算法相比，平均運(yùn)行時(shí)間減少了16.9%；與蟻群算法相比，平均運(yùn)行時(shí)間減少了25.1%。ACO-RRT*算法搜索速度提升的主要原因是引入了雙向搜索策略和平滑策略，才使得該算法在三維環(huán)境中既能避開(kāi)障礙物，又能保證路徑的高效搜索。

(3)路徑平滑度

在環(huán)境1中，3種算法的多組實(shí)驗(yàn)平滑度對(duì)比如圖8所示。

圖8 環(huán)境1中3種算法多實(shí)驗(yàn)組平滑度對(duì)比

在環(huán)境2中，3種算法的多組實(shí)驗(yàn)平滑度對(duì)比如圖9所示。

圖9 環(huán)境2中3種算法多實(shí)驗(yàn)組平滑度對(duì)比

由圖8可得，在環(huán)境模型1中，本文設(shè)計(jì)的ACO-RRT*算法與RRT*算法相比，路徑平滑度平均提高了26.2%；與蟻群算法相比，路徑平滑度平均提高了20.5%。由圖9可得，在環(huán)境模型2中，本文設(shè)計(jì)的ACO-RRT*算法與RRT*算法相比，路徑平滑度平均提高了11.2%；與蟻群算法相比，路徑平滑度平均提高了20.8%。由此可得，ACO-RRT*算法在復(fù)雜三維環(huán)境中搜索的可行路徑質(zhì)量較高，路徑更平滑且耗費(fèi)時(shí)間更少。

4 結(jié)束語(yǔ)

針對(duì)RRT*算法和蟻群算法在三維環(huán)境中進(jìn)行路徑規(guī)劃時(shí)收斂速度慢和搜索具有盲目性等問(wèn)題，本文提出了一種融合算法ACO-RRT*。首先對(duì)RRT*算法進(jìn)行改進(jìn)，以此來(lái)適應(yīng)精細(xì)化三維建模環(huán)境和解決三維環(huán)境中地面起伏不平坦的問(wèn)題。之后采用雙向搜索策略，進(jìn)行算法融合，在起點(diǎn)和終點(diǎn)同時(shí)運(yùn)行改進(jìn)后的RRT*算法和蟻群算法，相向而行。最后引入B樣條曲線平滑策略，對(duì)路徑進(jìn)行平滑優(yōu)化。將本文設(shè)計(jì)的ACO-RRT*算法用于不同三維環(huán)境中，并與RRT*算法和蟻群算法進(jìn)行對(duì)比分析。仿真實(shí)驗(yàn)結(jié)果表明，ACO-RRT*算法能夠在短時(shí)間內(nèi)規(guī)劃出一條長(zhǎng)度更短、更平滑且更加穩(wěn)定的路徑，可有效用于三維環(huán)境中的路徑規(guī)劃。