葉鴻達(dá)， 黃 山， 涂海燕

(四川大學(xué)電氣工程學(xué)院，成都 610000)

0 引言

移動機器人具有自主運行、移動靈活等特點，所以在國防科技、生活服務(wù)、生產(chǎn)建設(shè)等重要領(lǐng)域具有廣闊的開發(fā)前景。移動機器人不斷普及，若行進路徑效率不高，則會嚴(yán)重影響其工作質(zhì)量[1]。移動機器人路徑規(guī)劃指在障礙物環(huán)境下，機器人從起始狀態(tài)到目標(biāo)狀態(tài)找到一條滿足自身和環(huán)境約束的無碰撞路徑[2]。常見的路徑規(guī)劃算法有人工勢場法[3-4]、蟻群算法[5]、JPS算法[6]和A*算法[7]等。當(dāng)機器人自由度、環(huán)境建模的復(fù)雜度增加時，以上算法的規(guī)劃復(fù)雜度也會呈指數(shù)增長，從而出現(xiàn)“維度災(zāi)難”問題[8]。因此，上述算法不適合解決高復(fù)雜環(huán)境下的規(guī)劃問題。

因不需要對環(huán)境進行精確建模，基于隨機采樣的規(guī)劃算法在多維空間中具有明顯的優(yōu)勢而得到廣泛關(guān)注[9]。快速探索隨機樹(Rapidly-exploring Random Tree，RRT)[10]算法在具有復(fù)雜環(huán)境的多維空間中可以進行有效的路徑搜索，對要求微分約束的系統(tǒng)，RRT算法也展現(xiàn)出優(yōu)異的性能。RRT算法可快速得到一條無碰撞路徑，但該路徑存在轉(zhuǎn)角過多、非最優(yōu)、未考慮動力學(xué)約束等問題。針對這些問題，國內(nèi)外已進行了大量的研究工作。文獻(xiàn)[11]提出了EPF-RRT算法，該算法在復(fù)雜環(huán)境下速度更快、路徑質(zhì)量更優(yōu)，然而EPF-RRT算法雖具有概率完備性，但也無法找到最優(yōu)路徑。文獻(xiàn)[12]提出的EGK-RRT算法在滿足動力學(xué)約束的情況下，相比經(jīng)典RRT算法顯著減少了執(zhí)行時間，在特殊環(huán)境下，比如狹窄通道，其性能超過了RRT算法，但由于EGK-RRT算法不提供最優(yōu)路徑，其生成的路徑長度略大于RRT算法生成的路徑。

針對RRT算法生成路徑質(zhì)量不高、非最優(yōu)等問題，文獻(xiàn)[13]提出了RRT*算法，該算法在RRT算法的基礎(chǔ)上引入了路徑代價函數(shù)和重布線操作,通過重新選擇父節(jié)點，使得路徑的代價函數(shù)值最優(yōu)。但是，大多數(shù)采樣算法都具有隨機性，隨著環(huán)境復(fù)雜程度和障礙物數(shù)量的增加，花費在碰撞檢測和重布線等操作上的時間也隨之增加，相比RRT算法，在路徑質(zhì)量更優(yōu)的情況下，收斂到最優(yōu)路徑的時間卻更多。針對碰撞檢測導(dǎo)致計算效率降低的問題，文獻(xiàn)[14]提出一種Lazy-PRM*算法，該算法在通過PRM*建立的拓?fù)鋱D上使用Dijkstra或者A*算法進行路徑搜索，然后再進行碰撞檢測，從而提高算法的執(zhí)行效率；隨著采樣節(jié)點的增多，其算法效率會降低但路徑質(zhì)量卻沒有顯著提高。JORDAN等提出了Bi-RRT*算法[15]，該算法從初始位置和目標(biāo)位置分別擴展生長樹，當(dāng)兩棵樹之間的最短距離小于閾值Slim，就表示兩樹相遇，然后從相遇節(jié)點各自回溯到根節(jié)點，完成路徑的規(guī)劃；該方法減小了目標(biāo)點位置對規(guī)劃的影響，使得目標(biāo)點不局限在一個位置，大幅度提高了RRT*算法的收斂速度，從而提高了整體規(guī)劃效率。

