基于NURBS和Slerp插補機器人S型速度規劃

龐智慧,王桂榮,陳 曉

(1. 中國計量大學機電工程學院,浙江 杭州 311018;2. 浙江錢塘機器人及智能裝備研究有限公司,浙江 杭州 310000)

1 引言

工業機器人的作業過程中,末端軌跡的柔順性是評價機器人作業質量的重要指標之一[1]。實際工業生產過程中,欲使機器人按照規劃路徑運動,一般要經過軌跡規劃、速度規劃、插補三個階段[2]。

機器人末端的軌跡規劃包含對末端位置和姿態的規劃。李永梅等[3]針對機器人的碼垛作業,根據路徑點要求,設計了多段直線的末端軌跡,保證精確通過各路徑點,適用于對作業精度要求不很精確的情形;John[4]以拋物線過渡為例,運用直線+多項式曲線過渡的方式,在復雜曲線的關鍵路徑點附近采用多項式曲線過渡,得到了較為光滑的末端軌跡,但此算法的復雜度隨著中間點的增多而急劇上升。而且,上述算法主要針對末端姿態不變的情形,如末端姿態發生變化,要根據描述末端姿態的方式,選用對應的規劃方式[5]。李黎等[6]基于RPY角描述末端姿態的方式,對三個角度分別進行規劃,結合對位置變量,實現了末端位姿軌跡的規劃。此方法適用于絕大多數姿態,但T. Teramae[7]指出,當繞任意軸旋轉角接近0°或180°時調整繞固定軸旋轉的相鄰姿態會出現姿態的過渡不連續;Y. Nakamura[8]采用四元數描述旋轉運動可有效避免上述問題,可以用來描述機器人末端的姿態。

上述對機器人末端進行軌跡規劃時,解決了末端按照何種路徑作業的問題;而速度規劃解決了機器人經過規劃路徑的方式。J. R[9]對比了梯形速度規劃和S型速度規劃,指出梯形速度曲線由0加速到勻速vmax的過程中,會出現加速度的突變,作業過程會出現抖動,而S型速度曲線將作業過程的速度分為加加速、勻加速、減加速、勻速、減減速、勻減速、加減速7段,保證了加速度的連續性,有效避免了速度突變,進而改善作業質量。

本文針對六軸串聯型機器人,對光滑性較好的NURBS曲線,運用S型速度規劃方法,實現了末端軌跡的柔順性,及作業過程中關節空間與笛卡爾空間下速度的平滑,確保加速度不會發生突變,保證了機器人作業過程的平穩性,減小機器人作業時的沖擊與抖動。最后,通過仿真與實驗,對本文算法進行驗證。

2 機器人末端軌跡構造

對于機器人末端作業路徑的規劃,一般先確定作業過程中的關鍵點,通過插補構造末端路徑。其中,以微小直線段連接相鄰關鍵路徑點的方式最為簡單,但在兩相鄰線段的公共點處,會有方向的突變,整個作業過程也會有很多不光滑點,從而影響作業的質量。文獻[3]的規劃方法在構造復雜軌跡時捉襟見肘,文獻[4]的規劃方法在中間點過多時算法效率較低,且缺少對末端姿態的規劃;而SHI[10]中提出的NURBS曲線可以根據一系列關鍵路徑點作為控制頂點,并在不同的路徑點處給定不同的權因子,構造出一種統一的曲線模型,同時,由于NURBS曲線具有可微性,滿足機器人作業中對于末端軌跡的光滑性要求。此外,結合基于四元數描述的機器人末端姿態規劃,可以滿足復雜作業軌跡的要求。

2.1 NURBS曲線構造

一般而言,一條k次NURBS曲線表達式如下式(1)

(1)

