三維環境中機器人路徑規劃算法改進

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

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

0 引 言

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

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

1 算法基礎

1.1 RRT*算法

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

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

圖1 RRT*重選父節點過程

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

圖2 RRT*重布線過程

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

1.2 蟻群算法

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

(1)

蟻群算法的信息素更新規則為

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

(2)

(3)

(4)

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

2 ACO-RRT*算法設計

2.1 RRT*算法改進

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

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

(5)

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

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

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

(6)

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

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

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

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

(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);

//遍歷近鄰節點, 尋找最低代價節點

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

(14)ifLine(Xnew,Xnear)

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

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

(16)Xmin←Xnear;

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

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

(19)returnTA,B(G)

2.2 ACO-RRT*算法設計

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

步驟1 初始化算法各項參數，輸入機器人三維環境模型。

步驟2

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

圖3 路徑代價判斷

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

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

步驟4 剔除起點和終點，若RRT*算法擴展的新節點與蟻群算法中某個體的節點記錄列表產生重合，即當第一個重合的路徑節點被找到時，停止路徑搜索，輸出路徑。

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

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

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

(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; //更新父節點

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

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

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

(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//判斷路徑節點是否重合

(22) coutpath

2.3 B樣條曲線平滑策略

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

由公式

(7)

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

(8)

(9)

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

(10)

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

(11)

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

(12)

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

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

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

2.4 設計路徑平滑度函數

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

(13)

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

(14)

(15)

(16)

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

3 仿真實驗結果與分析

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

3.1 仿真實驗環境

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

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

表1 仿真環境描述

3.2 仿真實驗結果

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

表2 算法參數

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

圖5 環境1中3種算法路徑規劃結果

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

圖6 環境2中3種算法路徑規劃結果

3.3 仿真實驗對比分析

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

(1)路徑長度

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

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

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

(2)運行時間

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

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

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

(3)路徑平滑度

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

圖8 環境1中3種算法多實驗組平滑度對比

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

圖9 環境2中3種算法多實驗組平滑度對比

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

4 結束語

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