基于改進(jìn)RRT*算法的菠蘿采收機(jī)導(dǎo)航路徑規(guī)劃

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

基于改進(jìn)RRT*算法的菠蘿采收機(jī)導(dǎo)航路徑規(guī)劃

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

（華南農(nóng)業(yè)大學(xué)工程學(xué)院，廣州 510642）

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

雷達(dá)；算法；菠蘿收獲；路徑規(guī)劃；快速搜索隨機(jī)樹(shù)

0 引 言

中國(guó)是世界十大菠蘿生產(chǎn)國(guó)之一，產(chǎn)量和需求量逐年上升?，F(xiàn)階段，國(guó)內(nèi)主要依靠人工采收菠蘿，但農(nóng)業(yè)勞動(dòng)力老齡化問(wèn)題日益嚴(yán)重[1]。無(wú)人駕駛、自動(dòng)導(dǎo)航是菠蘿采收機(jī)未來(lái)發(fā)展的主要方向。

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

1 導(dǎo)航路徑規(guī)劃方法

1.1 技術(shù)框架

農(nóng)機(jī)作業(yè)路徑規(guī)劃需要在滿足實(shí)際作業(yè)約束條件前提下，以最小化調(diào)度成本建立調(diào)度模型。由于作業(yè)環(huán)境中障礙物較多，作業(yè)時(shí)單一路徑規(guī)劃算法無(wú)法滿足導(dǎo)航要求，故需要將多種方法進(jìn)行整合，在全局路徑規(guī)劃的前提下進(jìn)行局部路徑調(diào)整以保證全局最優(yōu)。當(dāng)農(nóng)機(jī)在導(dǎo)航過(guò)程中與周?chē)h(huán)境發(fā)生沖突時(shí)進(jìn)行局部規(guī)劃，沖突解決后繼續(xù)按全局規(guī)劃路徑行進(jìn)。本文導(dǎo)航路徑規(guī)劃的技術(shù)框架如圖1所示。

圖1 導(dǎo)航路徑規(guī)劃技術(shù)框架

具體步驟如下：

1）根據(jù)初始環(huán)境信息構(gòu)建整體靜態(tài)地圖，路徑信息反饋到農(nóng)機(jī)的決策系統(tǒng)，決策系統(tǒng)將路徑信息傳輸?shù)江h(huán)境感知系統(tǒng)，根據(jù)規(guī)劃任務(wù)為路徑規(guī)劃系統(tǒng)提供路徑信息，規(guī)劃全局路徑；

2）使用路徑規(guī)劃算法規(guī)劃出一條從起點(diǎn)到目標(biāo)點(diǎn)的最優(yōu)或接近最優(yōu)的全局路徑；

3）根據(jù)農(nóng)機(jī)上裝載的傳感器對(duì)周?chē)h(huán)境進(jìn)行實(shí)時(shí)感知，獲取動(dòng)態(tài)的障礙物信息；

4）將動(dòng)態(tài)環(huán)境信息反饋給農(nóng)機(jī)的執(zhí)行裝置生成新的決策方案實(shí)時(shí)調(diào)整全局路徑信息。在運(yùn)動(dòng)過(guò)程中對(duì)農(nóng)機(jī)進(jìn)行定位，包括農(nóng)機(jī)的位置、行為狀態(tài)等信息引導(dǎo)農(nóng)機(jī)沿著規(guī)劃的路徑安全運(yùn)動(dòng)到目標(biāo)點(diǎn)。

1.2 算法介紹

RRT*算法是一種單向搜索算法，每次搜索路徑時(shí)從起始點(diǎn)出發(fā)開(kāi)始向目標(biāo)點(diǎn)搜索，當(dāng)快速搜索隨機(jī)樹(shù)搜索到目標(biāo)點(diǎn)后，規(guī)劃出一條可行路徑。RRT*算法在拓展時(shí)隨機(jī)產(chǎn)生采樣點(diǎn)，這導(dǎo)致快速搜索隨機(jī)樹(shù)拓展的方向具有不確定性，容易與目標(biāo)位置產(chǎn)生偏離從而導(dǎo)致拓展時(shí)間過(guò)長(zhǎng)且耗費(fèi)大量?jī)?nèi)存。改進(jìn)后的算法融入自啟發(fā)式思想[13]、人工勢(shì)場(chǎng)[14]和雙向拓展思想[15]對(duì)采樣點(diǎn)進(jìn)行約束，以提高采樣點(diǎn)的目標(biāo)導(dǎo)向性，同時(shí)對(duì)新節(jié)點(diǎn)的生成方式優(yōu)化，防止新節(jié)點(diǎn)在拓展過(guò)程中過(guò)度遠(yuǎn)離目標(biāo)點(diǎn)。

