陳宇鵬，高偉強(qiáng)，盧一光

（廣東工業(yè)大學(xué) 機(jī)電工程學(xué)院，廣東 廣州 510006）

示教是工業(yè)機(jī)器人在噴涂應(yīng)用中的一個(gè)重要環(huán)節(jié)。機(jī)器人的示教方法可以分為兩大類：一種是直接面向機(jī)器人本體進(jìn)行示教，稱為直接示教；另一種是遠(yuǎn)離機(jī)器人本體通過計(jì)算機(jī)技術(shù)對(duì)機(jī)器人進(jìn)行示教，稱為離線示教。直接示教方法主要有示教盒示教和手把手示教兩種。手把手示教是由操作人員直接拖拽機(jī)器人末端工具的方式實(shí)現(xiàn)復(fù)雜軌跡的示教。與示教盒示教和離線編程相比，手把手示教噴涂效果所見即所得，無需編程，簡(jiǎn)單易學(xué)，效率高，只需簡(jiǎn)短培訓(xùn)就可使原噴涂工人直接成為機(jī)器人的操作工人，解決了中小涂裝企業(yè)技術(shù)升級(jí)后機(jī)器人應(yīng)用人才短缺問題，并且避免了大量噴涂工下崗帶來的社會(huì)問題。

在手把手示教過程中，主要存在兩方面的問題。一方面，受到機(jī)器人拖拽靈活性和操作人員自身等因素的影響，示教路徑和速度難以光順，噴涂時(shí)速度和加速度會(huì)有突變，產(chǎn)生振動(dòng)、沖擊，影響噴涂質(zhì)量。另一方面，由于機(jī)器人拖拽靈活性的影響，示教時(shí)難以達(dá)到或者難以迅速達(dá)到機(jī)器人期望的工作速度，導(dǎo)致機(jī)器人再現(xiàn)噴涂時(shí)只能同時(shí)提高整體的速度。因?yàn)檫@兩方面問題的存在，使手把手示教機(jī)器人的現(xiàn)實(shí)應(yīng)用受到一定的詬病。為解決上述問題，本文提出了一種對(duì)手把手示教噴涂采樣數(shù)據(jù)進(jìn)行運(yùn)動(dòng)規(guī)劃的方法。

機(jī)器人關(guān)節(jié)角空間運(yùn)動(dòng)規(guī)劃主要是對(duì)關(guān)節(jié)速度、加速度進(jìn)行規(guī)劃，以時(shí)間最優(yōu)和平滑過渡為目標(biāo)進(jìn)行優(yōu)化[1-3]。由于從關(guān)節(jié)坐標(biāo)到機(jī)器人末端工具軸位姿映射過程存在大量的三角函數(shù)等非線性運(yùn)算，直接對(duì)關(guān)節(jié)坐標(biāo)優(yōu)化后會(huì)造成機(jī)器人末端機(jī)構(gòu)位姿誤差難以控制，使再現(xiàn)位姿與采樣位姿產(chǎn)生較大誤差。本文提出在笛卡爾空間中運(yùn)用樣條曲線對(duì)機(jī)器人手把手示教軌跡進(jìn)行光順處理，對(duì)再現(xiàn)運(yùn)動(dòng)速度進(jìn)行重新規(guī)劃，使得機(jī)器人再現(xiàn)運(yùn)動(dòng)時(shí)能夠以較高的速度平穩(wěn)運(yùn)行，解決手把手示教存在的上述問題。{[ A1,···,A6]i}構(gòu)成示教軌跡。對(duì)采樣得到的n個(gè)6關(guān)節(jié)坐標(biāo)數(shù)據(jù)進(jìn)行6關(guān)節(jié)機(jī)器人正運(yùn)算，得到示教軌跡的矩陣變換表示，記為{ Mi}，如式(1)所示。

式中， Ri∈R3×3表示第i個(gè)示教點(diǎn)對(duì)機(jī)器人基坐標(biāo)系的旋轉(zhuǎn)變換矩陣，包含噴槍的姿態(tài)信息； Pi∈R3×1表示第i個(gè)示教點(diǎn)對(duì)機(jī)器人基坐標(biāo)系的平移變換矩陣，包含噴槍的空間位置信息。噴槍空間位置集合記為{Pi}，旋轉(zhuǎn)變換集合記為{ Ri}。

