楊軼焬，劉建群，高偉強(qiáng)

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

0 引言

工業(yè)機(jī)器人的笛卡爾空間運(yùn)動(dòng)一般是由多段直線或者圓弧軌跡構(gòu)成的運(yùn)動(dòng)[1]。軌跡銜接處理的優(yōu)劣影響加工質(zhì)量和工作效率。如果機(jī)器人在拐角處不做處理，直接減速停止，將會(huì)增加工作時(shí)間，導(dǎo)致大幅度的電機(jī)加減速；如果保持速度直接通過(guò)拐角，可能產(chǎn)生機(jī)器振動(dòng)，影響工件質(zhì)量，降低機(jī)器人壽命[2]。

軌跡段間的銜接處理主要體現(xiàn)在軌跡規(guī)劃和速度規(guī)劃。軌跡規(guī)劃，其規(guī)劃的優(yōu)劣可以通過(guò)曲線連續(xù)度判斷。一般而言，G3級(jí)的曲線連續(xù)度可以滿足機(jī)器加速度平穩(wěn)的要求。速度規(guī)劃，不僅要考慮銜接處的速度，還需要考慮軌跡長(zhǎng)度的情況。目前，針對(duì)軌跡銜接過(guò)渡的常規(guī)做法是曲線擬合，在銜接處插入一段微型曲線，曲線不經(jīng)過(guò)拐點(diǎn)，通過(guò)犧牲位置精度來(lái)?yè)Q取過(guò)渡平滑。王濤等[3]提出了通過(guò)圓弧轉(zhuǎn)接實(shí)現(xiàn)拐角過(guò)渡的方法，但是忽略了銜接點(diǎn)向心加速度突變；BI等[4]提出了3次雙Bezier曲線構(gòu)建實(shí)時(shí)擬合曲線的方法，但是軌跡只能達(dá)到G2連續(xù)；BEUDAERT等[5]提出了3次B樣條曲線進(jìn)行直線段間過(guò)渡的方法，但是沒(méi)有給出對(duì)應(yīng)的插補(bǔ)規(guī)劃方法。

針對(duì)機(jī)器人拐角過(guò)渡的問(wèn)題，對(duì)軌跡段的銜接曲線模型及該模型的軌跡規(guī)劃、速度規(guī)劃進(jìn)行研究，實(shí)現(xiàn)了整體軌跡的G3連續(xù)和加速度連續(xù)，提升了多段連續(xù)軌跡拐角的銜接速度，減小了速度波動(dòng)，避免加速度突變，實(shí)現(xiàn)機(jī)器人的平穩(wěn)運(yùn)動(dòng)。

1 軌跡平滑過(guò)渡模型

1.1 過(guò)渡模型建立

假設(shè)當(dāng)前的運(yùn)行軌跡是由兩段相鄰的直線段構(gòu)成，直線段間的軌跡過(guò)渡模型如圖1所示，B、A、C是兩段直線的端點(diǎn)，相交于A點(diǎn)，其中兩段直線之間的夾角為α，一段微型樣條曲線構(gòu)造于d0d6之間。

圖1 直線段間的軌跡過(guò)渡模型

為了實(shí)現(xiàn)銜接曲線具有G3連續(xù)性，樣條曲線的次數(shù)為5次，控制點(diǎn)di個(gè)數(shù)為7，分別對(duì)應(yīng)d0,d1,…,d6；為了保證曲線起始點(diǎn)和終點(diǎn)能與首尾控制點(diǎn)重合，首尾向量節(jié)點(diǎn)應(yīng)設(shè)置6次的重復(fù)度，即u0=…=u5=0,u7=…=u12=1；為了保證曲線的對(duì)稱性，u6=0.5，所以該模型的節(jié)點(diǎn)向量設(shè)置為U=[0 0 0 0 0 0 0.5 1 1 1 1 1 1]T。

