基于機器視覺和PSO的機器人示教路徑優化研究

王亞超，黃沿江，張憲民

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

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

但是目前的機器人示教方法存在示教系統復雜、示教繁瑣、操作人員需要經過專門的培訓、難以適應生產任務的頻繁變化等諸多問題[2]。目前隨著產品的快速升級換代，制造業的生產模式從批量化、規模化、標準化制造轉變為定制化、個性化、分布式制造[3]。為了能快速調整生產線，傳統的機器人路徑規劃方法不能滿足工業生產的要求，需要通過機器視覺、人機交互、深度學習、虛擬現實等先進技術來實現更加智能和高效的機器人示教。

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

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

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

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

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

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

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

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

圖2 示教系統的實現過程Fig.2 Implementation process of the teaching-playback system

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

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

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

3 基于PSO 的人手演示路徑的優化

3.1 PSO 算法原理

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

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

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

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

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

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

式中：全局尋優能力隨著慣性權重粒子w 的增大而增強，局部尋優能力則隨著慣性權重粒子w 的增大而減弱。

3.2 PSO 對示教路徑的優化

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

本文中PSO 優化算法一共50 個粒子，除去示教起點point（x1，y1，z1）和示教終點point（xN+1，yN+1，zN+1），需要搜索出N-1 個示教點point（xi，yi，zi）i=1，2，…，18，假定每個示教點姿態保持不變，所以每個示教點都只需要搜索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）適應度值fitness，其中適應度函數（目標函數）如式（7）所示：

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

4 實驗與分析

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

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

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

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

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

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

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

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

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

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

經過中值濾波后的人手演示路徑如圖7（a）、7（b）和7（c）中曲線所示，與人手的實際運動規跡基本一致，與后續PSO 優化結果做對比。

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

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

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

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

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

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

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

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

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

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

5 結語

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