對(duì)手把手示教軌跡進(jìn)行優(yōu)化，可以轉(zhuǎn)化為對(duì)給定軌跡 {Mi}的運(yùn)動(dòng)規(guī)劃問題。其整體的運(yùn)動(dòng)優(yōu)化流程如圖1所示。主要流程是對(duì)示教軌跡進(jìn)行位姿擬合，使軌跡更加光滑。擬合完成后，再對(duì)其重新離散化，同時(shí)對(duì)其速度進(jìn)行規(guī)劃，使速度曲線平滑、加速度曲線無突變，最后進(jìn)行噴槍姿態(tài)的插值，保證姿態(tài)變化平滑，機(jī)器人再現(xiàn)運(yùn)動(dòng)平穩(wěn)。

1 示教軌跡的位姿擬合

在手把手示教過程中，工業(yè)機(jī)器人控制器對(duì)6個(gè)關(guān)節(jié)伺服電機(jī)的絕對(duì)位置編碼器數(shù)值進(jìn)行時(shí)間分割采樣，得到6關(guān)節(jié)坐標(biāo)時(shí)間分割采樣數(shù)據(jù)

圖 1 整體優(yōu)化流程Fig.1 Optimization process

1.1 軌跡預(yù)處理

軌跡的預(yù)處理主要目的是減少后續(xù)位姿擬合的計(jì)算量，適當(dāng)去除一些示教點(diǎn)，用較少的示教點(diǎn)來表示軌跡而不丟失軌跡自身的特征。如圖2所示，計(jì)算空間矢量 Si，Si+1之間的夾角αi+1，并與給定的閾值α0作比較。假如αi+1小 于等于閾值α0，則認(rèn)為3點(diǎn)處于同一空間直線上，此時(shí)舍去中間點(diǎn) Pi+1。 假如αi+1大于閾值α0，則保留中間點(diǎn) Pi+1。

圖 2 特征點(diǎn)的判斷示意圖Fig.2 Judgment diagram of feature points

運(yùn)用以上方法，對(duì)空間位置集合 {Pi}進(jìn)行處理，可以得到空間位置特征點(diǎn)點(diǎn)集合 {Pi}，m

旋轉(zhuǎn)矩陣R描述軌跡點(diǎn)的姿態(tài)較為復(fù)雜而且難以量化計(jì)算，本文引入單位四元數(shù)的方法描述機(jī)器人末端執(zhí)行器的姿態(tài)軌跡曲線[4-5]?？臻g某點(diǎn)在繞軸n旋 轉(zhuǎn)θ 角 度時(shí)，其單位四元數(shù)可以表示為 q=(cos(θ/2)，sin(θ/2)n)， 其中 n為三維單位向量。根據(jù)旋轉(zhuǎn)矩陣和四元數(shù)轉(zhuǎn)換公式[6]，將{ Ri}轉(zhuǎn)換成單位四元數(shù)表示{qi}。因此，示教軌跡特征點(diǎn)序列{ Mi}可以表示為式(2)所示。

1.2 位姿軌跡擬合

為獲得平滑連續(xù)的空間位置曲線和姿態(tài)曲線，對(duì)示教空間位置序列 {Pi}和姿態(tài)序列{ qi}構(gòu)造2階連續(xù)的插值函數(shù)。

對(duì)于序列{ Pi}，包含的是機(jī)器人末端執(zhí)行機(jī)構(gòu)的空間位置軌跡信息，可以使用3次非均勻B樣條曲線去描述位置曲線C (u)，如式(3)所示。

式 中， u ∈[0,1] ， di表 示B 樣 條 曲 線 的 控 制 頂 點(diǎn)，Ni,3(u)是定義在節(jié)點(diǎn)矢量U上的B樣條曲線基函數(shù)。想要明確曲線 C(u)的表達(dá)式，需要求取計(jì)算節(jié)點(diǎn)矢量U[0,0,0,u3,···,um+3,1,1,1] 以及控制頂點(diǎn) di，相關(guān)的求取計(jì)算方法可參考文獻(xiàn)[7]。

由單位四元數(shù) q=(cos(θ/2),sin(θ/2)n)的自然對(duì)數(shù)運(yùn)算見式(4)[8]。

令函數(shù) p(t)=ln(q(t))，根據(jù)文獻(xiàn)[9]可知，構(gòu)造2階連續(xù)的單位四元數(shù)插值函數(shù) q(t)可以轉(zhuǎn)化為構(gòu)造2階連續(xù)的函數(shù) p(t)。

