劉 慧 張世義 段云鵬 賈衛(wèi)東 沈 躍

(1.江蘇大學(xué)電氣信息工程學(xué)院，鎮(zhèn)江 212013; 2.江蘇大學(xué)農(nóng)業(yè)工程學(xué)院，鎮(zhèn)江 212013)

0 引言

隨著我國果園種植面積的不斷擴(kuò)大，果樹種植正朝著規(guī)范化、標(biāo)準(zhǔn)化和規(guī)模化的方向不斷發(fā)展，果園自主化作業(yè)的重要性日益凸顯[1-3]。近幾年人工智能、自動駕駛的迅速發(fā)展，也促使果園機(jī)器人朝著自主化、無人化作業(yè)方向快速發(fā)展[4]，果園機(jī)器人的廣泛應(yīng)用可以有效減輕農(nóng)機(jī)操作人員的勞動強(qiáng)度，提高作業(yè)精度和作業(yè)效率[5-6]。

果園機(jī)器人自主化作業(yè)效率主要依賴于機(jī)器人運動規(guī)劃所生成的作業(yè)軌跡。機(jī)器人運動規(guī)劃是指在環(huán)境先驗信息已知的情況下，生成一條從起始點到終止點的無碰撞且符合機(jī)器人運動學(xué)約束的作業(yè)軌跡。目前廣泛使用的全局路徑規(guī)劃算法主要分為以A*算法為代表的基于搜索的路徑規(guī)劃算法[7-8]和以快速隨機(jī)擴(kuò)展樹(RRT)算法為代表的基于采樣的路徑規(guī)劃算法[9-11]。

RRT算法由LAVALLE[12]于1998年提出，該算法對整個環(huán)境空間進(jìn)行采樣，具有搜索快速、概率完備等特點，但是在復(fù)雜的地圖中搜索效率低且采樣點利用率低。針對此問題，一些學(xué)者提出多種RRT算法的改進(jìn)方法。為提高搜索效率，偏向RRT[13]、RRT-Connect[14]、雙向RRT[15]被提出，但是仍存在路徑過于曲折且繞遠(yuǎn)的問題，不利于后期軌跡優(yōu)化；KARAMAN等[16]提出RRT*算法，該算法具有漸進(jìn)最優(yōu)性，路徑的長度隨著迭代次數(shù)的增大而變短，最終只能無限逼近最優(yōu)路徑，存在著收斂速度慢的問題；JORDAN等[17]提出Bi-RRT*算法，在RRT*基礎(chǔ)上，結(jié)合RRT-Connect和啟發(fā)式的思想，保證RRT*漸進(jìn)最優(yōu)的同時也加快了路徑收斂速度，但在復(fù)雜環(huán)境下節(jié)點計算量較大;劉奧博等[18]提出目標(biāo)偏置雙向RRT*算法，在雙向RRT*基礎(chǔ)上采用目標(biāo)偏置策略，提高了搜索效率，但是生成路徑存在尖峰狀況；張偉民等[19]提出基于目標(biāo)約束采樣和目標(biāo)偏置擴(kuò)展的改進(jìn)RRT*算法，并用三階B樣條進(jìn)行平滑處理，提高了狹窄通道等特殊環(huán)境下的路徑規(guī)劃效率和平滑性，但是沒有考慮到在路徑轉(zhuǎn)彎處機(jī)器人的曲率約束。

本文以果園作為環(huán)境地圖，結(jié)合機(jī)器人運動學(xué)模型將離散果樹轉(zhuǎn)換為連續(xù)狹窄走廊模型，并根據(jù)此環(huán)境特點提出一種基于動態(tài)末梢節(jié)點導(dǎo)向和勢場導(dǎo)向采樣的雙向RRT*算法。在尋路結(jié)束后路徑回溯過程中，對已生成路徑點進(jìn)行路徑點去冗余和相鄰折線段轉(zhuǎn)角約束處理，最后結(jié)合機(jī)器人曲率約束通過三階準(zhǔn)均勻B樣條曲線[20-21]對已知路徑點進(jìn)行軌跡優(yōu)化。

