呂亞輝，么 嬈，嚴雨靈

(上海工程技術大學 航空運輸學院，上海 201620)

0 引言

隨著機器人技術與人機交互技術的發(fā)展，將手勢識別應用于工業(yè)機器人的路徑規(guī)劃已經(jīng)可以實現(xiàn)。傳統(tǒng)機器人示教的過程復雜、操作繁瑣對操作人員的要求較高，基于自然手勢交互獲得機器人軌跡具有簡單、學習成本低使用方便的優(yōu)點，并可以降低對操作人員的要求。如陳暢等[1]使用Leap Motion傳感器獲取手勢坐標動態(tài)，以使機器人運動，實現(xiàn)移動、抓取和放置等動作。

而初始手勢軌跡路徑離散性較大，所得軌跡具有運行時間慢且沖擊較大，為提高工業(yè)機器人的工作效率和穩(wěn)定性，可以對基于手勢所得運行軌跡進行時間-沖擊最優(yōu)軌跡規(guī)劃。付榮等[2]使用遺傳算法進行時間最優(yōu)軌跡規(guī)劃，使用兩個適應度函數(shù)尋找最優(yōu)3-5-3插值多項式，得到了時間最優(yōu)的軌跡。但是該方法并未考慮沖擊，其結果中可以發(fā)現(xiàn)加加速度有很大突變，具有較大的沖擊，不僅對機器人的工作精度有影響，還會縮短使用壽命[3]。為了解決這種問題，文獻[4]中進行最優(yōu)沖擊軌跡規(guī)劃，采用S型曲線對B樣條進行插補，沖擊明顯減小。除此之外為了在要求運動穩(wěn)定性的前提下尋找最優(yōu)時間軌跡。文獻[5]研究了時間-沖擊最優(yōu)軌跡規(guī)劃，首先通過最優(yōu)時間軌跡規(guī)劃得到最短的運行時間，并在最短的時間內(nèi)用自適應遺傳算法搜索最優(yōu)沖擊軌跡，該方法在保證速度的情況下減小了沖擊。Lin[6]使用PSO算法和類聚合算法在關節(jié)空間進行規(guī)劃，通過優(yōu)化軌跡中所安插的節(jié)點求得最優(yōu)沖擊軌跡。尤田等[7]提出使用GA-PSO算法對機器人的焊接路徑規(guī)劃，證明了其較強的搜索能力。

針對手勢軌跡問題，本文提出使用改進GA-PSO算法進行時間-沖擊最優(yōu)軌跡規(guī)劃，改進手勢軌跡，以更好地適用于工業(yè)機器人，算法采用自適應的慣性權重和交叉變異概率，相比于GA-PSO算法收斂速度更快。使用5-5-5插值多項式對保證機器人關節(jié)空間的速度、加速度、加加速度的連續(xù)，并于Epson機器人上進行驗證，機器人軌跡在不降低時間的情況下沖擊大幅減少。

1 手勢獲取坐標

本文使用Leap Motion(圖1)進行手部運動捕捉，獲取手和手指的位置手勢和動作。本文取長100mm、寬為80mm的Z型線，以食指沿下半部分反Z型為軌跡，其軌跡信息如圖1所示(由于設備為自下而上獲取坐標，坐標信息為鏡面反射關系)，其與獲取坐標信息消除z軸如圖2所示。

圖1 Leap Motion手勢提取設備

圖2 提取手勢坐標信息

由于Leap Motion的內(nèi)置坐標系與機器人不同，故本文基于手勢坐標的相對坐標確定機器人軌跡，以機器人原點作為手勢第一點坐標出發(fā)進行相對運動，由下式1分別獲取機器人x和y方向上坐標的相對位移。

(1)

其中，Xi、Yi為修改前坐標，Xt、Yt為修改后坐標，L和W分別為模板的長和寬，根據(jù)坐標范圍比例以獲得機器人坐標。

以EPsonC3機器人為模型，使用機器人工具箱仿真(相對于原模型10：1縮小)，將修改后坐標進行逆解可得連續(xù)坐標的六軸角度，可驅(qū)動機器人模型以手勢軌跡運動，其結果路徑如圖3所示。

圖3 機器人工具箱仿真

