荀 一 李道政 王 勇 黃旭婷 王志恒 楊慶華

(1.浙江工業(yè)大學(xué)特種裝備制造與先進(jìn)加工技術(shù)教育部/浙江省重點(diǎn)實(shí)驗(yàn)室，杭州 310023;2.浙江艾佳食品有限公司，衢州 324000)

0 引言

采摘機(jī)器人大多依賴于多自由度機(jī)械臂與視覺結(jié)合的方式實(shí)現(xiàn)定位和抓取過程[1-6]。其關(guān)鍵技術(shù)之一就是機(jī)械臂的路徑規(guī)劃，即機(jī)械臂從起始位置運(yùn)動到目標(biāo)位置，且中間不與非目標(biāo)障礙物發(fā)生碰撞的一項(xiàng)技術(shù)。目前，國內(nèi)外學(xué)者針對路徑規(guī)劃技術(shù)進(jìn)行了大量的研究，并取得了相應(yīng)成果[7-10]。但將面向低維度空間使用的路徑規(guī)劃算法運(yùn)用在多自由度機(jī)械臂上時，隨著維度的增加，這些算法的復(fù)雜度就會呈“指數(shù)”增長，影響了規(guī)劃的實(shí)時性[11]。為解決這個問題，基于采樣的路徑規(guī)劃方法和一些降維的規(guī)劃算法被提出。鄒宇星等[12]提出一種改進(jìn)的概率地圖(Probabilistic roadmap, PRM)路徑規(guī)劃算法應(yīng)用在采摘機(jī)械臂上，算法將機(jī)械臂發(fā)生碰撞的位姿遍歷，映射到構(gòu)型空間中，再使用PRM算法搜索路徑。姚立健等[13]針對茄子采摘機(jī)器人，將三維空間的路徑規(guī)劃降低為二維空間后再應(yīng)用A*算法規(guī)劃，降低了計(jì)算復(fù)雜度。張勤等[14]等研究出基于空間分割的實(shí)時運(yùn)動規(guī)劃算法最優(yōu)采摘空間(Optimal picking space, OPS)，引入評價函數(shù)選取最優(yōu)采摘空間，最后在采摘任務(wù)的基礎(chǔ)上加入實(shí)時避障子任務(wù)，引導(dǎo)機(jī)械臂躲避障礙完成任務(wù)。面對非結(jié)構(gòu)環(huán)境的復(fù)雜情況，馬冀桐等[15]利用在構(gòu)形空間中增加先驗(yàn)知識引導(dǎo)點(diǎn)的方式，在路軌規(guī)劃中避開封閉多邊形障礙物，再使用RRT-connect規(guī)劃引導(dǎo)點(diǎn)前后兩段路徑并將它們進(jìn)行合并。而陽涵疆等[7]則是將混聯(lián)采摘機(jī)械臂中串聯(lián)和并聯(lián)的部分分開，對串聯(lián)機(jī)械臂部分使用RRT算法規(guī)劃，降低了規(guī)劃的維度，加快了規(guī)劃速度，且混聯(lián)方式可以更好地應(yīng)對復(fù)雜非結(jié)構(gòu)環(huán)境。張勤等[16]為解決RRT算法采樣效率低下的問題，提出了柯西分布的采樣方式增加采樣的目的性，并引入目標(biāo)引力和動態(tài)調(diào)整步長的方式，在增加目的性同時減小碰撞概率，其中用到的節(jié)點(diǎn)拒絕策略減少長路徑節(jié)點(diǎn)的增加。但是其生成的路徑為斷續(xù)的折線，直接運(yùn)用在機(jī)械臂上會致其發(fā)生碰撞，且在較遠(yuǎn)處開始規(guī)劃時，沒有考慮相機(jī)精度，可能導(dǎo)致采摘點(diǎn)產(chǎn)生誤差。GAMMELL等[17]根據(jù)第1次RRT*得出的路徑長度來設(shè)置一個對應(yīng)長軸長度的超橢球采樣空間，將后續(xù)采樣都限制在該采樣空間中，每次有更優(yōu)路徑產(chǎn)生時，就更新長軸長度，逐步縮小采樣空間，直至找到最優(yōu)路徑。這種方式加快了規(guī)劃速度，具有更高的效率。除了基于采樣的算法外，近幾年還出現(xiàn)了利用深度學(xué)習(xí)和強(qiáng)化學(xué)習(xí)規(guī)劃的算法[18-19]。

