基于雙目視覺定位的機械臂智能抓取控制研究

陳晗彬

（福建省特種設備檢驗研究院國家特種機器人產品質量檢驗檢測中心（福建），福州 350028）

0 引言

近年來，隨著互聯網、人工智能、智能制造的迅速發展，再加上“中國制造2025”發展戰略的推出，傳統的通過單一示教執行單一指令的機器人已經難以滿足復雜的生產需求，越來越多的機器人廠家希望機器人能夠適應更加復雜的生產環境，例如在機械臂需要抓取物體大小、位置、形狀發生改變時，會出現丟失目標無法抓取等問題，需要人為進行干預[1-2]。為了讓工業機器人更加智能、高效地適應工業生產的絕大部分應用場景，應用基于雙目視覺的機器人是一個很好的解決方案。使用雙目視覺相機作為其外部傳感器，賦予其感知能力，不僅可以識別二維圖像信息，還可以通過三維點云圖獲得物體的輪廓和位置信息，在視覺識別方面有著廣泛應用[3-4]。利用雙目視覺傳感器設備在復雜環境下識別待抓取物體，并獲得物體的尺寸、位置屬性和周圍環境信息，再由ROS 機器人操作系統進行信息處理、判斷和模擬仿真確定機械臂的行徑軌跡，最后與機械臂進行數據溝通實現現實環境下的抓取操作[5-6]。這樣機械臂就被賦予了判斷和決策的能力，變得更加智能化、自主化[7-8]。

本文主要對基于雙目視覺的機械臂進行識別定位及抓取控制研究，主要包括機器視覺的手眼標定、機械臂D-H 方法描述，ROS 系統上的模擬仿真、現實環境下的識別抓取實驗等內容，為后續機械臂能更好地適應復雜工作環境以及為產業化發展提供理論參考[9]。

1 雙目視覺定位原理介紹

1.1 雙目視覺相機介紹

為在視覺方面獲得更加豐富和清晰的信息，采用的視覺輸入設備為Intel RealSense D435 雙目深度相機，其主體外觀和各相機功能位置如圖1所示。

圖1 Intel RealSense D435雙目深度相機

該相機具有全局快門，可以捕捉快速移動的物體，雙目視覺分辨率最高為1 280 像素×720 像素、RGB 分辨率最高為1 920像素×1 080像素、相機的深度識別距離為0.2～10 m，而且相機內置了一個6 自由度的IMU 單元，可檢測剛體在三維空間中的前后左右上下平移，以及旋轉、俯仰、搖擺等動作。

1.2 眼在手上相機標定方法介紹

在實際的視覺定位抓取實驗中，雙目視覺相機在獲得目標的坐標位置后，通過手眼標定設置好的轉化矩陣將物體在相機中的坐標轉化到機械臂的坐標系中，然后根據機械臂空間坐標系物體所在的位置進行路徑規劃得出每個關節的運動情況，從而控制機械臂完成抓取操作[10]。手眼標定是確定像素坐標系和空間機械臂坐標系的坐標轉化關系，標定的精度嚴重影響機械臂抓取操作的準確程度。

眼在手上（Eye-in-hand）的手眼標定方法指的是將相機固定在機械臂的末端夾具上進行標定，目的是找到相機與機械手末端的坐標關系。手眼標定方式如圖2所示。

圖2 眼在手上標定方式示意

在求解過程中，會使用到機械臂基坐標系（base）、機械手坐標系（tool）、相機坐標系（cam）、標定板坐標系（cal）。首先需要使用齊次變換矩陣來表示在標定過程中不變的坐標系間的相互關系量：相機與夾具坐標系的轉換關系、標定板與機械臂基坐標系轉化關系，和2 個變換但可求解已知的關系量：夾具與機械臂基坐標系的轉化關系、相機與標定板坐標系的轉化關系。其中，相機與機械臂末端的坐標關系為手眼標定最終想要求解得到的未知量[11]。

在經過機械臂多次的姿態變換后，根據以上公式可得式（2）～（3）：

式（6）～（8）可化簡為式（9）：

可設兩次機械臂運動的夾具與機械臂基座之間的位姿變換矩陣的乘積為A、相機與標定板之間的位姿變換矩陣乘積為B，所以可得式（10）：

最終簡化為一般表達式（11）完成求解。

式（11）一般采用李群李代數方法替換轉換為線性方程，運用“兩步法”先對旋轉矩陣進行求解，再得平移向量[6]，或者采用時對偶四元數，用對偶四元數表達旋轉和平移，進行統一求解，最終求解出X。求解完成后可以得到相機與機械臂基坐標系的變換矩陣關系[12-14]。

1.3 相機安裝方式選擇

本文綜合考慮到以下幾點原因選擇相機的安裝方式。

（1）眼在手外的相機視野單一，在復雜情況下容易被遮擋導致識別失敗，相機在手上可以隨著機械臂末端的移動改變視野位置從而避開遮擋進行識別。

（2）如果想要為機械臂加上移動底盤，讓機械臂能夠在不同地點進行移動，眼在手上的相機可以隨著機械臂一起移動，其識別面積就不像眼在手外僅有一個區域。

