基于高階非均勻有理B樣條插補的多軸運動控制方法研究

張旭陽,魯文其,鮑 敏，王浩亮，潘國忠

(1.浙江理工大學機械與自動控制學院，浙江 杭州 310018；2.江蘇遠東電機制造有限公司，江蘇 泰州 225300；3.浙江禾川科技有限公司，浙江 杭州311100)

0 引言

在工業(yè)生產(chǎn)中很多復雜的運動都需要依靠多軸聯(lián)動來完成[1-2]。如何讓多軸運動控制系統(tǒng)更好地完成作業(yè)一直是運動控制領域中的研究熱點和難點[3]。為了在現(xiàn)代工業(yè)中提高生產(chǎn)制造過程的作業(yè)質(zhì)量，從算法層面提高多軸控制系統(tǒng)的性能顯得尤為重要[4-7]。軌跡規(guī)劃是多軸運動控制系統(tǒng)的重要組成部分，其擬合精度和平順性關系到末端位姿的精確度。傳統(tǒng)的三次多項式插補算法在作業(yè)中會產(chǎn)生加速度不連續(xù)，導致電機抖動和沖擊，五次多項式插補在多軸運動過程中對加加速度無限制導致設備產(chǎn)生較大沖擊[8-10]，而且三次和五次多項式插補在插補過程中無勻速運行段，因此在很多工作場景中不適合。在梯形插補過程中因為其加速度不連續(xù)從而切換點產(chǎn)生的加加速度值為無窮大，很可能造成設備的損傷[11]。S曲線插補算法雖然可以解決梯形插補算法中的加速度不連續(xù)性，能夠保持最大速度運行，但是插補軌跡不能進行局部修改[12]。馬睿等[13]通過三次多項式插值進行軌跡插補，為保證運動中速度、加速度的有效性，引入了2個虛擬插值點。但是由于三次多項式根本不能保證加速度平滑性和加加速度連續(xù)性，在實際運行過程中會造成比較大的沖擊和震蕩。游文輝等[14]采用五次多項式進行軌跡規(guī)劃，進行四軸機械臂軌跡規(guī)劃插補。但是五次多項式插補算法中加加速度不能進行限制，對沖擊不能進行有效限制，軌跡誤差較大。王鵬[15]通過非均勻B樣條曲線插補以及速度規(guī)劃的原理，面向復雜工件的高速、高精度制造問題,提出了一種新的前瞻控制算法。首先根據(jù)加工精度要求得到自適應進給速度曲線，然后識別速度曲線中的速度極值點，分析速度極值點之間的速度干涉情況，對速度極值點進行優(yōu)化，最后對每個插補區(qū)間分別進行速度規(guī)劃。王光澤田[16]通過加減速規(guī)劃，系統(tǒng)分析了傳統(tǒng)的五次多項式在關節(jié)空間下進行規(guī)劃時存在的難題，通過研究對此方法進行改進的方法，給出了優(yōu)化后五次多項式規(guī)劃的一些重要參數(shù)的解決方法以及如何提出約束條件的方法。實現(xiàn)了在保持五次多項式優(yōu)點的前提條件下提高速度的結果。因為非均勻有理B樣條能夠通過統(tǒng)一的數(shù)學表達式精確表達任意自由曲線[17-18]，相應地其導矢也是具有同樣的B樣條性質(zhì)。為了保證運動控制過程中的速度、加速度、加加速度的連續(xù)以及軌跡過渡的穩(wěn)定性和平滑性[19-23]，對設計的運動控制器進行高階非均勻有理B樣條插補，達到速度、加速度和加加速度的連續(xù)性，避免產(chǎn)生突變，以及具有好的軌跡平滑性[24-26]。

為了解決傳統(tǒng)軌跡插補算法軌跡不平順和容易引起變速沖擊問題，本文采用高階非均勻有理B樣條對多軸機械臂進行軌跡規(guī)劃，對其速度、加速度和加加速度進行約束。

1 基于非均勻有理B樣條的軌跡插補算法

本文提出基于非均勻B樣條的多軸控制插補算法，其中包含B樣條插補和非均勻B樣條插補。

1.1 B樣條插補原理

B樣條函數(shù)是一種進行數(shù)值分析的特殊函數(shù)，由分段多項式進行構造。在插補過程中樣條函數(shù)擁有比多項式函數(shù)更好的插值性質(zhì)。

B樣條函數(shù)的基本表達式為

(1)

