黃 元， 徐志博， 孫浩博， 岳一杰， 萬(wàn)芳新

(甘肅農(nóng)業(yè)大學(xué)機(jī)電工程學(xué)院，甘肅 蘭州 730070)

隨著人工智能的發(fā)展，機(jī)器人被許多國(guó)家列為一項(xiàng)重點(diǎn)發(fā)展的新技術(shù)，并已被應(yīng)用在諸多領(lǐng)域[1]。路徑規(guī)劃是機(jī)器人技術(shù)的重要組成部分，其算法的好壞會(huì)直接影響機(jī)器人的作業(yè)質(zhì)量和效率。對(duì)于果園機(jī)器人，路徑規(guī)劃是實(shí)現(xiàn)其作業(yè)目的的基礎(chǔ)。牛潤(rùn)新[2]等提出了一種基于激光雷達(dá)的樹干檢測(cè)算法，解決了丘陵山區(qū)果園坡度和雜草對(duì)果樹檢測(cè)精度的影響，為精確農(nóng)業(yè)設(shè)備在丘陵山地果園的導(dǎo)航應(yīng)用提供參考。為了解決導(dǎo)航機(jī)器人在溫室果園環(huán)境中的快速工作問題，劉繼展[3]等提出了一種基于電子羅盤和激光雷達(dá)航向信息融合的位姿檢測(cè)方法，完成了自主導(dǎo)航系統(tǒng)的快速啟動(dòng)，實(shí)現(xiàn)了線角的優(yōu)化。為了解決二維激光掃描儀在果園導(dǎo)航中感知信息少，不能有效處理復(fù)雜的三維果園場(chǎng)景的問題，劉偉洪[4]等提出了一種基于三維激光雷達(dá)的果園線對(duì)線導(dǎo)航方法，該系統(tǒng)可廣泛應(yīng)用于標(biāo)準(zhǔn)果園和復(fù)雜三維果園機(jī)械的自主導(dǎo)航，具有可靠的穩(wěn)定性。為了提高操作設(shè)備在果園與林行之間的自主導(dǎo)航性能，劉明星[5]等提出了一種基于最小二乘法和支持向量機(jī)融合的樹行識(shí)別與導(dǎo)航方法，該方案可有效提高果園作業(yè)機(jī)器的工作效率。

在移動(dòng)機(jī)器人路徑規(guī)劃的算法研究中，RRT算法憑借其擴(kuò)展隨機(jī)性、靈活性得到廣泛關(guān)注。Kuffner[6]等提出了改進(jìn)型RRT-connect算法，其效率相較原始RRT算法提高顯著，但目標(biāo)函數(shù)較復(fù)雜。阮曉鋼[7]等提出基于信息增益與RRT相結(jié)合的機(jī)器人環(huán)境探索策略，提高了RRT算法偏置率，但算法結(jié)構(gòu)復(fù)雜，內(nèi)存占用較大。司徒華杰[8]等提出了一種利用人工勢(shì)場(chǎng)與RRT算法相結(jié)合算法，相比傳統(tǒng)RRT算法，該算法具有更高的效率和更少的節(jié)點(diǎn)，但是其步長(zhǎng)固定，在復(fù)雜環(huán)境有一定局限性。LaValle[9]等提出了雙向搜索RRT算法，該算法提高了搜索速度，但是算法目標(biāo)偏置率仍然較低。Hidalgo[10]等在雙向RRT算法基礎(chǔ)上提出四向RRT算法，但目標(biāo)函數(shù)復(fù)雜，算法復(fù)雜度高，同時(shí)對(duì)硬件要求高。

本文在原始的RRT算法及其改進(jìn)算法基礎(chǔ)上，探索智能步長(zhǎng)對(duì)各種算法下路徑質(zhì)量的影響，為后續(xù)果園作業(yè)機(jī)器人路徑規(guī)劃的研究提供理論依據(jù)。

1 算法介紹

1.1 RRT算法