針對以上算法存在的計算效率低、收斂時間長等問題，本文提出了一種改進Bi-RRT*算法。該算法隨著路徑長度的變化，采樣空間也隨之優(yōu)化，從而減少無效節(jié)點的擴展；在節(jié)點擴展階段，引入目標(biāo)偏向策略，降低算法擴展的盲目性，以降低算法執(zhí)行的隨機性；結(jié)合一種NCC-RRT*算法[16]，對碰撞檢測機制進行優(yōu)化，并結(jié)合雙向擴展機制，顯著提高了算法的執(zhí)行效率；最后，對基礎(chǔ)Bi-RRT*算法執(zhí)行流程進行優(yōu)化，并對生成的路徑進行最優(yōu)節(jié)點篩選，提高路徑的質(zhì)量和算法對內(nèi)存的利用率。通過仿真實驗證實了該算法的有效性。

1 問題描述

本章將會對路徑規(guī)劃的可行性和最優(yōu)性問題進行描述[2]。在進行路徑規(guī)劃之前需要對環(huán)境信息建模。本文首先對障礙物進行離散化，使用有限均勻的點表示連續(xù)的障礙物。

定義X=(0,1)d是移動機器人的d維配置空間，d∈N,d≥2。其中，Xobs為障礙物空間，Xfree為自由空間，且Xfree=cl(X/Xobs)，cl(·)為封閉集合。定義迭代次數(shù)N∈N，初始狀態(tài)xinit∈Xfree，目標(biāo)狀態(tài)xgoal∈Xfree，||

xi,xi+1||

表示配置空間中任意兩個狀態(tài)點之間的標(biāo)準(zhǔn)歐氏距離，并且對?xi,i∈N滿足：||

xi,xi+1||

=||

xi+1,xi||

≥0。?xi,xj∈X,L(xi,xj)表示連接xi和xj的線段，c(xi,xj)表示L(xi,xj)的代價函數(shù)值，c(xi)表示從根節(jié)點到xi的代價函數(shù)值。

定義1路徑σ:[0,1]→Rd是有界函數(shù):

1) 如果它是連續(xù)的，被稱為路徑;

2) 如果它是路徑，且σ(τ)∈Xfree,?τ∈[0,1]，被稱為無碰撞路徑;

3) 如果它是無碰撞路徑，并且滿足σ(0)=Xinit，σ(1)∈cl(Xgoal)，被稱為可行路徑。

問題1 可行路徑規(guī)劃：考慮一個路徑規(guī)劃問題(Xfree,Xinit,Xgoal)，可以找到一條路徑σ:[0,1]→Xfree，并且σ(0)=Xinit，σ(1)∈cl(Xgoal)，否則返回失敗。

問題2 最優(yōu)路徑規(guī)劃：考慮一個路徑規(guī)劃問題(Xfree,Xinit,Xgoal)和一個代價函數(shù)c:∑→R+，可以找到一條最優(yōu)的可行路徑σ*，并且c(σ*)=min{c(σ)}，其中σ是可行路徑，否則返回失敗。

2 改進Bi-RRT*算法

2.1 基礎(chǔ)Bi-RRT*算法

基礎(chǔ)Bi-RRT*算法在保留RRT*算法特性的同時，引入了雙向隨機樹擴展機制，從而加快了算法收斂速度，使RRT*算法的效率得到大幅度提升。

算法1 Bi-RRT*算法偽代碼

V←{xinit,xgoal};E←?;i←0;

Ta←{xinit,E};Tb←{xgoal,E};

cbest←∞;

fori=1 toNdo

