基于改進RRT*算法的菠蘿采收機導航路徑規劃

劉天湖，張 迪，鄭 琰，程一豐，裘 健，齊 龍

基于改進RRT*算法的菠蘿采收機導航路徑規劃

劉天湖，張 迪，鄭 琰，程一豐，裘 健，齊 龍

（華南農業大學工程學院，廣州 510642）

為了提高菠蘿收獲的機械化和自動化水平，該研究采用改進RRT*（Rapidly-exploring Random Trees Star）算法進行菠蘿采收機全局作業路徑規劃。首先引入自啟發式思想約束采樣點的生成，借鑒人工勢場引入方向權重對新節點拓展方向進行約束，同時計算合適的權重取值范圍，采用雙向拓展加快迭代速度，然后利用貪心算法修剪路徑冗余節點，并利用Cantmull-Rom插值函數對路徑進行平滑處理。根據農田道路情況創建多障礙物、迷宮和狹窄通道3種仿真環境，分別對RRT*算法、雙向RRT*算法和改進后RRT*算法的性能進行測試。試驗結果表明：3種環境下，本文算法的平均收斂時間是RRT*算法的18%，是雙向RRT*算法的46.12%，平均規劃速度是RRT*算法的5.7倍，是雙向RRT*算法的2.3倍左右，平均拓展節點數量比 RRT* 算法減少87.23%，比雙向 RRT* 算法減少 52.52%，平均路徑長度比 RRT* 算法減少 3.81%，比雙向 RRT* 算法減少 6.08%。田間試驗結果表明：本文算法的規劃時間僅為RRT*算法的14.12%，為雙向RRT*的20.34%，迭代次數比RRT*算法減少80.89%，比雙向RRT*減少69.70%。另外，RRT*和雙向RRT*算法規劃路徑上大于60°的轉角分別是本文算法的1.56和2.06倍，大于100°的轉角分別是本文算法的1.55和2.18倍，本文算法規劃的路徑更平滑。研究結果可為菠蘿采收機導航提供參考。

雷達；算法；菠蘿收獲；路徑規劃；快速搜索隨機樹

0 引 言

中國是世界十大菠蘿生產國之一，產量和需求量逐年上升。現階段，國內主要依靠人工采收菠蘿，但農業勞動力老齡化問題日益嚴重[1]。無人駕駛、自動導航是菠蘿采收機未來發展的主要方向。

在農機導航路徑研究方面，賈闖等[2]研究了果園單軌運輸機超聲波避障系統，通過在果園預先鋪設軌道實現運輸車的自主移動，但是運輸車只能在軌道上運行，缺乏靈活性。為了提高導航的自主性，有學者使用路徑規劃來完成導航。導航路徑規劃根據對環境感知信息的完整性可分為靜態全局路徑規劃和局部動態路徑規劃[3]。全局路徑規劃需要先獲取環境的完整信息，直接使用路徑規劃算法規劃出一條全局路徑。局部路徑規劃則在未知全部環境信息的情況下，使用傳感器先對周圍環境進行感知，再通過優化算法規劃出一條合理的路徑。部分學者基于機器視覺或激光雷達進行了農業機器人局部路徑規劃的相關研究。例如，彭順正等[4]提出矮化密植棗園收獲作業視覺導航路徑提取方法，通過最小二乘法（Least Squares Method, LSM）原理擬合左右兩側邊緣，提取邊緣線上各行的幾何中心點生成棗園導航基準線。張振乾等[5]提出一種基于雙目視覺的香蕉園巡檢路徑提取方法。局部路徑規劃有效保障了規劃路徑的安全性與實時性，但只使用局部動態路徑規劃不能保證得到最優解，容易陷入局部最優導致導航效率低下[6]，所以需要先使用全局路徑規劃找到一條合理的路徑。李云伍等[7]對A*算法估價函數進行改進，將路口節點處的道路曲率及道路起伏信息引入代價函數進行全局路徑規劃，但是A*算法在大地圖內規劃速度較低[8]。為此，本文引入快速擴展隨機樹（Rapid-exploration Random Trees, RRT）算法進行全局路徑規劃。RRT算法不需要對系統進行建模，且對搜索空間的覆蓋率高、搜索范圍廣，適用于大規模環境的路徑規劃[9]。但是RRT算法不具備漸進最優性，且搜索過程中計算代價較大[10]。在考慮代價路徑的基礎上，文獻[11]提出了漸進最優快速拓展隨機樹(Rapid-exploration Random Trees Star, RRT*)算法，通過選擇最優父親節點和相鄰節點回溯重構保證算法達到全局最優解。大多數農業作業發生在以時間和空間快速變化為特征的非結構化環境中[12]。農田道路狀況復雜，存在道路狹窄和多障礙物等情況，RRT* 算法仍然存在規劃效率低、算法搜索到的目標點與實際位置存在偏差等問題。故本文提出一種改進RRT*算法，重點解決大地圖內規劃效率較低和位置偏差問題。根據道路環境創建仿真地圖，對比RRT*算法、雙向RRT*算法和改進RRT*算法在不同靜態地圖下的性能。