RRT(快速探索隨機(jī)樹)是一種通用算法，可用于任意類型機(jī)器人。其工作原理如圖1所示。以二維環(huán)境為例，其算法實(shí)現(xiàn)可分為3個(gè)步驟。(1)樹的生成。首先在環(huán)境中將初始節(jié)點(diǎn)定義為xinit，然后在環(huán)境中得到隨機(jī)節(jié)點(diǎn)xrand，如果xrand不在障礙物區(qū)域，則連接xinit和xrand得到一條連線L，如果L整個(gè)不在障礙物內(nèi)，則沿著L，從xinit向xrand的方向移動(dòng)一定的距離，得到一個(gè)新的點(diǎn)xnew，則xinit、xnew和它們之間的線段構(gòu)成了一顆最簡(jiǎn)單的樹。(2)樹的擴(kuò)展。繼續(xù)重復(fù)(1)樹的生成過(guò)程，在環(huán)境中取點(diǎn)，得到無(wú)障礙物區(qū)域的點(diǎn)xrand，然后在已經(jīng)存在的樹上找一個(gè)離xrand最近的點(diǎn)xnear，連接兩個(gè)點(diǎn)，如果這條線沒有與障礙物碰撞，則沿著這條線擴(kuò)展得到xnew。(3)xnew節(jié)點(diǎn)的確定。從xnear到xrand移動(dòng)一定的距離，得到新的點(diǎn)xnew，該點(diǎn)被添加到已經(jīng)存在的樹上規(guī)劃。重復(fù)上述過(guò)程，直到目標(biāo)點(diǎn)(或其附近的點(diǎn))被添加到樹上，這時(shí)就可以在樹上找到一條從起點(diǎn)到目標(biāo)點(diǎn)的路徑。

圖1 RRT算法擴(kuò)展過(guò)程

1.2 RRT*算法

利用已知信息改進(jìn)的RRT算法，被稱為RRT*算法。RRT*算法在每次迭代后，都會(huì)在局部更新搜索樹，以達(dá)到優(yōu)化路徑目的。如圖2(a)所示，在新產(chǎn)生的節(jié)點(diǎn)xnew附近即定義的半徑范圍內(nèi)尋找“近鄰”，作為替換xnew父節(jié)點(diǎn)的備選。共分為3個(gè)步驟。(1)計(jì)算xnew“近鄰”節(jié)點(diǎn)到起點(diǎn)的路徑代價(jià)。(2)計(jì)算到每個(gè)“近鄰”的路徑代價(jià)。(3)將步驟1和步驟2中的路徑代價(jià)相加，并將xnew連接到路徑代價(jià)最小的一個(gè)“近鄰”。

在為xnew重新選擇父節(jié)點(diǎn)之后，為進(jìn)一步使得隨機(jī)樹節(jié)點(diǎn)間連接的代價(jià)盡量小，需為隨機(jī)樹進(jìn)行重新布線，以得到最優(yōu)的xnew節(jié)點(diǎn)。重新布線過(guò)程如圖2(b)所示，可分為2個(gè)步驟：第一步為xnew選擇新的父節(jié)點(diǎn)，第二步借助最小路徑代為xnew選擇節(jié)點(diǎn)，從而達(dá)到優(yōu)化的效果。

1.3 雙向RRT*

為加快算法搜索速度及搜索效率，基于RRT*算法，LaValle提出了雙向RRT*算法，算法偽碼如下：

算法1：雙向RRT*算法

1.V1{qinit}；E1；G1( )；

2.V2{qgoal}；E2；G2( )；i+0；

3.While i

4. qrandSample(i)；ii+1；

5. qnearstnearst(G1，qrand)；

6. qnewSteer(qnearst，qnew)；

7. If no_collision (q1，qnew)then

8. V1{qnew}；

9. (q1，qnew)}；

15. do

20. ；

21. else break；

24. If |V2| |V1| then swap(V1，V2)；

圖2 RRT*算法

