基于ROS的六自由度機(jī)械臂避障研究

（江西理工大學(xué) 機(jī)電工程學(xué)院，贛州 341000）

0 引言

機(jī)器人的運(yùn)動(dòng)規(guī)劃一直是機(jī)器人研究的重點(diǎn)，目前工業(yè)上使用的機(jī)器人大多數(shù)為工人點(diǎn)對(duì)點(diǎn)示教實(shí)現(xiàn)的，人為來(lái)示教即費(fèi)時(shí)又費(fèi)力。要想機(jī)器人能夠生成一條無(wú)碰撞的最優(yōu)軌跡，必須解決機(jī)器人避障的難題。

計(jì)算機(jī)虛擬控制及仿真平臺(tái)為機(jī)器人運(yùn)動(dòng)規(guī)劃的研究提供了方便，SolidWorks、Ansys、MATLAB、Adams等是目前使用較為普遍的軟件平臺(tái)，這些平臺(tái)各有千秋，但對(duì)完成復(fù)雜環(huán)境下的機(jī)器人運(yùn)動(dòng)不夠方便直接。MATLAB作為一款數(shù)學(xué)計(jì)算軟件，能夠利用其強(qiáng)大的算法來(lái)完成仿真實(shí)驗(yàn)，但對(duì)3D模型的表現(xiàn)力較差，且很難與外部系統(tǒng)和其他功能需求相結(jié)合[1]。SolidWorks等三維建模軟件雖然可以出色的完成機(jī)械的建模，但卻不能實(shí)現(xiàn)算法的驗(yàn)證[2]?，F(xiàn)階段，大多都使用C、C++、Java等編程語(yǔ)言與Open Inventor或OpenGL想結(jié)合編寫(xiě)圖形仿真系統(tǒng)，但是操作起來(lái)比較復(fù)雜，其仿真系統(tǒng)也主要是觀察系統(tǒng)的運(yùn)動(dòng)過(guò)程，數(shù)據(jù)參數(shù)不具有參考性[1]。在跨平臺(tái)，分布式處理，代碼復(fù)用，多語(yǔ)言支持與硬件抽象等一些共性問(wèn)題上也支持不夠。而ROS開(kāi)源操作系統(tǒng)能夠很好的解決這些問(wèn)題，且能夠直接應(yīng)用于實(shí)際系統(tǒng)[3]。本文基于ROS開(kāi)源平臺(tái)，以ABB公司IRB1410型工業(yè)機(jī)器人為研究對(duì)象進(jìn)行避障運(yùn)動(dòng)規(guī)劃的仿真研究。

1 機(jī)械臂運(yùn)動(dòng)學(xué)分析

正運(yùn)動(dòng)學(xué)問(wèn)題主要涉及機(jī)械臂的各個(gè)關(guān)節(jié)與工具或末端執(zhí)行器的位置和姿態(tài)之間的關(guān)系。正運(yùn)動(dòng)學(xué)的求解過(guò)程就是根據(jù)機(jī)器人已知關(guān)節(jié)變量，來(lái)求解末端執(zhí)行器的位姿。運(yùn)用D-H參數(shù)法進(jìn)行串聯(lián)機(jī)器人正運(yùn)動(dòng)學(xué)求解，就是通過(guò)坐標(biāo)轉(zhuǎn)換的思想把末端執(zhí)行器的位姿在坐標(biāo)系里表示出來(lái)[4]。根據(jù)D-H參數(shù)法建立的ABBIRB1410機(jī)械臂坐標(biāo)系如圖1所示。

圖中基坐標(biāo)系為X0Y0Z0組成的坐標(biāo)系，末端執(zhí)行處的坐標(biāo)系為X6Y6Z6組成的坐標(biāo)系。根據(jù)IRB1410機(jī)械臂的相關(guān)參數(shù)可以列出機(jī)械臂的D-H參數(shù)表，i表示連桿，di表示連桿偏置，ai表示連桿長(zhǎng)度，αi表示連桿扭曲，θi表示變量關(guān)節(jié)角度，如表1所示。

圖1 ABB IRB1410機(jī)械臂連桿坐標(biāo)系

表1 機(jī)械臂D-H參數(shù)表

