基于ROS的工業(yè)機器人運動規(guī)劃

劉啟帆，謝 明，姜 磊

(南京工業(yè)大學(xué) 電氣工程與控制科學(xué)學(xué)院 智能系統(tǒng)與機器人研究所，南京 211816)

基于ROS的工業(yè)機器人運動規(guī)劃

劉啟帆，謝 明，姜 磊

(南京工業(yè)大學(xué) 電氣工程與控制科學(xué)學(xué)院 智能系統(tǒng)與機器人研究所，南京 211816)

ROS是一個開源的機器人操作系統(tǒng)，能夠很好的驗證各類運動規(guī)劃算法。通過對該系統(tǒng)的研究，提出一種以ROS為平臺在笛卡爾空間下進行運動規(guī)劃的方法。首先將工業(yè)機器人的三維模型轉(zhuǎn)制為ROS中的URDF格式，完成機器人的初步配置。然后使用MoveIt!工具添加了使用最小二乘迭代的逆解算法，該方法通用性強，且可得到唯一最優(yōu)解。對于規(guī)劃過程中的姿態(tài)插補，則采用單位四元數(shù)法，該方法有效解決了萬向節(jié)鎖死和旋轉(zhuǎn)不平滑等問題。最后讓機器人分別沿著直線和圓弧軌跡進行運動仿真，從實驗結(jié)果可以看出機器人運動軌跡平滑，表明了此方法的可行性。

ROS；工業(yè)機器人；單位四元數(shù)；運動規(guī)劃

0 引言

近年來，隨著機器人技術(shù)的快速發(fā)展，在不適宜人類工作的環(huán)境中，工業(yè)機器人得到了越來越廣泛的應(yīng)用。但由于機器人連桿結(jié)構(gòu)的復(fù)雜性和多自由度的工作空間，使得機器人的運動規(guī)劃變得越發(fā)復(fù)雜。在這種情況下，將機器人技術(shù)與計算機虛擬控制和圖形化仿真平臺結(jié)合起來，會在機器人研究和應(yīng)用方面發(fā)揮巨大作用。但當(dāng)前許多平臺普遍存在代碼冗余、可移植性差等問題，無法勝任復(fù)雜環(huán)境下對機器人控制軟件的開發(fā)需求。機器人操作系統(tǒng)(ROS)從眾多平臺中脫穎而出，除了能很好的解決上述平臺中的問題，同時還兼有多編程語言、跨平臺、軟件資源豐富等多個優(yōu)點，越來越多的成為機器人開發(fā)的首選工具。

運動規(guī)劃是要讓機器人按照指定的路徑軌跡來完成運動，由于規(guī)劃好的軌跡存在于笛卡爾空間中，而完成運動則取決于關(guān)節(jié)空間，這就需要實現(xiàn)兩個空間的轉(zhuǎn)換，轉(zhuǎn)換過程也就是逆運動學(xué)的求解過程。文獻[1]采用分離變量法進行逆運動學(xué)的求解，可以得到所有組解，但沒有進行最優(yōu)解的選取，文獻[2]給出了最優(yōu)逆解的選取過程，但不具有較好的魯棒性。對于運動規(guī)劃過程中的姿態(tài)插補，文獻[3]指出了使用歐拉角描述姿態(tài)的4個問題，其中包括了萬向節(jié)鎖死和三維旋轉(zhuǎn)不平滑等問題。

本文以ABB IRB-120工業(yè)機器人作為研究對象。首先將機器人的三維模型轉(zhuǎn)制為ROS可識別的URDF格式，完成對機器人模型的初步配置。然后使用MoveIt!工具添加了使用最小二乘迭代的逆解算法，該算法通用性強，且可以得到唯一最優(yōu)解。使用單位四元素來表示姿態(tài)，能夠解決萬向節(jié)鎖死和三維旋轉(zhuǎn)不平滑等問題。最后分別對機器人的直線軌跡和圓弧軌跡進行了仿真，通過ROS工具導(dǎo)出的各關(guān)節(jié)的位置變化信息可以看出機器人運動軌跡平滑。

1 機器人模型構(gòu)建

