基于ROS的機械臂結(jié)合RGBD相機避障運動規(guī)劃

郝 奇，許四祥，江天琦，方建中

（安徽工業(yè)大學(xué) 機械工程學(xué)院，馬鞍山 243000）

0 引言

目前工業(yè)機器人大多數(shù)是人工示教，少部分使用離線仿真程序，兩者都無法避免因真實工作環(huán)境發(fā)生變化而發(fā)生碰撞的問題。而現(xiàn)有的方法，如加裝防護欄和傳感器碰撞檢測法，前者不利于人機交互且占用作業(yè)空間，后者傳感器如關(guān)節(jié)力矩傳感器和皮膚壓力傳感器成本過高且精度有限，而且仍會接觸到障礙物[1]。針對這個問題，有人提出了基于ROS的機械臂避障方法[2]，在此基礎(chǔ)上，作者提出了一種協(xié)作機器人結(jié)合RGBD相機使用PCL點云避障的新方法，并在ROS框架下進行了聯(lián)合避障運動規(guī)劃的研究。

1 ROS避障系統(tǒng)總體設(shè)計

ROS機械臂控制系統(tǒng)基于MoveIt架構(gòu)，包括控制核心MoveGroup類、參數(shù)服務(wù)器接口、控制器接口、Python的moveit_commander接口、傳感器接口和RVIZ插件接口[3～6]。其中參數(shù)服務(wù)器加載機器人的config和xacro機器人配置信息；Position控制器接口與MoveGroup類交換JointTrajectoryAction的關(guān)節(jié)運動數(shù)據(jù)信息；機器人傳感器獲得JointState關(guān)節(jié)信息，便于MoveGroup調(diào)用運動規(guī)劃算法求得正逆運動學(xué)解和生成運動軌跡。

Gazebo獨立于ROS，它能在三維環(huán)境中對多個機器人、傳感器及物體進行仿真，產(chǎn)生實際傳感器反饋和物體之間的物理響應(yīng)[7～9]。本研究在Gazebo中使用RGBD相機，把障礙物深度信息傳遞給MoveIt，并轉(zhuǎn)化為PCL點云信息，通過topic在RVIZ中顯示。MoveIt使用避障規(guī)劃JointTrajectoryAction關(guān)節(jié)運動數(shù)據(jù)控制Gazebo中的機械臂運動。其聯(lián)合控制系統(tǒng)結(jié)構(gòu)如圖1所示。

圖1 MoveIt和Gazebo聯(lián)合控制系統(tǒng)架構(gòu)

2 避障系統(tǒng)配置流程

避障系統(tǒng)配置流程如圖2所示。第一步：構(gòu)建URDF和Xacro，并在其中添加RGBD相機。第二步：使用Setup_Assistant配置MoveIt和Gazebo控制系統(tǒng)功能包，添加統(tǒng)一的控制器和傳感器插件。第三步：配置RGBD相機，測試rgb和depth圖像，配置MoveIt點云插件，測試點云。第四步：結(jié)合RRTConnect算法，啟動聯(lián)合控制系統(tǒng)和Python運動規(guī)劃程序，分析運動規(guī)劃結(jié)果。

圖2 避障系統(tǒng)配置流程

2.1 URDF和Xacro的創(chuàng)建

URDF是統(tǒng)一描述機器人仿真模型設(shè)定的XML語言。主要定義了link和joint，其中joint中包含關(guān)節(jié)名稱、關(guān)節(jié)類型、坐標(biāo)值、子父系連桿和limit信息，而link中包含inertial、collision和visual信息。通過SolidWorks繪制了UR5及工作環(huán)境模型，每個連桿設(shè)定如圖3所示的coordinate坐標(biāo)系和axis旋轉(zhuǎn)軸，使用sw_urdf_exporter插件順序定義關(guān)節(jié)的子父系link、參考坐標(biāo)系、旋轉(zhuǎn)軸和限位參數(shù)，即可導(dǎo)出URDF文件。

圖3 機器人零部件圖

Xacro與URDF都是表示機器人模型的XML語言文件。Xacro使用宏定義，能有效縮減代碼量，還包含機器人在Gazebo物理仿真世界中的仿真環(huán)境、控制器插件和傳感器等其他特性。Xacro基于URDF改編而成，其前兩行代碼指定命名空間，內(nèi)部參數(shù)使用＜xacro:/＞標(biāo)簽進行包含，最后添加transmission傳動標(biāo)簽、gazebo_ros_control插件和RGBD相機插件，保證Gazebo功能及相機的正常使用。

