基于新型RRT算法與視覺定位的機械臂工件抓取*

李 哲，于海生，吳賀榮，孟祥祥

(青島大學a.自動化學院；b.山東省工業(yè)控制技術重點實驗室，青島 266071)

0 引言

隨著機器人技術的發(fā)展，工業(yè)機器人已廣泛應用于各類工業(yè)現(xiàn)場，而隨著工業(yè)任務的復雜多樣化，機器人在運動規(guī)劃、抓取精度等方面有著更加嚴格的要求[1]。機械臂在執(zhí)行任務過程中，通常會被工作空間的一些物體所阻礙，如果以傳統(tǒng)示教的方式進行控制，在避免碰撞等方面會有很高的難度。智能避障路徑規(guī)劃算法[2]可以有效地解決這種問題，使得機械臂可以適應更加復雜的環(huán)境。同時傳統(tǒng)示教模式下執(zhí)行任務大都是進行機械式地重復，如果工件位置出現(xiàn)偏差，機器人就可能與工件發(fā)生碰撞而導致作業(yè)失敗。而隨著機器視覺技術的迅速發(fā)展，將機器視覺應用到工業(yè)機器人可以賦予其對目標的識別定位能力[3]，機器人智能性得到極大的增強。

針對機械臂避障問題，國內(nèi)外學者提出了一些路徑規(guī)劃的方法，主要有人工勢場法[4]，A*算法[5]，遺傳算法[6]，快速擴展隨機樹 (RRT)[7-8]等。其中RRT算法因其不需要預處理狀態(tài)空間，以及搜索速度快,便于處理約束等特點,更適用于本系統(tǒng)。但基本的RRT搜索算法具有盲目性的缺陷[9-10]，近年來各個學者提出了許多基于RRT的改進算法。CAO等[11]在節(jié)點擴展時考慮了目標偏向思想，引導節(jié)點向目標點擴展并采用遺傳算法優(yōu)化所生成路徑。代偉等[12]使用Sobol序列代替快速擴展隨機樹中的偽隨機序列，提高了采樣點質(zhì)量和采樣效率。針對目標位置不確定問題，國內(nèi)外學者提出了一些利用機器視覺技術[13-14]進行識別定位的方法。劉正瓊等[15]采用提取工件上特征點進行形狀匹配的方法來實現(xiàn)工件的識別，并且設計了由粗到精抓取網(wǎng)絡的方案。JIANG等[16]提出了基于多重腐蝕和膨脹算子的圖像處理方法，并基于掩模圖像的多次減法來唯一確定抓取角度。

基于以上分析，本文提出一種基于目標偏置的拓展點選擇策略，使得算法在采樣過程中以一定概率向目標點快速拓展，達到算法快速收斂的目的，并通過路徑優(yōu)化和三次樣條插值法，除去規(guī)劃路徑中的冗余點，使得規(guī)劃的路徑精煉且平滑連續(xù)。在視覺定位方面，本文分別采用霍夫圓檢測算法與輪廓提取算法對工件與托盤凹槽進行識別，通過對目標工件和托盤的雙重檢測，使目標識別更加精準，增加了目標定位的準確性和穩(wěn)定性。

1 機械臂抓取系統(tǒng)設計

本文建立的機械臂智能抓取系統(tǒng)，共分為機械臂避障路徑規(guī)劃與目標工件識別兩個模塊，由ABB公司生產(chǎn)的IRB120型6軸機械臂、維視圖像的MV-E EM系列工業(yè)相機、PC機構成。其中PC端與相機系統(tǒng)總體構成如圖1所示。

圖1 系統(tǒng)總體結構圖

在避障路徑規(guī)劃模塊中，通過新型快速拓展隨機樹算法對起始點到原目標點上側之間進行路徑規(guī)劃，采用包絡法將障礙物以及機械臂關節(jié)進行包絡以簡化碰撞檢測，并采用三次樣條插值法針對所規(guī)劃路徑點進行連接平滑化。