但上述算法均依靠視覺提供的目標(biāo)點(diǎn)空間信息，屬于“看后動”的開環(huán)控制形式，若視覺信息或者機(jī)械臂位置信息有誤差，將導(dǎo)致機(jī)械臂末端執(zhí)行器的最終位姿和目標(biāo)位姿有偏差。針對這種情況，有學(xué)者提出將視覺信息用于閉環(huán)控制的方式，即視覺伺服控制[20]。最常用于農(nóng)業(yè)采摘的視覺伺服控制方式就是用于“眼在手上”(eye-in-hand)的基于圖像的視覺伺服方式(Image based visual servoing, IBVS)。金玉成等[21]建立手眼協(xié)調(diào)的坐標(biāo)關(guān)系，據(jù)此提出了由遠(yuǎn)及近手眼協(xié)調(diào)策略，提高了采摘效率和精度。MEHTA等[22]根據(jù)球形果實(shí)的形心和距離等少量特征，設(shè)計(jì)了旋轉(zhuǎn)控制器和平移控制器用于IBVS控制，并用李雅普諾夫證明了其穩(wěn)定性。基于視覺伺服的運(yùn)動規(guī)劃精度高，穩(wěn)定性好，利于應(yīng)用于采摘工作。但是其無法考慮環(huán)境碰撞因素而導(dǎo)致應(yīng)用范圍受限，只能用于無障礙的空曠場景。

為解決傳統(tǒng)采樣方法目的性弱、采樣效率低,采樣終點(diǎn)定位存在誤差,普通視覺伺服控制無法避障的問題，本文提出結(jié)合視覺伺服的改進(jìn)隨機(jī)快速搜索樹算法(VS-IRRT)。針對傳統(tǒng)RRT算法所存在的采樣無目的性和路徑雜亂的情況，通過使用超橢球引力偏置和密度減小措施改變采樣方式，同時用貪婪思想和B樣條曲線優(yōu)化所得路徑。在接近目標(biāo)點(diǎn)時使用基于平移控制器的圖像視覺伺服方式消除位置誤差，最后到達(dá)目標(biāo)點(diǎn)完成采摘過程。

1 空間環(huán)境建模

目標(biāo)果實(shí)周圍會存在如枝干和非目標(biāo)果實(shí)等障礙，啟動采摘過程之前，采摘機(jī)器人需要先對其周圍環(huán)境進(jìn)行感知和建模。本文使用的是eye-in-hand的相機(jī)安裝模式，如圖1所示，RGB-D相機(jī)安裝在末端執(zhí)行器上，跟隨末端執(zhí)行器運(yùn)動。采摘機(jī)器人通過使用由遠(yuǎn)及近手眼協(xié)調(diào)策略[21]，在初始位姿處獲取全局圖像信息，根據(jù)針孔相機(jī)模型，將像素平面上的點(diǎn)結(jié)合相機(jī)內(nèi)參和對應(yīng)的深度信息，計(jì)算出目標(biāo)果實(shí)周圍的點(diǎn)云信息。對點(diǎn)云信息濾波后再轉(zhuǎn)換為八叉樹圖信息(Octomap)來提高避障效率。除了障礙物信息，還需要對圖像中的目標(biāo)果實(shí)識別提取，通過將果園拍攝的500幅果實(shí)圖像使用Labelme軟件打上標(biāo)簽訓(xùn)練，用YOLO v5模型以高準(zhǔn)確率和快速度得到目標(biāo)果實(shí)在像素平面上的位置，最后再結(jié)合RGB-D相機(jī)的深度信息，獲取果實(shí)中心在空間中的坐標(biāo)，整個空間環(huán)境信息處理如圖2所示。圖2a所示的是原圖像經(jīng)過YOLO v5識別后的效果，數(shù)字部分為預(yù)測準(zhǔn)確率；圖2b為對應(yīng)的點(diǎn)云圖像；圖2c為點(diǎn)云轉(zhuǎn)換的八叉樹圖，用像素塊來近似表示障礙物，減小了避障的計(jì)算復(fù)雜度。