對(duì)全部連桿規(guī)定坐標(biāo)系后，由兩個(gè)旋轉(zhuǎn)和兩個(gè)平移來(lái)建立相鄰兩連桿i-1與i之間的相對(duì)關(guān)系，且可由四個(gè)其次變換來(lái)描述，可表示為A矩陣：

式(1)中cθi=cosθi，sθi=sinθi，cαi=cosαi，sαi=sinαi，進(jìn)一步得到機(jī)器人的運(yùn)動(dòng)學(xué)方程為：

式(2)中n，o，a表示末端執(zhí)行器相對(duì)于基坐標(biāo)的姿態(tài)；p表示末端執(zhí)行器相對(duì)于基坐標(biāo)的位置關(guān)系。把表1中的參數(shù)分別代入式(1)、式(2)，便可得到機(jī)器人運(yùn)動(dòng)學(xué)正解[5]。

2 運(yùn)動(dòng)規(guī)劃功能包配置

2.1 開(kāi)源機(jī)器人操作系統(tǒng)ROS

ROS操作系統(tǒng)是一款開(kāi)源的機(jī)器人后操作系統(tǒng)，為了提高機(jī)器人研發(fā)領(lǐng)域的代碼復(fù)用率，提高開(kāi)發(fā)效率，ROS對(duì)機(jī)器人硬件進(jìn)行了封裝，使得不同的機(jī)器人和不同的傳感器都能夠在ROS里通過(guò)一樣的形式來(lái)表示，從而給上層的功能包（運(yùn)動(dòng)規(guī)劃，三維導(dǎo)航等）提供調(diào)用。且ROS可以在不同的計(jì)算機(jī)，不同的操作系統(tǒng)和不同的機(jī)器人上使用，做到真正的跨平臺(tái)[6]。

ROS操作系統(tǒng)集成了許多的開(kāi)源工具與先進(jìn)的算法，如MoveIt移動(dòng)機(jī)械臂運(yùn)動(dòng)規(guī)劃模塊，Rviz 3D可視化工具，Player二維仿真平臺(tái)，OpenRave運(yùn)動(dòng)規(guī)劃平臺(tái)，OpenCV視覺(jué)開(kāi)源項(xiàng)目，OMPL運(yùn)動(dòng)規(guī)劃開(kāi)源項(xiàng)目，Gazebo機(jī)器人開(kāi)源仿真平臺(tái)，PCL開(kāi)源點(diǎn)云處理庫(kù)等。且能夠支持多種編程語(yǔ)言，如C++、Python、Octave等，采用模塊化的通訊機(jī)制，開(kāi)發(fā)者對(duì)各模塊中的代碼能夠單獨(dú)編譯。ROS系統(tǒng)免費(fèi)且開(kāi)源，這是其快速發(fā)展的主要因素之一[6]。

2.2 由SolidWorks制作URDF文件

統(tǒng)一機(jī)器人描述格式（Unified Robot Description Format）是一種用于描述機(jī)器人結(jié)構(gòu)，關(guān)節(jié)，自由度等的XML格式文件。URDF文件描述了機(jī)器人的各個(gè)部件與相關(guān)關(guān)節(jié)的連接關(guān)系，確定了從基座到執(zhí)行末端各個(gè)部件的坐標(biāo)系和坐標(biāo)原點(diǎn)。制作URDF文件，首先需要通過(guò)SolidWorks對(duì)機(jī)械臂進(jìn)行三維建模，然后通過(guò)SW2urdf插件對(duì)機(jī)械臂的各個(gè)關(guān)節(jié)和連接進(jìn)行設(shè)置，最后導(dǎo)出URDF文件。

2.3 由MoveIt！配置功能包

MoveIt！是ROS中專門針對(duì)移動(dòng)機(jī)械臂操作的功能包，它結(jié)合了運(yùn)動(dòng)規(guī)劃，三維感知，運(yùn)動(dòng)學(xué)，碰撞檢測(cè)，控制和導(dǎo)航等，它為開(kāi)發(fā)先進(jìn)的機(jī)器人提供了一個(gè)簡(jiǎn)單，實(shí)用，開(kāi)放的平臺(tái)。使用MoveIt Setup Assisant可生成MoveIt！配置功能包。具體步驟如圖2所示。