該模型的非均勻B樣條曲線構(gòu)造公式[6]為：

(1)

式中，k為樣條曲線次數(shù)；u為樣條參數(shù)；di為樣條曲線控制點(diǎn)；Ni,k(u)為k次B樣條基函數(shù)。

為了實(shí)現(xiàn)樣條曲線在銜接點(diǎn)處與直線相切，控制點(diǎn)d0、d1、d2應(yīng)該位于直線AB上，控制點(diǎn)d3位于拐點(diǎn)A。同理，控制點(diǎn)d4、d5、d6應(yīng)該位于直線AC上。為了使曲線在控制點(diǎn)d0和d6處與直線相接具有G3的連續(xù)性，在u=0和u=1，曲線p(u)相對(duì)于弧長(zhǎng)s的二階和三階導(dǎo)數(shù)應(yīng)該為零，即：

(2)

由導(dǎo)數(shù)的鏈?zhǔn)椒▌t可以將上式展開(kāi)得：

(3)

由文獻(xiàn)[7]可知，上式成立的充分條件為：

(4)

5次B樣條曲線在u=0的二次三次導(dǎo)數(shù)為：

(5)

化簡(jiǎn)可得：

(6)

同理u=1時(shí)可得：

(7)

由于樣條曲線的節(jié)點(diǎn)向量中心對(duì)稱，所以得到的曲線具有中心對(duì)稱的性質(zhì)。當(dāng)曲線參數(shù)u=0.5時(shí)，此時(shí)樣條曲線的最高點(diǎn)F與拐點(diǎn)A距離最遠(yuǎn)，曲線與原軌跡的偏移距離最大，所以可以通過(guò)控制拐點(diǎn)處F的偏移距離來(lái)控制過(guò)渡曲線的軌跡精度。該模型軌跡最大偏移距離δ推導(dǎo)如下：

(8)

(9)

將式(9)代入式(8)進(jìn)行化簡(jiǎn)，最大偏移距離為：

(10)

(11)

確定過(guò)渡模型的單位過(guò)渡長(zhǎng)度后，將該值代入式(9)，可以獲得控制點(diǎn)間的相對(duì)位移關(guān)系。最后，利用直線方程可以求得曲線控制點(diǎn)的空間坐標(biāo)，將曲線控制點(diǎn)代入式(1)，即可建立過(guò)渡曲線的方程。

1.2 過(guò)渡曲線長(zhǎng)度估算

使用樣條曲線過(guò)渡后，優(yōu)化后的軌跡與原直線軌跡相比，路徑長(zhǎng)度縮短，需要重新計(jì)算樣條曲線的實(shí)際長(zhǎng)度。由于樣條曲線弧長(zhǎng)公式難以直接求得解析解，所以使用自適應(yīng)辛普森積分方法，通過(guò)拋物線近似曲線的方式求取弧長(zhǎng)近似解。先求取樣條曲線公式的一階導(dǎo)數(shù)。該模型的銜接曲線一階導(dǎo)數(shù)公式為：

(12)

由于曲線光滑，并且長(zhǎng)度較短，適合使用數(shù)值積分法估算曲線長(zhǎng)度。使用自適應(yīng)辛普森法[8]進(jìn)行數(shù)值積分，計(jì)算u=[0,1]區(qū)間的弧長(zhǎng)，有：

(13)

2 多軌跡段銜接運(yùn)動(dòng)規(guī)劃算法

