基于3-5-3多項式插補(bǔ)的六軸機(jī)器人軌跡優(yōu)化

馮建超,魯文其,林軍華,魯玉軍,岑國建

(1.浙江理工大學(xué)機(jī)械工程學(xué)院,浙江 杭州 310018;2.邁得醫(yī)療工業(yè)設(shè)備股份有限公司,浙江 臺州 317607;3.寧波中大力德智能傳動股份有限公司,浙江 寧波 315301)

0 引言

多軸機(jī)器人在生產(chǎn)生活中扮演著重要角色[1-5],已被廣泛應(yīng)用于工業(yè)生產(chǎn)、醫(yī)療設(shè)備和國防軍事等社會領(lǐng)域[6-8]。在多軸機(jī)器人的軌跡規(guī)劃研究中,軌跡規(guī)劃的優(yōu)劣對于機(jī)器人作業(yè)完成的質(zhì)量有著重要影響[9-10]。目前已經(jīng)研究出眾多的軌跡規(guī)劃方法[11-14],并且在軌跡規(guī)劃中會根據(jù)很多優(yōu)化指標(biāo)來進(jìn)行軌跡優(yōu)化[15-19]。如文獻(xiàn)[20]采用了3次多項式插值法進(jìn)行軌跡優(yōu)化,該方法通過引入2個虛擬插值點,從而達(dá)到運行過程中速度和加速度穩(wěn)定的目的,但是存在因為3次多項式插值法無法做到使加速度和加加速度也保持平滑和連續(xù),所以機(jī)器人在實際運行中會產(chǎn)生抖動的問題。文獻(xiàn)[21]和文獻(xiàn)[22]提出了一種高階非均勻有理B樣條插補(bǔ)的控制方法,能夠使多軸機(jī)器人工作時軌跡轉(zhuǎn)彎處不平滑,從而容易產(chǎn)生變速沖擊的問題得到有效改善,但是存在計算相對比較復(fù)雜的問題。

在使用3次多項式插補(bǔ)法時能夠保證位移和速度保持連續(xù),而加速度軌跡無法保持連續(xù),且位移和加速度軌跡的各關(guān)節(jié)末端點均不為0,加速度軌跡在起始點存在跳變,容易造成啟停抖動;在使用5次多項式插補(bǔ)法時能夠保證位移、速度和加速度都保持連續(xù),且加速度軌跡的起始點和末端點均為0,可以避免啟停抖動的影響,但是相對于3次多項式而言,整個軌跡規(guī)劃過程都采用高階多項式的計算量會過于復(fù)雜。對此,為了給六軸機(jī)器人提供一種能優(yōu)化加速度連續(xù)性、啟停穩(wěn)定性又能簡化計算量的控制策略,提出了一種基于3-5-3多項式插補(bǔ)的機(jī)器人軌跡規(guī)劃控制策略,該方法通過設(shè)置2個插值點,利用5次多項式對3次多項式進(jìn)行插補(bǔ)軌跡優(yōu)化,下面對其原理進(jìn)行闡述及驗證。

1 5次多項式插補(bǔ)軌跡規(guī)劃

5次多項式有6個需要求解的多項式系數(shù),可以同時設(shè)置起始點和目標(biāo)點的約束條件,包括角度、角速度和角加速度,設(shè)關(guān)節(jié)角滿足以下公式,即

(1)

用θ0和θf表示2個相近的軌跡點,可以將其看成一段軌跡的起始點和終止點,并用v0和vf表示起始速度和終止速度,所以可以得到表達(dá)式為

(2)

將約束條件代入式(1),可以求得所需的多項式系數(shù),從而得到5次多項式軌跡規(guī)劃的軌跡,即

(3)

2 基于3-5-3多項式插補(bǔ)的機(jī)器人軌跡規(guī)劃控制

5次多項式插補(bǔ)法軌跡規(guī)劃可以保證位移、速度和加速度都保持連續(xù),而3次多項式插補(bǔ)法軌跡規(guī)劃僅能保證位移和速度保持連續(xù),無法保證加速度保持連續(xù),且3次多項式軌跡規(guī)劃的各關(guān)節(jié)加速度軌跡在起始點存在跳變,在末端點均不為0,容易造成啟停抖動,影響機(jī)器人關(guān)節(jié)運行的穩(wěn)定性。

