區(qū)域限制及人工勢場引導(dǎo)的改進(jìn)RRT路徑規(guī)劃算法

徐勁力，曹 其

(武漢理工大學(xué) 機(jī)電工程學(xué)院，湖北 武漢 430070)

路徑規(guī)劃是指從起始位置到目標(biāo)位置生成一條無碰撞路徑，在無人駕駛、手術(shù)醫(yī)療、航空航天[1-3]等多個(gè)領(lǐng)域都有廣泛應(yīng)用。到目前為止，根據(jù)采用的類型方法，常用的路徑規(guī)劃算法有圖搜索法[4-5]、隨機(jī)采樣法[6]、人工勢場法[7]和智能算法[8]等。其中人工勢場法算法簡單且容易實(shí)現(xiàn)，同時(shí)所規(guī)劃的路徑平滑，但容易陷入局部極小值?？焖偬剿麟S機(jī)樹(rapidly-exploring random trees, RRT)算法避免了空間建模，能夠高效解決高維空間和復(fù)雜約束的路徑規(guī)劃問題，但由于該方法在生成擴(kuò)展樹時(shí)在整個(gè)采樣空間內(nèi)隨機(jī)采樣，因此擴(kuò)展會(huì)過于平均，整個(gè)算法的效率較低。蟻群算法、神經(jīng)網(wǎng)絡(luò)法等智能算法應(yīng)用于復(fù)雜環(huán)境時(shí)需要大量迭代去尋找并優(yōu)化路徑，因此求解困難、收斂時(shí)間長。

為了提高搜索速度，Karaman等[9]提出了漸進(jìn)最優(yōu)RRT算法(RRT*)。針對RRT算法的采樣生成點(diǎn)盲目性強(qiáng)的缺點(diǎn)，Bruce等[10]將啟發(fā)思想融入RRT算法形成了Informed RRT*算法，該算法建立一個(gè)長軸上的焦點(diǎn)位于起始點(diǎn)和目標(biāo)點(diǎn)的橢圓區(qū)域，在該區(qū)域進(jìn)行隨機(jī)點(diǎn)采樣，并在路徑搜索過程中不斷調(diào)整橢圓區(qū)域，以提高隨機(jī)采樣點(diǎn)可用概率。為了利用環(huán)境信息引導(dǎo)隨機(jī)樹生長，很多學(xué)者研究了RRT算法與人工勢場法的結(jié)合。文獻(xiàn)[11-12]在人工勢場法陷入局部最小值時(shí)，利用RRT算法的隨機(jī)性選擇臨時(shí)目標(biāo)點(diǎn)，以此跳出局部極小值點(diǎn)；文獻(xiàn)[13]先采用RRT進(jìn)行全局路徑規(guī)劃，再使用人工勢場法進(jìn)行局部避障；文獻(xiàn)[14]采用人工勢場法優(yōu)化RRT*的采樣過程，減少了算法的計(jì)算量。

筆者在現(xiàn)有研究基礎(chǔ)上，針對現(xiàn)有的RRT算法隨機(jī)性強(qiáng)、沒有偏向性以及采樣空間密集等問題，提出區(qū)域限制及人工勢場引導(dǎo)的改進(jìn)RRT路徑規(guī)劃算法(AL-APFG RRT —— Goal_bias RRT with area limitation and guided by artificial potential field)。算法通過在采樣開始前根據(jù)起始位置與目標(biāo)位置，規(guī)劃包含起始點(diǎn)與目標(biāo)點(diǎn)的有界連通區(qū)域，并根據(jù)循環(huán)次數(shù)調(diào)整采樣區(qū)域；在規(guī)劃過程中根據(jù)節(jié)點(diǎn)與障礙物的相對關(guān)系自適應(yīng)調(diào)整選擇目標(biāo)點(diǎn)作為隨機(jī)點(diǎn)的概率P，在人工勢場法引導(dǎo)下，規(guī)劃出一條無碰撞的路徑。

1 傳統(tǒng)算法

1.1 RRT算法

