基于時間最優的機器人位姿同步軌跡規劃方法*

曹鈺婷,王金濤,劉明敏,樊志偉

(1.中國科學院沈陽自動化研究所機器人學國家重點實驗室,沈陽 110016;2.中國科學院機器人與智能制造創新研究院,沈陽 110169;3.中國科學院大學,北京 100049;4.沈陽新松機器人自動化股份有限公司,沈陽 110168)

0 引言

目前,工業機器人被廣泛應用于汽車、電子、輕工與航空航天等行業[1-2]。由于生產任務的加重和生產需求不斷提高,制造業對工業機器人的性能提出了更高的要求:時間最優、能耗最優或沖擊最優[3-4]。其中,時間最優代表效率更高,對于現代企業更有價值,所以優化機器人的運動時間在近年來一直備受廣大學者關注。

工業機器人的時間最優軌跡規劃是一個高度耦合的非線性問題,一些傳統的方法難以直接獲取最優結果,例如不等式約束法、經驗法等。越來越多的專家學者開始嘗試智能優化算法,例如遺傳算法、粒子群算法、神經網絡和模擬退火算法等。GAO等[5]采用粒子群算法(PSO)對4-3-4多項式插值軌跡規劃進行時間優化,該算法參數根據經驗設計,方便使用但具有隨機性,易于陷入局部最優解。汪婷等[6]采用混合優化遺傳算法(GA-DE-SQP)對五次多項式插值軌跡規劃進行優化,優化關節空間中的位置-時間序列,該算法收斂速度更快,適應度值更優。連黎明[7]提出一種多變量時間最優軌跡優化方法,以優化機器人各關節運動軌跡達到時間最優,該方法通用性強,能夠使機器人運行更加平穩。眾多學者在研究機器人時間最優軌跡規劃問題時,普遍在關節空間中進行規劃,無法可視化機器人的末端位置,且求解效率較低,難以完成對機器人末端路徑有明確要求的工作任務,例如弧焊、打磨等。

在此,本文以工業機器人為研究對象,提出了一種基于時間最優的位姿同步軌跡規劃方法,該方法分為路徑插值和速度規劃兩個部分。路徑點位姿通過六維旋量表示,采用共用節向量的B樣條曲線插值方法實現笛卡爾空間中路徑點位置和姿態的同步;根據參數化的插值曲線方程,考慮機器人關節的速度與加速度約束,通過線性規劃計算得到笛卡爾空間路徑參數的相平面圖,進而求得最優參數速度。最后利用雅可比矩陣為機器人各關節分配速度,實現時間最優速度規劃。

1 路徑插值

平滑的路徑插值能有效提高工業機器人的工作性能和使用壽命,使機器人在運動過程中免受因路徑插值不連續而造成的猛烈沖擊和損壞,以免降低機器人的工作效率。工業機器人末端的路徑插值又分為位置插值和姿態插值兩個部分。

1.1 位置插值

根據機器人的作業任務需求,對于給定的路徑點,進行位置插值十分必要。B樣條曲線因其插值平滑、便于修改等優良特性被廣泛應用于軌跡規劃。

B樣條曲線的數學定義為[8]:

(1)

式中:di是控制點,Ni,k(u)是基函數,k是基函數的次數,i是序列標簽,u是曲線的參數?；瘮凳菢嬙霣樣條曲線的基礎,定義在一組參數區間上,由Cox-deBoor遞歸公式給出:

(2)

在眾多B樣條曲線中,三次均勻B樣條曲線計算簡便,能構造各種形狀,具有高平滑性和C2連續性。所以本文選用三次均勻B樣條曲線進行插值:

(3)

式中:D是控制點向量。由式(3)可知,若已知控制點,則可獲得樣條曲線的表達式。然而,大多數情況下是根據任務需求給定曲線會經過的路徑節點。因此需要通過節點反解出控制點,得到曲線方程。

為防止B樣條曲線變化劇烈,考慮將曲線起始和終止節點處的曲率設置為零,即令第一個和第二個、倒數第一個和倒數第二個控制點分別重合。則三次均勻B樣條曲線的控制點可以通過式(4)求得:

(4)

式中:Pi(i=1,2,…,n)是路徑節點?？刂泣cdi與節點pi之間的關系如圖1所示。

圖1 具有自由端點的B樣條曲線

此時通過曲線節點反解控制點的公式如下所示:

(5)

將求得的控制點代入式(3),實現用三次均勻B樣條曲線在路徑節點之間進行位置插值。

1.2 姿態插值

空間剛體姿態的表達方式有旋轉矩陣、歐拉角和四元數。其中四元數由于計算復雜度小、不存在萬向節鎖等優點被廣泛應用于姿態插值。其定義為:

Q=q0+q1i+q2j+q3k

(6)

式中:q0、q1、q2和q3是實數,i、j和k是復數的虛軸。寫成向量形式為Q=[q0,(q1,q2,q3)]。

(7)

式中:ω=(u,v,w)是單位旋轉軸,θ是旋轉角。

根據文獻[9],先對四元數取對數,即:

ln(Q)=θω=(x,y,z)=Pori

(8)

式中:(x,y,z)=(θu,θv,θw),Pori是R3空間中的三維點。

此時,可以使用三次均勻B樣條曲線插值理論。Pori作為節點P,通過式(5)反解出控制點,再將控制點代入式(3),即可得到每兩個節點之間插值點Pori(t)=[x(t),y(t),z(t)],t∈[0,1]。插值點寫成旋轉角與單位旋轉軸相乘的形式:

(9)