圖1 空間環(huán)境示意圖

圖2 空間環(huán)境信息處理

2 VS-IRRT算法

2.1 VS-IRRT算法組成

本文提出的VS-IRRT算法主要由改進(jìn)RRT算法和基于平移控制器的視覺伺服方法兩部分組成，如圖3所示。圖中VS-IRRT分為兩部分，其中改進(jìn)RRT算法中包含了采樣方式的改進(jìn)和路徑優(yōu)化；而采樣方式的改進(jìn)體現(xiàn)在超橢球引力偏置采樣方法和密度減小策略上。圖中箭頭表示同一層次部分算法的運(yùn)行順序。

圖3 VS-IRRT算法組成示意圖

2.2 基于超橢球的引力偏置采樣方法

在傳統(tǒng)的RRT算法中，采樣點(diǎn)搜尋使用的是隨機(jī)均勻采樣的方法，這種方式的路徑搜索沒有目的性，收斂速度慢，整個算法效率不高。針對這一點(diǎn)，引入了基于超橢球的引力偏置采樣方法來代替隨機(jī)均勻采樣，如圖4所示。引力偏置是指采樣點(diǎn)被超橢球吸引，出現(xiàn)在超橢球內(nèi)部的概率變大。搜索樹通過隨機(jī)采樣節(jié)點(diǎn)從采樣開始節(jié)點(diǎn)搜索出一條無障礙路徑到達(dá)采樣終點(diǎn)。

圖4 超橢球引力偏置節(jié)點(diǎn)擴(kuò)展過程

采樣開始時分別設(shè)置采樣概率閾值Pout和Pgoal，其中Pgoal為偏向目標(biāo)的采樣概率，一般取值為0.9～0.95，本文取0.9。采樣概率P在(0,1)中隨機(jī)均勻采樣。當(dāng)P>Pgoal時，采樣點(diǎn)Xrand=Xend,即直接選擇目標(biāo)點(diǎn)作為采樣點(diǎn)進(jìn)行生長；當(dāng)Pout

‖Xrand-Xstart‖+‖Xrand-Xend‖≤2a

(1)

其中

a2=(1+γ2)c2

(2)

式中Xrand——隨機(jī)采樣點(diǎn)

Xstart——采樣起始點(diǎn)

Xend——采樣終點(diǎn)

a——超橢球長軸長度

c——超橢球焦點(diǎn)間距，也是Xstart和Xend間的距離

γ——比例系數(shù)，取0～1

γ決定超橢球的短軸長度，γ越小，超橢球越扁平，采樣目的偏向性更強(qiáng)，當(dāng)障礙物較少時，該值可增大，本文取γ=0.5。

使用上述采樣方式雖然在特定的環(huán)境下Pout有不錯的效果，但如果每次更換采樣環(huán)境時，都需要尋找最合適的采樣閾值，這使得算法的環(huán)境適應(yīng)性下降。針對這種問題，本文提出了自適應(yīng)閾值的方式，即在采樣過程中采樣閾值動態(tài)變化適應(yīng)環(huán)境。采樣閾值自適應(yīng)的思想是：當(dāng)采樣到的碰撞點(diǎn)增多時，說明采樣目標(biāo)偏向的方向有大量障礙物，此時需要提高采樣的隨機(jī)性，即超橢球外的采樣概率。據(jù)此，可以得到自適應(yīng)閾值的公式為

(3)

(4)

式中Pmin——自適應(yīng)閾值的限定最小值

Pmax——自適應(yīng)閾值的限定最大值

β——影響因子

α——此次采樣之前的采樣點(diǎn)碰撞次數(shù)

λ——環(huán)境復(fù)雜系數(shù)，環(huán)境越復(fù)雜，λ取值越大，使得Pout變大，采樣偏向于隨機(jī)均勻采樣，λ一般取值大于1，本文取1.25

