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

Vision-guided Robot Joint Space Dynamic Trajectory Planning

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

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

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

Vision-guided Robot Joint Space Dynamic Trajectory Planning

過志強(qiáng)1殷國亮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)換為對應(yīng)的機(jī)器人關(guān)節(jié)角度矢量；采用3次B樣條曲線“動態(tài)”插值關(guān)節(jié)位置序列，通過增加B樣條節(jié)點(diǎn)向量和控制頂點(diǎn)的方法來動態(tài)延長樣條曲線，得到平滑、連續(xù)的關(guān)節(jié)軌跡。選用二自由度并聯(lián)機(jī)器人進(jìn)行實(shí)驗(yàn)，實(shí)際測試結(jié)果表明：該算法能夠在視覺引導(dǎo)下動態(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é)的振動。關(guān)節(jié)空間的軌跡規(guī)劃方法一般采用高階多項(xiàng)式[4]、B樣條插值[5-7]等方法解決軌跡平滑性問題，有效降低了軌跡跟蹤誤差，但是高階多項(xiàng)式會出現(xiàn)龍格現(xiàn)象，造成跟蹤系統(tǒng)不穩(wěn)定。

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

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

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

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

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

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

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

B樣條曲線方程為：

(1)

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

(2)

(3)

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

(4)

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

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

相機(jī)為定時觸發(fā)，采樣間隔恒定，節(jié)點(diǎn)沿參數(shù)軸是均勻分布的，為了計算方便，且主要考慮還需通過增加節(jié)點(diǎn)向量和控制點(diǎn)來延長B樣條曲線，采用均勻的節(jié)點(diǎn)向量來對數(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)個數(shù)據(jù)點(diǎn)pi的k次B樣條曲線方程可寫為：

(5)

(6)

簡化為：

(7)

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

(8)

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

(9)

(10)

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

CNd=p

(11)

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

(12)

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

2.3　延長B樣條曲線

經(jīng)上一節(jié)在起始段距離內(nèi)規(guī)劃了機(jī)器人各關(guān)節(jié)空間樣條軌跡，傳送帶連續(xù)運(yùn)動，相機(jī)也會定時捕獲軌跡上新的點(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)向量的值并同時增加控制頂點(diǎn)來輔助曲線的設(shè)計，在節(jié)點(diǎn)向量末端增加節(jié)點(diǎn)值，并在末端增加新的控制頂點(diǎn)，可以延長B樣條軌跡。

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

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

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

(13)

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

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

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

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

圖3　軌跡跟蹤效果圖

圖4　軌跡跟蹤誤差圖

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

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

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

4結(jié)束語

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

實(shí)際測試表明：本文提出的算法能夠在視覺引導(dǎo)下實(shí)時規(guī)劃出機(jī)器人關(guān)節(jié)空間B樣條軌跡，得到的關(guān)節(jié)軌跡連續(xù)平滑，減少了機(jī)械振動，且提高了軌跡跟蹤精度。本算法同樣適用于其他類型的并聯(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].中國機(jī)械工程,2012,24(7):2946-2949.

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

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

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

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

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

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

中圖分類號：TP391

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

DOI:10.16086/j.cnki.issn1000-0380.201503020

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

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

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

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

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