當機械臂到達原工件位置上側，托盤和工件即使發(fā)生位置偏差也會出現(xiàn)在相機視野中。在目標視覺定位模塊中，利用維視圖像SDK采集圖像并進行預處理和邊緣檢測。在處理完成后，本文通過霍夫圓檢測算法進行圓型工件識別，同時通過輪廓提取和多邊形逼近法識別出托盤凹槽。最終，將凹槽內(nèi)工件的坐標經(jīng)解算后發(fā)送給機器人控制系統(tǒng)。

2 避障路徑規(guī)劃

2.1 碰撞檢測

節(jié)點拓展算法中每隨機出一個新節(jié)點，都需要通過碰撞檢測來判別節(jié)點的有效性，所以機械臂路徑規(guī)劃的效率和碰撞檢測的效率直接相關。因機械臂在工業(yè)任務中，所需考慮的碰撞對象為機械臂連桿和環(huán)境中的剛體，所以本文通過包絡法將碰撞檢測進行簡化。

為了簡便運算，通過包絡法將環(huán)境中的障礙物用球體進行包絡，將機械臂連桿用圓柱體進行包絡，同時將圓柱體的半徑疊加到球體的半徑上r=r1+r2，由此將碰撞檢測問題轉換成線段和球的位置關系，計算量得到大大簡化。

設球心O到直線AB的垂直距離為dvert，到點A和點B的相對較短距離為dmin=min(dOA,dOB)，則根據(jù)包絡體之間的位置關系分為以下情況討論：

(1)球心在線段上的垂足p位于連桿AB上，且dvert>r，則連桿與障礙物不發(fā)生碰撞；

(2)球心在線段上的垂足p位于連桿AB上，且dvert

(3)球心在線段上的垂足p位于連桿AB的延長線上，且dmin>r，此時連桿與包絡球體沒有交點，即與障礙物不會發(fā)生碰撞；

(4)球心在線段上的垂足p位于連桿AB的延長線上，且dmin

圖2 包絡法障礙物建模

2.2 RRT算法原理

圖3 隨機樹拓展

本文利用RRT算法在機械臂關節(jié)空間中進行搜索，得到一條從初始角度qinit到目標角度qgoal的無碰撞路徑，避免了在笛卡爾空間中進行規(guī)劃所需的大量逆運動學求解，算法示意圖如圖3所示。

算法基本原理是：

步驟1：將拓展樹T進行初始化，并添加初始點qinit；

步驟2：通過節(jié)點生成算法產(chǎn)生隨機點qrand；

步驟3：在拓展樹中搜索與隨機點qrand距離最短的鄰近點qnear；

步驟4：通過拓展算法沿向量qnear、qrand的方向生成qnew；

步驟5：將新產(chǎn)生的節(jié)點qnew和鄰近點qnear之間進行碰撞檢測，在不發(fā)生的碰撞的情況下保留qnew，否則轉到步驟2重新尋找節(jié)點；

步驟6：檢查節(jié)點qnew與目標點qgoal之間的距離，如果新產(chǎn)生的節(jié)點已到達目標點或者在一定約束范圍以內(nèi)，則成功結束算法；

步驟7：如果算法沒有成功結束，則重復進行步驟2～步驟6。

雖然傳統(tǒng)的RRT算法可以在機械臂工作空間中進行應用，但是在規(guī)劃過程中存在很多缺陷，比如算法規(guī)劃時間長、導向性差、路徑點冗余以及路徑不夠光滑等。針對以上問題，本文提出相應的解決方案進行改進。

2.3 RRT算法優(yōu)化

2.3.1 拓展點選擇策略

針對傳統(tǒng)的RRT算法在路徑規(guī)劃中的高隨機性、收斂時間長等特點，本文提出一種基于目標偏置的拓展點選擇策略?；诟怕蔖thr的目標偏置策略的RRT算法偽代碼描述如表1所示。

表1 拓展點選擇策略

策略核心思想主要是在迭代過程中引入目標偏置概率Pthr，并在0～1范圍內(nèi)取隨機概率Pran，若Pran大于閾值Pthr，則拓展樹向目標點qgoal方向開拓新節(jié)點，否則，拓展樹向隨機生成函數(shù)產(chǎn)生的新節(jié)點方向進行生長。

另考慮拓展過程中所收集到的空間和障礙物信息，對目標偏置概率Pthr進行調(diào)整。當新節(jié)點拓展過程中沒有與障礙物發(fā)生碰撞時，增大Pthr，當拓展過程中與障礙物發(fā)生碰撞時，減小Pthr，以使隨機樹更快地覆蓋較少受阻的空間區(qū)域。