1 改進(jìn)雙向RRT*算法

1.1 傳統(tǒng)雙向RRT*算法

傳統(tǒng)雙向RRT*算法[15]將RRT*算法和RRT-Connect算法相結(jié)合，在保留RRT-Connect算法搜索快速、概率完備特點的同時，也繼承了RRT*算法漸進(jìn)最優(yōu)的優(yōu)點。但是，傳統(tǒng)雙向RRT*算法自身也存在著稠密障礙物或連續(xù)多層走廊環(huán)境下采樣點利用率低、搜索效率低和生成路徑折線多的缺點。

1.2 基于動態(tài)末梢點導(dǎo)向的偏置采樣

本文針對帶有障礙物的多重狹窄走廊的果園環(huán)境，對雙向RRT*偏置采樣算法做進(jìn)一步改進(jìn)，將靜態(tài)目標(biāo)點偏置導(dǎo)向優(yōu)化為動態(tài)末梢目標(biāo)點偏置導(dǎo)向，并重新定義偏置規(guī)則。

圖1呈現(xiàn)了動態(tài)目標(biāo)偏置導(dǎo)向的雙向擴(kuò)展RRT*搜索樹擴(kuò)展示意圖，黃色樹代表從起點生長的搜索樹，用Ts表示，ninit是其根節(jié)點；藍(lán)色樹代表從終點生長的搜索樹，用Te表示，ngoal為其根節(jié)點。為提高搜索效率，兩棵樹分別同時從節(jié)點ninit和節(jié)點ngoal進(jìn)行擴(kuò)展。在擴(kuò)展樹生長過程中，為提高搜索樹的目標(biāo)導(dǎo)向性，設(shè)置目標(biāo)偏置概率pgoal(pgoal∈[0,1])，并設(shè)置隨機(jī)概率p(p∈[0,1])，當(dāng)p>pgoal時，在空間中隨機(jī)產(chǎn)生一個采樣點nrand；當(dāng)p≤pgoal時，以末梢節(jié)點nlast(搜索樹上距離根節(jié)點最遠(yuǎn)的節(jié)點)作為隨機(jī)采樣點nrand，即

(1)

如圖1所示，針對目標(biāo)導(dǎo)向，文獻(xiàn)[19]采用的是紅色虛線箭頭所示方向，本文采用的是綠色虛線箭頭所示方向，將對方搜索樹上的末梢節(jié)點nlast作為目標(biāo)約束偏置節(jié)點，當(dāng)p≤pgoal時，本文方法為：

圖1 以末梢節(jié)點為目標(biāo)導(dǎo)向的偏置采樣示意圖

(1)將nlast作為采樣點nrand，并根據(jù)擴(kuò)展步長確定點B，若點C到點B無障礙物，則將點B作為新節(jié)點nnew來擴(kuò)展搜索樹。

(2)若點C到點B之間有障礙物，則調(diào)整搜索樹擴(kuò)展方向，按照由D到C的方向進(jìn)行擴(kuò)展，過點B作垂線垂直DC于點E，若點C到點E之間無障礙物，則將點E作為新節(jié)點nnew來擴(kuò)展搜索樹。

(3)若以上全不成立，則重新采樣(resample)。

綜上可得

(2)

1.3 基于勢場導(dǎo)向采樣點偏置處理

為進(jìn)一步提高采樣點的利用率并提高路徑搜索效率，結(jié)合人工勢場法思想[22-24]，在采樣時利用勢場導(dǎo)向?qū)⒙湓谡系K物區(qū)域的采樣點沿著該點勢場下降方向排斥至無障礙區(qū)，以實現(xiàn)采樣點再利用。本文將果樹和障礙物所在區(qū)域均看作障礙物區(qū)域，根據(jù)噴霧機(jī)器人本體尺寸對障礙物膨脹化處理，保證機(jī)器人與障礙物間的安全區(qū)域，障礙物膨脹尺寸Lexpend為噴霧機(jī)器人寬度Wcar的一半,即

