張 穎，平雪良，仇恒坦，楊子豪

(江南大學(xué) 機(jī)械工程學(xué)院 江蘇省食品先進(jìn)制造裝備技術(shù)重點(diǎn)實(shí)驗(yàn)室，江蘇 無錫 214122)

ROS平臺(tái)下多軸機(jī)器人PVT運(yùn)動(dòng)控制研究*

張 穎，平雪良，仇恒坦，楊子豪

(江南大學(xué) 機(jī)械工程學(xué)院 江蘇省食品先進(jìn)制造裝備技術(shù)重點(diǎn)實(shí)驗(yàn)室，江蘇 無錫 214122)

針對傳統(tǒng)機(jī)器人控制系統(tǒng)大多面向特定對象，移植性差，代碼復(fù)用率低，提出采用開源機(jī)器人控制系統(tǒng)ROS為平臺(tái)，研究運(yùn)動(dòng)控制實(shí)現(xiàn)。通過研究PVT控制模式的規(guī)劃算法，提出了在開源機(jī)器人操作系統(tǒng)ROS平臺(tái)下建立機(jī)器人運(yùn)動(dòng)學(xué)模型并設(shè)計(jì)控制結(jié)構(gòu)載入該算法、構(gòu)建PVT運(yùn)動(dòng)控制功能模塊以替代傳統(tǒng)運(yùn)動(dòng)控制器的部分功能。該控制模塊具有開源、可擴(kuò)展、可移植的特點(diǎn)，有效提高控制系統(tǒng)開發(fā)效率，節(jié)約控制系統(tǒng)的開發(fā)成本。該控制方案在工業(yè)6自由度串聯(lián)機(jī)器人上得到了驗(yàn)證，表明該方案有效可行，具有良好的穩(wěn)定性，可代替?zhèn)鹘y(tǒng)運(yùn)動(dòng)控制器。

PVT運(yùn)動(dòng)控制；開源機(jī)器人操作系統(tǒng)ROS；工業(yè)機(jī)器人

0 引言

機(jī)器人技術(shù)是一門綜合了多學(xué)科的高新技術(shù)。近年隨著科學(xué)技術(shù)的不斷發(fā)展，機(jī)器人在自動(dòng)化和智能化方面取得了顯著的提高，已經(jīng)滲透到各行各業(yè)。由于機(jī)器人系統(tǒng)的復(fù)雜性和任務(wù)的多樣性給機(jī)器人的程序編寫工作帶來了諸多困難。以往，基于各自獨(dú)立的系統(tǒng)平臺(tái)和軟件架構(gòu)，即便相同的算法在不同機(jī)器人間移植也要耗費(fèi)大量勞動(dòng)。ROS，Robot Operating System[1-2]是近年來國際上正在迅速發(fā)展的一種開源機(jī)器人操作系統(tǒng)開發(fā)平臺(tái)。通過提供統(tǒng)一接口，點(diǎn)對點(diǎn)松耦合通訊方式使得代碼重用和模塊化設(shè)計(jì)變得更加簡便，可顯著提高機(jī)器人控制系統(tǒng)應(yīng)用開發(fā)速度，降低開發(fā)成本。

針對傳統(tǒng)控制系統(tǒng)復(fù)用率低和移植性差的不足，本文研究ROS系統(tǒng)下控制系統(tǒng)實(shí)現(xiàn)原理，在ROS平臺(tái)下構(gòu)建PVT[3-4]運(yùn)動(dòng)控制模式的實(shí)時(shí)運(yùn)動(dòng)控制模塊，以替代傳統(tǒng)的運(yùn)動(dòng)控制器功能，實(shí)現(xiàn)由PC直接控制機(jī)器人的一種低成本、可擴(kuò)展的開源機(jī)器人控制模塊。

1 ROS簡介

ROS 起源于斯坦福大學(xué)與willow garage 公司的個(gè)人機(jī)器人計(jì)劃的STAIR 項(xiàng)目[5]，ROS 提供一些標(biāo)準(zhǔn)的操作系統(tǒng)服務(wù)，如硬件抽象描述、底層驅(qū)動(dòng)程序管理、程序間消息傳遞、程序發(fā)行包管理等，也提供一些工具程序和庫，用于獲取、建立、編寫和運(yùn)用多機(jī)整合的程序。