θ——機(jī)械臂自由度，取6

這種采樣方式增加了采樣的目標(biāo)偏向性，加快了收斂速度，同時還保留了一定的隨機(jī)性，避免因?yàn)檎系K物過多導(dǎo)致采樣困在局部區(qū)域的現(xiàn)象。

2.3 密度減小策略

在傳統(tǒng)RRT算法中，確定完采樣點(diǎn)后，將選擇樹中距離采樣點(diǎn)最近的節(jié)點(diǎn)Xnear向采樣點(diǎn)方向擴(kuò)展一個固定步長得到新節(jié)點(diǎn)Xnew。但固定步長使擴(kuò)展的新節(jié)點(diǎn)可能出現(xiàn)在樹的內(nèi)部，如圖5a所示，使得樹內(nèi)存在其它節(jié)點(diǎn)與新節(jié)點(diǎn)的距離更近，最后導(dǎo)致樹的節(jié)點(diǎn)密度激增，算法效率降低。

為解決固定步長導(dǎo)致密度激增的問題，本文提出了一種密度減小措施，如圖5b所示，通過找到距離新節(jié)點(diǎn)Xnew最近的樹內(nèi)節(jié)點(diǎn)Xnearest，若該節(jié)點(diǎn)就是新節(jié)點(diǎn)的父節(jié)點(diǎn)，即Xnearest=Xparent，則保存新節(jié)點(diǎn)到樹中，否則拋棄新節(jié)點(diǎn)。密度減小措施可以減小整個生成樹的密度，通過篩選減少采樣點(diǎn)的個數(shù)來提高路徑搜索算法的速度。

2.4 路徑優(yōu)化方法

在算法執(zhí)行完成后，生成的路徑不一定是整個已連接節(jié)點(diǎn)組成路徑中最短的，且路徑為折線。這樣的路徑直接應(yīng)用在機(jī)械臂路徑規(guī)劃中可能會引起機(jī)械臂的振蕩，影響采摘效果。針對這些問題，本文使用貪心思想和B樣條曲線來優(yōu)化生成的路徑。貪心思想的整個過程如圖6a所示，在已生成的路徑中，從Xstart開始依次向后面的節(jié)點(diǎn)連接，若兩點(diǎn)連接路線上沒有障礙物，則繼續(xù)向后面的節(jié)點(diǎn)發(fā)起連接，直至連接(Xstart,Xn)經(jīng)過了障礙物。此時，將連接(Xstart,Xn-1)代替節(jié)點(diǎn)Xn-1前的所有連接，然后將Xn作為新的起點(diǎn)重復(fù)上述步驟，在連接到Xend時結(jié)束。

圖6 路徑優(yōu)化示意圖

貪心思想可以縮短原始路徑，但是并不能解決路徑曲折的問題。針對這個問題，考慮到B樣條曲線的局部性和連續(xù)性[23]，本文使用B樣條曲線對優(yōu)化后的路徑平滑擬合，公式為

(5)

式中Pi——曲線的控制點(diǎn)，本文選擇相連的路徑點(diǎn)作為控制點(diǎn)來形成局部的三次B樣條曲線

Fi,k(t)——k階B樣條基函數(shù)

平滑后的圖像如圖6b所示。

2.5 基于平移控制器的圖像視覺伺服方法

僅依靠改進(jìn)RRT算法仍然不能解決因視覺定位信息和機(jī)械臂位置產(chǎn)生誤差導(dǎo)致采摘失敗的情況，還需要引入連續(xù)視覺圖像作為反饋信息，從而形成閉環(huán)的視覺伺服控制方法。為將兩種路徑規(guī)劃方法結(jié)合起來，本文提出了分段控制的思想：在開始規(guī)劃路徑之前，先選擇采樣終點(diǎn)Xend作為兩段控制的分界點(diǎn)，即在機(jī)械臂到達(dá)Xend前采用改進(jìn)RRT的規(guī)劃方法，到達(dá)Xend之后采用視覺伺服的規(guī)劃方法。本文以目標(biāo)果實(shí)為中心處建立半徑為R的隨機(jī)空間作為Xend的選擇空間，當(dāng)R選擇適當(dāng)值時，機(jī)械臂在視覺伺服過程中和果實(shí)的距離較小，可看作在無障礙環(huán)境下規(guī)劃路徑。