(3)

障礙物膨脹后噴霧機(jī)器人被理想化為一個點。

人工勢場法中合勢場是由引力場和斥力場疊加得到，本文所使用的勢場導(dǎo)向法只需要考慮斥力場。將障礙物區(qū)域設(shè)為高勢場，非障礙物區(qū)域設(shè)為低勢場，并設(shè)置一個安全距離，在安全距離外不受斥力場影響。落在障礙物區(qū)域的采樣點在斥力的作用下會被排斥到非障礙區(qū)。

根據(jù)人工勢場法建立斥力勢場函數(shù)

(4)

式中Q*——斥力勢場臨界值，Q*∈R

q——雙向RRT*隨機(jī)采樣點

η——斥力勢場增益(決定勢場變化快慢)

D(q)——采樣點距最近障礙物中心的距離函數(shù)

Urep(q)——采樣點處的斥力勢場

斥力勢場梯度函數(shù)為

(5)

1.4 回溯路徑點處理

改進(jìn)雙向RRT*算法生成的原始路徑點由一系列稠密稀疏不均但有序的點集組成，且不滿足阿克曼運動學(xué)曲率約束。因此，在進(jìn)行軌跡優(yōu)化前，需要對已生成路徑做兩點處理——路徑點去冗余和相鄰折線段轉(zhuǎn)角約束。

1.4.1路徑點去冗余

從實際應(yīng)用角度分析，并不是每個路徑點都是必需的，冗余擁擠的路徑點可以被舍棄掉，使用具有代表性且可以保留原有路徑信息的少數(shù)點代替。

本文對路徑點去冗余所遵循的條件為

(6)

式中 Δd——相鄰路徑點距離

Δθ——相鄰折線段夾角

dthr——相鄰路徑點距離閾值

θthr——相鄰折線段夾角閾值

當(dāng)滿足式(6)時，根據(jù)給定閾值動態(tài)修改此段路徑點，路徑點修改公式為

(7)

式中ni——第i個待修改的路徑點

n′i——第i個修改后的路徑點

ni-1,i——從第i-1個待修改點指向第i個待修改點的向量

ni,i+1——從第i個待修改點指向第i+1個待修改點的向量

1.4.2相鄰折線段轉(zhuǎn)角約束

阿克曼底盤具有非完整性約束的特點，可以將其理想化為自行車模型進(jìn)行分析，如圖2所示。若要改變航向，需要滿足曲率約束條件

圖2 車輛自行車模型

(8)

式中κ——轉(zhuǎn)彎曲率α——前輪轉(zhuǎn)角

L——軸距αmax——前輪轉(zhuǎn)角最大值

因此，在換行轉(zhuǎn)彎情況下，對曲率不滿足噴霧機(jī)器人曲率約束條件的路徑點進(jìn)行修正，通過在轉(zhuǎn)彎起止點之間插入路徑點來約束每次轉(zhuǎn)彎角度，從而減小轉(zhuǎn)彎曲率。

在進(jìn)行相鄰折線段轉(zhuǎn)角約束的同時，也要考慮碰撞檢測，以保證生成的新路徑點以及相鄰路徑點的連線均在可行區(qū)域內(nèi)。碰撞檢測示意圖如圖3所示，矩形ABCD表示障礙物區(qū)域，EF表示任意一段路徑段，若線段EF上存在一點M，滿足

圖3 碰撞檢測示意圖

(9)

式中l(wèi)AB、lAD——路徑AB、AD長度

nAM、nAB、nAD——對應(yīng)向量

則說明路徑段EF經(jīng)過障礙物，該路徑點不可用。

1.5 改進(jìn)雙向RRT*算法流程