常見(jiàn)的過(guò)渡軌跡規(guī)劃是對(duì)整個(gè)過(guò)渡軌跡進(jìn)行速度和位置規(guī)劃，但是由于本模型過(guò)渡曲線的速度約束主要源于高曲率點(diǎn)F，若直接限制銜接曲線的最大速度將嚴(yán)重削減過(guò)渡效率。利用該模型的對(duì)稱性，將樣條曲線對(duì)半拆分，把拆分曲線與原軌跡段拼接成新軌跡段。假設(shè)當(dāng)前運(yùn)行至第i段直線軌跡A→B，使用曲線過(guò)渡模型進(jìn)行優(yōu)化后，當(dāng)前段的運(yùn)行軌跡變?yōu)镕1→F2，由兩段拆分樣條曲線和直線拼接(左樣條F1P1-直線P1P2-右樣條P2F2)，拼接軌跡形狀如圖2所示,實(shí)線為當(dāng)前段軌跡，虛線為相鄰段軌跡。下文將從速度規(guī)劃和位置規(guī)劃兩個(gè)層面，闡述該模型的運(yùn)動(dòng)規(guī)劃算法。

圖2 拼接軌跡形狀示意

2.1 軌跡段間的自適應(yīng)速度前瞻

為了讓機(jī)器人在軌跡段間過(guò)渡時(shí)，末端TCP能夠平穩(wěn)運(yùn)行，需要在F1和F2處設(shè)置一個(gè)合理的軌跡銜接速度。針對(duì)曲線過(guò)渡模型給出了自適應(yīng)速度規(guī)劃方法。步驟如下：

步驟1：計(jì)算所有軌跡與下一段軌跡銜接的最大允許拐角速度Vcor。拐角速度是由三個(gè)約束速度的最小值決定的，分別有用戶設(shè)定速度Vuser、弦高誤差限制允許速度Verr、向心加速度限制允許速度Vcir。

(14)

由于樣條曲線的曲率越高，曲率半徑越小，電機(jī)需要越大的加速度改變運(yùn)動(dòng)方向，所以需保證銜接點(diǎn)處的加速度不超過(guò)最大加速度設(shè)定值A(chǔ)max。最大加速度允許的速度Vcir為：

(15)

綜上所述，每一軌跡段的最大允許拐角速度Vcor：

Vcor=min{Vuser,Verr,Vcir}

(16)

步驟2：計(jì)算第i段的理論終點(diǎn)速度Ve(i)。若當(dāng)前段為首段(i=1)，則該段的起始速度設(shè)置為系統(tǒng)最小速度Vmin；若當(dāng)前段為尾段，則該段的終點(diǎn)速度設(shè)置為系統(tǒng)最小速度Vmin，同時(shí)結(jié)束速度規(guī)劃；若當(dāng)前段為中間段，以上一段Ve(i-1)的終點(diǎn)速度作為當(dāng)前段的起始速度Vs(i)。利用起始速度Vs(i)和軌跡長(zhǎng)度Si計(jì)算當(dāng)前段能實(shí)現(xiàn)的最大速度Vm(i)。第i段的理論終點(diǎn)速度為Ve(i)=min{Vcor(i),Vm(i)}。由于前一段的終點(diǎn)速度就是后一段的起始速度，所以第i+1段的起點(diǎn)速度Vs(i+1)=Ve(i)。

步驟3：計(jì)算以第i+1段的起始速度Vs(i+1)減速到系統(tǒng)最小速度Vmin的極限減速距離Sde。

步驟4：計(jì)算前瞻段數(shù)k。累加后續(xù)段的長(zhǎng)度L=L+S(i+k)，累加1次k遞增1，直到累加長(zhǎng)度L大于極限減速距離Sde或者所有軌跡段遍歷完成，記錄當(dāng)前k值。

步驟5：重算第i段的理論終點(diǎn)速度Ve(i)。將第i+k段的終點(diǎn)速度Ve(i+k)設(shè)置為系統(tǒng)最小速度Vmin。從第i+k段終點(diǎn)速度Ve(i+k)反向前推得到該段能實(shí)現(xiàn)的起始速度Vs(i+k)，那么第i+k-1段的終點(diǎn)速度為Ve(i+k-1)=min{Vs(i+k),Vcor(i+k-1)}，依次遞推計(jì)算前一段的終點(diǎn)速度，直到計(jì)算到第i段，可得其實(shí)際終點(diǎn)速度Ve(i)=min{Vs(i+2),Ve(i)}，退出前推循環(huán)。

