多策略模式下RRT算法的優(yōu)化*

笪 晨，宋天麟，施 維

(蘇州大學(xué)應(yīng)用技術(shù)學(xué)院，昆山 215300)

0 引言

隨著時代的發(fā)展，機(jī)器人在人類的生活中扮演著非常重要的角色，機(jī)器人底盤的穩(wěn)定、高效是移動機(jī)器人工作的基礎(chǔ)。路徑規(guī)劃是指機(jī)器人在復(fù)雜環(huán)境中，根據(jù)自身傳感器信息特征和環(huán)境地圖，在一定時間內(nèi)規(guī)劃出來的一條無碰撞地從起點(diǎn)到終點(diǎn)的技術(shù)。

近年來，國內(nèi)外許多學(xué)者都提出了路徑優(yōu)化策略，比如基于搜索方法的Dijkstra、A*算法[1]；基于采樣的RRT算法；以及遺傳、粒子群、蟻群等智能算法[2]。最近隨著神經(jīng)網(wǎng)絡(luò)的興起，有些學(xué)者采用殘差卷積神經(jīng)網(wǎng)絡(luò)[3]和卷積評估[4]進(jìn)行路徑規(guī)劃算法的研究；但是面對復(fù)雜的工作環(huán)境和快速導(dǎo)航的要求，傳統(tǒng)的方法顯得力不從心，而新方法處在實驗階段。RRT是一種基于隨機(jī)采樣的快速路徑規(guī)劃算法，具有良好的概率完備性和漸近最優(yōu)性但是也存在搜索區(qū)域大，路徑不平滑等缺點(diǎn)[5]。ADNAN等[6]結(jié)合地形粗糙度，找到最近鄰的地形，基于歐氏距離進(jìn)行導(dǎo)航，以此找到目標(biāo)位置，但是移動機(jī)器人大多工作在平坦的環(huán)境，該算法局限性較大。劉恩海、陳俠等[7-10]針對RRT采樣的隨機(jī)性，結(jié)合人工勢場法，處理目標(biāo)點(diǎn)、最近點(diǎn)、障礙點(diǎn)之間引力與斥力的關(guān)系，為樹的生長點(diǎn)明方向，但是人工勢場法本身計算量較大，容易陷入局部最優(yōu)解，繼而影響引導(dǎo)點(diǎn)的選擇。郗楓飛等[11]采用植物生長的方法，利用植物生長的三大原則設(shè)置增長引導(dǎo)點(diǎn)。臧強(qiáng)等[12]根據(jù)障礙物與擴(kuò)展樹之間的距離設(shè)置了動態(tài)步長來加快算法的收斂速度。上述改進(jìn)的算法中，大多學(xué)者選擇設(shè)置引導(dǎo)點(diǎn)和動態(tài)步長來加快算法的收斂速度，且取得了較好的結(jié)果。

本文在復(fù)雜的大環(huán)境下，基于前人的研究，分析原生RRT算法流程，提出了一種多策略模式下改進(jìn)的RRT算法。優(yōu)化場景地圖，提出了目標(biāo)點(diǎn)采樣的新方法，設(shè)置動態(tài)步長以優(yōu)化路徑，最后通過仿真實驗，驗證M-RRT算法的有效性。

1 M-RRT算法

1.1 傳統(tǒng)RRT算法

RRT算法是通過隨機(jī)采樣的方法構(gòu)建一棵從起點(diǎn)Pinit到終點(diǎn)Pgoal的搜索樹，隨機(jī)獲得生成點(diǎn)Prand，根據(jù)條件在生成樹上尋找符合條件的Pnear，然后沿著Prand，Pnear的方向生長Step長度得到Pnew。

圖1 標(biāo)準(zhǔn)RRT算法

圖1為標(biāo)準(zhǔn)RRT算法的生成樹，算法的隨機(jī)性導(dǎo)致計算量增大，生長點(diǎn)缺乏方向；路徑規(guī)劃的長度與時間不穩(wěn)定。圖中淺灰色線框增大了搜索的時間，點(diǎn)A、B、C可以通過優(yōu)化來縮短距離，固定步長的Step生長速度緩慢。因此本文提出了M-RRT算法來解決這些缺點(diǎn)，表1為該算法的偽代碼。