基于改進(jìn)RRT*算法的全局路徑規(guī)劃流程如圖2所示。首先，根據(jù)實(shí)際農(nóng)田的路徑信息建立全局靜態(tài)地圖；然后利用改進(jìn)RRT*算法規(guī)劃出一條從起始點(diǎn)到目標(biāo)點(diǎn)的路徑，并且修剪路徑上的冗余節(jié)點(diǎn)；最后經(jīng)過(guò)插值函數(shù)的路徑曲線平滑處理生成一條連續(xù)無(wú)障礙的全局作業(yè)路徑。

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

1.3 構(gòu)建全局地圖

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

1.4 算法改進(jìn)

1.4.1 目標(biāo)約束采樣

采樣點(diǎn)位置影響即將生成的新節(jié)點(diǎn)位置，為了提高采樣點(diǎn)的目標(biāo)導(dǎo)向性，本文借鑒自啟發(fā)式思想設(shè)置目標(biāo)偏置概率p（p的取值范圍為0～1，本文中p=0.2），在采樣中按照均勻概率隨機(jī)產(chǎn)生一個(gè)概率值。當(dāng)p>p時(shí)，在自由空間內(nèi)隨機(jī)產(chǎn)生一個(gè)采樣點(diǎn)，否則把目標(biāo)點(diǎn)作為采樣點(diǎn)，目標(biāo)約束采樣公式為

式中X為采樣點(diǎn)，X為目標(biāo)點(diǎn)，Sample()為隨機(jī)采樣函數(shù)。

1.4.2 節(jié)點(diǎn)偏置拓展

RRT*算法完全由采樣點(diǎn)控制新節(jié)點(diǎn)的拓展，生成采樣點(diǎn)的隨機(jī)性導(dǎo)致新節(jié)點(diǎn)在拓展時(shí)具有盲目性，從而使搜索時(shí)間過(guò)長(zhǎng)。為了克服這一問(wèn)題，本文對(duì)節(jié)點(diǎn)拓展方式進(jìn)行優(yōu)化，引入目標(biāo)點(diǎn)使新節(jié)點(diǎn)在拓展時(shí)被目標(biāo)點(diǎn)和采樣點(diǎn)約束。同時(shí)借鑒人工勢(shì)場(chǎng)算法中的引力場(chǎng)思想，分別在目標(biāo)點(diǎn)和采樣點(diǎn)方向分配不同的權(quán)重，使快速搜索隨機(jī)樹(shù)每一次都向更接近目標(biāo)點(diǎn)的方向拓展，加快搜索速率。拓展過(guò)程如圖3所示。

注：Xstart為起始點(diǎn)；Xrand為采樣點(diǎn)；Xnear為快速搜索隨機(jī)樹(shù)上距離采樣點(diǎn)最近的節(jié)點(diǎn)；Xgoal為目標(biāo)點(diǎn)；Xnew為拓展的新節(jié)點(diǎn)；wg和wk為方向權(quán)重；δ為拓展步長(zhǎng)。

新節(jié)點(diǎn)在拓展時(shí)分別在采樣點(diǎn)和目標(biāo)點(diǎn)方向分配權(quán)重w和w。新節(jié)點(diǎn)方向矢量和坐標(biāo)如式（2）所示。

權(quán)重w影響節(jié)點(diǎn)的拓展方向，合適的w值可以約束快速搜索隨機(jī)樹(shù)的拓展方向，并防止拓展的新節(jié)點(diǎn)過(guò)度遠(yuǎn)離目標(biāo)點(diǎn)或者接近障礙物，所以需要對(duì)w的取值范圍進(jìn)行約束。將最近點(diǎn)和目標(biāo)點(diǎn)連接線上的障礙物當(dāng)作質(zhì)點(diǎn)，以任意質(zhì)點(diǎn)為圓心設(shè)置半徑為的膨脹區(qū)，且膨脹區(qū)半徑不小于菠蘿采收機(jī)的半徑。圖4為w范圍求解示意圖。

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

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