pi(i=0,1,…,n)為控制曲線形狀的頂點，又稱為德布爾點；Ni,k(u)為k次規(guī)范B樣條曲線基函數(shù)，也叫調(diào)和函數(shù)。且有

(2)

i為序號；k為B樣條的次數(shù)；ui為節(jié)點；由節(jié)點ui組成的集合U即為節(jié)點向量；由控制點pi逐次連接而成的折線即是控制整個B樣條曲線的多邊形；[ui,ui+1]為第i個節(jié)點空間。

在集合U中，某些節(jié)點ui(ui=ui+1=…=ui+k-1)的值可能相等，也存在節(jié)點區(qū)間不存在的情況。若節(jié)點ui的值出現(xiàn)s次，則記重復次數(shù)為重復度s。若節(jié)點間距長度相同，則為均勻節(jié)點向量，反之為非均勻節(jié)點向量。

由該函數(shù)的基本公式(2)可知，k=0基函數(shù)為階梯函數(shù)，當ui≤u≤ui+1時Ni,0(u)=1，u不在此區(qū)間時則Ni,0(u)=0。計算第i個函數(shù)Ni,k(u)時，必須要用到k+2個節(jié)點ui，ui+1，…，ui+k+1，k+2個節(jié)點構成k+1個節(jié)點區(qū)間[ui，ui+k+1]，而且區(qū)間[ui，ui+k+1]為基函數(shù)Ni,k(u)的支撐區(qū)間。因此，在參數(shù)軸上任意點u∈[ui,ui+k+1]處時，最多有k+1個非零的k次B樣條Ni,k(u)，而其余k次B樣條在此均為0。所以B樣條曲線方程為

(3)

在運動插補過程中，需要B樣條曲線首末控制點與運動插補過程中的首末插值點彼此重疊，以便于對曲線端點產(chǎn)生更好的控制作用?；瘮?shù)與各個節(jié)點之間的相對間距長度有關，與u的絕對值的大小無關，為使B樣條曲線具備相同階次貝齊爾曲線的端點幾何特性，針對開曲線與非周期性閉曲線，對首末節(jié)點取k+1的重復度并對曲線的定義域進行歸一化的過程處理，相對于均勻參數(shù)化，弦長參數(shù)化可以將2個相鄰數(shù)據(jù)點的參數(shù)值之差與其距離成一定的比例，更加合理。將曲線的定義域與規(guī)范參數(shù)域進行匹配，即取u∈[uk,un+1]=[0,1]。因此，其余節(jié)點u0=u1=…=uk=0，un+1=un+2=…=un+k+1=1。并通過哈德利-賈德法對剩余n-k個內(nèi)節(jié)點uk+1,uk+2,…，un進行計算。記控制B樣條曲線多邊形的邊長為li=|di-di-1|，節(jié)點為k重頂點時將控制多邊形k條邊的和進行替代，相鄰分段連接點參數(shù)值之差與相鄰頂點間距離成正比于實際存在差異，使

(4)

故內(nèi)節(jié)點ui為

(5)

得到所有節(jié)點的值后，將節(jié)點值ui順序代入式(1)，可得到B樣條曲線上的點p(u)。但為了簡便計算，本文采用德布爾算法進行p(u)的計算，即

(6)

1.2 非均勻B樣條函數(shù)的構造

非均勻B樣條曲線方程有理分式為

(7)

ωi(i=0,1,…,n)為權因子組成的集合，與控制頂點pi(i=0,1,…,n)逐次對應。首末權重ω0>0,ωn>0，其他權因子ωi≥0。A(u)為矢量函數(shù)，將其當做與原非均勻B樣條曲線節(jié)點矢量值相同，而控制頂點則為ωipi(i=0,1,…,n)的k次B樣條曲線，B(u)將其看做與原來的非均勻B樣條曲線節(jié)點矢量相同，而控制頂點為pi(i=0,1,…,n)的k次B樣條曲線。Ni,k(u) (i=0,1,…,n)為定義在非均勻、非周期節(jié)點向量U上第i段為k次B樣條基函數(shù)，由式(2)可得。

假定所給的時間-位置點對應序列為(ti,pi)，則從起點到終點的總運動時間ttotal=tend-t0。為使2個數(shù)據(jù)點的參數(shù)值之差與其距離成比例，通過累計弦長參數(shù)法對時間節(jié)點ti進行歸一化處理，得出k階非均勻B樣條的節(jié)點向量為u=[u0,u1,…,un+2k]，且