表1 M-RRT算法

1.2 導(dǎo)航地圖優(yōu)化

在工程應(yīng)用當(dāng)中，工程師會對地圖進(jìn)行手動修改與優(yōu)化，銳化障礙物邊緣，連接斷層區(qū)域，這樣可以減少機(jī)器人導(dǎo)航的時間。復(fù)雜的環(huán)境中存在各種各樣的物體，龍建全等[13]通過過濾掉較小的障礙物來尋找引導(dǎo)點(diǎn)，但是這樣風(fēng)險較大障礙物的選擇沒有科學(xué)標(biāo)準(zhǔn)。本文基于圖像形態(tài)學(xué)處理的方法來處理障礙物，圖2中安全區(qū)黑方塊部分代表機(jī)器人，以半個單位進(jìn)行膨脹，在四鄰域上劃定安全區(qū)域，保證在導(dǎo)航過程中機(jī)器人與障礙物之間的安全。理論上說機(jī)器人可以到達(dá)凸形障礙物周圍的任何地方，但是對凹形障礙物有一定的選擇性。對障礙物1和2進(jìn)行膨脹后，對比發(fā)現(xiàn)凹形障礙物仍然存在可通過區(qū)域，所以需要進(jìn)行重新標(biāo)定。

膨脹后的障礙物仍然是不規(guī)則的，不利于算法的執(zhí)行，需要獲得它們的最小外接圓或外接矩形，為了得到最小的障礙物面積，防止對障礙物進(jìn)行過處理，圖3展示了外接圓和外接矩形的獲取過程，淺灰色部分為障礙物的最小外接矩形。

圖2 障礙物膨脹 圖3 外接圓，矩形

通過預(yù)處理的方法，合理處理地圖中所有的障礙物，為模擬植物生長提供較為規(guī)則的生長區(qū)域，防止光線進(jìn)入凹槽中陷入“黑洞”，同時也為樹的生長提供引導(dǎo)點(diǎn)。

1.3 M-RRT算法原理

1.3.1 模擬植物生長算法

科學(xué)家將生物群體及個體對環(huán)境的自適應(yīng)模式作為算法設(shè)計的思想啟發(fā)源。植物作為自然界最成功的物種，能夠快速的適應(yīng)周圍環(huán)境，逐步形成了對光、溫度、機(jī)械振動、水、重力、電信號和化學(xué)物質(zhì)的感知能力和應(yīng)對措施，這些因素都可以與工作環(huán)境中的參數(shù)相映射。

圖4 引導(dǎo)點(diǎn)獲取策略

植物生長主要有3大原則：向光性、遮擋物影響、負(fù)向地。植物的頂端永遠(yuǎn)是向著光繞開障礙物背著地進(jìn)行生長。對于導(dǎo)航系統(tǒng)而言，機(jī)器人永遠(yuǎn)從起點(diǎn)出發(fā)，向著終點(diǎn)避開障礙物進(jìn)行移動。光照是影響植物生長的主要因素，圖4中灰色枝芽代表當(dāng)前點(diǎn)Current Point，太陽Goal表示終點(diǎn)，枝芽可以在Current Area區(qū)域內(nèi)朝著任意方向生長。從當(dāng)前點(diǎn)出發(fā)沿著太陽方向進(jìn)行生長，灰色直線代表生長方向growth dir。面對圖中復(fù)雜的障礙物，這肯定不是一個最好的選擇。因此，選擇從Goal處發(fā)射出多條光線，遇到障礙物進(jìn)行反射，每次反射能量損耗20%，最后在反射光線的交匯處得到多個次級太陽點(diǎn)Sub Goal，將這些點(diǎn)作為樹生長過程中的引導(dǎo)點(diǎn)。

1.3.2 頂芽生長點(diǎn)獲取

植物從根節(jié)點(diǎn)B0開始生長，節(jié)點(diǎn)的生長與光照密切相關(guān)，如圖5所示，當(dāng)生長到B2點(diǎn)的時候，選擇距離陽光最近的3個引導(dǎo)點(diǎn)G1、G2、G3。引導(dǎo)點(diǎn)對B2點(diǎn)在x方向與y方向影響如式(1)所示，g(Gi)表示當(dāng)前引導(dǎo)點(diǎn)的光照強(qiáng)度值。