2.3.2 路徑優(yōu)化

在使用上述拓展點選擇策略的情況下，因算法本身的隨機性所產(chǎn)生的節(jié)點仍有可能冗余，使得規(guī)劃的路徑有所偏離，所以有必要對路徑中的節(jié)點進行篩選優(yōu)化。路徑優(yōu)化的偽代碼描述如表2所示。

表2 路徑優(yōu)化策略

算法首先確定RRT成功搜索后的路徑點數(shù)組Pplan長度，并將Pplan中的初始路徑點添加到優(yōu)化路徑點數(shù)組Pnew當中。之后對初始路徑點Pplan[1]與相差一個路徑點的Pplan[3]進行碰撞檢測，如果沒有發(fā)生碰撞，則說明中間路徑點Pplan[2]不是必要路徑點，刪去該點并檢查Pplan[1]與Pplan[4]的碰撞情況，以此類推，直到檢測出發(fā)生碰撞為止；如果發(fā)生碰撞，則該中間點為必要路徑點，將其添加到優(yōu)化路徑點數(shù)組Pnew當中，接下來從該中間路徑點開始進行以上步驟，最終得到優(yōu)化后的路徑點數(shù)組Pnew。

2.3.3 平滑處理

算法規(guī)劃的路徑在通過優(yōu)化之后仍會存在曲線折段，若節(jié)點之間差距過大則會導致機械臂在運行過程中產(chǎn)生較大的抖動和沖擊，機械臂的平穩(wěn)性不能得到有效保證，甚至可能受到損壞。

本文采用三次樣條插值法對經(jīng)過優(yōu)化過后的路徑點進行平滑連接，使規(guī)劃后的末端軌跡更加平滑，減少機械臂在運動中的卡頓和抖動現(xiàn)象。

θi,j=a0+a1t+a2t2+a3t3

(1)

式中，i∈(1,n)為規(guī)劃后的路徑點個數(shù)；j∈(1,6)分別為機械臂的6個關節(jié)軸。

3 目標工件的視覺定位

3.1 預處理及邊緣檢測

相機直接采集的圖像，通常會受到各種噪聲的干擾，因此本文采用圖像濾波、圖像分割等方法對圖像作預處理以改善目標的特征信息。

中值濾波采用非線性的方法，可以保護圖像尖銳的邊緣，在平滑脈沖噪聲方面非常有效，所以本文在對圖像進行灰度化后，采用3×3的中值濾波模板進行圖像濾波，針對圖像中的每一個像素值，都采用該點與其鄰域中內(nèi)8個像素點的中值代替。

(2)

圖像分割方法一般分為全局二值化和局部二值化，其中全局二值化可能會過濾掉圖像中必要的特征信息，故本文采用局部二值化進行圖像分割，將圖像分成25×25的鄰域塊，對不同的鄰域塊取相應的局部平均值作為二值化閥值。中值濾波和局部二值化的效果如圖4b和圖4c所示。

(a) 初始圖 (b) 中值濾波 (c) 局部二值化 (d) 邊緣檢測

因實驗平臺中的輪廓相對復雜，所以本文采用能夠盡可能多地標識圖像邊緣的Canny算子，檢測效果如圖4d所示。Canny算法使用Sobel算子來計算像素點的梯度，相應的卷積模板如式(3)所示。

(3)

(4)

在得到梯度幅值與方向后，沿著梯度方向找出其鄰域內(nèi)兩個相鄰點Gs1Gs2，并判斷該點是否為局部最大值G(i,j)≥max(Gs1,Gs2)，將不滿足條件的非邊緣點剔除。同時采用雙閾值算法，去除梯度幅值過低的點，并對雙閾值范圍之間的點進行二次篩選，最后連接所有邊緣以得到圖像輪廓。

3.2 目標識別

3.2.1 圓型工件檢測

