葉子涵 陳俊洪 楊振國(guó) 劉文印

（廣東工業(yè)大學(xué)計(jì)算機(jī)學(xué)院，廣州，510006）

0 引言

近年來(lái)，在中國(guó)制造2025的政策背景下，機(jī)器人技術(shù)已成為智能制造的重要標(biāo)志之一，并且隨著人工智能的快速發(fā)展以及機(jī)器人的靈巧性的提高，機(jī)器人在醫(yī)療、服務(wù)等領(lǐng)域的應(yīng)用也愈加廣泛，這讓人們對(duì)機(jī)器人智能化程度有了更高的需求[1-2]。

示教學(xué)習(xí)是機(jī)器人快速學(xué)習(xí)技能的一種重要方式，也被稱作模仿學(xué)習(xí)，機(jī)器人通過(guò)模仿人類動(dòng)作生成相應(yīng)的運(yùn)動(dòng)軌跡，并對(duì)軌跡進(jìn)行泛化學(xué)習(xí)，進(jìn)而得到相應(yīng)的運(yùn)動(dòng)技能[3-4]。

根據(jù)示教者是否與機(jī)器人直接發(fā)生物理上的接觸可分為動(dòng)覺(jué)示教和遠(yuǎn)程示教。動(dòng)覺(jué)示教是示教者人工操縱機(jī)器人完成相應(yīng)工作動(dòng)作并記錄運(yùn)動(dòng)軌跡，再利用學(xué)習(xí)算法對(duì)運(yùn)動(dòng)軌跡進(jìn)行學(xué)習(xí)，最終將學(xué)習(xí)到的軌跡傳輸給機(jī)器人執(zhí)行[5]；遠(yuǎn)程示教是通過(guò)視覺(jué)、穿戴式傳感器或其他遠(yuǎn)程控制工具來(lái)控制機(jī)器人運(yùn)動(dòng)[6]。以示教學(xué)習(xí)為代表的軌跡規(guī)劃方法，著重通過(guò)學(xué)習(xí)和模仿人類演示動(dòng)作，使得機(jī)器人可以與操作者協(xié)同作業(yè)。

作為機(jī)器人學(xué)的重要分支，示教學(xué)習(xí)吸引了大量的科學(xué)家對(duì)此進(jìn)行研究。Calinon等人[7-8]將動(dòng)力學(xué)系統(tǒng)參數(shù)估計(jì)問(wèn)題表示為在不同坐標(biāo)系中具有投影的高斯混合回歸（GMR）問(wèn)題，使用高斯混合模型（GMM）對(duì)運(yùn)動(dòng)軌跡進(jìn)行建模，能有效處理大規(guī)模數(shù)據(jù)，但是存在生成軌跡不一定連續(xù)等缺點(diǎn)。為了有效地解決軌跡不連續(xù)的問(wèn)題， Schaal等人[9]于2002年提出了基于動(dòng)態(tài)系統(tǒng)示教學(xué)習(xí)方法的動(dòng)態(tài)運(yùn)動(dòng)基元(Dynamical Movement Primitives, DMP)算法。該算法源于二階彈簧阻尼模型，它可以用一組線性微分方程表示簡(jiǎn)單的線性動(dòng)態(tài)系統(tǒng)，通過(guò)向該系統(tǒng)添加一個(gè)可自主學(xué)習(xí)的非線性項(xiàng)，將其轉(zhuǎn)換為具有特定吸引子的弱非線性系統(tǒng)。由于彈簧阻尼模型的特性是朝著目標(biāo)位置收斂，因此由非線性項(xiàng)調(diào)制的動(dòng)態(tài)運(yùn)動(dòng)基元最終也會(huì)朝著特定目標(biāo)位置收斂，并在收斂到目標(biāo)位置的過(guò)程中利用策略參數(shù)來(lái)生成任意形狀的運(yùn)動(dòng)軌跡，其中，策略參數(shù)可以通過(guò)局部加權(quán)回歸或強(qiáng)化學(xué)習(xí)等方法來(lái)學(xué)習(xí)，模型通過(guò)學(xué)習(xí)示教軌跡的運(yùn)動(dòng)信息使新生成的軌跡具有原軌跡的運(yùn)動(dòng)趨勢(shì)。2013年，Ijspeert等人[10]對(duì)此算法進(jìn)行了改進(jìn)，他們重新構(gòu)造了非線性項(xiàng)的方程，用起始位置、目標(biāo)位置和時(shí)間三者共同調(diào)制非線性函數(shù)，這意味著當(dāng)系統(tǒng)達(dá)到目標(biāo)位置時(shí)，非線性項(xiàng)可以有效地消失，從而保證系統(tǒng)最后趨于穩(wěn)定。他們還使用相位變量對(duì)時(shí)間進(jìn)行替換，避免了明確的時(shí)間依賴性。

