基于機器視覺和PSO的機器人示教路徑優(yōu)化研究

王亞超，黃沿江，張憲民

（華南理工大學(xué) 機械與汽車工程學(xué)院，廣州 510640）

工業(yè)機器人可以幫助人們快速完成繁重的重復(fù)任務(wù)，也可以在極端環(huán)境中工作，因此廣泛應(yīng)用于汽車制造、工件分揀、產(chǎn)品裝配和碼垛等各個行業(yè)[1]。但是工業(yè)機器人在每次執(zhí)行新的生產(chǎn)任務(wù)之前，都需要對其進行示教。

但是目前的機器人示教方法存在示教系統(tǒng)復(fù)雜、示教繁瑣、操作人員需要經(jīng)過專門的培訓(xùn)、難以適應(yīng)生產(chǎn)任務(wù)的頻繁變化等諸多問題[2]。目前隨著產(chǎn)品的快速升級換代，制造業(yè)的生產(chǎn)模式從批量化、規(guī)模化、標(biāo)準(zhǔn)化制造轉(zhuǎn)變?yōu)槎ㄖ苹€性化、分布式制造[3]。為了能快速調(diào)整生產(chǎn)線，傳統(tǒng)的機器人路徑規(guī)劃方法不能滿足工業(yè)生產(chǎn)的要求，需要通過機器視覺、人機交互、深度學(xué)習(xí)、虛擬現(xiàn)實等先進技術(shù)來實現(xiàn)更加智能和高效的機器人示教。

因此，本文提出了一種基于機器視覺和人手演示的機器人示教方法。首先，通過使用Microsoft Kinect2．0 傳感器獲得人手演示的初路徑，然后通過PSO 算法對示教路徑進行優(yōu)化，最后把示教信息發(fā)送給機器人控制柜，讓機器人完成示教任務(wù)。

1 基于機器視覺和人手演示的機器人示教方法

目前，工業(yè)機器人的示教再現(xiàn)方法可分為在線示教和離線示教。在線示教[4]是指機器人參與示教過程。因此需要控制機器人以獲得準(zhǔn)確的示教點的位置和姿態(tài)，通常分為編程器示教、力反饋示教和遙控示教。離線示教包括離線編程、虛擬示教和現(xiàn)實離線示教[5-7]。離線示教是指示教過程中不涉及機器人的運動，通過虛擬工作環(huán)境或機器人與工件之間空間位置的標(biāo)定，可以獲得示教點的準(zhǔn)確位置和姿態(tài)。文獻[8]中通過編程器示教的方法對“Unimate”機器人進行示教，在工業(yè)生產(chǎn)中具有廣泛的應(yīng)用。文獻[9]提出了一種機器人示教再現(xiàn)方法，該方法通過安裝在等比例縮放的機械臂模型關(guān)節(jié)處的壓電陶瓷傳感器獲得壓電信號，并進行信號的處理，可以遠程控制真實機械臂的運動。

近年來，機器視覺技術(shù)發(fā)展迅速，已廣泛應(yīng)用于目標(biāo)識別、缺陷檢測、工況監(jiān)視、尺寸測量等領(lǐng)域。機器視覺在機器人示教中的應(yīng)用可以分為2類：基于機器視覺的被動機器人示教和基于機器視覺主動引導(dǎo)的機器人示教。在1965年，視覺傳感器在麻省理工學(xué)院被用于積木空間識別和定位[10]。文獻[11]中基于手柄形式設(shè)計了數(shù)字筆，使用雙目立體視覺技術(shù)來跟蹤和測量手柄的位置和姿態(tài)。但是，由于使用被動標(biāo)記點，系統(tǒng)對環(huán)境光的變化很敏感。文獻[12]使用線性激光器和CCD 相機來構(gòu)建結(jié)構(gòu)光測量系統(tǒng)。通過該系統(tǒng)測量焊縫的位置，引導(dǎo)機器人焊接并實時校正偏差。文獻[13]使用相機來捕捉抓住并用手放置目標(biāo)的過程，在多次重復(fù)捕獲和放置操作之后，由CNN 神經(jīng)網(wǎng)絡(luò)訓(xùn)練捕獲的視頻以識別幾個動作的序列以完成抓握和放置的任務(wù)。然后控制機器人根據(jù)識別的動作步驟重復(fù)捕獲和放置物體。這種示教方法具有一定的智能性，但它也僅限于完成特定任務(wù)的示教操作。

綜上所述，目前的機器人示教系統(tǒng)還存在以下問題：①操作人員需要特殊培訓(xùn)；②示教操作過程復(fù)雜耗時；③示教系統(tǒng)復(fù)雜、成本高、不容易被廣泛使用；④示教系統(tǒng)不夠靈活，無法快速適應(yīng)任務(wù)的變化。

