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

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

1 引言

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

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

2 機器人系統(tǒng)仿真平臺搭建

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

圖1 系統(tǒng)模型

2.1 Kinect V2 RGB-D傳感器

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

2.2 機械臂系統(tǒng)

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

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

2.3 仿真平臺

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

3 路徑規(guī)劃

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

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

圖2 RRT的生長過程圖

圖3 RRT算法偽代碼

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

圖4 RRT-Connect算法偽代碼

4 識別與抓取

4.1 物體識別

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

4.2 物體抓取

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

5 仿真與分析

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

圖5 機械臂仿真模型

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

圖6 點云信息

圖7 RGB圖像

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

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

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

圖9 機械臂關節(jié)角度變化曲線

6 結語

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