基于改進(jìn)RRT算法的獼猴桃采摘機(jī)器人全局路徑規(guī)劃

崔永杰 王寅初 何 智,3 曹丹丹 馬 利 李 凱,3

(1.西北農(nóng)林科技大學(xué)機(jī)械與電子工程學(xué)院，陜西楊凌 712100；2.農(nóng)業(yè)農(nóng)村部農(nóng)業(yè)物聯(lián)網(wǎng)重點(diǎn)實(shí)驗(yàn)室，陜西楊凌 712100；3.陜西省農(nóng)業(yè)信息感知與智能服務(wù)重點(diǎn)實(shí)驗(yàn)室，陜西楊凌 712100)

0 引言

路徑規(guī)劃是獼猴桃采摘機(jī)器人的關(guān)鍵技術(shù)之一，主要分為基于地圖環(huán)境已知的全局路徑規(guī)劃和基于傳感器信息的局部路徑規(guī)劃[1-3]。全局路徑規(guī)劃主要包括尋找一條將起始點(diǎn)和目標(biāo)點(diǎn)連接同時(shí)避免與障礙物發(fā)生碰撞的路徑，其路徑長(zhǎng)短、算法時(shí)間和空間復(fù)雜度是評(píng)判全局路徑規(guī)劃算法性能的主要指標(biāo)[4-6]。常見(jiàn)的全局路徑規(guī)劃算法有A*算法、RRT算法、遺傳算法等[7-9]。其中由LAVALLE[10]提出的RRT算法概率完備、結(jié)構(gòu)簡(jiǎn)單且具有靈活的搜索能力，適用于各種復(fù)雜環(huán)境下的路徑規(guī)劃，但因其隨機(jī)采樣的機(jī)制而存在節(jié)點(diǎn)利用率低、路徑復(fù)雜度高且收斂速度慢等問(wèn)題[11-13]。

針對(duì)RRT算法存在的不足，學(xué)者們提出多種改進(jìn)方法，文獻(xiàn)[14]提出目標(biāo)引力-RRT算法，將人工勢(shì)場(chǎng)法中的引力思想融入RRT算法，通過(guò)采樣節(jié)點(diǎn)與目標(biāo)點(diǎn)的共同引力作用影響隨機(jī)樹(shù)的擴(kuò)展，增強(qiáng)了節(jié)點(diǎn)擴(kuò)展過(guò)程中的目標(biāo)偏向性，有效提高了路徑的搜索效率和收斂速度。文獻(xiàn)[15]提出RRT-Connect算法，該算法基于構(gòu)造兩個(gè)快速搜索隨機(jī)樹(shù)分別從起始點(diǎn)和目標(biāo)點(diǎn)生長(zhǎng)，雙向生長(zhǎng)特性有效縮短了算法收斂時(shí)間。文獻(xiàn)[16]提出RRT*算法，在隨機(jī)樹(shù)節(jié)點(diǎn)擴(kuò)展過(guò)程中引入了隨機(jī)幾何圖與剪枝優(yōu)化理論，保證了該算法的漸進(jìn)最優(yōu)性，極大降低了算法路徑復(fù)雜度。文獻(xiàn)[17]提出的Quick-RRT*算法，利用三角不等式原理改進(jìn)RRT*父節(jié)點(diǎn)選擇和重新布線(xiàn)的方式，提高了初始路徑的質(zhì)量和收斂到最優(yōu)解的速度[18-19]。