為使手勢軌跡能夠應用實際生產(chǎn)，需消除硬件設備的不穩(wěn)定性和操作者的不連續(xù)性，一般情況下，可以對帶有噪聲的信息進行濾波[8]過濾產(chǎn)生誤差的幀，以保證手勢控制的精度。而隨著機器人視覺的發(fā)展，通過機器人所獲圖像處理反饋到伺服控制器修改原有軌跡[9-10]可以完成復雜軌跡的再現(xiàn)。手勢坐標再歸一化后所獲取坐標可直接應用于機器人運行，可以獲得機器人沿手勢所走路線的機器人運行軌跡。

對于工業(yè)生產(chǎn)中，如圖2類似的生產(chǎn)任務需要高頻率與速度，若按照未經(jīng)優(yōu)化的軌跡會產(chǎn)生振動和磨損，本文使用手勢識別獲取坐標，并根據(jù)生產(chǎn)需要選取關鍵點，對軌跡中關鍵點進行時間-沖擊最優(yōu)軌跡規(guī)劃，最終獲得可以通過關鍵點的高速低沖擊的軌跡。本文軌跡以取起點、終點和兩處拐點四點關鍵點為例進行軌跡規(guī)劃，獲取基于手勢軌跡的時間-沖擊最優(yōu)軌跡。

2 最優(yōu)軌跡規(guī)劃

自然手勢識別可以應用于機器人的自動化編程中，除上節(jié)完全應用手勢路徑外，有時還需要對于工業(yè)機器人點對點軌跡規(guī)劃中，基于手勢規(guī)劃機器人關鍵路徑點，并對其進行最優(yōu)軌跡規(guī)劃。在無特定路徑要求下，通常根據(jù)始末位置進行插補算法以得到軌跡[11]。

2.1 插值函數(shù)的構造

在考慮時間與沖擊最優(yōu)情況下，5次多項式可以通過多階導數(shù)獲得曲線的速度、加速度、加加速度，為保證4處關鍵點的3段運行時間段連接處的沖擊連續(xù)，本文采用5-5-5插值多項式曲線，并使用智能算法搜索滿足沖擊約束的多項式系數(shù)。付榮等[2]使用3-5-3多項式插值曲線對時間最優(yōu)軌跡規(guī)劃，但其兩段時間相連接處的沖擊未考慮在內(nèi)，本文構造5-5-5插值多項式，其中5次多項式可以保證軌跡連接處的加速度連續(xù)，其多項式通式為：

(2)

(3)

(4)

其中，Xij為第i個軸的第j段的角度，在點對點的軌跡規(guī)劃中起點和終點的角度、角速度、角加速度已知，且路徑點Xi20、Xi30已知，則可根據(jù)時間t1、t2、t3獲得多項式系數(shù)，并由角度位移的5次多項式求導計算該段時間內(nèi)的速度、加速度和加加速度。

根據(jù)每段時間段的對應關系則可有下列矩陣與系數(shù)a的關系式，如式(5)～式(7)。其中a為5次多項式系數(shù)，r0、r1、r2、r3分別為該軸起點、第一點、第二點、終點的角度，由于矩陣的特性要求每步運算時需要考慮矩陣A的可逆性。

(5)

b=[0 0 0 0 0 0 0 0r3 0 0 0r0 0 0 0
r2r1]

(6)

A*a=b

(7)

2.2 GA-PSO算法

粒子群優(yōu)化算法是一種基于群體智能的全局進化優(yōu)化算法，模擬鳥群覓食行為，種群個體之間通過相互協(xié)作來尋求最優(yōu)解，每個優(yōu)化問題的潛在解都是搜索空間中的一個粒子。每個粒子具有位置和速度兩個特征，粒子的位置坐標對應的目標函數(shù)值即可作為該粒子的適應度，算法通過適應度來衡量粒子的優(yōu)劣。在每一次迭代中，粒子通過跟蹤兩個“極值”來更新自己。第一個極值就是粒子本身所找到的最優(yōu)解，叫做個體極值pbest；另一個極值是整個種群目前找到的最優(yōu)解，這個極值是全局極值gbest。在找到這兩個最優(yōu)值時，粒子根據(jù)如下公式更新自己的速度和位置：

vt+1=W*vt+C1*rand*(pbest-xt)+C2*rand*(gbest-xt)

(8)

xt+1=xt+vt+1

(9)

其中，W為慣性權重，是上次粒子速度對此次粒子速度的影響程度，Kennedy提出作為收縮因子，使其隨迭代減小粒子震蕩，從而加快收斂速度。對于慣性權重，本文采用隨迭代次數(shù)而自適應變化的時變權重：

(10)