使用的RGBD相機為華碩Xtion_Pro_Live，首先需要在模型文件夾中添加相機的sensors文件夾，然后在Xacro文件中添加如圖4所示代碼：

圖4 代碼1

其中第一個＜xacro:/＞標(biāo)簽內(nèi)包含深度相機的模型和參數(shù)文件。第二個標(biāo)簽定義了相機相對于base_link坐標(biāo)系所處的位置及歐拉角信息。

2.2 MoveIt和Gazebo聯(lián)合系統(tǒng)的建立

MoveIt控制系統(tǒng)功能的實現(xiàn)來源于其配置，在得到合理的Xacro以后，使用Setup_Asssistant來配置moveit和gazebo控制功能包，其具體流程如圖5所示。

圖5 Setup_Assistant配置流程

其中：碰撞矩陣列出不可能發(fā)生碰撞的連桿，以減少運動規(guī)劃碰撞檢測時間。添加由base_link到吸盤gripper_link的arm規(guī)劃組，選擇KDL工具求解正逆運動學(xué)姿態(tài)。

在Moveit和Gazebo包中分別添加相同的軌跡控制器JointTrajectoryController和位置控制器JointPositionController，其中每個關(guān)節(jié)對應(yīng)一個電機位置控制器。

啟動系統(tǒng)后的GUI界面如圖6所示，左邊RVIZ界面顯示模擬仿真環(huán)境，其中綠色為機器人初始工位的位姿，黃色為最終工位的位姿，機器人上方黑色的小長方體為RGBD相機；右邊Gazeb界面顯示真實物理環(huán)境，表示機器人真實所處的位姿及物理環(huán)境；中間是RGBD相機的從Gazebo采集的實時RGB圖像，監(jiān)控機器人運行狀況；左下角終端顯示加載的控制參數(shù)和提示信息。

圖6 機器人系統(tǒng)可視化界面

2.3 RGBD相機和PCL點云的配置

使用的Xtion_Pro_Live相機在正面有三個攝像頭，左邊和右邊分別是IR紅外線發(fā)射器和接收器，用來感測深度值，中間的則是RGB傳感器，用來擷取一般的彩色影像，兩邊的小孔是Audio麥克風(fēng)。如圖7所示。

圖7 RGBD相機

在相機xacro文件中定義camera參數(shù)和發(fā)布圖像話題的插件。首先定義相機工作角度為58°，圖像格式為RGB8，分辨率為真實相機分辨率249×329，工作范圍為0.05～4m。最后定義相機發(fā)布rgb圖像和depth圖像的topic信息插件，代碼如圖8所示。

圖8 代碼2

啟動聯(lián)合避障系統(tǒng)，使用rostopic list命令列出發(fā)布的話題，出現(xiàn)rgbd_camera相關(guān)的topic就說明相機已經(jīng)正常運行[9～11]。通過終端spawn_model命令在Gazebo的工作臺上添加兩個可樂瓶模型，在新終端中輸入rosrun image_view和topic命令顯示相機的rgb和depth深度圖像，如圖9所示。

圖9 rgb圖像和depth深度圖像

PCL點云環(huán)境是三維重建的過程，需要得到RBGD圖像每個像素點的三維坐標(biāo)。RGBD相機掃描得到的每一幀數(shù)據(jù)是由彩色RGB圖像和灰色的depth圖像組成[10,11]?；疑膁epth圖像每個像素點包含一個灰度值，這個值是PCL點到深度相機所在垂直平面的距離，這個值Z通過結(jié)構(gòu)光編碼技術(shù)測得。像素坐標(biāo)（u，v）和世界坐標(biāo)（X，Y，Z）的映射關(guān)系如圖10所示。

圖10 像素坐標(biāo)和世界坐標(biāo)映射圖

由相似原理：

PCL點的三維坐標(biāo)：

其中：fx和fy為像素表示的焦距長；cx和cy為圖像坐標(biāo)系原點在像素坐標(biāo)系的坐標(biāo)。