針對上述問題，本文提出了一種基于機器視覺和人手演示的機器人示教方法。本文提出的示教再現(xiàn)系統(tǒng)如圖1所示，首先通過使用Microsoft Kinect2．0傳感器獲得示教過程中人手指尖的位置和姿態(tài)，并通過坐標(biāo)變換將位姿信息轉(zhuǎn)換為機器人基座坐標(biāo)系下的位置和姿態(tài)。然后在PC 電腦上通過粒子群搜索算法（PSO）對示教路徑進行優(yōu)化。最后通過以太網(wǎng)將位姿信息傳遞到機器人控制柜，然后機器人將實現(xiàn)對工件的抓取和放置。

圖1 基于機器視覺的機器人示教系統(tǒng)Fig.1 Robot teaching-playback system based on machine vision

示教系統(tǒng)的實現(xiàn)過程如圖2所示，主要分為5個部分：①手掌跟蹤算法研究；②手掌空間位置和姿態(tài)估計；③食指指尖的位置和姿態(tài)估計；④人手演示路徑的優(yōu)化；⑤示教信息傳輸給機器人。手掌的跟蹤算法是指采用Kinec2．0 傳感器獲取彩色圖像，通過粒子濾波算法跟蹤人手演示示教過程中手掌的ROI 區(qū)域，追蹤人手的運動軌跡[14]。手掌的空間位置和姿態(tài)的估計[15]是指使用被動標(biāo)記點通過P4P算法估計彩色攝像機坐標(biāo)系中ROI 區(qū)域中手掌的位姿參數(shù)。

圖2 示教系統(tǒng)的實現(xiàn)過程Fig.2 Implementation process of the teaching-playback system

2 人手演示的機器人示教路徑的獲取

首先通過手眼標(biāo)定（Eye to Hand）的方法，獲得Microsoft Kinect2．0 傳感器在機器人坐標(biāo)下的坐標(biāo)變化關(guān)系，如式（1）所示。

操作機器人，讓機器人末端夾持器保持水平的姿態(tài)運動到一個確定位置，讀出此時機器人的末端夾持器靠近機器人基座的一側(cè)的夾爪在A 機器人基坐標(biāo)下的齊次變換矩陣，人手運動到機器人的末端，使人手食指指尖與機器人末端夾持器靠近機器人基座的一側(cè)的夾爪對齊，此時人手食指指尖與機器人末端夾持器可近似看做同一位置，即人手食指指尖在機器人坐標(biāo)系下的齊次變換矩陣，此時采用基于Mark 點的人手掌位置和姿態(tài)估計的方法，通過Kinect2．0 讀出手背Mark 點在相機空間中的齊次變換矩陣。由式（2）和式（3）可以得出食指指尖在Mark 點坐標(biāo)下的齊次變換矩陣。

3 基于PSO 的人手演示路徑的優(yōu)化

3.1 PSO 算法原理

粒子群優(yōu)化算法（PSO）是1995年由Eberhart和Kennedy 受鳥群和魚群捕食行為模型啟發(fā)提出的一種全局隨機尋優(yōu)算法[16]。粒子群優(yōu)化算法的基本思想是通過群體中粒子之間的協(xié)作和信息共享來尋求最優(yōu)解。該算法計算量小，編程實現(xiàn)簡單，收斂速度快，設(shè)置參數(shù)少，對非線性問題具有較強的全局搜索能力[17-18]。PSO 解決優(yōu)化問題時，粒子的位置對應(yīng)優(yōu)化問題的潛在解。并且每個粒子都有一個速度，決定飛行的距離和方向，根據(jù)自身的搜索到的歷史最優(yōu)解和種群搜索到的歷史最優(yōu)解調(diào)整自身的速度和方向進行搜索，從而找到全局或局部的最優(yōu)解。

假設(shè)PSO 算法中的每個粒子都沒有重量和大小，在一個D 維的搜索空間中，有M 個粒子組成一個種群，則第i 個粒子在D 維的空間位置為D 維 向 量Xi=｛xi1，xi2，…，xiD｝i=1，…，M，粒子的 飛行 速度表示為D 維向量Vi=｛vi1，vi2，…，viD｝i=1，…，M。通過適應(yīng)度函數(shù)計算出每個粒子的適應(yīng)度值，并記錄第i 個粒子搜尋到的最優(yōu)位置為個體極值為pbest=｛pi1，pi2，…，piD｝i=1，…，M和自身的空間位置為Xi。種群所有粒子找到的最優(yōu)位置為gbest=｛gi1，gi2，…，giD｝i=1，…，M。

通過隨機生成的方法對PSO 算法的原始粒子進行初始化，通過不斷地迭代尋求優(yōu)化問題的全局最優(yōu)解。每次迭代根據(jù)個體極值和全局極值由式（4）來調(diào)整飛行的方向和距離：