近些年機(jī)器視覺(jué)在機(jī)器人領(lǐng)域的應(yīng)用越來(lái)越廣泛，其中基于深度學(xué)習(xí)的目標(biāo)檢測(cè)是近年來(lái)發(fā)展最為迅速的科研熱點(diǎn)之一，它有效地解決了機(jī)器人執(zhí)行操作中所需要的物體識(shí)別以及分類的問(wèn)題[11-12]。

2014年Ross Girshick等人[13]提出了R-CNN（Regionbased Convolutional Neural Network，區(qū)域卷積神經(jīng)網(wǎng)絡(luò)），使用基于區(qū)域的卷積神經(jīng)網(wǎng)絡(luò)來(lái)進(jìn)行目標(biāo)檢測(cè)，將PASCAL VOC數(shù)據(jù)集上的檢測(cè)率從35.1%提升到53.7%，但訓(xùn)練時(shí)間以及測(cè)試時(shí)間過(guò)長(zhǎng)。為了解決該問(wèn)題，Ross Girshick團(tuán)隊(duì)在2015年基于R-CNN提出了新的算法Fast R-CNN，它避免了對(duì)每一個(gè)候選區(qū)域都進(jìn)行特征提取的做法，取而代之的是先對(duì)一張圖像進(jìn)行特征提取，再根據(jù)候選區(qū)域在相應(yīng)的特征上進(jìn)行劃分，得到對(duì)應(yīng)區(qū)域的特征，從而大幅提升了目標(biāo)檢測(cè)的速度。此外，為了節(jié)省找出候選框所需要的時(shí)間，Ren等人進(jìn)一步改進(jìn)了Fast R-CNN，提出了利用神經(jīng)網(wǎng)絡(luò)提取物體邊緣特征作為候選框，大大節(jié)省了尋找候選框所需要的時(shí)間，該算法也被命名為Faster R-CNN[14]。為了有效地識(shí)別出物體的輪廓信息，He等人在Faster R-CNN的基礎(chǔ)上提出了Mask R-CNN[15]，通過(guò)在Faster R-CNN結(jié)構(gòu)上添加mask預(yù)測(cè)分支，完成對(duì)物體輪廓的預(yù)測(cè)，并且改良了ROI Pooling，在實(shí)例分割上擁有不錯(cuò)的效果。

在上述基礎(chǔ)上，本文提出一種基于視覺(jué)和動(dòng)態(tài)運(yùn)動(dòng)基元的機(jī)械臂抓取軌跡規(guī)劃模型，該模型用來(lái)自主識(shí)別待抓取物體、自主學(xué)習(xí)示教軌跡并生成新的抓取軌跡，最后在Baxter七自由度機(jī)械臂上進(jìn)行實(shí)驗(yàn)，實(shí)驗(yàn)結(jié)果證明了該方法能有效地識(shí)別出待抓取物體，并根據(jù)原有的示教軌跡泛化出新的抓取軌跡。

動(dòng)態(tài)運(yùn)動(dòng)基元分為非周期運(yùn)動(dòng)和周期運(yùn)動(dòng)兩種場(chǎng)景，本文只討論非周期運(yùn)動(dòng)的情況。

1 動(dòng)態(tài)運(yùn)動(dòng)基元算法

1.1 動(dòng)態(tài)運(yùn)動(dòng)基元算法原理

動(dòng)態(tài)運(yùn)動(dòng)基元（DMP）算法的模型最開(kāi)始是來(lái)自于二階線性彈簧系統(tǒng)，該系統(tǒng)的特性是以特定的運(yùn)動(dòng)朝著目標(biāo)位置收斂。DMP的基本思想是使用非線性項(xiàng)調(diào)制具有良好穩(wěn)定特性的動(dòng)力學(xué)系統(tǒng)，由非線性函數(shù)控制系統(tǒng)的運(yùn)動(dòng)過(guò)程，以實(shí)現(xiàn)期望的吸引子行為。DMP將彈簧阻尼模型抽象為二階微分方程，并加入強(qiáng)迫項(xiàng)，如公式（1）所示:

其中， 表示系統(tǒng)的運(yùn)動(dòng)狀態(tài)，在單自由度的運(yùn)動(dòng)下即為位移，、對(duì)應(yīng)的一階導(dǎo)數(shù)和二階導(dǎo)數(shù)，分別表示速度和加速度。 表示系統(tǒng)期望的運(yùn)動(dòng)狀態(tài)，即目標(biāo)點(diǎn)或者稱為吸引子，在軌跡規(guī)劃過(guò)程中為笛卡爾空間中點(diǎn)的位置或是機(jī)械臂的關(guān)節(jié)角度。是時(shí)間常數(shù)，和為正常數(shù)，通過(guò)設(shè)置適當(dāng)?shù)哪Ｐ蛥?shù)和，如，系統(tǒng)可以被臨界阻尼，使得單調(diào)收斂于 ，最終保證系統(tǒng)趨于穩(wěn)定公式。

公式（1）稱為轉(zhuǎn)換系統(tǒng)。

公式（2）稱為規(guī)范系統(tǒng)，用來(lái)表示對(duì)時(shí)間的抽象?？梢员豢醋鍪且粋€(gè)相位變量，從一個(gè)初始狀態(tài)如開(kāi)始，單調(diào)收斂為0，當(dāng)接近0就意味著目標(biāo)基本上已經(jīng)實(shí)現(xiàn)。為用來(lái)調(diào)節(jié)規(guī)范系統(tǒng)衰減速度的時(shí)間縮放因子，為常系數(shù)。

1.2 多自由度動(dòng)態(tài)運(yùn)動(dòng)基元耦合

為了將動(dòng)態(tài)運(yùn)動(dòng)基元應(yīng)用到多自由度的軌跡學(xué)習(xí)，本研究的模型需要對(duì)系統(tǒng)的多個(gè)自由度進(jìn)行耦合。如圖1所示，本文所采用的方法是在所有的自由度之間共享一個(gè)規(guī)范系統(tǒng)，并且對(duì)每個(gè)自由度保留一組獨(dú)立的轉(zhuǎn)換系統(tǒng)。規(guī)范系統(tǒng)提供各自由度之間的時(shí)間耦合，轉(zhuǎn)換系統(tǒng)實(shí)現(xiàn)各自由度的期望吸引子，即目標(biāo)狀態(tài)。本文所使用的的機(jī)械臂在三維笛卡爾空間中進(jìn)行軌跡規(guī)劃，因此將動(dòng)態(tài)運(yùn)動(dòng)基元擴(kuò)展到3自由度。

圖1 3自由度系統(tǒng)動(dòng)態(tài)運(yùn)動(dòng)基元

圖2為三維空間中動(dòng)態(tài)運(yùn)動(dòng)基元對(duì)示教軌跡的模仿和泛化的示例。其中，藍(lán)色實(shí)線表示原始軌跡，黑色虛線表示模型根據(jù)多個(gè)不同的起始點(diǎn)和目標(biāo)點(diǎn)生成的新的軌跡。

圖2 三維動(dòng)態(tài)運(yùn)動(dòng)基元模仿和泛化示教軌跡示例

2 Mask R-CNN算法

本文所采用的的目標(biāo)檢測(cè)算法是Mask R-CNN，這是一個(gè)集成了物體檢測(cè)和實(shí)例分割兩大功能的高性能神經(jīng)網(wǎng)絡(luò)。Mask R-CNN在目標(biāo)檢測(cè)算法Faster R-CNN上進(jìn)行了改進(jìn)，在原有的分類識(shí)別和回歸定位的基礎(chǔ)上增加了一個(gè)mask預(yù)測(cè)分支，即預(yù)測(cè)物體的掩膜，并使用RoI Align層代替RoI Pooling層來(lái)獲取固定大小的特征圖。Mask R-CNN的網(wǎng)絡(luò)結(jié)構(gòu)包含兩個(gè)部分，第一個(gè)部分是使用RPN（Region Proposal Networks，區(qū)域推薦網(wǎng)絡(luò)）查找候選區(qū)域，另一部分則用來(lái)對(duì)RPN生成的候選區(qū)域中的每一個(gè)RoI（Region of Interest，興趣區(qū)域）進(jìn)行分類、框回歸和mask預(yù)測(cè)。整體網(wǎng)絡(luò)框架如圖3所示。