2 PVT算法實(shí)現(xiàn)原理

機(jī)器人的規(guī)劃算法分為兩部分：一個(gè)是針對笛卡爾空間的路徑規(guī)劃算法，一個(gè)是針對關(guān)節(jié)空間變量的軌跡規(guī)劃算法即精插補(bǔ)算法。路徑規(guī)劃主要研究如何在笛卡爾空間規(guī)劃出一條合理的連續(xù)路徑。而軌跡規(guī)劃主要研究已有路徑與時(shí)間的函數(shù)，規(guī)劃出每個(gè)路徑點(diǎn)的速度和加速度等。由于笛卡爾空間位姿和關(guān)節(jié)空間變量不是連續(xù)對應(yīng)的，因此通常情況下軌跡規(guī)劃是在關(guān)節(jié)空間下進(jìn)行，主要研究關(guān)節(jié)變量與時(shí)間的函數(shù)，保證其一階二階導(dǎo)數(shù)的連續(xù)可導(dǎo)，從而避免速度加速度的突變，以免造成對電機(jī)的沖擊。

在對關(guān)節(jié)空間變量進(jìn)行軌跡規(guī)劃時(shí)，大量的工作是關(guān)于關(guān)節(jié)角度變量的插值運(yùn)算，研究關(guān)節(jié)空間軌跡的插值運(yùn)算，采用三次埃爾米特樣條函數(shù)插值。關(guān)節(jié)空間變量插值的目的是保證關(guān)節(jié)空間角度和角速度的連續(xù)性，同時(shí)保證其平穩(wěn)運(yùn)行和加速度不發(fā)生突變。轉(zhuǎn)化成數(shù)學(xué)問題為根據(jù)指定的邊界條件研究生成經(jīng)過各個(gè)軌跡點(diǎn)且二次連續(xù)可導(dǎo)的函數(shù)。這種插值不僅要求節(jié)點(diǎn)處的函數(shù)值相等，還要求其對應(yīng)的高階導(dǎo)數(shù)相等，其插值多項(xiàng)式稱之為埃爾米特插值多項(xiàng)式[6]。三次埃爾米特樣條曲線插值根據(jù)初始條件不同分為兩種：一種是已知型值點(diǎn)處的函數(shù)約束和一階導(dǎo)數(shù)約束，求解埃爾米特樣條函數(shù)；另一種是已知型值點(diǎn)處的函數(shù)約束和二階導(dǎo)數(shù)約束，求解埃爾米特樣條函數(shù)。

本文研究的軌跡規(guī)劃插值算法的數(shù)據(jù)來源于高層運(yùn)動(dòng)規(guī)劃的關(guān)節(jié)信息，根據(jù)關(guān)節(jié)空間軌跡的角度值和角速度值，求解中間插值點(diǎn)的角度值和角速度值，即已知邊界兩端節(jié)點(diǎn)處函數(shù)值及其一階導(dǎo)數(shù)，求取中間型值點(diǎn)的函數(shù)和一階導(dǎo)數(shù)。具體可用下述五個(gè)步驟實(shí)現(xiàn)樣條曲線的插值。

2.1 在[0,1]區(qū)間上帶一階導(dǎo)數(shù)的插值

定義：設(shè)在區(qū)間[a,b]上給定一分割變量△，將區(qū)間分成a=x0

(1)

一階導(dǎo)數(shù)為：

(2)

將y0、y1、y0′、y1′這4個(gè)條件代入，解得4個(gè)系數(shù)，代入式(1)整理得：

一階導(dǎo)數(shù)為：

(3)

令：

(4)

則三次多項(xiàng)式方程可寫成：

(5)

(6)

2.2 推廣至在區(qū)間[xi-1,xi]上帶一階導(dǎo)數(shù)的插值

假設(shè)在區(qū)間[xi-1,xi](i=1,2,3,…,n)上，已知當(dāng)x=xi-1和x=xi時(shí)的函數(shù)值為yi-1,yi，其一階導(dǎo)數(shù)為mi-1,mi，為方便計(jì)算，首先對區(qū)間進(jìn)行轉(zhuǎn)換，令：