1 導航路徑規劃方法

1.1 技術框架

農機作業路徑規劃需要在滿足實際作業約束條件前提下，以最小化調度成本建立調度模型。由于作業環境中障礙物較多，作業時單一路徑規劃算法無法滿足導航要求，故需要將多種方法進行整合，在全局路徑規劃的前提下進行局部路徑調整以保證全局最優。當農機在導航過程中與周圍環境發生沖突時進行局部規劃，沖突解決后繼續按全局規劃路徑行進。本文導航路徑規劃的技術框架如圖1所示。

圖1 導航路徑規劃技術框架

具體步驟如下：

1）根據初始環境信息構建整體靜態地圖，路徑信息反饋到農機的決策系統，決策系統將路徑信息傳輸到環境感知系統，根據規劃任務為路徑規劃系統提供路徑信息，規劃全局路徑；

2）使用路徑規劃算法規劃出一條從起點到目標點的最優或接近最優的全局路徑；

3）根據農機上裝載的傳感器對周圍環境進行實時感知，獲取動態的障礙物信息；

4）將動態環境信息反饋給農機的執行裝置生成新的決策方案實時調整全局路徑信息。在運動過程中對農機進行定位，包括農機的位置、行為狀態等信息引導農機沿著規劃的路徑安全運動到目標點。

1.2 算法介紹

RRT*算法是一種單向搜索算法，每次搜索路徑時從起始點出發開始向目標點搜索，當快速搜索隨機樹搜索到目標點后，規劃出一條可行路徑。RRT*算法在拓展時隨機產生采樣點，這導致快速搜索隨機樹拓展的方向具有不確定性，容易與目標位置產生偏離從而導致拓展時間過長且耗費大量內存。改進后的算法融入自啟發式思想[13]、人工勢場[14]和雙向拓展思想[15]對采樣點進行約束，以提高采樣點的目標導向性，同時對新節點的生成方式優化，防止新節點在拓展過程中過度遠離目標點。

基于改進RRT*算法的全局路徑規劃流程如圖2所示。首先，根據實際農田的路徑信息建立全局靜態地圖；然后利用改進RRT*算法規劃出一條從起始點到目標點的路徑，并且修剪路徑上的冗余節點；最后經過插值函數的路徑曲線平滑處理生成一條連續無障礙的全局作業路徑。

圖2 使用RRT*算法生成全局路徑的流程

1.3 構建全局地圖

地圖信息是獲取導航路徑規劃的關鍵。本文使用無人機、遙感衛星捕獲農田圖像，然后進行圖像處理（包括顏色增強、灰度化、二值化分割和高斯濾波等操作）[16]從農田圖像中分割出靜態地圖。或使用裝載在無人駕駛拖拉機上的環境感知工具如激光雷達、深度相機等創建全局地圖。

1.4 算法改進

1.4.1 目標約束采樣

采樣點位置影響即將生成的新節點位置，為了提高采樣點的目標導向性，本文借鑒自啟發式思想設置目標偏置概率p（p的取值范圍為0～1，本文中p=0.2），在采樣中按照均勻概率隨機產生一個概率值。當p>p時，在自由空間內隨機產生一個采樣點，否則把目標點作為采樣點，目標約束采樣公式為

式中X為采樣點，X為目標點，Sample()為隨機采樣函數。

1.4.2 節點偏置拓展

RRT*算法完全由采樣點控制新節點的拓展，生成采樣點的隨機性導致新節點在拓展時具有盲目性，從而使搜索時間過長。為了克服這一問題，本文對節點拓展方式進行優化，引入目標點使新節點在拓展時被目標點和采樣點約束。同時借鑒人工勢場算法中的引力場思想，分別在目標點和采樣點方向分配不同的權重，使快速搜索隨機樹每一次都向更接近目標點的方向拓展，加快搜索速率。拓展過程如圖3所示。

注：Xstart為起始點；Xrand為采樣點；Xnear為快速搜索隨機樹上距離采樣點最近的節點；Xgoal為目標點；Xnew為拓展的新節點；wg和wk為方向權重；δ為拓展步長。

新節點在拓展時分別在采樣點和目標點方向分配權重w和w。新節點方向矢量和坐標如式（2）所示。

權重w影響節點的拓展方向，合適的w值可以約束快速搜索隨機樹的拓展方向，并防止拓展的新節點過度遠離目標點或者接近障礙物，所以需要對w的取值范圍進行約束。將最近點和目標點連接線上的障礙物當作質點，以任意質點為圓心設置半徑為的膨脹區，且膨脹區半徑不小于菠蘿采收機的半徑。圖4為w范圍求解示意圖。

