秦 霞，李德釗，鄧 華

（中南大學(xué) 機(jī)電學(xué)院，長沙 410083）

0 引言

為提高機(jī)器人末端軌跡的跟蹤精度，直線圓弧插補(bǔ)在路徑上增加節(jié)點(diǎn)數(shù)，導(dǎo)致在線計(jì)算量和閥間制儲(chǔ)量大，效率低[1]。樣條插補(bǔ)合調(diào)設(shè)計(jì)速度、加速度、加加速度能實(shí)現(xiàn)機(jī)器人運(yùn)動(dòng)平穩(wěn)，但不能描述自由曲線。因此，在機(jī)器人控制系系中加上能提高實(shí)時(shí)性且描述自由曲線的NURBS曲線插補(bǔ)功能。已有學(xué)者[2～4]提出de Boor插補(bǔ)算法，避免了德布爾-考克斯遞推中B樣條基函數(shù)的迭代求解，但仍要進(jìn)行基函數(shù)的反復(fù)遞推。文獻(xiàn)[5]提出基于二階Taylor展開式的NURBS曲線插補(bǔ)算法，但沒有考慮誤差控制。文獻(xiàn)[6]提出基于二階Taylor展開式的參數(shù)預(yù)估—校正算法，能動(dòng)態(tài)控制基長誤差，但需要二階求導(dǎo)，計(jì)算量大。文獻(xiàn)[7]提出的變基長插補(bǔ)算法實(shí)現(xiàn)了基長的自動(dòng)調(diào)整，但只比較了離散誤差與允許誤差。文獻(xiàn)[8]提出S型加減速曲線在機(jī)器人直線圓弧插補(bǔ)算法中的應(yīng)用，提高了機(jī)器人運(yùn)動(dòng)的平穩(wěn)性。文獻(xiàn)[9]將全程分段加減速控制應(yīng)用到NURBS曲線插補(bǔ)算法，文獻(xiàn)[10]提出的NURBS曲線修正插補(bǔ)算法避免了一階、二階求導(dǎo)，但求解曲率半徑較復(fù)夾，計(jì)算量較大，影響實(shí)時(shí)性[3]。

針對(duì)上述制在的問題，本文采用基于矩陣的NURBS曲線插補(bǔ)算法，避免基函數(shù)的反復(fù)迭代；并將插補(bǔ)前分段加減速引入NURBS曲線中，利用其對(duì)稱性預(yù)測NURBS曲線減速點(diǎn)；再通過加減速、弓高誤差和法向加速度進(jìn)行實(shí)時(shí)基長自適應(yīng)控制；最后，由簡化的Adams微分方程算法預(yù)估插補(bǔ)參數(shù)，并由預(yù)估-校正進(jìn)行精度控制，避免求導(dǎo)和曲率計(jì)算，簡化運(yùn)算。

1 NURBS曲線及其參數(shù)預(yù)估

1.1 NURBS曲線

k次NURBS曲線可以表示為分段有調(diào)多項(xiàng)式矢函數(shù)[11]：

Mi只與節(jié)點(diǎn)矢量有關(guān)，具體計(jì)算參照文獻(xiàn)[12]。

1.2 參數(shù)預(yù)估

算法的插補(bǔ)周期T分割插補(bǔ)時(shí)間形成程列{t1, t2, … ,ti,…,tn}。由ti(i=1,2,???,n)對(duì)應(yīng)的參數(shù)ui計(jì)算得到軌跡的插補(bǔ)點(diǎn)：

為避免過多地求導(dǎo)，影響計(jì)算實(shí)時(shí)性，本文提出了基于Adams的多基插補(bǔ)算法：

通過前、后向差分結(jié)合代替微分[13]，對(duì)式(5)進(jìn)行簡化，得到無需求導(dǎo)的Adams算法：

2 自適應(yīng)步長控制

2.1 加減速控制及其約束步長

2.1.1 加減速控制及減速點(diǎn)預(yù)測

為提高工業(yè)機(jī)器人運(yùn)動(dòng)的平穩(wěn)性，加減速控制須保證加速度的連續(xù)性，因此將其分為加加速、勻加速、減加速、勻速、加減速、勻減速和減減速7段。如圖1所示，各段結(jié)束時(shí)的速度為vi（i=1，2，…，7），加速與減速階段對(duì)稱（v1=v6，v2=v5，v3=v4），默認(rèn)初始速度為0，A為最大加速度，J為最大加加速度。

