基于VS-IRRT算法的采摘機械臂路徑規劃

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

(1.浙江工業大學特種裝備制造與先進加工技術教育部/浙江省重點實驗室，杭州 310023;2.浙江艾佳食品有限公司，衢州 324000)

0 引言

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

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

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

1 空間環境建模

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

圖1 空間環境示意圖

圖2 空間環境信息處理

2 VS-IRRT算法

2.1 VS-IRRT算法組成

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

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

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

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

圖4 超橢球引力偏置節點擴展過程

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

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

(1)

其中

a2=(1+γ2)c2

(2)

式中Xrand——隨機采樣點

Xstart——采樣起始點

Xend——采樣終點

a——超橢球長軸長度

c——超橢球焦點間距，也是Xstart和Xend間的距離

γ——比例系數，取0～1

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

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

(3)

(4)

式中Pmin——自適應閾值的限定最小值

Pmax——自適應閾值的限定最大值

β——影響因子

α——此次采樣之前的采樣點碰撞次數

λ——環境復雜系數，環境越復雜，λ取值越大，使得Pout變大，采樣偏向于隨機均勻采樣，λ一般取值大于1，本文取1.25

θ——機械臂自由度，取6

這種采樣方式增加了采樣的目標偏向性，加快了收斂速度，同時還保留了一定的隨機性，避免因為障礙物過多導致采樣困在局部區域的現象。

2.3 密度減小策略

在傳統RRT算法中，確定完采樣點后，將選擇樹中距離采樣點最近的節點Xnear向采樣點方向擴展一個固定步長得到新節點Xnew。但固定步長使擴展的新節點可能出現在樹的內部，如圖5a所示，使得樹內存在其它節點與新節點的距離更近，最后導致樹的節點密度激增，算法效率降低。

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

2.4 路徑優化方法

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

圖6 路徑優化示意圖

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

(5)

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

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

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

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

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

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

圖7 采樣終點選取

(6)

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

(7)

(8)

Rmin=dmin+r

(9)

式中Trand——采樣空間中隨機位置

Rrand——采樣空間中隨機位姿

X、Y、Z——Xend坐標

XO、YO、ZO——果實中心坐標

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

dmin——深度相機能夠測量深度的最小距離

r——果實半徑

考慮機械臂不與附近實體發生干涉，根據實地情況測量估計Rmax=1.5Rmin比較合適。

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

圖8 采樣終點處相機視野示意圖

為判斷目標果實是否位于相機視野內，就需要獲取目標果實與相機的相對位姿關系。根據各坐標系間的轉換關系，可以得到

(10)

(11)

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

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

(12)

(13)

(14)

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

(15)

(16)

(17)

(18)

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

ev1——果實在圖像像素平面中與原點的偏差

ev2——果實深度距離和采摘距離的偏差

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

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

vz——相機在z方向的速度

vp——相機x-y平面上的速度

I2——2×2單位矩陣

本文只參考文獻[22]中平移控制器，沒有使用旋轉控制器，原因在于旋轉控制器的計算過于復雜，且作用是將果實保持在相機視野范圍內，在本文上述內容中已經解決這個問題。但只使用平移控制器來完成視覺伺服會產生新的問題：當相機距離目標果實很近時，相機和果實間即使發生很小的錯位，也會導致果實在像素平面內產生巨大誤差，從而使得x-y平面速度突變?？杀硎緸閐→0,ev1→∞,vp→∞。為解決該問題，本文修改相機速度vp的表達式為

(19)

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

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

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

3 算法仿真

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

3.1 改進的RRT算法仿真

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

圖10 改進的RRT算法路徑規劃

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

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

表1 算法性能比較

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

本文用數值模擬的方式在Matlab上對平移控制器的視覺伺服方法仿真。設置相機的初始坐標tinit、初始旋轉矩陣Rinit以及目標點坐標O為

tinit=(50,100,1 500)mm

O=(250,350,2 000)mm

使用相機提供的SDK對其自身標定，得到內參矩陣為

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

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

圖12 平移控制器速度與目標誤差曲線

4 模擬環境采摘試驗與分析

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

圖13 采摘試驗平臺

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

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

圖14 機械臂VS-IRRT采摘過程

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

圖15 機械臂各關節角變化曲線

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

表2 采摘試驗結果

5 結論

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

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