一種基于DTW-GMM 的機器人多機械臂多任務協同策略

劉成菊 林立民 劉 明 陳啟軍

相比單臂機器人,多臂機器人可以通過協作完 成一些更加復雜的動作,實現各種靈巧操作任務,如搬運、焊接和裝配等.多臂機器人各個機械臂的工作空間一般存在著相互重疊的區域,即它們之間的協作工作區域,在這個區域內各個機械臂可以完成協作任務[1-3].然而,多臂機器人如何在這個工作區域協調好多個任務一直是研究熱點和難點.

Lim 等[4]研究了雙臂機器人的抓取,通過對雙臂的軌跡進行提前規劃,并由微分變換算出手臂的移動增量,成功實現了對水平放置的圓柱體的抓取和搬移;Ortenzi 等[5]實現了保持末端相對位置不變的雙機械臂軌跡規劃策略,先對主機械臂的軌跡進行規劃,然后通過建立的運動約束方程來生成從機械臂的軌跡;Ramirez-Alpizar 等[6]利用Leap Motion 運動捕捉器采集人類手臂執行零件裝配任務時的運動,然后提取裝配任務時軌跡關鍵點的手指所處的位置和姿態,用于控制Baxter 機器人生成裝配任務的軌跡;Tuan 等[7]提出了一種基于姿態估計的滑模控制策略,設計了一種具有魯棒性的自適應控制器,用于雙臂機器人的3D 協作運動,但是該算法作者只將其運用到單臂自由度為2 的雙臂機器人上.這些多臂協作控制策略提前規劃機器人的執行軌跡,實時性有待提高,往往也需要建立復雜的機器人動力學模型.近年來,研究學者希望通過示教學習的策略,讓機器人學習人類如何執行靈巧操作的任務.動態運動基元 (Dynamic movement primitive,DMP)模型[8-12]和高斯混合模型 (Gaussian mixture model,GMM)[13-19]是典型的示教學習算法.

DMP 模型由動作變化系統和標準時間系統構成[8].該模型通過對機器人的示教軌跡進行擬合,提取示教軌跡的形狀特征,并通過設定期望的軌跡終點和時長生成具有泛化性的機器人軌跡.DMP 的優點在于可以控制生成軌跡的時長和幅度,但是這種示教學習模型只能應用于軌跡終點速度為0 的場景.Kober 等[9]對原始DMP 模型進行了改進,將其應用于終點速度不為0 的場景,但是如果軌跡稍有差異,都需要重新進行模型的設計,即基于DMP的學習算法只能實現單條示教軌跡的學習.如果示教軌跡存在噪聲,泛化得到的輸出軌跡效果非常不理想.GMM 示教學習模型利用高斯混和模型來對機器人軌跡進行建模,用概率的方法來提取軌跡之間的相關關系,從而對示教軌跡進行表征;然后利用高斯混合回歸可以實現軌跡的泛化輸出.基于GMM 的軌跡學習模型能夠很好地保持示教軌跡的空間形狀特征,但是一旦示教軌跡存在時長上的差異,GMM 模型的軌跡學習效果不佳.

針對DMP 和GMM 軌跡學習算法各自存在的問題,本文提出基于動態時間規整-高斯混合模型(Dynamic time warping-Gaussian mixture model,DTW-GMM)的多機械臂協同軌跡生成方法.論文主要內容分為三部分: 1)針對機器人示教軌跡往往存在時間長短差異較大的問題,采用DTW 算法來規整機器人的示教軌跡.文獻[20]利用DTW 方法來規整人類不同語速的發音,消除語音的發音時間長短不一對語音識別的影響.本文將DTW 核心思想進行遷移,用其來規整機器人的示教軌跡.2)基于DTW 所規整的示教軌跡,利用GMM 概率模型來學習示教軌跡的共同特征,并設計多機械臂多任務協同的軌跡生成策略,利用高斯混合回歸(Gaussian mixture regression,GMR)泛化輸出各機械臂的執行軌跡.3)設計機器人的運動控制引擎并利用Pepper 機器人平臺,完成手臂協作搬運和漢字軌跡書寫實驗,驗證本文提出的多機械臂協同控制策略的可行性和有效性.

