王 青， 葉明露， 賈秀海

(西安工程大學 機電工程學院， 陜西 西安 710048)

機器人的軌跡是指機器人根據(jù)外部環(huán)境任務要求，從起點運動到終點的一段運行軌跡。通常任務會給定首末端點或一些中間路徑點，再通過運動學逆解獲取相應的關節(jié)角度，最后利用插值函數(shù)對軌跡進行插值計算，力求獲得光滑連續(xù)沒有沖擊突變的運行軌跡，這一過程通常稱為軌跡規(guī)劃。

機器人的軌跡規(guī)劃是運動控制的前提，機器人在運動過程中能夠快速平穩(wěn)、連續(xù)無沖擊地完成指定任務是研究者追求的目標[1]。文獻[2-3]中對3次多項式插值方法進行了研究，規(guī)劃的軌跡雖然滿足對位置和速度的任務要求，但加速度存在突變，這會產生沖擊振動，增加機器人的磨損進而影響其使用壽命。文獻[4]中提出了一種考慮狀態(tài)和輸入約束的解析方法，但該方法的模型建立及計算較為復雜，對機器人的控制器性能要求較高。文獻[5-6]中研究了5次多項式插值方法，實驗結果表明加速度無突變，整體精度和穩(wěn)定性比3次多項式好，但并未考慮加加速度對機器人運行軌跡的影響。

機器人的加加速度存在突變，往往意味著加速度曲線存在尖點，即加速度不光滑連續(xù)，這會增加機器人運行過程中的不穩(wěn)定性，影響工作的完成[7]。李黎等[8]利用5次和7次等高階多項式進行插值，雖然彌補了3次多項式插值的缺點，但是隨著多項式階次的提高，約束條件、計算復雜程度亦會增加。SEDDAOUI等[9]在笛卡爾空間中進行軌跡規(guī)劃，利用動力學耦合效應完成目標的抓取，但并未考慮到逆運動學方程的存在而導致的動力學奇異性問題。針對此問題，文獻[10]中提出了一種結合阻尼最小二乘和反饋補償?shù)姆椒?，將軌跡規(guī)劃問題轉化為多目標優(yōu)化問題，有效避免了奇異性的影響。文獻[11-12]中分別對4-3-4和3-5-3分段多項式插值方法進行了研究，結果表明軌跡和速度曲線均平滑連續(xù)，但加速度曲線不平滑，加加速度曲線不連續(xù)。文獻[13]中采用3次B樣條曲線進行機械臂的軌跡規(guī)劃研究，并基于隨機分布示教點對機械臂進行軌跡規(guī)劃；文獻[14]中也采用3次B樣條曲線進行機械臂的軌跡規(guī)劃研究，采用復合柯特斯積分公式完成了對自由曲線的擬合，但都未考慮3次B樣條曲線存在的加加速度突變問題。文獻[15-16]中以時間、沖擊等為優(yōu)化目標，采用5次非均勻B樣條插值方法進行軌跡規(guī)劃，結果表明機械臂軌跡精度與穩(wěn)定性均較好，工作效率顯著提高。

綜上分析，軌跡規(guī)劃中常用的低階多項式和分段多項式插值都存在加加速度不連續(xù)光滑的問題，高階多項式雖然保證了加加速度的連續(xù)性，但是約束條件和計算復雜性都顯著增加；而低階B樣條曲線插值曲線同樣也不能保證加加速度的連續(xù)光滑性。針對上述問題，課題組以某公司SCARA 4自由度機器人為對象，研究一種5次非均勻B樣條曲線插值方法，以期得到一種光滑性和穩(wěn)定性良好的機器人運行軌跡。

1 SCARA 4自由度機器人

課題組以圖1所示的SCARA 4自由度機器人為對象進行研究。該機器人由3個旋轉關節(jié)和1個移動關節(jié)組成，各旋轉關節(jié)軸線互相平行，在水平面內進行定位和定向，移動關節(jié)用于實現(xiàn)末端執(zhí)行器在垂直方向上的的運動。這種機器人具有速度快、精度高和柔性好等特點。

圖1 SCARA 4自由度機器人

對機械臂進行軌跡規(guī)劃的前提是對機械臂進行運動學分析。由各關節(jié)的姿態(tài)變量經(jīng)齊次變換矩陣計算得到末端執(zhí)行器的位姿，這是正運動學解；由末端執(zhí)行器的位姿逆推得到各關節(jié)的姿態(tài)是運動學逆解。首先對機械臂各連桿關節(jié)從0開始依次編號，然后根據(jù)D-H參數(shù)法建立機械臂連桿坐標系如圖2所示。

圖2 機器人連桿坐標系

由建立好的連桿坐標系得到表1所示的各連桿參數(shù)。其中，l1=250 mm，l2=150 mm，θ1，θ2和θ4為旋轉關節(jié)變量，d3為移動關節(jié)變量。

表1 機器人D-H連桿參數(shù)

