劉 堯, 胡 博, 張 輝, 郭 強(qiáng), 張勇軍*, 李宏毅
(1.北京科技大學(xué) 工程技術(shù)研究院,北京 100083; 2.中國(guó)石油集團(tuán)長(zhǎng)城鉆探工程有限公司鉆井一公司,遼寧 盤錦 124010)
軌跡規(guī)劃一直都是工業(yè)機(jī)器人研究的重點(diǎn),是國(guó)內(nèi)外機(jī)器人應(yīng)用領(lǐng)域研究的重點(diǎn)內(nèi)容之一[1-2]。軌跡規(guī)劃可以分為兩個(gè)方面,一個(gè)是機(jī)器人運(yùn)動(dòng)過(guò)程中的路徑選擇,另一個(gè)是減少機(jī)器人關(guān)節(jié)運(yùn)行時(shí)的振動(dòng)與沖擊[3-4]。Starcevic等[5]研究了機(jī)器人的空間路徑規(guī)劃過(guò)程,對(duì)機(jī)器人軌跡進(jìn)行插值時(shí)結(jié)合了三次多項(xiàng)式和四次多項(xiàng)式,這種軌跡規(guī)劃的方法對(duì)于機(jī)器人運(yùn)行中的軌跡突變而引起的機(jī)器人運(yùn)行振動(dòng)等問(wèn)題起到不錯(cuò)的改善作用。文獻(xiàn)[6]用兩種不同頻率的正弦加速度函數(shù)進(jìn)行軌跡規(guī)劃,函數(shù)充分利用了正弦函數(shù)的優(yōu)點(diǎn),把插補(bǔ)段分為加速段、勻速段、減速段進(jìn)行插補(bǔ),降低了速度和加速度的峰值,使機(jī)器人運(yùn)行更加穩(wěn)定,但是該方法算法復(fù)雜、運(yùn)算量大,且不能實(shí)現(xiàn)實(shí)時(shí)控制。
在運(yùn)動(dòng)控制領(lǐng)域中常用的加減速控制算法有梯形速度曲線控制算法和S型速度曲線控制算法[7]。S型速度曲線能夠在一定程度上避免梯形速度曲線加速度不連續(xù)而導(dǎo)致的機(jī)械沖擊或振動(dòng)效應(yīng),是一種較為常用的機(jī)器人運(yùn)動(dòng)控制算法。但理想的S型速度曲線控制算法運(yùn)動(dòng)過(guò)程程序?qū)崿F(xiàn)較為復(fù)雜,而且其加加速度不連續(xù),存在軟沖擊。文獻(xiàn)[8]等提出了S型速度曲線的新算法,有效地將7段S型曲線簡(jiǎn)化為5段S型曲線,簡(jiǎn)化了計(jì)算過(guò)程,但其加加速度不連續(xù)的情況依然存在。Sun等[9]提出了三角函數(shù)構(gòu)造的柔性加減速曲線,使速度、加速度、加加速度連續(xù)可導(dǎo),運(yùn)動(dòng)平穩(wěn)性較高,但其計(jì)算過(guò)程中包含三角函數(shù)運(yùn)算,增加了計(jì)算時(shí)間且丟失了部分精度。
本文以提高系統(tǒng)運(yùn)行效率和安全裕度為目標(biāo),將多項(xiàng)式速度曲線控制算法與傳統(tǒng)S型速度曲線控制算法相結(jié)合,提出一種改進(jìn)的S型速度曲線控制算法,可減小加加速度突變次數(shù),提高加速度連續(xù)變化平滑性,使速度連續(xù)變化更快且移動(dòng)效率更高,然后對(duì)上下料系統(tǒng)典型運(yùn)動(dòng)軌跡進(jìn)行優(yōu)化,將機(jī)械手運(yùn)行過(guò)程中的直角拐彎路徑改為在拐角處以弧線方式過(guò)渡的路徑。改進(jìn)后S型速度曲線控制算法,有效減少發(fā)生沖擊或振動(dòng)的可能性,達(dá)到了提高系統(tǒng)工作穩(wěn)定性、運(yùn)行效率和控制精度的目標(biāo),并通過(guò)仿真與實(shí)際運(yùn)行測(cè)試對(duì)該方法進(jìn)行了驗(yàn)證。
現(xiàn)有的設(shè)備在某些加工要求不高的生產(chǎn)過(guò)程中,一般采用梯形加減速算法或指數(shù)型加減速算法。這2種算法的優(yōu)點(diǎn)是實(shí)現(xiàn)簡(jiǎn)單、計(jì)算量小,但缺點(diǎn)是在加減速啟停階段,會(huì)出現(xiàn)速度和加速度突變,會(huì)對(duì)機(jī)械系統(tǒng)產(chǎn)生沖擊[10]。在加工要求較高的生產(chǎn)過(guò)程中,一般采用7段S型加減速算法,相比于前兩種方法,這種方法的優(yōu)點(diǎn)是能夠計(jì)算出平滑的速度和加速度曲線,其缺點(diǎn)是精度有限、快速響應(yīng)依舊會(huì)產(chǎn)生突變。
梯形加減速控制算法是指系統(tǒng)在加減速過(guò)程中,速度沿某個(gè)固定斜率直線變化,加速度保持恒定,速度均勻變化[11],如圖1所示。
圖1 梯形加減速速度曲線變化圖
梯形加減速速度曲線由以下3個(gè)部分構(gòu)成。
① 加速階段:速度沿固定斜率直線線性增加,公式為
vi=at
(1)
② 勻速階段:速度保持恒定,公式為
vi=vc
(2)
③ 減速階段:速度沿固定斜率線性減小,公式為
vi=ve-at
(3)
梯形加減速算法的優(yōu)點(diǎn)是實(shí)現(xiàn)簡(jiǎn)單,運(yùn)算量小,適用于低速慢變化、低成本的生產(chǎn)加工系統(tǒng)。但由于算法采用恒定的加速度,因而在加減速過(guò)程的起點(diǎn)和終點(diǎn)位置加速度有突變,這就導(dǎo)致在實(shí)際系統(tǒng)中,電機(jī)運(yùn)行過(guò)程中會(huì)有各種振動(dòng)和噪聲,系統(tǒng)運(yùn)行不夠平滑。
指數(shù)型加減速算法是指在加減速階段速度按照指數(shù)規(guī)律上升或下降[12],如圖2所示。
圖2 指數(shù)型加減速速度曲線變化圖
指數(shù)型加減速速度曲線也分3個(gè)階段,具體如下。
① 加速階段:速度曲線按指數(shù)規(guī)律變化,公式為
vi=vc(1-e-i/T)
(4)
② 勻速階段:速度保持不變,公式為
vi=vc
(5)
③ 減速階段:速度沿指數(shù)曲線下降,公式為
vi=vcei-tc/T
(6)
相比于梯形加減速算法,指數(shù)型加減速算法平滑性有所提高,但運(yùn)算量要比梯形加減速算法大,故導(dǎo)致動(dòng)態(tài)響應(yīng)變慢。此外,在加減速過(guò)程的起點(diǎn)和終點(diǎn),依舊存在速度與加速度突變的情況。
S型速度曲線控制算法是一種在機(jī)電系統(tǒng)運(yùn)動(dòng)控制的加減速過(guò)程中對(duì)速度變化曲線進(jìn)行柔性控制的方式[13-14]。與常見的梯形速度曲線控制算法相比,S型速度曲線控制算法加減速曲線變化平滑,加速度是連續(xù)、線性的,可以減少機(jī)械系統(tǒng)因加速度不連續(xù)而產(chǎn)生沖擊[15]。如圖3所示,S型速度曲線軌跡可以分為加加速、勻加速、減加速、勻速、加減速、勻減速、減減速共7個(gè)階段。
圖3 S型速度曲線軌跡圖
若定義運(yùn)動(dòng)位移為s,時(shí)間為t,正向最大加速度為amax,正向最大加加速度為jmax,負(fù)向最大加速度為-amax,負(fù)向最大加加速度為-jmax,則S型速度曲線的軌跡可用以下方程表示:
(7)
(8)
(9)
(10)
在實(shí)際應(yīng)用中,受上下料機(jī)器人機(jī)械部件的限制,手爪在進(jìn)行橫向或縱向移動(dòng)時(shí),其允許的最大加速度、最大速度已經(jīng)確定,在加速度、速度、位移等參數(shù)已知的條件下,可計(jì)算出加速段、勻速段、減速段對(duì)應(yīng)的時(shí)間,進(jìn)而可以得到S型速度曲線的7個(gè)階段對(duì)應(yīng)的7個(gè)時(shí)間,并可進(jìn)一步確定速度曲線的軌跡變化。
若系統(tǒng)最大加速度amax、最大加加速度jmax、初速度v0、末速度ve等條件已知,當(dāng)系統(tǒng)到達(dá)最大加速度amax且無(wú)勻加速、勻減速、勻速段時(shí),此時(shí)位移s、速度v、加速度a和加加速度j的變化如圖4所示,圖4中位移大小為s4,速度最大值為vmax。
圖4 S型曲線變化圖
根據(jù)s4與實(shí)際位移大小L以及vmax與系統(tǒng)設(shè)定的最大速度vs之間的大小關(guān)系,可對(duì)以下情況進(jìn)行分析。
① 若L
② 若vs ③ 若L>s4,vs>vmax,L的長(zhǎng)度足夠長(zhǎng)可以使加速度增大至amax,并且vmax沒有超過(guò)系統(tǒng)最大速度vs,此時(shí)存在勻加速段、勻減速段和勻速段,速度v與加速度a的變化關(guān)系如圖5(c)所示。 圖5 速度與加速度變化曲線 可以看出,S型速度曲線控制算法提高了加減速過(guò)程中的穩(wěn)定性,使運(yùn)動(dòng)過(guò)程中的加速度、速度變化連續(xù),有較好的平滑性,但其參數(shù)較多、計(jì)算復(fù)雜,且存在加加速度不連續(xù)的問(wèn)題,容易使機(jī)器人在運(yùn)動(dòng)過(guò)程中發(fā)生沖擊或振動(dòng),系統(tǒng)穩(wěn)定性還需進(jìn)一步提高。 為解決傳統(tǒng)的S型速度曲線控制算法程序?qū)崿F(xiàn)復(fù)雜,且加加速度不連續(xù)易造成軟沖擊的問(wèn)題。在上述研究的基礎(chǔ)上,將多項(xiàng)式速度曲線控制算法與S型速度曲線控制算法相結(jié)合,提出了一種改進(jìn)的S型速度曲線控制算法,其速度、加速度、加加速度連續(xù)變化,平滑性好。 若定義O、s1、s2、s3分別為起始點(diǎn)、勻速起點(diǎn)、勻速終點(diǎn)、結(jié)束點(diǎn)的位移量,vs是起始點(diǎn)的速度,vm是勻速段的速度,vf是結(jié)束點(diǎn)的速度,ama和amb為實(shí)際情況下系統(tǒng)所允許的最大加速度、最大減速度。改進(jìn)后S型速度曲線位移s、速度v、加速度a、加加速度j之間的變化關(guān)系如圖6所示。圖6中[0,t1]為加速區(qū)間,(t1,t2]為勻速區(qū)間,(t2,t3]為減速區(qū)間,區(qū)間[0,t1]和區(qū)間(t2,t3]的加加速度曲線為二次曲線,區(qū)間[0,t1]和區(qū)間(t2,t3]的加速度曲線為三次曲線。 當(dāng)t∈[0,T1]時(shí),加加速度j在t=0時(shí)有最大值,可令其函數(shù)表達(dá)式為 j(t)=b1t2+b2 (11) 式中,b1和b2為二次函數(shù)待定系數(shù)。根據(jù)位移、速度、加速度、加加速度之間的微積分關(guān)系,可推導(dǎo)出改進(jìn)的S型速度曲線在加速區(qū)間t∈[0,t1]的運(yùn)動(dòng)方程為 (12) 實(shí)際運(yùn)動(dòng)過(guò)程中,需要減少加速段和減速段的時(shí)間來(lái)提高運(yùn)動(dòng)效率,所以加速度曲線a(t)與坐標(biāo)軸t所包圍的面積要盡可能地大,而加速度曲線應(yīng)滿足a(t)max≤ama,結(jié)合初始速度、初始加速度等條件,可建立如下已知條件: (13) 其中ta∈[0,t1],在ta時(shí)刻加速度a(t)取得最大值,此時(shí)j(ta)=0,聯(lián)立式(12)和式(13)可解得: (14) 將式(14)解得的值代入式(5)~式(8),可求得加速段t∈[0,t1]的運(yùn)動(dòng)方程為 (15) 同理,可以推導(dǎo)出改進(jìn)的S型速度曲線在勻速段t∈(t1,t2]的運(yùn)動(dòng)方程為 (16) 也可以推導(dǎo)出改進(jìn)的S型速度曲線在減速段t∈(t2,t3]的運(yùn)動(dòng)方程為 (17) 在實(shí)際的運(yùn)動(dòng)系統(tǒng)中,最大速度即勻速段速度vm和目標(biāo)位移sf一般是給定的,可以求得各個(gè)階段的時(shí)間參數(shù)t1、t2、t3: (18) 由改進(jìn)的S型速度曲線運(yùn)動(dòng)方程可知,當(dāng)時(shí)間參數(shù)t1、t2、t3確定好之后,便可以求得各個(gè)時(shí)間的位移、速度、加速度和加加速度。與傳統(tǒng)的S型速度曲線控制算法相比,改進(jìn)后的算法求解相對(duì)簡(jiǎn)單,實(shí)際運(yùn)用中易于實(shí)現(xiàn),且加速度、加加速度均連續(xù),可以使運(yùn)動(dòng)系統(tǒng)具有更好的高速運(yùn)動(dòng)平穩(wěn)性。 以某汽車零部件加工企業(yè)的數(shù)控機(jī)床自動(dòng)上下料系統(tǒng)為背景,對(duì)改進(jìn)前后S型速度曲線控制算法在桁架式機(jī)器人自動(dòng)上下料流程中機(jī)械手爪運(yùn)動(dòng)路徑規(guī)劃問(wèn)題進(jìn)行研究。自動(dòng)上下料系統(tǒng)要求桁架式機(jī)械人能夠在兩臺(tái)數(shù)控機(jī)床之間完成連續(xù)取料、換料、放料的任務(wù),代替人工操作實(shí)現(xiàn)待加工工件在兩道工序之間的傳遞,現(xiàn)場(chǎng)布局示意圖如圖7所示。 圖7 自動(dòng)上下料系統(tǒng)現(xiàn)場(chǎng)布局示意圖 自動(dòng)上下料機(jī)器人系統(tǒng)的工作流程如圖8所示。首先機(jī)器人手爪從取料料盤處取料,然后分別移動(dòng)待加工工件經(jīng)過(guò)兩臺(tái)數(shù)控機(jī)床,完成兩次加工與換料,最后將加工好的工件放入放料料盤并經(jīng)傳送帶傳送到工件存儲(chǔ)處。 圖8 自動(dòng)上下料機(jī)器人系統(tǒng)的工作流程示意圖 兩臺(tái)數(shù)控機(jī)床的上下料軌跡基本類似,且下料過(guò)程是上料過(guò)程的逆運(yùn)動(dòng),上料軌跡與下料的逆向軌跡也基本系統(tǒng)。本文僅對(duì)一臺(tái)數(shù)控機(jī)床的上料軌跡進(jìn)行分析分析,將機(jī)械手爪上料過(guò)程的運(yùn)動(dòng)軌跡劃分為4段,即由AB、BC、CD、DE組成的直角拐彎路徑,如圖9虛線所示。但這種上料軌跡存在很明顯的缺陷,主要表現(xiàn)在工作效率較低,且在拐角處易造成裝置的抖動(dòng),會(huì)對(duì)設(shè)備造成損害。 圖9 機(jī)械手爪上料運(yùn)動(dòng)軌跡對(duì)比 針對(duì)直角拐彎路徑存在的缺點(diǎn),將直角拐彎改成弧線過(guò)渡,規(guī)劃后的軌跡如圖9實(shí)線所示,這樣可以提高系統(tǒng)的工作效率,并且使速度變化平穩(wěn),提高了系統(tǒng)的穩(wěn)定性。假設(shè)AB長(zhǎng)度為S1,A點(diǎn)到B點(diǎn)的時(shí)間為T1;BC長(zhǎng)度為S2,B點(diǎn)到C點(diǎn)的時(shí)間為T2;CD長(zhǎng)度為S3,C點(diǎn)到D點(diǎn)的時(shí)間為T3;DE長(zhǎng)度為S4,D點(diǎn)到E點(diǎn)的時(shí)間為T4。已知在實(shí)際的上料過(guò)程中,S1 ①t=0時(shí)刻為上升段S1的初始時(shí)刻。 桁架式數(shù)控機(jī)床上下料機(jī)器人的機(jī)械手爪移動(dòng)分為橫向運(yùn)動(dòng)和縱向運(yùn)動(dòng)兩個(gè)分運(yùn)動(dòng),對(duì)于各分運(yùn)動(dòng),分別采用梯形控制算法、S型速度曲線控制算法和改進(jìn)后S型速度曲線控制算法對(duì)兩個(gè)方向的機(jī)械手爪運(yùn)動(dòng)進(jìn)行加減速控制,以驗(yàn)證算法的使用效果。 根據(jù)實(shí)際用于雙數(shù)控機(jī)床上下料的桁架式機(jī)器人設(shè)計(jì)參數(shù)設(shè)定仿真過(guò)程中的數(shù)據(jù),其中縱向位移S1=1000 mm,橫向位移S2=2500 mm,縱向位移S3=800 mm,橫向位移S4=600 mm,此外為避免機(jī)械臂受到過(guò)大沖擊,加加速度最大上限不應(yīng)超過(guò)6500 mm/s3。為了驗(yàn)證文中提出的改進(jìn)的S型曲線控制算法的有效性,采用Matlab仿真軟件對(duì)梯形控制算法、傳統(tǒng)S型速度曲線控制算法和改進(jìn)S型曲線控制算法分別對(duì)機(jī)械手爪的位移、速度、加速度、加加速度的運(yùn)動(dòng)進(jìn)行仿真,仿真結(jié)果分別如圖10和圖11所示。 圖10 不同算法橫向、縱向位移與速度仿真結(jié)果 圖11 不同算法橫向、縱向加速度和加加速度仿真結(jié)果 梯形控制算法采用恒定的加速度,是一種較易使用的算法。由圖10可知,相比于兩種S型速度曲線控制算法,梯形控制算法的加減速過(guò)程中會(huì)出現(xiàn)明顯的拐點(diǎn)、系統(tǒng)運(yùn)行不夠平滑,可能對(duì)系統(tǒng)造成沖擊。從圖11可知,在梯形控制算法最大加加速度大于兩種S型速度曲線控制算法的情況下,梯形控制算法完成移動(dòng)使用時(shí)間仍大于兩種S型速度曲線控制算法。由此可以看出,兩種S型速度曲線控制算法運(yùn)行速度的穩(wěn)定性方面要優(yōu)于梯形控制算法。在此基礎(chǔ)上,進(jìn)一步探究?jī)煞NS型速度曲線控制算法的性能。 兩種S型速度曲線控制算法的位移、速度、加速度和加加速度具體參數(shù)如表1所示。從表1中可以看出,橫向最終位移是3100 mm,縱向最終位移是200 mm,改進(jìn)前后最大運(yùn)動(dòng)速度均為614 mm/s。其中,橫向勻速運(yùn)動(dòng)時(shí)間從3.125 s增加為3.135 s,縱向勻速運(yùn)動(dòng)時(shí)間為從1.010 s增加為1.051 s;橫向、縱向變速運(yùn)動(dòng)時(shí)間從3.84 s減小為3.6 s。在完成相同路程的前提下,勻速運(yùn)動(dòng)時(shí)間增加且變速運(yùn)動(dòng)時(shí)間減小,說(shuō)明機(jī)器人可更快完成加減速過(guò)程進(jìn)入平穩(wěn)運(yùn)行狀態(tài)。結(jié)合位移和速度曲線可知,改進(jìn)后運(yùn)動(dòng)軌跡中的位移和速度曲線相比于改進(jìn)前,在最大運(yùn)動(dòng)速度和勻速運(yùn)動(dòng)時(shí)間保持一致的情況下,完成相同的動(dòng)作位移所需時(shí)間要少0.3 s。說(shuō)明改進(jìn)后軌跡的加減速過(guò)程用時(shí)更短,在一定程度上可提升運(yùn)行效率。 表1 兩種S型速度曲線控制算法參數(shù)指標(biāo)表 改進(jìn)前的加加速度為定值3000 mm/s3,改進(jìn)后的加加速度可連續(xù)變化,最大值為6020 mm/s3;改進(jìn)前加速度最大值為1000 mm/s2,改進(jìn)后加速度最大值為1020 mm/s2。對(duì)比加速度、加加速度曲線(如圖12所示),改進(jìn)后的加加速度減少了階躍變化的次數(shù),使加速度曲線可以更加平滑,降低了軟沖擊次數(shù)。改進(jìn)后加速度曲線相比于改進(jìn)前,無(wú)明顯的拐點(diǎn)并減少了不可導(dǎo)點(diǎn)數(shù)量,有效降低了發(fā)生沖擊、振動(dòng)或系統(tǒng)抖動(dòng)的可能性。因此可以在保證系統(tǒng)工作穩(wěn)定性、運(yùn)行效率和控制精度的前提下,提高加加速度的上限,進(jìn)一步減少運(yùn)動(dòng)時(shí)間。 在Matlab仿真基礎(chǔ)上,將改進(jìn)后S型速度曲線控制算法應(yīng)用于新代機(jī)械臂驅(qū)控一體機(jī)SCD-80RA,測(cè)試結(jié)果如表2所示。與Matlab仿真相比,實(shí)際物理驗(yàn)證雖略有差異,但改進(jìn)后控制算法能夠很好地實(shí)現(xiàn)兩臺(tái)數(shù)控機(jī)床系統(tǒng)進(jìn)行自動(dòng)上下料這一任務(wù)要求,并能夠在不影響系統(tǒng)的控制精度、穩(wěn)定性的條件下獲得更大的加加速度,減小加速度變化所需時(shí)間和整體運(yùn)動(dòng)時(shí)間,有效提升了工作效率。 表2 改進(jìn)后S型速度曲線控制算法實(shí)際測(cè)試與仿真性能指標(biāo)表 對(duì)于機(jī)械臂上下料等機(jī)器人應(yīng)用場(chǎng)景,要求末端機(jī)構(gòu)運(yùn)動(dòng)盡可能平穩(wěn),即系統(tǒng)加減速過(guò)程需要具有高度的柔性。傳統(tǒng)的梯形加減速和指數(shù)型加減速算法在運(yùn)動(dòng)過(guò)程中存在突變沖擊,不適于快響應(yīng)系統(tǒng)。七段S型加減速算法雖有一定提升,但有時(shí)還無(wú)法滿足某些系統(tǒng)平穩(wěn)運(yùn)行的要求。為此本文以桁架式上下料機(jī)器人機(jī)械手爪的運(yùn)動(dòng)控制為研究對(duì)象,將易于實(shí)現(xiàn)的多項(xiàng)式曲線融入到S型速度曲線控制算法中,使得改進(jìn)后的S型速度曲線控制算法求解過(guò)程更易于程序?qū)崿F(xiàn),且加速度、加加速度均連續(xù)。通過(guò)對(duì)典型雙機(jī)床桁架式上下料機(jī)器人的軌跡優(yōu)化,將機(jī)械手爪運(yùn)行過(guò)程中的直角拐彎路徑改為在拐角處以弧線方式過(guò)渡的路徑。Matlab仿真實(shí)驗(yàn)分析和物理驗(yàn)證結(jié)果表明,在改進(jìn)后的S型速度曲線控制算法和帶有弧線過(guò)渡路徑的模式下,機(jī)械手爪的位移、速度、加速度可以隨時(shí)間連續(xù)變化且運(yùn)行平穩(wěn),從而達(dá)到縮短上下料過(guò)程中機(jī)械手的工作時(shí)間、提高系統(tǒng)運(yùn)行效率和穩(wěn)定性的目的。2 改進(jìn)后S型速度曲線控制算法
3 改進(jìn)后S型速度曲線控制算法的實(shí)現(xiàn)
4 仿真與實(shí)驗(yàn)驗(yàn)證
5 結(jié)束語(yǔ)