>時(shí)<，cos>cos；<時(shí)>，cosw的取值在符合上述條件的情況下將引導(dǎo)快速搜索隨機(jī)樹(shù)向目標(biāo)點(diǎn)的方向拓展，提高搜索效率和成功率。

1.最近點(diǎn)X向膨脹區(qū)作的切線 2.新節(jié)點(diǎn)拓展的方向矢量 3.障礙物 4.膨脹區(qū)

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

注：X為障礙物點(diǎn)；為膨脹區(qū)半徑，mm；為X與X之間距離與的差值，mm；為新節(jié)點(diǎn)拓展的方向矢量；為切線方向矢量和目標(biāo)點(diǎn)方向矢量夾角，（°）；為新節(jié)點(diǎn)拓展方向矢量和目標(biāo)點(diǎn)方向矢量之間夾角，（°）。

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 方向權(quán)重w求解原理圖

Fig.4 Solution schematic for direction weightw

1.4.3 雙向拓展

為了提高收斂速度，本文改進(jìn)RRT*算法著重于提高兩棵快速搜索隨機(jī)樹(shù)的連接速度。搜索過(guò)程中，從起始點(diǎn)出發(fā)向目標(biāo)點(diǎn)方向拓展快速搜索隨機(jī)樹(shù)，記作Tree1。從目標(biāo)點(diǎn)向Tree1方向拓展快速搜索隨機(jī)樹(shù)，記作Tree2。

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

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

注：X1為T(mén)ree1上距離采樣點(diǎn)最近的節(jié)點(diǎn)；X2為T(mén)ree2上距離采樣點(diǎn)最近的節(jié)點(diǎn)；X2為T(mén)ree2的采樣點(diǎn)；X1為T(mén)ree1的新節(jié)點(diǎn)；X2為T(mén)ree2的新節(jié)點(diǎn)。

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 雙向拓展過(guò)程示意圖

Fig.5 Schematic diagram of the bidirectional expansion process

1.5 路徑優(yōu)化

1.5.1 路徑剪枝

兩棵快速搜索隨機(jī)樹(shù)連接后形成一條從起始點(diǎn)到目標(biāo)點(diǎn)的可行路徑，該路徑是一條由多個(gè)離散節(jié)點(diǎn)組成的折線段，該路徑上還存在許多非必要的節(jié)點(diǎn)。這些節(jié)點(diǎn)會(huì)增加路徑長(zhǎng)度，導(dǎo)致菠蘿采收機(jī)需要花費(fèi)更多的時(shí)間進(jìn)行轉(zhuǎn)向。因此，對(duì)路徑上多余的節(jié)點(diǎn)進(jìn)行修剪，尋找出一條無(wú)碰撞最短路徑。

本文根據(jù)貪心算法[17]提出拉直法去除路徑上的多余節(jié)點(diǎn)，修剪過(guò)程如圖6所示。首先將起點(diǎn)作為拉直路徑的第一個(gè)點(diǎn)，記為0，依次遍歷路徑上的節(jié)點(diǎn)，直到遇到節(jié)點(diǎn)5。當(dāng)連接0與5之間的線段與障礙物產(chǎn)生碰撞時(shí)，停止遍歷。拉直0與4間折線段的同時(shí)刪除所有不在這條線段上的節(jié)點(diǎn)；將4作為拉直路徑上的第二個(gè)點(diǎn)繼續(xù)拉直操作。重復(fù)拉直操作直到遍歷到目標(biāo)點(diǎn)11。

注：P0～P11為控制節(jié)點(diǎn)；灰色區(qū)域?yàn)檎系K物；實(shí)線段為原始路徑；虛線段為修剪后的路徑。

1.5.2 Cantmull-Rom樣條插值

修剪冗余節(jié)點(diǎn)后的路徑存在較大的轉(zhuǎn)折角度，可能導(dǎo)致菠蘿采收機(jī)在轉(zhuǎn)彎時(shí)耗費(fèi)更多時(shí)間，因此對(duì)路徑進(jìn)行平滑處理。常見(jiàn)的路徑平滑方法有貝塞爾函數(shù)法、樣條插值法等，本文使用Cantmull-Rom樣條差值函數(shù)[18]對(duì)路徑進(jìn)行平滑處理。

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

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