對于隨機采樣點X，如果采樣點的位置在切線方向和目標點方向之間的區域，向采樣點方向拓展已經靠近目標點的方向，為了減少計算量，此時w=1，只需討論采樣點落在區域外的情況。節點拓展時，如果最近點到障礙物質點距離與膨脹區半徑的差值大于拓展步長，代表新節點拓展的方向偏向目標點方向，<。否則拓展方向偏向隨機點方向，>。計算權重時比較cos和cos的值，cos和cos表示如下：

式中(,)為最近點到障礙物的距離，和的取值范圍為0～π。

>時<，cos>cos；<時>，cosw的取值在符合上述條件的情況下將引導快速搜索隨機樹向目標點的方向拓展，提高搜索效率和成功率。

1.最近點X向膨脹區作的切線 2.新節點拓展的方向矢量 3.障礙物 4.膨脹區

1.Tangent of the nearest pointXto the expansion zone 2.Direction vector of the new node expansion 3.Obstacle 4.Expansion zone

注：X為障礙物點；為膨脹區半徑，mm；為X與X之間距離與的差值，mm；為新節點拓展的方向矢量；為切線方向矢量和目標點方向矢量夾角，（°）；為新節點拓展方向矢量和目標點方向矢量之間夾角，（°）。

Note:Xis the obstacle point;is the radius of the expansion zone, mm;is the length of the distance betweenXand Xminus the length of，mm; The vectoris the direction vector of the new node expansion;is the angle between the tangent direction vector and the target point direction vector, (°);is the angle between the new node expansion direction vector and the target point direction vector, (°).

圖4 方向權重w求解原理圖

Fig.4 Solution schematic for direction weightw

1.4.3 雙向拓展

為了提高收斂速度，本文改進RRT*算法著重于提高兩棵快速搜索隨機樹的連接速度。搜索過程中，從起始點出發向目標點方向拓展快速搜索隨機樹，記作Tree1。從目標點向Tree1方向拓展快速搜索隨機樹，記作Tree2。

為加快雙向搜索速度，本文提出一種動態步長優化方法：首先，判斷Tree1新節點X1與Tree2上距離X1最近的點之間是否存在障礙物，如果不存在則將Tree1上的新節點X1作為Tree2的新節點，此時采用大步長step_max=(X2,X1)。該步驟的執行優先級為I級，如圖5a所示。如果存在障礙物，如圖5b所示，則設置閾值1、2，當兩棵樹的距離D大于1時，則兩棵樹還未進入連接狀態，步長由Tree2上最近節點X與障礙物之間的距離d決定。當d大于2時，認定菠蘿采收機處在安全區域內，此時采用中步長，中步長step_mid=。當最近節點與障礙物距離小于閾值2時，采用小步長step_min。小步長小于固定步長的長度，當固定步長的取值較小時也可等于固定步長。該步驟執行優先級為II級。步長計算公式為

式中=0表示X1與X2之間無障礙物，=1表示存在障礙物。

注：X1為Tree1上距離采樣點最近的節點；X2為Tree2上距離采樣點最近的節點；X2為Tree2的采樣點；X1為Tree1的新節點；X2為Tree2的新節點。

Note:X1is the node closest to the sampling point on Tree1;X2is the node closest to the sampling point on Tree2;X2is the sampling point of Tree2;X1is the new node of Tree1;X2is the new node of Tree2.

圖5 雙向拓展過程示意圖

Fig.5 Schematic diagram of the bidirectional expansion process

1.5 路徑優化

1.5.1 路徑剪枝

兩棵快速搜索隨機樹連接后形成一條從起始點到目標點的可行路徑，該路徑是一條由多個離散節點組成的折線段，該路徑上還存在許多非必要的節點。這些節點會增加路徑長度，導致菠蘿采收機需要花費更多的時間進行轉向。因此，對路徑上多余的節點進行修剪，尋找出一條無碰撞最短路徑。

本文根據貪心算法[17]提出拉直法去除路徑上的多余節點，修剪過程如圖6所示。首先將起點作為拉直路徑的第一個點，記為0，依次遍歷路徑上的節點，直到遇到節點5。當連接0與5之間的線段與障礙物產生碰撞時，停止遍歷。拉直0與4間折線段的同時刪除所有不在這條線段上的節點；將4作為拉直路徑上的第二個點繼續拉直操作。重復拉直操作直到遍歷到目標點11。

注：P0～P11為控制節點；灰色區域為障礙物；實線段為原始路徑；虛線段為修剪后的路徑。

1.5.2 Cantmull-Rom樣條插值

修剪冗余節點后的路徑存在較大的轉折角度，可能導致菠蘿采收機在轉彎時耗費更多時間，因此對路徑進行平滑處理。常見的路徑平滑方法有貝塞爾函數法、樣條插值法等，本文使用Cantmull-Rom樣條差值函數[18]對路徑進行平滑處理。

Cantmull-Rom樣條插值函數至少受4個控制點影響，分別記為P2、P1、P、P1，其中P處的切線記為(P1?P1)，會影響曲線的扭曲程度。Cantmull-Rom插值函數可表示為