根據(jù)式(4)，可以將姿態(tài)序列{ qi}轉(zhuǎn)換成一個(gè)三維向量序列集合{ pi}。 同理，對(duì){ pi}采用3次B樣條曲線進(jìn)行描述，如式(5)所示。

根據(jù)同樣的方法可以計(jì)算得到曲線 p(t)的節(jié)點(diǎn)矢量 Γ[0,0,0,t3,···,tm+3,1,1,1]和 控制頂點(diǎn)φi。得到姿態(tài)曲線 q(t)，如式(6)所示。

2 機(jī)器人位置曲線的插補(bǔ)

為獲得新的離散示教路徑，需要分別對(duì)位置曲線C (u)和 姿態(tài)曲線 q(t)進(jìn)行插補(bǔ)離散。

2.1 B樣條曲線泰勒展開插補(bǔ)算法

曲線參數(shù)化插補(bǔ)，即由軌跡空間的進(jìn)給步長(zhǎng)?L映射到參數(shù)空間從而求取參數(shù)ui+1=ui+?ui。本文采用一階泰勒展開進(jìn)行空間位置曲線C (u)的插補(bǔ)計(jì)算，其計(jì)算公式如式(7)所示[10-11]。

式中，Tc為 機(jī)器人運(yùn)動(dòng)插補(bǔ)周期，o (Tc)為泰勒展開余項(xiàng)

對(duì)機(jī)器人末端位置擬合曲線 C(u)， 在t時(shí)刻末端的瞬時(shí)速度可以表示為式(8)所示。。

由式(8)，有式(9)

忽略泰勒余項(xiàng)，式(7)可以寫為式(10)所示。

式中，σi=//C′(u)//，v (ti)是 機(jī)器人末端在ti時(shí)刻的速度。

反向插補(bǔ)計(jì)算時(shí)，有式(11)。

2.2 位置曲線插補(bǔ)過程

工人示教速度不能滿足機(jī)器人作業(yè)速度，不能很好地適用于產(chǎn)品加工，機(jī)器人再現(xiàn)速度需要提升。為了使機(jī)器人作業(yè)時(shí)速度更加平穩(wěn)，插補(bǔ)時(shí)使用區(qū)域連續(xù)S型速度曲線對(duì)速度進(jìn)行規(guī)劃[12-13]。

位置曲線 C(u)往往是比較復(fù)雜的，機(jī)器人末端執(zhí)行機(jī)構(gòu)需要在適當(dāng)?shù)奈恢锰崆皽p速，以較低的安全速度通過一些高曲率的點(diǎn)，避免速度變化給機(jī)器人本體帶來沖擊。這就需要在插補(bǔ)前對(duì)曲線 C(u)進(jìn)行速度前瞻處理，提前找出速度突變點(diǎn)，并對(duì)突變點(diǎn)進(jìn)行速度限制。速度突變點(diǎn)可以根據(jù)系統(tǒng)最大加速度的約束來尋找，其約束條件見式(12)所示。

式(12)中， κ(ui) 是曲線C (u) 在參數(shù)ui處的曲率，Amax是 系統(tǒng)限定的最大加速度，vf是機(jī)器人作業(yè)速度。

突變點(diǎn)對(duì)應(yīng)的限制速度可以通過系統(tǒng)法向加速度限制和系統(tǒng)弓高誤差限制進(jìn)行確定[14-15]。在找到C(u)中所有的突變點(diǎn)后，相當(dāng)于將曲線分成了若干速度規(guī)劃段，插補(bǔ)時(shí)根據(jù)各個(gè)速度突變點(diǎn)對(duì)應(yīng)的限制速度對(duì)每段曲線進(jìn)行區(qū)域連續(xù)S型曲線速度規(guī)劃，這樣就能確保機(jī)器人末端執(zhí)行機(jī)構(gòu)可以按限制速度通過突變點(diǎn)，并保證速度的平滑。

對(duì)曲線 C(u)進(jìn)行區(qū)域連續(xù)S型曲線速度規(guī)劃需要提前找到速度突變點(diǎn)的減速點(diǎn)，這就要求精確知道規(guī)劃路徑的總長(zhǎng)度，但是B樣條曲線不能精確計(jì)算得到規(guī)劃路徑長(zhǎng)度。