2 B樣條插值曲線構造

2.1 B樣條曲線概述

B樣條曲線是貝塞爾曲線的一種一般化，具有幾何不變性、凸包性和局部支撐性等許多優(yōu)良性質，在計算機圖形學中應用廣泛[17]。

給定n+1個控制點Di(i=0,1,2,…,n),可定義k次B樣條曲線的表達式：

(1)

式中，Ni,k(u)是k次B樣條基函數(shù)。

由Cox-de Boor遞歸公式推導如下：

(2)

U是m+1個非遞減數(shù)的集合，u0≤u1≤u2≤…≤um，ui稱為節(jié)點，集合U稱為節(jié)點向量，n，m和k必須滿足m=n+k+1。特別地，規(guī)定0/0=0。[ui,ui+1]是第i個節(jié)點區(qū)間，某些ui可能相等，節(jié)點區(qū)間會不存在。如果一個節(jié)點ui出現(xiàn)k次(即ui=ui+1=…=ui+k-1)，其中k>1，則ui是一個重復度為k的多重節(jié)點。為保證樣條曲線經(jīng)過首、末端點，一般令兩端點處重復度為k+1，即u0=u1=…=uk=0，un+k=un+k+1=…=un+2k=1。基函數(shù)Ni,k(u)在[ui,ui+k+1]上非零,即Ni,k(u)在k+1個節(jié)點區(qū)間[ui,ui+1],[ui+1,ui+2],…,[ui+k,ui+k+1]上非零，在其他區(qū)間均為零。因此B樣條曲線也可表示為：

(3)

式(3)體現(xiàn)出k次B樣條曲線上的點p(u)最多和k+1個控制點有關，與其他頂點無關。也就是說如果想要修改機械臂的某一段軌跡，只要重新計算該段的軌跡控制點，并不用對整條軌跡重新進行計算。

2.2 5次B樣條插值曲線

k次B樣條曲線具有Ck-1連續(xù)的特性[18]。且為保證軌跡曲線無沖擊突變，插值函數(shù)至少4階可導，即要求k≥4。由于4次B樣條加加速度存在突變，且高次B樣條函數(shù)計算更為復雜，因此課題組采用5次B樣條函數(shù)進行曲線插值，既降低了計算難度又可保證運動軌跡平滑、連續(xù)、無突變。

由2.1節(jié)內容可知若想規(guī)劃一條B樣條曲線，則需求解出控制點di和節(jié)點矢量U。然而B樣條雖由一系列控制點決定，但并不經(jīng)過這些控制點，所以在軌跡規(guī)劃時，選取的機械臂末端路徑點并不能直接作為控制點使用，但是可以將這些路徑點作為型值點，進而反求出曲線上對應的控制點。

具有n+1個型值點pi(i=0,1,2,…,n)的k次B樣條曲線將由n+3個控制頂點Di(i=0,1,2,…,n,n+1,n+2)以及節(jié)點矢量U=[u0,u1,…,un+k+3]定義。節(jié)點矢量最后的下標是n+k+3，是因為節(jié)點數(shù)比控制頂點多出的數(shù)量為r=k+1，而控制頂點數(shù)比型值點數(shù)多2個，多出的2個為與首末型值點重合的點。

(4)

(5)

由Cox-de Boor遞歸公式推導出機械臂關節(jié)各階導矢的遞推公式：

(6)

設位置時間節(jié)點序列為：

Q=(pi,ti),i=1,2,…,n。

(7)

為使規(guī)劃的軌跡經(jīng)過選定的路徑點，下面進行控制點Di的求解，列出如下n+1個方程：

(8)

曲線所要求解的控制點為n+k個，即需要n+k個方程，因此在式(8)提供的n+1個方程的基礎上再補充k-1=4個條件：

(9)

式中：v0和ve分別表示關節(jié)起始和終止時的速度;a0和ae分別表示關節(jié)起始和終止時的加速度。

由式(8)和(9)可得到下列n+5個方程組：

AD=p。

(10)

式(10)中：

(11)

(12)

由式(10)可反解出控制點：

D=A-1p。

(13)

根據(jù)求出的控制點和節(jié)點便可得到關節(jié)在t∈[t0,tn]的軌跡，由式(6)可推導出其各階導曲線：

(14)

由式(14)可知，只要知道關節(jié)的角度位置與其對應的時間節(jié)點，便可求解出其軌跡曲線及相應的速度v(t)、加速度a(t)和加加速度j(t)曲線。

3 仿真實驗

以SCARA 4自由度機器人為研究對象，利用MATLAB中的機器人工具箱Robotics Toolbox對機器人進行建模，其三維模型如圖3所示。左側Teach部分為示教模塊，x，y和z為機器人末端坐標；R，P和Y為末端繞定軸的旋轉角，分別代表滾動、俯仰和偏轉；q1，q2和q4分別為第1、2和4關節(jié)的角度；q3為第3關節(jié)的移動距離，可通過拖動滑塊調節(jié)數(shù)值以控制機械臂的運動。