圖7為Cantmull-Rom函數的插值原理，據此可求解式（6）中的0、1、2、3。

注：P2、P1、P、P+1為控制節點；實線()為平滑操作后的曲線；虛線為曲線()在節點P-1和P處的切線。

Note:P2,P1,P,P+1are the control nodes; the solid curve() is the curve after smooth operation; dashed lines are the tangents of the curve() at nodesP1andPrespectively.

圖7 Cantmull-Rom函數插值原理

Fig.7 The principle of interpolation of Cantmull-Rom function

()取值范圍為0～1，由圖7可知，點P1即為()在=0的值，點P即為()在=1的值，將0和1代入式（6）有：

由式（7）計算可得0、1、2、3，由此得到Cantmull-Rom插值函數的轉換關系為

Cantmull-Rom插值函數進行路徑平滑處理時存在一個缺點：平滑后的曲線不經過第一控制點和最后一個控制點。但在路徑規劃中必須保證路徑曲線經過起始點和目標點，所以在實際應用時需要分別在起始點和目標點附近取一個鄰近點，以保證平滑后的曲線經過所有的控制節點。

2 仿真與試驗

為了驗證本文改進算法的性能，參照魏武等[19-20]研究，并結合農田路徑實際情況，搭建多障礙物環境、迷宮環境和狹窄通道環境進行仿真試驗。仿真試驗系統為Ubuntu18.04，仿真軟件為ROS melodic，Gazebo9和RViz，采用學習平臺Turtlebot3，硬件配置為NVIDIA GEFORCE RTX 2060Ti 4GB顯存，搭載Intel?Core?i5 -10400U CPU，計算機運行內存為8 GB。

試驗前在Gazebo中建立三維仿真地圖，控制Turtlebot3在三維地圖中運動。激光雷達在掃描三維環境的同時使用ROS平臺的gmapping功能包在RViz中構建對應的二維靜態地圖，3種環境在RViz中的二維靜態地圖如圖8所示。

圖8 仿真環境

將本文路徑規劃算法作為插件添加到ROS中，使用move base功能包調用該插件，試驗從內存占用率、路徑規劃時間和路徑長度3個方面對比分析RRT*算法、雙向RRT*算法和本文改進RRT*算法性能，其中內存占用采用節點數量表征，節點數量越少占用內存越少。仿真中設定Turtlebot3的半徑為200 mm，對障礙物進行膨脹處理，膨脹區為250 mm，試驗時將機器人作為質點。每個環境分別進行30次試驗。各環境仿真結果如圖9和表1所示。

注：紫色線條為障礙物；藍色部分為膨脹區；紅色曲線為最終規劃出的路徑；綠色線條是快速搜索隨機樹的分支。

表1 3種算法在不同環境中的試驗數據統計

2.1 多障礙物環境仿真結果分析

靜態環境地圖大小為==5 000 mm，起始點為[600，500]，終點為[4 600，4 500]，拓展步長100 mm，圖9a為RRT*算法、雙向RRT*算法和本文算法在多障礙物靜態環境地圖中的仿真結果。本文算法和雙向RRT*算法的節點數量明顯少于RRT*算法，本文算法在多障礙物環境中能找到一條相對平緩的路徑。

由表1可知，本文改進RRT*算法的收斂時間僅為RRT*算法的14.54%，是雙向RRT*算法的55.71%。本文算法的規劃速度是RRT*算法的6.9倍，是雙向RRT*算法的1.8倍左右，節點數量比RRT*算法少91.91%，比雙向RRT*算法少40.29%，規劃的路徑長度比RRT*算法減少1.56%，比雙向RRT*算法減少 6.39%。

2.2 迷宮環境仿真結果分析

靜態環境地圖大小為==5 000 mm，起始點為[300，300]，終點為[4 500，4 500]，拓展步長為100 mm，圖9b為 RRT*算法、雙向RRT*算法和本文算法在迷宮靜態環境地圖中的仿真結果。

RRT*算法規劃的路徑中搜索到的目標點與實際目標點之間存在明顯的偏差。本文算法的擴展節點數遠少于其他2種算法。迷宮環境存在長路徑和短路徑，如圖9b中第1和第3張圖中的紅色曲線所示，在30次試驗中改進算法有86.67%的概率找到短路徑，而RRT*算法有46.67%的概率找到短路徑，雙向RRT*算法只有26.67%的概率找到短路徑。這在一定程度上反映了本文算法在迷宮環境下性能的優異性，能有更有效地減少規劃路徑的長度。

由表1可知，本文改進RRT*算法的收斂時間僅為RRT*算法的18.17%，是雙向RRT*算法的47.88%，規劃速度是RRT*算法的5.5倍，是雙向RRT*算法的2.1倍左右，節點數量比RRT*算法少84.88%，比雙向RRT*算法少48.13%，路徑長度比RRT*算法減少7.48%，比雙向RRT*算法減少10%。

