視覺引導的機器人關節空間動態軌跡規劃

Vision-guided Robot Joint Space Dynamic Trajectory Planning

過志強1　殷國亮2　肖文皓2　白瑞林2

(無錫信捷電氣股份有限公司1，江蘇 無錫　214072;江南大學輕工過程先進控制教育部重點實驗室2,江蘇 無錫　214122)

視覺引導的機器人關節空間動態軌跡規劃

Vision-guided Robot Joint Space Dynamic Trajectory Planning

過志強1殷國亮2肖文皓2白瑞林2

(無錫信捷電氣股份有限公司1，江蘇 無錫214072;江南大學輕工過程先進控制教育部重點實驗室2,江蘇 無錫214122)

摘要：為實現視覺引導的機器人軌跡精確跟蹤，提出一種機器人關節空間在線軌跡規劃算法。利用工業相機獲取跟蹤軌跡坐標，并將其轉換為對應的機器人關節角度矢量；采用3次B樣條曲線“動態”插值關節位置序列，通過增加B樣條節點向量和控制頂點的方法來動態延長樣條曲線，得到平滑、連續的關節軌跡。選用二自由度并聯機器人進行實驗，實際測試結果表明：該算法能夠在視覺引導下動態規劃出機器人關節空間軌跡，跟蹤誤差在±1 mm以內，保證了關節位置和速度的連續與平滑，提高了軌跡跟蹤精度。

關鍵詞：機器人視覺引導B樣條軌跡規劃在線跟蹤關節空間

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引言

機器人軌跡規劃是軌跡跟蹤控制的基礎，也是機器人執行作業任務的基礎。目前，機器人應用大多還停留在人工示教階段，加入視覺引導是機器人邁向智能化的關鍵一步，研究視覺引導下機器人關節空間軌跡規劃具有重要意義。

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

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

1軌跡跟蹤系統

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

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

圖1　系統硬件方案圖

2算法設計與實現

2.1　B樣條及其性質

B樣條曲線方程為：

(1)

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

(2)

(3)

式中：k為B樣條次數；i為B樣條序號；ui為節點向量，為非遞減序列[9]。

(4)

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

2.2　計算B樣條曲線控制頂點

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

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

(5)

(6)

簡化為：

(7)

這是因為：

(8)

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

(9)

(10)

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

CNd=p

(11)

當構造三次B樣條曲線時，則有：

(12)

vs、ve為起始位置和終止位置設定的機器人關節角速度。

2.3　延長B樣條曲線

經上一節在起始段距離內規劃了機器人各關節空間樣條軌跡，傳送帶連續運動，相機也會定時捕獲軌跡上新的點坐標。新增點坐標經過坐標轉換和逆運算得到各關節角度值，即關節空間B樣條軌跡末端新增了插值點。B樣條允許通過增加節點向量的值并同時增加控制頂點來輔助曲線的設計，在節點向量末端增加節點值，并在末端增加新的控制頂點，可以延長B樣條軌跡。

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

2.4　計算B樣條軌跡上的位置點

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

(13)

設定運動控制器的控制周期T，根據控制周期計算出關節軌跡在每一個時間點上的關節坐標，作為伺服控制的位置輸入，使伺服電機沿著規劃的關節空間軌跡運動。關節軌跡反映在笛卡爾空間則是機器人末端跟蹤目標軌跡運動。

3試驗測試與分析

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

圖2　三次B樣條擬合前后的關節空間軌跡圖

圖3　軌跡跟蹤效果圖

圖4　軌跡跟蹤誤差圖

由圖2可以看出，在視覺引導下可以通過動態延長B樣條曲線的方法實時構造關節空間三次B樣條軌跡，且B樣條曲線擬合能夠很好地平滑關節軌跡，有效降低了機械運動時的抖動與磨損。

根據機器人兩個主動關節處伺服電機編碼器反饋的脈沖信號，將其轉換為角度值，然后采用二自由度并聯機器人正運動學算法計算得到機器人末端的實時坐標。

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

4結束語

本文提出了一種視覺引導的機器人關節空間動態B樣條軌跡規劃方法。該方法的主要特點是：機器人關節空間軌跡規劃的插值點是經過相機坐標提取和逆運動學運算實時得到。針對這種情況，在起始段根據關節空間插值點采用3次B樣條曲線插值方法規劃一段關節軌跡，然后根據新增插值點，通過動態增加樣條曲線節點向量和控制頂點的方法來延長樣條曲線。

實際測試表明：本文提出的算法能夠在視覺引導下實時規劃出機器人關節空間B樣條軌跡，得到的關節軌跡連續平滑，減少了機械振動，且提高了軌跡跟蹤精度。本算法同樣適用于其他類型的并聯或串聯機器人。

參考文獻

[1] 李林峰,馬蕾. 三次均勻B樣條在工業機器人軌跡規劃中的應用研究[J].科學技術與工程,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] 胡小平,彭濤,左富勇.一種基于多項式和Newton插值法的機械手軌跡規劃方法[J].中國機械工程,2012,24(7):2946-2949.

[5] 劉松國,朱世強,吳文祥.具有運動時間約束的機械手最優平滑軌跡規劃[J].電機與控制學報,2009,13(6):897-902.

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

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

[8] 辛洪兵,余躍慶.平面五桿并聯機器人運動學導論[M].北京:國防工業出版社,2007.

[9] 季晨.工業機器人姿態規劃及軌跡優化研究[D].哈爾濱:哈爾濱工業大學,2013.

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

中圖分類號：TP391

文獻標志碼：A

DOI:10.16086/j.cnki.issn1000-0380.201503020

江蘇省科技成果轉化專項資金資助項目(編號：BA2011032)；

江蘇省產學研前瞻性聯合研究基金資助項目(編號：BY2012056)；

江蘇高校優勢學科建設工程基金資助項目(編號：PAPD)。

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

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