如圖7所示，Xend包括在隨機(jī)空間中，其中隨機(jī)空間為兩虛線代表的球中間夾著的空間，表示為

圖7 采樣終點(diǎn)選取

(6)

Rmin≤(X-XO)2+(Y-YO)2+(Z-ZO)2≤Rmax

(7)

(8)

Rmin=dmin+r

(9)

式中Trand——采樣空間中隨機(jī)位置

Rrand——采樣空間中隨機(jī)位姿

X、Y、Z——Xend坐標(biāo)

XO、YO、ZO——果實(shí)中心坐標(biāo)

σx、σy、σz——(0,1)間互相獨(dú)立的隨機(jī)數(shù)，隨機(jī)空間是半徑為Rmin和Rmax的兩個球之間的空間

dmin——深度相機(jī)能夠測量深度的最小距離

r——果實(shí)半徑

考慮機(jī)械臂不與附近實(shí)體發(fā)生干涉，根據(jù)實(shí)地情況測量估計(jì)Rmax=1.5Rmin比較合適。

確定完Xend的平移量Trand后，還需要對其旋轉(zhuǎn)量Rrand進(jìn)行篩選。式(8)中的旋轉(zhuǎn)量使用單位四元數(shù)形式表示。Rrand的值會反映到機(jī)械臂末端的姿態(tài)上，進(jìn)而影響到相機(jī)對目標(biāo)果實(shí)的信息獲取情況，如圖8所示，本文使用的視覺伺服方式是基于圖像的視覺伺服，故在整個伺服過程中目標(biāo)果實(shí)要保持出現(xiàn)在相機(jī)的視野范圍內(nèi)(Field of view, FOV)，相機(jī)在圖中Xend處需完整觀測到果實(shí)部分。

圖8 采樣終點(diǎn)處相機(jī)視野示意圖

為判斷目標(biāo)果實(shí)是否位于相機(jī)視野內(nèi)，就需要獲取目標(biāo)果實(shí)與相機(jī)的相對位姿關(guān)系。根據(jù)各坐標(biāo)系間的轉(zhuǎn)換關(guān)系，可以得到

(10)

(11)

式中O為目標(biāo)果實(shí)的坐標(biāo)，其左上標(biāo)B、e、c分別表示參考坐標(biāo)系為機(jī)械臂基坐標(biāo)系、機(jī)械臂末端參考坐標(biāo)系、相機(jī)坐標(biāo)系，T為坐標(biāo)系轉(zhuǎn)換矩陣，轉(zhuǎn)換對象分別標(biāo)注在左上方和左下方，下標(biāo)init表示在機(jī)械臂初始位姿的條件下。坐標(biāo)系e、c的相對關(guān)系可以通過手眼標(biāo)定[24]的方法確定下來，坐標(biāo)系B、e也可以通過機(jī)械臂D-H建模后[25]，使用正運(yùn)動學(xué)得出。

在隨機(jī)生成Xend后，由式(10)、(11)可以推導(dǎo)出目標(biāo)果實(shí)和相機(jī)的位置關(guān)系，cO=(Xc,Yc,d),其中d為RGB-D相機(jī)測出的深度信息，與相機(jī)坐標(biāo)系下目標(biāo)果實(shí)在Zc方向的值相等。如圖8所示，相機(jī)的視場角為δ×γ，相機(jī)視野范圍為一矩形平面，矩形的尺寸與深度d有關(guān)。綜合上述信息可以得出目標(biāo)果實(shí)在相機(jī)視野范圍內(nèi)的判定條件

(12)

(13)

(14)