2.3 狹窄通道環境仿真結果分析

靜態環境地圖大小為í=5 000 mmí5 250 mm，起始點為[1 000，500]，終點為[1 000，4 500]，拓展步長為100 mm，圖9c為RRT*算法、雙向RRT*算法和本文算法在狹窄通道靜態環境地圖中的仿真結果。RRT*算法規劃的路徑中搜索到的目標點與實際目標點存在明顯偏差。本文算法擴展的節點數少于另外2種算法。在狹窄通道環境中，改進RRT*算法的可行路徑比其他2種算法更接近障礙物，拐點更少。

由表1可知，本文改進RRT*算法的收斂時間僅為RRT*算法的21.28%，是雙向RRT*算法的34.78%，規劃速度是RRT*算法的4.7倍，是雙向RRT*算法的2.9倍左右。節點數量比RRT*算法少84.89%，比雙向RRT*算法少69.14%，路徑長度比RRT*算法減少2.39%，比雙向RRT*算法減少1.84%。

仿真試驗結果表明，本文改進算法在3種環境中的平均收斂時間是RRT*算法的18%，雙向RRT*算法的46.12%，平均規劃速度是RRT*算法的5.7倍，雙向RRT*算法的2.3倍左右，平均拓展的節點數量比 RRT* 算法少87.23%，比雙向 RRT* 算法少52.52%，平均路徑長度比 RRT* 算法減少3.81%，比雙向 RRT* 算法減少6.08%。改進算法在減少拓展節點數量、降低內存占用方面效果明顯，且在復雜環境下更容易找到最短路徑。大部分情況下的路徑規劃時間比RRT*算法和雙向RRT*算法短，證明了算法的優越性，路徑也更加平滑。

2.4 使用Gazebo軟件仿真菠蘿采收機的行走

使用建模軟件 Blender（Blender Foundation，Amsterdam，Netherlands）構建高度900 mm、最大直徑800 mm的菠蘿植株模型，如圖10a所示。將模型導入Gazebo仿真軟件，搭建U型菠蘿地場景，如圖10b所示。圖10c為改進算法平滑后的最終路徑。

圖10 仿真地圖及改進算法的規劃路徑

2.5 田間試驗

為了驗證本文改進算法路徑規劃效果，分別在菠蘿行間、果樹行間進行路徑規劃試驗和菠蘿采收機導航試驗。使用思嵐A2M12激光雷達采集環境信息，激光雷達測量半徑為12 m，采樣頻率為16 k，掃描頻率為10 Hz。菠蘿植株高分布比較分散，最大值為930 mm，大部分集中在 650～850 mm之間，枝葉密集部位大部分集中在250～450 mm之間[21]。測量的果樹平均高度為2.98 m，枝葉密集部位大部分集中在1.50 m以上。

2.5.1 菠蘿行間路徑規劃試驗

試驗時間為2022年8月18日，地點為華南農業大學。菠蘿行間實際情況如圖11a所示。激光雷達掃描到的環境信息如圖11b所示。由于菠蘿種植間距較小且枝葉密集，所以雷達掃描到的特征點足夠多，不需要膨化處理。圖11c為RRT*算法、雙向RRT*算法和改進算法在菠蘿行間的路徑規劃結果，本文算法規劃出的路徑更平滑且轉折點較少，RRT*算法和雙向RRT*算法規劃出的路徑則存在較多的轉折點。

圖11 菠蘿行間路徑規劃試驗結果

將規劃時間、迭代次數、轉角大于60°的個數和轉角大于100°的個數作為評價指標，10次路徑規劃試驗的平均值如表2所示。本文算法的規劃時間為RRT*算法的14.12%，為雙向RRT*的20.34%；迭代次數比RRT*算法減少了80.90 %，比雙向RRT*減少了69.70%。RRT*和雙向RRT*算法規劃路徑上大于60°的轉角分別是本文算法的1.56和2.06倍，大于100°的轉角分別是本文算法的1.55和2.18倍，本文算法規劃出的路徑比RRT*算法和雙向RRT*算法更平滑且轉折點較少。

表2 菠蘿行間路徑規劃試驗統計數據

2.5.2果樹行間路徑規劃試驗

為了進一步驗證本文算法的適用性，開展果數行間路徑規劃試驗。試驗時間為2022年8月21日，地點為華南農業大學。果樹行間的實際情況如圖12a所示。激光雷達掃描的環境信息如圖12b所示，部分果樹由于樹冠較小導致雷達掃描到的特征點較少，為了提高路徑規劃的成功率在RViz中對特征點進行膨化處理，膨脹半徑設置為10 cm。圖12c為RRT*算法、雙向RRT*算法和改進RRT*算法試驗結果。本文算法規劃路徑更加平滑且轉彎較少。

圖12 果樹行間路徑規劃試驗結果