1 基于DTW-GMM 的示教軌跡特征提取

1.1 動態時間規整算法

動態時間規整(Dynamic time warping,DTW)算法可以用來規整機器人的示教軌跡.該方法主要的核心思想在于,將兩個長度不一的時間信號通過對齊拉伸到相同長度,且拉伸后使得兩條軌跡之間的距離最短.DTW算法可以由如下目標方程描述[21]:

式中,Wx=[aij](1≤i ≤nx,1≤j ≤n,aij ∈{0,1})和Wy=[aij](1≤i ≤ny,1≤j ≤n,aij ∈{0,1}) 表示二值時間規整矩陣,通過將時間信號X和Y中的相關向量進行拉伸,最終將兩個軌跡矩陣規整到相同長度.

時間規整矩陣必須同時滿足以下三個約束條件:

1)X和Y的初始和終止向量點必須對齊;

2)規整矩陣本質上表示的是時間序列,因此規整矩陣中取值為1 的點所依次連成的路徑(即規整路徑)必須是單調的;

3) 由于信號需要保持有序性,因此矩陣X和Y中對應向量的前后順序必須保持不變.

求解目標方程(1)中的時間規整矩陣可以使用動態規劃(Dynamical programming,DP)算法或廣義典型時間規整(Generalized canonical time warping,GCTW)算法[21-22],兩種求解算法的時間復雜度的對比如表1 所示,其中l用于設置GCTW算法規整后軌跡信號的長度,原則上選取l=1.2×max(nx,ny),m表示單調增函數基的個數,一般m的取值為5 左右.GCTW 比DP 算法具有較低的時間復雜度,后文采用GCTW 算法進行多機械臂示教軌跡的規整策略.

表1 算法的時間復雜度Table 1 Time complexity of algorithms

圖1 表示兩個示教軌跡序列在DTW 算法規整下的結果.圖1(b)所示的規整路徑將圖1(a)原始序列規整成圖1(c).由此可見,DTW 算法可以很好地改善原有示教軌跡存在的時間差異較大的問題,從而統一時間的變化.

1.2 GMM 軌跡特征提取

經DTW 規整后的示教軌跡,利用GMM 概率模型[23-25]進行表征,從而提取示教軌跡的共同特征.對于多機械臂示教數據,本文將其表示為:

式中,yi,s,yi,t分別表示示教軌跡的空間信息和時間信息,T表示示教軌跡中示教點的個數.

對于多維示教變量y=(y1,y2,···,yT),建模GMM 為:

式中,p(y)表示概率密度函數,K表示高斯分布的個數,πk表 示第k個高斯分布所占的權重,N(y;μk,Σk)表示以μk為均值,Σk為協方差矩陣的高斯概率密度函數,D代表示教軌跡的維度.

相比于高斯分布的參數估計,混合高斯分布的參數估計更加復雜.主要原因在于隱變量的存在.對于示教樣本集y=(y1,y2,···,yT),通過隱變量γt,k的引入,即表示yt這個樣本由第k個模型生成,可以將數據展開成完全數據:

若yt由 第1類采樣而來,則有γt,1=1,γt,2=0,···,γt,K=0,表示為(yt,1,0,···,0).

故完全數據的似然函數為:

可以利用期望值最大(Expectation maximum,EM) 算法進行迭代計算,求取GMM模型的參數πk,μk,Σk.先定義Q 函數如下:

式中,μi,Σi,πi分別表示第i次迭代時GMM 模型的各個高斯分布模型的均值,協方差矩陣和權重集合;表示對γ的估計:

對Q 函數進行求導,并令其導數為0,可得:

為了使得EM 算法能夠更快地收斂,本文將經由DTW 規整后的多機械臂示教軌跡利用K 均值(K-means)聚類算法進行聚類,劃分數據的所屬類別,進行期望值最大算法的參數初始化.同時利用赤池信息準則[26](Akaike information criterion,AIC),通過最優化AIC 指標函數對K-means 的K值進行選取:

式中,C(K)表示GMM 概率模型參數數目;D表示示教數據集的維度;l nL(y)表示初始參數下示教樣本的對數似然函數.采用DTW-GMM 算法對機器人的多機械臂示教軌跡進行特征提取的步驟如算法1 所示.

算法1.基于DTW-GMM 算法的多機械臂示教軌跡特征提取

2 基于GMR 的軌跡泛化輸出

圖2 是基于本文提出的算法生成的二維雙臂協同軌跡泛化輸出.圖2(a)為雙臂協作的示教軌跡,左、右機械臂分別負責 “U”和 “Z”字形的任務執行軌跡.圖2(b)則為基于DTW-GMM 算法的軌跡表征圖,此處經由AIC 準則確定的K取值為3.圖2(c)對應以左手臂空間矢量作為查詢向量下,右手臂的泛化輸出圖.由圖2 可以看出,本文算法可以對多機械臂的示教軌跡進行表征,并在GMR 回歸下生成具有一定泛化性的機械臂執行軌跡,用于完成多臂協作任務.

圖2 雙臂協同軌跡泛化輸出Fig.2 Generalized dual-arm collaborative trajectory output

對機器人進行示教時,軌跡的噪聲時時存在.圖3 探討了本文采用的GMR 泛化輸出策略對示教噪聲的抗干擾性.圖3(a)為單條示教軌跡時,漢字軌跡 “打”存在框選處的噪聲時,軌跡的表征輸出結果.而圖3(b)對應3 條示教軌跡時,GMR 的輸出情況.由圖3 可以看出,本文的軌跡泛化策略具有一定的抗干擾性,即使存在噪聲,算法依舊可以充分提取示教軌跡的共同特征.

3 總體系統架構

為了將本文提出的多機械臂多任務協同策略運用到機器人控制中,設計了如圖4 所示的控制系統架構[27].其中多機械臂協同軌跡生成器主要負責多機械臂示教軌跡的特征學習與協同軌跡輸出.其通過傳感器反饋的姿態信息為查詢向量,實時輸出機械臂的協同軌跡.而運動引擎主要負責對多機械臂協同輸出的軌跡進行跟蹤控制.

圖4 總體系統架構設計Fig.4 The system architecture block diagram

3.1 多機械臂協同軌跡生成器

多機械臂軌跡生成原器的原理框圖如圖5 所示.首先,對多機械臂進行軌跡示教 然后利用DTWGMM 算法對示教軌跡進行特征提取.其中EM 算法用于GMM 概率模型參數的學習,而K-means算法對經由DTW 算法規整后的示教軌跡進行聚類學習,獲得EM 算法迭代的初始參數μ0、Σ0、π0.與參數隨機初始化相比,K-means 可以加快EM 算法的收斂.而K-means算法中K值的選定由AIC指標函數確定,防止模型的過擬合.為了滿足機械臂軌跡生成的實時性要求,多機械臂協同軌跡生成器需要進行預訓練,否則軌跡的規整和參數的學習將耗費較多的時間.預訓練之后的軌跡生成器實時接收查詢向量的輸入,利用GMR 在線輸出各個機械臂的協同軌跡.

圖5 多機械臂協同軌跡生成器框圖Fig.5 Multi-arm collaborative trajectory generator block diagram

3.2 運動引擎

在機器人運動學中,關節空間與工作空間的對應關系可以描述為:

式中,T=[T1,T2,···,TN] 表示機器人末端的位置和姿態;θ=[θ1,θ2,···,θM] 中的θi表示第i個連桿相對于第i -1 個連桿的旋轉角度或者位移.

本文采用分解速度控制法[28](Resolved motion rate control,RMRC)來控制機器人的關節角速度.機器人的微分運動學和逆微分運動學方程可以表示為:

式中,J(θ)、J-1(θ)分別表示雅克比矩陣和其逆矩陣.

當機器人系統冗余時,即M＞N時,雅可比矩陣可能存在非滿秩的狀態.此時,可以用雅可比矩陣的偽逆矩陣代替其逆矩陣[29]:

為了防止奇異性問題的出現,通過引入阻尼系數λ來保證偽逆矩陣有意義:

由此,設計基于微分逆運動學的機器人運動控制引擎如圖6 所示.其中,Xref表示多機械臂協同學習系統輸出的參考軌跡,J表示雅可比矩陣,qreal表示機器人的實時關節角.

圖6 基于微分逆運動學的機器人運動控制引擎設計框圖Fig.6 Block diagram of the motion control engine based on differential inverse kinematics

4 仿真與實驗

4.1 雙臂搬運實驗

4.1.1 雙臂搬運示教與DTW 規整

如圖7 所示,本文選用Pepper 仿人機器人作為實驗驗證平臺.Pepper 高1.2 m,裝有加速度計和陀螺儀等多種傳感器,共有20 個自由度,本文只考慮其左、右手臂各有的5 個自由度.其帶有的操作系統NAOqi 支持python SDK 和C++SDK.本文利用Matlab 良好的數據處理特性進行DTW多機械臂示教軌跡的規整、GMM 概率模型的軌跡表征訓練以及Pepper 正逆運動學的計算等工作,然后將泛化輸出的多機械臂協同軌跡數據存儲為.mat 格式的文件,最后調用這些文件控制Pepper 的雙臂完成協作搬運.

圖7 Pepper 實驗平臺Fig.7 Pepper experimental platform

圖8 所示為Pepper 雙臂協作完成搬運的示教過程序列,不同人的示教過程往往存在時間長短和速度差異較大的問題.實驗時進行三次搬運示教,將示教得到的軌跡數據傳到Matlab,并利用DTW算法規整原始示教軌跡,結果如圖9 所示.左側表示三次示教時右手臂末端的軌跡和x、y、z三維上的變化情況.左側對應的原始示教軌跡序列表明,對于三次相同動作的示教軌跡,軌跡的時間差異明顯,而右側經由DTW 規整后的軌跡消除了示教過程中的時間不一的影響,統一了時間維度上的變化.

圖8 雙臂協作搬運示教Fig.8 Demonstrations of dual-arm collaborative moving trajectory

圖9 右手臂軌跡DTW 規整Fig.9 DTW output of right arm trajectory

4.1.2 DTW-GMM 軌跡表征

基于DTW 規整后的多機械臂示教軌跡,利用算法1 所述流程進行軌跡的表征學習.實驗中相關參數設置如表2 所示.

表2 DTW-GMM 算法參數設置Table 2 Parameter setting of the DTW-GMM algorithm

圖10 是GMM 模型跟本文提出的DTWGMM 模型針對搬運時右手臂的x、y、z的三維特征表征情況.此處由式(11) AIC 準則確定的K的取值為4.GMM 是一種概率模型的軌跡表征策略,圖10 中用橢圓來描述示教軌跡的特征提取結果.橢圓的形狀由高斯分布的協方差矩陣所決定,其面積大小表示右手臂三維空間位置的不確定度.面積越大,不確定度越大,即軌跡的特征提取效果越差.DTW-GMM 的表征結果優于GMM.表3 和表4是GMM和DTW-GMM 表征的同一個高斯分布的協方差矩陣表.由協方差矩陣表的定量分析可以看出,DTW-GMM 對應的各維度變量之間具有較小的協方差,算法可以更好地提取機械臂示教軌跡的特征,具有更好的表征結果.

圖10 右手臂軌跡表征Fig.10 Right arm trajectory characterization

表3 GMM 表征協方差矩陣表Table 3 Covariance matrix of GMM algorithm

表4 DTW-GMM 表征協方差矩陣表Table 4 Covariance matrix of DTW-GMM algorithm

4.1.3 GMR 軌跡泛化輸出

對機械臂示教軌跡經GMM 編碼后,利用GMR進行軌跡重構,可以得到泛化的輸出軌跡.圖11 是右手臂的泛化輸出示意圖.經由DTW 規整后的示教軌跡特征更加明顯,泛化輸出時具有更低的不確定度.

圖11 右手臂軌跡泛化輸出Fig.11 Generalized right arm trajectory output

