李 浩，盧 軍，孫姝麗

(陜西科技大學(xué) 機(jī)電工程學(xué)院，西安 710021)

0 引言

笛卡爾空間下的軌跡規(guī)劃能夠直觀地描述路徑，運(yùn)動(dòng)軌跡明確[1-2]，獲得了廣泛的研究與應(yīng)用。主要分為位姿規(guī)劃和速度規(guī)劃[3]，在笛卡爾平面坐標(biāo)系下，位姿規(guī)劃的主要任務(wù)是擬合運(yùn)動(dòng)軌跡，采用Akima算法獲得的軌跡曲線擬合程度高，且保證了段內(nèi)與段間的連續(xù)性。來(lái)燕菁等[4]通過(guò)仿真驗(yàn)證了修正弦長(zhǎng)參數(shù)化法Akima插值曲線擬合效果優(yōu)于累加弦長(zhǎng)參數(shù)化法；周鵬等[5]以雙弦與弦切誤差約束條件對(duì)Akima曲線進(jìn)行分段擬合，測(cè)試了不同閾值下的Akima曲線相關(guān)誤差。速度規(guī)劃中S型速度曲線平滑性好且運(yùn)算速度快，Jahanpour J等[6]提出采用S型速度規(guī)劃可有效降低加速度不連續(xù)對(duì)系統(tǒng)的影響；F Zou[7]提出了在加速和減速階段時(shí)間不對(duì)稱的改進(jìn)S型速度曲線規(guī)劃算法，改善了系統(tǒng)響應(yīng)性能。但傳統(tǒng)的軌跡規(guī)劃方法大多是在特定的工作條件下以離線編程的方式對(duì)末端執(zhí)行機(jī)構(gòu)的位姿與速度等進(jìn)行規(guī)劃，實(shí)時(shí)性差且缺乏環(huán)境感知能力。

針對(duì)以上問(wèn)題，本文提出一種基于視覺(jué)引導(dǎo)的機(jī)器人軌跡規(guī)劃方法，在機(jī)器人控制中引入視覺(jué)引導(dǎo)技術(shù)，通過(guò)視覺(jué)圖像獲取目標(biāo)軌跡輪廓離散點(diǎn)，以弧長(zhǎng)參數(shù)化Akima法擬合運(yùn)動(dòng)軌跡，引導(dǎo)機(jī)器人采用改進(jìn)的五段式S型速度規(guī)劃實(shí)現(xiàn)任意給定軌跡的運(yùn)動(dòng)，提高了軌跡質(zhì)量及機(jī)器人系統(tǒng)的智能性與靈活性。

1 視覺(jué)引導(dǎo)

1.1 手眼標(biāo)定

基于視覺(jué)引導(dǎo)控制工業(yè)機(jī)器人運(yùn)動(dòng)，首先要對(duì)機(jī)器人末端執(zhí)行機(jī)構(gòu)與相機(jī)之間的位置關(guān)系進(jìn)行Eye-in-hand手眼標(biāo)定[8]，然后通過(guò)標(biāo)定矩陣實(shí)現(xiàn)視覺(jué)獲取的待測(cè)圖像中目標(biāo)物體的像素坐標(biāo)與機(jī)器人機(jī)械坐標(biāo)間的坐標(biāo)轉(zhuǎn)換。

Eye-in-hand手眼標(biāo)定過(guò)程如下：

(1)利用Grab_image算子在線采集標(biāo)定圖像；

(2)提取標(biāo)定圖像中的Mark點(diǎn)：對(duì)圖像進(jìn)行Threshold閾值分割和Select_shape形狀特征選擇,用于區(qū)分Mark點(diǎn)與圖像背景；對(duì)所選Mark點(diǎn)區(qū)域進(jìn)行Sort_region排序，調(diào)用Area_center算子計(jì)算Mark點(diǎn)中心坐標(biāo)如圖1所示。

圖1 Mark點(diǎn)中心圖像坐標(biāo)

(3)控制機(jī)器人末端執(zhí)行機(jī)構(gòu)按順序移動(dòng)至各Mark點(diǎn)中心，記錄各Mark點(diǎn)中心對(duì)應(yīng)的機(jī)械坐標(biāo)；

(4)根據(jù)9組對(duì)應(yīng)坐標(biāo)建立HomMat2D標(biāo)定矩陣并獲取標(biāo)定矩陣各仿射變換參數(shù)的值。

(1)

式中，Tx為沿X方向平移的距離；Ty為沿Y方向平移的距離；Phi為旋轉(zhuǎn)角度；Theta為斜切角度；Sx為X方向的縮放因子；Sy為Y方向的縮放因子。