本文針對(duì)傳統(tǒng)RRT算法搜索效率低、收斂速度慢且路徑復(fù)雜度高等問(wèn)題，提出一種基于采樣狀態(tài)實(shí)時(shí)引導(dǎo)隨機(jī)樹(shù)擴(kuò)展的改進(jìn)方法。首先引入評(píng)價(jià)指數(shù)與閾值實(shí)時(shí)劃分隨機(jī)樹(shù)的采樣狀態(tài)，根據(jù)采樣狀態(tài)決定采樣節(jié)點(diǎn)的選取方式，實(shí)時(shí)引導(dǎo)隨機(jī)樹(shù)的擴(kuò)展，避免隨機(jī)樹(shù)的盲目探索。其次，為增強(qiáng)算法對(duì)不同環(huán)境的自適應(yīng)性及快速避開(kāi)不規(guī)則障礙物，引入動(dòng)態(tài)閾值及改進(jìn)最近節(jié)點(diǎn)選擇機(jī)制。最后對(duì)路徑進(jìn)行優(yōu)化處理，得到平滑且復(fù)雜度低的路徑。

1 算法原理

1.1 問(wèn)題描述

定義X=Rd為配置空間，Xobs∈X為障礙空間、Xfree∈X為無(wú)障礙空間，xinit∈Xfree為起始點(diǎn)，xgoal∈Xfree為目標(biāo)點(diǎn)。‖x1,x2‖為配置空間X中節(jié)點(diǎn)x1、x2的標(biāo)準(zhǔn)歐氏距離，設(shè)路徑β:[0,1]∈X為一有界變化函數(shù)。τ為路徑β上的點(diǎn)，如果?τ∈[0,1],β(τ)∈Xfree則路徑β為可行路徑。

問(wèn)題1：在給定的條件(Xfree，xinit，xgoal)下尋找一條可行路徑，其中β:[0,1] →Xfree,β(0)=xinit且β(1)=xgoal。

問(wèn)題2：在給定的條件(Xfree，xinit，xgoal)下在最短的時(shí)間t內(nèi)找到可行路徑β:[0,1]。

1.2 RRT算法

RRT算法基于構(gòu)造一棵隨機(jī)采樣并逐步擴(kuò)展的無(wú)沖突路徑樹(shù)，從起始點(diǎn)尋找目標(biāo)點(diǎn)[20]。隨機(jī)樹(shù)以起始點(diǎn)xinit為根節(jié)點(diǎn)，在每次迭代中從配置空間X中隨機(jī)選取一個(gè)采樣節(jié)點(diǎn)xrand，搜索隨機(jī)樹(shù)中的所有節(jié)點(diǎn)并找到距離xrand最近的節(jié)點(diǎn)記為xnearest。以xnearest為起點(diǎn)沿著xnearest和xrand的方向以一個(gè)特定的步長(zhǎng)創(chuàng)建一個(gè)新節(jié)點(diǎn)xnew，如果xnew∈Xfree且xnew和xnearest之間的連線(xiàn)σ∈Xfree，則將xnew添加到隨機(jī)樹(shù)中，并將xnearest作為它的父節(jié)點(diǎn)。計(jì)算每一個(gè)新節(jié)點(diǎn)與目標(biāo)點(diǎn)的距離，如果小于設(shè)定的值且兩點(diǎn)的連線(xiàn)與障礙物無(wú)碰撞則認(rèn)為找到路徑，將新節(jié)點(diǎn)與目標(biāo)點(diǎn)相連，通過(guò)從目標(biāo)點(diǎn)回溯到起始點(diǎn)來(lái)繪制路徑，否則重復(fù)上述搜索過(guò)程。

2 改進(jìn)RRT算法

2.1 評(píng)價(jià)指數(shù)

Straight-RRT將生成的新節(jié)點(diǎn)分為兩類(lèi)，分別是向著目標(biāo)收斂的新節(jié)點(diǎn)和趨于探索的新節(jié)點(diǎn)。設(shè)評(píng)價(jià)指數(shù)U初始值為1，實(shí)數(shù)i、e為評(píng)價(jià)指數(shù)增、減量，每當(dāng)新節(jié)點(diǎn)加入隨機(jī)樹(shù)中計(jì)算新節(jié)點(diǎn)xnew與目標(biāo)點(diǎn)xgoal的歐氏距離d=‖xnew,xgoal‖，如果新節(jié)點(diǎn)不是當(dāng)前隨機(jī)樹(shù)中最接近目標(biāo)點(diǎn)的節(jié)點(diǎn)(如圖1a所示)，則認(rèn)為該新節(jié)點(diǎn)是趨于探索的新節(jié)點(diǎn)，評(píng)價(jià)指數(shù)加i，否則認(rèn)為是向著目標(biāo)收斂的新節(jié)點(diǎn)，如圖1b所示，評(píng)價(jià)指數(shù)減e。評(píng)價(jià)指數(shù)可以實(shí)時(shí)反映隨機(jī)樹(shù)當(dāng)前的探索情況，評(píng)價(jià)指數(shù)越大表示在當(dāng)前階段內(nèi)隨機(jī)樹(shù)向著未知區(qū)域探索的點(diǎn)占比越大，向著目標(biāo)收斂的點(diǎn)占比越小。