在MoveIt傳感器插件中定義PointCloudOctomapUpdater插件，相機圖像的像素點在RVIZ環(huán)境中進行三維重建，顯示為PCL點云。定義插件訂閱的點云話/rgbd_camera/depth/points，發(fā)布的點云話題為output_cloud。重新啟動系統(tǒng)，如圖11所示。右邊Gazebo相機視野下的兩個可樂瓶在左邊MoveIt的可視化插件RVIZ中轉(zhuǎn)化為綠色的點云圖。

圖11 Rviz可視化和Gazebo物理世界仿真

3 RRTConnect算法結(jié)合PCL避障規(guī)劃實現(xiàn)

3.1 RRTConnect避障規(guī)劃算法

ROS中的OMPL開源運動規(guī)劃庫是采樣規(guī)劃算法庫，相比傳統(tǒng)規(guī)劃算法需要在一個確定空間對障礙物進行建模，采樣規(guī)劃算法適合解決多自由度機器人在復(fù)雜環(huán)境的規(guī)劃[12,13]。原始RRT算法只有一棵樹從初始點擴展搜索整個狀態(tài)空間，本文使用的RRTConnect算法稱為連結(jié)型雙樹RRT算法，是從初始點和目標(biāo)點同時生長兩棵RRT樹，能夠快速有效地搜索狀態(tài)空間。RRTConnect算法流程如圖12所示。

圖12 代碼3

RRTConnect算法構(gòu)建過程：首先對起點和目標(biāo)點的兩棵樹初始化，然后建立循環(huán)，隨機拋出一個點Prand，第一棵樹T1計算最近點Pnearst，最近點沿著Prand擴展一個步長ε，沒有碰撞則得到新節(jié)點Pnew。新節(jié)點作為第二棵樹T2擴展的方向，擴展一個步長得到P'new，沒有碰撞則同向擴展一次；每次迭代后進行比較，選擇節(jié)點少的樹擴展；經(jīng)過N次迭代，直到P'new=Pnew，兩棵樹連結(jié)在一起，整個算法結(jié)束。

3.2 RRTConnect算法結(jié)合PCL的避障規(guī)劃

啟動MoveIt和Gazebo聯(lián)合避障系統(tǒng)，運行 Python運動規(guī)劃程序，UR5末端執(zhí)行器進行左工位至右工位的機器人取放操作任務(wù)，在無障礙物的環(huán)境中運動規(guī)劃路徑如圖13所示。

圖13 MoveIt無障礙運動軌跡

終端使用spawn_model命令，在Gazebo中添加障礙物遮擋原機器人工作路徑，通過RGBD相機，MoveIt的RVIZ立刻建立障礙物面向相機兩個面的PCL點云模型,結(jié)合RRTConnect算法，機器人末端執(zhí)行器吸盤繞過障礙物從左工位移動到右工位。如圖14所示，左邊的RVIZ顯示運動規(guī)劃路徑；左下角終端提示算法用時0.056s計算出規(guī)劃路徑；右邊的Gazebo環(huán)境和RGB圖顯示真實環(huán)境機器人最終到達的狀態(tài)。

圖14 MoveIt避障規(guī)劃軌跡及Gazebo結(jié)果

終端使用rqt_plot命令，添加/joint_states話題數(shù)據(jù)，實時監(jiān)控機器人各個關(guān)節(jié)角度隨時間變化曲線，如圖15所示，避障運動過程用時約4.5s，機器人6個關(guān)節(jié)在避障工作下能實現(xiàn)平滑的軌跡過渡。

圖15 機器人各關(guān)節(jié)角度隨時間變化曲線

4 結(jié)束語

通過在ROS中構(gòu)建URDF和Xacro模型、添加RGBD相機、設(shè)計配置MoveIt和Gazebo聯(lián)合控制系統(tǒng)、配置RGBD相機及點云、結(jié)合RRTConnect算法進行避障規(guī)劃。通過上述工作流程，在ROS中實現(xiàn)了機器人在工作環(huán)境發(fā)生變化的情況下，能立刻避開障礙物平穩(wěn)運行。結(jié)果表明，該避障系統(tǒng)便捷有效，機械臂實現(xiàn)了在障礙物環(huán)境中自動避障，解決了離線仿真機械臂因環(huán)境變化而出現(xiàn)的碰撞問題，對工業(yè)機器人在ROS下的應(yīng)用研究有著重要的科學(xué)意義。