1.2 目標(biāo)輪廓提取

圖像輪廓是以某種方式表示圖像中曲線上點(diǎn)的列表，獲取一幅圖像中目標(biāo)物體的輪廓在圖像處理及應(yīng)用中扮演著重要的角色[9]。基于視覺(jué)引導(dǎo)的機(jī)器人軌跡規(guī)劃算法可面向任意曲線，由于光源等工況環(huán)境的影響，會(huì)在待處理圖像中產(chǎn)生噪點(diǎn)[10]，需對(duì)采集圖像進(jìn)行處理以獲取準(zhǔn)確的曲線輪廓。

(1)對(duì)采集的目標(biāo)圖像進(jìn)行灰度處理，將24位RGB圖像轉(zhuǎn)化為8位灰度圖像；

(2)使用Threshold算子對(duì)灰度圖像進(jìn)行閾值分割，將圖像分割為目標(biāo)與背景，從而提取到目標(biāo)輪廓區(qū)域[11]；

(3)目標(biāo)輪廓區(qū)域局部出現(xiàn)“毛刺”，利用openning_circle算子對(duì)圖像執(zhí)行開(kāi)運(yùn)算操作，平滑目標(biāo)輪廓，消除“毛刺”；

(4)通過(guò)skeleton算子提取目標(biāo)區(qū)域中央的骨骼部分，利用gen_contour_region_xld算子根據(jù)輪廓區(qū)域創(chuàng)建一個(gè)輪廓XLD，最后調(diào)用get_contour_xld算子計(jì)算目標(biāo)輪廓上各點(diǎn)的像素坐標(biāo)；

(5)將輪廓上各點(diǎn)的像素坐標(biāo)通過(guò)HomMat2D標(biāo)定矩陣轉(zhuǎn)換為對(duì)應(yīng)的機(jī)械坐標(biāo)，并將數(shù)據(jù)發(fā)送至控制器，從而根據(jù)識(shí)別得到的像素坐標(biāo)引導(dǎo)末端執(zhí)行機(jī)構(gòu)完成相應(yīng)動(dòng)作。圖像處理及算法流程如圖2和圖3所示。

(a) 視覺(jué)獲取圖像 (b) 局部區(qū)域出現(xiàn)毛刺

(c) 開(kāi)運(yùn)算消除毛刺 (d) 輪廓骨架 圖2 圖像處理

圖3 目標(biāo)輪廓提取算法流程圖

2 Akima法軌跡擬合

2.1 Akima法基本原理

Akima算法是Hiroshi Akima提出的一種在給定的數(shù)據(jù)點(diǎn)中，每?jī)蓚€(gè)點(diǎn)建立一條三次多項(xiàng)式擬合曲線，且保證整條曲線一階連續(xù)可導(dǎo)的分段三次多項(xiàng)式曲線擬合算法[12]。

將已知的n個(gè)給定數(shù)據(jù)點(diǎn)Pi(xi,yi)(i=1,2,...,n)按x坐標(biāo)軸嚴(yán)格升序排列，在任意兩個(gè)相鄰的數(shù)據(jù)點(diǎn)Pi(xi,yi)與Pi+1(xi+1,yi+1)間建立一條三次多項(xiàng)式曲線：

y(x)=a0+a1(x-xi)+a2(x-xi)2+a3(x-xi)3

(2)

由兩點(diǎn)坐標(biāo)可得已知條件：

(3)

代入式(2)中可得：

(4)

其中，mi為相鄰兩個(gè)數(shù)據(jù)點(diǎn)間線段的斜率。

(5)

di為數(shù)據(jù)點(diǎn)處的切線斜率，對(duì)于首末數(shù)據(jù)點(diǎn)需補(bǔ)充定義4個(gè)輔助斜率以求得各數(shù)據(jù)點(diǎn)Pi(xi,yi)處的切線斜率di，計(jì)算如下：

(6)

(7)

2.2 篩選約束點(diǎn)

在機(jī)器人運(yùn)動(dòng)過(guò)程中，不同的輪廓軌跡對(duì)應(yīng)的插補(bǔ)運(yùn)動(dòng)點(diǎn)分布不同，曲率變化較小的平直輪廓插補(bǔ)點(diǎn)較分散，曲率較大的輪廓處插補(bǔ)點(diǎn)密集，且隨曲率的增大而密度增加，因此需對(duì)視覺(jué)獲取的軌跡輪廓離散點(diǎn)進(jìn)行篩選。