步驟6：進(jìn)行樣條曲線、圓弧軌跡的切向加速度調(diào)整。由于曲線運(yùn)動(dòng)的加速度是切線加速度和向心加速度的疊加，所以有必要對(duì)切向加速度進(jìn)行限制，使合成加速度滿足設(shè)定的要求。由于S型速度曲線(如圖4所示)當(dāng)插補(bǔ)時(shí)間處于t=T1/2時(shí)，此時(shí)切向加速度達(dá)到最大值，利用之前速度前瞻得到的當(dāng)前段起始速度、終點(diǎn)速度、最大速度，計(jì)算當(dāng)前段所需運(yùn)動(dòng)時(shí)間，可以得到當(dāng)切向加速度到達(dá)最大時(shí)對(duì)應(yīng)的速度V和曲率半徑R，則第i段的切向加速度調(diào)整為：

(17)

若切向加速度需要修改，則再次執(zhí)行步驟(2)～步驟(5)。最終，第i段的實(shí)際終點(diǎn)速度計(jì)算完成。

步驟7：計(jì)算第i+1段的終點(diǎn)速度Ve(i+1)。將第i段的實(shí)際終點(diǎn)速度Ve(i)作為第i+1段的實(shí)際起始速度Vs(i+1)，再次執(zhí)行步驟(2)～步驟(5)，直到循環(huán)退出。

2.2 基于龍格庫(kù)塔的軌跡位置規(guī)劃

樣條曲線的位置規(guī)劃是本模型規(guī)劃算法的核心。下文先進(jìn)行樣條曲線插補(bǔ)方法的研究。

由于樣條曲線上的插補(bǔ)點(diǎn)是通過(guò)參數(shù)u求取的，而插補(bǔ)點(diǎn)的分布與速度規(guī)劃相關(guān)，所以需要尋找參數(shù)u與速度v之間存在的關(guān)系。根據(jù)速度的定義，將樣條曲線p(u)對(duì)時(shí)間t作一階導(dǎo)數(shù)，展開(kāi)可得關(guān)系式：

(18)

上式將參數(shù)u與時(shí)間t的映射問(wèn)題轉(zhuǎn)變?yōu)槲⒎址匠痰那蠼鈫?wèn)題。由于較難直接求得解析解，方程采用數(shù)值方法求解；為了提高計(jì)算的精度階數(shù)，避免求取樣條曲線的高階導(dǎo)矢，采用四階龍格庫(kù)塔方法對(duì)參數(shù)u進(jìn)行估算，計(jì)算公式為：

(19)

上述中利用插補(bǔ)速度可以求出曲線參數(shù)，但是由于實(shí)際曲線運(yùn)動(dòng)軌跡是由多段細(xì)微直線逼近的，在曲率變化頻繁的區(qū)域，實(shí)際直線插補(bǔ)位移比理論的曲線弧長(zhǎng)短，導(dǎo)致實(shí)際運(yùn)動(dòng)速度與預(yù)期規(guī)劃速度不同，容易在銜接區(qū)域發(fā)生速度波動(dòng)的情況，所以有必要對(duì)曲線參數(shù)u進(jìn)行矯正。這里采用割線法，將上步使用龍格庫(kù)塔計(jì)算的參數(shù)當(dāng)作初值代入割線迭代公式，迭代計(jì)算u，直到當(dāng)前插補(bǔ)周期的實(shí)際位移近似等于期望規(guī)劃位移，結(jié)束迭代。該模型的n次迭代割線公式為：

(20)

為了實(shí)現(xiàn)機(jī)器人末端工具TCP運(yùn)動(dòng)具有高階速度連續(xù)性，同時(shí)避免速度波動(dòng)。針對(duì)曲線過(guò)渡模型給出了一個(gè)銜接軌跡的位置規(guī)劃方法。假設(shè)當(dāng)前第拼接軌跡如圖3所示。