將規劃時間、迭代次數、轉角大于60°的個數和轉角大于100°的個數作為評價指標，10次路徑規劃試驗的平均值如表3所示。本文算法的規劃時間為RRT*算法的25.26%，雙向RRT*的36.16%，迭代次數比RRT*算法減少了65.14%，比雙向RRT*減少38.04%。RRT*和雙向RRT*算法規劃路徑中大于60°的轉角分別是本文算法的2.70和3.26倍，大于100°的轉角分別是本文算法的2.68和3.05倍。本文算法在果樹間路徑規劃的路徑減少了急轉拐角，規劃路徑質量更高。

2.5.3 導航試驗

菠蘿行間試驗時間為2022年10月15日，地點為華南農業大學，試驗設備為在履帶式高床作業機上開發的無人駕駛菠蘿采收機，如圖13a所示，主要包括采收裝置、履帶式高床作業機和無人駕駛控制裝置。導航系統使用Nvidia Jetson Nano開發板，基于python和c++在ROS 平臺進行上位機開發，搭載思嵐系列激光雷達。

表3 果樹行間路徑規劃試驗統計數據

圖13 菠蘿采收機導航試驗

設置雷達與地面的距離為300 mm（雷達與地面距離可調），試驗場景如圖13b所示。試驗使用RViz軟件實現環境信息可視化，驗證改進算法的可行性。采收機以0.2、0.4、0.6 m/s的行駛速度分別進行5次試驗，測量目標點和采收機中心點的導航偏差，結果如表4所示。經過15次試驗，菠蘿采收機均可沿著規劃地路徑運行到目標點，隨著運動速度的提升，位置偏差和航向偏差有增加的趨勢。平均位置偏差由5.75 cm增加到8.96 cm，平均航向偏差由7.78°增加到12.57°。由于菠蘿畦間溝寬大于50 cm, 履帶寬度為13 cm，平均位置偏差和航線偏差增加后仍然能滿足菠蘿采收機行走要求。

表4 導航精度偏差

3 結 論

1）綜合考慮路徑代價、路徑平滑和碰撞檢測等因素，將路徑規劃分為全局路徑規劃與局部動態避障，分析了菠蘿采收機導航路徑規劃問題。針對全局路徑規劃提出了基于改進RRT*算法的路徑規劃方案。改進算法改善了RRT*算法的盲目性、收斂性差和不穩定的問題；對路徑的冗余節點進行修剪，并利用Cantmull-Rom插值對路徑做平滑處理。

2）建立了多障礙物、迷宮和狹窄通道仿真地圖在RViz軟件中通過仿真試驗對算法進行驗證，本文算法的平均規劃速度是RRT*算法的5.7倍，是雙向RRT*算法的2.3倍左右，平均路徑長度比RRT*算法減少3.81%，比雙向RRT*算法減少6.08%。對全局路徑進行路徑剪枝后，有效降低了轉彎次數。利用Cantmull-Rom函數線進行路徑平滑后，路徑轉折點處的尖峰得到優化。

3）田間試驗結果表明，菠蘿采收機可沿著規劃地路徑運行到目標點，但隨著運動速度由0.2 m/s增加到0.6 m/s，平均位置偏差由5.75 cm增加到8.96 cm，航向偏差由7.78°增加到12.57°。

[1] Wu L L, Li G C, Zhou X S. Change of factor endowments and China agricultural growth path selection[J]. China Population, Resources and Environment, 2015, 25(8): 144-152.

[2] 賈闖，李加念，洪添勝，等. 山地果園單軌運輸機超聲波避障系統的設計與試驗[J]. 農業工程學報，2015，31(增刊2)：69-74.