注：P2、P1、P、P+1為控制節(jié)點(diǎn)；實(shí)線()為平滑操作后的曲線；虛線為曲線()在節(jié)點(diǎn)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函數(shù)插值原理

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

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

由式（7）計(jì)算可得0、1、2、3，由此得到Cantmull-Rom插值函數(shù)的轉(zhuǎn)換關(guān)系為

Cantmull-Rom插值函數(shù)進(jìn)行路徑平滑處理時(shí)存在一個(gè)缺點(diǎn)：平滑后的曲線不經(jīng)過(guò)第一控制點(diǎn)和最后一個(gè)控制點(diǎn)。但在路徑規(guī)劃中必須保證路徑曲線經(jīng)過(guò)起始點(diǎn)和目標(biāo)點(diǎn)，所以在實(shí)際應(yīng)用時(shí)需要分別在起始點(diǎn)和目標(biāo)點(diǎn)附近取一個(gè)鄰近點(diǎn)，以保證平滑后的曲線經(jīng)過(guò)所有的控制節(jié)點(diǎn)。

2 仿真與試驗(yàn)

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

試驗(yàn)前在Gazebo中建立三維仿真地圖，控制Turtlebot3在三維地圖中運(yùn)動(dòng)。激光雷達(dá)在掃描三維環(huán)境的同時(shí)使用ROS平臺(tái)的gmapping功能包在RViz中構(gòu)建對(duì)應(yīng)的二維靜態(tài)地圖，3種環(huán)境在RViz中的二維靜態(tài)地圖如圖8所示。

圖8 仿真環(huán)境

將本文路徑規(guī)劃算法作為插件添加到ROS中，使用move base功能包調(diào)用該插件，試驗(yàn)從內(nèi)存占用率、路徑規(guī)劃時(shí)間和路徑長(zhǎng)度3個(gè)方面對(duì)比分析RRT*算法、雙向RRT*算法和本文改進(jìn)RRT*算法性能，其中內(nèi)存占用采用節(jié)點(diǎn)數(shù)量表征，節(jié)點(diǎn)數(shù)量越少占用內(nèi)存越少。仿真中設(shè)定Turtlebot3的半徑為200 mm，對(duì)障礙物進(jìn)行膨脹處理，膨脹區(qū)為250 mm，試驗(yàn)時(shí)將機(jī)器人作為質(zhì)點(diǎn)。每個(gè)環(huán)境分別進(jìn)行30次試驗(yàn)。各環(huán)境仿真結(jié)果如圖9和表1所示。

注：紫色線條為障礙物；藍(lán)色部分為膨脹區(qū)；紅色曲線為最終規(guī)劃出的路徑；綠色線條是快速搜索隨機(jī)樹(shù)的分支。

表1 3種算法在不同環(huán)境中的試驗(yàn)數(shù)據(jù)統(tǒng)計(jì)

2.1 多障礙物環(huán)境仿真結(jié)果分析

靜態(tài)環(huán)境地圖大小為==5 000 mm，起始點(diǎn)為[600，500]，終點(diǎn)為[4 600，4 500]，拓展步長(zhǎng)100 mm，圖9a為RRT*算法、雙向RRT*算法和本文算法在多障礙物靜態(tài)環(huán)境地圖中的仿真結(jié)果。本文算法和雙向RRT*算法的節(jié)點(diǎn)數(shù)量明顯少于RRT*算法，本文算法在多障礙物環(huán)境中能找到一條相對(duì)平緩的路徑。

由表1可知，本文改進(jìn)RRT*算法的收斂時(shí)間僅為RRT*算法的14.54%，是雙向RRT*算法的55.71%。本文算法的規(guī)劃速度是RRT*算法的6.9倍，是雙向RRT*算法的1.8倍左右，節(jié)點(diǎn)數(shù)量比RRT*算法少91.91%，比雙向RRT*算法少40.29%，規(guī)劃的路徑長(zhǎng)度比RRT*算法減少1.56%，比雙向RRT*算法減少 6.39%。

2.2 迷宮環(huán)境仿真結(jié)果分析

靜態(tài)環(huán)境地圖大小為==5 000 mm，起始點(diǎn)為[300，300]，終點(diǎn)為[4 500，4 500]，拓展步長(zhǎng)為100 mm，圖9b為 RRT*算法、雙向RRT*算法和本文算法在迷宮靜態(tài)環(huán)境地圖中的仿真結(jié)果。