xrand←Sample(i);

i←i+1;

(V,E)←Extend(Ta,Xrand);

xconnect←Nearest(Tb,V.xnew);

ccost←ConnectTree(Tb,xconnect,xnew);

ifccost

returnTa,Tb=(V,E)；

首先定義從起點擴展的樹為Ta，從終點擴展的樹為Tb。然后進行算法初始化，設(shè)置兩棵生成樹的初始位置xinit，xgoal和總迭代數(shù)N，并對地圖進行離散化。通過Sample(i)進行隨機均勻采樣，分別從xinit和xgoal節(jié)點使用Extend(Ta,xrand)擴展隨機節(jié)點xrand，然后通過Nearest(Tb,V.xnew)在樹Tb上找離V.xnew(表示xnew∈V,下同)最近的節(jié)點xconnect，再通過ConnectTree(Tb,xconnect,xnew)連接xconnect和xnew，并在迭代數(shù)N內(nèi)不斷優(yōu)化xconnect和xnew之間的代價值，每次迭代結(jié)束通過SwapTree(Ta,Tb)使兩棵樹交替生長。相比Bi-RRT*算法， RRT*算法在執(zhí)行Extend(Ta,xrand)函數(shù)后便進行下一次路徑優(yōu)化迭代。

算法2Extend(G,xrand)

V′←V;E′←E;

xnearest←Nearest(G,xrand);

xnew←Steer(xnearest,xrand);

ifNotObstacleFree(xnew,xnearest) then

Xnear←NearVertex(G,xnew,|V|);

for allxnear∈Xneardo

ifNotObstacleFree(xnew,xnear) then

_continue;

_ccost=c(xnear)+c(xnew,xnear) ;

c(xnew)=min(ccost)

xmin=xnear;

V′←V′ {xnew};

E′←E′ {xnew,xmin};

for allxnear∈Xnearxmindo

ifNotObstacleFree(xnew,xnear) then

ifc(xnear)>c(xnew)+c(xnew,xnear) then

xparent←xnear.parent;

E′←E′{xparent,xnear};

_E′←E′{xnew,xnear};

-

returnG′=(V′,E′);

其中：Nearest(G,xrand)在樹G中搜索離xrand最近的節(jié)點xnearest；Steer(xnearest,xrand)在xnearest和xrand連線上，是以xnearest為起點擴展相應(yīng)步長得到的節(jié)點xnew；NearVertex(G,xnew,|V|)在以xnew節(jié)點為圓心、以R為半徑的范圍內(nèi)搜索樹G上的節(jié)點，組成集合Xnear；第7～11行偽代碼計算xnew的最小代價函數(shù)值min(ccost)，并將其設(shè)為xnew的代價函數(shù)值，將對應(yīng)的節(jié)點xmin設(shè)為xnew的父節(jié)點，然后更新節(jié)點集V′和邊集E′。第13～21行偽代碼更新Xnear的父節(jié)點及其代價函數(shù)值。NotObstacleFree(xi,xj) 為碰撞檢測函數(shù)，表示xi和xj連線之間存在障礙物。

2.2 NCC-RRT*算法

NCC-RRT*算法主要用來解決RRT*在碰撞檢測上耗費大量計算時間而產(chǎn)生收斂速度慢、效率低及容易被困于狹窄通道環(huán)境等問題[15]。NCC-RRT*算法的核心思想是采用碰撞風(fēng)險評估策略代替原始的碰撞檢測策略，從而剔除碰撞檢測函數(shù)，提高RRT*算法的收斂速度和效率。

2.2.1 碰撞風(fēng)險評估函數(shù)

Line(xnew,xnear)的碰撞風(fēng)險評估函數(shù)[15]表示為

(1)

式中:N表示在邊L(xnew,xnear)上均勻選擇N個點；H(pi)表示任意一個點pi的碰撞風(fēng)險

(2)