圖1 評(píng)價(jià)指數(shù)Fig.1 Evaluation index

2.2 擴(kuò)展階段劃分

設(shè)定初始閾值為一常數(shù)a，根據(jù)評(píng)價(jià)指數(shù)是否達(dá)到閾值將隨機(jī)樹(shù)的擴(kuò)展過(guò)程分為探索階段和收斂階段。當(dāng)評(píng)價(jià)指數(shù)小于閾值時(shí)，隨機(jī)樹(shù)為探索階段，采樣節(jié)點(diǎn)依舊為自由空間內(nèi)隨機(jī)點(diǎn)，如圖2a所示。隨著迭代次數(shù)的增加，當(dāng)評(píng)價(jià)指數(shù)的值達(dá)到閾值時(shí)，說(shuō)明該階段趨于探索的點(diǎn)占比過(guò)大，隨機(jī)樹(shù)進(jìn)入收斂階段，在收斂階段隨機(jī)樹(shù)以目標(biāo)點(diǎn)作為采樣節(jié)點(diǎn)進(jìn)行擴(kuò)展，如圖2b所示。直至與障礙物發(fā)生碰撞，隨機(jī)樹(shù)結(jié)束收斂階段，如圖2c所示。隨后評(píng)價(jià)指數(shù)回到初始值1，隨機(jī)樹(shù)重新回到探索階段，如圖2d所示，當(dāng)評(píng)價(jià)指數(shù)下一次達(dá)到閾值時(shí)隨機(jī)樹(shù)會(huì)再次進(jìn)入收斂階段。通過(guò)探索和收斂階段的不斷轉(zhuǎn)換，使隨機(jī)樹(shù)在擴(kuò)展過(guò)程中能夠根據(jù)當(dāng)前的探索情況實(shí)時(shí)修正采樣節(jié)點(diǎn)的采樣位置，減少盲目探索，提高算法的收斂速度。

圖2 探索及收斂階段Fig.2 Exploration and convergence phase

2.3 改進(jìn)收斂階段最近節(jié)點(diǎn)選擇機(jī)制

每當(dāng)隨機(jī)樹(shù)結(jié)束一次收斂階段時(shí)，將開(kāi)始進(jìn)入收斂階段時(shí)的最近節(jié)點(diǎn)xnearest以及在此次收斂階段擴(kuò)展的新節(jié)點(diǎn)xnew記錄到點(diǎn)集R中，下一次隨機(jī)樹(shù)進(jìn)入收斂階段將不會(huì)選擇點(diǎn)集R中的點(diǎn)作為最近節(jié)點(diǎn)xnearest。

圖3 收斂階段最近節(jié)點(diǎn)選擇Fig.3 Selection of the nearest node in convergence stage