RRT的思想是快速擴(kuò)張一群像樹一樣的路徑以探索空間的大部分區(qū)域，伺機(jī)找到可行的路徑。將起始點(diǎn)作為根節(jié)點(diǎn)，從根節(jié)點(diǎn)開始生長枝丫；在機(jī)器人的構(gòu)型空間中，生成一個(gè)隨機(jī)點(diǎn)；在樹上找到距離隨機(jī)點(diǎn)最近的那個(gè)點(diǎn)，朝著最近點(diǎn)到隨機(jī)點(diǎn)的方向生長，如果沒有碰到障礙物就把生長后的樹枝和端點(diǎn)添加到樹上，直至目標(biāo)點(diǎn)也添加到樹上，最終找到一條連接起始點(diǎn)與目標(biāo)點(diǎn)的完整路徑。經(jīng)典的RRT算法是完全隨機(jī)的，搜索過程也沒有偏向性，因此效率比較低。

Goal_bias RRT在RRT算法的基礎(chǔ)上，添加了以一定的概率P選擇目標(biāo)點(diǎn)作為隨機(jī)點(diǎn)的概率，當(dāng)隨機(jī)數(shù)小于P時(shí)，選擇目標(biāo)點(diǎn)作為隨機(jī)點(diǎn)，進(jìn)行碰撞檢測和生長；當(dāng)隨機(jī)數(shù)大于等于P時(shí)，在地圖范圍內(nèi)生成隨機(jī)點(diǎn)。從而引導(dǎo)隨機(jī)樹在一定程度上向目標(biāo)點(diǎn)生長。

1.2 人工勢場法

人工勢場法是一種將物理學(xué)中場的概念引入到路徑規(guī)劃中的算法，是應(yīng)用最廣泛的機(jī)器人路徑規(guī)劃算法之一。人工勢場法的基本思想是把機(jī)器人在環(huán)境中的運(yùn)動(dòng)視為一種在虛擬力場中的運(yùn)動(dòng)，將機(jī)器人假設(shè)成一個(gè)點(diǎn)，虛擬力場由目標(biāo)點(diǎn)對機(jī)器人的引力場和障礙物對機(jī)器人的斥力場組成。引力場由目標(biāo)點(diǎn)產(chǎn)生，斥力場是由所有的障礙物產(chǎn)生的合力場組成。因此，人工勢場法的勢場函數(shù)定義為引力場與斥力場的勢場和，如式(1)所示。

U(q)=Uatt(q)+Urep(q)

(1)

式中:q為機(jī)器人位置節(jié)點(diǎn)；Uatt(q)為引力勢場函數(shù)；Urep(q)為斥力勢場函數(shù)；U(q)為勢場函數(shù)和。

其中，引力勢場函數(shù)為：

(2)

式中：Katt為引力常數(shù);qgoal為目標(biāo)節(jié)點(diǎn)。

斥力勢場函數(shù)為：

(3)

式中：Krep為斥力常數(shù)；ρ0為障礙物影響距離；ρobs(q)為機(jī)器人位置節(jié)點(diǎn)q指向距離最近的障礙物的向量。

引力函數(shù)為：

Fatt(q)=-Katt(q-qgoal)

(4)

斥力函數(shù)為：

(5)

2 改進(jìn)算法

2.1 新節(jié)點(diǎn)的生成

Goal_bias RRT算法以一定概率選擇目標(biāo)點(diǎn)作為隨機(jī)點(diǎn)，在一定程度上使算法具有了偏向目標(biāo)點(diǎn)生長的能力，提高了搜索效率。但在搜索過程中，沒有考慮到路徑規(guī)劃環(huán)境的影響，因此采用人工勢場法的思想，將引力勢場和斥力勢場引入隨機(jī)樹的節(jié)點(diǎn)生長過程中，合理利用目標(biāo)點(diǎn)及附近障礙物的信息，計(jì)算最近點(diǎn)qnear處所受的引力和斥力，結(jié)合所生成的隨機(jī)點(diǎn)，共同決定生長方向。大多數(shù)RRT算法采用等步長擴(kuò)展的方法，不容易通過狹窄區(qū)域。筆者采用了限制最大步長的方法，使距離超過最大步長的統(tǒng)一按照最大步長擴(kuò)展，距離小于最大步長的，則直接按照隨機(jī)分量和勢場合力分量共同作用擴(kuò)展，使隨機(jī)樹容易通過狹窄區(qū)域，并遠(yuǎn)離障礙物，偏向目標(biāo)點(diǎn)生長。