式中：n表示環(huán)境中障礙物點的數(shù)量；hj(pi)表示任意一個障礙物點對點pi的影響

(3)

式中，d表示環(huán)境中障礙物點到pi的歐氏距離。

GaussCollisionAssess(xnew,xnear)取值范圍為0～1，cGauss=kp×GaussCollisionAssess(xnew,xnear)表示邊L(xnew,xnear)實際碰撞情況，當(dāng)cGauss≥kp/10時，邊L(xnew,xnear)必定發(fā)生碰撞。

2.2.2 改進碰撞風(fēng)險評估函數(shù)

NCC-RRT*算法使用地圖中所有障礙物點評估L(xnew,xnear)的碰撞情況，但當(dāng)離散障礙物點數(shù)量較多時，GaussCollisionAssess(xnew,xnear)的時間復(fù)雜度遠(yuǎn)遠(yuǎn)超過NotObstacleFree(xi,xj) ?；诖藛栴}，本文在L(xnew,xnear)上僅使用邊上存在的障礙物點O3對p1進行碰撞風(fēng)險評估，如圖1所示。

圖1 L(xnew,xnear)碰撞風(fēng)險評估Fig.1 Collision risk assessment of L(xnew,xnear)

在圖2中實驗，靜態(tài)環(huán)境地圖大小為x=y=600 m，起點為[200 m，300 m]，終點為[400 m，300 m]，擴展步長為15 m，標(biāo)準(zhǔn)差σ=2.2，kp=360000，記錄30次實驗的平均數(shù)據(jù)作為結(jié)果保存在表1中。

圖2 靜態(tài)環(huán)境地圖Fig.2 Static environment map

表1 碰撞風(fēng)險評估函數(shù)計算時間數(shù)據(jù)Table 1 Calculation time of collision risk assessment function s

如圖2所示，在L(p1,p3)上均勻選擇3個點p4,p5,p6，將L(p4,p6)上的障礙物透明化以方便識別p5。標(biāo)準(zhǔn)碰撞風(fēng)險評估函數(shù)使用環(huán)境中所有障礙物對點p4,p5,p6進行風(fēng)險評估，改進后把L(p4,p6)上的障礙物離散為n個障礙物點Oi，再對點p4,p5,p6進行風(fēng)險評估，其中，障礙物點Oi到點p4,p5,p6的距離用歐氏距離表示，最后把相應(yīng)的點坐標(biāo)代入式(1)，即可評估點p4,p5,p6各自的碰撞風(fēng)險。

根據(jù)表1數(shù)據(jù)可知，標(biāo)準(zhǔn)碰撞風(fēng)險評估函數(shù)的計算時間大約是0.29 s，與環(huán)境中障礙物數(shù)量成正比，對其改進之后，計算時間與待評估的邊上存在的障礙物點數(shù)量相關(guān)，計算時間大約是0.001 2 s。環(huán)境中所有障礙物點和邊上存在的障礙物點數(shù)量的差距與環(huán)境地圖相關(guān)，由于圖2大小為X=Y=600 m，環(huán)境中障礙物數(shù)量和邊上的數(shù)量差距極大，因此，算法改進后的計算效率得到大幅度提高。

2.3 動態(tài)目標(biāo)區(qū)域采樣

標(biāo)準(zhǔn)RRT*算法使用全局均勻采樣策略獲得地圖的環(huán)境信息，卻導(dǎo)致樹生長具有隨機性、計算效率低和內(nèi)存空間大量浪費等問題產(chǎn)生，因此，本文提出一種動態(tài)目標(biāo)區(qū)域采樣策略，其原理如圖3所示。

圖3 動態(tài)圓形區(qū)域采樣策略Fig.3 Sampling strategy of dynamic circular area

