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

王亞超，黃沿江，張憲民

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

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

但是目前的機(jī)器人示教方法存在示教系統(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)槎ㄖ苹?、個(gè)性化、分布式制造[3]。為了能快速調(diào)整生產(chǎn)線，傳統(tǒng)的機(jī)器人路徑規(guī)劃方法不能滿足工業(yè)生產(chǎn)的要求，需要通過機(jī)器視覺、人機(jī)交互、深度學(xué)習(xí)、虛擬現(xiàn)實(shí)等先進(jìn)技術(shù)來實(shí)現(xiàn)更加智能和高效的機(jī)器人示教。

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

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

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

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

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

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

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

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

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

2 人手演示的機(jī)器人示教路徑的獲取

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

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

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

3.1 PSO 算法原理

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

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

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

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

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

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

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

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

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

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

（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 實(shí)驗(yàn)與分析

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

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

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

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

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

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

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

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

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

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

經(jīng)過中值濾波后的人手演示路徑如圖7（a）、7（b）和7（c）中曲線所示，與人手的實(shí)際運(yùn)動(dòng)規(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

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

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

圖8 機(jī)器人實(shí)際運(yùn)動(dòng)時(shí)間Fig.8 Robot actual motion time

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

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

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

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

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

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

5 結(jié)語

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