針對待識別的圓型目標工件，本文采用霍夫圓變換算法來檢測圖像中的圓形，基本思想是將圖像的空間域(u,v)變換到圓形參數(shù)空間(x,y,r)，首先進行圓心估計，對邊緣檢測圖再執(zhí)行一次Sobel算子，計算出所有像素點的鄰域梯度。算法之后遍歷圖中的非0像素點，沿著所計算梯度方向畫線，并將線段所經(jīng)過點(a,b)對應的累加器N(a,b)+=1，最后統(tǒng)計排序N(a,b)，得出可能的圓心。

圖5 霍夫圓變換

半徑估計首先計算所有非0像素點和圓心的距離，將像素點按照距離進行排序，并根據(jù)閾值選取可能半徑，之后再次遍歷像素點，對于每個點所估計的半徑r，使其相應累加器N(r)+=1，最后統(tǒng)計排序N(r)，得到可能的半徑值。在算法進行檢測后，過濾掉半徑微小的圓，最終效果如圖5所示。

3.2.2 托盤凹槽檢測

在運用Canny算子對圖像進行邊緣檢測后，通過findContours( )函數(shù)進行外層輪廓提取，得出一系列點集。針對所提取的閉合輪廓，本文通過多邊形擬合法進行近似逼近，以輪廓中相距最遠的兩個點為基礎，不斷將其他點添加到擬合輪廓當中，直到新的擬合多邊形輪廓與原輪廓的弧長差值滿足精度要求，最終多邊形擬合效果如圖6a所示。針對擬合多邊形，以矩形特征為標準進行篩選，并過濾掉過小的多邊形，篩選效果如圖6b所示。

(a) 多邊形擬合 (b) 原圖顯示

在篩選之后提取出對應的矩形輪廓信息，得到x×n個輪廓點的圖像坐標pi(ui,vi)，其中x為篩選所得矩形數(shù)量。通過類比質(zhì)心法，可計算得到所擬合矩形的形心，具體公式為：

(5)

圖7 目標工件識別

3.3 位置坐標計算

相機標定可以得到內(nèi)參矩陣MI和相應外參矩陣MO，并利用外參矩陣MO和對應機械臂末端姿態(tài)Thand2base進行手眼標定，得出機械臂末端與相機的坐標變換矩陣Tcam2hand。通過內(nèi)參矩陣MI和手眼矩陣Tcam2hand即可將目標工件在圖像坐標系中的坐標(u,v)轉化到機械臂坐標系上。

首先已知基坐標系轉換到圖像坐標系公式：

(6)

式中，內(nèi)參矩陣MI、手眼矩陣Tcam2hand均可由上述標定環(huán)節(jié)計算得到。Zc為相機坐標系下目標工件的高度，可由目標實際高度Zw與式(6)推導得出。

對上述公式進行逆變換，進一步可得：

(7)

式中，(Xw,Yw,Zw)為目標工件在機械臂坐標系下的對應坐標，并可根據(jù)與基坐標系之間的坐標轉換矩陣Thand2base得到基坐標下的工件空間坐標(Xb,Yb,Zb)。

4 仿真及實驗驗證

4.1 路徑規(guī)劃仿真

本文通過MATLAB驗證所設計的新型RRT算法，利用robotics toolbox針對IRB120機器人的DH參數(shù)進行建模，并分別在坐標(0.2,-0.35,0.1)、(0.25,-0.25,0.35)處進行直徑0.1 m的障礙物模擬，設置機器人起始角度為(-90°,15°,20°,0°,50°,0°)，目標角度為(0°,0°,0°,0°,90°,0°)，任務為從機械臂初始位姿到目標位姿之間規(guī)劃出一條無碰撞的路徑，其中初始位姿如圖8a所示，目標位姿如圖8b所示。

(a) 初始位姿 (b) 目標位姿

為了說明本文所提新型RRT算法的有效性,將新型RRT與傳統(tǒng)RRT算法進行對比。兩種算法相對應的節(jié)點拓展情況和最終規(guī)劃路徑如圖9和圖10所示。

(a) 末端路徑搜索 (b) 機械臂模擬運行

(a) 末端路徑搜索 (b) 機械臂模擬運行