其中,di—n+1個控制頂點,i=0,1,…,n;ωi—n+1個控制頂點處的權因子,ω0>0,ωn>0,其它ωi≥0;u∈[0,1]—歸一化因子;U=[u0,u1,…,un+k+1]—節點向量,各ui非遞減;兩端節點處重復度一般取為k+1,u0=u1=…=uk=0,un+1=un+2=…=un+k+1=1;中間各ui步長為1/(n+1-k),即uk+1=1/(n+1-k),uk+2=2/(n+1-k),…un=(n-k)/(n+1-k);Ni,k(u)—n+1個控制頂點處對應的k次B樣條基函數,詳見文獻[10]。由式(1)可知,NURBS曲線可以表示為笛卡爾空間下各坐標分量的形式。

由文獻[11]中所述,NURBS曲線的一階、二階導數公式如下

(2)

(3)

其中p(u)=A(u)/w(u);根據式(1)中NURBS曲線的分量形式,結合式(2)、式(3)中NURBS曲線的一階、二階導數公式,可求得各節點處三個分量方向的速度、加速度。

運用NURBS曲線進行插補[12]時,需要已知當前時刻的弧長才可進行速度規劃。從p(u1)點到p(u2)點的NURBS曲線長度L為

(4)

一般而言,式(4)中的被積函數很難得到原函數,故選用文獻[13]所述的Simpson數值積分的方法計算NURBS曲線長度,如下式(5)所示

(5)

2.2 基于四元數描述的機器人末端姿態規劃

由本文引言部分所述,基于四元數的機器人末端軌跡規劃可以插值得到平滑的旋轉,有效解決RPY角線性插值時三維旋轉的不平滑問題。

單位四元數有兩種描述形式

q=s+ai+bj+ck

(6)

(7)

式(7)中旋轉軸為f=fxi+fyj+fzk;

聯立式(6)、式(7),有

(8)

式(6)中所述四元數對應的姿態矩陣為

(9)

笛卡爾空間下繞單位向量f旋轉θ角度,得到式(10)所述的旋轉矩陣

(10)

其中,逆時針旋轉為正,順時針為負。顯然,式(9)與式(10)等價。

聯立式(9)、式(10),有

(11)

由式(11)可直接由機器人末端姿態矩陣求出對應的四元數。由于未規定旋轉方向,故對應2個共軛的四元數。

對于由四元數描述的任意兩組姿態q0和q1,一般有兩種方法,Lerp和Slerp。Lerp即為線性插值,即

Lerp(q0,q1,h)=q0(1-h)+q1h

(12)

其中h可理解為NURBS曲線中的參數u,h∈[0,1]。

圖1中,Lerp插值沿著單位圓圓弧上從q0到q1的弦上插值,因此Lerp插值出來的四元數不是單位四元數。將插值得到的四元數正規化,為Nlerp插值,即

圖1 Lerp插值&Slerp插值

(13)

上述插值方法中,當h在區間[0,1]內均勻分布時,得到的插補段并不是均勻分布的,即Lerp插值只具有形式上的“線性插值”。

Slerp(球面線性插值)是對角度的線性插值,即

θh=(1-h)θ0+hθ1

(14)

其中,θ0和θ1分別為四元數q0和q1的旋轉角。令θ0=0,則θh=hθ1。由文獻[7]可知,Slerp插值可以表述為

qh=Slerp(q0,q1,h)=αq0+βq1

(15)

(16)

Slerp插值給出了四元數描述的球面上兩點之間最短的插值曲線,且轉角均勻變化。為了提高四元數對姿態的規劃效率,避免姿態的冗余變化,選擇q1與-q1中與q0夾角較小的做Slerp插值,即

Slerp(q0,q1,h)=

(17)

Slerp插值對h一階、二階導數分別為

(18)

(19)

(20)

3 機器人軌跡規劃算法設計

本文設計的機器人軌跡規劃算法如下:

1)構造末端軌跡

機器人的末端軌跡包含機器人末端的位置和姿態,合稱為位姿,是作業過程時間t的函數;因此,可以對位置和姿態分別規劃。由本文第一章所述,機器人末端位置采用NURBS曲線規劃,姿態采用四元數的Slerp規劃;

