王校峰，王建文，曹鵬勇，楊云茂

(華東理工大學(xué)機(jī)械與動(dòng)力工程學(xué)院,上海 200237)

0 前言

機(jī)器人技術(shù)在生產(chǎn)和生活中的應(yīng)用越來越廣泛，如何利用機(jī)械臂對移動(dòng)目標(biāo)進(jìn)行抓取是當(dāng)前機(jī)械臂工作中的一個(gè)重要技術(shù)問題。通常情況下，機(jī)械臂通過示教編程的方式對固定工件進(jìn)行抓取，而對移動(dòng)目標(biāo)進(jìn)行抓取需要附加傳感系統(tǒng)對工件進(jìn)行識(shí)別和趨近操作。在視覺傳感器參與構(gòu)建機(jī)器人系統(tǒng)后，配合適當(dāng)算法使機(jī)器人有了一定的自主操作能力。倪鶴鵬等提出一種基于時(shí)間與工件位置的圖像算法，根據(jù)預(yù)測工件直線運(yùn)動(dòng)到達(dá)固定參考點(diǎn)的時(shí)間進(jìn)行抓取。王兆權(quán)等利用Kalman濾波算法對目標(biāo)位置進(jìn)行預(yù)測，通過速度補(bǔ)償實(shí)現(xiàn)機(jī)械臂移動(dòng)過程并完成抓取。在實(shí)際應(yīng)用中，利用視覺技術(shù)預(yù)測目標(biāo)位置和定點(diǎn)抓取的方法，同樣存在無法適應(yīng)目標(biāo)運(yùn)動(dòng)速度、運(yùn)動(dòng)方向及其他工作環(huán)境要素的變化,動(dòng)態(tài)抓取效率不高的問題。

工業(yè)機(jī)器人的自由度多，控制復(fù)雜。為解決傳統(tǒng)機(jī)械臂控制系統(tǒng)設(shè)計(jì)難度大、移植性差、算法實(shí)現(xiàn)困難的問題，TOSEPH等研究出了廣泛為大家接受的機(jī)器人操作系統(tǒng)(Robot Operating System，ROS)，為工業(yè)機(jī)器人避障策略的研究提供了統(tǒng)一的軟件平臺(tái)。該平臺(tái)實(shí)現(xiàn)了控制代碼的成組設(shè)計(jì)和機(jī)器系統(tǒng)的模塊化設(shè)計(jì)，提高了機(jī)器人設(shè)計(jì)效率。

本文作者基于ROS框架搭建基于視覺反饋的UR5機(jī)械臂動(dòng)態(tài)抓取平臺(tái)，實(shí)現(xiàn)對目標(biāo)物體的快速識(shí)別，并通過視覺測距的方式計(jì)算機(jī)械臂末端偏移量。設(shè)計(jì)注視逼近的視覺反饋控制策略，實(shí)現(xiàn)機(jī)械臂對傳送帶上移動(dòng)目標(biāo)的抓取操作。

1 實(shí)驗(yàn)環(huán)境搭建

基于ROS機(jī)器人操作系統(tǒng)免費(fèi)且開源、代碼易擴(kuò)展等優(yōu)點(diǎn)，本文作者在Ubuntu18.04上利用ROS-Melodic搭建動(dòng)態(tài)抓取平臺(tái)，核心部件包括：傳輸帶、RealSense D415攝像頭、UR5機(jī)械臂等。攝像頭采用Eye-in-Hand方式與機(jī)械臂末端連接，用于獲取執(zhí)行端視野。在傳動(dòng)帶運(yùn)行方向側(cè)方，固定UR5機(jī)械臂，如圖1所示。

圖1 UR5機(jī)械臂動(dòng)態(tài)抓取系統(tǒng)

1.1 機(jī)械臂配置

采用SolidWorks建立六自由度UR5機(jī)器人模型,利用sw_urdf_exporter插件，確定機(jī)械臂各關(guān)節(jié)部件的連接關(guān)系和坐標(biāo)關(guān)系，自動(dòng)生成URDF文件及展示用的launch文件。URDF(Unified Robot Description Format)是一種使用XML格式描述機(jī)器人模型的文件，供ROS平臺(tái)使用。