若滿足式(12)的條件，則判定Xend為有效采樣終點(diǎn)，否則重新隨機(jī)生成新的采樣終點(diǎn)。在得到采樣終點(diǎn)后，從該點(diǎn)開始對目標(biāo)果實(shí)實(shí)行圖像視覺伺服規(guī)劃。視覺伺服中最重要的就是圖像雅可比矩陣Jv，它的逆可以將圖像當(dāng)前特征點(diǎn)和目標(biāo)特征點(diǎn)的誤差轉(zhuǎn)換映射為相機(jī)的笛卡爾坐標(biāo)下的速度。Jv是維度2×6的矩陣，對其求逆時可能會在奇異點(diǎn)附近而使得到的速度產(chǎn)生突變，這不利于機(jī)械臂的控制，故特征點(diǎn)的數(shù)量一般要求大于3個。在使用YOLO v5識別果實(shí)時，會發(fā)現(xiàn)除了果實(shí)中心坐標(biāo)外，沒有穩(wěn)定的特征點(diǎn)供選擇，傳統(tǒng)的視覺伺服無法直接應(yīng)用在采摘過程中。參考文獻(xiàn)[22]，將特征設(shè)計(jì)成果實(shí)在圖像像素中的坐標(biāo)(u,v)以及果實(shí)深度距離d，應(yīng)用其平移PD控制器，得到相機(jī)的速度為

(15)

(16)

(17)

(18)

式中kpv1、kdv1、kpv2、kdv2——平面誤差控制和深度誤差控制PD控制器系數(shù)

ev1——果實(shí)在圖像像素平面中與原點(diǎn)的偏差

ev2——果實(shí)深度距離和采摘距離的偏差

J′v——圖像雅可比矩陣Jv的前2列

J″v——圖像雅可比矩陣Jv的第3列

vz——相機(jī)在z方向的速度

vp——相機(jī)x-y平面上的速度

I2——2×2單位矩陣

本文只參考文獻(xiàn)[22]中平移控制器，沒有使用旋轉(zhuǎn)控制器，原因在于旋轉(zhuǎn)控制器的計(jì)算過于復(fù)雜，且作用是將果實(shí)保持在相機(jī)視野范圍內(nèi)，在本文上述內(nèi)容中已經(jīng)解決這個問題。但只使用平移控制器來完成視覺伺服會產(chǎn)生新的問題：當(dāng)相機(jī)距離目標(biāo)果實(shí)很近時，相機(jī)和果實(shí)間即使發(fā)生很小的錯位，也會導(dǎo)致果實(shí)在像素平面內(nèi)產(chǎn)生巨大誤差，從而使得x-y平面速度突變。可表示為d→0,ev1→∞,vp→∞。為解決該問題，本文修改相機(jī)速度vp的表達(dá)式為

(19)

式中e為當(dāng)前特征點(diǎn)位置和目標(biāo)特征點(diǎn)位置的距離。K為比例因子，取0.01，用于減小速度vp，使得ev1能夠在ev2接近0之前收斂為0，防止速度突變發(fā)生。

基于VS-IRRT算法的采摘機(jī)械臂路徑規(guī)劃流程圖如圖9所示。

圖9 基于VS-IRRT算法的采摘機(jī)械臂路徑規(guī)劃流程圖

3 算法仿真

本文使用Matlab 2020a分別對VS-IRRT中的改進(jìn)RRT算法和僅基于平移控制器的視覺伺服算法進(jìn)行仿真，驗(yàn)證算法的有效性和可行性。

3.1 改進(jìn)的RRT算法仿真

本文將改進(jìn)的RRT算法與RRT、RRT*-connect、Informed-RRT*算法分別在二維空間與三維空間中進(jìn)行比較，各算法的路徑規(guī)劃仿真過程如圖10所示。圖10中，2個紅色圓點(diǎn)分別為起始點(diǎn)位置和終點(diǎn)位置，藍(lán)色路徑為樹的拓展路徑，紅色路徑為找到的目標(biāo)路徑，其它幾何體為被視為障礙物的非目標(biāo)果實(shí)。對比后發(fā)現(xiàn)改進(jìn)的RRT算法目的性更強(qiáng)，無效的采樣點(diǎn)數(shù)量較少，執(zhí)行效率就高，同時其路徑振蕩程度小，路徑平滑，利于在機(jī)械臂上直接應(yīng)用。

圖10 改進(jìn)的RRT算法路徑規(guī)劃