圖3 Mask R-CNN網(wǎng)絡(luò)框架圖

在圖3中，網(wǎng)絡(luò)的第一部分（conv）一共包含13個(gè)卷積層、13個(gè)ReLU激勵(lì)層和4個(gè)池化層，它們主要用來(lái)提取輸入圖像的特征圖，然后將得到的特征圖送入RPN網(wǎng)絡(luò)進(jìn)行候選框的提取。

RPN在輸入的特征圖上進(jìn)行滑動(dòng)，并將輸入的特征圖映射為一個(gè)512維的特征向量，在這個(gè)特征向量后面連接兩個(gè)并聯(lián)的全連接層，這兩個(gè)全連接層分別用來(lái)進(jìn)行邊界框回歸和物體分類，最后生成若干個(gè)候選區(qū)域；RPN的主要功能是實(shí)現(xiàn)目標(biāo)定位，輸出以[x,y,w,h]表示的多個(gè)矩形目標(biāo)候選框。

由于Faster R-CNN使用的RoI Pooling對(duì)特征圖進(jìn)行了兩次量化操作，丟失了圖像像素的精度導(dǎo)致實(shí)例分割的效果大幅度降低，所以Mask R-CNN采用RoI Align代替RoI Pooling層來(lái)生成固定大小為7×7的特征圖，并使用雙線性插值來(lái)精確地找出每個(gè)候選區(qū)域?qū)?yīng)的特征圖。

最后，將固定大小的特征圖送入全連接層，利用Softmax進(jìn)行分類識(shí)別，利用L1 Loss回歸進(jìn)行邊框的定位，并增加一個(gè)mask分支進(jìn)行實(shí)例分割。

Mask R-CNN進(jìn)行目標(biāo)檢測(cè)的效果如圖4所示。

圖4 目標(biāo)檢測(cè)效果

3 抓取任務(wù)實(shí)驗(yàn)設(shè)計(jì)

實(shí)驗(yàn)平臺(tái)為Ubuntu16.04操作系統(tǒng)和ROS（Robot Operating System，機(jī)器人操作系統(tǒng)）的Kinetic版本，使用的機(jī)械臂為Baxter雙臂協(xié)同機(jī)器人自帶機(jī)械臂，并使用Kinect v2深度相機(jī)進(jìn)行視覺(jué)感知。

3.1 Kinect深度相機(jī)標(biāo)定

為了獲取環(huán)境的圖像信息和三維信息，本文使用Kinect 2.0版本的深度相機(jī)進(jìn)行視覺(jué)感知，而在感知之前，需要事先對(duì)Kinect進(jìn)行內(nèi)部參數(shù)和外部參數(shù)的標(biāo)定。

圖5 Kinect參數(shù)標(biāo)定

由于Kinect的鏡頭是一種透鏡，因此存在徑向和切向畸變，需要內(nèi)部參數(shù)標(biāo)定得到矯正后的相機(jī)參數(shù)，減小或消除捕獲圖像的畸變程度。本文參考張正友[16]標(biāo)定算法，采用5×7×0.03的棋盤(pán)格進(jìn)行內(nèi)部參數(shù)標(biāo)定，如圖5（a）所示，標(biāo)定程序自動(dòng)識(shí)別黑白格鄰接點(diǎn)，即角點(diǎn)的位置，并與已知的標(biāo)定板角點(diǎn)位置配對(duì)，最后分別完成對(duì)Kinect相機(jī)彩色和紅外鏡頭的參數(shù)標(biāo)定。

為了實(shí)現(xiàn)攝像機(jī)坐標(biāo)系與機(jī)器人坐標(biāo)系之間的坐標(biāo)轉(zhuǎn)換，本文將Kinect坐標(biāo)系下采集到的物體的三維坐標(biāo)信息映射到機(jī)器人坐標(biāo)系下的場(chǎng)景，具體的外部參數(shù)標(biāo)定過(guò)程如圖5（b）所示。