該采樣策略在未找到可行路徑前，以矩形地圖的短邊d1為直徑構(gòu)建圓形采樣區(qū)域R1，當(dāng)搜尋到可行路徑，便以路徑長度d2為直徑構(gòu)建圓形采樣區(qū)域R2，在路徑不斷優(yōu)化的過程中，圓形采樣區(qū)域會逐漸縮小，從而減少無效采樣節(jié)點數(shù)，提高算法收斂速度和內(nèi)存利用率，構(gòu)建動態(tài)圓形區(qū)域采樣的偽代碼如下。

算法3 動態(tài)圓形區(qū)域采樣算法偽代碼

r=radius×sqrt(rand(1));

seta=2×π×rand(1);

point.x=circle_center.x+radius×cos(seta);

point.y=circle_center.y+radius×sin(seta);

returnpoint；

算法原理：其中rand(1)均勻生成0～1之間的隨機數(shù)，r∈[0,radius],seta∈[0,2π],通過極坐標(biāo)法均勻生成在以circle_center為圓心、以radius為半徑圓內(nèi)的采樣點。

2.4 扇形區(qū)域樹枝生長策略

盡管動態(tài)目標(biāo)區(qū)域采樣策略使采樣節(jié)點盡可能分布在有用的區(qū)域，但樹生長依然具有隨機性，為使樹生長具有導(dǎo)向性，結(jié)合貪心思想和人類直觀思維，本文提出一種扇形區(qū)域樹枝生長策略，其原理如圖4所示。

圖4 扇形區(qū)域樹枝生長策略Fig.4 Branch growth strategy in sector area

2.5 路徑優(yōu)化

針對Bi-RRT*算法生成的初始路徑存在大轉(zhuǎn)角、交叉線和冗余節(jié)點從而導(dǎo)致算法收斂速度慢以及在復(fù)雜環(huán)境下不適合機器人進行軌跡跟蹤等問題，本文對標(biāo)準(zhǔn)Bi-RRT*算法的優(yōu)化策略進行改進，主要分兩個階段。第一階段：采用快速擴展優(yōu)化策略，在未搜索到初始路徑之前采用雙樹擴展機制，加快路徑搜索速度，在搜索到初始路徑后，提取Tb樹中存在于路徑中的節(jié)點并與Ta樹組合成一棵單樹，在剩下的迭代次數(shù)中對組合的Ta樹采用RRT*算法的優(yōu)化思路進行重布線操作。第二階段：算法收斂后生成的初始路徑往往存在冗余的節(jié)點，使得路徑不夠平滑，針對此問題，通過冗余節(jié)點剔除方法[17]對路徑進行優(yōu)化，從而得到適合機器人跟蹤的路徑。如圖4所示，定義初始路徑節(jié)點集合為V，從xgoal開始依次遍歷集合中的每個節(jié)點xi，如果L(xgoal,xi)邊上無碰撞發(fā)生，此時的節(jié)點xi為冗余節(jié)點，如果發(fā)生碰撞，則碰撞點的父節(jié)點為有效節(jié)點，將其加入集合V′，通過該方法得到的優(yōu)化路徑如圖5實線所示，其中虛線為初始路徑。

圖5 冗余節(jié)點剔除Fig.5 Redundant node elimination

3 仿真實驗與分析

3.1 實驗環(huán)境

假設(shè)機器人為理想圓點，為驗證改進Bi-RRT*算法的有效性和搜索效率，在Matlab R2019b軟件中進行實驗。PC配置：Windows 10 OS，CPU為Intel Core i5-9400F，RAM為16 GiB，分別對RRT*,NCC-RRT*,Bi-RRT*和改進Bi-RRT*算法進行仿真，為確保數(shù)據(jù)準(zhǔn)確性，統(tǒng)計30次實驗的平均數(shù)據(jù)作為實驗結(jié)果。

3.2 實驗分析

3.2.1 靜態(tài)環(huán)境地圖一