初始化時(shí)隨機(jī)樹T只包含一個(gè)節(jié)點(diǎn)，即根節(jié)點(diǎn)qinit。首先Sample函數(shù)從狀態(tài)空間中隨機(jī)選擇一個(gè)采樣點(diǎn)qrand(第4行)，然后Nearest函數(shù)從隨機(jī)樹中選擇一個(gè)距離qrand最近的節(jié)點(diǎn)qnearst(第5行)，最后Extend函數(shù)通過(guò)從qnearst向qrand擴(kuò)展一段距離，得到一個(gè)新的節(jié)點(diǎn)qnew(第8行)。如果qnew與障礙物發(fā)生碰撞，則Extend函數(shù)返回空，放棄這次生長(zhǎng)，否則qnew將加入到隨機(jī)樹中。重復(fù)上述步驟直到qnearst和目標(biāo)點(diǎn)qgoal距離小于一個(gè)閾值，則代表隨機(jī)樹到達(dá)了目標(biāo)點(diǎn)，算法返回成功(第6～7行)。

1.4 智能步長(zhǎng)算法

傳統(tǒng)RRT算法在擴(kuò)展時(shí)，如果使用固定步長(zhǎng)進(jìn)行工作，將可用節(jié)點(diǎn)相連，在無(wú)障礙區(qū)域產(chǎn)生節(jié)點(diǎn)較多。本文在RRT算法的兩種改進(jìn)算法中添加了智能步長(zhǎng)，即在節(jié)點(diǎn)探索過(guò)程中根據(jù)是否遇到障礙物自動(dòng)調(diào)節(jié)步長(zhǎng)。該改進(jìn)算法可以使節(jié)點(diǎn)在無(wú)障礙物的空曠區(qū)域加快搜索速度，擴(kuò)大搜索區(qū)域，同時(shí)節(jié)點(diǎn)進(jìn)入障礙物區(qū)域時(shí)可以保證節(jié)點(diǎn)有效擴(kuò)展，提高擴(kuò)展的效率。算法偽碼如下：

算法2：智能步長(zhǎng)算法

1.R

2.Safe_distance

3.Influence_area| (sx+R，sy+R)

4.IfSafe_distance>Influence_area：

5.d=step_size|α(α>1)

6.Else：

7.d=step_size( )

8.Returnd

其中R為安全距離，由半徑r和障礙物影響范圍的值構(gòu)成的；(sx，sy)是圓心坐標(biāo)，α、β分別為擴(kuò)展因子和收縮因子。算法由3部分組分：第一部分初始化參數(shù)(第1～3行)，第1行生成安全距離R，第2行安全距離幾何計(jì)算，第3行生成斥力影響范圍；第二部分為影響范圍判斷(第4行)，判斷安全距離與斥力影響范圍幾何關(guān)系；第三部分確定步長(zhǎng)長(zhǎng)度(第5～8行)，當(dāng)安全距離大于斥力影響范圍時(shí)，步長(zhǎng)選擇擴(kuò)展因子(第4～5行)，同理當(dāng)安全距離小于斥力影響范圍時(shí)，步長(zhǎng)選擇收縮因子(第6～7行)。完成上述步驟結(jié)束算法得到步長(zhǎng)d(第8行)。

2 算法仿真試驗(yàn)

2.1 仿真試驗(yàn)條件

為分析智能步長(zhǎng)算法的有效性，進(jìn)行算法仿真試驗(yàn)驗(yàn)證。本文用智能步長(zhǎng)替換RRT算法及改進(jìn)算法中的固定步長(zhǎng)，在Pycharm軟件中分別運(yùn)行RRT算法、RRT*算法與雙向RRT*算法。