在添加運(yùn)動(dòng)鏈的時(shí)候，我們需要對(duì)求解正運(yùn)動(dòng)學(xué)與逆運(yùn)動(dòng)學(xué)的求解工具進(jìn)行選擇，本文選擇KDL（The Kinematics and Dynamics Library）作為運(yùn)動(dòng)學(xué)求解工具。KDL是一個(gè)運(yùn)動(dòng)學(xué)與動(dòng)力學(xué)的庫(kù)，其優(yōu)勢(shì)在于能夠輕易的解決多自由度單鏈機(jī)械結(jié)構(gòu)的正逆運(yùn)動(dòng)學(xué)問(wèn)題。配置好相關(guān)參數(shù)就可以對(duì)機(jī)械臂進(jìn)行運(yùn)動(dòng)規(guī)劃。

3 機(jī)械臂避障規(guī)劃

3.1 避障規(guī)劃算法

圖2 MoveIt！配置步驟

避障路徑規(guī)劃問(wèn)題就是在具有障礙物的特定環(huán)境中，依照相關(guān)評(píng)價(jià)標(biāo)準(zhǔn)（找到最優(yōu)解或找到有效解），規(guī)劃出一條從起始點(diǎn)到目標(biāo)點(diǎn)的無(wú)碰撞路徑。對(duì)于找到最優(yōu)解，大多數(shù)算法使用最優(yōu)規(guī)劃，對(duì)于快速找到有效解，一般使用采樣規(guī)劃。ROS平臺(tái)中的OMPL是一個(gè)包含了許多運(yùn)動(dòng)規(guī)劃領(lǐng)域前沿算法的開(kāi)源運(yùn)動(dòng)規(guī)劃庫(kù)，其中大部分算法基于采樣原理。本文以RRT及其擴(kuò)展算法作為規(guī)劃算法對(duì)機(jī)械臂進(jìn)行仿真分析[7]。

RRT算法是一種基于隨機(jī)采樣的快速搜索算法，它首先通過(guò)采樣得到一個(gè)隨機(jī)樣本，然后把它存儲(chǔ)在一個(gè)樹(shù)狀的結(jié)構(gòu)中，通過(guò)樹(shù)狀結(jié)構(gòu)的分支繼續(xù)往未知空間搜索，直到到達(dá)目標(biāo)點(diǎn)為止。RRT算法的運(yùn)動(dòng)樹(shù)如圖3所示。

圖3 RRT算法運(yùn)動(dòng)樹(shù)

RRT迅速的在各個(gè)方向擴(kuò)展，快速的擴(kuò)展到空白區(qū)域的四個(gè)角落。給定一個(gè)初始狀態(tài)qinit，和一個(gè)活動(dòng)區(qū)域C，我們可以建立一個(gè)樹(shù)狀結(jié)構(gòu)G來(lái)探索如何在C中活動(dòng)，并最終到達(dá)目的地。假設(shè)當(dāng)前G中共有K個(gè)頂點(diǎn)(vertex)。那么RRT可以表示為以下流程：

RRT算法的構(gòu)建過(guò)程為：給出起始點(diǎn)qstart，隨機(jī)的選取一個(gè)新?tīng)顟B(tài)qrand，找出樹(shù)G上離隨機(jī)點(diǎn)qrand最近的點(diǎn)qnear，最后把qnear沿著qrand的方向移動(dòng)Δq，得到新的節(jié)點(diǎn)qnew，不停地搜索下去，直到達(dá)到目標(biāo)點(diǎn)為止。

3.2 ROS避障規(guī)劃實(shí)現(xiàn)