(1)

根據(jù)植物概率生長模型[15]，利用式(2)和式(3)考慮周圍環(huán)境對生長點(diǎn)的影響，在每個引導(dǎo)點(diǎn)上定義適應(yīng)度函數(shù)，gi為Gi點(diǎn)的能量；s為Gi與B2之間的障礙物影響；hi為終點(diǎn)與Gi的距離函數(shù)。B2的生長點(diǎn)選擇與函數(shù)g、h、s相關(guān)。設(shè)定α、β、γ3個影響因子，fi表示各個引導(dǎo)點(diǎn)對B2的綜合影響力。

(2)

h=h(dis(Goal,Gi))i=1,2,3
s=s(dis(Goal,Gi))i=1,2,3

(3)

1.4 動態(tài)步長生長

根據(jù)fi可以確定生長方向，在圖5中B2、G2之間存在受障礙物的情形與G1、G3點(diǎn)的生長策略有所不同。

為了加快規(guī)劃速度，式(4)提出了自適應(yīng)步長的策略。首先判斷B2與Gi之間是否存在無障礙物，無障礙物時設(shè)定獎勵措施，直接將Gi選為生長點(diǎn)如圖6所示；當(dāng)存在障礙物時，根據(jù)B2與障礙物之間的距離設(shè)置懲罰措施，dis表示障礙物與B2的距離，比較dis與step的大小設(shè)置不同的策略，為了保證陽光的唯一性，每個引導(dǎo)點(diǎn)只能使用一次。

(4)

圖5 引導(dǎo)策略 圖6 生長點(diǎn)選擇策略

1.5 航跡優(yōu)化

M-RRT生長點(diǎn)的選擇具備一定的隨機(jī)性，導(dǎo)致規(guī)劃的路徑蜿蜒曲折，路徑中存在很多冗余的節(jié)點(diǎn)需要優(yōu)化。從起始點(diǎn)B0開始，依次判斷與B2～Bn的連線，假設(shè)Bi與B0的連線第一次經(jīng)過障礙物，那么將Bi-1定義為新的B1點(diǎn)，依次類推直至終點(diǎn)。這樣能夠剔除路徑中多余的導(dǎo)航點(diǎn)，圖7表示剪枝優(yōu)化過程。為了消除導(dǎo)航過程中出現(xiàn)的拐點(diǎn)，以拐點(diǎn)Bi為圓心，生長步長為半徑，在相鄰節(jié)點(diǎn)的連線上獲得兩個固定點(diǎn)P0，P2，引入?yún)?shù)t，結(jié)合P1點(diǎn)，用二階貝塞爾曲線化路徑，如圖8所示。

圖7 剪枝優(yōu)化 圖8 貝塞爾曲線

2 M-RRT實驗分析

2.1 環(huán)境準(zhǔn)備

為了驗證M-RRT算法，設(shè)置了20 m×20 m和50 m×50 m兩種面積不同的地圖，在上面分別放置簡單和復(fù)雜的障礙物。在Python3.9開發(fā)環(huán)境，利用Dijkstra和A*算法在兩種地圖中進(jìn)行采樣測試，計算導(dǎo)航的路徑長度和時間，以此作為縱向?qū)Ρ葪l件；其次將M-RRT算法與RRT和Informed RRT*算法作橫向?qū)Ρ茸C明算法的真實、有效性。

2.2 采樣測試

Dijkstra算法可以獲得最優(yōu)的導(dǎo)航路徑，但是規(guī)劃時間較長，只適用于小規(guī)模導(dǎo)航的場景；A*算法兼顧路徑與時間，在實際生產(chǎn)當(dāng)中應(yīng)用廣泛。首先對地圖進(jìn)行預(yù)處理，將機(jī)器人當(dāng)成質(zhì)點(diǎn)處理。圖9為20 m×20 m地圖在簡單障礙物下的測試結(jié)果，圖10為50 m×50 m地圖在復(fù)雜障礙物下的測試結(jié)果，表2選取了100次規(guī)劃結(jié)果的平均值，得到最優(yōu)路徑長度和規(guī)劃時間，機(jī)器人半徑選擇0.1 m，每次搜尋的步長設(shè)置為0.4 m。