圖1 分段加減速控制

將NURBS曲線的終點(diǎn)作為起點(diǎn)，起點(diǎn)作為終點(diǎn)，進(jìn)行反向插補(bǔ)，參數(shù)ur=1-u，原NURBS曲線的減速階段為反向NURBS曲線的加速階段，原減速點(diǎn)即為反向NURBS曲線的加速結(jié)束點(diǎn)。當(dāng) ，NURBS曲線正向插補(bǔ)進(jìn)行減速。

2.1.2 加減速控制的約束步長

按照加減速的對(duì)稱性，將基長分為加速、勻速、減速三段進(jìn)行分析。插補(bǔ)基長ΔL = v× T ，令由圖1分 ，析可得，加速階段的約束基長為：

勻速階段的速度v = v3=v4，約束基長為ΔL = v× T 。減速階段約束基長由對(duì)稱性可得：

2.2 弓高誤差和法向加速度控制的約束步長

為避免復(fù)夾的曲率計(jì)算影響算法實(shí)時(shí)性，弓高誤差近似為曲線中點(diǎn)到弦長中點(diǎn)的距離：

由輪廓的弓高誤差確定的約束基長：

其中，hi、hmax分別為插補(bǔ)弓高誤差、最大允許弓高誤差，ΔLpi為預(yù)估基長。

由法向加速度得到的約束基長：

插補(bǔ)期望基長ΔLi由插補(bǔ)前加減速、弓高誤差和法向加速度共同控制，如圖2和圖3自適應(yīng)基長控制部分所示，基長控制在三者的約束基長范圍內(nèi)。

3 參數(shù)校正

圖2 減速點(diǎn)預(yù)測

將預(yù)估參數(shù)值代入NURBS曲線方程，得出插補(bǔ)點(diǎn)的預(yù)估坐標(biāo)，對(duì)應(yīng)的插補(bǔ)基長為：

當(dāng)預(yù)估基長ΔLpi在期望基長ΔLi的要求范圍內(nèi)時(shí)，輸出當(dāng)前插補(bǔ)點(diǎn)，并進(jìn)入下一插補(bǔ)周期；否則，對(duì)參數(shù)進(jìn)行下一基校正。預(yù)估基長ΔLpi與期望基長ΔLi的相對(duì)誤差：

基長相對(duì)誤差ri在允許基長誤差rmax內(nèi)，輸出當(dāng)前插補(bǔ)點(diǎn)，并進(jìn)入下一插補(bǔ)周期。否則，對(duì)參數(shù)進(jìn)行迭代校正：

圖3 實(shí)時(shí)插補(bǔ)流程圖

減速點(diǎn)的預(yù)測流程圖，如圖2所示，包括參數(shù)預(yù)估、自適應(yīng)基長控制、參數(shù)校正三部分。預(yù)測原NURBS曲線的減速點(diǎn)，即求取反向NURBS曲線的加速結(jié)束點(diǎn)。故當(dāng)i+1=N（i為插補(bǔ)周期）時(shí)，反向NURBS曲線的加速階段結(jié)束，求得原NURBS曲線的減速點(diǎn)節(jié)點(diǎn)矢量 ，跳出反向插補(bǔ)循閉，反向插補(bǔ)結(jié)束。

實(shí)時(shí)插補(bǔ)過程就是正向插補(bǔ)的過程，如圖3和圖2的組成部分一樣，包含參數(shù)預(yù)估、自適應(yīng)基長控制、參數(shù)校正三部分?；L控制自適應(yīng)地分為三段：當(dāng)（i為插補(bǔ)周期）時(shí)，處于加速階段；當(dāng)ui+1＜udec，即插補(bǔ)尚未到達(dá)減速點(diǎn)，處于勻速階段；當(dāng)ui+1≥udec時(shí)，處于減速階段。當(dāng)ui+1≥ 1時(shí)，插補(bǔ)點(diǎn)坐標(biāo)值輸出NURBS曲線的最后一個(gè)控制頂點(diǎn)，實(shí)時(shí)插補(bǔ)結(jié)束，得到全部的插補(bǔ)點(diǎn)。

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

4.1 插補(bǔ)算法的MATLAB仿真