2)劃分關鍵點

對Step1中構造出的機器人末端軌跡按照機器人的插補周期劃分為n段,得到n+1個關鍵路徑點;

3)對關鍵點處求逆解,逆解取優

工業機器人的工作空間分為關節空間和笛卡爾空間。關節空間下機器人通過運動學正解可得到一組關節角對應的末端位姿;反過來,笛卡爾空間下通過機器人的運動學逆解可求得對應的關節角,而且運動學逆解所得的關節角通常不唯一,需要進行逆解取優。

機器人正運動學方程為

(21)

式(21)也可表述為

x=x(θ)

(22)

式(22)兩端對時間t求導,有

(23)

式(23)也可表述為

(24)

式(24)兩端對時間t求導,有

(25)

機器人的逆運動學求解,可以用KDL、IKFAST[15]等運動算法庫。KDL采用Newton-Raphson迭代法[16]求解運動學逆解的數值解;IKFAST針對滿足Pieper法則[17]的機器人,求解運動學逆解的解析解。機器人運動學逆解的求解速度直接影響了機器人軌跡規劃的實時性,因此選用一種快速準確的運動算法庫具有重要的意義。

機器人求解運動學逆解時,一般需要進行逆解取優。逆解取優原則通常為各關節的運動變化量最小,即運動姿態變化最平緩。此處可以參照笛卡爾空間下位移的定義,定義關節空間下的“位移”

(26)

即式(26)中的Δθ最小,或Δθi最小。

4)在關節空間進行S型速度規劃

S型速度規劃可以保證加速度連續變化,有效減小了機器人作業過程中關節電機的沖擊,避免作業過程中的頻繁起停,因此可選用S型速度曲線在關節空間下對各關節角度進行規劃。

七段式S型速度規劃中,包含加加速、勻加速、減加速、勻速、減減速、勻減速、加減速7段加減速過程,如圖2所示:

圖2 七段式S型速度規劃

圖2中,每段結束時刻記為t1～t7,每段用時為T1～T7,位移為s,有

T1=T3=T5=T7,T2=T6

(27)

S型速度規劃中,五段式缺少勻加速、勻減速段,即T2=T6=0;六段式缺少勻速段,即T4=0;四段式缺少勻加速、勻速、勻減速段,即T2=T4=T6=0。

S型速度規劃流程圖如圖3所示。

圖3 S型速度規劃流程

圖3中,s1、s2分別為經過加加速段、勻加速段、減加速段達到最大速度完成的位移,以及僅經過加加速段、減加速段達到最大速度完成的位移。

(28)

(29)

S型速度規劃過程中,需要輸入待規劃段的起點終點的位姿坐標、輸入最大速度vmax、最大加速度amax,以及各關節電機的加加速度J,計算s、s1、s2,根據圖3中的規劃流程,判斷S型速度規劃的類型,根據文獻[14]計算S型速度規劃過程中由本章(2)中n+1個關鍵路徑點所得n段軌跡的時間、速度、位移;之后按照機器人插補周期得到n段軌跡內所有插補點的關節角,發送至各關節電機,按照本文所述的規劃方式得出規劃軌跡,與原有激勵軌跡進行對比分析;

5)將4)中n段軌跡內規劃所得所有插補點的關節角度、角速度、角加速度代入式(22)、式(23)、式(25)所述運動學正解、速度、加速度的表達式,得到笛卡爾空間下位姿的各個分量關于時間t的曲線,以及末端速度、加速度曲線。

機器人末端軌跡規劃的流程圖如圖4所示。

圖4 機器人末端軌跡規劃流程圖

4 仿真結果分析

4.1 機器人運動學模型

機器人正逆運動學的求解,需要先建立機器人的連桿坐標系,求解機器人的DH參數。本文采用SR4C機器人作為研究對象,其DH參數如表1所示:

表1 SR4C的DH參數

