Vision-guided Robot Joint Space Dynamic Trajectory Planning

過志強(qiáng)1　殷國(guó)亮2　肖文皓2　白瑞林2

(無錫信捷電氣股份有限公司1，江蘇 無錫　214072;江南大學(xué)輕工過程先進(jìn)控制教育部重點(diǎn)實(shí)驗(yàn)室2,江蘇 無錫　214122)

視覺引導(dǎo)的機(jī)器人關(guān)節(jié)空間動(dòng)態(tài)軌跡規(guī)劃

Vision-guided Robot Joint Space Dynamic Trajectory Planning

過志強(qiáng)1殷國(guó)亮2肖文皓2白瑞林2

(無錫信捷電氣股份有限公司1，江蘇 無錫214072;江南大學(xué)輕工過程先進(jìn)控制教育部重點(diǎn)實(shí)驗(yàn)室2,江蘇 無錫214122)

摘要：為實(shí)現(xiàn)視覺引導(dǎo)的機(jī)器人軌跡精確跟蹤，提出一種機(jī)器人關(guān)節(jié)空間在線軌跡規(guī)劃算法。利用工業(yè)相機(jī)獲取跟蹤軌跡坐標(biāo)，并將其轉(zhuǎn)換為對(duì)應(yīng)的機(jī)器人關(guān)節(jié)角度矢量；采用3次B樣條曲線“動(dòng)態(tài)”插值關(guān)節(jié)位置序列，通過增加B樣條節(jié)點(diǎn)向量和控制頂點(diǎn)的方法來動(dòng)態(tài)延長(zhǎng)樣條曲線，得到平滑、連續(xù)的關(guān)節(jié)軌跡。選用二自由度并聯(lián)機(jī)器人進(jìn)行實(shí)驗(yàn)，實(shí)際測(cè)試結(jié)果表明：該算法能夠在視覺引導(dǎo)下動(dòng)態(tài)規(guī)劃出機(jī)器人關(guān)節(jié)空間軌跡，跟蹤誤差在±1 mm以內(nèi)，保證了關(guān)節(jié)位置和速度的連續(xù)與平滑，提高了軌跡跟蹤精度。

關(guān)鍵詞：機(jī)器人視覺引導(dǎo)B樣條軌跡規(guī)劃在線跟蹤關(guān)節(jié)空間

Abstract：To achieve accurate vision guided robot trajectory tracking, the robot joint space online trajectory planning algorithm is proposed. The tracking trajectory coordinates are obtained by industrial cameras, and converted to corresponding robot joint angle vector. By adopting 3rd order B-spline curve dynamic interpolation joint position sequence, then by increasing the B-spline node vectors, and the method of control vertices to dynamically extend spline curve for obtaining smooth and continuous joint trajectory. The two degrees of freedom parallel robot is selected for experiments, the result of practical tests show that the algorithm can dynamically programming the robot joint space trajectory tracking under vision guidance, with error ±1 mm or less, to ensure the continuous and smooth joint position and velocity, and improve track tracking accuracy.

Keywords：RobotVision-guidedB-splineTrajectory planningOnline trackingJoint space

0引言

機(jī)器人軌跡規(guī)劃是軌跡跟蹤控制的基礎(chǔ)，也是機(jī)器人執(zhí)行作業(yè)任務(wù)的基礎(chǔ)。目前，機(jī)器人應(yīng)用大多還停留在人工示教階段，加入視覺引導(dǎo)是機(jī)器人邁向智能化的關(guān)鍵一步，研究視覺引導(dǎo)下機(jī)器人關(guān)節(jié)空間軌跡規(guī)劃具有重要意義。

