劉啟帆，謝 明，姜 磊

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

基于ROS的工業(yè)機(jī)器人運(yùn)動(dòng)規(guī)劃

劉啟帆，謝 明，姜 磊

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

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

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

0 引言

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

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

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

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

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

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

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

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

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

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

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

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

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

圖3 RViz中機(jī)器人三維模型圖

2 機(jī)器人逆運(yùn)動(dòng)學(xué)

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

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

(1)

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

(2)

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

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

(3)

(4)

(5)

(6)

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

dP=Jdθ

(6)

將上式進(jìn)行偽逆矩陣變換可得：

(7)

3 機(jī)器人運(yùn)動(dòng)規(guī)劃

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

3.1 位置插補(bǔ)

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

(8)

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

對(duì)于圓弧插補(bǔ)，由空間不共線的三點(diǎn)A，B，C可以確定的一個(gè)圓弧平面，這個(gè)平面的方程為：

Ax+By+Cz+D=0

(9)

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

圖4 空間圓弧示意圖

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

(10)

(11)

(12)

3.2 姿態(tài)插補(bǔ)

(13)

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

(14)

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

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

4 仿真與實(shí)驗(yàn)

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

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

圖5 直線插補(bǔ)運(yùn)動(dòng)軌跡圖

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

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

圖7 圓弧插補(bǔ)運(yùn)動(dòng)軌跡圖

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

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

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

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

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

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

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

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

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

[6] 孟韶南,梁雁冰,師恒. 基于ROS平臺(tái)的六自由度機(jī)械臂運(yùn)動(dòng)規(guī)劃[J]. 上海交通大學(xué)學(xué)報(bào),2016(S1):94-97.

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

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

[9] 丁吉祥. 工業(yè)機(jī)器人運(yùn)動(dòng)學(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é)碩士研究生，研究方向?yàn)闄C(jī)器人三維仿真與控制，(E-mail)1530221539@qq.com。

TH166;TG659

A