如圖3a所示，評(píng)價(jià)指數(shù)達(dá)到閾值隨機(jī)樹(shù)進(jìn)入收斂階段，隨機(jī)樹(shù)以節(jié)點(diǎn)x為最近節(jié)點(diǎn)向著目標(biāo)點(diǎn)擴(kuò)展，直到與障礙物發(fā)生碰撞，隨機(jī)樹(shù)結(jié)束收斂階段，將節(jié)點(diǎn)x以及在此次收斂階段擴(kuò)展的新節(jié)點(diǎn)記錄在點(diǎn)集R中，如圖3b所示(點(diǎn)集R中的點(diǎn)被標(biāo)記為黃色)。當(dāng)評(píng)價(jià)指數(shù)達(dá)到閾值隨機(jī)樹(shù)再一次進(jìn)入收斂階段時(shí)如圖3c所示，雖然節(jié)點(diǎn)y距離采樣點(diǎn)最近，但在收斂階段時(shí)點(diǎn)集R中的點(diǎn)不會(huì)被選為最近節(jié)點(diǎn)xnearest，所以點(diǎn)z作為最近節(jié)點(diǎn)xnearest向著目標(biāo)方向擴(kuò)展，直至與障礙物發(fā)生碰撞結(jié)束收斂階段，將點(diǎn)z以及此次收斂階段擴(kuò)展的新節(jié)點(diǎn)記錄到點(diǎn)集R中，如圖3d所示。通過(guò)改進(jìn)收斂階段的最近節(jié)點(diǎn)選擇機(jī)制可避免在收斂階段以重復(fù)的點(diǎn)作為最近節(jié)點(diǎn)，從而更快地避開(kāi)不規(guī)則障礙，加快算法收斂速度。

2.4 動(dòng)態(tài)閾值設(shè)定

Straight-RRT通過(guò)評(píng)價(jià)指數(shù)是否達(dá)到閾值來(lái)判斷當(dāng)前隨機(jī)樹(shù)的擴(kuò)展情況從而調(diào)整采樣方式，對(duì)于簡(jiǎn)單的環(huán)境，不需要隨機(jī)樹(shù)進(jìn)行過(guò)多的探索所以較小的閾值可以更快地找到路徑，對(duì)于障礙物較多的復(fù)雜環(huán)境需要更大的閾值以便隨機(jī)樹(shù)對(duì)環(huán)境進(jìn)行足夠的探索。為了使算法適應(yīng)各種環(huán)境，本文引入動(dòng)態(tài)閾值，將隨機(jī)樹(shù)進(jìn)入收斂階段的次數(shù)作為實(shí)時(shí)評(píng)判環(huán)境復(fù)雜度的標(biāo)準(zhǔn)，閾值T隨著隨機(jī)樹(shù)的探索過(guò)程進(jìn)行自適應(yīng)調(diào)整，閾值T表達(dá)式為

T=a+n

(1)

式中n——當(dāng)前隨機(jī)樹(shù)進(jìn)入收斂階段的次數(shù)

如圖4a所示，當(dāng)前隨機(jī)樹(shù)進(jìn)入收斂階段的次數(shù)為1，則當(dāng)前閾值為T(mén)=a+1，如圖4b所示，隨機(jī)樹(shù)進(jìn)入收斂階段的次數(shù)為2，則當(dāng)前閾值為T(mén)=a+2。動(dòng)態(tài)閾值在隨機(jī)樹(shù)擴(kuò)展過(guò)程中實(shí)時(shí)變化使隨機(jī)樹(shù)在復(fù)雜的環(huán)境下能夠進(jìn)行足夠的探索，從而提高算法對(duì)不同環(huán)境的自適應(yīng)性。

圖4 動(dòng)態(tài)閾值Fig.4 Dynamic threshold

2.5 路徑優(yōu)化