李林峰[1]和Ming J.Tsai[2]利用B樣條方法模擬末端執(zhí)行器軌跡，構(gòu)造三維跟蹤軌跡。這種方法只適用于離線編程系統(tǒng)的機(jī)器人，無法實(shí)現(xiàn)在線跟蹤。Chanyu Wu[3]等利用結(jié)構(gòu)光和CCD相機(jī)構(gòu)造三維點(diǎn)陣列，然后用B樣條逼近三維曲線，完成鞋面噴涂任務(wù)并證明了方法的有效性，但這種方法無法避免各關(guān)節(jié)的振動(dòng)。關(guān)節(jié)空間的軌跡規(guī)劃方法一般采用高階多項(xiàng)式[4]、B樣條插值[5-7]等方法解決軌跡平滑性問題，有效降低了軌跡跟蹤誤差，但是高階多項(xiàng)式會(huì)出現(xiàn)龍格現(xiàn)象，造成跟蹤系統(tǒng)不穩(wěn)定。

從以上的研究可以看出，B樣條曲線能夠保證軌跡的連續(xù)與平滑，加入視覺能夠完成軌跡定位，但并非實(shí)時(shí)引導(dǎo)。本文提出了一種在視覺引導(dǎo)的同時(shí)進(jìn)行機(jī)器人關(guān)節(jié)空間B樣條軌跡規(guī)劃的方法，視覺獲取坐標(biāo)點(diǎn)信息，同時(shí)利用這些坐標(biāo)信息進(jìn)行關(guān)節(jié)空間軌跡規(guī)劃與跟蹤控制，提高了機(jī)器人軌跡跟蹤的精度、平滑性和適應(yīng)性。

1軌跡跟蹤系統(tǒng)

視覺引導(dǎo)的機(jī)器人軌跡跟蹤系統(tǒng)包括：一個(gè)二自由度并聯(lián)機(jī)器人、一臺(tái)智能相機(jī)和一個(gè)附著軌跡的傳送帶。二自由度并聯(lián)機(jī)器人固定在一個(gè)立體支架內(nèi)，在兩個(gè)方向上運(yùn)動(dòng)；智能相機(jī)固定在支架前端；傳送帶沿著垂直于并聯(lián)機(jī)器人運(yùn)動(dòng)平面的方向運(yùn)動(dòng)。并聯(lián)機(jī)器人為無錫信捷電氣有限公司自主研發(fā)工業(yè)機(jī)器人，智能相機(jī)采用公司自主研發(fā)的640×480分辨率，30萬(wàn)像素的CMOS黑白相機(jī)，其配置軟件可提供軌跡坐標(biāo)提取的作業(yè)工具等。

基于并聯(lián)機(jī)器人控制系統(tǒng)對(duì)靈活性、開放性和實(shí)時(shí)性的要求，采用PC+NI PCIe-7852R多功能RIO板卡的方式構(gòu)建控制平臺(tái)。該RIO板卡提供可編程FPGA芯片和96個(gè)數(shù)字輸入輸出口，適合板級(jí)處理和靈活的I/O操作，如給伺服驅(qū)動(dòng)器的PWM方波輸出和方向信號(hào)輸出、伺服電機(jī)編碼器反饋信號(hào)的高速采集、濾波和計(jì)數(shù)、并聯(lián)機(jī)器人報(bào)警信號(hào)、限位信號(hào)、原點(diǎn)信號(hào)的輸入和相機(jī)的通信模塊等都可以在板卡上完成。程序在硬件中運(yùn)行，有助于直接而及時(shí)地控制全部I/O信號(hào)，保證了控制的實(shí)時(shí)性。PC機(jī)作為工控機(jī)完成機(jī)器人逆運(yùn)動(dòng)學(xué)運(yùn)算、軌跡規(guī)劃運(yùn)算、控制算法等[8]。利用伺服電機(jī)驅(qū)動(dòng)和編碼器反饋實(shí)現(xiàn)閉環(huán)控制，同時(shí)可對(duì)機(jī)器人軌跡精度和速度特性進(jìn)行分析。系統(tǒng)硬件方案如圖1所示。

圖1　系統(tǒng)硬件方案圖

2算法設(shè)計(jì)與實(shí)現(xiàn)

2.1　B樣條及其性質(zhì)

B樣條曲線方程為：

(1)

式中：di(i=0,1,…，n)為n+1個(gè)控制頂點(diǎn)；Ni,k(u)(i=0,1,…,n)為k次規(guī)范B樣條基函數(shù)，也稱之為混合函數(shù)。按照Cox-deBoor遞歸公式定義為：