考慮到RRT是基于采樣的算法，其隨機(jī)性較大，每次計(jì)算的過程都有差異。為了更客觀比較各算法的性能，本文使各算法在相同的仿真環(huán)境下各運(yùn)行200次，性能數(shù)據(jù)取200次運(yùn)行得出的平均值，數(shù)據(jù)如表1所示。

由表1可以發(fā)現(xiàn)，RRT*算法因?yàn)槭褂弥剡x父節(jié)點(diǎn)和重布線的方式，使得其路徑成本最小，但其帶來的時間代價是巨大的。RRT*-connect算法通過結(jié)合RRT-connect算法的速度優(yōu)勢和RRT*算法的漸近最優(yōu)的優(yōu)勢，能以更快的方式獲取最優(yōu)路徑，圖10c、10g中藍(lán)色和紅色路徑分布代表2顆樹的路徑。而Informed-RRT*算法在RRT*算法的基礎(chǔ)上添加了超橢球范圍限制，在第1次使用RRT*算法得到最優(yōu)路徑后，根據(jù)路徑代價更新Informed-RRT*算法的超橢球長軸長度來縮小采樣范圍，以獲取更快的規(guī)劃速度。本文提出的改進(jìn)RRT算法通過超橢球引力偏置采樣的方式提高了采樣的目的性，用密度減小策略減少采樣次數(shù)，提高效率，最后用貪婪思想和B樣條曲線優(yōu)化生成路徑。不同的是改進(jìn)RRT算法的超橢球不會隨著上一輪迭代找到的更優(yōu)的路徑而縮小超橢球的空間，而且改進(jìn)RRT算法的超橢球依然保持在超橢球外部的采樣概率，這使得路徑規(guī)劃具有完備性，雖然改進(jìn)RRT算法不會漸近最優(yōu)，但根據(jù)實(shí)際應(yīng)用場景，在采摘過程中，其擁有更高的采摘效率。從結(jié)果上來看，改進(jìn)RRT算法采樣點(diǎn)數(shù)較RRT*-connect算法減少92.9%，規(guī)劃時間較RRT*-connect算法減少86.1%，路徑成本雖然沒有RRT*-connect算法小，但較RRT算法也減少17.2%。

表1 算法性能比較

3.2 基于平移控制器的視覺伺服方法仿真

本文用數(shù)值模擬的方式在Matlab上對平移控制器的視覺伺服方法仿真。設(shè)置相機(jī)的初始坐標(biāo)tinit、初始旋轉(zhuǎn)矩陣Rinit以及目標(biāo)點(diǎn)坐標(biāo)O為

tinit=(50,100,1 500)mm

O=(250,350,2 000)mm

使用相機(jī)提供的SDK對其自身標(biāo)定，得到內(nèi)參矩陣為

根據(jù)文獻(xiàn)[21]中的PD控制器的參數(shù)選擇方式，選擇PD控制器的比例系數(shù)和微分系數(shù)為kdv1=0.1,kpv1=10,kdv2=0.4,kpv2=1。

圖11 平移控制器的笛卡爾空間和像素空間路徑

圖12 平移控制器速度與目標(biāo)誤差曲線

4 模擬環(huán)境采摘試驗(yàn)與分析

以胡柚采摘為例，為驗(yàn)證VS-IRRT應(yīng)用在采摘中的效果，保證其在實(shí)際環(huán)境中的可行性，將VS-IRRT算法移植入機(jī)器人操作系統(tǒng)(Robot operating system, ROS)，結(jié)合采摘機(jī)械臂在搭建的模擬試驗(yàn)環(huán)境中試驗(yàn)驗(yàn)證算法，如圖13所示。

圖13 采摘試驗(yàn)平臺

圖13中采摘機(jī)械臂使用UR3六自由度機(jī)械臂，工作空間是以基座為球心的半徑500 mm的近似球形空間，最大負(fù)載3 kg；用于控制的上位機(jī)搭載Intel Core i5處理器、16 GB內(nèi)存、GTX960m顯卡，操作系統(tǒng)為Windows 10以及虛擬機(jī)上的Ubuntu kinetic；使用Intel RealSense D435i型相機(jī)，調(diào)整使其分辨率為640像素×480像素，深度測量范圍為0.16～2.00 m，F(xiàn)OV為69°×42°，安裝在UR3機(jī)械臂的末端，便于實(shí)現(xiàn)視覺伺服。