ROS中采用通用機器人描述格式URDF(Unified Robot Description Format)來定制機器人模型。不同于傳統(tǒng)的CAD模型文件，URDF文件以XML文本文件的格式描述機器人模型，可存儲模型的外觀、運動學(xué)、動力學(xué)等信息，同時還支持模型的自碰撞檢測。由于大部分機器人模型是用CAD工具創(chuàng)建的，所以ROS同時也支持將CAD模型文件轉(zhuǎn)換成URDF模型文件。

在URDF文件中，機器人的模型被分成連桿(Link)和關(guān)節(jié)(Joint)兩種基本架構(gòu)。如圖1所示。連桿用于描述機器人剛體，存儲其尺寸、外形、顏色、慣性、碰撞屬性等信息；關(guān)節(jié)則用于表達兩連桿的連接，存儲關(guān)節(jié)運動類型(線性或旋轉(zhuǎn))、關(guān)節(jié)運動和速度限制等信息。

圖1 URDF關(guān)節(jié)模型示意圖

通常一個關(guān)節(jié)連接兩個連桿，并且兩個連桿又被分成一個父連桿和一個子連桿。因為每一個連桿都有其固定的坐標(biāo)系，所以父連桿和子連桿間的轉(zhuǎn)換關(guān)系是可以確定的。本文中使用的模型是ABB的IRB-120機器人，根據(jù)上述原理將其模型進行轉(zhuǎn)制，圖2為轉(zhuǎn)制后生成的機器人關(guān)節(jié)、連桿樹狀圖。

圖2 機器人關(guān)節(jié)、連桿樹狀圖

RViz是ROS中常用的三維可視化工具。將URDF文件導(dǎo)入RViz便可以顯示虛擬機器人模型，以便用戶進一步配置和修改URDF文件。啟動RViz節(jié)點和其他必要的節(jié)點就可以建立一個ROS網(wǎng)絡(luò)，以此來控制每個關(guān)節(jié)的位置。這樣便可以檢查每個關(guān)節(jié)的連接效果，圖3是IRB-120在RViz中的效果圖，下面是主要的幾個活動節(jié)點。

關(guān)節(jié)狀態(tài)發(fā)布節(jié)點：該節(jié)點讀取機器人描述參數(shù)，并以一定的頻率發(fā)布來自URDF模型中非固定關(guān)節(jié)的信息，比如位置、速度、加速度等。同時，它也提供了一個控制界面來設(shè)置機械臂的關(guān)節(jié)位置。

機器人狀態(tài)發(fā)布節(jié)點：該節(jié)點用于發(fā)布機器人狀態(tài)給TF庫。通過訂閱了機器人當(dāng)前關(guān)節(jié)位置，并使用URDF模型中的運動學(xué)發(fā)布每個關(guān)節(jié)軸的坐標(biāo)轉(zhuǎn)換數(shù)據(jù)。

TF節(jié)點：TF允許用戶隨時間來追蹤多個坐標(biāo)系。TF能將坐標(biāo)系之間的關(guān)系以樹狀結(jié)構(gòu)實時存貯，并能讓用戶在任何時間點轉(zhuǎn)換兩個坐標(biāo)系間的點或者向量。

圖3 RViz中機器人三維模型圖

2 機器人逆運動學(xué)

MoveIt!是ROS下的一個通用機器人運動控制包。通過使用MoveIt!配置助理便可以根據(jù)機器人的URDF模型來生成一系列的配置文件，這些文件中包含了機器人的運動學(xué)求解、關(guān)節(jié)限位、控制器等信息。通過這些生成的文件，我們就可以使用RViz來替代真實的機器人進行運動仿真。

對于運動學(xué)求解，MoveIt!中默認(rèn)使用KDL庫來進行正、逆運動學(xué)的計算，但同時也提供了接口方便用戶采用自定 義的逆運動學(xué)求解算法。考慮到不同機器人機構(gòu)的多樣性，本文采用數(shù)值解法來進行工業(yè)機器人逆向運動學(xué)問題的求解。設(shè)機器人末端位姿和6個關(guān)節(jié)角之間的關(guān)系如下：

(1)

本文通過數(shù)值解法進行不斷迭代求出各關(guān)節(jié)角的原理如下：

(2)

式中，k為迭代次數(shù)，θ(0)為關(guān)節(jié)角初始值，dθ為每次迭代過程中關(guān)節(jié)角的增量。