為了解決上述問題,提出了一種基于3-5-3多項式插補(bǔ)軌跡優(yōu)化的控制方法,在3次多項式插補(bǔ)法軌跡規(guī)劃的基礎(chǔ)上,通過設(shè)置插值點,在插值點之間插補(bǔ)5次多項式軌跡,充分利用5次多項式能夠保證加速度保持連續(xù)的優(yōu)點,下面對其原理進(jìn)行闡述。

首先,使用3次多項式對機(jī)器人軌跡的起始點和終止點之間進(jìn)行插補(bǔ),將關(guān)節(jié)角和時間記作函數(shù)θ(t),讓函數(shù)依次通過每個路徑點,3次多項式的表達(dá)式為

θ(t)=a0+a1t+a2t2+a3t3

(4)

由于初始點和終止點的速度都是0,其中tf表示為結(jié)束時間,則得到

(5)

運動軌跡上的關(guān)節(jié)速度和加速度為

(6)

將初始點和終止點的速度和加速度代入到式(4)和式(6)可以得到

(7)

根據(jù)以上條件,求解可以得到以下系數(shù)代入到式(4),即

(8)

在路徑上設(shè)置插值點,將路徑點上2個相鄰的插值點作為該段插值路徑的初始點和終止點,此時關(guān)節(jié)速度肯定不為0,所以需要計算過路徑點的3次多項式插值表達(dá)式,此時的速度約束條件為

(9)

將約束條件代入式(4)和式(6)可得

(10)

根據(jù)以上條件,求解可以得到以下系數(shù)代入式(4),即

(11)

其次,為了保證加速度軌跡保持連續(xù)以及避免3次多項式啟停時會產(chǎn)生沖擊的問題,對3-5-3多項式的樣條插值函數(shù)進(jìn)行構(gòu)造。

在插值點處,樣條曲線是一個k次多項式,它具有保持k-1階導(dǎo)數(shù)連續(xù)性的優(yōu)點。3-5-3多項式的樣條插值函數(shù)具有以下特征:1階導(dǎo)數(shù)表示速度軌跡的連續(xù)性;2階導(dǎo)數(shù)表示加速度軌跡的連續(xù)性;3階導(dǎo)數(shù)表示軌跡的穩(wěn)定性。

3-5-3樣條多項式的通式為

(12)

未知系數(shù)aj1i、aj2i、aj3i為第j個關(guān)節(jié)軌跡中3段樣條插值函數(shù)的第i個系數(shù);j=1,2,…,n,n為關(guān)節(jié)個數(shù);hj1(t)、hj2(t)、hj3(t)分別為第j段關(guān)節(jié)中的3段軌跡,3段軌跡依次是3次多項式軌跡、5次多項式軌跡和3次多項式軌跡。

用Xji表示第j段關(guān)節(jié)的插值位置,其中i=0,1,2,3,表示4個插值點的序號,包括起始點、終止點和2個插值點。在笛卡爾坐標(biāo)系中Xji表示機(jī)器人空間坐標(biāo)通過逆運動學(xué)計算得到的關(guān)節(jié)角。

軌跡規(guī)劃有以下已知條件:初始點和末端點的速度和加速度(速度一般取0),第j個關(guān)節(jié)中每一段軌跡的初始點Xj0、插值點Xj1和Xj2、末端點Xj3,并保證插值點間的速度和加速度保持連續(xù)。從而推導(dǎo)計算得到系數(shù)aji和插值點的關(guān)系式為:

A=

(13)

t1、t2、t3為第j段關(guān)節(jié)的3段多項式插值的時間。式(13)為3-5-3多項式系數(shù)a的計算公式。

根據(jù)式(13)可得到式(12)的多項式系數(shù),即可得到優(yōu)化后的3-5-3多項式插補(bǔ)軌跡規(guī)劃方程。

3 實驗測試與分析

3.1 實驗平臺設(shè)計