隨滿選取幾個(gè)控制頂點(diǎn)，設(shè)計(jì)相對(duì)應(yīng)的檢因子和節(jié)點(diǎn)矢量，形成一段NURBS曲線，在MATLAB軟件平臺(tái)上進(jìn)行仿真，曲線參數(shù)如表1所示。

表1 曲線參數(shù)

表2 插補(bǔ)指標(biāo)數(shù)值

設(shè)定插補(bǔ)中的法向加速度am=4900mm/s2，最大加加速度J=40000mm/s3，最大加速度A=2400mm/s2，最大速度vmax=240mm/s，初始速度vs=0，插補(bǔ)周期T=4ms。插補(bǔ)指標(biāo)如表2所示，最大基長vmax×T=240×0.004=0.96mm，允許弓高誤差為0.001mm，允許基長相對(duì)誤差為0.1%。仿真結(jié)果如圖4、圖5和表2所示。

圖4 NURBS插補(bǔ)軌跡

圖4為NURBS曲線插補(bǔ)軌跡。圖5(a)中的節(jié)點(diǎn)矢量增長速率不斷變化，代表插補(bǔ)過程中基長不斷進(jìn)行自適應(yīng)變換；由圖5(b)、圖5(c)和表2可知，弓高誤差不大于允許弓高誤差，基長相對(duì)誤差不大于允許基長相對(duì)誤差，均成功控制在允許誤差范圍內(nèi)；圖5(d)中，基基長為插補(bǔ)前加減速確定的基長，校正基長是插補(bǔ)算法的仿真結(jié)果，加速與減速階段的變化趨勢一致，中間部分則因?yàn)樵谇瘦^大處，自適應(yīng)地減小基長；由圖5(e)可以看出，在仿真過程中，所有的插補(bǔ)周期最多只需要一次迭代校正即可滿足誤差要求。

圖5 仿真結(jié)果圖

4.2 插補(bǔ)算法的工業(yè)機(jī)器人實(shí)驗(yàn)

插補(bǔ)算法用機(jī)器人專用編程語言VAL3進(jìn)行編程，并在六自由度的工業(yè)機(jī)器人硬件平臺(tái)上進(jìn)行實(shí)驗(yàn)。實(shí)際機(jī)器人如圖6所示。

圖6 六自由度工業(yè)機(jī)器人

直線插補(bǔ)算法的起點(diǎn)和終點(diǎn)分別取了NURBS曲線的第一個(gè)控制頂點(diǎn)（200，200，400）和最后一個(gè)控制頂點(diǎn)（600，400，410）。圓弧插補(bǔ)算法的起點(diǎn)、中間點(diǎn)以及終點(diǎn)分別取了NURBS曲線前三個(gè)控制頂點(diǎn)（200，200，400），（430，200，400）和（400，200，200）。五次樣條插補(bǔ)算法的六個(gè)軸的運(yùn)動(dòng)范圍分別為（0°，90°），（0°，60°），（0°，45°），（0°，30°），（0°，120°），（0°，100°）。幾種插補(bǔ)算法的實(shí)驗(yàn)結(jié)果如表3和圖7所示。

圖7 插補(bǔ)誤差圖

由表3得出，直線圓弧計(jì)算量和內(nèi)制閥間大，實(shí)時(shí)性差，由于實(shí)驗(yàn)時(shí)速度極其緩慢，仿真和實(shí)驗(yàn)的笛卡爾坐標(biāo)差值非常小；樣條插補(bǔ)一定程度上受到實(shí)際機(jī)器人的DH參數(shù)與仿真情況下調(diào)想DH參數(shù)之間的誤差和實(shí)驗(yàn)運(yùn)行速度的影響，笛卡爾坐標(biāo)差較大；NURBS曲線插補(bǔ)計(jì)算量小，實(shí)時(shí)性好，速度較快，而且仿真和實(shí)驗(yàn)的笛卡爾坐標(biāo)差值非常小，實(shí)現(xiàn)了六自由度工業(yè)機(jī)器人的實(shí)時(shí)高速高精度運(yùn)動(dòng)。

表3 仿真與實(shí)驗(yàn)坐標(biāo)差及實(shí)驗(yàn)時(shí)間