綜上所述，本文提出的改進(jìn)雙向RRT*算法經(jīng)動態(tài)末梢點導(dǎo)向采樣、勢場導(dǎo)向采樣以及回溯路徑點處理后可生成作業(yè)路徑，但考慮到路徑平滑性和噴霧機(jī)器人最大曲率約束，本文采用三階準(zhǔn)均勻B樣條曲線[25-26]優(yōu)化軌跡，從而得到最終作業(yè)軌跡，改進(jìn)雙向RRT*算法整體流程如圖4所示。

圖4 改進(jìn)雙向RRT*算法實現(xiàn)流程圖

2 仿真驗證

2.1 果園場景模擬地圖

為驗證本文改進(jìn)雙向RRT*算法的有效性，進(jìn)行了基于Matlab的仿真試驗。

首先根據(jù)果園中果樹行分布特點設(shè)計果園場景模擬地圖，如圖5a所示。根據(jù)樹干位置確定黑色邊框作為搜索采樣邊界，為路徑規(guī)劃提供封閉場景。在該場景下，果樹樹干、障礙物和四周邊界統(tǒng)一構(gòu)成障礙物區(qū)域，并對障礙物區(qū)域進(jìn)行膨脹化處理，膨脹尺寸理論上應(yīng)為噴霧機(jī)器人本體寬度的一半。

為解決果樹離散性問題，保證尋找出適合條件的路徑，在對圖5a中障礙物區(qū)域進(jìn)行膨脹化處理后，還需對每一列果樹進(jìn)行連續(xù)性處理，形成封閉的連續(xù)狹窄走廊，如圖5所示。

2.2 改進(jìn)雙向RRT*算法驗證

在圖5b中采用人工勢場思想對二維地圖做處理，每一列果樹中軸線處是勢場的高勢點，每一列可行區(qū)域的中軸線處是勢場低勢點，將整幅地圖勢場化，如圖5c所示。圖5d為封閉果園勢場三維仿真圖，是根據(jù)圖5c的二維圖再結(jié)合z軸勢場值所確定，從圖5d中可以看出整幅果園地圖的勢場分布以及變化趨勢。

圖5 運動規(guī)劃仿真試驗圖

基于這一思想，在雙向RRT*算法采樣過程中，當(dāng)采樣點落到障礙物區(qū)域，該采樣點便會沿著該點勢場下降方向被排斥到非障礙區(qū)，接下來再繼續(xù)通過RRT*算法對新的采樣點進(jìn)行后續(xù)處理，此方法大大提高了路徑搜索效率和采樣點利用率。

經(jīng)1.2、1.3節(jié)所述改進(jìn)后的雙向RRT*算法規(guī)劃生成一條可行路徑，并經(jīng)1.4節(jié)所述思想進(jìn)行路徑點處理，最終生成的路徑如圖5e所示。

2.3 三階準(zhǔn)均勻B樣條軌跡優(yōu)化

圖5e所示路徑雖然滿足位置點連續(xù)，但無法滿足路徑平滑和曲率約束，會產(chǎn)生運動狀態(tài)不連續(xù)、軌跡跟蹤效果差的問題。為解決以上問題，采用三階準(zhǔn)均勻B樣條優(yōu)化路徑，生成平滑且滿足最大曲率約束的作業(yè)軌跡。

B樣條曲線定義為

(10)

式中C(u)——B樣條曲線

Pi——第i個控制點

Ni,p(u)——具有i個節(jié)點的p次多項式

第i個p次B樣條基函數(shù)記為Ni,p(u)，B樣條基函數(shù)遞歸定義為

(11)

式中ui——第i個節(jié)點

為使優(yōu)化生成的軌跡通過給定路徑點，應(yīng)將路徑點作為型值點而非控制點，所以首先需要通過型值點反求控制點。三階B樣條曲線的型值點Q和控制點P存在關(guān)系

(12)

為使生成的軌跡曲線經(jīng)過起點和終點，需滿足

(13)

由式(12)、(13)可得型值點與控制點關(guān)系的線性方程

(14)

可算出所求控制點為

(15)

為滿足軌跡曲線的起始階段和終止階段分別與起始線段P0P1、終止線段Pn-1Pn相切，保證構(gòu)造出的B樣條曲線在兩端曲率為0，需滿足