采用雙弦誤差與弦切誤差兩種約束條件對(duì)視覺(jué)獲取的目標(biāo)輪廓離散點(diǎn)進(jìn)行預(yù)處理。圖4a中，li-1，li為三個(gè)相鄰輪廓點(diǎn)Pi-1，Pi，Pi+1間的連線長(zhǎng)度，R為三點(diǎn)確定唯一圓的半徑，δ1和δ2為雙弦誤差。雙弦誤差δ1，δ2均在誤差閾值范圍內(nèi)，則認(rèn)定小線段Pi-1Pi和PiPi+1為符合Akima算法擬合條件的連續(xù)小線段。對(duì)于滿足雙弦誤差的數(shù)據(jù)點(diǎn)進(jìn)行弦切誤差約束測(cè)試，圖4b中N為相鄰兩數(shù)據(jù)點(diǎn)Pi-1和Pi切線的交點(diǎn)，d為交點(diǎn)N到直線PiPi+1的距離，Pi-1N和PiN分別為數(shù)據(jù)點(diǎn)Pi-1和Pi點(diǎn)處的弦切值[6]。

(a) 雙弦誤差 (b) 弦切誤差 圖4 誤差測(cè)試

設(shè)定約束閾值條件，依次搜索視覺(jué)獲取的軌跡輪廓點(diǎn)，當(dāng)滿足約束條件時(shí)繼續(xù)搜索；當(dāng)搜索到不滿足閾值條件的數(shù)據(jù)點(diǎn)，暫停搜索，將其作為分段點(diǎn)，擬合該段Akima曲線，之后繼續(xù)搜索下一個(gè)分段點(diǎn)，直至擬合出整條Akima曲線。

2.3 參數(shù)化Akima曲線

常用的參數(shù)化方法主要有累加弦長(zhǎng)參數(shù)法和修正弦長(zhǎng)參數(shù)法。在累加弦長(zhǎng)參數(shù)法參數(shù)化過(guò)程中，絕對(duì)曲率偏大曲線段會(huì)出現(xiàn)實(shí)際弦長(zhǎng)偏短的情況，針對(duì)這種情況，修正弦長(zhǎng)參數(shù)法引入了修正系數(shù)ki對(duì)弦長(zhǎng)做出了修正，獲得的修正弦長(zhǎng)更接近于實(shí)際弧長(zhǎng)，故而擬合效果更佳[4]。

設(shè)前后相鄰弦線夾角的外角為θi：

(8)

引入修正系數(shù)ki為：

(9)

求得數(shù)據(jù)點(diǎn)Pi(xi,yi)處的修正弦長(zhǎng)為：

(10)

其中，

(11)

3 S型速度規(guī)劃

傳統(tǒng)的S型速度曲線為7段式曲線[13]，如圖5所示，即加加速段(Ⅰ)、勻加速段(Ⅱ)、減加速段(Ⅲ)、勻速段(Ⅳ)，加減速段(Ⅴ)、勻減速段(Ⅵ)、減減速段(Ⅶ)。

圖5 傳統(tǒng)S型曲線

圖5中，以ti表示階段變化時(shí)刻，Ti表示各段用時(shí)，vmax表示最大速度，amax表示最大加速度，J表示加加速度，設(shè)定S為路徑長(zhǎng)度，考慮到軌跡曲線的對(duì)稱性，可知各段用時(shí)Ti為：

(12)

(13)

(14)

又知各階段的加加速度為定值，通過(guò)對(duì)各段用時(shí)Ti二次積分即可得到速度曲線函數(shù)：

(15)

推得分段運(yùn)行距離為：

(16)

最終獲得S型速度規(guī)劃位移函數(shù)為：

S(t)=

(17)

根據(jù)傳統(tǒng)7段式S型速度規(guī)劃曲線的幾何對(duì)稱性可知，在位移約束條件S>2S3下，7段式S型速度曲線可達(dá)到最大速度vmax和最大加速度amax。當(dāng)改變位移、速度、加速度、加加速度等約束條件后，通過(guò)改變各階段用時(shí)Ti可改進(jìn)S型速度規(guī)劃曲線，一次積分加加速度J得到改進(jìn)的加速度曲線如圖6所示，推導(dǎo)求得對(duì)應(yīng)的約束條件如下：

(a) 情形1

(b) 情形2

(c) 情形3 圖6 改進(jìn)的加速度曲線

(18)

(19)

(20)

情形3：S<2(S1+S3)，此時(shí)T2=T4=T6=0，即不存在勻加速段(Ⅱ)、勻速段(Ⅳ)和勻減速段(Ⅵ)，S型速度規(guī)劃曲線改進(jìn)為四段式，在約束條件內(nèi)，最大速度vmax、最大加速度amax均不能達(dá)到，修正最大速度和最大加速度為：

