非結(jié)構(gòu)環(huán)境下機(jī)械臂避障運動規(guī)劃

梁 元，張 禹

(沈陽工業(yè)大學(xué) 機(jī)械工程學(xué)院，遼寧 沈陽 110870)

0 引言

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

1 機(jī)器人操作系統(tǒng)

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

2 運動規(guī)劃功能包配置

2.1 建立機(jī)器人模型

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

圖1 機(jī)械臂樣機(jī)簡要模型

2.2 添加RGBD相機(jī)

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

圖2 在Xacro文件中添加Kinect相機(jī)代碼

2.3 MoveIt_Setup_Assistant配置功能包

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

圖3 MoveIt Setup Assistant配置流程

2.4 MoveIt！與Gazebo聯(lián)合系統(tǒng)建立

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

圖4 Rviz、Gazebo和相機(jī)的界面

3 構(gòu)造空間概率地圖

3.1 使用OctoMap描述非結(jié)構(gòu)環(huán)境下的空間障礙物

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

圖5 八叉樹數(shù)據(jù)結(jié)構(gòu)

3.2 八叉樹構(gòu)建相機(jī)采集的圖像

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

圖6 Rviz和Gazebo的環(huán)境界面

4 RRT*(RRT star)算法結(jié)合PCL避障算法規(guī)劃實現(xiàn)

4.1 RRT*避障規(guī)劃算法

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

圖7 RRT*算法流程

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

4.2 RRT*算法結(jié)合PCL的避障規(guī)劃

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

圖8 Gazebo環(huán)境下機(jī)械臂位姿變化

圖9 機(jī)械臂躲避障礙物軌跡

圖10 使用的RRT*算法和規(guī)劃時間

由圖11可以看出，機(jī)械臂各關(guān)節(jié)位置平穩(wěn)連續(xù)，證明了該算法的可行性。

圖11 機(jī)器人各關(guān)節(jié)位置隨時間變化曲線

5 結(jié)語

本文利用SoildWorks建模，通過插件轉(zhuǎn)換urdf文件，再由urdf轉(zhuǎn)換Xacro文件，添加Kinect相機(jī)，在Gazebo仿真環(huán)境通過相機(jī)使非結(jié)構(gòu)環(huán)境變?yōu)榻Y(jié)構(gòu)化，結(jié)合在MoveIt!中調(diào)用RRT*算法規(guī)劃出無碰撞路徑，實現(xiàn)機(jī)械臂避障運動規(guī)劃。通過機(jī)械臂各關(guān)節(jié)曲線圖可以看出，機(jī)械臂各關(guān)節(jié)的位置變化平穩(wěn)且連續(xù)，表明機(jī)械臂在非結(jié)構(gòu)化環(huán)境下實現(xiàn)了自動避障，解決了機(jī)械臂因環(huán)境變化而出現(xiàn)碰撞的問題，對工業(yè)機(jī)器人在ROS平臺下的應(yīng)用研究有著重要意義。