（3）考慮到外界移動碰觸的因素，在實驗室的環境下，眼在手外的相機很容易被移動導致需要重新標定，而眼在手上的相機只需標定一次就可以一直使用。

最終選擇將雙目視覺相機放在機械臂末端，選用眼在手上的手眼標定方式，相機的安裝方式如圖3所示。

圖3 雙目視覺相機安裝方式

2 ROS仿真平臺介紹

利用ROS 系統在進行機器人開發時，最大的一個優點就是ROS 擁有豐富的功能包，在對機械臂運動學仿真時，ROS 提供了幾類常用工具，如Rviz、Moviet！、Rqt、Gazebo 等。Gazebo 和Moveit！提供了仿真功能的實現、Rviz 和rqt提供了調試與可視化功能。對于機器人開發而言，仿真是機器人控制的基礎，因為機器人造價高昂，為降低機器人學習、開發、調試的成本，需要通過仿真來完成大量前期工作。進行模擬仿真時，首先要對機械臂進行建模，描述和表達出機械臂的關節和姿態。在ROS 中，主要使用urdf 文件和xacro 文件對機械臂模型進行描述，這2 種文件都采用了XML 文件格式，可以直觀地通過各類標簽了解機械臂的相關描述屬性，直接閱讀也十分方便。機器人模型文件主要根據D-H 參數法[15-16]進行建模，在urdf 文件中，使用＜link＞標簽格式描述機器人上連桿的大小、顏色、碰撞參數等外觀和物理屬性；使用＜joint＞標簽格式描述2 個連桿之間的運動學和動力學屬性；使用＜robot＞標簽格式描述整個機器人。這3 類標簽組成了完整的機器人模型描述[17]。

3 機械臂智能抓取仿真試驗

3.1 攝像頭手眼標定

在給機械臂添加視覺模塊時，為確定相機位置，調高機械臂抓取精準度，需要對相機進行手眼標定。標定過程采用了camera＿calibration 功能包，功能包使用棋盤格圖案作為標定靶，在標定過程中標定靶與機械臂基座固定，機械臂末端上的相機會對棋盤格進行拍照，通過檢測棋盤格中的邊角識別坐標，之后機械臂各關節會進行轉動，調節末端位置，但每次移動末端相機視角均會指向標定靶[18]。

功能包標定界面如圖4所示。界面中x、y、Size表示標定時相機的左右、上下和前后移動，Skew 表示相機傾斜轉動，在右上角的進度全部完成后，就可以得到標定結果，最后將數據結果進行發布，等待ROS調用即可。

圖4 標定功能包界面

3.2 自主物體識別定位

視覺部分將采用OpenCV 開源庫實現圖像的處理工作，使用OpenCV 庫可以更簡便高效地完成視覺方面的應用開發，在OpenCV 與ROS 進行數據傳輸時需要使用cv＿bridge 功能包將OpenCV 和ROS 的圖像數據相互轉化和通信，OpenCV與ROS的集成框架[19]如圖5所示。

圖5 ROS與OpenCV集成框架

OpenCV 開源庫的使用使相機能夠進行基礎的圖像識別和ROS 通信，但這些功能遠遠不能滿足精準的物體識別與物體定位功能。為實現這些功能，采用ROS 的一個強大的物體識別框架——ORK 功能包，功能包提供了模型訓練和模型數據庫配置的工具，以及多種物體識別的算法。ORK 兼容性良好，識別精準率高，使用較為簡單，是很好的三維識別定位軟件。

ORK 功能包進行物體識別的算法思路是模板匹配，首先需要建立待識別的物體三維模型，接著在數據庫中對模型進行數據訓練，生成匹配模板，訓練完成后就可以在ROS 中根據模板進行三維物體識別，識別效果如圖6 所示。由圖可知，相機已經識別出了物體，并用白色區域將可樂罐的面積覆蓋，并在可樂罐處建立坐標系，這樣識別物體的位置和姿態都可以獲得，最終識別到的物體的位置信息和模板匹配信息會在終端打印并進行發布，為之后抓取做好準備。

圖6 三維物體識別效果

3.3 機械臂障礙規避

機械臂在環境復雜的情況下進行抓取，單單只識別到物體是不夠的，如果忽視了障礙物，會使得機械臂產生碰撞造成危險。為了讓機械臂能夠更加智能地避開障礙物，需要使用雙目視覺相機對障礙物生成點云信息。

待抓取物體周圍環境的點云信息可以由雙目視覺相機獲取，獲得的清晰點云圖信息如圖7所示。在ROS中，實驗使用MoveIt！的OctoMap 插件將雙目視覺相機識別到的點云圖環境信息以柵格化的形式生成八叉樹（Octree）去描述，這樣大大減輕了系統對環境信息處理的負擔，可以更快知道物體在場景中的位置。最后，使用點云圖完成MoveIt！中的障礙物建立后，需要將待抓取物體進行標記以忽略其作為障礙物的識別，在Rviz 可視化窗口顯示的障礙物識別情況如圖7所示，圖中紅色的物料為待抓取的物體，其點云圖并不會被生成為八叉樹，而兩邊的水杯以及桌面就會被識別為障礙物，以柵格化的形式標注障礙物體積，等機械臂進行抓取規劃時，會使用RRT路徑規劃算法來計算能夠抓取物體且可以完成避障的路徑[20]。