將式(1)對關(guān)節(jié)角進行求導(dǎo)可得如下式：

(3)

(4)

(5)

(6)

根據(jù)上述公式，式(3)可表示為：

dP=Jdθ

(6)

將上式進行偽逆矩陣變換可得：

(7)

3 機器人運動規(guī)劃

針對笛卡爾空間下的軌跡規(guī)劃，MoveIt!沒有直接提供解決方案，但提供了相關(guān)的API函數(shù)方便用戶自行開發(fā)。笛卡爾空間軌跡規(guī)劃是指機器人末端在已知初始狀態(tài)和終止?fàn)顟B(tài)的條件下，利用插補算法求解出運動軌跡的中間狀態(tài)，并使用逆運動學(xué)求解出各中間狀態(tài)所對應(yīng)的關(guān)節(jié)角度，以達到操控機器人運動的目的。在笛卡爾空間坐標(biāo)系下機器人的狀態(tài)包括位置和姿態(tài)，所以要分別對位置和姿態(tài)進行插補，最后再進行整合。

3.1 位置插補

位置插補可以分為針對直線和圓弧軌跡的插補。對于直線插補，設(shè)直線的起始點為A(xA,yA,zA)，終點為B(xB,yB,zB)，插補次數(shù)為N。則中間插補點的坐標(biāo)為：

(8)

式中，i=0,1,2,…,N。

對于圓弧插補，由空間不共線的三點A，B，C可以確定的一個圓弧平面，這個平面的方程為：

Ax+By+Cz+D=0

(9)

在這個平面上取直線AB和BC的垂直平分線，兩線相交于點O(x0,y0,z0)，該點即為圓弧的圓心坐標(biāo)，進而可計算出半徑R，則弧AB和弧BC所對應(yīng)的圓心角θ1和θ2也可求得。現(xiàn)在以O(shè)為坐標(biāo)系原點，OA作為U軸，W軸在圓弧平面，V軸垂直于圓弧平面，建立UVW坐標(biāo)系，如圖4所示。

圖4 空間圓弧示意圖

便可以得到UVW坐標(biāo)系相對于基坐標(biāo)系的變換矩陣為：

(10)

(11)

(12)

3.2 姿態(tài)插補

(13)

其中s=cosθ，a=kxsinθ，b=kysinθ，c=kzsinθ。

(14)

式中i=1,2,…,N。

將插補得到的中間狀態(tài)的四元數(shù)轉(zhuǎn)換為姿態(tài)矩陣，與位置插補中得到的位置坐標(biāo)進行組合，便可得到位姿插補矩陣。對于圓弧軌跡的姿態(tài)插補，由于存在中間過渡點B，所以對兩段軌跡分別進行姿態(tài)插補，每段的插補方法與上面的直線姿態(tài)插補一樣，最后將兩段整合起來。

4 仿真與實驗

為了實現(xiàn)機械臂的軌跡規(guī)劃仿真，首先從MoveIt!生成的配置文件中啟動RViz仿真器，此時RViz作為功能節(jié)點存在，主要負(fù)責(zé)接收運動指令，控制仿真機器人運動并反饋實際關(guān)節(jié)的位置信息。然后將插值算法作為一個單獨的節(jié)點啟動，并建立兩個節(jié)點之間的通信連接。這時插值算法節(jié)點便可將軌跡點以一定的頻率發(fā)送給RViz節(jié)點，RViz在接收到指令后立即做出響應(yīng)。

rqt_plot是ROS下的一個繪圖插件，可用來繪制二維數(shù)值曲線圖。通過接收RViz節(jié)點實時反饋的關(guān)節(jié)位置信息，rqt_plot插件便可以實時繪制出機器人關(guān)節(jié)角度的變化曲線。

圖5 直線插補運動軌跡圖

使用rqt_plot插件繪制出的關(guān)節(jié)角度變化如圖6所示，圖例中position[0]至position[5]對應(yīng)6個關(guān)節(jié)角度。

圖6 直線插補關(guān)節(jié)角度變化圖

圖7 圓弧插補運動軌跡圖

使用rqt_plot插件繪制出的關(guān)節(jié)角度變化如圖8所示。

