基于5次非均勻B樣條曲線的4自由度機器人軌跡規劃

王 青， 葉明露， 賈秀海

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

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

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

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

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

1 SCARA 4自由度機器人

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

圖1 SCARA 4自由度機器人

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

圖2 機器人連桿坐標系

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

表1 機器人D-H連桿參數

2 B樣條插值曲線構造

2.1 B樣條曲線概述

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

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

(1)

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

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

(2)

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

(3)

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

2.2 5次B樣條插值曲線

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

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

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

(4)

(5)

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

(6)

設位置時間節點序列為：

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

(7)

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

(8)

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

(9)

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

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

AD=p。

(10)

式(10)中：

(11)

(12)

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

D=A-1p。

(13)

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

(14)

由式(14)可知，只要知道關節的角度位置與其對應的時間節點，便可求解出其軌跡曲線及相應的速度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關節的角度；q3為第3關節的移動距離，可通過拖動滑塊調節數值以控制機械臂的運動。

圖3 機器人三維模型

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

表2 各關節節點

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

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

圖5 機器人末端執行器運行軌跡

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

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

由以上仿真結果可看出：

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

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

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

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

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

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

4 結語

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