創(chuàng)建一個(gè)MoveIt配置的功能包，MoveIt配置助手(setup assistant)針對機(jī)械臂模型做一系列的配置并且完成編譯，選用KDL(The Kinematics and Dynamics Library)進(jìn)行正逆運(yùn)動(dòng)學(xué)求解，完成機(jī)器人的配置、可視化和仿真工作。具體步驟如圖2所示。

圖2 機(jī)械臂配置流程

1.2 機(jī)械臂運(yùn)動(dòng)學(xué)分析

對UR5機(jī)械臂進(jìn)行運(yùn)動(dòng)學(xué)分析，圖3所示為UR5機(jī)械臂結(jié)構(gòu)圖。

圖3 UR5機(jī)械臂結(jié)構(gòu)圖

機(jī)械臂正運(yùn)動(dòng)學(xué)問題是從基坐標(biāo)依次通過坐標(biāo)變換的方式求取末端執(zhí)行器的位姿的過程。UR5機(jī)械臂模型的D-H坐標(biāo)系表達(dá)各運(yùn)動(dòng)關(guān)節(jié)間的位姿變換關(guān)系，其D-H參數(shù)如表1所示。

表1 UR5機(jī)械臂D-H參數(shù)

確定機(jī)械臂的各連桿參數(shù)與坐標(biāo)系后，使用改進(jìn)的D-H參數(shù)法來確定各連桿變換矩陣。將各連桿的變換矩陣相乘就可獲得最終的變換矩陣。公式(1)所示為第-1連桿的變換矩陣：

(1)

式中:c=cos；s=sin；c=cos；s=sin，由此可得UR5機(jī)械臂的坐標(biāo)轉(zhuǎn)換公式:

(2)

式中:、、表示末端執(zhí)行器相對于基坐標(biāo)的姿態(tài)；表示末端執(zhí)行器相對于基坐標(biāo)的位置。

2 目標(biāo)識(shí)別及空間定位

2.1 目標(biāo)識(shí)別

在目標(biāo)定位跟蹤前需對目標(biāo)進(jìn)行快速準(zhǔn)確的識(shí)別，本文作者提出一種融合RGB算法和SITF(Scale-Invariant Feature Transform，尺度不變特征變換)特征匹配算法的目標(biāo)識(shí)別算法。

在實(shí)驗(yàn)過程中發(fā)現(xiàn)SITF算法計(jì)算復(fù)雜程度高，特征匹配占用大量計(jì)算資源，不滿足實(shí)時(shí)性要求。針對這種情況，考慮到實(shí)驗(yàn)環(huán)境中目標(biāo)物為簡單規(guī)則形狀，通過特征點(diǎn)主方向夾角的角度差閾值消除誤匹配點(diǎn)，降低運(yùn)算復(fù)雜程度。在融合算法的SITF策略中，參考影像與待匹配影像第對特征點(diǎn)的主方向夾角應(yīng)滿足約束條件公式：

|-′|<

(3)

()=|-′|+|-′|+|-′|<

(4)

式中：為參考影像主方向夾角；′為待匹配影像對應(yīng)主方向的夾角；為單角度差閾值。

若第對特征點(diǎn)的主方向夾角之差大于給定閾值，則認(rèn)為是錯(cuò)誤匹配點(diǎn)并予以舍棄。對小于閾值的特征點(diǎn)對進(jìn)行下一步計(jì)算。通過選擇合適的閾值排除主方向差異較大的特征點(diǎn)，減少匹配運(yùn)算量并保證較高精度。

通過預(yù)判斷的SITF策略明確了目標(biāo)的形狀特征，然后再通過對目標(biāo)的顏色特征進(jìn)行二次匹配來提高匹配精度。

2.2 目標(biāo)空間定位

確認(rèn)目標(biāo)識(shí)別算法和中心點(diǎn)后，基于小孔成像技術(shù)，采用基于特征點(diǎn)的單目視覺測距方法，將相機(jī)采集的圖像從三維場景投影到二維成像平面上，通過三角形相似原理估算目標(biāo)中心與相機(jī)的空間位置關(guān)系，通過坐標(biāo)變換引導(dǎo)視覺機(jī)械臂對工件進(jìn)行抓取。單目視覺測距原理如圖4所示。

圖4 單目視覺測距原理