在新的終端中啟動(dòng)前面MoveIt！所配置的功能包中的demo文件，進(jìn)入Rviz界面，可以自己編輯添加障礙物文件，通過(guò)拖動(dòng)機(jī)械臂末端來(lái)設(shè)置機(jī)械臂的起始位置與目標(biāo)位置，選擇OMPL運(yùn)動(dòng)規(guī)劃庫(kù)中的規(guī)劃算法RRT，并發(fā)布到當(dāng)前環(huán)境中，點(diǎn)擊Plan規(guī)劃按鈕，系統(tǒng)就會(huì)生成了一種可能的規(guī)劃路徑，并顯示在Rviz窗口中。圖4所示為配置好的仿真環(huán)境，深色為設(shè)置好的機(jī)械臂起始位置，淺色為目標(biāo)位置。機(jī)械臂必須繞過(guò)小桌平面上的長(zhǎng)方體型障礙物到達(dá)目標(biāo)位置才算完成規(guī)劃。當(dāng)沒(méi)有使用RRT規(guī)劃算法時(shí)，機(jī)械臂會(huì)直接穿過(guò)障礙物到達(dá)目標(biāo)位置。圖5所示為通過(guò)RRT算法進(jìn)行避障規(guī)劃形成的路徑，可以非常直觀的看到機(jī)械臂成功避開(kāi)了障礙物。

圖4 機(jī)械臂起始位置與目標(biāo)位置

圖5 機(jī)械臂避障規(guī)劃路徑

【】【】

在終端輸入rqt_plot命令，在地址欄里輸入節(jié)點(diǎn)joint_states，能夠得到各個(gè)關(guān)節(jié)運(yùn)動(dòng)曲線圖，圖6所示為關(guān)節(jié)的位置曲線圖，橫坐標(biāo)為時(shí)間變化，縱坐標(biāo)為關(guān)節(jié)位置變化，可以較為清楚的看出各個(gè)關(guān)節(jié)位置變化曲線。

圖6 MoveIt！各關(guān)節(jié)位置曲線圖

我們也可以通過(guò)rosbag record–a命令記錄下機(jī)械臂運(yùn)動(dòng)過(guò)程中相關(guān)的參數(shù)數(shù)據(jù)，并利用MATLAB進(jìn)行二次分析，圖7圖8為MATLAB分析得到的曲線圖?？梢钥闯雠crqt_plot所繪制的曲線基本相同。

圖7 MATLAB關(guān)節(jié)位置曲線

4 結(jié)束語(yǔ)

本文利用ROS平臺(tái)，結(jié)合隨機(jī)采樣算法對(duì)機(jī)械臂進(jìn)行避障路徑規(guī)劃，較為形象的展示了機(jī)械臂的運(yùn)動(dòng)過(guò)程，并可以導(dǎo)出相關(guān)數(shù)據(jù)利用MATLAB進(jìn)行二次分析，相比其他平臺(tái)，ROS平臺(tái)直觀高效，具有很高的開(kāi)放性與擴(kuò)展性，極大的方便了機(jī)器人的研究。

圖8 MATLAB各關(guān)節(jié)位置曲線

[1]祁若龍,周維佳,劉金國(guó),等.VC平臺(tái)下機(jī)器人虛擬運(yùn)動(dòng)控制及3D運(yùn)動(dòng)仿真的有效實(shí)現(xiàn)方法[J].機(jī)器人,2013,35(5):594-599.

[2]陳盛龍,平雪良,曹正萬(wàn),等.基于ROS串聯(lián)機(jī)器人虛擬運(yùn)動(dòng)控制及仿真研究[J].組合機(jī)床與自動(dòng)化加工技術(shù),2015(10):108-111.

[3]Bihlmaier A,W?rn H. ROS-Based Cognitive Surgical Robotics[C].Workshop Proceedings of, Intl.Conf. on Intelligent Autonomous Systems.2014:253-255.

[4]馬克W.斯龐.機(jī)器人建模和控制[M].機(jī)械工業(yè)出版社,2016.

[5]韓秀姝,王吉岱,魏軍英,等.碼垛機(jī)器人運(yùn)動(dòng)學(xué)分析[J].機(jī)械傳動(dòng),2014(9):109-112.

[6]陳金寶.ROS開(kāi)源機(jī)器人控制基礎(chǔ)[M].上海交通大學(xué)出版社,2016.

[7]代彥輝,梁艷陽(yáng),謝鋼.基于RRT搜索算法的六自由度機(jī)械臂避障路徑規(guī)劃[J].自動(dòng)化技術(shù)與應(yīng)用,2012,31(10):31-37.