針對(duì)這一問題，采用雙向插補(bǔ)離散的方式尋找減速點(diǎn)。如圖3所示，點(diǎn) C(un) 和C (um)是兩相鄰?fù)蛔凕c(diǎn)，對(duì)應(yīng)參數(shù)為un和um，對(duì)應(yīng)限制速度為vn和 vm。雙向插補(bǔ)是從點(diǎn)C (un) 正方向和點(diǎn)C (um)反方向同時(shí)進(jìn)行S型速度曲線加速段的插補(bǔ)，正方向進(jìn)行插補(bǔ)計(jì)算1次，反方向也跟著插補(bǔ)計(jì)算1次，并且反向插補(bǔ)計(jì)算完成后才能進(jìn)行下一次正向插補(bǔ)。根據(jù)文獻(xiàn)[11]中的速度規(guī)劃可知，對(duì)于給定的最大速度vmax、系統(tǒng)最大加速度amax、 起始速度vs，S型速度曲線加速段的表達(dá)式如式(13)所示。

圖 3 雙向插補(bǔ)示意圖Fig.3 Two-way interpolation diagram

其中， T1為從初始速度加速到最大速度所需要的時(shí)間，且T1=1.5(vmax?vs)/amax。

尋找減速點(diǎn)C (ud)的過程為

步驟1：假設(shè)曲線段| C(un)C(um)|所能達(dá)到的最大速度vmax=vf，給定最大加速為系統(tǒng)限定最大加速度Amax、 插補(bǔ)周期 Tc，由式(13)得到由速度 vn和 速度vm加速至作業(yè)速度vf的速度表達(dá)式van(t)和 vam(t)。

步驟8：令 vmax=αvmax，一般有0 .5 ?α<1，回步驟1更新，重新進(jìn)行查找計(jì)算。

步驟9：結(jié)束查找流程。

上述流程在查找減速點(diǎn)C (ud)的時(shí)候，已經(jīng)完成加速段和減速段的插補(bǔ)離散化，可以保留其插補(bǔ)結(jié)果，避免重復(fù)計(jì)算。

3 姿態(tài)與位置的同步

在對(duì)位置曲線C (u)進(jìn)行插補(bǔ)離散化后得到機(jī)器人末端執(zhí)行機(jī)構(gòu)的空間位置路徑離散序列{ Pi}，為了生成示教路徑，還需要對(duì)姿態(tài)曲線 q(t)進(jìn)行離散化得到姿態(tài)路徑序列{ qi}。

姿態(tài)曲線離散化時(shí)應(yīng)該盡可能地保持與原姿態(tài)同步，也就是機(jī)器人末端機(jī)構(gòu)運(yùn)行通過新的示教路徑上某一空間點(diǎn)時(shí)，姿態(tài)應(yīng)當(dāng)與原來的姿態(tài)盡可能保持一致。因此，需要建立位置曲線 C(u)參數(shù)u與姿態(tài)曲線 q(t)參數(shù)t直接的映射關(guān)系，如式(14)所示。

因?yàn)槲恢们€C (u)是根據(jù)示教軌跡特征點(diǎn)序列{Mi}中的空間位置信息進(jìn)行擬合計(jì)算得到的，姿態(tài)曲線 q(t) 則是根據(jù){ Mi}中的姿態(tài)信息進(jìn)行擬合計(jì)算得到的，并且C (u)和 q(t)的節(jié)點(diǎn)矢量都是通過規(guī)范積累弦長(zhǎng)化方法構(gòu)造的。所以曲線 C(u)的節(jié)點(diǎn)矢量U與曲線 q(t) 的節(jié)點(diǎn)矢量Γ 具有一一對(duì)應(yīng)的關(guān)系，利用這一關(guān)系可以給出參數(shù)u對(duì)和參數(shù)t簡(jiǎn)單的線性映射關(guān)系。

首先判斷參數(shù)u所在的節(jié)點(diǎn)矢量區(qū)間 [uj,uj+1]，通過節(jié)點(diǎn)矢量U和節(jié)點(diǎn)矢量 Γ的對(duì)應(yīng)關(guān)系，得到參數(shù)t所在區(qū)間 [tj,tj+1]。然后建立參數(shù)u、t簡(jiǎn)單的線性關(guān)系，如式(15)所示。

4 實(shí)驗(yàn)與仿真驗(yàn)證