對(duì)Kinect采集的深度圖進(jìn)行采樣，分別得到Kinect和Baxter基坐標(biāo)系下的點(diǎn)的坐標(biāo)，最后計(jì)算相應(yīng)的轉(zhuǎn)換矩陣。

3.2 演示軌跡記錄

為了機(jī)器人從演示中學(xué)習(xí)動(dòng)作，示教人員首先需要記錄演示的動(dòng)作軌跡并獲取非線性系統(tǒng)的基本參數(shù)，圖6展示了人工示教過(guò)程，人為抓取機(jī)械臂的末端執(zhí)行器并拖動(dòng)產(chǎn)生軌跡，軌跡由時(shí)間和笛卡爾空間中的3維位置組成。

圖6 人工示教過(guò)程

3.3 示教軌跡特征提取

3.4 示教軌跡重現(xiàn)和泛化

在獲得新的起始點(diǎn)或目標(biāo)點(diǎn)后，通過(guò)代入動(dòng)態(tài)運(yùn)動(dòng)基元算法可以得到泛化后的新軌跡，由于新的環(huán)境下強(qiáng)迫函數(shù)權(quán)重與示教軌跡權(quán)重相同，所以新生成的運(yùn)動(dòng)軌跡在形狀上與示教軌跡相似，從而達(dá)到了軌跡學(xué)習(xí)的目的。本文將視覺(jué)和動(dòng)態(tài)運(yùn)動(dòng)基元相結(jié)合，使用視覺(jué)感知捕獲待抓取物體位置，即新的目標(biāo)點(diǎn)，3.4.1和3.4.2節(jié)分別展示了通過(guò)視覺(jué)獲取目標(biāo)點(diǎn)并泛化軌跡的方法。

3.4.1 獲取目標(biāo)位置

通過(guò)鼠標(biāo)點(diǎn)擊Kinect捕捉到的RGB彩色圖像坐標(biāo)，根據(jù)Kinect內(nèi)置算法將2維坐標(biāo)映射到Kinect坐標(biāo)系下笛卡爾空間3維點(diǎn)云坐標(biāo)，再通過(guò)外部參數(shù)標(biāo)定得到的矩陣轉(zhuǎn)換為Baxter基坐標(biāo)系下的笛卡爾坐標(biāo)，最后將目標(biāo)點(diǎn)發(fā)布到ROS中的話題（topic）中；DMP通過(guò)訂閱該topic獲取該目標(biāo)點(diǎn)，并作為示教軌跡新的目標(biāo)點(diǎn)，進(jìn)而生成新的軌跡。圖7（a）顯示了鼠標(biāo)點(diǎn)擊獲取目標(biāo)點(diǎn)，圖7（b）顯示了為該目標(biāo)點(diǎn)生成的新的軌跡，其中藍(lán)色實(shí)線為示教軌跡，藍(lán)色虛線為新生成的軌跡。

圖7 鼠標(biāo)點(diǎn)擊獲取目標(biāo)點(diǎn)抓取示例

表1給出了在規(guī)范系統(tǒng)的不同時(shí)刻，原始示教軌跡和學(xué)習(xí)后的軌跡在笛卡爾空間中的三維位置。在起始點(diǎn)和目標(biāo)點(diǎn)不變的情況下，新生成的軌跡具有與原始軌跡相同的運(yùn)動(dòng)趨勢(shì)，達(dá)到了軌跡學(xué)習(xí)的目的。

表1 軌跡學(xué)習(xí)過(guò)程

3.4.2 目標(biāo)位置實(shí)時(shí)檢測(cè)

本文的重點(diǎn)在于將深度學(xué)習(xí)和動(dòng)態(tài)運(yùn)動(dòng)基元相結(jié)合，基于視覺(jué)實(shí)現(xiàn)機(jī)械臂抓取軌跡規(guī)劃。因此，筆者對(duì)待檢測(cè)的物體進(jìn)行數(shù)據(jù)標(biāo)注，建立了自己的數(shù)據(jù)集；將數(shù)據(jù)集送入Mask R-CNN網(wǎng)絡(luò)進(jìn)行訓(xùn)練，得到訓(xùn)練好的網(wǎng)絡(luò)模型。