靜態(tài)環(huán)境地圖一大小為x=y=600 m，規(guī)劃起點為[100 m，300 m]，終點為[500 m，300 m]，擴展步長為15 m，標(biāo)準(zhǔn)差σ=2，kp=360 000，路徑長度標(biāo)準(zhǔn)閾值設(shè)為495 m，當(dāng)采樣節(jié)點數(shù)大于15 000或路徑長度小于標(biāo)準(zhǔn)閾值時停止搜索。圖6對比RRT*,NCC-RRT*,Bi-RRT*和改進Bi-RRT*算法在靜態(tài)環(huán)境地圖一中的仿真結(jié)果；表2記錄了每個算法30次實驗的平均數(shù)據(jù)。

圖6 靜態(tài)環(huán)境地圖一仿真結(jié)果Fig.6 Simulation result of static environment map Ⅰ

表2 靜態(tài)環(huán)境地圖一仿真實驗數(shù)據(jù)Table 2 Simulation experiment data of static environment map Ⅰ

3.2.2 靜態(tài)環(huán)境地圖二

路徑長度的標(biāo)準(zhǔn)閾值設(shè)置為755 m，其余參數(shù)同2.2.2節(jié)。當(dāng)采樣節(jié)點數(shù)大于15 000或路徑長度小于標(biāo)準(zhǔn)閾值時停止搜索。圖7對比了RRT*,NCC-RRT*,Bi-RRT*和改進Bi-RRT*算法在靜態(tài)環(huán)境地圖二中的仿真結(jié)果；表3為每個算法30次實驗的平均數(shù)據(jù)。

圖7 靜態(tài)環(huán)境地圖二仿真結(jié)果Fig.7 Simulation result of static environment map Ⅱ

根據(jù)表2數(shù)據(jù)可知，在靜態(tài)環(huán)境地圖一中RRT*和Bi-RRT*算法由于節(jié)點采樣和樹擴展的隨機性，相比NCC-RRT*和改進Bi-RRT*算法需要更多的搜索時間和迭代次數(shù)才能收斂；由于NCC-RRT*算法使用碰撞風(fēng)險評估策略代替了碰撞檢測，NCC-RRT*算法的執(zhí)行效率得到大幅度提高，在迭代上限內(nèi)NCC-RRT*算法可以收斂；根據(jù)表3數(shù)據(jù)可知，靜態(tài)環(huán)境地圖二的類型不屬于S型，其最短路徑和搜索復(fù)雜度比靜態(tài)環(huán)境地圖一更復(fù)雜，其搜索時間和迭代次數(shù)均顯著增加，但RRT*,NCC-RRT*和Bi-RRT*算法仍能在迭代上限15 000次內(nèi)收斂；綜合表2、表3的數(shù)據(jù)可知，由于改進Bi-RRT*算法使用了改進碰撞風(fēng)險評估策略，并且在節(jié)點采樣機制、樹生長的導(dǎo)向性等方面做出了改進，其執(zhí)行效率相比RRT*,NCC-RRT*和Bi-RRT*算法提高了約54%,38%和30%。

表3 靜態(tài)環(huán)境地圖二仿真實驗數(shù)據(jù)Table 3 Simulation experiment data of static environment map Ⅱ

4 結(jié)論

針對RRT*算法在道路狹窄、障礙物較多的環(huán)境下執(zhí)行效率低、收斂速度慢和路徑曲折等問題，本文結(jié)合NCC-RRT*算法，并對其碰撞風(fēng)險評估函數(shù)進行改進，再通過動態(tài)目標(biāo)區(qū)域采樣、扇形區(qū)域樹枝生長和路徑優(yōu)化等方法，提出了一種改進Bi-RRT*算法。仿真實驗表明，在相同的迭代上限和收斂閾值內(nèi)，改進Bi-RRT*算法相比NCC-RRT*算法提高了路徑質(zhì)量和搜索效率，顯著減少了迭代次數(shù)，在更短的時間內(nèi)可收斂到滿足要求的最優(yōu)路徑。