(8)

將k階非均勻B樣條曲線定義域[uk，uk+1]范圍內(nèi)的節(jié)點值逐次代入k階非均勻B樣條曲線方程中，讓其分別等于n+1個插值數(shù)據(jù)點pi，得到符合插值要求的n+1個方程為

(9)

由式(9)可知對于k階非均勻B樣條曲線的構造，必須先對節(jié)點向量U和控制頂點pi加以確定。

k階非均勻B樣條曲線上節(jié)點處的r階導矢pr(u),u∈[ui,ui+1]?[uk,uk+1]，根據(jù)下列遞推公式可求得：

(10)

(11)

由式(11)可知，k階B樣條曲線的r階導矢，通過k-r次的B樣條曲線構建，可由式(11)遞推求得它的控制頂點。

對于k階非均勻B樣條曲線，需要n+k個控制頂點di(i=0,1,…,n+k-1)。因此，就需要用k-1個由切矢條件來構建的附加方程。假設初始點速度、加速度分別為v0、a0，終止點速度和加速度分別為vf、af，再根據(jù)B樣條曲線公式得r階導矢公式，即：

(12)

(13)

(14)

(15)

聯(lián)系式(9)～式(13)便可以得出n+k個關于樣條曲線的控制頂點。

1.3 高階非均勻B樣條曲線數(shù)據(jù)預處理

運動插補的關鍵前提是對曲線相應的幾何數(shù)據(jù)進行預處理，主要目標是得出對應的非均勻有理B樣條曲線方程，使運動過程更加平穩(wěn)、光滑以及可靠。選取五次非均勻B樣條曲線作為研究對象，因此k取值為5，則節(jié)點向量中的首末2個節(jié)點的重復度均取為k+1次(6次)，則節(jié)點向量U=[0,0,0,0,0,0,u6,…,un,1,1,1,1,1,1]。1條非均勻B樣條是由多段多項式曲線依次連接而成。對曲線中的單段多項式曲線進行研究是研究整條非均勻B樣條曲線的關鍵。記pi(u)為第i段曲線，控制這段曲線的6個控制點pi與權因子wj相關，基函數(shù)Nj,5(u)同理。因此可知第i段曲線pi(u)可表示為

(16)

u∈[ui,ui+1]

在獲得樣條曲線的次數(shù)、權因子集合以及節(jié)點列表數(shù)據(jù)后，式(16)中只有Nj,k(u)未知，因此求出Nj,k(u)的值，根據(jù)式(2)可以推導出

(17)

由式(2)可知：

(18)

(19)

由式(18)和式(19)解得Ni,5(u)，同理可得Ni-1,5(u)、Ni-2,5(u)、Ni-3,5(u)、Ni-4,5(u)、Ni-5,5(u)，通過求得以上基函數(shù)便可求出i段曲線方程pi(u)，其他段方程也可由此求得，最后將這些曲線段依次連接起來，從而得到整條非均勻有理B樣條p(u)。

1.4 高階非均勻B樣條控制點的反算

存在n個型值點的k次非均勻B樣條曲線，存在n+2個未知控制點。為了保證速度、加速度等的約束，令控制點為n+k。五次非均勻B樣條曲線，首末節(jié)點取重復度r=k+1=6，因此具有n+k+3個節(jié)點，由式(8)可知u1=u2=u3=u4=u5=u6=0，un+5=un+6=un+7=un+8=un+9=un+10=1。通過積累弦長參數(shù)化法求得ui。

在計算機輔助設計時，通常需要建立插值曲線用于已存在的曲線形狀描述。非均勻有理B樣條曲線能夠通過給定型值點，從而構建通過該組型值點的曲線所對應的控制點，作為已存在的樣條曲線的形狀描述?？刂泣c的反算開始通過積累弦長參數(shù)化法對節(jié)點矢量進行計算，其次要求首末端點的切線方向固定，即

(20)

1.5 基于高階非均勻B樣條的運動軌跡插補

確定控制點與節(jié)點向量之后，非均勻B樣條曲線的插補即在曲線起始點與終止點之間針對不同的ui值，得到曲線上的點構成插補點的集合，由式(8)可知曲線上ui的值。

五次非均勻B樣條曲線首末節(jié)點重復度s為6,整條曲線的定義域為u∈[u6,un+1]，由此可知式(16)中i的取值范圍為6in，第i段非均勻曲線方程pi(u)和控制點pj為三維向量，表示為

