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

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

ROS平臺下多軸機(jī)器人PVT運動控制研究*

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

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

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

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

0 引言

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

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

1 ROS簡介

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

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

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

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

本文研究的軌跡規(guī)劃插值算法的數(shù)據(jù)來源于高層運動規(guī)劃的關(guān)節(jié)信息，根據(jù)關(guān)節(jié)空間軌跡的角度值和角速度值，求解中間插值點的角度值和角速度值，即已知邊界兩端節(jié)點處函數(shù)值及其一階導(dǎo)數(shù)，求取中間型值點的函數(shù)和一階導(dǎo)數(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個條件代入，解得4個系數(shù)，代入式(1)整理得：

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

(3)

令：

(4)

則三次多項式方程可寫成：

(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ù)值為yi-1,yi，其一階導(dǎo)數(shù)為mi-1,mi，為方便計算，首先對區(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ù)性條件

為計算出yi″(x)，可利用相鄰兩區(qū)間端點處的二階導(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ū)間起點(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個未知量的線性方程組，方程的個數(shù)為n-1，因此不能唯一確定mi，需要兩個限制條件。

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

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

2m1+u1m2=c1-λ1m0

(21)

第n-1個方程組為：

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

(22)

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

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

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

(23)

上式的系數(shù)為三對角矩陣，已知m0和mn，可用“追趕法”求解。求出所有mi后，分段埃爾米特曲線可由式(11)確定，由此得出整個區(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)

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

圖1 運動控制功能模塊結(jié)構(gòu)

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

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

3.2 ROS下PVT控制策略

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

圖3 運動控制模塊

運動控制模塊負(fù)責(zé)系統(tǒng)最重要的底層PID控制和樣條曲線插補，運動控制模塊的實現(xiàn)需借助通用機(jī)器人描述(URDF，Unified Robot Description Format)[8]所蘊含的機(jī)器人運動學(xué)信息，在調(diào)用URDF之前必須對URDF進(jìn)行簡化，剔除與插補、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)容描述連接兩個連桿的關(guān)節(jié)的參數(shù)[9]。

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

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

3.3 ROS平臺下PVT控制驗證

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

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

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

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

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

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

4 結(jié)束語

基于ROS平臺的實時運動控制模塊不僅適用于本文所應(yīng)用六自由度關(guān)節(jié)機(jī)器人，也可適用于其他類型關(guān)節(jié)型機(jī)器人。由實驗的關(guān)節(jié)空間角度變量和角速度變量的時間軌跡來看：在ROS系統(tǒng)下使用PVT控制模塊運動控制效果連續(xù)平滑，滿足控制要求，機(jī)器人運行平穩(wěn)，可以取代運動控制器的作用，具有開源、低成本、可擴(kuò)展等特點，為后續(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ī)器人實時控制系統(tǒng)研究[J]. 機(jī)械設(shè)計與研究, 2014,30(3):29-33,41.

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

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

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

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

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

[9] 錢偉. 基于ROS的移動操作機(jī)械臂底層規(guī)劃及運動仿真[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é)基金項目(61305016)

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

TH659;TG68

A

(編輯李秀敏)