圖4中,攝像頭透鏡光心為相機(jī)坐標(biāo)系原點(diǎn)，軸為攝像機(jī)的光軸；、′平面分別為攝像機(jī)采集圖像平面和成像平面；′與軸線交于點(diǎn)，以點(diǎn)(，)在′平面建立圖像直角坐標(biāo)系，、分別表示行和列。通過對目標(biāo)物體顏色空間的識(shí)別，確認(rèn)在平面中目標(biāo)水平方向中心點(diǎn)?！錇橹行狞c(diǎn)在′中投影點(diǎn)，在′平面以圖像直角坐標(biāo)系表示。

根據(jù)小孔成像原理，世界坐標(biāo)系下的某點(diǎn)(,,)與它在π′平面上投影點(diǎn)′(,)圖像坐標(biāo)的變換關(guān)系如公式(5)所示：

(5)

其中:、分別為軸和軸的有效焦距；、分別為攝像機(jī)內(nèi)參數(shù)和外參數(shù)；為旋轉(zhuǎn)向量；為平移向量。

假定世界坐標(biāo)系位于平面內(nèi)，即點(diǎn)的世界坐標(biāo)(,,0)，=，=[0 0]，式(5)化為

(6)

將平面內(nèi)矩形沿方向分為份，將第份面積記為，在′上的相應(yīng)投影面積記為′，則總面積為

圖3中：Qe1、Qw分別為熱交換前煙氣熱量、濕垃圾熱量；Qe2、Qd、Qg分別為熱交換后煙氣熱量、干垃圾熱量、水蒸氣熱量；Qt為干燥過程中損失的熱量；Ve為煙氣體積；Ww、Wd、Wg分別為濕垃圾質(zhì)量、干垃圾質(zhì)量、水蒸氣質(zhì)量；Te1、Tm1分別為熱交換前煙氣溫度、濕垃圾溫度；Te2、Tm2、Tg分別為熱交換后煙氣溫度、干垃圾溫度、水蒸氣溫度；ce1、ce2分別為熱交換前后煙氣的比熱容；cw、cd分別為濕垃圾、干垃圾的比熱容；cg為水蒸氣比熱容。

(7)

即：

(8)

根據(jù)上述測距模型，計(jì)算目標(biāo)水平和豎直方向的空間位置相對在、軸方向的偏移量。

如圖5所示，光心與點(diǎn)的連線與′平面的交點(diǎn)，即光心到成像平面的距離,為目標(biāo)中心到成像平面的距離,可得：

圖5 x向測量示意

(9)

通過公式(9)可求得目標(biāo)在水平方向相對于攝像頭光軸在軸方向的偏移量，同理可求得目標(biāo)在豎直方向的相對偏移量，即：

(10)

聯(lián)合公式(7)可求得點(diǎn)在相機(jī)坐標(biāo)系下的三維空間位置信息：

(11)

綜上，求得平面中點(diǎn)相對攝像機(jī)光心的、、坐標(biāo)方向的偏移量。

3 逼近抓取

3.1 逼近策略

考慮目標(biāo)處于非穩(wěn)定運(yùn)動(dòng)狀態(tài)，由攝像頭的圖像坐標(biāo)投射到三維坐標(biāo)后通過逆運(yùn)動(dòng)學(xué)求解的方法比較困難。本文作者通過視覺反饋替代控制UR5機(jī)械臂運(yùn)動(dòng)，即從圖像中獲得位置誤差，根據(jù)誤差反饋進(jìn)行位移控制，然后再利用圖像獲得目標(biāo)位置與光心誤差，依次循環(huán)，直到達(dá)到可接受的誤差。視覺反饋控制原理如圖6所示。

圖6 視覺反饋控制原理

圖7 機(jī)械臂抓取流程

啟動(dòng)UR5機(jī)械臂，初始化所有數(shù)據(jù)和機(jī)械臂動(dòng)作，等待目標(biāo)進(jìn)入攝像機(jī)視野，若檢測到目標(biāo)存在，則計(jì)算各方向誤差。

當(dāng)誤差不在允許范圍內(nèi)時(shí)，利用MoveIt運(yùn)行文中提出的融合算法進(jìn)行軌跡規(guī)劃，控制機(jī)械臂運(yùn)動(dòng)彌補(bǔ)誤差。一次規(guī)劃完成后，檢測圖像并再次對誤差進(jìn)行計(jì)算和判斷，進(jìn)行下一次循環(huán)。若誤差在允許范圍內(nèi)，則控制機(jī)械臂進(jìn)行抓取動(dòng)作。