(21)

因此邊界條件得以確定，并且首末端點具有零曲率。設置每個控制點對應的權因子，對控制點進行反算。

2 機械臂軌跡插補仿真與分析

已知型值點，對樣條控制點進行反算后，對整個樣條曲線軌跡進行擬合插補，通過MATLAB建立多軸機械臂模型，進行插補算法仿真。

2.1 機械臂建模

在進行目標軌跡規(guī)劃時，軌跡插補算法的好壞直接影響運動控制系統(tǒng)的整體質(zhì)量，所以在進行多軸運動的軌跡規(guī)劃插補前，需要對插補算法的可行性進行相關的理論建模和系統(tǒng)分析。但是在進行插補算法仿真之前，必須要對多軸機械臂進行建模。采用標準D-H法構建機械臂模型[27]，機械臂的D-H參數(shù)如表1所示。

表1 D-H參數(shù)

2.2 機械臂末端軌跡規(guī)劃

通過給定的型值點[28]，生成期望位置，從空間原點(0,0,0)通過三次非均勻B條和五次非均勻B樣條經(jīng)過型值點(3,11,1)、(21,17,2)、(26,6,3)、(32,1,10)、(60,13,11)、(50,23,15)、(70,28,20)，計算出實際位置運行至(75,35,24)。三次非均勻B樣條運行實際位置如圖1所示，五次非均勻B樣條運行實際位置如圖2所示。雖然五次相對于三次計算量更大，但是通過圖1和圖2對比可知三次和五次非均勻B樣條曲線都可以平順地到達末端。

圖1 三次非均勻B樣條運行實際位置

圖2 五次非均勻B樣條運行實際位置

2.3 非均勻B樣條控制點

通過給定型值點根據(jù)非均勻B樣條算法生成從起始點(0,0,0)到終點(75,35,24)的樣條曲線，根據(jù)控制點求解算法，求出三次非均勻B樣條控制點依次連接生成如圖3所示的控制多邊形，五次非均勻B樣條控制點依次連接生成圖4所示的控制多邊形。由于五次相對于三次控制點更多，因此，圖4比圖3控制局部修改作用更好。

圖3 三次非均勻B樣條控制多邊形

圖4 五次非均勻B樣條控制多邊形

2.4 高階非均勻有理B樣條算法仿真結果分析

為驗證本文算法的可行性和有效程度，采用機械臂模型作為仿真的對象，對本文算法進行仿真，軌跡規(guī)劃由高階非均勻B樣條通過給定型值點進行構建，權重因子皆為1。將圖1和圖2運行軌跡在三維方向進行分解，得到x,y,z軸分別對應的位置(圖5和圖6)、速度(圖7和圖8)、x軸加速度(圖9)、y軸加速度(圖10)、z軸加速度(圖13)。

由圖5～圖8仿真波形可知，五次非均勻B樣條相對于三次非均勻B樣條位置和速度的過渡處都比較平滑，位置軌跡基本一致。

但是由圖9～圖11仿真對比波形可知，五次非均勻B樣條在加速度的過渡處相對于三次非均勻B樣條更加平滑，能夠有效減少運動過程中產(chǎn)生的沖擊。

圖9 x軸加速度對比 圖10 y軸加速度對比

圖11 z軸加速度對比 圖12 x軸加加速度對比

圖13 y軸加加速度對比 圖14 z軸加加速度對比

由圖12～圖14加加速度對比波形可知，五次非均勻B樣條在加加速度方面的過渡平滑提升更加明顯，能夠有效地減少運行過程中的沖擊和振動，增加運動過程中的平穩(wěn)性。

3 結束語

為了解決傳統(tǒng)軌跡規(guī)劃轉折處不平順，易產(chǎn)生變速沖擊和軌跡不能進行局部修改的問題，本文進行樣條算法的構造，進行數(shù)據(jù)點預處理，生成樣條曲線，計算得到曲線控制點，采用MATLAB對機械臂進行建模，用相同的型值點對機械臂進行三維空間的軌跡插補。根據(jù)仿真結果，在滿足速度、加速度和加加速度的要求下，均能夠獲得比較平滑的運行軌跡和速度曲線，但是五次非均勻B樣條實際軌跡更加圓滑，而且機械臂的加速度和加加速度突變處過渡更加光滑，能夠有效的減少機械臂在運行過程中的沖擊，使運行更加平穩(wěn)。