ROS環境下機械臂物體抓取技術研究*

王晶航 韓江桂 張文群 孫颙琰

（海軍工程大學動力工程學院 武漢 430033）

1 引言

機械臂的自主抓取大多是基于視覺的控制[1～2]。當前，對于復雜環境的非結構化，再加上被抓取模型的不確定性，使得機械臂的自主抓取難度大大提升[3]。解決這一問題的首要任務就是要解決機械臂自身的感知系統或者借助于外部設備實現機械臂的感知。微軟研制的Kinect V2 RGB-D傳感器能夠提供二維和深度圖像的信息，因此可以運用Kinect V2來進行研究[4]。當前，國內外不少學者對機械臂自主抓取進行分析研究，叢明等[5]利用機器視覺進行定位導航（SLAM）[6～7]；Kriminger[8]等利用Online active learning進行自主的目標檢測[9]；針對機械臂在非結構化的動態環境，Yuan，C[10]等提出了EBG-RRT算法高效地實現了路徑重規劃[11～12]。

本文借鑒了Kinect V2視覺傳感器在機器人系統中的應用，利用其能夠同時提供二維圖像及深度信息的特性，簡化了目標檢測的內容，并通過RRT-Connect算法，完成機械臂的運動軌跡規劃，從而最終實現了機械臂系統自主完成目標抓取的功能。本文在ROS環境下，將機械臂的仿真模型建立在Rviz和Gazebo環境中，最后完成Rviz和Gazebo聯合仿真的實驗，對機械臂的物體識別和抓取技術進行研究。

2 機器人系統仿真平臺搭建

本文的機器人系統是由Kinect V2 RGB-D傳感器、機械臂系統及主控計算機組成[12]，其中機械臂系統又包括機械臂AUBO-i5和Robotiq兩指夾爪。整個系統的模型如圖1所示。

圖1 系統模型

2.1 Kinect V2 RGB-D傳感器

系統中的視覺傳感器為微軟公司研發的Kinect V2 RGB-D傳感器。Kinect V2 RGB-D傳感器主要由一個普通的RGB攝像頭、一個紅外發射器、一個紅外攝像頭以及一組麥克風陣列組成[13]。普通RGB攝像機能夠以30幀/s的速率傳送640×480分辨率的RGB圖像，而紅外發射器和攝像機則是用來采集相應的深度信息，可以得到320×240分辨率的深度圖像。

2.2 機械臂系統

AUBO-i5協作機器臂是一款高品質、低成本的6自由度協作輕型機械臂。AUBO-i5協作機器臂本體靈活、輕便，可以使用開源的ROS接口對機械臂本體實現運動控制并在基礎上進行開發研究。

為了實現機械臂目標抓取，本實驗采用Robotiq兩指電動夾爪，夾取穩定，而且被夾持物件不容易滑落。同時，該夾爪支持ROS，能夠與AUBO-i5機械臂相互配合實現抓取過程。

2.3 仿真平臺

ROS全稱是Robot Operating System（機器人操作系統），集成了很多的工具、庫等，包含三維可視化功能包Rviz、機械臂運動控制功能包Moveit、物理仿真器Gazebo等開發工具[14]。Gazebo是一個三維動態物理仿真器，能夠建立測試機器人的仿真場景，通過添加物體庫，用來模仿人類世界。同時，能夠提供機器人在仿真過程中所需的靜力學、動力學環境，從而可以觀察研究機械臂在真實環境下的工作狀況[15]。

3 路徑規劃

機械臂的路徑規劃就是從起始點到目標點的過程中，機械臂能夠按照一定的算法策略，規劃出其路徑，完成這一過程。本文我們選用隨機采樣算法，目前應用最廣的隨機采樣算法主要有概率路標圖法 PRM（Probabilistic Roadmap Methods）和快速擴展隨機樹法RRT（Rapidly-exploring Random Tree）[16]。PRM參數少，結構相對簡單，但是采樣過程中采樣點大多會偏離目標位置，使得采樣過程中計算量變大，路徑規劃效率降低。因此選擇RRT算法。

RRT是一種通過概率遍歷全圖，在高維空間便于搜索的算法，圖2為RRT的生長過程圖。算法策略是給定一個起始點qstart，將qstart存儲到qnodes中，在空間中全地圖隨機采樣點qrand，尋找qnodes到達qrand中最近的一個點為qnear，在qnear到qrand的方向上以一定的步長δ前進至qnew，在此過程中進行碰撞檢測，若未檢測到碰撞，則將qnew存儲到qnodes中，反之檢測到碰撞，則進行重新采樣重復上述過程。當 |qnew-qgoal|＜Error視為迅即到目標點，將qnew存入到qnodes中。最后在qnodes根據各節點的父子關系，反向搜索找到規劃路徑。圖3為RRT算法的偽代碼。函數中有三個返回值：“Advanced”代表搜索到新的節點但不知其是否在目標點誤差區間附近；“Reached”代表新的節點到達目標節點的誤差區間內，即完成路徑規劃；“Trapped”代表在擴展過程中發生碰撞，擴展失敗；“Graph”代表生成搜索樹路徑圖。

