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

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

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

0 引言

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

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

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

1 算法原理

1.1 問題描述

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

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

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

1.2 RRT算法

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

2 改進RRT算法

2.1 評價指數

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

圖1 評價指數Fig.1 Evaluation index

2.2 擴展階段劃分

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

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

2.3 改進收斂階段最近節點選擇機制

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

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

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

2.4 動態閾值設定

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

T=a+n

(1)

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

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

圖4 動態閾值Fig.4 Dynamic threshold

2.5 路徑優化

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

(2)

其中

(3)

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

α——貝塞爾曲線參數

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

圖5 路徑優化示意圖Fig.5 Schematic of path optimization

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

3 算法仿真與實驗

3.1 不同環境下仿真實驗

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

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

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

圖6 仿真環境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 不同環境下算法仿真結果Tab.1 Algorithm simulation results in different environments

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

圖10 迭代次數箱線圖Fig.10 Number of iterations boxplot

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

3.2 獼猴桃果園環境場地實驗

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

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

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

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

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

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

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

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

圖16 地圖環境及路徑規劃效果Fig.16 Map environment and path planning effect

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

表3 獼猴桃果園環境路徑規劃結果Tab.3 Planning results of environmental paths in kiwifruit orchards

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

4 結論

(1)引入評價指數，通過對新節點的分類實時更新評價指數。基于評價指數與閾值劃分采樣階段，通過改變不同階段的采樣策略實時引導隨機樹的擴展。

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

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

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

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