表1所示的DH參數表中,ai(mm)為連桿長度,αi為扭轉角,di(mm)為連桿偏移量,θi為關節角。SR4C機器人各關節均為旋轉關節,因此關節角可在限位范圍內任意給定。

4.2 仿真結果分析

為了驗證本文所述算法的有效性,選取機器人常用的伯努利雙紐線作為激勵軌跡,據此構造NURBS曲線。伯努利雙紐線方程

(x2+y2)2=2a2(x2-y2)

(30)

(31)

取焦距a=50√2mm;激勵軌跡如圖5所示。

圖5 激勵軌跡

NURBS曲線參數:曲線次數k=3;節點向量U=[0,0,0,0,1/313,2/313,…,312/313,1,1,1,1];控制頂點由式(32)確定:

(32)

其中i=0,1,…,315;式(32)為伯努利雙紐線的參數方程,其中θ取值范圍[0,π/4],[3π/4,5π/4],[7π/4,2π],θi步長為0.01rad;控制因子為ω=[1,1,…,1];控制頂點個數n+1=316;所構造NURBS曲線是機器人末端按照伯努利雙紐線作業時,在機器人基坐標系的坐標表示。

機器人末端姿態的規劃采用本文2節所述的Slerp規劃,即轉角均勻變化。由式(21)中的機器人運動學方程可求出式(32)對應的位姿矩陣,結合式(11)即可求得對應的四元數。

本文中,由式(32)可以得到規劃過程中的所有路徑點,結合本文算法可構造出末端的激勵軌跡,輸入的速度、加速度、加加速度參數如表2所示:

表2 各關節參數

為了說明本文所提算法的有效性,選取基于5次多項式的S型速度規劃的方式做對比,即式(32)中相鄰路徑點之間采用5次多項式構造激勵軌跡,得到激勵軌跡的對比圖,如圖6所示。

圖6 本文算法與5次多項式

由圖6可知,相比于5次多項式的規劃方式,本文所提算法規劃出的軌跡更加接近真實的激勵軌跡,且軌跡的光滑性有顯著的提升。采用本文算法與5次多項式規劃的激勵軌跡的曲率圖如圖7所示。

圖7 不同規劃算法的曲率圖

其中,NURBS曲線上任意點曲率計算公式可由文獻[11]確定。

(33)

由圖7可知,采用5次多項式規劃所得的激勵軌跡只在有限個點處曲率值不為0,說明5次多項式規劃的激勵軌跡由多段直線組成,激勵軌跡平滑性有待提高;而本文所提算法基于NURBS的曲線構造方法,整段激勵軌跡上曲率均不為0,且曲率值低于原激勵軌跡,即構造較為平緩的NURBS曲線,有效提高了激勵軌跡的平滑性。

根據圖4中的軌跡規劃流程,可得本文算法規劃所得的各關節位移、速度、加速度曲線,如圖8所示。

圖8 本文規劃下關節空間曲線

圖8中,按照本文所提規劃算法作業過程中,各關節的角加速度隨時間連續變化,未出現突變現象,保證了角速度、角度曲線的光滑性,避免了5次多項式規劃中出現的頻繁起停的問題,減小了關節電機作業過程中的沖擊,一定程度上延長了關節電機壽命;同時,隨著關節電機角度曲線光滑性的提升,作業質量也隨之改善。

5 小結

本文對工業機器人作業過程中末端軌跡的平滑性進行研究,基于NURBS曲線規劃的方式構造一出種統一的曲線模型,改善了作業路徑的平滑性;通過基于四元數的Slerp規劃方法對末端姿態使機器人末端姿態均勻變化,減少了姿態冗余的情形;S型速度曲線使得速度曲線變得平滑,避免了5次多項式規劃方式中機器人頻繁起停的問題,各關節加速度不會產生突變,減小了作業過程中對關節電機的沖擊,保證了作業質量,延長了各關節電機的使用壽命。