(7)

其中：

hi=xi-xi-1,i=1,2,3,…,n

(8)

則有：

(9)

參照式(5)得出第i段曲線的表達(dá)式為：

yi(x)=yi-1F0(u)+yiF1(u)+hi[mi-1G0(u)+miG1(u)]

(10)

用矩陣表達(dá)：

(11)

2.3 在小區(qū)間[xi-1,xi]上利用連續(xù)性條件

為計(jì)算出yi″(x)，可利用相鄰兩區(qū)間端點(diǎn)處的二階導(dǎo)數(shù)連續(xù)的條件，即：

yi″(xi)=y″i+1(xi)i=1,2,…,n-1

(12)

對式(10)求二階導(dǎo)數(shù)，得到：

(13)

其中：

(14)

那么針對第i段區(qū)間末端(u=1)，有：

(15)

針對第i+1段區(qū)間起點(diǎn)(u=0)，有：

(16)

以上相鄰兩區(qū)間在x=xi處二階導(dǎo)數(shù)連續(xù)，則式(15)和式(16)相等，即：

(17)

整理得：

(18)

令：

(19)

則式(18)可改寫為：

λimi-1+2mi+uimi+1=ci(i=1,2,…,n-1)

(20)

上式可看成是關(guān)于m0,m1,…mn這n+1個(gè)未知量的線性方程組，方程的個(gè)數(shù)為n-1，因此不能唯一確定mi，需要兩個(gè)限制條件。

2.4 利用大區(qū)間[X0,Xn]端點(diǎn)條件

通常情況下在首末端點(diǎn)處x=x0,x=xn的導(dǎo)數(shù)是已知的，現(xiàn)設(shè)為m0,mn，則式(20)方程組第一個(gè)方程為：

2m1+u1m2=c1-λ1m0

(21)

第n-1個(gè)方程組為：

λn-1mn-2+2mn-1=cn-1-un-1mn

(22)

則式(20)就轉(zhuǎn)化為關(guān)于n-1個(gè)未知量的線性方程組，從而可求出唯一解。

2.5 關(guān)于n-1個(gè)未知量線性方程組的求解

將式(20)寫成矩陣形式：

(23)

上式的系數(shù)為三對角矩陣，已知m0和mn，可用“追趕法”求解。求出所有mi后，分段埃爾米特曲線可由式(11)確定，由此得出整個(gè)區(qū)間內(nèi)樣條曲線方程表達(dá)式：

y(x)=y(xi) (i=1,2,…,n)

(24)

3 控制系統(tǒng)總體結(jié)構(gòu)

3.1 控制系統(tǒng)結(jié)構(gòu)

一般具有實(shí)時(shí)系統(tǒng)程序的運(yùn)動(dòng)控制模塊對機(jī)器人的控制有很好的效果，但對CPU性能、編程技巧等有很高的要求，具體實(shí)現(xiàn)則更加困難。而非實(shí)時(shí)程序控制則無法保證結(jié)果的時(shí)間節(jié)點(diǎn)，也無法及時(shí)對程序內(nèi)部及外界做出反應(yīng)。機(jī)器人上層程序諸多模塊任務(wù)多且復(fù)雜，若要保證自身在規(guī)定時(shí)間內(nèi)的實(shí)時(shí)效果，必須分配大量的系統(tǒng)資源，難免降低其他任務(wù)執(zhí)行效率，但系統(tǒng)資源有限且預(yù)留部分資源給底層控制模塊。因此，本文結(jié)合ROS和Linux系統(tǒng)底層設(shè)計(jì)了圖1所示的運(yùn)動(dòng)控制功能模塊結(jié)構(gòu)，預(yù)留PVT控制接口，實(shí)現(xiàn)控制。

圖1 運(yùn)動(dòng)控制功能模塊結(jié)構(gòu)

上圖所示為ROS平臺(tái)上遵循ros_control標(biāo)準(zhǔn)的運(yùn)動(dòng)控制模塊架構(gòu)，圖中實(shí)時(shí)運(yùn)動(dòng)控制模塊由控制模塊管理器調(diào)度，載入和切換由不同算法實(shí)現(xiàn)的各個(gè)運(yùn)動(dòng)控制算法模塊。圖2所示為控制模塊在管理器中的運(yùn)行過程，根據(jù)時(shí)間和運(yùn)行狀態(tài)的不同可分為四個(gè)過程：init()、starting()、update()、stopping()。