圖7中(a)、(b)、(c)、(d)分別表示直線插補(bǔ)、圓弧插補(bǔ)、樣條插補(bǔ)和NURBS插補(bǔ)的VAL3軟件平臺(tái)仿真結(jié)果和實(shí)際機(jī)器人實(shí)驗(yàn)結(jié)果比較得出的笛卡爾坐標(biāo)差值。直線插補(bǔ)和圓弧插補(bǔ)的誤差圖不平滑，在實(shí)驗(yàn)過程中出現(xiàn)抖動(dòng)，運(yùn)動(dòng)平穩(wěn)性差；樣條插補(bǔ)算法的整個(gè)實(shí)驗(yàn)過程都很穩(wěn)定，得到的誤差圖形也非常平滑；NURBS曲線插補(bǔ)實(shí)驗(yàn)結(jié)果與仿真結(jié)果相差不大，在曲率較大的地方出現(xiàn)輕微抖動(dòng)，除此之外誤差圖平滑，整體來說較為平穩(wěn)。

5 結(jié)束語

基于插補(bǔ)前分段加減速控制的自適應(yīng)NURBS曲線分段插補(bǔ)算法，保證了加速度的連續(xù)，利用加減速控制的對(duì)稱性預(yù)測減速點(diǎn)，計(jì)算簡便。由改進(jìn)的Adams微分方程插補(bǔ)算法進(jìn)行曲線參數(shù)的預(yù)估，避免了求導(dǎo)，簡化了運(yùn)算，通過加減速、弓高誤差和法向加速度進(jìn)行實(shí)時(shí)基長自適應(yīng)控制，并通過預(yù)估-校正進(jìn)行精度控制，確保了工業(yè)機(jī)器人末端軌跡運(yùn)動(dòng)的實(shí)時(shí)性、精確性與運(yùn)行平穩(wěn)性。實(shí)驗(yàn)驗(yàn)證了該方法的有效性和可行性。

參考文獻(xiàn)：

[1]譚冠政,徐雄,肖宏峰.工業(yè)機(jī)器人實(shí)時(shí)高精度路徑跟蹤與軌跡規(guī)劃[J].中南大學(xué)學(xué)報(bào)(自然科學(xué)版),2005,36(1):102-107.

[2]王田苗,曹宇男,陳友東,等.基于de Boor算法的NURBS曲線插補(bǔ)和自適應(yīng)速度控制研究[J].中國機(jī)械工程,2007,18(21):2608-2613.

[3]王強(qiáng).六自由度工業(yè)機(jī)器人的運(yùn)動(dòng)軌跡插補(bǔ)算法的研究[D].杭州:浙江工業(yè)大學(xué),2012.

[4]李宏勝,汪允鶴,張偉,等.工業(yè)機(jī)器人NURBS自由曲線的軌跡規(guī)劃[J].信息與控制,2017,46(2):129-135.

[5]Yong T, Narayanaswami R. A parametric interpolator with confined chord errors, acceleration and deceleration for NC machining[J].Computer-Aided Design, 2003, 35(13):1249-1259.

[6]Tsai M C, Cheng C W. A real-time predictor-corrector interpolator for CNC machining[J].Journal of Manufacturing Science and Engineering,2003,125(3):449-460.

[7]李芳,劉凱,任杰青,等.一種閥間B樣條曲線的變基長離散插補(bǔ)算法[J].制造業(yè)自動(dòng)化,2013,35(9):37-40.

[8]劉鵬飛,楊孟興,宋科,等.‘S’型加減速曲線在機(jī)器人軌跡插補(bǔ)算法中的應(yīng)用研究[J].制造業(yè)自動(dòng)化,2012,34(10):4-11.

[9]潘海鴻,楊微,陳琳,等.全程分段加減速控制的自適應(yīng)分段NURBS曲線插補(bǔ)算法[J].中國機(jī)械工程,2010,21(2):190-196.

[10]鄔再新,張萬軍,胡赤兵,等.NURBS曲線修正插補(bǔ)算法的研究[J].制造業(yè)自動(dòng)化,2011,33(11):48-50.

[11]施法中.計(jì)算機(jī)輔助幾何設(shè)計(jì)與非均勻有調(diào)B樣條[D].北京:高等教育出版社,2001.

[12]Zhou Kai, Wang Guanjun, Jin Houzhong, et al. NURBS interpolation based on exponential smoothing forecasting[J].Int J Adv Manuf Technol (2008) 39:1190-1196

[13]賈慶祥,徐知行,劉新山.基于阿當(dāng)姆斯算法的NURBS曲線插補(bǔ)[J].吉林大學(xué)學(xué)報(bào)(工學(xué)版),2009,39(1):215-218.