RRT算法因其隨機(jī)采樣的特性導(dǎo)致其規(guī)劃的路徑產(chǎn)生較多冗余點(diǎn)，需要對(duì)其進(jìn)行優(yōu)化處理降低路徑復(fù)雜度[21]。首先將路徑起始點(diǎn)作為修剪起始點(diǎn)，依次檢測(cè)路徑后續(xù)節(jié)點(diǎn)與修剪起始點(diǎn)相連是否與障礙物發(fā)生碰撞，如果無(wú)碰撞則刪除兩節(jié)點(diǎn)之間的路徑節(jié)點(diǎn)并直接將兩節(jié)點(diǎn)相連，隨后將新得到的路徑中的第2個(gè)點(diǎn)作為修剪起始點(diǎn)，再次執(zhí)行上述操作，直至目標(biāo)點(diǎn)成為修剪起始點(diǎn)，結(jié)束修剪過(guò)程得到最終路徑。如圖5a所示，初始路徑為1-2-3-4-5-6-7-8-9，修剪后路徑為1-5-7-9。但修剪后路徑依舊存在轉(zhuǎn)折點(diǎn)，為使路徑更加平滑，對(duì)路徑上每個(gè)轉(zhuǎn)折點(diǎn)處進(jìn)行二次貝塞爾曲線(xiàn)擬合，定義擬合空間n+1個(gè)點(diǎn)的位置Pi(i=0,1,2,…,n)，則n次貝塞爾曲線(xiàn)表達(dá)式為

(2)

其中

(3)

式中Pi——貝塞爾曲線(xiàn)的控制點(diǎn)

α——貝塞爾曲線(xiàn)參數(shù)

Bi,n(α)——n次Bernstein基函數(shù)

圖5 路徑優(yōu)化示意圖Fig.5 Schematic of path optimization

因二次貝塞爾曲線(xiàn)需3個(gè)點(diǎn)進(jìn)行擬合，為了保持?jǐn)M合曲線(xiàn)和原軌跡曲線(xiàn)一致性，如圖5b所示，設(shè)每個(gè)轉(zhuǎn)折點(diǎn)為P1，在轉(zhuǎn)折點(diǎn)兩側(cè)線(xiàn)段上分別選取兩點(diǎn)P0及P2作為貝塞爾曲線(xiàn)的控制點(diǎn)，且兩點(diǎn)與轉(zhuǎn)折點(diǎn)的距離為其所在線(xiàn)段長(zhǎng)度的1/10，對(duì)每處轉(zhuǎn)折點(diǎn)進(jìn)行二次貝塞爾曲線(xiàn)擬合，最后將多段貝塞爾曲線(xiàn)進(jìn)行拼接，得到一條復(fù)雜度低且平滑的路徑。

3 算法仿真與實(shí)驗(yàn)

3.1 不同環(huán)境下仿真實(shí)驗(yàn)

為驗(yàn)證改進(jìn)算法的有效性，將Straight-RRT算法與RRT算法、文獻(xiàn)[14]中的目標(biāo)引力-RRT算法(引力系數(shù)取0.5)、RRT-Connect算法在如圖6所示環(huán)境下進(jìn)行對(duì)比仿真實(shí)驗(yàn)，仿真實(shí)驗(yàn)中對(duì)4種算法均進(jìn)行路徑優(yōu)化處理，4種算法搜索步長(zhǎng)取2，Straight-RRT初始閾值a取5，評(píng)價(jià)指數(shù)增、減量i、e分別取1、0.5，地圖1、2、3規(guī)模均為[70，70]，在3種環(huán)境下每種算法分別進(jìn)行100次獨(dú)立運(yùn)行。算法運(yùn)行環(huán)境為64 bit Windows 10，處理器Intel(R) Core(TM) i5-6300HQ，內(nèi)存4 GB。

如圖6所示，地圖1為障礙物相對(duì)較少的環(huán)境，用于檢驗(yàn)算法在簡(jiǎn)單環(huán)境下的搜索能力，起始點(diǎn)位置為[5,35],目標(biāo)點(diǎn)位置為[65,35],地圖2為多障礙物排列雜亂的環(huán)境，用于檢驗(yàn)算法在多障礙物環(huán)境下的搜索能力，起始點(diǎn)位置為[5，5],目標(biāo)點(diǎn)位置為[65，65]，地圖3為迷宮環(huán)境，地圖起始點(diǎn)位置為[15，7],目標(biāo)點(diǎn)位置為[65，65]。算法運(yùn)行效果如圖7～9所示。從圖7～9可以看出，4種算法均能完成起始點(diǎn)到目標(biāo)點(diǎn)的全局路徑規(guī)劃，且經(jīng)過(guò)路徑優(yōu)化處理后均能獲得較為平滑的路徑。其中本文改進(jìn)的Straight-RRT算法的采樣節(jié)點(diǎn)數(shù)比其他算法明顯減少，4種算法100次運(yùn)行的平均搜索時(shí)間、平均迭代次數(shù)如表1所示。