其中，i為當前迭代次數(shù)，T為總迭代次數(shù)，當k>0時，其次數(shù)比的平方為由0～1的凹曲線，即慣性權重W為由1～0.3的凸曲線，則慣性權重在迭代前期受上次粒子速度影響較大而在迭代后期影響程度迅速減小，可一定程度防止粒子群過早收斂，且不影響迭代后期的搜索精度。

為尋找最優(yōu)時間-沖擊軌跡規(guī)劃，Gasparetto等[12]給予每個優(yōu)化目標相應的權重，將其轉(zhuǎn)化為單目，通過調(diào)整時間、沖擊兩個優(yōu)化目標的權重，獲得了保證沖擊較小的同時保證了速度的提高，運動時間有較大縮短。文獻[13]中將時間、加速度和沖擊作為對象進行混合最優(yōu)軌跡規(guī)劃。本文對粒子的加加速度與最大加加速度約束的差值加權，并與總時間求和作為適應度函數(shù)，如式(11)所示。

f1=Wt(t1+t2+t3)+Wv*|Vmax-maxVi|+Wa*|amax-maxa|+Wj*|jmax-maxj|

(11)

而當該次迭代的全局最優(yōu)解不滿足約束，則需對該單維時間段進行修正調(diào)整，以滿足約束條件的最短時間為全局最優(yōu)解進入下次迭代，其修正函數(shù)如式(12)所示。