Jia Chuang, Li Jianian, Hong Tiansheng, et al. Design and test of ultrasonic obstacle avoidance system for mountain orchard monorail conveyor[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2015, 31(Supp.2): 69-74. (in Chinese with English abstract)

[3] 孫玉山，冉祥瑞，張國成，等. 智能水下機器人路徑規劃研究現狀與展望[J]. 哈爾濱工程大學學報，2020，41(8)：1111-1116.

Sun Yushan, Ran Xiangrui, Zhang Guocheng, et al. Research status and prospect of path planning for autonomous underwater vehicles[J]. Journal of Harbin Engineering University, 2020, 41(8): 1111-1116. (in Chinese with English abstract)

[4] 彭順正，坎雜，李景彬. 矮化密植棗園收獲作業視覺導航路徑提取[J]. 農業工程學報，2017，33(9)：45-52.

Peng Shunzheng, Kan Za, Li Jingbin. Extraction of visual navigation directrix for harvesting operation in short-stalked and close-planting jujube orchard[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2017, 33(9): 45-52. (in Chinese with English abstract)

[5] 張振乾，李世超，李晨陽，等. 基于雙目視覺的香蕉園巡檢機器人導航路徑提取方法[J]. 農業工程學報，2021，37(21)：9-15.

Zhang Zhenqian, Li Shichao, Li Chenyang, et al. Navigation path detection method for a banana orchard inspection robot based on binocular vision[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2021, 37(21): 9-15. (in Chinese with English abstract)

[6] 霍鳳財，遲金，黃梓健，等. 移動機器人路徑規劃算法綜述[J]. 吉林大學學報（信息科學版），2018，36(6)：639-647.

Huo Fengcai, Chi Jin, Huang Zijian, et al. Review of path planning for mobile robots[J]. Journal of Jilin University (Information Science Edition), 2018, 36(6): 639-647. (in Chinese with English abstract)

[7] 李云伍，徐俊杰,，王銘楓，等. 丘陵山區田間道路自主行駛轉運車及其視覺導航系統研制[J]. 農業工程學報，2019，35(1)：52-61.

Li Yunwu, Xu Junjie, Wang Mingfeng, et al. Development of autonomous driving transfer trolley on field roads and its visual navigation system for hilly areas[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2019, 35(1): 52-61. (in Chinese with English abstract)

[8] 蔡雨岑，杜鵬楨. 基于平衡鯨魚優化算法的無人車路徑規劃[J]. 控制與決策，2021，36(11)：2647-2655.

Cai Yucen, Du Pengzhen. Path planning of unmanned ground vehicle based on balanced whale optimization algorithm[J]. Control and Decision, 2021, 36(11): 2647-2655. (in Chinese with English abstract)

[9] LaValle S M. Rapidly-exploring random trees: A new tool for path planning[R]. Ames, USA: Iowa State University. Computer Science Dept. 1998.

[10] Nasir J, Islam F, Malik U, et al. RRT*-SMART: A rapid convergence implementation of RRT[J]. International Journal of Advanced Robotic Systems, 2013, 10(7): 1651-1656.

[11] Karaman S, Frazzoli E. Sampling-based algorithms for optimal motion planning[J]. The International Journal of Robotics Research, 2011, 30(7): 846-894.

[12] 孟慶寬，楊曉霞，張漫，等. 基于語義分割的非結構化田間道路場景識別[J]. 農業工程學報，2021，37(22)：152-160.

Meng Qingkuan, Yang Xiaoxia, Zhang Man, et al. Recognition of unstructured field road scene based on semantic segmentation model[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2021, 37(22): 152-160. (in Chinese with English abstract)

[13] Hart P E, Nilsson N J, Raphael B. A formal basis for the heuristic determination of minimum cost paths[J]. IEEE transactions on Systems Science and Cybernetics, 1968, 4(2): 100-107.

[14] 張建英，趙志萍，劉暾. 基于人工勢場法的機器人路徑規劃[J]. 哈爾濱工業大學學報，2006，38(8)：1306-1309.

Zhang Jianying, Zhao Zhiping, Liu Tun. A path planning method for mobile robot based on artificial potential field[J]. Journal of Harbin Institute of Technology, 2006, 38(8): 1306-1309.

[15] 杜爽，尚偉偉，劉坤，等. 基于雙向RRT算法的仿人機器人抓取操作[J]. 中國科學技術大學學報，2016，46(1)：12-20.

Du Shuang, Shang Weiwei, Liu Kun, et al. Bidirectional RRT algorithm based grasping manipulation of humanoid robots[J]. Journal of University of Science and Technology of China, 2016, 46(1): 12-20. (in Chinese with English abstract)

[16] 劉永波，雷波，胡亮，等. 機器視覺在HSV顏色空間下稻瘟病病程分級判定研究[J]. 農學學報，2020，10(10)：83-90.

Liu Yongbo, Lei Bo, Hu Liang, et al. The grading determination of rice blast: HSV color space method based on machine vision[J]. Journal of Agriculture, 2020, 10(10): 83-90. (in Chinese with English abstract)

[17] 梁亞杰，楊麗麗，徐媛媛，等. 不確定場景下無人農機多機動態路徑規劃方法[J]. 農業工程學報，2021，37(21)：1-8.

Liang Yajie, Yang Lili, Xu Yuanyuan, et al. Dynamic path planning method for multiple unmanned agricultural machines in uncertain scenarios[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2021, 37(21): 1-8. (in Chinese with English abstract)

[18] Song X, Fan X, Cao Z, et al. A tc-rrt-based path planning algorithm for the nonholonomic mobile robots[C]//2017 36th Chinese Control Conference (CCC). DaLian: IEEE, 2017: 6638-6643.

[19] 魏武，韓進，李艷杰，等. 基于雙樹Quick-RRT算法的移動機器人路徑規劃[J]. 華南理工大學學報（自然科學版），2021，49(7): 51-58.

Wei Wu, Han Jin, Li Yanjie, et al. Path planning of mobile robots based on dual-tree Quick RRT* algorithm[J]. Journal of South China University of Technology (Natural Science Edition), 2021, 49(7): 51-58. (in Chinese with English abstract)

[20] 馬小陸，梅宏. 基于改進勢場蟻群算法的移動機器人全局路徑規劃[J]. 機械工程學報，2021，57(1)：19-27.

Ma Xiaolu, Mei Hong. Mobile robot global path planning based on improved ant colony system algorithm with potential field[J]. Journal of Mechanical Engineering, 2021, 57(1): 19-27. (in Chinese with English abstract)

[21] 劉天湖，劉偉，曾霆俊，等. 多柔性指滾筒菠蘿采收機構工作原理及設計[J]. 農業工程學報，2022，38(8)：21-26.

Liu Tianhu, Liu Wei, Zeng Tingjun, et al. Working principle and design of the multi-flexible fingered roller pineapple harvesting mechanism[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2022, 38(8): 21-26. (in Chinese with English abstract)

Navigation path planning of the pineapple harvester based on improved RRT*algorithm

Liu Tianhu, Zhang Di, Zheng Yan, Cheng Yifeng, Qiu Jian, Qi Long

(,,510642,)

All pineapples are harvested manually at present in China. But manual harvesting cannot fully meet the large-scale production against the ever-increasing greying workforce. Fortunately, automatic navigation can be expected to develop the pineapple harvester. In this study, a path planning algorithm was proposed as the navigation scheme to improve the mechanization and automation level of pineapple harvesting. An improved RRT* algorithm was also used for global path planning. Firstly, the self-heuristic idea was used to constrain the generation range of sampling points. Then, the bias probabilitypwas introduced to generate the random sampling points. Specifically, the sampling points were randomly generated with the probabilityin the space, when>p. Otherwise, the target point was used as the sampling point, in order to decrease the blindness of sampling point generation. Thirdly, the gravitational field of the artificial potential field, and the concept of direction weight were introduced in the new node expansion. The weightswandwwere assigned to the directions of the sampling and target point, respectively, where the direction was constrained in the expansion of the new node. Fourthly, the bidirectional expansion was used to speed up the iteration speed in the double-tree expansion. Finally, the greedy algorithm was applied to prune the redundant nodes of the path. The Cantmull-Rom interpolation function was also used to smooth the path corners. Three environments (including multiple obstacles, mazes, and narrow passages) were created to simulate the path planning process, in order to evaluate the performance among the improved navigation path planning, RRT* and bidirectional RRT* algorithm. Planning time, node number, and path length were selected as the indicators. Each algorithm experimented with 30 times in every single environment. The average, maximum, minimum, and standard deviation were calculated for the simulation data of the three indicators, respectively. The simulation results showed that the average planning time of the path planning algorithm of this paper in the three environments was 18%, and 46.12% higher than that of the RRT* and bidirectional RRT* algorithms, respectively, while the average programming speed was 5.7, and 2.3 times as rapid as that of the RRT*, and bidirectional RRT* algorithm, respectively. Furthermore, the average node number was 87.23% and 52.52% less than that of the RRT* and bidirectional RRT* algorithms, respectively. The average path length was 3.81% and 6.08% less than the RRT* and bidirectional RRT* algorithms, respectively. The field test showed that the planning time was only 14.12% and 20.34% of the RRT* and bidirectional RRT*, respectively. The iteration number was 80.89% and 69.70% less than that of the RRT* and bidirectional RRT*, respectively. In addition, the rotation angles larger than 60° on the path planned by RRT* and bidirectional RRT* algorithms were 1.56 and 2.06 times as much as that of the improved, respectively, and the rotation angles larger than 100° on the path were 1.55 and 2.18 times. The improved RRT* algorithm can fully meet the path navigation requirements of agricultural machinery in the field. The pineapple harvester can run along the planned path to the target point as moving with the speed of 0.2, 0.4, and 0.6 m/s, but the position and heading deviation increase with the moving speed. This finding can provide a sound reference for the navigation development in agricultural machines.

radar; algorithms; pineapple harvester; path planning; rapidly-exploring random tree

10.11975/j.issn.1002-6819.2022.23.003

S224.24

A

1002-6819(2022)-23-0020-09

劉天湖，張迪，鄭琰，等. 基于改進RRT*算法的菠蘿采收機導航路徑規劃[J]. 農業工程學報，2022，38(23)：20-28.doi：10.11975/j.issn.1002-6819.2022.23.003 http://www.tcsae.org

Liu Tianhu, Zhang Di, Zheng Yan, et al. Navigation path planning of the pineapple harvester based on improved RRT* algorithm[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2022, 38(23): 20-28. (in Chinese with English abstract) doi：10.11975/j.issn.1002-6819.2022.23.003 http://www.tcsae.org

2022-06-24

2022-11-14

國家自然科學基金資助項目（52175229）；廣東省農業科技創新十大主攻方向“揭榜掛帥”項目（2022SDZG03）

劉天湖，博士，副教授，研究方向為水果機械化采收，采摘機器人。Email：liuparalake@scau.edu.cn