圖中淺灰色線條是RRT算法最終尋找到的路徑，灰色線條是算法在搜索過程中進行的嘗試。經(jīng)過對圖9、圖10進行比較，可見本文所提出的新型RRT算法的空間節(jié)點數(shù)明顯少于傳統(tǒng)RRT方法，搜索過程更具有導向性，且顯著提升了算法效率，路徑比較平滑。結合表3數(shù)據(jù)可得，新型RRT算法相較于傳統(tǒng)RRT算法的平均采樣節(jié)點數(shù)由145.3縮減到38.2，減少了約73.71%，平均路徑長度由1.292 m縮短到0.848 m，減少了約34.37%，平均規(guī)劃時間層面上由26.8 s降低到10.3 s，減少了約61.57%。

表3 實驗結果平均值

本文采用三次樣條插值法對RRT算法拓展得到的一系列路徑點進行連接插值，最終對機械臂各關節(jié)所規(guī)劃的角度曲線如圖11所示。

圖11 三次樣條插值法

從圖中可以看出，機械臂的各關節(jié)角度都平穩(wěn)變化，避免了因角度發(fā)生突變而造成的機械臂抖振情況，滿足了機械臂控制的平穩(wěn)性要求。

4.2 實驗平臺驗證

圖12 機器人實驗平臺

為驗證本文所述機器人智能抓取系統(tǒng)的可行性和有效性，本文搭建實驗平臺如圖12所示。系統(tǒng)硬件部分由IRB120機器人、PC端、維視圖像MV-E EM系列工業(yè)相機構成，軟件任務由QT完成，負責路徑規(guī)劃、圖像的采集與處理、以及與機器人的通訊。

實驗共分為機械臂路徑規(guī)劃與機器視覺識別定位兩個部分：首先通過新型RRT算法進行路徑規(guī)劃，控制機器人沿規(guī)劃路徑點到達原工件的上側位置，當機械臂到達當指定位置后，通過TCP/IP通訊向PC端發(fā)送成功信號；PC端接收到信號之后，啟動視覺模塊以識別出等待抓取的圓型工件，將其中心坐標經(jīng)過處理轉化到機器人坐標系，并發(fā)送到機械臂，機械臂則根據(jù)由視覺信息進行精準抓取。其中機械臂控制過程如圖13所示。

(a) 初始位姿 (b) 路徑規(guī)劃 (c) 目標位姿 (d) 識別抓取

實驗結果表明，機械臂經(jīng)過路徑規(guī)劃成功地由初始位姿到達了原工件位置的上方，系統(tǒng)切換為視覺定位控制方案。在工件識別實驗中，將托盤位置進行適量的偏移，隨機地將圓型工件進行擺放且確保有一個工件位于托盤凹槽內(nèi)，該工件即為待抓取的目標工件，通過本文所提出的機器視覺算法對目標進行識別定位，累積進行50次實驗，識別結果以及位置誤差如圖14和圖15所示。

(a) 目標位于右側凹槽 (b) 目標位于左側凹槽 (c) 目標位于左側凹槽 (d) 目標位于中間凹槽

圖15 位置誤差圖

結果表明，本文所提出的視覺定位方案成功對目標工件進行識別，計算的工件坐標與實際位置在X方向的平均誤差為1.382 mm，在Y方向的平均誤差為1.132 mm，由于機械臂末端執(zhí)行裝置為氣動吸嘴，所以誤差范圍內(nèi)工件都可以成功吸取。

5 結論

本文針對傳統(tǒng)RRT算法的規(guī)劃時間長、收斂性較差以及路徑尖點較多等缺點，提出一種結合基于目標偏置的拓展點選擇策略以及路徑平滑優(yōu)化策略的改進方案，結果表明改進后的新型算法在搜索效率、路徑長度以及用時等方面都優(yōu)于傳統(tǒng)RRT算法，在經(jīng)過路徑優(yōu)化以及三次樣條插值后，機械臂可以平滑地通過更加簡捷的路徑點。在到達目標路徑點后，本文提出的采用霍夫圓變換法和多邊形逼近法進行雙重檢測的視覺定位方案，可以有效地識別出圓型工件和托盤凹槽，成功地對等待抓取的工件進行定位。結果證明了所提算法在機械臂上應用的可行性，具有良好的發(fā)展空間。但本文設計的機器視覺算法以固定高度的工件為處理對象，所以未來的研究重點將主要考慮獲取目標的深度信息以對工件高度進行精準定位等。