本文以佛山某機(jī)器人公司自行研制的手把手示教噴涂機(jī)器人為實(shí)驗(yàn)平臺(tái)，對(duì)提出的運(yùn)動(dòng)軌跡優(yōu)化算法進(jìn)行驗(yàn)證。實(shí)驗(yàn)截取廣州某衛(wèi)浴公司實(shí)際浴缸生產(chǎn)中玻纖?樹脂復(fù)合材料噴涂的一段手把手示教路徑，進(jìn)行實(shí)驗(yàn)分析。該段示教軌跡由870個(gè)示教點(diǎn)組成，將這些示教點(diǎn)進(jìn)行轉(zhuǎn)化得到對(duì)應(yīng)的機(jī)器人6個(gè)關(guān)節(jié)角角度，再通過機(jī)器人正運(yùn)算，得到笛卡爾坐標(biāo)系下的噴槍位姿。經(jīng)過擬合優(yōu)化處理，可以得到相應(yīng)的位姿擬合曲線。給定噴涂速度 vf為500 mm/s，最大加速度Amax為 1 000 mm/s2，插補(bǔ)時(shí)間Tc為10 ms，對(duì)擬合曲線進(jìn)行離散化，可以得到優(yōu)化后的示教路徑。機(jī)器人噴槍優(yōu)化前、后運(yùn)動(dòng)軌跡如圖4(a)、(b)所示，空間軌跡上的短線條代表機(jī)器人噴槍的姿態(tài)向量。從圖4中可以看出，優(yōu)化后該段示教軌跡更加光滑，姿態(tài)變化連續(xù)、與原示教軌跡姿態(tài)同步。優(yōu)化后路徑點(diǎn)數(shù)目為350個(gè)，相比于原始數(shù)據(jù)減少了一半，再現(xiàn)噴涂整體速度得到提高。

圖 4 優(yōu)化前后噴槍位姿對(duì)比Fig.4 Comparison of poses before and after optimization

為了進(jìn)一步分析其擬合效果，進(jìn)行位置曲線擬合誤差計(jì)算，可以得到最大擬合誤差為1.095 mm，平均擬合誤差為0.073 mm，標(biāo)準(zhǔn)差為0.144 mm，擬合誤差能滿足機(jī)器人的噴涂精度。擬合誤差分布如圖5所示。

圖 5 軌跡擬合誤差分布Fig.5 Fitting error distribution of trajectory

圖 6 優(yōu)化前后噴槍速度、加速度對(duì)比圖Fig.6 Comparison of spray gun speed-acceleration before and after optimization

對(duì)優(yōu)化效果進(jìn)行驗(yàn)證，分別對(duì)原始軌跡和優(yōu)化軌跡的機(jī)器人噴槍速度、加速度進(jìn)行對(duì)比分析。圖6是優(yōu)化前、后噴槍速度、加速度對(duì)比圖。從圖6可以明顯觀察到原始軌跡速度不穩(wěn)定，波動(dòng)較大而且整體速度較低。原始軌跡加速度存在不規(guī)則波動(dòng)，而且在一些點(diǎn)處有很大的突變。經(jīng)過優(yōu)化處理后的機(jī)器人噴槍軌跡運(yùn)動(dòng)速度曲線呈現(xiàn)出所規(guī)劃的S型，整體較為平滑，整體運(yùn)動(dòng)速度得到提高，提高了機(jī)器人的工作效率。優(yōu)化軌跡的加速度雖然仍然存在突變，但是比較優(yōu)化前減小了一個(gè)數(shù)量級(jí)，加速度曲線得到改善。將優(yōu)化軌跡數(shù)據(jù)轉(zhuǎn)換成示教再現(xiàn)數(shù)據(jù)格式，在機(jī)器人上進(jìn)行再現(xiàn)實(shí)驗(yàn)，實(shí)驗(yàn)結(jié)果顯示，有效地減小了再現(xiàn)噴涂過程中的振動(dòng)，噴涂效果明顯改善，同時(shí)噴涂效率也得到了提高。

5 總結(jié)

本文提出的手把手示教機(jī)器人再現(xiàn)運(yùn)動(dòng)優(yōu)化方法得到實(shí)驗(yàn)驗(yàn)證，結(jié)果表明此優(yōu)化方法能夠在保持原示教噴涂軌跡位姿精度的前提下，提高了機(jī)器人再現(xiàn)噴涂的平穩(wěn)性，噴涂效果明顯改善，同時(shí)也改善了工人示教速度與預(yù)期噴涂速度不匹配的問題，對(duì)提高手把手示教機(jī)器人在涂裝行業(yè)上的使用具有一定的應(yīng)用價(jià)值。