從表1可以看出，在3種地圖環(huán)境下Straight-RRT的搜索時(shí)間及迭代次數(shù)均小于其他規(guī)劃算法，在地圖1環(huán)境下Straight-RRT算法相比于RRT算法、目標(biāo)引力-RRT算法及RRT-Connect算法的平均搜索速度分別提升了88.23%、74.35%、16.66%，迭代次數(shù)縮減了65.77%、49.53%、38.76%。地圖2環(huán)境下平均搜索速度分別提升90.35%、74.11%、12.00%，迭代次數(shù)縮減77.15%、60.82%、27.10%。地圖3環(huán)境下平均搜索速度分別提升79.38%、49.15%、25.30%，迭代次數(shù)縮減52.51%、40.76%、40.35%。4種算法的初始路徑及路徑優(yōu)化后的路徑長(zhǎng)度如表2所示。4種算法在不同地圖環(huán)境下，總體初始路徑的平均長(zhǎng)度分別為97.71、109.29、122.50，優(yōu)化后路徑的平均長(zhǎng)度為77.00、93.61、100.30，平均路徑縮減了21.19%、14.34%、18.12%。進(jìn)一步說(shuō)明了路徑優(yōu)化有效地減小了4種算法初始路徑的復(fù)雜度。

圖6 仿真環(huán)境Fig.6 Simulation environment

圖7 地圖1算法運(yùn)行效果Fig.7 Algorithm operation effect of map 1

圖8 地圖2算法運(yùn)行效果Fig.8 Algorithm operation effect of map 2

圖9 地圖3算法運(yùn)行效果Fig.9 Algorithm operation effect of map 3

表1 不同環(huán)境下算法仿真結(jié)果Tab.1 Algorithm simulation results in different environments

表2 路徑優(yōu)化前后對(duì)比結(jié)果Tab.2 Comparison results before and after path optimization

圖10 迭代次數(shù)箱線(xiàn)圖Fig.10 Number of iterations boxplot

4種算法100次仿真實(shí)驗(yàn)中迭代次數(shù)如圖10所示。從圖10可以看出，本文算法在保持更少迭代次數(shù)的同時(shí)，分布結(jié)果也較為集中。綜合上述仿真實(shí)驗(yàn)可知，本文改進(jìn)的Straight-RRT算法有效地提高了RRT算法的收斂速度、路徑質(zhì)量及穩(wěn)定性。

3.2 獼猴桃果園環(huán)境場(chǎng)地實(shí)驗(yàn)

基于西北農(nóng)林科技大學(xué)眉縣獼猴桃試驗(yàn)站環(huán)境進(jìn)行路徑規(guī)劃實(shí)驗(yàn)，果園為棚架式種植模式，內(nèi)部環(huán)境如圖11所示，橫向間隔為4 m，列向間隔2 m，高1.8 m，為避免獼猴桃采摘機(jī)器人行至地膜區(qū)域，將地膜區(qū)域設(shè)為障礙物，此外為保證機(jī)器人安全移動(dòng)，對(duì)障礙物邊界進(jìn)行膨脹處理，二維環(huán)境地圖如圖12所示，圖中黑色區(qū)域?yàn)檎系K物區(qū)域，藍(lán)色區(qū)域?yàn)榕蛎泤^(qū)域。

圖11 眉縣獼猴桃試驗(yàn)站環(huán)境Fig.11 Environment of Meixian kiwifruit test station

圖12 獼猴桃果園二維地圖Fig.12 Two-dimensional map of kiwifruit orchard