新節(jié)點(diǎn)qnew的生成過程如圖1所示，其計(jì)算公式如式(6)所示。

圖1 新節(jié)點(diǎn)生成過程

(6)

式中：F為機(jī)器人在qnear處所受的合力；λ為最大步長；ε為勢場力分量因子，取(0,1)之間的數(shù)，通過調(diào)整ε的大小可以調(diào)整勢場對新節(jié)點(diǎn)生成方向的影響；qrand為隨機(jī)點(diǎn)。

2.2 自適應(yīng)概率采樣

Goal_bias RRT按照一定概率選取目標(biāo)點(diǎn)作為隨機(jī)點(diǎn)，一定程度上引導(dǎo)隨機(jī)樹向著目標(biāo)點(diǎn)生長，同時(shí)還有優(yōu)先生長一條分支的特性，有利于快速規(guī)劃出一條可行路徑。但是由于Goal_bias RRT算法中選擇目標(biāo)點(diǎn)作為隨機(jī)點(diǎn)的概率p是一個(gè)固定值，使得在障礙物環(huán)境復(fù)雜或狹窄區(qū)域時(shí)，選取目標(biāo)點(diǎn)為隨機(jī)點(diǎn)往往是無效的。因此筆者提出了一種根據(jù)樹上節(jié)點(diǎn)與障礙物環(huán)境的相對關(guān)系，自適應(yīng)的調(diào)節(jié)概率p的方法。

當(dāng)最近點(diǎn)qnear附近有障礙物時(shí)，減小選擇目標(biāo)點(diǎn)作為隨機(jī)點(diǎn)的概率p，以便盡快繞開障礙物。當(dāng)目標(biāo)點(diǎn)qgoal附近有障礙物時(shí)，如果選擇目標(biāo)點(diǎn)作為隨機(jī)點(diǎn)，不容易擴(kuò)展新節(jié)點(diǎn)，因此容易陷入局部極小值，為了減少這種情況，也減小選擇目標(biāo)點(diǎn)作為隨機(jī)點(diǎn)的概率p。概率的獲得方法如下：

(7)

式中：pmax為最大概率；ρq2obs為節(jié)點(diǎn)到最近障礙物的距離；ρgoal2obs為目標(biāo)點(diǎn)到最近障礙物的距離。

2.3 區(qū)域限制

為了減少計(jì)算量和過多的搜索空間，增強(qiáng)計(jì)算效率，在算法開始采樣前，在配置空間Z中確定一個(gè)包含起始位置和目標(biāo)位置的連通區(qū)域，如圖2所示，該連通區(qū)域以起始點(diǎn)A與目標(biāo)點(diǎn)B的連線為角平分線。起始搜索區(qū)域的角度為30°，如圖2(a)所示。隨著搜索次數(shù)的增加，連線AC與AD分別沿逆時(shí)針方向與順時(shí)針方向旋轉(zhuǎn)一定的角度(15°)以此擴(kuò)大采樣區(qū)域，直到生成全局路徑或者將區(qū)域逐步擴(kuò)展到全圖，如圖2(b)所示，在區(qū)域擴(kuò)展為210°時(shí)找到一條可行路徑。

圖2 采樣區(qū)域的擴(kuò)展

3 仿真分析

為了驗(yàn)證所提出的算法AL_APFG RRT 的有效性和收斂速度，在MATLAB 2018a中進(jìn)行了多組對比實(shí)驗(yàn)。仿真在聯(lián)想小新V3000(CPU：Intel Core i7-5500U(2.4 GHz/L3 4 M)；內(nèi)存：8 GB DDR3L 1600；硬盤：500 GB HDD，8 GB；顯卡：AMD Radeon R5 M230獨(dú)立顯卡)上進(jìn)行。在仿真環(huán)境中的參數(shù)設(shè)置如表1所示。對基本RRT算法、Goal_bias RRT 算法、人工勢場引導(dǎo)的Goal_bias RRT算法(APFGRRT)[14]和AL_APFG RRT 算法進(jìn)行仿真，給出4種算法最終生成的隨機(jī)樹和連接節(jié)點(diǎn)的最終路徑，對這幾種算法在相同地圖環(huán)境和相同參數(shù)下進(jìn)行50次重復(fù)測試，取平均值作為實(shí)驗(yàn)結(jié)果。