圖3 拼接軌跡和插補(bǔ)線段圖

軌跡位置規(guī)劃流程具體步驟如下：

步驟1：進(jìn)行S型速度曲線插補(bǔ)規(guī)劃。將軌跡段的速度信息V(起始速度Vs，終止速度Ve，最大速度Vm)和軌跡長(zhǎng)度S(左右曲線各自長(zhǎng)度的一半和中間直線段長(zhǎng)度的和)，代入三次S型曲線的加減速控制公式[9]，就可以進(jìn)行軌跡段插補(bǔ)規(guī)劃。該算法可以實(shí)現(xiàn)機(jī)器人運(yùn)動(dòng)時(shí)加速度平滑和加加速度柔性，三次S型加減速模型曲線如圖4所示。

圖4 三次S型加減速模型

使用三次S型速度規(guī)劃后，可以得到對(duì)應(yīng)的絕對(duì)位移(速度)-時(shí)間序列。然后將絕對(duì)位移(速度)序列上的元素通過(guò)直線方程或者樣條曲線方程，進(jìn)行插補(bǔ)規(guī)劃，映射為機(jī)器人工具末端TCP坐標(biāo)點(diǎn)。

步驟2：進(jìn)行樣條部分的插補(bǔ)規(guī)劃。進(jìn)行“左樣條”的插補(bǔ)，規(guī)劃方向從左到右，曲線起始參數(shù)u=0.5，使用龍格庫(kù)塔-割線公式依次迭代遞增直到當(dāng)前u>1結(jié)束，將最后一個(gè)插補(bǔ)點(diǎn)作為uL，理論插補(bǔ)位置對(duì)應(yīng)圖3插補(bǔ)線段中的SL。再進(jìn)行“右樣條”的插補(bǔ)，規(guī)劃方向從右往左，曲線起始參數(shù)u=0.5，使用龍格庫(kù)塔-割線公式依次迭代遞減直到當(dāng)前u<0，將最后一個(gè)插補(bǔ)點(diǎn)作為uR,理論插補(bǔ)位置對(duì)應(yīng)插補(bǔ)線段中的SR，完成左右樣條部分的插補(bǔ)。

3 實(shí)驗(yàn)過(guò)程及分析

為了驗(yàn)證本文算法在機(jī)器人上的實(shí)際運(yùn)行效果，搭建實(shí)驗(yàn)平臺(tái)如圖5所示。使用工業(yè)機(jī)器人Yaskawa-HP20D，平均位置精度為0.17 mm；使用激光跟蹤儀Leica-AT960，配合安裝于機(jī)器人末端的跟蹤探測(cè)器T-Mac，對(duì)機(jī)器人末端的運(yùn)動(dòng)軌跡進(jìn)行數(shù)據(jù)采集。

圖5 實(shí)驗(yàn)平臺(tái)

設(shè)定一個(gè)貨物搬運(yùn)的場(chǎng)景如圖6所示，圖中線條為機(jī)器人末端TCP的運(yùn)動(dòng)軌跡，要求機(jī)器人按照設(shè)定的工作軌跡進(jìn)行運(yùn)動(dòng)。

圖6 貨物搬運(yùn)場(chǎng)景

以搬運(yùn)點(diǎn)P10的紙箱為例，首先機(jī)器人末端TCP位于起始點(diǎn)P1，末端夾具運(yùn)動(dòng)到點(diǎn)P2夾取流水線上的紙箱，回到起始點(diǎn)P1后運(yùn)動(dòng)到點(diǎn)位P9托板正上方，然后運(yùn)動(dòng)到點(diǎn)位P10將紙箱放置于托板上，最后原路返回到起始點(diǎn)P1。整個(gè)軌跡由10個(gè)空間點(diǎn)構(gòu)成，原始軌跡長(zhǎng)度為11 519.76 mm。表1為機(jī)器人笛卡爾空間運(yùn)動(dòng)規(guī)劃參數(shù)，其中過(guò)渡偏移距離是指被過(guò)渡曲線最高點(diǎn)到原軌跡銜接點(diǎn)的距離，即圖2中的F2B段。