由式（5）更新自己在種群空間的位置：

式中：i=1，2，…，M，M 是群體中粒子的個數(shù)；c1，c2是學(xué)習(xí)因子，一般c1+c2≤4；rand（）表示（0，1）范圍內(nèi)的隨機數(shù)。為防止種群粒子速度過大，一般把對粒子的速度限定在一定的范圍［vmin，vmax］，其vmax-vmin范圍一般是種群搜尋空間的15%～25%，并且可以根據(jù)實驗的實際情況進行調(diào)節(jié)。

由于全局版的PSO 算法容易陷入局部最優(yōu)解，Shi Y和Eberthart R 引入慣性權(quán)重粒子w 對全局尋優(yōu)的PSO 算法進行改進，其更新和變異公式如式（6）所示。

式中：全局尋優(yōu)能力隨著慣性權(quán)重粒子w 的增大而增強，局部尋優(yōu)能力則隨著慣性權(quán)重粒子w 的增大而減弱。

3.2 PSO 對示教路徑的優(yōu)化

Microsoft Kinect2．0 傳感器的采樣頻率為30 Hz，在獲得人手演示的示教路徑之后，對示教路徑的示教點位置進行優(yōu)化，進行按照時間先后進行分段，劃分PSO 的搜索空間。隨著劃分空間的段數(shù)的增加，人手演示示教路徑越接近于人手真實的運動軌跡，但相應(yīng)PSO 搜索時間會快速增大。整條示教路徑被分為N 段，每一段空間近似于20 mm*20 mm*20 mm 的立方體。每一段空間搜索出一個示教點，整條示教路徑一共N+1 個點。以搜索出來的N+1 點每相鄰兩點之間的距離之和為目標(biāo)函數(shù)，進行適應(yīng)度值的計算。

本文中PSO 優(yōu)化算法一共50 個粒子，除去示教起點point（x1，y1，z1）和示教終點point（xN+1，yN+1，zN+1），需要搜索出N-1 個示教點point（xi，yi，zi）i=1，2，…，18，假定每個示教點姿態(tài)保持不變，所以每個示教點都只需要搜索x，y，z 3 個變量，每個粒子的維度是3×（N-1）維。每個被搜索的粒子都具有以下四部分信息：

（1）速度V｛x2，y2，z2，x3，y3，z3，…，xN，yN，zN｝；

（2）位置X｛x2，y2，z2，x3，y3，z3，…，xN，yN，zN｝；

（3）適應(yīng)度值fitness，其中適應(yīng)度函數(shù)（目標(biāo)函數(shù)）如式（7）所示：

（4）粒子自身最優(yōu)位置pbest，pbest｛x2，y2，z2，x3，y3，z3，…，xN，yN，zN｝和與自身最佳位置對應(yīng)的適應(yīng)度值pbest，fitness。所有粒子每一代所找到的最優(yōu)位gbest，gbest｛x2，y2，z2，x3，y3，z3，…，xN-1，yN-1，zN-1｝。

4 實驗與分析

如圖3所示，本文實驗平臺由Microsoft Kinect2．0傳感器、ABB IRB120 機器人、PC 電腦、機器人控制柜和Mark 標(biāo)記點五部分組成。

ABB IRB120 是ABB 公司制造的六軸工業(yè)機器人。本實驗中機器人的運動速度為100 mm/s，機器人的運動范圍是580 mm，其他具體參數(shù)見ABB 官網(wǎng)[19]。

圖3 示教系統(tǒng)實驗平臺Fig.3 Experimental platform of the teaching-palyback system

PC 電腦配置如圖4所示，人手演示初路徑的獲取通過Visual Studio2013 編程實現(xiàn)，PSO 的優(yōu)化算法通過MATLAB 編程實現(xiàn)。Microsoft Kinect2．0 傳感器具有RGB 攝像頭、紅外攝像頭和深度攝像頭3個攝像頭，本實驗中只采用RGB 攝像頭，Microsoft Kinect2．0 傳感器與PC 通過USB3．0 連接，機器人控制柜和PC 電腦之間通過以太網(wǎng)進行通信。PSO 對人手演示獲取的示教路徑優(yōu)化流程如圖4所示。

圖4 PSO 算法流程Fig.4 Flow chart of PSO algorithm

在進行PSO 算法優(yōu)化示教初路徑時，綜合考慮本次實驗的機器人運動空間、人手演示的時間以及PSO 優(yōu)化需要搜索的時間，整條示教路徑被分為19段，即N=19，所以每個粒子維度為3×（N-1）＝54。PSO 通過隨機數(shù)對粒子的位置和速度進行初始化。此外，加速常數(shù)以c1=c2=1．8。慣性權(quán)重粒子w=0．73，經(jīng)過實驗驗證，迭代次數(shù)100 次時，優(yōu)化結(jié)果達到最優(yōu)，隨著迭代次數(shù)的再次增大，優(yōu)化結(jié)果優(yōu)化效果基本不變，所以迭代終止條件T=100。