圖9 20 m×20 m采樣 圖10 50 m×50 m采樣

表2 Dijkstra，A*100次結(jié)果平均表

2.3 簡單障礙物實驗

2.3.1 實驗準(zhǔn)備

圖11 20 m×20 m地圖中的引導(dǎo)點(diǎn)

利用光線照射障礙物發(fā)生反射的原理，在圖11中畫出從光源處以30°發(fā)射出的4條光線，用4種不同的顏色表示，在障礙物區(qū)域反射10次，計算2條光線交叉處的坐標(biāo)和能量，表3選取了部分引導(dǎo)點(diǎn)，接著在RRT算法中增加引導(dǎo)點(diǎn)，根據(jù)式(2)和式(3)來引導(dǎo)樹的生長。

表3 部分引導(dǎo)點(diǎn)坐標(biāo)能量表

2.3.2 實驗結(jié)果

選取RRT、Informed RRT*、以及M-RRT算法進(jìn)行對比，實驗選取起點(diǎn)和終點(diǎn)為(2,3)、(18,15)搜索步長為0.4 m，迭代次數(shù)為3000次。

圖12 M-RRT導(dǎo)航

圖12為M-RRT的導(dǎo)航結(jié)果，圖中灰色的*號表示引導(dǎo)點(diǎn)，黑色曲線為初始路徑，淺灰色的線為剪枝優(yōu)化后的結(jié)果，黑色虛線的為二階貝塞爾曲線優(yōu)化拐點(diǎn)后的路徑。選取RRT系列3種的算法，進(jìn)行100次實驗，得到規(guī)劃路徑與時間的平均值，表4為3種算法的對比結(jié)果。

表4 20 m×20 m地圖100次實驗對比表

經(jīng)過多次實驗，測定式(2)中α=0.5，β=0.4，γ=0.1為最優(yōu)值，為了防止陷入局部解，分別設(shè)定10%，15%，75%的概率指向終點(diǎn)、隨機(jī)點(diǎn)和引導(dǎo)點(diǎn)。

2.4 復(fù)雜障礙物實驗

圖13為在復(fù)雜障礙物地圖中設(shè)置的5條光線，計算得到引導(dǎo)點(diǎn)的能量與坐標(biāo)。

選取RRT、Informed RRT*、以及M-RRT算法進(jìn)行對比，實驗選取起點(diǎn)和終點(diǎn)為(2,3)、(45,35) 搜索步長為1 m，迭代次數(shù)為3000次。

在50 m×50 m的地圖中，存在多種不規(guī)則的障礙物中，圖14為M-RRT算法的導(dǎo)航結(jié)果，黑色曲線為初始路徑，淺灰色的線為優(yōu)化后的結(jié)果，表5為3種算法的對比結(jié)果。

圖13 50 m×50 m地圖中的引導(dǎo)點(diǎn) 圖14 M-RRT導(dǎo)航

表5 50 m×50 m地圖下100次實驗對比表

2.5 實驗分析

為了驗證M-RRT算法的效果，本文綜合對比了Dijkstra、A*算法、RRT系列算法的實驗數(shù)據(jù)。圖15、圖16為5種算法的路徑規(guī)劃時間和距離在兩種地圖下的對比。Dijkstra和A*算法在小地圖中規(guī)劃時間和距離有明顯的優(yōu)勢，但是在大地圖中，RRT系列算法的優(yōu)勢就更加明顯。

圖15 時間對比 圖16 路徑對比