表1 貨物搬運(yùn)實(shí)驗(yàn)運(yùn)動(dòng)規(guī)劃參數(shù)

使用SpatialAnalyzer軟件進(jìn)行空間軌跡的測(cè)量如圖7所示。將標(biāo)定實(shí)驗(yàn)得到的機(jī)器人基座坐標(biāo)系設(shè)置為測(cè)量軟件的工作坐標(biāo)系，然后設(shè)置激光跟蹤儀的測(cè)量模式為空間掃描，測(cè)量頻率為1000 Hz。使用本文算法與常用的圓弧過(guò)渡算法進(jìn)行搬運(yùn)軌跡運(yùn)動(dòng)，對(duì)比兩種算法的機(jī)器人末端速度和加速度的測(cè)量曲線和運(yùn)動(dòng)所需時(shí)間。

圖7 SpatialAnalyzer空間軌跡測(cè)量

圖8為使用圓弧過(guò)渡算法對(duì)應(yīng)的末端速度、加速度曲線，圖9為使用本文算法的末端速度、加速度曲線。其中實(shí)線為使用激光跟蹤儀的測(cè)量數(shù)據(jù)，虛線為理論規(guī)劃數(shù)據(jù)。

圖8 圓弧過(guò)渡法末端速度、加速度曲線

圖9 樣條過(guò)渡法末端速度、加速度曲線

由兩種算法的速度曲線對(duì)比可知，本文算法和圓弧過(guò)渡算法在拐角處有明顯的速度提升。本文算法在拐角區(qū)域過(guò)渡速度較為平穩(wěn)，而圓弧過(guò)渡存在速度波動(dòng)。從加速度曲線對(duì)比可知，本文算法在圖9中的加速度曲線變化相對(duì)平穩(wěn)、接近規(guī)劃曲線；圓弧過(guò)渡在圖8中加速度曲線出現(xiàn)明顯的震蕩，雖然該算法保證了G1的軌跡相切連續(xù)，但是瞬間產(chǎn)生的向心加速度導(dǎo)致在過(guò)渡區(qū)域有較大幅度的加速度跳變，導(dǎo)致機(jī)器人末端運(yùn)動(dòng)過(guò)程伴隨顫振。不同算法的規(guī)劃時(shí)間如表2所示，本文算法上相對(duì)于不使用過(guò)渡方法在運(yùn)動(dòng)耗時(shí)上減少約18.8%，相較于圓弧過(guò)渡法減少約4.5%。

表2 三種算法運(yùn)動(dòng)所需時(shí)間對(duì)比

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

本文提出了一種基于非均勻B樣條曲線的過(guò)渡模型和針對(duì)該模型的運(yùn)動(dòng)規(guī)劃算法。該算法提升了多段連續(xù)軌跡的銜接速度，保證了示教編程機(jī)器人末端軌跡G3的連續(xù)性，實(shí)現(xiàn)機(jī)器人運(yùn)動(dòng)加速度變化連續(xù)，避免了加速度突變。利用安川機(jī)器人和激光跟蹤儀進(jìn)行實(shí)驗(yàn)，結(jié)果表明，該模型對(duì)比傳統(tǒng)圓弧過(guò)渡銜接，運(yùn)動(dòng)時(shí)間縮短4.5%，機(jī)器人運(yùn)動(dòng)更加平穩(wěn)，適用于搬運(yùn)機(jī)器人等對(duì)速度要求較高、同時(shí)要求運(yùn)動(dòng)平穩(wěn)的場(chǎng)景，具有一定的應(yīng)用價(jià)值。