得到右手臂的GMR 泛化輸出軌跡后,利用第3 節所述多機械臂的協同策略,以作為查詢向量,實時獲取左手臂的位置控制向量,最后得到搬運實驗時左、右手臂的協作執行軌跡如圖12所示.其中,虛線表示原始的三條示教軌跡,實線是泛化輸出的執行軌跡.原始示教軌跡由于噪聲的存在,軌跡的平滑性能較差.本文所提出的DTW-GMM策略對噪聲具有一定的抗干擾性,能夠充分提取示教軌跡的共同特征,且最終輸出的執行軌跡具有較好的平滑性.

圖12 搬運實驗左右手臂軌跡泛化輸出Fig.12 Generalized dual-arm trajectory output in moving experiment

4.1.4 實體機器人實驗

在Pepper 機器人平臺上,基于本文所提出的DTW-GMM 策略,以右手臂的空間位置信息和DTW 規整后的時間信息為查詢向量,實時獲取左右臂的協同執行軌跡信息.在第3.2 節設計的運動控制引擎作用下,對軌跡進行跟蹤控制.圖13 是Pepper 機器人雙臂協作完成搬運實驗的截圖.通過實驗可以發現,本文的多機械臂協同軌跡生成時間約為1 ms,實驗的硬件環境為16.00 GB RAM,Intel i7-7500U CPU,軟件環境為Matlab 2018a.實驗結果表明了基于DTW-GMM 模型對控制機器人雙機械臂完成協同任務的可行性.

圖13 雙臂協作搬運實驗截圖Fig.13 Snapshots of dual-arm to collaboratively move basket

4.2 漢字書寫實驗

漢字軌跡由筆畫組成,是一種離散化的軌跡[30-34].當前的漢字軌跡書寫主要是基于預規劃軌跡的執行策略.通過將漢字軌跡分段進行規劃,并設計控制器跟蹤所規劃的筆畫軌跡.這種策略模式單一,泛化性不足.近年來,研究人員希望通過示教學習的策略,來學習漢字軌跡的特征,提高軌跡生成的泛化性.而當前基于示教學習的研究主要將漢字軌跡進行連續化處理,這樣會失去原有漢字軌跡的基本特征,且當漢字軌跡較為復雜時進行連續化示教操作繁瑣.為此,本節提出基于DTW-GMM 的漢字軌跡書寫策略.

由于漢字軌跡的書寫主要是在x、y平面上的運動,所以可以將某一個機械臂的x、y平面上的位置信息向量和經由DTW 規整后的時間t,即以作為查詢向量,通過GMM 和GMR 的多機械臂軌跡的表征和泛化,輸出其他機械臂的協同軌跡.圖14(a)是基于本文提出的協同策略的漢字軌跡"木"的書寫情況.將 “木”字按照筆畫分解為“一”、“丨”、“丿”、“丶”四段軌跡,由4 個機械臂分別執行,并以執行 “一”的機械臂的位置矢量作為查詢向量,實時獲取其余機械臂的協同軌跡數據.“木”字軌跡較為簡單,當漢字的筆畫較為復雜時,比如對于交叉型的軌跡,可以適當調整策略,通過將某些筆畫分配給同一個機械臂,可以減少需要進行協同的機械臂的數量,減少書寫任務的復雜性,如對于漢字軌跡 “打”字,可以利用雙機械臂分別對軌跡的 “扌”和 “丁”進行學習,最終 “打”字的軌跡表征與生成情況如圖14(b)所示.

圖14 基于DTW-GMM 算法的漢字軌跡書寫Fig.14 Chinese character trajectory generation based on DTW-GMM algorithm

5 結束語

本文提出了一種基于概率模型的多機械臂示教軌跡表征策略,可以用于實現機器人的多臂協作.利用DTW-GMM 算法對原始示教軌跡進行時間規整并提取共同特征,然后設計多機械臂協同控制策略,最后基于GMR 回歸多機械臂的協同軌跡.Pepper 機器人雙臂協作完成搬運和漢字軌跡書寫實驗驗證了本文所提出的基于DTW-GMM 的多機械臂多任務協同策略的有效性.下一步預期在軌跡生成時融入視覺信息反饋和基于強化學習的參數學習策略,實現復雜環境下的多機械臂適應性協同軌跡的輸出.