圖2 控制模塊運(yùn)行狀態(tài)

3.2 ROS下PVT控制策略

運(yùn)動(dòng)控制模塊是機(jī)器人控制系統(tǒng)最底層模塊，直接和機(jī)器人硬件伺服系統(tǒng)通訊，是對路徑規(guī)劃模塊任務(wù)的具體實(shí)施者。在ROS平臺(tái)上開發(fā)運(yùn)動(dòng)控制模塊可統(tǒng)一采用ros_control標(biāo)準(zhǔn)框架，本文結(jié)合自主研發(fā)的基于EtherCat[7]總線的機(jī)器人硬件環(huán)境開發(fā)底層運(yùn)動(dòng)控制模塊如圖3所示。

圖3 運(yùn)動(dòng)控制模塊

運(yùn)動(dòng)控制模塊負(fù)責(zé)系統(tǒng)最重要的底層PID控制和樣條曲線插補(bǔ)，運(yùn)動(dòng)控制模塊的實(shí)現(xiàn)需借助通用機(jī)器人描述(URDF，Unified Robot Description Format)[8]所蘊(yùn)含的機(jī)器人運(yùn)動(dòng)學(xué)信息，在調(diào)用URDF之前必須對URDF進(jìn)行簡化，剔除與插補(bǔ)、PID無關(guān)的關(guān)節(jié)信息如形狀、大小、慣性等以降低代碼的冗余度，保留關(guān)節(jié)配置信息。URDF文件以XML文本文件的格式描述機(jī)器人模型。機(jī)器人的機(jī)械結(jié)構(gòu)主要包括連桿和關(guān)節(jié)，URDF文件中標(biāo)簽之間的內(nèi)容描述連桿包括質(zhì)量特性、外觀形狀和碰撞屬性在內(nèi)的基本參數(shù)，標(biāo)簽間的內(nèi)容描述連接兩個(gè)連桿的關(guān)節(jié)的參數(shù)[9]。

實(shí)驗(yàn)所用的機(jī)器人如圖4所示，構(gòu)建的URDF文件可通過rviz進(jìn)行加載顯示，同時(shí)使用ros提供的tf[10]工具可以方便查看各個(gè)連桿坐標(biāo)系，各連桿坐標(biāo)系在rviz中顯示如圖5所示。

圖4 實(shí)驗(yàn)所用六自由度工業(yè)機(jī)器人 圖5 通過rivz顯示各連桿坐標(biāo)系

3.3 ROS平臺(tái)下PVT控制驗(yàn)證

基于ROS開發(fā)的PVT控制功能模塊與ROS平臺(tái)下PID算法結(jié)合實(shí)現(xiàn)機(jī)器人本體關(guān)節(jié)空間軌跡實(shí)時(shí)有效控制。通過使用EtherCAT總線開發(fā)的驅(qū)動(dòng)模塊對上述控制模塊進(jìn)行驗(yàn)證。在空間中任意選擇一個(gè)點(diǎn)，運(yùn)行程序使機(jī)器人運(yùn)行到目標(biāo)位置處，通過ros下rviz顯示工具可以看到末端坐標(biāo)系的軌跡如圖6所示。

圖6 rviz下機(jī)器人末端軌跡顯示

運(yùn)動(dòng)過程中通過ROS提供的rxplot工具實(shí)時(shí)監(jiān)控各個(gè)關(guān)節(jié)變量。如圖7所示。圖中表示機(jī)器人各個(gè)關(guān)節(jié)的位置、速度隨時(shí)間的變化。

從圖中可以看到機(jī)器人在運(yùn)行過程中各關(guān)節(jié)的位置和速度變化連續(xù)平滑。PVT控制模式能夠提供位置、速度對時(shí)間的約束，保證系統(tǒng)運(yùn)行的穩(wěn)定和平滑。為多軸機(jī)器人運(yùn)動(dòng)控制奠定了基礎(chǔ)。

