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

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

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

0 引言

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

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

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

1 算法原理

1.1 問題描述

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

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

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

1.2 RRT算法

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

2 改進RRT算法

2.1 評價指數(shù)

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

圖1 評價指數(shù)Fig.1 Evaluation index

2.2 擴展階段劃分

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

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

2.3 改進收斂階段最近節(jié)點選擇機制

每當隨機樹結束一次收斂階段時，將開始進入收斂階段時的最近節(jié)點xnearest以及在此次收斂階段擴展的新節(jié)點xnew記錄到點集R中，下一次隨機樹進入收斂階段將不會選擇點集R中的點作為最近節(jié)點xnearest。

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

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

2.4 動態(tài)閾值設定

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

T=a+n

(1)

式中n——當前隨機樹進入收斂階段的次數(shù)

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

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

2.5 路徑優(yōu)化

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

(2)

其中

(3)

式中Pi——貝塞爾曲線的控制點

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

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

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

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

3 算法仿真與實驗

3.1 不同環(huán)境下仿真實驗

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

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

從表1可以看出，在3種地圖環(huán)境下Straight-RRT的搜索時間及迭代次數(shù)均小于其他規(guī)劃算法，在地圖1環(huán)境下Straight-RRT算法相比于RRT算法、目標引力-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)化后的路徑長度如表2所示。4種算法在不同地圖環(huán)境下，總體初始路徑的平均長度分別為97.71、109.29、122.50，優(yōu)化后路徑的平均長度為77.00、93.61、100.30，平均路徑縮減了21.19%、14.34%、18.12%。進一步說明了路徑優(yōu)化有效地減小了4種算法初始路徑的復雜度。

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

圖7 地圖1算法運行效果Fig.7 Algorithm operation effect of map 1

圖8 地圖2算法運行效果Fig.8 Algorithm operation effect of map 2

圖9 地圖3算法運行效果Fig.9 Algorithm operation effect of map 3

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

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

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

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

3.2 獼猴桃果園環(huán)境場地實驗

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

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

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

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

圖13 獼猴桃采摘機器人Fig.13 Kiwifruit harvesting robot

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

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

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

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

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

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

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

4 結論

(1)引入評價指數(shù)，通過對新節(jié)點的分類實時更新評價指數(shù)?；谠u價指數(shù)與閾值劃分采樣階段，通過改變不同階段的采樣策略實時引導隨機樹的擴展。

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

(3)引入動態(tài)閾值，閾值隨著環(huán)境復雜度實時調(diào)整，提高算法對不同環(huán)境的自適應性。

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

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