通過(guò)Kinect v2深度相機(jī)的彩色攝像頭讀取彩色圖像并作為Mask R-CNN的輸入圖像，可以得到待抓取物體以及目標(biāo)位置的定位（方框），并用一個(gè)四元組 (x,y,w,h) 來(lái)表征方框的位置，這4個(gè)參數(shù)分別表示方框左上角的2維坐標(biāo)以及方框的寬度和高度。圖8展示了對(duì)待抓取物體的目標(biāo)檢測(cè)效果，其中類別為bottle的物體為待抓取物體。

圖8 目標(biāo)檢測(cè)效果

通過(guò)目標(biāo)檢測(cè)得到的待抓取物體在圖像中的位置以及三維點(diǎn)云信息，筆者可以獲得其在Kinect坐標(biāo)系下的3維坐標(biāo)；和3.4.1節(jié)類似，將該坐標(biāo)點(diǎn)轉(zhuǎn)換為Baxter基坐標(biāo)系下的坐標(biāo)，并發(fā)布到ROS中，DMP訂閱該目標(biāo)點(diǎn)并生成新的抓取軌跡。

更改起始點(diǎn)（機(jī)械臂末端執(zhí)行器的位置）和目標(biāo)點(diǎn)（瓶子位置），泛化出的新的軌跡如圖9所示，其中，紅色圓圈表示起始點(diǎn)，藍(lán)色圓圈表示目標(biāo)點(diǎn)。機(jī)械臂抓取過(guò)程如圖10所示，對(duì)于不同的目標(biāo)點(diǎn)和起始點(diǎn)，新生成的軌跡仍具有與原始示教軌跡相同的運(yùn)動(dòng)趨勢(shì)，證明模型的泛化效果有效。

圖9 目標(biāo)檢測(cè)獲取的目標(biāo)點(diǎn)泛化軌跡

圖10 機(jī)械臂抓取過(guò)程

3.5 性能分析

與本文提出的兩階段的軌跡規(guī)劃方法相比，傳統(tǒng)的機(jī)械臂軌跡規(guī)劃方法得到的目標(biāo)位置信息和實(shí)際位置存在較大偏差，并且由于要進(jìn)行逆運(yùn)動(dòng)學(xué)求解等運(yùn)算，存在耗時(shí)較久的問(wèn)題。本文將傳統(tǒng)軌跡規(guī)劃方法與上述兩階段模型做對(duì)比實(shí)驗(yàn)，實(shí)驗(yàn)數(shù)據(jù)如表2所示，數(shù)據(jù)顯示，動(dòng)態(tài)運(yùn)動(dòng)基元和深度視覺(jué)相結(jié)合的方法生成的軌跡沒(méi)有誤差且耗時(shí)較短。

表2 軌跡規(guī)劃方案對(duì)比分析

此外，軌跡規(guī)劃分為軌跡學(xué)習(xí)和泛化兩個(gè)階段，大多數(shù)的軌跡規(guī)劃方法在第二階段并未與視覺(jué)相結(jié)合，或者僅僅使用顏色識(shí)別或貼標(biāo)記（marker）識(shí)別等簡(jiǎn)單方式，因此泛化性能大大降低，本文所提出的兩階段模型生成了一套完整的擁有自主識(shí)別能力的軌跡規(guī)劃系統(tǒng)，較好地提升了軌跡的泛化性能和自主學(xué)習(xí)能力。

4 總結(jié)

本文提出了一種基于深度視覺(jué)和動(dòng)態(tài)運(yùn)動(dòng)基元的機(jī)械臂抓取軌跡規(guī)劃方法，通過(guò)對(duì)示教軌跡的學(xué)習(xí)，機(jī)械臂可以根據(jù)不同的環(huán)境自主調(diào)整運(yùn)動(dòng)策略，實(shí)現(xiàn)對(duì)原始軌跡的重現(xiàn)和泛化；同時(shí)，本文集成的基于深度學(xué)習(xí)的目標(biāo)檢測(cè)技術(shù)，可以有效地對(duì)外界環(huán)境進(jìn)行視覺(jué)感知，保障實(shí)現(xiàn)機(jī)械臂對(duì)物體抓取的軌跡規(guī)劃，更好地適應(yīng)服務(wù)機(jī)器人的多變環(huán)境。