基于改進(jìn)雙向RRT*的果園機(jī)器人運(yùn)動(dòng)規(guī)劃算法

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

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

0 引言

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

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

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

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

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

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

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

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

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

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

(1)

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

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

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

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

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

綜上可得

(2)

1.3 基于勢(shì)場(chǎng)導(dǎo)向采樣點(diǎn)偏置處理

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

(3)

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

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

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

(4)

式中Q*——斥力勢(shì)場(chǎng)臨界值，Q*∈R

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

η——斥力勢(shì)場(chǎng)增益(決定勢(shì)場(chǎng)變化快慢)

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

Urep(q)——采樣點(diǎn)處的斥力勢(shì)場(chǎng)

斥力勢(shì)場(chǎng)梯度函數(shù)為

(5)

1.4 回溯路徑點(diǎn)處理

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

1.4.1路徑點(diǎn)去冗余

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

本文對(duì)路徑點(diǎn)去冗余所遵循的條件為

(6)

式中 Δd——相鄰路徑點(diǎn)距離

Δθ——相鄰折線段夾角

dthr——相鄰路徑點(diǎn)距離閾值

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

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

(7)

式中ni——第i個(gè)待修改的路徑點(diǎn)

n′i——第i個(gè)修改后的路徑點(diǎn)

ni-1,i——從第i-1個(gè)待修改點(diǎn)指向第i個(gè)待修改點(diǎn)的向量

ni,i+1——從第i個(gè)待修改點(diǎn)指向第i+1個(gè)待修改點(diǎn)的向量

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

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

圖2 車(chē)輛自行車(chē)模型

(8)

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

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

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

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

圖3 碰撞檢測(cè)示意圖

(9)

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

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

則說(shuō)明路徑段EF經(jīng)過(guò)障礙物，該路徑點(diǎn)不可用。

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

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

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

2 仿真驗(yàn)證

2.1 果園場(chǎng)景模擬地圖

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

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

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

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

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

圖5 運(yùn)動(dòng)規(guī)劃仿真試驗(yàn)圖

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

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

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

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

B樣條曲線定義為

(10)

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

Pi——第i個(gè)控制點(diǎn)

Ni,p(u)——具有i個(gè)節(jié)點(diǎn)的p次多項(xiàng)式

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

(11)

式中ui——第i個(gè)節(jié)點(diǎn)

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

(12)

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

(13)

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

(14)

可算出所求控制點(diǎn)為

(15)

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

(16)

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

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

3 試驗(yàn)驗(yàn)證

3.1 試驗(yàn)平臺(tái)

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

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

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

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

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

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

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

圖8 試驗(yàn)場(chǎng)景圖

圖9 果園二維地圖

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

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

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

圖10 噴霧機(jī)器人期望運(yùn)動(dòng)路徑

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

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

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

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

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

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

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

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

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

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

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

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

表4 試驗(yàn)參數(shù)

4 結(jié)論

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

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

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