非結構環境下機械臂避障運動規劃

梁 元，張 禹

(沈陽工業大學 機械工程學院，遼寧 沈陽 110870)

0 引言

在機器人的實際工作環境中，障礙物廣泛存在，傳統躲避障礙物的方法是對障礙物空間模型計算無碰撞軌跡或者通過示教的方法來進行避障，所以在實際應用中不能很好地實現自動避障。本文提出添加RGBD相機在ROS(Robot Operating System，機器人操作系統)平臺下實現自動避障。該相機在操作空間采集環境點云信息，然后轉換OctoMap空間概率地圖，使用RRT*(RRTstar)算法在Moveit平臺下對機械臂在該環境下進行運動規劃。

1 機器人操作系統

ROS是用于機器人編程的開放源碼、靈活的軟件框架，還是管道(通信)、工具、功能等的組合。ROS提供了硬件抽象層，可以使用ROS構建機器人應用程序，還提供了不同的可視化工具和調試機器人數據的軟件工具，其中包括3D可視化平臺rviz、MoveIt！機械臂控制平臺，并且ROS還能很好地兼容三維物理仿真平臺Gazebo。

2 運動規劃功能包配置

2.1 建立機器人模型

采用SoildWorks建立機械臂樣機的簡要模型，并給機械臂添加基準軸和坐標系，如圖1所示。通過ROS提供SolidWorks轉URDF插件(sw2urdf)對機械臂各連桿和關節進行設置，然后導出相應的URDF文件。URDF文件是一種機器人描述文件，內容上是XML格式，包括關節名稱、連桿、運動學參數、可視化模型、碰撞檢測等。再由Xacro優化URDF，Xacro有兩大突出優勢，即精簡模型代碼和提供可編程接口。

圖1 機械臂樣機簡要模型

2.2 添加RGBD相機

使用微軟公司生產的Kinect RGBD深度相機，在Xacro文件中添加Kinect相機代碼，如圖2所示。其中代表相對link的坐標系，代表視覺屬性，代表形狀，代表碰撞屬性。

圖2 在Xacro文件中添加Kinect相機代碼

2.3 MoveIt_Setup_Assistant配置功能包

在機器人urdf文件導入合理的Xacro文件以后，使用MoveIt Setup Assistant來完成一些配置工作，配置工作流程如圖3所示。

圖3 MoveIt Setup Assistant配置流程

2.4 MoveIt！與Gazebo聯合系統建立

在MoveIt！與Gazebo分別添加相同的軌跡控制器JointTrajectoryController和位置控制器JointPositionController，其中每個關節相對應一個電機位置控制器。啟動系統后的顯示界面如圖4所示，Rviz界面顯示仿真環境，其中機械臂末端拖動球是方便拖動規劃姿態，機器人上方的小長方體為Kinect相機；Gazebo界面顯示機器人真實物理環境與所處的位姿；Kinect相機實時采集RGB圖像，監控機器人運行狀況。

圖4 Rviz、Gazebo和相機的界面

3 構造空間概率地圖

3.1 使用OctoMap描述非結構環境下的空間障礙物

OctoMap是基于八叉樹的3D創建工具,能夠有效壓縮點云數據，減小對其內存的占用,方便對其進行碰撞檢測。八叉樹是一種用于描述三維空間的樹狀數據結構，將整個空間作為八叉樹的根節點，不斷分割，每個父節點有8個子節點，代表8個小空間，然后將子節點再進行分割，若沒有達到設定的最大遞歸深度就繼續分割，直到符合分辨率大小的葉子節點，就不需再進行分割了。如分辨率為0.08 m，則每個葉子節點大小就是8 cm的立方體。八叉樹數據結構如圖5所示，左側為體積模型，右側為相應的樹狀結構。

圖5 八叉樹數據結構

3.2 八叉樹構建相機采集的圖像

在MoveIt！運動規劃平臺可以直接使用ROS提供的開源OctoMap庫中空間三維點云來構建OctoMap，能更好地實現機械臂周圍三維環境信息表達。Rviz和Gazebo的環境界面如圖6所示，在Gazebo仿真環境下添加障礙物，通過相機在Rviz三維可視化平臺構建出障礙物模型。

圖6 Rviz和Gazebo的環境界面

4 RRT*(RRT star)算法結合PCL避障算法規劃實現

4.1 RRT*避障規劃算法

快速擴展隨機樹RRT(Rapidly-explooring Random Trees)算法從一棵樹初始點擴展到整個工作空間，搜到終點找到路徑，但該路徑并不是最佳路徑。本文使用的RRT*算法稱為RRTstar算法，是漸進最優的，解決了RRT搜索路徑不是最佳路徑的問題。該算法考慮了路徑代價，并加入了父節點擇優選擇和鄰節點回溯重構的策略，最大程度地找到全局最優解。RRT*算法流程如圖7所示，其中，qinit為初始點，qgoal為目標點。

圖7 RRT*算法流程

RRT*算法構建具體過程如下：從構型空間隨機采樣一個點qrand,從隨機樹中旋轉一個最近的節點qnearest朝著qrand方向生成一個新節點qnew，當檢查qnearest到qnew之間不碰撞后，qnearest并不直接連接qnew，而是先用函數Near(T,qnew)在T(T為更新初始點)中搜索與qnew最近的點集合Xnear；然后從點集合Xnear中利用FindBestParent(Xnear,qnew)函數返回一個父節點qparent，用這個返回的父節點連接qnew；當連接完成后，使用ReWrite(T,Xnear,qnear)函數來重新構造樹的結構，對已連接的點進行優化。

4.2 RRT*算法結合PCL的避障規劃

啟動Moveit和Gazebo進行聯合仿真，通過在Gazebo仿真器中添加RGBD相機使非結構化環境成為結構化，在Gazebo仿真環境中獲取障礙物的三維點云信息，將其轉換為OctoMap后傳遞給MoveIt！進行避障運動規劃，然后將通過RRT*算法得到無碰撞路徑再傳遞回Gazebo用以對機械臂進行運動規劃控制。Gazebo環境下機械臂位姿變化如圖8所示，在Rviz顯示的機械臂躲避障礙物軌跡如圖9所示，此次規劃使用的RRT*算法以及規劃時間如圖10所示，機器人各關節位置隨時間變化曲線如圖11所示。

圖8 Gazebo環境下機械臂位姿變化

圖9 機械臂躲避障礙物軌跡

圖10 使用的RRT*算法和規劃時間

由圖11可以看出，機械臂各關節位置平穩連續，證明了該算法的可行性。

圖11 機器人各關節位置隨時間變化曲線

5 結語

本文利用SoildWorks建模，通過插件轉換urdf文件，再由urdf轉換Xacro文件，添加Kinect相機，在Gazebo仿真環境通過相機使非結構環境變為結構化，結合在MoveIt!中調用RRT*算法規劃出無碰撞路徑，實現機械臂避障運動規劃。通過機械臂各關節曲線圖可以看出，機械臂各關節的位置變化平穩且連續，表明機械臂在非結構化環境下實現了自動避障，解決了機械臂因環境變化而出現碰撞的問題，對工業機器人在ROS平臺下的應用研究有著重要意義。