(16)

最終，在Matlab中仿真生成的噴霧機(jī)器人作業(yè)軌跡如圖5f、5g所示，圖5f表示在果樹膨脹化處理后的地圖里生成的軌跡，從圖中可以看出，軌跡保持平滑性且整體轉(zhuǎn)彎少的同時，也和障礙物有一定間距，這保證噴霧機(jī)器人在作業(yè)過程中始終是安全的。然后將生成的軌跡在原始果園環(huán)境中表示出來，如圖5g所示。其中障礙物膨脹尺寸Lexpend為15像素，改進(jìn)算法擴(kuò)展步長為25像素，目標(biāo)偏置概率pgoal為0.3，斥力勢場臨界值Q*為45像素，斥力勢場增益η為20，相鄰路徑點距離閾值dthr為60像素，相鄰折線段夾角閾值θthr為45°。

表1分別從尋路時間、成功搜索路徑所需迭代次數(shù)、采樣點利用率、生成路徑的節(jié)點數(shù)和路徑成本5個方面對比了雙向RRT算法、雙向RRT*算法和本文改進(jìn)雙向RRT*算法。由表1可知，改進(jìn)雙向RRT*算法在以上5個方面均優(yōu)于雙向RRT算法和雙向RRT*算法，可以通過相對較少的時間和節(jié)點數(shù)量生成相對更優(yōu)的路徑。

3 試驗驗證

3.1 試驗平臺

本文算法驗證使用阿克曼底盤的噴霧機(jī)器人作為試驗平臺，如圖6a所示。噴霧機(jī)器人主要由機(jī)器人底盤(hunter 2.0型，松靈機(jī)器人(深圳)有限公司)、中央計算機(jī)(英特爾酷睿八代i7-8565u)、組合導(dǎo)航系統(tǒng)(高精度組合導(dǎo)航系統(tǒng)X1，北云科技)、二維激光雷達(dá)傳感器(LR-1BS2型，歐鐳激光)和精準(zhǔn)變量噴霧系統(tǒng)組成。其中，機(jī)器人底盤后輪為直流無刷電機(jī)驅(qū)動，前輪為阿克曼轉(zhuǎn)向結(jié)構(gòu)，輪距0.61 m，軸距0.65 m，最小轉(zhuǎn)彎半徑1.5 m，最大速度1.5 m/s，車體三維坐標(biāo)系如圖6b所示；組合導(dǎo)航系統(tǒng)支持雙天線定向，通過接收基站差分信號實現(xiàn)厘米級定位精度，X1安裝在機(jī)器人后輪中軸線上，X1坐標(biāo)系和車體坐標(biāo)系重合；二維激光雷達(dá)傳感器感知角度范圍0°～270°，距離范圍0～10 m，厘米級精度，安裝在噴霧機(jī)器人前方，距地面高度約0.40 m，激光雷達(dá)局部坐標(biāo)系和車體坐標(biāo)系的XY平面在同一平面，且X軸重合。

圖6 噴霧機(jī)器人示意圖

本研究使用中央計算機(jī)裝載Ubuntu 18.04LTS操作系統(tǒng)，并基于機(jī)器人操作系統(tǒng)(Robot operating system，ROS)進(jìn)行開發(fā)。

噴霧機(jī)器人運動規(guī)劃系統(tǒng)框圖如圖7所示。首先通過二維激光雷達(dá)采集樹干相對噴霧機(jī)器人的相對坐標(biāo)，噴霧機(jī)器人的全局坐標(biāo)可由組合導(dǎo)航系統(tǒng)X1精確定位，經(jīng)坐標(biāo)轉(zhuǎn)換后計算出樹干在全局坐標(biāo)系下的坐標(biāo)，根據(jù)轉(zhuǎn)換后的樹干坐標(biāo)建立果園全局地圖，然后根據(jù)噴霧機(jī)器人的尺寸參數(shù)和轉(zhuǎn)彎半徑對全局地圖做封閉和膨脹處理，并在此地圖上對改進(jìn)雙向RRT*算法進(jìn)行驗證，最后通過軌跡跟蹤控制算法生成控制噴霧機(jī)器人的運動指令，經(jīng)CAN協(xié)議與底層控制系統(tǒng)通信，實現(xiàn)對噴霧機(jī)器人的行為控制。