獼猴桃采摘機(jī)器人如圖13所示，由履帶式移動(dòng)底盤(pán)搭載6自由度UR5機(jī)械臂組成，機(jī)械臂工作空間為半徑0.85 m的半球區(qū)域，機(jī)械臂底座高度為1.1 m。結(jié)合棚架高度，計(jì)算出獼猴桃采摘機(jī)器人實(shí)際有效采摘區(qū)域?yàn)閳D14中左側(cè)所示的黃色區(qū)域，將該區(qū)域投影至二維平面，計(jì)算出有效采摘區(qū)域在二維平面的投影為一個(gè)半徑為0.48 m的圓形區(qū)域，其最大內(nèi)接正方形邊長(zhǎng)為0.68 m，見(jiàn)圖14右側(cè)。

圖13 獼猴桃采摘機(jī)器人Fig.13 Kiwifruit harvesting robot

圖14 獼猴桃采摘機(jī)器人有效采摘區(qū)域Fig.14 Effective harvesting area of kiwifruit harvesting robot1.履帶式移動(dòng)底盤(pán) 2.機(jī)械臂工作空間 3.棚架 4.有效采摘區(qū)域 5.UR5機(jī)械臂 6.有效采摘區(qū)域二維平面投影 7.有效采摘區(qū)域二維平面投影的最大內(nèi)接正方形

為實(shí)現(xiàn)獼猴桃采摘機(jī)器人有效采摘區(qū)域?qū)ΛJ猴桃果園的全覆蓋，對(duì)獼猴桃果園進(jìn)行分區(qū)處理。將果園每個(gè)行間區(qū)域設(shè)置為單獨(dú)的工作區(qū)，在每個(gè)工作區(qū)內(nèi)，取獼猴桃采摘機(jī)器人有效采摘區(qū)域二維平面投影的最大內(nèi)接正方形，依次排布直至鋪滿(mǎn)整個(gè)工作區(qū)，并依次進(jìn)行編號(hào)，如圖15所示。其中每個(gè)正方形區(qū)域的中心點(diǎn)即為采摘機(jī)器人的一個(gè)導(dǎo)航目標(biāo)點(diǎn)，在一個(gè)工作區(qū)內(nèi)，獼猴桃采摘機(jī)器人以1號(hào)點(diǎn)為起始點(diǎn)，按編號(hào)順序依次經(jīng)過(guò)該工作區(qū)內(nèi)所有導(dǎo)航目標(biāo)點(diǎn)后，隨后移動(dòng)到下一個(gè)工作區(qū)的1號(hào)點(diǎn)，繼續(xù)按編號(hào)依次遍歷每個(gè)導(dǎo)航點(diǎn)，直至遍歷所有工作區(qū)的導(dǎo)航目標(biāo)點(diǎn)，則認(rèn)為獼猴桃采摘機(jī)器人的有效采摘區(qū)域完成了對(duì)獼猴桃果園的全覆蓋。

選取2個(gè)工作區(qū)進(jìn)行路徑規(guī)劃實(shí)驗(yàn)，為驗(yàn)證算法對(duì)復(fù)雜環(huán)境的適應(yīng)性，對(duì)工作區(qū)1隨機(jī)增加一些障礙物。地圖環(huán)境如圖16a所示，紅色點(diǎn)為一個(gè)工作區(qū)中的起始導(dǎo)航點(diǎn)，綠色點(diǎn)為最終導(dǎo)航點(diǎn)，黃色點(diǎn)為獼猴桃采摘機(jī)器人需要依次經(jīng)過(guò)的中間導(dǎo)航點(diǎn)。采用本文算法在此地圖環(huán)境下進(jìn)行路徑規(guī)劃實(shí)驗(yàn)，路徑規(guī)劃效果如圖16b所示，其中紅色線(xiàn)段、藍(lán)色線(xiàn)段、紫色線(xiàn)段分別為獼猴桃采摘機(jī)器人在工作區(qū)1、工作區(qū)2、工作區(qū)間的規(guī)劃路徑。從圖16可以看出，規(guī)劃路徑從工作區(qū)1的起始導(dǎo)航點(diǎn)開(kāi)始依次遍歷了每一個(gè)導(dǎo)航點(diǎn)，同時(shí)具有較低的路徑復(fù)雜度，可以實(shí)現(xiàn)獼猴桃采摘機(jī)器人有效采摘區(qū)域?qū)ΛJ猴桃果園的全覆蓋。