為了驗證本文所提算法的有效性,設(shè)計了實驗平臺(如圖1所示)進(jìn)行測試,其主要由上位機(jī)、多軸運動控制器、伺服驅(qū)動器和六軸機(jī)械臂等組成。

圖1 實驗測試平臺

3.2 六軸機(jī)器人性能測試

為了驗證本文算法有效性,對引入3-5-3多項式插補(bǔ)函數(shù)的六軸機(jī)械臂進(jìn)行性能測試。

關(guān)節(jié)1的位置、速度和加速度實驗波形如圖2所示。由波形可知,引入算法前,僅能保證速度軌跡的始末點為0,加速度軌跡的起始點存在跳變,末端點不為0,造成關(guān)節(jié)1運行啟停抖動,使位移軌跡產(chǎn)生偏移;引入算法后,關(guān)節(jié)1軌跡在約t=1.7 s和t=3.2 s時引入插值點,位移軌跡從0變化到1.6 rad,速度軌跡由于插入五次多項式軌跡在插值點之間會產(chǎn)生波動,在t=1.8 s時達(dá)到最高點1.6 rad/s,在t=2.6 s時達(dá)到最低點 -0.7 rad/s,加速度軌跡在t=2.1 s時達(dá)到最低點-3.5 rad/s2,在t=3 s時達(dá)到最高點2.5 rad/s2,加速度軌跡的起始點和末端點均為0,能夠避免啟停抖動影響關(guān)節(jié)啟停的穩(wěn)定性。

圖2 關(guān)節(jié)1位置、速度和加速度波形

關(guān)節(jié)2的位置、速度和加速度實驗波形如圖3所示。由波形可知,引入算法前,僅能保證速度軌跡的始末點為0,加速度軌跡的起始點存在跳變,末端點不為0,造成關(guān)節(jié)2運行啟停抖動,使位移軌跡產(chǎn)生偏移;引入算法后,關(guān)節(jié)2軌跡在約t=1.7 s和t=3.2 s時引入插值點,位移軌跡在-0.6 ～1.5 rad之間變化,速度軌跡由于插入五次多項式軌跡在插值點之間會產(chǎn)生波動,在t=1.8 s時達(dá)到最高點1.6 rad/s,在t=2.6 s時達(dá)到最低點 -2.6 rad/s,加速度軌跡在t=2.1 s時達(dá)到最低點-6.2 rad/s2,在t=3 s時達(dá)到最高點5.5 rad/s2,加速度軌跡的起始點和末端點均為0,能夠避免啟停抖動影響關(guān)節(jié)啟停的穩(wěn)定性。

關(guān)節(jié)3的位置、速度和加速度實驗波形如圖4所示。由波形可知,引入算法前,僅能保證速度軌跡的始末點為0,加速度軌跡的起始點存在跳變,末端點不為0,造成關(guān)節(jié)3運行啟停抖動,使位移軌跡產(chǎn)生偏移;引入算法后,關(guān)節(jié)3軌跡在約t=1.7 s和t=3.2 s時引入插值點,位移軌跡在 -1.4 ～0.6 rad之間變化,速度軌跡由于插入五次多項式軌跡在插值點之間會產(chǎn)生波動,在t=1.8 s時達(dá)到最高點1.4 rad/s,在t=2.6 s時達(dá)到最低點 -2.3 rad/s,加速度軌跡在t=2.1 s時達(dá)到最低點-6.0 rad/s2,在t=3 s時達(dá)到最高點5.0 rad/s2,加速度軌跡的起始點和末端點均為0,能夠避免啟停抖動影響關(guān)節(jié)啟停的穩(wěn)定性。

圖4 關(guān)節(jié)3位置、速度和加速度波形

關(guān)節(jié)4的位置、速度和加速度實驗波形如圖5所示。由波形可知,引入算法前,僅能保證速度軌跡的始末點為0,加速度軌跡的起始點存在跳變,末端點不為0,造成關(guān)節(jié)4運行啟停抖動,使位移軌跡產(chǎn)生偏移; 引入算法后,關(guān)節(jié)4軌跡在約t=1.7 s和

圖5 關(guān)節(jié)4位置、速度和加速度波形