圖2 RRT的生長過程圖

圖3 RRT算法偽代碼

RRT算法對于機械臂路徑規劃來講，有些路徑不夠光滑，可能會包含棱角，通常也遠離最優路徑，效率較低。所以本文采用RRT-Connect算法，偽代碼如圖4。在起始狀態點qstart和目標狀態點qgoal同時生長兩顆快速擴展隨機樹tree1和tree2，這樣擴展效率更高。搜索空間這兩個擴展的方式與基礎的RRT算法一樣，是基于整個空間隨機采樣進行擴展，先擴展完tree1的第一個節點qnew.1，隨后第二棵樹tree2朝著qnew.1的方向進行擴展，得到qnew.2，在這個過程中，若沒有碰撞，則進行下一步擴展，并一直重復上述過程。直到發生碰撞擴展失敗或者 |qnew.1-qnew.2|＜Error實現兩棵樹連接起來，即RRT-Connect算法實現。

圖4 RRT-Connect算法偽代碼

4 識別與抓取

4.1 物體識別

從Kinect V2傳感器獲取圖像，標定機械臂各關節，經過世界坐標系與圖像坐標系的轉換，得到關節點在空間中的坐標，通過幾何運動學解算出此時各關節角的角度；其次，計算目標角度與當前角度的誤差，使用反饋控制來減小誤差[17]；最后，由于本文是以Rviz與Gazebo聯合仿真實驗，所以可以不必進行Kinect V2內參和外參的標定。對于物體識別的過程，采用find_object_2d快速實現物體檢測，識別物體后能夠通過KinectV2傳感器獲得物體在空間的位置，并將返回值賦給機械臂末端夾爪。

4.2 物體抓取

在4.1節物體識別的過程中，已經得到機械臂各關節的坐標和待抓取物體的空間位置，對不同顏色的物品進行分類放置于不同的盒子中，再使用Moveit中的 KDL（Kinematics and Dynamics Library）插件對整個過程進行逆運動學求解，完成運動規劃及避障，從而實現物體的識別與抓取，即實現了非結構環境下的物體抓取過程。

5 仿真與分析

統一機器人描述格式文件URDF（Unified Robot Description Format）是ROS中通用的機器人描述文件，能夠描述機器人的結構，主要由連桿（Link）和關節（Joint）兩種標簽構成，連桿標簽用于描述機器人連桿的幾何特性、運動學特性以及動力學特性，關節標簽則用于描述機器人關節的相關特性。同時，這兩個標簽還有描述幾何信息和物理信息的子標簽。在建立好機械臂的URDF文件后，將URDF文件在Rviz中打開，建立的機械臂仿真模型如圖5所示。

圖5 機械臂仿真模型

為了驗證機械臂在非結構環境下目標識別及抓取的有效性。在ROS環境下，采用Rviz與Gazebo聯合仿真。以Aubo-i5協作機械臂為研究對象，通過Kinect V2傳感器來采集環境信息，在Gazebo中設置抓取環境。在Rviz中顯示的點云信息如圖6（a），當物體被抓取離開攝像機視野后如圖6（b），Rviz實時檢測更新環境。同時Kinect V2也可以向Rviz返回RGB圖像信息并利用如圖7所示。

圖6 點云信息

圖7 RGB圖像

圖8為Gazebo仿真器中機械臂在抓取過程中的位姿變化。其中圖8（a）是機械臂的零點位置，設為抓取過程的起始點和終止點。圖8（b）顯示機械臂正在抓取桌上物體，并逐個將其分配到盒子中如圖8（c）。圖8（d）完成將物體分開放置在兩個盒子中回到零點位置。

圖8 Gazebo仿真器中機械臂位姿變化

在機械臂一次的運動抓取過程中，通過訂閱6個關節的角位置話題，6個關節的角度變化如圖9所示。機械臂通過Kinect V2傳感器識別后對目標進行規劃抓取，按照RRT-Connect算法規劃得到的路徑，能夠成功地將物體放置在盒子中。從起始零點位置到抓取結束回到終止零點位置過程中，機械臂能夠平穩運行，各個關節能實現平滑的軌跡過渡且角度未發生突變。

圖9 機械臂關節角度變化曲線

6 結語

本文在ROS環境下搭建了機械臂識別抓取仿真系統，采用KinectV2 RGB-D傳感器來采集環境信息，通過Rviz和Gazebo聯合仿真，從而實現目標抓取，運用RRT-Connect算法對機械臂進行路徑規劃，Gazebo來模擬機器人在真實條件下的運動，驗證了機械臂在非結構環境下路徑規劃的有效性。