圖16 地圖環(huán)境及路徑規(guī)劃效果Fig.16 Map environment and path planning effect

使用RRT算法、目標(biāo)引力-RRT算法、RRT-Connect算法分別在此地圖環(huán)境下進(jìn)行路徑規(guī)劃實(shí)驗(yàn)，總路徑及工作區(qū)1、工作區(qū)2、工作區(qū)間路徑規(guī)劃結(jié)果如表3所示。

表3 獼猴桃果園環(huán)境路徑規(guī)劃結(jié)果Tab.3 Planning results of environmental paths in kiwifruit orchards

在工作區(qū)1中，本文算法搜索時(shí)間相比于其他3種算法分別縮減了73.67%、54.69%、27.46%，說(shuō)明本文算法在工作區(qū)中存在障礙物的情況下具有更強(qiáng)的適應(yīng)性及搜索能力。由于工作區(qū)2中不存在障礙物，4種算法在工作區(qū)2中的搜索效率均大幅提升，除RRT算法外其他3種算法搜索時(shí)間均保持在0.5 s以?xún)?nèi)。工作區(qū)間路徑為從工作區(qū)1的最終導(dǎo)航點(diǎn)到工作區(qū)2的起始導(dǎo)航點(diǎn)的路徑，在工作區(qū)間路徑中4種算法的平均搜索時(shí)間為0.262 s，本文算法相比于平均搜索時(shí)間縮短了31.67%，說(shuō)明本文算法在工作區(qū)間的路徑規(guī)劃上具有更高的搜索效率。從總體路徑來(lái)看，本文算法的搜索時(shí)間相比其他3種算法分別縮減了74.31%、46.28%、26.60%，迭代次數(shù)縮減了86.52%、68.85%、29.23%。實(shí)驗(yàn)結(jié)果表明本文改進(jìn)算法在獼猴桃果園環(huán)境中具有更好的適應(yīng)性及規(guī)劃效率。

4 結(jié)論

(1)引入評(píng)價(jià)指數(shù)，通過(guò)對(duì)新節(jié)點(diǎn)的分類(lèi)實(shí)時(shí)更新評(píng)價(jià)指數(shù)。基于評(píng)價(jià)指數(shù)與閾值劃分采樣階段，通過(guò)改變不同階段的采樣策略實(shí)時(shí)引導(dǎo)隨機(jī)樹(shù)的擴(kuò)展。

(2)優(yōu)化收斂階段的最近節(jié)點(diǎn)選擇機(jī)制，使隨機(jī)樹(shù)更快避開(kāi)不規(guī)則障礙物。

(3)引入動(dòng)態(tài)閾值，閾值隨著環(huán)境復(fù)雜度實(shí)時(shí)調(diào)整，提高算法對(duì)不同環(huán)境的自適應(yīng)性。

(4)對(duì)路徑進(jìn)行優(yōu)化處理，去除路徑冗余點(diǎn)并對(duì)路徑進(jìn)行二次貝塞爾曲線(xiàn)擬合。

(5)基于棚架式獼猴桃果園環(huán)境進(jìn)行路徑規(guī)劃實(shí)驗(yàn)，本文改進(jìn)算法在獼猴桃果園環(huán)境下搜索時(shí)間為1.208 s，相比RRT算法、目標(biāo)引力-RRT算法及RRT-Connect算法分別縮減了74.31%、46.28%、26.60%，實(shí)驗(yàn)結(jié)果表明本文改進(jìn)算法在獼猴桃果園環(huán)境中具有良好的適應(yīng)性及規(guī)劃效率。