圖7 噴霧機(jī)器人系統(tǒng)框圖

3.2 數(shù)據(jù)采集與地圖生成

本文算法驗證所選試驗地點位于江蘇大學(xué)校園寬闊場地(32.203 557 4°N,119.512 970 3°E)，如 圖8 所示。模擬果園場景中使用仿真樹和PVC水管排成4行4列模擬果樹，模擬樹干直徑約為0.15 m，果樹行間距為3 m，同一行中果樹間距為1.5 m。通過機(jī)載前置水平二維激光雷達(dá)采集果園樹干位置信息，并根據(jù)機(jī)載全局定位系統(tǒng)將樹干位置信息從機(jī)體坐標(biāo)系轉(zhuǎn)換到全局坐標(biāo)系，在ROS中進(jìn)行處理生成以噴霧機(jī)器人當(dāng)前位置為原點的二維平面地圖并在Rviz中顯示，可視化地圖中柵格分辨率為0.5 m，所生成果園二維地圖如圖9所示，為方便觀察，圖中對樹干進(jìn)行放大處理。

圖8 試驗場景圖

圖9 果園二維地圖

3.3 改進(jìn)雙向RRT*算法驗證與分析

對于果園環(huán)境下噴霧機(jī)器人的運動規(guī)劃，在具有樹干位置信息的二維地圖基礎(chǔ)上，根據(jù)2.1節(jié)所述方法和噴霧機(jī)器人的運動學(xué)參數(shù)(長0.98 m，寬0.745 m，最小轉(zhuǎn)彎半徑1.5 m)對果樹尺寸進(jìn)行膨脹化處理并添加邊界約束，將整幅地圖處理為一個封閉的連續(xù)狹窄走廊場景。

為驗證無障礙果園環(huán)境下的改進(jìn)雙向RRT*算法規(guī)劃效果，設(shè)計試驗場景如圖10所示。以機(jī)器人當(dāng)前位置為起點，根據(jù)右前方的樹干位置推算終點，并根據(jù)當(dāng)前噴霧機(jī)位姿信息和采集樹干坐標(biāo)時噴霧機(jī)器人的位姿信息，對地圖進(jìn)行旋轉(zhuǎn)平移變換，在轉(zhuǎn)換后的地圖中分別運行傳統(tǒng)雙向RRT*算法和改進(jìn)雙向RRT*算法，在Rviz中顯示所生成軌跡，如圖11紅色曲線所示。

圖10 噴霧機(jī)器人期望運動路徑

圖11 無障礙環(huán)境下軌跡對比圖

通過對比圖11a、11b可知，改進(jìn)雙向RRT*算法相較于傳統(tǒng)雙向RRT*算法，可以生成更加平滑且轉(zhuǎn)彎次數(shù)更少的軌跡，只在每次換行時具有轉(zhuǎn)彎現(xiàn)象，更符合噴霧機(jī)器人的真實作業(yè)軌跡。

由表2可知，在無障礙果園環(huán)境下，改進(jìn)雙向RRT*算法相較于傳統(tǒng)雙向RRT*算法，路徑規(guī)劃用時縮短62%，采樣點利用率提高了33.97個百分點，最終所生成運動路徑長度平均縮短6.54%。圖12所示為以上2種算法經(jīng)過三階準(zhǔn)均勻B樣條優(yōu)化后生成軌跡的曲率隨軌跡上與起點距離的變化曲線，由圖12可以看出，改進(jìn)雙向RRT*生成軌跡比傳統(tǒng)雙向RRT*有更小的曲率波動且有一定規(guī)律，這一波動主要出現(xiàn)在換行轉(zhuǎn)彎處，基本上滿足最大曲率約束。

