三維環(huán)境中機器人路徑規(guī)劃算法改進

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

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

0 引 言

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

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

1 算法基礎

1.1 RRT*算法

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

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

圖1 RRT*重選父節(jié)點過程

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

圖2 RRT*重布線過程

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

1.2 蟻群算法

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

(1)

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

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

(2)

(3)

(4)

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

2 ACO-RRT*算法設計

2.1 RRT*算法改進

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

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

(5)

式中：(x1，y1) 和 (x2，y2) 表示機器人在路徑選擇過程中路徑節(jié)點的坐標。

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

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

(6)

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

本文改進的RRT*算法核心偽代碼如下。

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

(2)fori=1,…,ndo//擴展新節(jié)點

(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));
//記錄當前路徑代價

(11)forj=1,…,mdo

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

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

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

(14)ifLine(Xnew,Xnear)

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

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

(16)Xmin←Xnear;

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

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

(19)returnTA,B(G)

2.2 ACO-RRT*算法設計

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

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

步驟2

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

圖3 路徑代價判斷

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

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

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

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

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

(2)fori=1,…,ndo//擴展新節(jié)點

(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é)點

(12)V←V∪{Xnew}; //將該點加入到樹中

(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é)點是否重合

(22) coutpath

2.3 B樣條曲線平滑策略

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

由公式

(7)

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

(8)

(9)

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

(10)

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

(11)

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

(12)

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

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

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

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

本文設計了路徑平滑度函數(shù)Qk來計算算法在運行過程中路徑的總體平滑度[16]，該函數(shù)表示為

(13)

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

(14)

(15)

(16)

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

3 仿真實驗結(jié)果與分析

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

3.1 仿真實驗環(huán)境

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

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

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

3.2 仿真實驗結(jié)果

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

表2 算法參數(shù)

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

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

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

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

3.3 仿真實驗對比分析

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

(1)路徑長度

在兩種不同三維環(huán)境模型中，3種算法的多組實驗路徑長度對比見表3。

表3 3種算法多實驗組路徑長度對比

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

(2)運行時間

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

圖7 3種算法多實驗組運行時間對比

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

(3)路徑平滑度

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

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

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

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

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

4 結(jié)束語

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