圖3 機器人三維模型

將機器人末端執(zhí)行器的運行路徑離散化，選擇一組8個路徑點作為型值點，通過建立的機器人模型進行逆運動學的求解，得到各關節(jié)在路徑點處對應的位置如表2所示。由于關節(jié)4本身的轉動對機械臂整體的運行軌跡并無影響，所以為簡化計算過程將其值均設為零。關節(jié)3為移動關節(jié)，其余為轉動關節(jié)。

表2 各關節(jié)節(jié)點

針對上述數(shù)據(jù)和建立的模型，采用第2節(jié)中構造的5次B樣條函數(shù)對其軌跡曲線進行插值，得到4自由度機器人末端執(zhí)行器的運行軌跡曲線如圖4所示。從圖中可以看出插值曲線均通過8個型值點，有10個控制點，通過調節(jié)控制點便可改變插值曲線的形狀。圖5所示為機器人末端執(zhí)行器的實時軌跡，與圖4中5次B樣條函數(shù)插值出的曲線基本一致。

圖4 5次B樣條插值的軌跡曲線

圖5 機器人末端執(zhí)行器運行軌跡

設定總運行時間為20 s，起始和終止時刻的速度、加速度均為零，進行5次B樣條插值，得到如圖6所示的的機器人各關節(jié)位置、速度、加速度和加加速度曲線。為對比分析，同時進行了5次多項式軌跡規(guī)劃，得到結果如圖7所示。

圖6 5次B樣條插值的各關節(jié)運動曲線

由以上仿真結果可看出：

1) 機器人按照5次多項式與5次B樣條插值分別得到的運行軌跡運動，均能完成任務，且起始和終止時刻的速度、加速度均為零，滿足約束要求。

2) 觀察圖6(a)和圖7(a)的位置曲線發(fā)現(xiàn)按5次多項式插值方法規(guī)劃出的運動軌跡明顯沒有按5次B樣條插值得到的運動軌跡光滑流暢，即機器人按5次多項式插值規(guī)劃軌跡運動時不斷地轉彎動作，使得機器人實際運動難度加大。

圖7 5次多項式插值的各關節(jié)運動曲線

3) 觀察圖6(b)、6(c)和圖7(b)、7(c)的速度和加速度曲線均發(fā)現(xiàn)5次多項式插值規(guī)劃得到的速度和加速度曲線雖然也光滑連續(xù)，但是曲線波動非常劇烈，且波動幅度也非常大，速度波動區(qū)間為-1.05～1.22 rad/s，加速度波動區(qū)間為-1.75～1.75 rad/s2；反觀5次B樣條插值得到的速度和加速度曲線波動則要緩和得多，且波動范圍較小，分別為-0.87～1.05 rad/s和-0.79～0.61 rad/s2。速度和加速度波動幅度小、波動頻率低都意味著機器人運行更平穩(wěn)，因此5次B樣條插值軌跡顯然要好很多。

4) 由圖7(d)的加加速度曲線發(fā)現(xiàn)5次多項式對應的加加速度曲線變化非常劇烈，波動范圍為-13.96～15.71 rad/s3，且存在很多突變點，說明機器人在不斷地加速和減速，這會影響機器人運動的平穩(wěn)性；在機器人末端執(zhí)行器載有目標物的情況下，非常容易導致目標物的掉落，嚴重影響機器人的有效工作。圖6(d)5次B樣條插值的加加速度曲線則波動較為緩和，波動范圍為-0.70～0.70 rad/s3，且曲線光滑連續(xù)，說明對應的機器人加減速較為緩慢和連續(xù)，即機器人運動平穩(wěn)。

綜上可得，與5次多項式的插值曲線相比，5次B樣條插值的曲線更加光滑順暢，波動更少且峰值較低。這表明機器人在整個運動過程中順暢連續(xù)不會發(fā)生抖動卡頓等現(xiàn)象，能夠有效地完成工作任務。

4 結語

課題組以SCARA 4自由度機器人為研究對象，采用5次B樣條曲線對其運行軌跡進行了規(guī)劃研究。利用弦長參數(shù)法和平均值法選取節(jié)點，并用離散路徑點反解出控制點，通過指定端點導矢并且保證起始終止時刻速度、加速度都為零，得到機器人軌跡曲線。通過MATLAB仿真實驗，并與5次多項式插值結果進行對比，得出5次B樣條插值的曲線更加光滑順暢，波動更少且峰值較低，機器人末端執(zhí)行器運行路徑平穩(wěn)順滑，各關節(jié)位置、速度曲線光滑連續(xù)，加速度以及加加速度曲線亦無沖擊突變。分析結果表明：基于5次B樣條軌跡規(guī)劃的方法合理有效，可降低機器人運行過程中的振動沖擊、減少磨損，進而延長使用壽命。本研究也為后續(xù)的軌跡優(yōu)化和控制系統(tǒng)設計提供了參考。