表2 無障礙環(huán)境下算法效果比較

圖12 無障礙環(huán)境下軌跡曲率對比

為驗證存在行人障礙物的果園環(huán)境下改進(jìn)雙向RRT*規(guī)劃算法，設(shè)計如圖13所示試驗場景，模擬具有行人障礙物的果園環(huán)境。在該地圖中分別運行傳統(tǒng)雙向RRT*算法和改進(jìn)雙向RRT*算法，在Rviz中顯示該環(huán)境下生成的作業(yè)軌跡，如圖14所示。

圖13 存在行人障礙物的果園環(huán)境

圖14 行人障礙環(huán)境下軌跡對比圖

由圖14可知，當(dāng)果園環(huán)境中存在行人障礙物時，本文算法生成的軌跡只在換行轉(zhuǎn)彎和障礙物附近存在軌跡彎曲波動的現(xiàn)象，在其他無障礙物樹行中，仍然基本保持直線軌跡，從表現(xiàn)形式來看，仍優(yōu)于傳統(tǒng)雙向RRT*算法。

由表3可以看出，相對于無障礙物環(huán)境下，具有行人障礙物的環(huán)境下規(guī)劃時間整體延長，延長的時間主要花費在障礙物附近的狹窄走廊處。在行人障礙物環(huán)境下，改進(jìn)雙向RRT*算法相較于傳統(tǒng)雙向RRT*算法，規(guī)劃用時縮短53%，采樣點利用率提高23.12個百分點，優(yōu)化后路徑平均長度縮短7.73%，改進(jìn)算法整體性能優(yōu)于傳統(tǒng)算法。圖15為具有行人障礙環(huán)境下2種算法生成路徑經(jīng)三階準(zhǔn)均勻B樣條優(yōu)化后生成軌跡的曲率隨軌跡上與起點距離的變化曲線，在該環(huán)境下，所生成軌跡仍滿足噴霧機(jī)器人作業(yè)的最大曲率約束，在30 m附近的波動是由繞開障礙物附近軌跡引起的。以上2種試驗場景試驗參數(shù)如表4所示。

表3 行人障礙環(huán)境下算法效果比較

圖15 行人障礙環(huán)境下軌跡曲率對比

表4 試驗參數(shù)

4 結(jié)論

(1)為提高果園環(huán)境下噴霧機(jī)器人作業(yè)效率與自主性，設(shè)計了一種搭載有組合導(dǎo)航系統(tǒng)、二維激光雷達(dá)、中央計算機(jī)和精準(zhǔn)變量噴霧系統(tǒng)的果園噴霧機(jī)器人，并基于此平臺，提出了一種改進(jìn)雙向RRT*的全局路徑規(guī)劃算法，相較于傳統(tǒng)雙向RRT*規(guī)劃算法，改進(jìn)算法規(guī)劃時間平均減少57.5%，采樣點利用率平均提高28.55個百分點，最終路徑長度平均縮短7.14%(2種情況取平均)。

(2)搜索路徑成功后，經(jīng)回溯路徑點處理并結(jié)合三階準(zhǔn)均勻B樣條曲線，生成可供果園噴霧機(jī)器人運行的精準(zhǔn)平滑軌跡，在無障礙物環(huán)境和行人障礙物環(huán)境下，生成的軌跡整體轉(zhuǎn)彎較少，僅在換行轉(zhuǎn)彎和躲避障礙物處出現(xiàn)軌跡彎曲現(xiàn)象，且曲率均滿足噴霧機(jī)器人底盤最大曲率約束。

(3)為進(jìn)一步提高果園噴霧機(jī)器人的自主性和智能性，在實現(xiàn)全局路徑規(guī)劃的基礎(chǔ)上，還需考慮在噴霧機(jī)器人作業(yè)過程中的實時動態(tài)路徑規(guī)劃以及遇到動態(tài)障礙物的決策規(guī)劃方法。