(2)

(3)

式中：k為B樣條次數(shù)；i為B樣條序號(hào)；ui為節(jié)點(diǎn)向量，為非遞減序列[9]。

(4)

這表明B樣條曲線的局部性質(zhì)的一個(gè)方面，即在k次B樣條曲線上定義域內(nèi)，每個(gè)樣條曲線段僅受(k+1)個(gè)控制頂點(diǎn)的影響，與其他頂點(diǎn)無關(guān)。

2.2　計(jì)算B樣條曲線控制頂點(diǎn)

相機(jī)為定時(shí)觸發(fā)，采樣間隔恒定，節(jié)點(diǎn)沿參數(shù)軸是均勻分布的，為了計(jì)算方便，且主要考慮還需通過增加節(jié)點(diǎn)向量和控制點(diǎn)來延長(zhǎng)B樣條曲線，采用均勻的節(jié)點(diǎn)向量來對(duì)數(shù)據(jù)點(diǎn)參數(shù)化。由于B樣條曲線在定義域上一點(diǎn)ui處有L重節(jié)點(diǎn)的連續(xù)性不低于k-L階，采用均勻節(jié)點(diǎn)向量避免了重節(jié)點(diǎn)，保證了在增加節(jié)點(diǎn)后曲線的連續(xù)性。確定節(jié)點(diǎn)向量為：u=[0,1,…,n+2k]。

用于插值(n+1)個(gè)數(shù)據(jù)點(diǎn)pi的k次B樣條曲線方程可寫為：

(5)

(6)

簡(jiǎn)化為：

(7)

這是因?yàn)椋?/p>

(8)

對(duì)于k次B樣條曲線還需要增加(k-1)個(gè)條件，通常由邊界條件給定。B樣條曲線上一點(diǎn)處的r階導(dǎo)數(shù)pr(u)可按如下遞推公式計(jì)算：

(9)

(10)

于是，可以聯(lián)立(n+k)個(gè)方程式求出B樣條插值曲線的控制頂點(diǎn)，求解方程為：

CNd=p

(11)

當(dāng)構(gòu)造三次B樣條曲線時(shí)，則有：

(12)

vs、ve為起始位置和終止位置設(shè)定的機(jī)器人關(guān)節(jié)角速度。

2.3　延長(zhǎng)B樣條曲線

經(jīng)上一節(jié)在起始段距離內(nèi)規(guī)劃了機(jī)器人各關(guān)節(jié)空間樣條軌跡，傳送帶連續(xù)運(yùn)動(dòng)，相機(jī)也會(huì)定時(shí)捕獲軌跡上新的點(diǎn)坐標(biāo)。新增點(diǎn)坐標(biāo)經(jīng)過坐標(biāo)轉(zhuǎn)換和逆運(yùn)算得到各關(guān)節(jié)角度值，即關(guān)節(jié)空間B樣條軌跡末端新增了插值點(diǎn)。B樣條允許通過增加節(jié)點(diǎn)向量的值并同時(shí)增加控制頂點(diǎn)來輔助曲線的設(shè)計(jì)，在節(jié)點(diǎn)向量末端增加節(jié)點(diǎn)值，并在末端增加新的控制頂點(diǎn)，可以延長(zhǎng)B樣條軌跡。

根據(jù)方程可以求解得到每一個(gè)新增插值點(diǎn)pi對(duì)應(yīng)的新增控制頂點(diǎn)di+k-1。方程保證了B樣條曲線在分段連接處與相應(yīng)的新增數(shù)據(jù)點(diǎn)一致。如果判定pi為最后一個(gè)插值點(diǎn)，則修正最后一個(gè)控制頂點(diǎn)di+k-1等于di-1，使軌跡在末端點(diǎn)pi處的速度為零。由于節(jié)點(diǎn)向量在動(dòng)態(tài)插值過程中均勻遞增，所以k階B樣條仍具有(k-1)階幾何連續(xù)性。

2.4　計(jì)算B樣條軌跡上的位置點(diǎn)

