機器視覺主導的機械臂動態抓取策略研究

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

(華東理工大學機械與動力工程學院,上海 200237)

0 前言

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

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

本文作者基于ROS框架搭建基于視覺反饋的UR5機械臂動態抓取平臺，實現對目標物體的快速識別，并通過視覺測距的方式計算機械臂末端偏移量。設計注視逼近的視覺反饋控制策略，實現機械臂對傳送帶上移動目標的抓取操作。

1 實驗環境搭建

基于ROS機器人操作系統免費且開源、代碼易擴展等優點，本文作者在Ubuntu18.04上利用ROS-Melodic搭建動態抓取平臺，核心部件包括：傳輸帶、RealSense D415攝像頭、UR5機械臂等。攝像頭采用Eye-in-Hand方式與機械臂末端連接，用于獲取執行端視野。在傳動帶運行方向側方，固定UR5機械臂，如圖1所示。

圖1 UR5機械臂動態抓取系統

1.1 機械臂配置

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

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

圖2 機械臂配置流程

1.2 機械臂運動學分析

對UR5機械臂進行運動學分析，圖3所示為UR5機械臂結構圖。

圖3 UR5機械臂結構圖

機械臂正運動學問題是從基坐標依次通過坐標變換的方式求取末端執行器的位姿的過程。UR5機械臂模型的D-H坐標系表達各運動關節間的位姿變換關系，其D-H參數如表1所示。

表1 UR5機械臂D-H參數

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

(1)

式中:c=cos；s=sin；c=cos；s=sin，由此可得UR5機械臂的坐標轉換公式:

(2)

式中:、、表示末端執行器相對于基坐標的姿態；表示末端執行器相對于基坐標的位置。

2 目標識別及空間定位

2.1 目標識別

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

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

|-′|<

(3)

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

(4)

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

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

通過預判斷的SITF策略明確了目標的形狀特征，然后再通過對目標的顏色特征進行二次匹配來提高匹配精度。

2.2 目標空間定位

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

圖4 單目視覺測距原理

圖4中,攝像頭透鏡光心為相機坐標系原點，軸為攝像機的光軸；、′平面分別為攝像機采集圖像平面和成像平面；′與軸線交于點，以點(，)在′平面建立圖像直角坐標系，、分別表示行和列。通過對目標物體顏色空間的識別，確認在平面中目標水平方向中心點。′為中心點在′中投影點，在′平面以圖像直角坐標系表示。

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

(5)

其中:、分別為軸和軸的有效焦距；、分別為攝像機內參數和外參數；為旋轉向量；為平移向量。

假定世界坐標系位于平面內，即點的世界坐標(,,0)，=，=[0 0]，式(5)化為

(6)

將平面內矩形沿方向分為份，將第份面積記為，在′上的相應投影面積記為′，則總面積為

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

(7)

即：

(8)

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

如圖5所示，光心與點的連線與′平面的交點，即光心到成像平面的距離,為目標中心到成像平面的距離,可得：

圖5 x向測量示意

(9)

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

(10)

聯合公式(7)可求得點在相機坐標系下的三維空間位置信息：

(11)

綜上，求得平面中點相對攝像機光心的、、坐標方向的偏移量。

3 逼近抓取

3.1 逼近策略

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

圖6 視覺反饋控制原理

圖7 機械臂抓取流程

啟動UR5機械臂，初始化所有數據和機械臂動作，等待目標進入攝像機視野，若檢測到目標存在，則計算各方向誤差。

當誤差不在允許范圍內時，利用MoveIt運行文中提出的融合算法進行軌跡規劃，控制機械臂運動彌補誤差。一次規劃完成后，檢測圖像并再次對誤差進行計算和判斷，進行下一次循環。若誤差在允許范圍內，則控制機械臂進行抓取動作。

3.2 抓取流程

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

圖8 Gazebo動態抓取工作示意

4 實驗分析

為測試融合算法的處理效果，利用上述方法在UR5動態抓取平臺上設計了兩組性能測試實驗：視覺抓取成功率測試和抓取偏移量測試。測試現場如圖9所示。

圖9 動態抓取工作測試現場

根據速度的不同共設置6組自主抓取實驗，每次實驗中目標方塊運動前在初始位置隨機擺放，每組實驗進行100次完整抓取,結果如表2所示。可知：隨著傳送帶前進速度加快，抓取成功率逐步降低，當傳輸速度大于等于12 mm/s時，出現漏抓即未成功識別情況。總體目標物體識別成功率達到99.6%，動態逼近抓取成功率達到98.33%以上，基本滿足工業應用要求。

表2 動態抓取測試結果

此外，考慮到抓取位置精度問題，在上述速度為0、3、9、15 mm/s的實驗中各隨機選取20次，目標坐標系軸正方向為運動方向，機械臂末端實際夾取位置與目標中心位置的軸相對偏差如圖10所示。可知:機械臂逼近抓取實際位置與預期位置的相對誤差隨速度增加而增大，抓取位置向軸負方向偏移，誤差在許可范圍內，驗證了抓取算法的精確性。

圖10 抓取位置相對誤差

5 結論

針對傳統工業機器人在移動目標的抓取中表現出的適應性低、柔性差等缺點，提出基于視覺反饋的動態目標抓取方法。設計一套完整的動態抓取策略，使得機器人在移動目標抓取中的自主性和靈活性大大提高。

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