將式(9)代入式(7),并對方程兩邊取指數,可得到插值后的單位四元數的實部和復部:

(10)

由式(10)得到一系列路徑插值點的姿態,實現對機器人末端的姿態插值,且與位置插值是同步進行的。

2 時間最優速度規劃

2.1 路徑參數化

針對具有n自由度的機器人系統,其位形空間由n維向量q∈Rn表示[11]。對于在笛卡爾空間中給定的幾何路徑P,可以表示為函數P(s)。其中,路徑參數s∈[0,send],s同時也是關于時間t的單調遞增標量函數s:[0,T]→[0,send],因此幾何路徑可以表示為P(s(t)),t∈[0,1]。因此P的速度和加速度可以表示為:

(11)

2.2 約束條件

機器人的運動約束是指機器人在運動過程中必須遵守的物理約束。本文考慮機器人的運動學約束,即對機器人關節進行角速度和角加速度的約束:

(12)

關節的導數通過雅可比矩陣映射到笛卡爾空間中[12],如式(13)所示。

(13)

(14)

將式(14)代入式(12)中,對關節角速度和角加速度的約束就轉換成了相應的路徑參數速度和加速度的約束,即:

(15)

上述式子中,對雅可比矩陣的描述如下:

J=Jp×Jb=

(16)

式中:Jb(q)∈R6×n表示幾何雅可比矩陣,r∈R3表示軸角的指數坐標,是關于s的函數。在Jp∈R6×6中,A(r)和Rsb(r)則被表示為:

(17)

式中:I是單位矩陣,r∧∈so(3)是向量r相對應的反對稱矩陣。

雅可比矩陣J相對于路徑參數s的一階導數為:

(18)

式中:r與s有關,則Jp的一階路徑參數導數表示為:

(19)

式中:Jb是關節值q的函數,Jb(q)的每一列都可以表示為伴隨矩陣由式(20)給出:

(20)

(21)

式中:

2.3 算法實現

(22)

式中:Δi=si+1-si。

時間最優速度規劃算法主要由正向遞推和逆向遞推兩個部分組成?；诳刂评碚撝锌蛇_性分析的思想,構建出整個算法的框架如圖2所示。

圖2 時間最優速度規劃方法流程圖

(23)

(24)

在算法的第一部分逆向迭代過程中,如果出現某一階段的可達集為0,或者第0階段的可達集不包含已知期望起始速度,則說明該路徑不可時間參數化。第二部分正向迭代在得到可達集的最優狀態值后,對控制量進行了重新選擇,以“貪婪”的方式更新可達集上界。

3 仿真與實驗分析

為驗證本文所提出的機器人時間最優軌跡規劃算法的可行性。以某公司的6自由度“火弧”機器人為研究對象,采用指數積法(product of exponential,PoE)建立機器人的運動學模型[15]。對“火弧”機器人模型機構簡化,當機器人處于零位時進行指數積建模如圖3所示。

圖3 “火弧”機器人指數積建模

圖中:L1=346 mm,L2=600 mm,L3=200 mm,L4=385 mm,L5=925 mm,L6=413 mm。機器人各螺旋軸的參數如表1所示。

表1 “火弧”機器人螺旋軸參數

3.1 路徑仿真

對工業機器人末端執行器的位置和姿態插值理論進行驗證,設置空間路徑節點的位置和姿態信息如表2所示。

表2 機器人末端執行器路徑位姿信息

經過實驗,得到插值過程如圖4所示,圖中稍大的點與長箭頭分別表示給定路徑節點的位置與姿態,稍小的點表示控制點的位置,連接路徑節點的曲線為三次均勻B樣條插值曲線,短箭頭則表示插值點的姿態。由此實現對機器人末端路徑的位姿同步插值。

圖4 三次B樣條插值過程

3.2 速度規劃實驗

為驗證該算法的可行性,本文基于MATLAB和CoppeliaSim搭建“火弧”機器人聯合仿真平臺,并對算法進行測試。已知笛卡爾空間的幾何路徑,對關節信息的約束由表3給出。

表3 運動學約束

通過算法的逆向和正向遞推過程,求解出可達集的范圍如圖5所示。由圖可知,更新后的可達集上界在初始可達集范圍內,且起始點和終止點均滿足約束條件,驗證了算法的可行性。

圖5 可達集范圍

已知路徑參數速度和加速度,通過機器人逆運動學以及式(14)可求得機器人運動過程中規劃的關節值、關節角速度和關節角加速度曲線,如圖6所示。

(a) 關節角度 (b) 關節角速度

根據求得的關節信息,在仿真平臺上進行測試,運動軌跡如圖7所示,由此可見該算法是可行的,并且能夠使機器人末端按照給定路徑曲線運動,可以完成很多對機器人末端路徑有要求的工作任務。

圖7 仿真測試運動軌跡

實驗結果表明在滿足機器人關節運動學約束的條件下,機器人盡可能地選用了最大關節速度或關節加速度,實現了運動的時間最優,提高了機器人的運行效率。

4 結論

針對當前工業機器人時間最優軌跡規劃算法的不足之處,提出了一種機器人末端在笛卡爾空間實現位姿同步的時間最優軌跡規劃方法。采用B樣條曲線和對數四元數插值機器人末端路徑點的位置和姿態,基于路徑曲線和機器人關節約束,以逆、正向遞推求解的方式實現機器人末端路徑的時間最優速度規劃。搭建基于MATLAB和CoppeliaSim的機器人聯合仿真平臺進行實驗,結果表明機器人可以高效且平滑的經過給定路徑節點,達到運行時間最優,實驗證明了該方法在工業機器人上應用的有效性。