經(jīng)前述兩步確定了節(jié)點(diǎn)矢量ui、B樣條次數(shù)k和控制頂點(diǎn)di，即可采用deBoor遞推公式計(jì)算B樣條曲線上對(duì)應(yīng)于每一個(gè)控制周期的位置點(diǎn)p(u)。其遞推公式為：

(13)

設(shè)定運(yùn)動(dòng)控制器的控制周期T，根據(jù)控制周期計(jì)算出關(guān)節(jié)軌跡在每一個(gè)時(shí)間點(diǎn)上的關(guān)節(jié)坐標(biāo)，作為伺服控制的位置輸入，使伺服電機(jī)沿著規(guī)劃的關(guān)節(jié)空間軌跡運(yùn)動(dòng)。關(guān)節(jié)軌跡反映在笛卡爾空間則是機(jī)器人末端跟蹤目標(biāo)軌跡運(yùn)動(dòng)。

3試驗(yàn)測(cè)試與分析

以二自由度并聯(lián)機(jī)器人為對(duì)象進(jìn)行試驗(yàn)。相機(jī)的采樣頻率為20Hz，傳送帶的運(yùn)動(dòng)速度為100mm/s，其上附著有軌跡，類似正弦函數(shù)曲線，振幅約為40mm；相機(jī)與機(jī)器人的縱向距離為300mm，經(jīng)計(jì)算，選定運(yùn)動(dòng)開始后相機(jī)獲得的前50個(gè)坐標(biāo)點(diǎn)來規(guī)劃初始段關(guān)節(jié)空間樣條軌跡，之后采用延長(zhǎng)樣條曲線的方法來動(dòng)態(tài)規(guī)劃關(guān)節(jié)軌跡。試驗(yàn)采用LabVIEW編程，經(jīng)3次B樣條曲線擬合前和擬合后的一段關(guān)節(jié)軌跡如圖2所示，橫坐標(biāo)為時(shí)間，縱坐標(biāo)為關(guān)節(jié)角度值。任務(wù)空間實(shí)際跟蹤軌跡如圖3所示，橫坐標(biāo)為時(shí)間，縱坐標(biāo)為機(jī)器人末端坐標(biāo)系X方向坐標(biāo)值，單位為mm。對(duì)應(yīng)圖3中跟蹤軌跡的誤差如圖4所示。

圖2　三次B樣條擬合前后的關(guān)節(jié)空間軌跡圖

圖3　軌跡跟蹤效果圖

圖4　軌跡跟蹤誤差圖

由圖2可以看出，在視覺引導(dǎo)下可以通過動(dòng)態(tài)延長(zhǎng)B樣條曲線的方法實(shí)時(shí)構(gòu)造關(guān)節(jié)空間三次B樣條軌跡，且B樣條曲線擬合能夠很好地平滑關(guān)節(jié)軌跡，有效降低了機(jī)械運(yùn)動(dòng)時(shí)的抖動(dòng)與磨損。

根據(jù)機(jī)器人兩個(gè)主動(dòng)關(guān)節(jié)處伺服電機(jī)編碼器反饋的脈沖信號(hào)，將其轉(zhuǎn)換為角度值，然后采用二自由度并聯(lián)機(jī)器人正運(yùn)動(dòng)學(xué)算法計(jì)算得到機(jī)器人末端的實(shí)時(shí)坐標(biāo)。

由圖3和圖4可以看出，機(jī)器人末端能夠很好地跟蹤目標(biāo)軌跡，跟蹤誤差在±1 mm以內(nèi)。誤差主要來源于機(jī)械間隙與伺服響應(yīng)，能夠滿足一些精度要求不是很高的工業(yè)應(yīng)用。

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

本文提出了一種視覺引導(dǎo)的機(jī)器人關(guān)節(jié)空間動(dòng)態(tài)B樣條軌跡規(guī)劃方法。該方法的主要特點(diǎn)是：機(jī)器人關(guān)節(jié)空間軌跡規(guī)劃的插值點(diǎn)是經(jīng)過相機(jī)坐標(biāo)提取和逆運(yùn)動(dòng)學(xué)運(yùn)算實(shí)時(shí)得到。針對(duì)這種情況，在起始段根據(jù)關(guān)節(jié)空間插值點(diǎn)采用3次B樣條曲線插值方法規(guī)劃一段關(guān)節(jié)軌跡，然后根據(jù)新增插值點(diǎn)，通過動(dòng)態(tài)增加樣條曲線節(jié)點(diǎn)向量和控制頂點(diǎn)的方法來延長(zhǎng)樣條曲線。