在采摘過程中，本文使用ROS中帶有的MoveIt插件導(dǎo)入相關(guān)路徑規(guī)劃算法進(jìn)行仿真試驗(yàn)，并在Rviz中顯示采摘相應(yīng)過程與RGB-D相機(jī)感知周圍環(huán)境所產(chǎn)生的點(diǎn)云信息，便于更清晰觀察機(jī)械臂在運(yùn)動過程中的表現(xiàn)，如圖14a所示。機(jī)械臂從初始狀態(tài)開始，經(jīng)過改進(jìn)RRT規(guī)劃到達(dá)采樣終點(diǎn)，之后改用基于平移控制器的視覺伺服，最終到達(dá)采摘點(diǎn)完成采摘，實(shí)際流程如圖14b所示。

圖14 機(jī)械臂VS-IRRT采摘過程

通過訂閱運(yùn)動過程中各個關(guān)節(jié)的角度變化信息，繪制出VS-IRRT算法運(yùn)動規(guī)劃時的關(guān)節(jié)角度變化圖，如圖15所示。觀察圖15發(fā)現(xiàn)試驗(yàn)過程中關(guān)節(jié)變化分為2部分，在1.5～5 s內(nèi)，VS-IRRT中的改進(jìn)RRT算法發(fā)揮作用，各關(guān)節(jié)角度變化較大。在5～6 s內(nèi)，機(jī)械臂切換為視覺伺服算法，獲取圖像和深度誤差信息，這個過程中機(jī)械臂沒有運(yùn)動。在6 s之后，機(jī)械臂使用基于平移控制器的視覺伺服算法，這個過程主要是縮小誤差，各個關(guān)節(jié)角變化較小。

圖15 機(jī)械臂各關(guān)節(jié)角變化曲線

通過對3.1節(jié)中每個算法各試驗(yàn)100次，試驗(yàn)結(jié)果取平均值，見表2。其中的路徑成本是機(jī)械臂在運(yùn)動過程中各關(guān)節(jié)角變化的弧度總和。由表2可知，VS-IRRT算法的采摘速度比RRT*-connect算法只提升48.36%，與改進(jìn)的RRT算法在仿真中的表現(xiàn)不同，這是因?yàn)閂S-IRRT算法在算法轉(zhuǎn)換以及視覺伺服的過程中，程序運(yùn)行速度較慢，導(dǎo)致總的采摘時間變長。在路徑成本方面，VS-IRRT算法路徑成本為6.53 rad，相較RRT算法減少了17.14%。VS-IRRT算法采摘成功率最高，與RRT*-connect算法相比，提升了2.1個百分點(diǎn)。

表2 采摘試驗(yàn)結(jié)果

5 結(jié)論

(1)針對自動化采摘過程中出現(xiàn)的定位誤差以及視覺伺服無法考慮障礙物的問題，提出了結(jié)合視覺伺服的改進(jìn)隨機(jī)快速搜索樹算法。該算法通過使用超橢球引力偏置和密度減小措施改變采樣方式，同時用貪婪思想和B樣條曲線優(yōu)化所得路徑，引導(dǎo)機(jī)械臂以快速平滑的避障方式到達(dá)采樣終點(diǎn)。在接近目標(biāo)點(diǎn)時使用基于平移控制器的圖像視覺伺服方式縮小機(jī)械臂末端執(zhí)行器與目標(biāo)果實(shí)間的誤差，完成采摘過程。

(2)采摘試驗(yàn)結(jié)果表明，VS-IRRT采摘1次的時間為17.79 s，與RRT*-connect算法相比降低48.36%，在路徑成本上較RRT算法降低17.14%，采摘成功率較RRT*-connect算法提升2.1個百分點(diǎn)，是上述算法中最高的。證明了VS-IRRT算法極大提高了機(jī)械臂采摘規(guī)劃的綜合性能。