試驗(yàn)設(shè)備如下。CPU：AMD Ryzen 5 4600H with Radeon Graphics (3.00 GHz)；運(yùn)行內(nèi)存16 G；顯卡：NVIDIA GeForce GTX1650；使用python語(yǔ)言pygame模塊，構(gòu)建信息圖，信息圖的尺寸是(80×60)與(800×600)。將原始的RRT算法、RRT*算法、雙向RRT*的算法在pycharm(2021)中做仿真分析。試驗(yàn)地圖為二維場(chǎng)景圖，并分為小型信息圖與大型信息圖。小型地圖如圖3(a)～3(d)所示，起點(diǎn)坐標(biāo)(10，10)，終點(diǎn)坐標(biāo)(50，50)；大型地圖如圖3(e)～3(f)所示，起點(diǎn)坐標(biāo)(10，10)，終點(diǎn)坐標(biāo)(790，590)。圖3中，黑色區(qū)域表示障礙物，白色區(qū)域均表示算法可覆蓋的區(qū)域，綠色線段表示擴(kuò)展步長(zhǎng)，紅色有限線段表示算法規(guī)劃出的路徑。

2.2 仿真試驗(yàn)步驟

仿真試驗(yàn)驗(yàn)證可分為4個(gè)步驟。(1)在pycharm軟件中生成地圖信息，確定起點(diǎn)、終點(diǎn)和障礙物位置；(2)在步驟(1)生成的地圖中分別運(yùn)行原始的RRT算法、RRT*算法、雙向RRT*算法，記錄其運(yùn)行時(shí)間、路徑長(zhǎng)度并保存路徑規(guī)劃圖；(3)分別運(yùn)行改進(jìn)后的三種算法，記錄其運(yùn)行時(shí)間，路徑長(zhǎng)度并保存路徑規(guī)劃圖；(4)整理步驟得到的數(shù)據(jù)，結(jié)合路徑規(guī)劃圖，對(duì)比分析試驗(yàn)數(shù)據(jù)。

2.3 仿真試驗(yàn)結(jié)果分析

圖3為仿真試驗(yàn)結(jié)果。由圖3 (a)、圖3(b)可以看出，步長(zhǎng)改進(jìn)算法對(duì)RRT算法提升不顯著，但對(duì)RRT*算法、雙向RRT*算法影響較大。

從圖3(c)、圖3(d)對(duì)比可知，傳統(tǒng)的RRT*算法相較改進(jìn)后的算法，重規(guī)劃次數(shù)多，無(wú)用節(jié)點(diǎn)多，算法占用的內(nèi)存空間較大；通過(guò)對(duì)比圖3(e)與圖3(f)可得出，智能步長(zhǎng)改進(jìn)的雙向RRT*算法相較于原始得到的雙向RRT*算法，探索范圍更大，冗余節(jié)點(diǎn)數(shù)量少，路徑較為平滑。

圖3 算法運(yùn)行結(jié)果

將RRT*算法、雙向RRT*算法及其改進(jìn)算法各運(yùn)行500次，整理所需參數(shù)如表1所示。

表1 算法運(yùn)行結(jié)果

通過(guò)表1可以得出，智能步長(zhǎng)提高了RRT*算法、雙向RRT*算法路徑質(zhì)量的部分指標(biāo)。其中路徑長(zhǎng)度相較原始算法變化并不明顯，但路徑長(zhǎng)度在近似相同的情況下算法運(yùn)行時(shí)間分別縮短8.62%、7.96%，同時(shí)改進(jìn)算法所得路徑相較初始算法生成路徑平滑度更高。

3 結(jié)論

本文提出的智能步長(zhǎng)改進(jìn)方案可適用于多種RRT改進(jìn)算法，通過(guò)引入智能步長(zhǎng)擴(kuò)展的思想，加快了隨機(jī)樹生長(zhǎng)速度，提高了算法的規(guī)劃效率，大大減少了搜索路徑的時(shí)間。通過(guò)與原始算法進(jìn)行仿真試驗(yàn)對(duì)比，證明了本文提出的智能步長(zhǎng)改進(jìn)算法在路徑規(guī)劃效率和路徑質(zhì)量上更具優(yōu)越性。研究對(duì)于移動(dòng)機(jī)器人的運(yùn)動(dòng)規(guī)劃具有一定的參考意義。