3.2 抓取流程

如圖8所示，圖(a)為相機(jī)視野，紅色目標(biāo)在識(shí)別后打印出中心坐標(biāo)；圖(b)為機(jī)械臂抓取工作狀態(tài)。機(jī)械臂具體工作過程：機(jī)械臂初始化運(yùn)動(dòng)到預(yù)定位置，等待傳送帶上的目標(biāo)進(jìn)入視覺相機(jī)的視野范圍內(nèi)，相機(jī)對目標(biāo)進(jìn)行識(shí)別并獲取相機(jī)坐標(biāo)系下的目標(biāo)位姿偏差(目標(biāo)中心點(diǎn)與相機(jī)光心的、、坐標(biāo)方向位置偏差)，通過視覺反饋引導(dǎo)機(jī)械臂逼近目標(biāo)；隨著傳送帶向前運(yùn)動(dòng)，機(jī)械臂不斷逼近目標(biāo)，當(dāng)位姿偏差減小到許可范圍內(nèi)時(shí)，抓取目標(biāo)。

圖8 Gazebo動(dòng)態(tài)抓取工作示意

4 實(shí)驗(yàn)分析

為測試融合算法的處理效果，利用上述方法在UR5動(dòng)態(tài)抓取平臺(tái)上設(shè)計(jì)了兩組性能測試實(shí)驗(yàn)：視覺抓取成功率測試和抓取偏移量測試。測試現(xiàn)場如圖9所示。

圖9 動(dòng)態(tài)抓取工作測試現(xiàn)場

根據(jù)速度的不同共設(shè)置6組自主抓取實(shí)驗(yàn)，每次實(shí)驗(yàn)中目標(biāo)方塊運(yùn)動(dòng)前在初始位置隨機(jī)擺放，每組實(shí)驗(yàn)進(jìn)行100次完整抓取,結(jié)果如表2所示?？芍弘S著傳送帶前進(jìn)速度加快，抓取成功率逐步降低，當(dāng)傳輸速度大于等于12 mm/s時(shí)，出現(xiàn)漏抓即未成功識(shí)別情況?？傮w目標(biāo)物體識(shí)別成功率達(dá)到99.6%，動(dòng)態(tài)逼近抓取成功率達(dá)到98.33%以上，基本滿足工業(yè)應(yīng)用要求。

表2 動(dòng)態(tài)抓取測試結(jié)果

此外，考慮到抓取位置精度問題，在上述速度為0、3、9、15 mm/s的實(shí)驗(yàn)中各隨機(jī)選取20次，目標(biāo)坐標(biāo)系軸正方向?yàn)檫\(yùn)動(dòng)方向，機(jī)械臂末端實(shí)際夾取位置與目標(biāo)中心位置的軸相對偏差如圖10所示?？芍?機(jī)械臂逼近抓取實(shí)際位置與預(yù)期位置的相對誤差隨速度增加而增大，抓取位置向軸負(fù)方向偏移，誤差在許可范圍內(nèi)，驗(yàn)證了抓取算法的精確性。

圖10 抓取位置相對誤差

5 結(jié)論

針對傳統(tǒng)工業(yè)機(jī)器人在移動(dòng)目標(biāo)的抓取中表現(xiàn)出的適應(yīng)性低、柔性差等缺點(diǎn)，提出基于視覺反饋的動(dòng)態(tài)目標(biāo)抓取方法。設(shè)計(jì)一套完整的動(dòng)態(tài)抓取策略，使得機(jī)器人在移動(dòng)目標(biāo)抓取中的自主性和靈活性大大提高。

本文作者提出基于RGB值和特征點(diǎn)匹配結(jié)合的新型特征識(shí)別定位方法，并設(shè)計(jì)注視逼近的視覺反饋控制策略進(jìn)行抓取。通過ROS機(jī)器人操作系統(tǒng)搭建平臺(tái)進(jìn)行實(shí)驗(yàn)驗(yàn)證。結(jié)果表明：所提出的動(dòng)態(tài)抓取策略對目標(biāo)物體識(shí)別成功率達(dá)到99.6%，抓取成功率達(dá)到98.33%以上，抓取位置偏差在許可范圍內(nèi)，基本滿足工程實(shí)際應(yīng)用。