圖7 障礙物識別情況

在ROS 系統中MoveIt！的運動規劃庫OMPL 支持避障規劃。由如圖8 所示的機械臂末端路徑曲線可知，在機械臂的仿真環境中可以在機械臂的行進路徑中添加一個障礙物，之后進行路徑規劃，此時機械臂會繞過添加的障礙物到達目標點位，機械臂即完成避障規劃。

圖8 機械臂障礙物規避規劃

3.4 機械臂控制連接

實際機器人控制在ROS 端的環境配置與聯合仿真相類似，而在實時系統的控制、與ROS 的通信和開發的接口方面，每一家機器人公司的配置都不盡相同。本次實驗的機械臂驅動包使用HSR—Co605 協作機器人配套的HIROP 軟件包，該軟件包集成了完善的通信框架和功能模塊，能實現機器人硬件和ROS軟件層的搭建。

配置完成軟件包，即可以在終端中輸入指令完成機械臂的連接與控制工作，通過HIROP 提供的各類函數庫和功能包文件，自定義特有的視覺抓取應用，可以執行的功能有識別牛奶盒、構建清除點云、抓取目標、放置目標、移動到目標點、打開合上夾爪等，通過整合單獨功能，機械臂最終可在現實情況下完成基于視覺的抓取操作，如圖9所示。

圖9 機械臂最終完成視覺抓取

圖10 識別抓取流程

3.5 智能識別抓取實驗

視覺機械臂實現識別抓取的流程圖如10所示。為保證視覺機械臂可以在不同環境下進行有效識別抓取，設計實驗對視覺機械臂進行測試。圖11所示為識別抓取實驗場景，場景1：待抓取物體牛奶盒被放置在高度適中的桌面，距離機械臂距離適中且周圍沒有障礙物遮擋；場景2：待識別物體離機械臂距離較近周圍沒有障礙物遮擋；場景3：待識別物體與機械臂距離適中，周圍有較少障礙物，物體正前無障礙物遮擋；場景4：待識別物體與機械臂距離適中，周圍有較多障礙物，物體正前有障礙物遮擋。

圖11 視覺抓取實驗場景

將實驗所得數據和結果匯總至表1，表中（X，Y，Z）表示物體實際底部中心點坐標，（X’，Y’，Z’）表示視覺系統識別到的坐標，坐標單位為mm。

表1 不同場景下識別抓取實驗

經過相同場景的多次試驗后，對實驗現象和結果進行分析，結論如下。

場景1：雙目相機可以識別到物體，且完成桌面的點云圖構建，機械臂完成桌面避障，對牛奶盒進行抓取，驗證了視覺機械臂識別抓取功能的完善性和穩定性。

場景2：雙目視覺相機在多次實驗中未完成對牛奶盒的識別，通過數據分析可知，相機在此視野下只能識別到牛奶盒的頂端一小部分，面對這樣的識別視野，ORK 沒有足夠的信息，不足以進行模板匹配，所以識別失敗。對此，可以使機械臂轉動，改變相機的識別姿態，從而完成復雜角度的識別。

場景3：在較為復雜的環境下，相機可以成功完成識別定位，機械臂在進行抓取操作時，夾爪會夾住牛奶盒的上半部分，表示已成功完成障礙物識別和規避，保證了機械臂成功完成抓取，實驗驗證了在復雜情況下機械臂對物體位姿狀態的識別和障礙物規避功能的可行性。

場景4：在識別物體下半部分有遮擋這種更加復雜的環境下，相機也可以完成識別，由識別坐標可知，坐標系被建立在物體上方，避免了在抓取時發生碰撞的危險。但是機械臂沒有完成運動規劃，通過錯誤排查得知，由于障礙物識別生成的柵格化八叉樹像素體積過大，使得抓取行徑路線完全被遮擋，導致運動規劃失敗。對此，可以使用算力更強的顯卡，以生成高像素八叉樹，使得障礙物的體積描述更加準確，從而面對理論上可以避障的環境時不會受障礙物識別的影響。

4 結束語

本文基于雙目視覺相機和HSR-Co605 協作機械臂以及ROS仿真平臺對智能抓取控制進行了仿真研究和試驗。

（1）在ROS 仿真平臺中完成模擬仿真代碼的編寫，實現機械臂仿真情況下的碰撞檢測、運動規劃和軌跡檢測并驗證了其可行性。

（2）對雙目視覺定位技術進行了試驗研究，使用手眼標定建立相機和機械臂之間的坐標聯系，使用Cvbridge 和ORK 功能包對物體進行識別、定位以及姿態顯示并實現各部分配置和連接。

（3）在不同場景下進行了視覺抓取實驗，特別是目標相對于相機太近時，需要轉動機械臂，改變相機的識別姿態才能更好地完成目標識別。在其他具有障礙物遮攔的場景下，距離適中時，也能很好地完成目標識別任務。通過大量的試驗驗證了這一技術的可行性。