實(shí)際測(cè)試表明：本文提出的算法能夠在視覺引導(dǎo)下實(shí)時(shí)規(guī)劃出機(jī)器人關(guān)節(jié)空間B樣條軌跡，得到的關(guān)節(jié)軌跡連續(xù)平滑，減少了機(jī)械振動(dòng)，且提高了軌跡跟蹤精度。本算法同樣適用于其他類型的并聯(lián)或串聯(lián)機(jī)器人。

參考文獻(xiàn)

[1] 李林峰,馬蕾. 三次均勻B樣條在工業(yè)機(jī)器人軌跡規(guī)劃中的應(yīng)用研究[J].科學(xué)技術(shù)與工程,2013(13):3621-3625，3646.

[2] Tsai M J,Nai A.An automatic golf head robotic welding system using 3D machine vision system[C]∥IEEE Workshop on Advanced robotics and Its Social Impacts,2008:1-6.

[3] Wu Chuanyu,He Leiying,Li Qinchuan,et al.Research on the generation of trajectory for shoe upper spraying based on structured light[C]∥IEEE International Conference on Industrial Technology,2008:1-5.

[4] 胡小平,彭濤,左富勇.一種基于多項(xiàng)式和Newton插值法的機(jī)械手軌跡規(guī)劃方法[J].中國(guó)機(jī)械工程,2012,24(7):2946-2949.

[5] 劉松國(guó),朱世強(qiáng),吳文祥.具有運(yùn)動(dòng)時(shí)間約束的機(jī)械手最優(yōu)平滑軌跡規(guī)劃[J].電機(jī)與控制學(xué)報(bào),2009,13(6):897-902.

[6] 肖文皓,白瑞林,許凡,等.基于信賴域算法的機(jī)械臂時(shí)間最優(yōu)軌跡規(guī)劃[J].傳感器與微系統(tǒng),2013(6):77-80.

[7] 李小霞,汪木蘭,劉坤,等.基于五次B樣條的機(jī)械手關(guān)節(jié)空間平滑軌跡規(guī)劃[J].組合機(jī)床與自動(dòng)化加工技術(shù),2012(8):39-42.

[8] 辛洪兵,余躍慶.平面五桿并聯(lián)機(jī)器人運(yùn)動(dòng)學(xué)導(dǎo)論[M].北京:國(guó)防工業(yè)出版社,2007.

[9] 季晨.工業(yè)機(jī)器人姿態(tài)規(guī)劃及軌跡優(yōu)化研究[D].哈爾濱:哈爾濱工業(yè)大學(xué),2013.

[10]毛征宇,劉中堅(jiān).一種三次均勻B樣條曲線的軌跡規(guī)劃方法[J].中國(guó)機(jī)械工程,2010,21(8):2569-2572,2577.------------------------------------------------------------------------------------------------

中圖分類號(hào)：TP391

文獻(xiàn)標(biāo)志碼：A

DOI:10.16086/j.cnki.issn1000-0380.201503020

江蘇省科技成果轉(zhuǎn)化專項(xiàng)資金資助項(xiàng)目(編號(hào)：BA2011032)；

江蘇省產(chǎn)學(xué)研前瞻性聯(lián)合研究基金資助項(xiàng)目(編號(hào)：BY2012056)；

江蘇高校優(yōu)勢(shì)學(xué)科建設(shè)工程基金資助項(xiàng)目(編號(hào)：PAPD)。

修改稿收到日期：2014-08-21。

第一作者過志強(qiáng)(1969-)，男，1991年畢業(yè)于江南大學(xué)工業(yè)電氣自動(dòng)化專業(yè)，獲學(xué)士學(xué)位，工程師；主要從事工業(yè)自動(dòng)化系統(tǒng)與裝備的研究。