f2=Wt(*(t1+t2+t3)+Wv*|maxVi|+Wa*|maxa|+Wj*|maxj|

(12)

粒子群算法搜索速度快、精度高，但易陷入局部最優(yōu)解，遺傳算法使用選擇、交叉、變異操作產(chǎn)生子代，在加入粒子群算法后，粒子群可以很好的保留粒子的多樣性，有效避免陷入局部最優(yōu)解。本文在粒子群優(yōu)化算法后嵌入遺傳算法交叉、變異操作保留粒子群的多樣性。為了不影響迭代前期粒子群優(yōu)化算法的尋優(yōu)性能，采用隨迭代次數(shù)增加而增加的自適應交叉變異概率。

(13)

式中，p為交叉或變異概率，在實驗過程中粒子群算法一般在三分之一迭代處迅速趨于最優(yōu)，故在此之前取較小的交叉、變異概率，并在之后逐步增加至所設定的最大交叉、變異概率。

具體算法流程如下：

(1) 初始化粒子；設定粒子數(shù)和其維數(shù)，粒子以3段時間t組成三維粒子；

(2)計算粒子的適應度；根據(jù)粒子的3段時間帶入式(4)～式(6)可求出系數(shù)向量a，并由此帶入式(1)～式(3)分別求出3段時間內(nèi)的最大速度、最大加速度、最大加加速度，以式(11)為適應度函數(shù)求取個體極值pbest和全局極值gbest；

(3)判斷全局最優(yōu)解是否超出最大約束，若未超出最大約束，則以式(11)為適應度函數(shù)對3段時間以式(7)～式(8)進行迭代，求得新的粒子，并對新的粒子進行交叉、變異操作，并對所得結果與原粒子群所得結果的適應度值進行比較，選取適應度值較低的為當前迭代所得粒子群；若超出最大約束，則對超出的該單維時間段進行以式(12)為適應度函數(shù)迭代選取新的滿足約束的全局最優(yōu)解；

(4)判斷是否達到終止條件，若否，則返回(2)；

(5)以滿足pipper準則的機器人為例，前3軸可確定機械手末端位置，則根據(jù)路徑規(guī)劃是否有姿態(tài)變化選擇6軸角度或3軸角度優(yōu)化，并分別對所需求解的6個軸或3個軸求取最優(yōu)解，并選取每段時間的最大值作為最終最優(yōu)時間，以最終3段時間帶入各軸軌跡點要求內(nèi)，求得最優(yōu)軌跡。

3 仿真實驗

本文以EpsonC4機器人為對象仿真驗證，以該機器人運動范圍與約束為基礎，尋找滿足約束的最優(yōu)時間-沖擊軌跡，具體約束如表1所示。

表1 機器人約束范圍

選取第一節(jié)中的軌跡拐點為例，即取坐標(241,-37,0)，(385,-34,0)，(200,38,0)，(400,36,0)。以第一點為起點，以最后一點為終點，其姿態(tài)坐標保持不變，對6軸關節(jié)角度尋優(yōu)，以圖2中機器人模型姿態(tài)保持4點姿態(tài)不變，可逆運動學求解得4點位姿狀態(tài)下的6軸關節(jié)角度如表2所示，其中第4軸為主要控制Z軸旋轉(zhuǎn)，該規(guī)矩未涉及Z軸旋轉(zhuǎn)，故為0。

表2 關節(jié)空間坐標點

分別使用PSO算法、GA-PSO算法和改進GA-PSO算法對對該問題求解，均設置30個粒子，其中PSO算法設置慣性權重W=0.7；GA-PSO算法使用固定的慣性權重0.7、且其交叉變異概率不變，分別為0.7和0.5；改進GA-PSO算法，設置30個粒子，前期交叉變異概率使用0.1，滿足條件后，增大概率，交叉概率最大為0.7，變異概率最大為0.5，并以式(9)設置慣性權重，其中設置k=1.5，在迭代次數(shù)為30的情況下其搜索第一軸最優(yōu)結果如圖4所示。

圖4 迭代收斂效果

如圖4所示，其中PSO算法所得機器人運行3段總時間的最優(yōu)時間為1.6447s；GA-PSO算法所得最優(yōu)時間為1.6181s；改進GA-PSO算法所得最優(yōu)時間為1.5481s。由上圖可知，PSO算法搜索速度快但單純PSO算法不能搜索到全局最優(yōu)值，易陷入局部最優(yōu)，GA-PSO算法在PSO算法的基礎上增加后期粒子的種群多樣性，但前期尋優(yōu)能力不如改進GA-PSO算法，由于后者在迭代前期減小交叉變異概率，充分發(fā)揮粒子群的速度優(yōu)勢，而在后期增加交叉變異概率，增加了種群多樣性，使之擁有較好的尋優(yōu)性能。

將改進GA-PSO算法應用于機器人6軸關節(jié)上，以速度為πrad/s、加速度為10 rad/s2、加加速度為50 rad/s3為約束，時間與沖擊的權重比為Wt:Wv=3:7，對6軸3段時間進行尋優(yōu)，分別求得6軸3段最優(yōu)時間，算法尋優(yōu)迭代后最終結果如圖5所示，其中角度以弧度為單位表示。

(a) 前3軸角度、速度、加速度、加加速度最優(yōu)曲線

(b) 后3軸角度、速度、加速度、加加速度最優(yōu)曲線 圖5 約束下最優(yōu)時6軸曲線

如圖5所示6軸速度、加速度、加加速度均在約束內(nèi)，每段時間選取6軸中最長時間，獲得3段時間：0.8395s、0.7017s、1.1492s，總時間2.6903s。其中第4軸角度未變化，第3個關鍵點處加加速度有突變，但大小均未超過40rad/s3，沖擊較小。

將所得3段最優(yōu)時間帶入式(5)～式(7)，求得6軸最終軌跡，其機器手末端軌跡路線如圖6所示，其中圖6a為原手勢軌跡機器人末端空間路線，路線處于同一平面內(nèi)；圖6b為沖擊優(yōu)化后的軌跡，為空間曲線，Z軸存在5mm內(nèi)的偏移，將曲線映射到機器人，鏡面反射重新恢復Z型，并將空間曲線的路徑在EPSON RC軟件C3 Sample中仿真，結果如圖6c所示。

(a) 原手勢軌跡 (b) 沖擊優(yōu)化軌跡

(c) 仿真中的路徑點 圖6 機器手末端軌跡

由圖6結果可知，在第3點處，犧牲部分沖擊以節(jié)約時間，在關節(jié)空間中，6個軸的速度、加速度、加加速度在約束范圍內(nèi)得到2.69s的經(jīng)過4處關鍵點的優(yōu)化后的軌跡結果，結果表明所得軌跡平滑，不超過沖擊約束。增加關鍵點可以增加與原軌跡匹配度但沖擊優(yōu)化效果降低，本文以4關鍵點為例，關鍵點數(shù)量應隨實際情況而定。

4 結論

本文使用Leap Motion獲取手勢軌跡信息，除原軌跡運行外研究了對原軌跡選取關鍵點進行優(yōu)化，并獲取時間-沖擊優(yōu)化軌跡的方法。其中遺傳粒子群算法結合粒子群的搜索速度和遺傳算法解決離散問題的優(yōu)點，以較快的速度收斂于全局最優(yōu)解，本文在此基礎上引入自適應慣性權重和交叉變異概率，在根據(jù)迭代次數(shù)與適應度值改變參數(shù)，應用于機器人時間-沖擊最優(yōu)軌跡規(guī)劃中，獲得了比一般GA-PSO算法更快的收斂速度和更短的約束內(nèi)總時間。