圖8 圓弧插補關(guān)節(jié)角度變化圖

5 結(jié)束語

本文在ROS環(huán)境下，將機器人的三維模型轉(zhuǎn)制為URDF格式，并使用可視化工具RViz檢驗轉(zhuǎn)化效果。通過MoveIt!對機器人進行運動學(xué)配置，并添加了使用最小二乘迭代的逆解算法。對運動規(guī)劃中的姿態(tài)插補給出了單位四元數(shù)法。最后對機器人的直線和圓弧運動軌跡進行了仿真，并導(dǎo)出規(guī)劃過程中機器人關(guān)節(jié)位置變化信息，驗證了該算法的正確性。為該算法在實際作業(yè)任務(wù)中的應(yīng)用起到了一定指導(dǎo)作用。

[1] 王曉強, 王帥軍, 劉建亭. 基于MATLAB的IRB2400工業(yè)機器人運動學(xué)分析[J]. 機床與液壓, 2014(3):54-57.

[2] 吳挺, 吳國魁, 吳海彬. 6R工業(yè)機器人運動學(xué)算法的改進[J]. 機電工程, 2013, 30(7):882-887.

[3] 劉松國. 六自由度串聯(lián)機器人運動優(yōu)化與軌跡跟蹤控制研究[D]. 杭州:浙江大學(xué), 2009.

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

[5] 曹正萬, 平雪良, 陳盛龍,等. 基于ROS的機器人模型構(gòu)建方法研究[J]. 組合機床與自動化加工技術(shù), 2015(8):51-54.

[6] 孟韶南,梁雁冰,師恒. 基于ROS平臺的六自由度機械臂運動規(guī)劃[J]. 上海交通大學(xué)學(xué)報,2016(S1):94-97.

[7] 錢偉. 基于ROS的移動操作機械臂底層規(guī)劃及運動仿真[D]. 哈爾濱:哈爾濱工業(yè)大學(xué), 2015.

[8] 高帥. 工業(yè)機器人三維仿真及其運動控制研究[D].南京:南京理工大學(xué), 2015.

[9] 丁吉祥. 工業(yè)機器人運動學(xué)參數(shù)標(biāo)定技術(shù)研究[D]. 南京:南京理工大學(xué),2015.

[10]QianW,XiaZ,XiongJ,etal.ManipulationtasksimulationusingROSandGazebo[C]//IEEEInternationalConferenceonRoboticsandBiomimetics.IEEE, 2015:2594-2598.

[11]ErgurS,OzkanM.Trajectoryplanningofindustrialrobotsfor3-DvisualizationaROS-basedsimulationframework[C]//IEEEInternationalSymposiumonRoboticsandManufacturingAutomation[C].IEEE, 2014：206-211.

[12]JosephL.MasteringROSforroboticsprogramming[M].Birm-ingham:PacketPublishingLtd, 2015.

(編輯 李秀敏)

The Motion Planning of Industrial Robots Based on ROS

LIU Qi-fan, XIE Ming, JIANG Lei

(Institute of Intelligent Systems and Robotics, Academy of Electrical Engineering and Control Science, Nanjing Tech University, Nanjing 211816, China)

ROS is an open source robot operating system, can be a good validation of various types of motion planning algorithm. Based on the research of this system, a method of motion planning in Cartesian space is proposed using ROS as the platform. Firstly, the 3D model of industrial robot is transformed into URDF format of ROS, and the initial configuration of the robot is completed. Then use the MoveIt! tool to add the least squares iterative inverse solution algorithm, the method is general, and can achieve the unique optimal answer. For the orientation interpolation process，using unit quaternion can effectively resolve gimbal lock and rotation unsmooth problems. Finally, for linear trajectory and circular trajectory respectively, simulation results show that the robot can achieve smooth motion along the planned trajectory. So it illustrates the feasibility of the method described above.

ROS; industrial robot; unit quaternion; motion planning

1001-2265(2017)05-0036-04

10.13462/j.cnki.mmtamt.2017.05.010

2016-08-28；

2016-10-09

劉啟帆(1992—)，男，江蘇蘇州人，南京工業(yè)大學(xué)碩士研究生，研究方向為機器人三維仿真與控制，(E-mail)1530221539@qq.com。

TH166;TG659

A