RRT*算法規(guī)劃的路徑中搜索到的目標(biāo)點(diǎn)與實(shí)際目標(biāo)點(diǎn)之間存在明顯的偏差。本文算法的擴(kuò)展節(jié)點(diǎn)數(shù)遠(yuǎn)少于其他2種算法。迷宮環(huán)境存在長(zhǎng)路徑和短路徑，如圖9b中第1和第3張圖中的紅色曲線所示，在30次試驗(yàn)中改進(jìn)算法有86.67%的概率找到短路徑，而RRT*算法有46.67%的概率找到短路徑，雙向RRT*算法只有26.67%的概率找到短路徑。這在一定程度上反映了本文算法在迷宮環(huán)境下性能的優(yōu)異性，能有更有效地減少規(guī)劃路徑的長(zhǎng)度。

由表1可知，本文改進(jìn)RRT*算法的收斂時(shí)間僅為RRT*算法的18.17%，是雙向RRT*算法的47.88%，規(guī)劃速度是RRT*算法的5.5倍，是雙向RRT*算法的2.1倍左右，節(jié)點(diǎn)數(shù)量比RRT*算法少84.88%，比雙向RRT*算法少48.13%，路徑長(zhǎng)度比RRT*算法減少7.48%，比雙向RRT*算法減少10%。

2.3 狹窄通道環(huán)境仿真結(jié)果分析

靜態(tài)環(huán)境地圖大小為í=5 000 mmí5 250 mm，起始點(diǎn)為[1 000，500]，終點(diǎn)為[1 000，4 500]，拓展步長(zhǎng)為100 mm，圖9c為RRT*算法、雙向RRT*算法和本文算法在狹窄通道靜態(tài)環(huán)境地圖中的仿真結(jié)果。RRT*算法規(guī)劃的路徑中搜索到的目標(biāo)點(diǎn)與實(shí)際目標(biāo)點(diǎn)存在明顯偏差。本文算法擴(kuò)展的節(jié)點(diǎn)數(shù)少于另外2種算法。在狹窄通道環(huán)境中，改進(jìn)RRT*算法的可行路徑比其他2種算法更接近障礙物，拐點(diǎn)更少。

由表1可知，本文改進(jìn)RRT*算法的收斂時(shí)間僅為RRT*算法的21.28%，是雙向RRT*算法的34.78%，規(guī)劃速度是RRT*算法的4.7倍，是雙向RRT*算法的2.9倍左右。節(jié)點(diǎn)數(shù)量比RRT*算法少84.89%，比雙向RRT*算法少69.14%，路徑長(zhǎng)度比RRT*算法減少2.39%，比雙向RRT*算法減少1.84%。

仿真試驗(yàn)結(jié)果表明，本文改進(jìn)算法在3種環(huán)境中的平均收斂時(shí)間是RRT*算法的18%，雙向RRT*算法的46.12%，平均規(guī)劃速度是RRT*算法的5.7倍，雙向RRT*算法的2.3倍左右，平均拓展的節(jié)點(diǎn)數(shù)量比 RRT* 算法少87.23%，比雙向 RRT* 算法少52.52%，平均路徑長(zhǎng)度比 RRT* 算法減少3.81%，比雙向 RRT* 算法減少6.08%。改進(jìn)算法在減少拓展節(jié)點(diǎn)數(shù)量、降低內(nèi)存占用方面效果明顯，且在復(fù)雜環(huán)境下更容易找到最短路徑。大部分情況下的路徑規(guī)劃時(shí)間比RRT*算法和雙向RRT*算法短，證明了算法的優(yōu)越性，路徑也更加平滑。

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

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

圖10 仿真地圖及改進(jìn)算法的規(guī)劃路徑

2.5 田間試驗(yàn)

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

2.5.1 菠蘿行間路徑規(guī)劃試驗(yàn)

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

圖11 菠蘿行間路徑規(guī)劃試驗(yàn)結(jié)果

