基于ROS的機械臂結合RGBD相機避障運動規劃

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

（安徽工業大學 機械工程學院，馬鞍山 243000）

0 引言

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

1 ROS避障系統總體設計

ROS機械臂控制系統基于MoveIt架構，包括控制核心MoveGroup類、參數服務器接口、控制器接口、Python的moveit_commander接口、傳感器接口和RVIZ插件接口[3～6]。其中參數服務器加載機器人的config和xacro機器人配置信息；Position控制器接口與MoveGroup類交換JointTrajectoryAction的關節運動數據信息；機器人傳感器獲得JointState關節信息，便于MoveGroup調用運動規劃算法求得正逆運動學解和生成運動軌跡。

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

圖1 MoveIt和Gazebo聯合控制系統架構

2 避障系統配置流程

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

圖2 避障系統配置流程

2.1 URDF和Xacro的創建

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

圖3 機器人零部件圖

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

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

圖4 代碼1

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

2.2 MoveIt和Gazebo聯合系統的建立

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

圖5 Setup_Assistant配置流程

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

在Moveit和Gazebo包中分別添加相同的軌跡控制器JointTrajectoryController和位置控制器JointPositionController，其中每個關節對應一個電機位置控制器。

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

圖6 機器人系統可視化界面

2.3 RGBD相機和PCL點云的配置

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

圖7 RGBD相機

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

圖8 代碼2

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

圖9 rgb圖像和depth深度圖像

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

圖10 像素坐標和世界坐標映射圖

由相似原理：

PCL點的三維坐標：

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

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

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

3 RRTConnect算法結合PCL避障規劃實現

3.1 RRTConnect避障規劃算法

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

圖12 代碼3

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

3.2 RRTConnect算法結合PCL的避障規劃

啟動MoveIt和Gazebo聯合避障系統，運行 Python運動規劃程序，UR5末端執行器進行左工位至右工位的機器人取放操作任務，在無障礙物的環境中運動規劃路徑如圖13所示。

圖13 MoveIt無障礙運動軌跡

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

圖14 MoveIt避障規劃軌跡及Gazebo結果

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

圖15 機器人各關節角度隨時間變化曲線

4 結束語

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