本文做了3 組不同的示教實驗，在每組實驗中人手演示10 次。如圖5（a）所示，在實驗I 中，人手演示避開一個長方體障礙物到達目標(biāo)點。如圖5（b）所示，在實驗II 中，人手演示避開2 個長方體障礙物到達目標(biāo)點。如圖5（c）所示在實驗III 中，人手演示先繞開一個正方體障礙物，再跨過一個長方體障礙物到達目標(biāo)點。

圖5 人手演示的路徑Fig.5 Human hand demo path

如圖6所示，由于人手演示時，會有抖動，示教原始路徑有突變和毛刺，所以需要對其進行一定的濾波處理，本文采用中值濾波對Microsoft Kinect2．0傳感器得到的原始軌跡進行平滑處理。

圖6 中值濾波優(yōu)化前后示教路徑Fig.6 Teaching-playback path before and after median filter optimization

經(jīng)過中值濾波后的人手演示路徑如圖7（a）、7（b）和7（c）中曲線所示，與人手的實際運動規(guī)跡基本一致，與后續(xù)PSO 優(yōu)化結(jié)果做對比。

圖7 中值濾波優(yōu)化路徑和PSO 優(yōu)化路徑Fig.7 Path optimized by median filter and the path optimized by PSO

在實驗I、實驗II和實驗III 中，每組實驗人手分別演示10 次，機器人按照中值濾波路徑運動10次，按照PSO 優(yōu)化路徑運動10 次，機器人運動時間平均值和標(biāo)準(zhǔn)誤差如圖8所示。

從圖8 可以看出，在實驗I 中機器人按照PSO優(yōu)化路徑的運動時間比中值濾波優(yōu)化路徑運動的時間縮短12．17%。在實驗II 中機器人按照PSO 優(yōu)化路徑運動的時間比中值濾波優(yōu)化路徑運動時間縮短18．64%。在實驗III 中機器人按照PSO 優(yōu)化路徑運動時間比中值濾波優(yōu)化路徑時間縮短16．07%。

圖8 機器人實際運動時間Fig.8 Robot actual motion time

與實驗II和實驗III 進行對比，在實驗I 中，人手演示的路徑較為簡單。通過中值濾波處理的示教路徑和經(jīng)過PSO 處理的示教路徑都比較接近人手的真實運動軌跡，所以PSO 優(yōu)化方法與中值濾波的方法優(yōu)化結(jié)果相比，采用PSO 的優(yōu)化方法對機器人運動時間的縮短相對較小。

在實驗II 中，人手演示的路徑需要避開2 個長方體障礙物，示教的路徑比實驗I 人手演示的路徑復(fù)雜。在PSO 的搜索空間中進行PSO 優(yōu)化搜索時，以機器人的運動距離為目標(biāo)函數(shù)進行優(yōu)化。從圖7（a）可以看出，在示教路徑（軌跡）的波峰處PSO 優(yōu)化效果明顯，機器人運動的距離減小，從而縮短機器人運動的時間。

在實驗III 中，示教的路徑人手演示時先繞開1個正方體障礙物，再跨過1 個長方體障礙物，示教路徑比實驗I和實驗II 更加復(fù)雜。從圖7（c）可以看出，在示教路徑（軌跡）的波峰處，PSO 的優(yōu)化效果明顯，機器人運動的距離減小，從而縮短機器人運動的時間。

圖9所示為機器人在實驗II 的場景中按照PSO 優(yōu)化后路徑進行運動。實驗結(jié)果表明，機器人能夠按照優(yōu)化后的示教路徑進行運動，并且能夠避開障礙物實現(xiàn)對圓柱工件的抓取和放置。

綜上所述，經(jīng)過PSO 算法對人手示教路徑的優(yōu)化，在人手演示完成后，能得到一條優(yōu)化的機器人示教再現(xiàn)的路徑，縮短機器人運動的時間，讓機器人完成對工件的快速抓取和放置。

圖9 PSO 優(yōu)化后的機器人運動情況Fig.9 Robot motion according to the path optimized by PSO

5 結(jié)語

本文提出一種基于機器視覺和PSO的機器人示教優(yōu)化方法，實驗結(jié)果表明，本文提出的方法能找到一條比人手演示原始路徑更短的路徑，讓機器人避開障礙物并實現(xiàn)對工件的抓取和放置，提高示教和工業(yè)生產(chǎn)的效率。后續(xù)會進一步改進算法，針對更加復(fù)雜路徑進行優(yōu)化。