(a)機(jī)器人各關(guān)節(jié)角度隨時(shí)間的變化

(b)機(jī)器人各關(guān)節(jié)角速度隨時(shí)間的變化圖7 關(guān)節(jié)空間軌跡實(shí)時(shí)運(yùn)動(dòng)軌跡

4 結(jié)束語

基于ROS平臺(tái)的實(shí)時(shí)運(yùn)動(dòng)控制模塊不僅適用于本文所應(yīng)用六自由度關(guān)節(jié)機(jī)器人，也可適用于其他類型關(guān)節(jié)型機(jī)器人。由實(shí)驗(yàn)的關(guān)節(jié)空間角度變量和角速度變量的時(shí)間軌跡來看：在ROS系統(tǒng)下使用PVT控制模塊運(yùn)動(dòng)控制效果連續(xù)平滑，滿足控制要求，機(jī)器人運(yùn)行平穩(wěn)，可以取代運(yùn)動(dòng)控制器的作用，具有開源、低成本、可擴(kuò)展等特點(diǎn)，為后續(xù)研究和開發(fā)打下了基礎(chǔ)。

[1] Quigley M, Conley K, Gerkey B, et al. ROS: an open-source Robot Operating System [C] //ICRA workshop on open source software,2009, 3(3.2): 5.

[2] Kerr J, Nickels K. Robot operating systems: Bridging the gap between human and robot[C]//System Theory(SSST), 2012 44th Southeastern Symposium on. IEEE,2012:99-104．

[3] 鄧?yán)杳? 趙現(xiàn)朝, 齊臣坤, 等. 開放式步行機(jī)器人實(shí)時(shí)控制系統(tǒng)研究[J]. 機(jī)械設(shè)計(jì)與研究, 2014,30(3):29-33,41.

[4] 林立明, 于東, 胡毅, 等. 一種嵌入式總線運(yùn)動(dòng)控制器的實(shí)時(shí)系統(tǒng)設(shè)計(jì)[J]. 中國機(jī)械工程, 2013, 24(4): 494-499.

[5] 舒志兵,嚴(yán)彩忠. PVT插補(bǔ)及位置伺服變加減速處理[J]. 電氣應(yīng)用,2007(4):86-89.

[6] 謝進(jìn), 檀結(jié)慶, 李聲鋒. 有理三次 Hermite 插值樣條及其逼近性質(zhì)[J]. 工程數(shù)學(xué)學(xué)報(bào), 2011,28(3): 385-392.

[7] 劉艷強(qiáng),王健,單春榮. 基于EtherCAT的多軸運(yùn)動(dòng)控制器研究[J]. 制造技術(shù)與機(jī)床,2008(6):100-103.

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

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

[10] Foote T.tf: The Transform Library[A]. Woburn, MA. 2013. Technologies for Practical Robot Applications (TePRA), 2013 IEEE International Conference on.

ResearchonMulti-axisRobotPVTMotionControlBasedonROS

ZHANG Ying, PING Xue-liang, QIU Heng-tan, YANG Zi-hao

(Jiangsu Province Key Laboratory of Advanced Food Manufacturing Equipment and Technology，School of Mechanical Engineering,Jiangnan University, Wuxi Jiangsu 214122,China)

Traditional robot control system is mostly for specific objects. Due to the poor portability and low rate of the code reusability, A motion control implementation based on robot operating system ROS was proposed. Research on the trajectory planning of PVT algorithm, a PVT motion control module based on ROS is designed to replace some parts function of traditional motion controller, thereby reducing the development cost of the control system and improving the reusability of the code. The experimental results indicate that the control system has good stability and can take part of the traditional motion controller.

PVT motion control; robot operating system (ROS); serial robot

1001-2265(2017)11-0057-04

10.13462/j.cnki.mmtamt.2017.11.015

2016-12-15；

2017-01-17

國家自然科學(xué)基金項(xiàng)目(61305016)

張穎(1992—)，女，江蘇連云港人，江南大學(xué)碩士研究生，研究方向?yàn)榛趓os的機(jī)器人控制系統(tǒng)的實(shí)現(xiàn)，(E-mail)1542005571@qq.com。

TH659;TG68

A

(編輯李秀敏)