表4和表5分析了RRT系列算法的實驗結(jié)果，標(biāo)準(zhǔn)RRT算法路徑的標(biāo)準(zhǔn)差最大，路徑最長，Informed RRT*引入了生長區(qū)域，限制了樹的生長方向，一定程度上減少了路徑的標(biāo)準(zhǔn)差，但是增長區(qū)域本身的不確定性，導(dǎo)致規(guī)劃時間的提高，出現(xiàn)規(guī)劃失敗的場景。在地圖面積小，障礙物少的時候，引導(dǎo)點(diǎn)數(shù)量偏??；而在大地圖中，障礙物多，引導(dǎo)點(diǎn)數(shù)量變多。在M-RRT算法中，引導(dǎo)點(diǎn)為光線反射的交點(diǎn)，遠(yuǎn)離障礙物，能夠很好表示環(huán)境中障礙物的密集程度，在小地圖導(dǎo)航的過程中，M-RRT算法沒有明顯的優(yōu)勢，甚至因為引導(dǎo)點(diǎn)數(shù)量偏少，導(dǎo)致出現(xiàn)了6%的規(guī)劃失敗率。

在大地圖中，隨著引導(dǎo)點(diǎn)數(shù)量的增多，引導(dǎo)點(diǎn)遠(yuǎn)離障礙物，可以減少迭代過程中碰撞檢測的次數(shù)，以此減少規(guī)劃的時間；利用引導(dǎo)點(diǎn)之間的連通性概率較大的特性，動態(tài)設(shè)置樹的生長步長；最后通過剪枝優(yōu)化和貝塞爾曲線得到光滑的導(dǎo)航路徑。表5顯示，M-RRT算法得到的路徑較Dijkstra、Informed RRT*算法減少了6%和3%，時間比最快的RRT減少了53%，不僅如此，在大量的實驗下，成功率達(dá)到了100%，規(guī)劃結(jié)果得到的時間標(biāo)準(zhǔn)差和距離標(biāo)準(zhǔn)差更加小，算法更加穩(wěn)定。所以本文提出的M-RRT導(dǎo)航算法能夠在大地圖、復(fù)雜地形下得到良好的導(dǎo)航效果。

3 結(jié)束語

本文首先分析了RRT算法存在的不足，設(shè)置引導(dǎo)點(diǎn)和動態(tài)步長的改進(jìn)措施。利用植物生長的特性設(shè)置引導(dǎo)點(diǎn)，綜合起點(diǎn)、引導(dǎo)點(diǎn)、終點(diǎn)的關(guān)系設(shè)置動態(tài)步長，利用圖像處理的方式對地圖進(jìn)行預(yù)先處理。基于以上多種改進(jìn)措施，提出了多策略模式下的改進(jìn)RRT算法。M-RRT算法優(yōu)勢體現(xiàn)在于：

(1)面對工作場景中復(fù)雜的障礙物，利用OpenCV圖像技術(shù)優(yōu)化地圖，得到障礙物的最小外接圓或外接矩形，將障礙物根據(jù)機(jī)器人的半徑進(jìn)行膨脹，將機(jī)器人作為質(zhì)點(diǎn)，提升算法的計算速度。

(2)利用植物生長的向光性原則，根據(jù)光線的反射原理在地圖中加入了引導(dǎo)點(diǎn)，綜合生長點(diǎn)、引導(dǎo)點(diǎn)和終點(diǎn)的多種關(guān)系，定義了適應(yīng)度函數(shù)，使得采樣點(diǎn)更加的多樣化。

(3)根據(jù)引導(dǎo)點(diǎn)與生長點(diǎn)的連線與障礙物之間的關(guān)系，設(shè)置了動態(tài)步長，同時設(shè)置多種判斷策略，進(jìn)行引導(dǎo)點(diǎn)與終點(diǎn)之間規(guī)劃。

與常用的算法相比，M-RRT算法在小地圖、低障礙物場景下，優(yōu)勢不大，但是在大地圖、復(fù)雜障礙物的

場景下，不僅可以減少規(guī)劃的時間與距離，還可以保證算法的穩(wěn)定性。本文設(shè)計的光線反射程序，需要規(guī)則的障礙物邊緣，勢必會導(dǎo)致機(jī)器人工作區(qū)間的減少，導(dǎo)致路徑不可達(dá)的現(xiàn)象，下一步將繼續(xù)優(yōu)化程序，對膨脹后的障礙物進(jìn)行合理性標(biāo)定，同時將繼續(xù)優(yōu)化極端條件下M-RRT算法的導(dǎo)航效果。