(21)

(22)

4 仿真分析及實(shí)驗(yàn)驗(yàn)證

4.1 Matlab仿真分析

通過(guò)雙弦誤差與弦切誤差約束條件篩選視覺(jué)獲取的軌跡輪廓離散點(diǎn)作為實(shí)驗(yàn)數(shù)據(jù)點(diǎn)，在Matlab2014環(huán)境下進(jìn)行弧長(zhǎng)參數(shù)化Akima軌跡擬合仿真測(cè)試，結(jié)果如圖7所示。對(duì)視覺(jué)獲取的216個(gè)輪廓軌跡離散點(diǎn)進(jìn)行雙弦誤差與弦切誤差約束誤差測(cè)試，通過(guò)調(diào)整誤差約束條件δ與d，最終選取47個(gè)數(shù)據(jù)點(diǎn)擬合Akima曲線。實(shí)驗(yàn)結(jié)果表示：獲得的Akima曲線能夠很好的描繪視覺(jué)數(shù)據(jù)點(diǎn)的形狀與凸凹性，相同x坐標(biāo)處擬合點(diǎn)與理論點(diǎn)的最大誤差△y為0.113mm，平均誤差0.0292mm。

(a) Akima擬合曲線

(b) 局部放大圖1 (c) 局部放大圖2 圖7 Akima擬合曲線

4.2 實(shí)驗(yàn)驗(yàn)證

本次設(shè)計(jì)的直角坐標(biāo)機(jī)器人主要用于小型零部件的涂膠密封，為獲得更高的涂膠質(zhì)量，需限定膠槍的運(yùn)動(dòng)速度，即在速度規(guī)劃中引入了路徑長(zhǎng)度和運(yùn)動(dòng)速度約束條件。采用修正弦長(zhǎng)參數(shù)法獲得的Akima曲線插補(bǔ)路徑長(zhǎng)度為1200unit，選取末端執(zhí)行機(jī)構(gòu)進(jìn)給速度為1300unit/s，以3000，1200，6000，6000(unit/s2)4組實(shí)驗(yàn)參數(shù)進(jìn)行實(shí)驗(yàn)，獲得的7段式、6段式、5段式和4段式S型速度規(guī)劃曲線實(shí)驗(yàn)數(shù)據(jù)如表1所示。

表1 4種S型曲線實(shí)驗(yàn)結(jié)果

由插補(bǔ)實(shí)驗(yàn)結(jié)果可知：引入路徑長(zhǎng)度與運(yùn)動(dòng)速度約束條件后，驗(yàn)證了4段式與6段式S型速度規(guī)劃曲線在約束條件內(nèi)不能達(dá)到最優(yōu)速度，5段式速度規(guī)劃較7段式能夠更快達(dá)到最優(yōu)速度，保持最佳進(jìn)給速度的時(shí)間更長(zhǎng)且總用時(shí)更短，為較理想的速度規(guī)劃曲線。5段式S型速度規(guī)劃曲線視覺(jué)引導(dǎo)實(shí)驗(yàn)效果圖如圖8所示。

(a) 任意軌跡 (b) 運(yùn)動(dòng)效果圖 圖8 任意軌跡運(yùn)動(dòng)效果圖

5 結(jié)論

為提高機(jī)器人在笛卡爾平面下的軌跡精度，本文提出了一種基于視覺(jué)引導(dǎo)的機(jī)器人軌跡規(guī)劃算法。仿真分析與實(shí)驗(yàn)結(jié)果表明，經(jīng)雙弦與弦切誤差約束條件篩選后的視覺(jué)軌跡輪廓離散點(diǎn)點(diǎn)數(shù)較少，獲得的弧長(zhǎng)參數(shù)化Akima曲線擬合精度高，有效減小了運(yùn)算量；在小型零部件涂膠密封軌跡長(zhǎng)度約束條件下，采用改進(jìn)的5段式S型速度規(guī)劃曲線能夠迅速達(dá)到最優(yōu)膠槍進(jìn)給速度，并能自動(dòng)調(diào)整加加速度，縮短運(yùn)動(dòng)時(shí)間，同時(shí)保證了加速度曲線的連續(xù)平穩(wěn)變化，有效減小了末端執(zhí)行機(jī)構(gòu)的振動(dòng)和機(jī)械沖擊，提高了軌跡精度。在下一步的研究中，將其運(yùn)用于小型零部件的涂膠密封工作中，進(jìn)一步優(yōu)化Akima軌跡擬合與5段式S型速度規(guī)劃算法以及硬件平臺(tái)，提高軌跡質(zhì)量和系統(tǒng)的可應(yīng)用性。