ROS環(huán)境下機(jī)械臂物體抓取技術(shù)研究*

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

（海軍工程大學(xué)動(dòng)力工程學(xué)院 武漢 430033）

1 引言

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

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

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

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

圖1 系統(tǒng)模型

2.1 Kinect V2 RGB-D傳感器

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

2.2 機(jī)械臂系統(tǒng)

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

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

2.3 仿真平臺(tái)

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

3 路徑規(guī)劃

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

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

圖2 RRT的生長過程圖

圖3 RRT算法偽代碼

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

圖4 RRT-Connect算法偽代碼

4 識(shí)別與抓取

4.1 物體識(shí)別

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

4.2 物體抓取

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

5 仿真與分析

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

圖5 機(jī)械臂仿真模型

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

圖6 點(diǎn)云信息

圖7 RGB圖像

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

圖8 Gazebo仿真器中機(jī)械臂位姿變化

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

圖9 機(jī)械臂關(guān)節(jié)角度變化曲線

6 結(jié)語

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