t=3.2 s時引入插值點,位移軌跡在 -1 ～0.6 rad之間變化,速度軌跡由于插入五次多項式軌跡在插值點之間會產(chǎn)生波動,在t=1.8 s時達(dá)到最高點1.2 rad/s,在t=2.6 s時達(dá)到最低點 -2 rad/s,加速度軌跡在t=2.1 s時達(dá)到最低點-5.2 rad/s2,在t=3 s時達(dá)到最高點4.5 rad/s2,加速度軌跡的起始點和末端點均為0,能夠避免啟停抖動影響關(guān)節(jié)啟停的穩(wěn)定性。

關(guān)節(jié)5的位置、速度和加速度實驗波形如圖6所示。由波形可知,引入算法前,僅能保證速度軌跡的始末點為0,加速度軌跡的起始點存在跳變,末端點不為0,造成關(guān)節(jié)5運行啟停抖動,使位移軌跡產(chǎn)生偏移;引入算法后,關(guān)節(jié)5軌跡在約t=1.7 s和t=3.2 s時引入插值點,位移軌跡從 0.4 rad變化到1.9 rad,速度軌跡由于插入五次多項式軌跡在插值點之間會產(chǎn)生波動,在t=1.8 s時達(dá)到最高點1.1 rad/s,在t=2.6 s時達(dá)到最低點 -0.5 rad/s,加速度軌跡在t=2.1 s時達(dá)到最低點-2.3 rad/s2,在t=3 s時達(dá)到最高點1.9 rad/s2,加速度軌跡的起始點和末端點均為0,能夠避免啟停抖動影響關(guān)節(jié)啟停的穩(wěn)定性。

圖6 關(guān)節(jié)5位置、速度和加速度波形

關(guān)節(jié)6的位置、速度和加速度實驗波形如圖7所示。由波形可知,引入算法前,僅能保證速度軌跡的始末點為0,加速度軌跡的起始點存在跳變,末端點不為0,造成關(guān)節(jié)6運行啟停抖動,使位移軌跡產(chǎn)生偏移;引入算法后,關(guān)節(jié)6軌跡在約t=1.7 s和t=3.2 s時引入插值點,位移軌跡從0變化到1.3 rad,速度軌跡由于插入五次多項式軌跡在插值點之間會產(chǎn)生波動,在t=1.8 s時達(dá)到最高點0.7 rad/s,在t=2.6 s時達(dá)到最低點0.1 rad/s,加速度軌跡在t=2.1 s時達(dá)到最低點-1.8 rad/s2,在t=3 s時達(dá)到最高點1.5 rad/s2,加速度軌跡的起始點和末端點均為0,能夠避免啟停抖動影響關(guān)節(jié)啟停的穩(wěn)定性。

4 結(jié)束語

為了解決3次多項式軌跡規(guī)劃僅能保證位移和速度保持連續(xù),無法保證加速度保持連續(xù),且3次多項式軌跡規(guī)劃的加速度軌跡在起始點存在跳變,在末端點均不為0,容易造成關(guān)節(jié)電機(jī)啟停抖動,使位移軌跡產(chǎn)生偏移的問題,提出了一種基于3-5-3多項式插補(bǔ)軌跡規(guī)劃控制方法,在3次多項式軌跡規(guī)劃的基礎(chǔ)上,通過插入2個插值點時間t,將路徑劃分為3段,根據(jù)設(shè)置的插值點時間t計算得到插值函數(shù)的系數(shù)a,從而得到3-5-3多項式插值函數(shù)的表達(dá)式。

由實驗測試結(jié)果可知,引入3-5-3多項式插值函數(shù)后,在插值點之間插入5次多項式軌跡規(guī)劃的軌跡,使得各關(guān)節(jié)加速度軌跡在起始點和末端點均為0,可以避免啟停時產(chǎn)生抖動的問題,保證機(jī)器人啟停時的穩(wěn)定性,且各關(guān)節(jié)的加速度運算關(guān)系能夠?qū)崿F(xiàn)連續(xù),但是相對的,軌跡插值點之間會產(chǎn)生5次多項式的波動影響,這就需要后續(xù)研究更高階的混合多項式插補(bǔ)等方法使軌跡運行更加平滑穩(wěn)定。