表1 參數(shù)設(shè)置

圖3中，x軸和y軸的坐標(biāo)范圍均為[1,800]，A為起始點(diǎn)，B為目標(biāo)點(diǎn)，*節(jié)點(diǎn)軌跡是根據(jù)樹上節(jié)點(diǎn)規(guī)劃出的軌跡。圖3是在一般場景下4種算法的表現(xiàn)。

圖3 4種算法在一般場景中的表現(xiàn)

從圖3(a)可知，一般RRT算法在空間中的采樣過于均勻，樹上節(jié)點(diǎn)過多，且有很多無用節(jié)點(diǎn)；從圖3(b)可知，Goal_bias RRT相對于基本RRT算法節(jié)點(diǎn)數(shù)已明顯減少；從圖3(c)可知，APFG RRT算法節(jié)點(diǎn)數(shù)進(jìn)一步減少，但不容易跳出局部極小值，經(jīng)過從頭生成另一個(gè)分支后才尋到路徑；從圖3(d)可知，AL_APFG RRT算法改善了前面3種算法采樣過于平均、不容易跳出局部極小值的缺點(diǎn)，以較少的節(jié)點(diǎn)數(shù)完成了路徑規(guī)劃。

表2為一般場景中4種算法的規(guī)劃時(shí)間、節(jié)點(diǎn)數(shù)量和迭代次數(shù)。

表2 4種算法在一般場景中的實(shí)驗(yàn)結(jié)果對比

從表2可知，筆者提出的算法在一般場景中路徑規(guī)劃所耗費(fèi)的平均時(shí)間上相對于初始的RRT算法減少了61.17%，平均節(jié)點(diǎn)數(shù)減少了68.13%；相對于Goal_bias RRT算法平均耗時(shí)減少了57.56%，平均節(jié)點(diǎn)數(shù)減少了42.94%；相對于APFGRRT算法平均耗時(shí)減少了41.46%，平均節(jié)點(diǎn)數(shù)減少了18.90%。

表3為狹縫場景中4種算法的實(shí)驗(yàn)結(jié)果。圖4為在狹縫場景下4種算法的表現(xiàn)。從圖4可知，在狹窄場景中，AL_APFG RRT算法相對于其他3種算法更容易找到可行路徑。

圖4 4種算法在狹縫場景中的表現(xiàn)

表3 4種算法在狹縫場景中的實(shí)驗(yàn)結(jié)果對比

從表3可知，筆者所提出的算法在狹縫場景中路徑規(guī)劃所耗費(fèi)的平均時(shí)間上相對于初始的RRT算法減少了86.41%，平均節(jié)點(diǎn)數(shù)減少了81.15%；相對于Goal_bias RRT算法平均耗時(shí)減少了72.36%，平均節(jié)點(diǎn)數(shù)減少了68.59%；相對于APFG RRT算法平均耗時(shí)減少了73.36%，平均節(jié)點(diǎn)數(shù)減少了61.47%

由于人工勢場的引入以及隨機(jī)取樣點(diǎn)的區(qū)域限制增加了迭代次數(shù)和計(jì)算量，導(dǎo)致本文算法的迭代次數(shù)有較大增加，但同時(shí)大大減少了無效節(jié)點(diǎn)的數(shù)量，進(jìn)而提高了搜索效率。由上面的結(jié)果可知，無論是平均規(guī)劃時(shí)間還是平均節(jié)點(diǎn)數(shù)，筆者所提出的算法均優(yōu)于前面3種算法。

4 結(jié)論

筆者提出了AL_APFG RRT算法的路徑規(guī)劃算法，改進(jìn)了RRT算法在取樣區(qū)域過于平均的問題，并且在擴(kuò)展新節(jié)點(diǎn)時(shí)充分利用了周圍的環(huán)境信息，增強(qiáng)了算法在采樣中的偏向性，路徑規(guī)劃的搜索效率進(jìn)一步提高。仿真結(jié)果表明，該算法在一般場景和狹縫場景中耗時(shí)更短，平均節(jié)點(diǎn)數(shù)更少，對機(jī)器人的路徑規(guī)劃研究有一定的參考意義。