將規(guī)劃時(shí)間、迭代次數(shù)、轉(zhuǎn)角大于60°的個(gè)數(shù)和轉(zhuǎn)角大于100°的個(gè)數(shù)作為評(píng)價(jià)指標(biāo)，10次路徑規(guī)劃試驗(yàn)的平均值如表2所示。本文算法的規(guī)劃時(shí)間為RRT*算法的14.12%，為雙向RRT*的20.34%；迭代次數(shù)比RRT*算法減少了80.90 %，比雙向RRT*減少了69.70%。RRT*和雙向RRT*算法規(guī)劃路徑上大于60°的轉(zhuǎn)角分別是本文算法的1.56和2.06倍，大于100°的轉(zhuǎn)角分別是本文算法的1.55和2.18倍，本文算法規(guī)劃出的路徑比RRT*算法和雙向RRT*算法更平滑且轉(zhuǎn)折點(diǎn)較少。

表2 菠蘿行間路徑規(guī)劃試驗(yàn)統(tǒng)計(jì)數(shù)據(jù)

2.5.2果樹(shù)行間路徑規(guī)劃試驗(yàn)

為了進(jìn)一步驗(yàn)證本文算法的適用性，開(kāi)展果數(shù)行間路徑規(guī)劃試驗(yàn)。試驗(yàn)時(shí)間為2022年8月21日，地點(diǎn)為華南農(nóng)業(yè)大學(xué)。果樹(shù)行間的實(shí)際情況如圖12a所示。激光雷達(dá)掃描的環(huán)境信息如圖12b所示，部分果樹(shù)由于樹(shù)冠較小導(dǎo)致雷達(dá)掃描到的特征點(diǎn)較少，為了提高路徑規(guī)劃的成功率在RViz中對(duì)特征點(diǎn)進(jìn)行膨化處理，膨脹半徑設(shè)置為10 cm。圖12c為RRT*算法、雙向RRT*算法和改進(jìn)RRT*算法試驗(yàn)結(jié)果。本文算法規(guī)劃路徑更加平滑且轉(zhuǎn)彎較少。

圖12 果樹(shù)行間路徑規(guī)劃試驗(yàn)結(jié)果

將規(guī)劃時(shí)間、迭代次數(shù)、轉(zhuǎn)角大于60°的個(gè)數(shù)和轉(zhuǎn)角大于100°的個(gè)數(shù)作為評(píng)價(jià)指標(biāo)，10次路徑規(guī)劃試驗(yàn)的平均值如表3所示。本文算法的規(guī)劃時(shí)間為RRT*算法的25.26%，雙向RRT*的36.16%，迭代次數(shù)比RRT*算法減少了65.14%，比雙向RRT*減少38.04%。RRT*和雙向RRT*算法規(guī)劃路徑中大于60°的轉(zhuǎn)角分別是本文算法的2.70和3.26倍，大于100°的轉(zhuǎn)角分別是本文算法的2.68和3.05倍。本文算法在果樹(shù)間路徑規(guī)劃的路徑減少了急轉(zhuǎn)拐角，規(guī)劃路徑質(zhì)量更高。

2.5.3 導(dǎo)航試驗(yàn)

菠蘿行間試驗(yàn)時(shí)間為2022年10月15日，地點(diǎn)為華南農(nóng)業(yè)大學(xué)，試驗(yàn)設(shè)備為在履帶式高床作業(yè)機(jī)上開(kāi)發(fā)的無(wú)人駕駛菠蘿采收機(jī)，如圖13a所示，主要包括采收裝置、履帶式高床作業(yè)機(jī)和無(wú)人駕駛控制裝置。導(dǎo)航系統(tǒng)使用Nvidia Jetson Nano開(kāi)發(fā)板，基于python和c++在ROS 平臺(tái)進(jìn)行上位機(jī)開(kāi)發(fā)，搭載思嵐系列激光雷達(dá)。

表3 果樹(shù)行間路徑規(guī)劃試驗(yàn)統(tǒng)計(jì)數(shù)據(jù)

圖13 菠蘿采收機(jī)導(dǎo)航試驗(yàn)

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

表4 導(dǎo)航精度偏差

3 結(jié) 論

1）綜合考慮路徑代價(jià)、路徑平滑和碰撞檢測(cè)等因素，將路徑規(guī)劃分為全局路徑規(guī)劃與局部動(dòng)態(tài)避障，分析了菠蘿采收機(jī)導(dǎo)航路徑規(guī)劃問(wèn)題。針對(duì)全局路徑規(guī)劃提出了基于改進(jìn)RRT*算法的路徑規(guī)劃方案。改進(jìn)算法改善了RRT*算法的盲目性、收斂性差和不穩(wěn)定的問(wèn)題；對(duì)路徑的冗余節(jié)點(diǎn)進(jìn)行修剪，并利用Cantmull-Rom插值對(duì)路徑做平滑處理。

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

3）田間試驗(yàn)結(jié)果表明，菠蘿采收機(jī)可沿著規(guī)劃地路徑運(yùn)行到目標(biāo)點(diǎn)，但隨著運(yùn)動(dòng)速度由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] 賈闖，李加念，洪添勝，等. 山地果園單軌運(yùn)輸機(jī)超聲波避障系統(tǒng)的設(shè)計(jì)與試驗(yàn)[J]. 農(nóng)業(yè)工程學(xué)報(bào)，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] 孫玉山，冉祥瑞，張國(guó)成，等. 智能水下機(jī)器人路徑規(guī)劃研究現(xiàn)狀與展望[J]. 哈爾濱工程大學(xué)學(xué)報(bào)，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] 彭順正，坎雜，李景彬. 矮化密植棗園收獲作業(yè)視覺(jué)導(dǎo)航路徑提取[J]. 農(nóng)業(yè)工程學(xué)報(bào)，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] 張振乾，李世超，李晨陽(yáng)，等. 基于雙目視覺(jué)的香蕉園巡檢機(jī)器人導(dǎo)航路徑提取方法[J]. 農(nóng)業(yè)工程學(xué)報(bào)，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] 霍鳳財(cái)，遲金，黃梓健，等. 移動(dòng)機(jī)器人路徑規(guī)劃算法綜述[J]. 吉林大學(xué)學(xué)報(bào)（信息科學(xué)版），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] 李云伍，徐俊杰,，王銘楓，等. 丘陵山區(qū)田間道路自主行駛轉(zhuǎn)運(yùn)車(chē)及其視覺(jué)導(dǎo)航系統(tǒng)研制[J]. 農(nóng)業(yè)工程學(xué)報(bào)，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] 蔡雨岑，杜鵬楨. 基于平衡鯨魚(yú)優(yōu)化算法的無(wú)人車(chē)路徑規(guī)劃[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] 孟慶寬，楊曉霞，張漫，等. 基于語(yǔ)義分割的非結(jié)構(gòu)化田間道路場(chǎng)景識(shí)別[J]. 農(nóng)業(yè)工程學(xué)報(bào)，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] 張建英，趙志萍，劉暾. 基于人工勢(shì)場(chǎng)法的機(jī)器人路徑規(guī)劃[J]. 哈爾濱工業(yè)大學(xué)學(xué)報(bào)，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ī)器人抓取操作[J]. 中國(guó)科學(xué)技術(shù)大學(xué)學(xué)報(bào)，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] 劉永波，雷波，胡亮，等. 機(jī)器視覺(jué)在HSV顏色空間下稻瘟病病程分級(jí)判定研究[J]. 農(nóng)學(xué)學(xué)報(bào)，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] 梁亞杰，楊麗麗，徐媛媛，等. 不確定場(chǎng)景下無(wú)人農(nóng)機(jī)多機(jī)動(dòng)態(tài)路徑規(guī)劃方法[J]. 農(nóng)業(yè)工程學(xué)報(bào)，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] 魏武，韓進(jìn)，李艷杰，等. 基于雙樹(shù)Quick-RRT算法的移動(dòng)機(jī)器人路徑規(guī)劃[J]. 華南理工大學(xué)學(xué)報(bào)（自然科學(xué)版），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ìn)勢(shì)場(chǎng)蟻群算法的移動(dòng)機(jī)器人全局路徑規(guī)劃[J]. 機(jī)械工程學(xué)報(bào)，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ī)構(gòu)工作原理及設(shè)計(jì)[J]. 農(nóng)業(yè)工程學(xué)報(bào)，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

劉天湖，張迪，鄭琰，等. 基于改進(jìn)RRT*算法的菠蘿采收機(jī)導(dǎo)航路徑規(guī)劃[J]. 農(nóng)業(yè)工程學(xué)報(bào)，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

國(guó)家自然科學(xué)基金資助項(xiàng)目（52175229）；廣東省農(nóng)業(yè)科技創(chuàng)新十大主攻方向“揭榜掛帥”項(xiàng)目（2022SDZG03）

劉天湖，博士，副教授，研究方向?yàn)樗麢C(jī)械化采收，采摘機(jī)器人。Email：liuparalake@scau.edu.cn