基于雙S形速度曲線的混聯(lián)碼垛機(jī)器人軌跡規(guī)劃

張志松

(比特?cái)?shù)控技術(shù)應(yīng)用有限公司，河北 廊坊 065000)

0 引 言

伴隨著“中國(guó)制造2025”概念的提出，國(guó)內(nèi)迎來了工業(yè)機(jī)器人的發(fā)展熱潮[1-2]。搬運(yùn)碼垛機(jī)器人以其高柔性工作能力、占地面積小、高效完成箱包裝物品搬運(yùn)的優(yōu)勢(shì)，在物流倉(cāng)儲(chǔ)領(lǐng)域得到廣泛應(yīng)用[3]。由于機(jī)器人對(duì)搬運(yùn)與碼垛的工作效率要求比較高，在工作過程中頻繁地快速啟動(dòng)、停止，對(duì)機(jī)械結(jié)構(gòu)的沖擊和磨損很嚴(yán)重。因此，為了實(shí)現(xiàn)搬運(yùn)機(jī)器人在碼垛過程中快速、平穩(wěn)的運(yùn)動(dòng)控制，這就需要研究出關(guān)于時(shí)間和空間比較復(fù)雜的位置、速度、加速度函數(shù)，以滿足搬運(yùn)碼垛機(jī)器人的控制要求。

機(jī)器人軌跡規(guī)劃描述了操作臂在笛卡爾空間和關(guān)節(jié)空間的運(yùn)動(dòng)曲線，其遵循的一個(gè)原則就是運(yùn)動(dòng)過程中避免速度及加速度突變，保持軌跡平穩(wěn)平滑[4]。目前機(jī)器人關(guān)節(jié)空間軌跡規(guī)劃方法大都用的是高階多項(xiàng)式插值[5-7]、樣條插值等方法[8-10]。

本文將通過對(duì)現(xiàn)有的軌跡曲線進(jìn)行分析，設(shè)計(jì)出性能優(yōu)越、符合要求的軌跡曲線，從而實(shí)現(xiàn)搬運(yùn)碼垛機(jī)器人高精度的實(shí)時(shí)運(yùn)動(dòng)控制。

1 碼垛機(jī)器人運(yùn)動(dòng)學(xué)分析

根據(jù)碼垛機(jī)器人實(shí)際工作需求，筆者在遵循機(jī)器人構(gòu)型選擇的基礎(chǔ)上，設(shè)計(jì)了串—并混聯(lián)式圓柱坐標(biāo)構(gòu)型碼垛機(jī)器人，其構(gòu)型如圖1所示。

圖1 混聯(lián)式碼垛機(jī)器人1—底座；2—腰座；3—水平直線單元；4—豎直直線單元；5—大臂連桿；6—后大臂；7—前大臂；8—轉(zhuǎn)動(dòng)鉸鏈；9—小臂；10—腕部

該碼垛機(jī)器人主要由腰部、后大臂、前大臂和腕部4部分組成，其中腰部關(guān)節(jié)實(shí)現(xiàn)底座1與腰座2的相對(duì)轉(zhuǎn)動(dòng)，水平直線單元3與豎直直線單元4分別驅(qū)動(dòng)后大臂6，前大臂7實(shí)現(xiàn)機(jī)器人腕部的水平和豎直方向運(yùn)動(dòng)，腕部關(guān)節(jié)實(shí)現(xiàn)腕部10與末端執(zhí)行器的相對(duì)轉(zhuǎn)動(dòng)，同時(shí)利用A1-B1-B2-B3以及B3-D3-D4-B4兩組輔助平行四邊形機(jī)構(gòu)使機(jī)器人末端始終保持水平，可以實(shí)現(xiàn)對(duì)末端工具豎直位移和水平位移的解耦控制。這種構(gòu)型一方面保留了圓柱坐標(biāo)型的運(yùn)動(dòng)直觀、結(jié)構(gòu)較為簡(jiǎn)單、動(dòng)作范圍大且在動(dòng)作范圍內(nèi)無奇異點(diǎn)、控制計(jì)算量小、運(yùn)行速度快等諸多優(yōu)點(diǎn)；另一方面利用了混聯(lián)機(jī)構(gòu)所綜合的串聯(lián)機(jī)構(gòu)較大的工作空間和并聯(lián)機(jī)構(gòu)較大的結(jié)構(gòu)剛度等優(yōu)點(diǎn)，在提高機(jī)器人的作業(yè)能力、節(jié)約制造成本等方面具有明顯的優(yōu)勢(shì)。

1.1 碼垛機(jī)器人正運(yùn)動(dòng)學(xué)分析

1.1.1 平行四邊形機(jī)構(gòu)運(yùn)動(dòng)學(xué)分析

(1)

當(dāng)兩直線單元分別由零位運(yùn)動(dòng)到圖1中某一位姿時(shí)(實(shí)線表示)，鉸接點(diǎn)A1，A2，B1，B2，B3在坐標(biāo)系X2O2Z2中的齊次坐標(biāo)可分別表示為：

(2)

(3)

從而可得臂部平行四邊形機(jī)構(gòu)的運(yùn)動(dòng)學(xué)關(guān)系為：

(4)

當(dāng)連桿A1B1，A2B1，B1B2，B3B4的長(zhǎng)度b,a,l1,l2滿足一定幾何約束時(shí)，式(4)可通過如下過程進(jìn)行求解。在圖1中，根據(jù)幾何關(guān)系，進(jìn)一步可得：

(5)

從而可推出：

(6)

當(dāng)l1/b=l2/a=j(j為常數(shù))時(shí)，則式(6)可表示為：

(7)

將式(7)代入式(4)，可得：

(8)

式(8)即為碼垛機(jī)器人平行四邊形機(jī)構(gòu)的運(yùn)動(dòng)學(xué)關(guān)系式。由于zB4與zA2、xB4與xA1均為線性關(guān)系，小臂末端鉸接點(diǎn)B4的豎直位移和水平位移在垂直面內(nèi)實(shí)現(xiàn)了運(yùn)動(dòng)解耦。

1.1.2 碼垛機(jī)器人正運(yùn)動(dòng)學(xué)方程

筆者建立碼垛機(jī)器人的D-H坐標(biāo)系，對(duì)應(yīng)的D-H參數(shù)如表1所示。

表1 碼垛機(jī)器人D-H參數(shù)

則相鄰坐標(biāo)系的變換矩陣分別為：

本研究將以上5個(gè)矩陣相乘，得到碼垛機(jī)器人正運(yùn)動(dòng)學(xué)方程：

(9)

由式(9)可看出，碼垛機(jī)器人末端工具的位置是由臂部平行四邊形機(jī)構(gòu)關(guān)節(jié)變量xA1和zA2及底座的旋轉(zhuǎn)關(guān)節(jié)變量θ0決定的，姿態(tài)由底座和腕部的旋轉(zhuǎn)關(guān)節(jié)變量θ0和θ4決定，因此該構(gòu)型的機(jī)器人相對(duì)通用關(guān)節(jié)式機(jī)器人更易于控制。

1.2 碼垛機(jī)器人逆運(yùn)動(dòng)學(xué)分析

為了對(duì)機(jī)器人進(jìn)行運(yùn)動(dòng)控制和軌跡規(guī)劃，需求解機(jī)器人的逆運(yùn)動(dòng)學(xué)方程。當(dāng)機(jī)器人的連桿長(zhǎng)度滿足式(8)時(shí)，結(jié)合式(9)，有如下關(guān)系：

(10)

若機(jī)器人末端坐標(biāo)系X5O5Z5相對(duì)于基坐標(biāo)系X0O0Z0的轉(zhuǎn)角(繞Z0軸)為β，則有：

(11)

根據(jù)式(9)可知：θ0+θ4=β，則

θ4=β-θ0

(12)

式(11)和式(12)即為該混聯(lián)機(jī)器人的逆運(yùn)動(dòng)學(xué)方程。

2 碼垛機(jī)器人軌跡規(guī)劃

由于碼垛機(jī)器人在高速運(yùn)行過程中頻繁地快速啟動(dòng)與停止，其加減速控制極為重要，這就要求加速度曲線首尾兩端的光滑度要好，避免加速度突變。因此，只有采用合適的運(yùn)動(dòng)軌跡曲線才能同時(shí)保證運(yùn)動(dòng)速度的高效和減小機(jī)械本體所受到?jīng)_擊。在通過逆運(yùn)動(dòng)學(xué)計(jì)算獲取碼垛機(jī)器人各關(guān)節(jié)位置相對(duì)時(shí)間的序列值之后，本研究采用雙S形速度曲線進(jìn)行碼垛機(jī)器人軌跡曲線的規(guī)劃。

軌跡的速度曲線兩端是由拋物線連接的，形狀類似英文字母“S”，所以稱之為雙S形速度曲線。又由于該軌跡由7段不同的常量Jerk曲線組成，筆者將其稱之為7段軌跡。

典型軌跡如圖2所示。

圖2 雙S形速度軌跡的位置、速度、加速度和Jerk曲線

本研究以碼垛機(jī)器人某一個(gè)關(guān)節(jié)角位置信息為研究對(duì)象，以函數(shù)qi(t)表示軌跡曲線，即：

q=qi(t),t∈[t0,tf]

(13)

為了獲得一條確定的光滑運(yùn)動(dòng)曲線，假設(shè)qf>q0，且t0=0，設(shè)軌跡曲線的邊界條件為：

圖2中的Tf1，Tj2為Jerk不為零(變加速)的持續(xù)時(shí)間，Tv為勻速運(yùn)動(dòng)時(shí)間。速度、加速度、Jerk滿足vmin=-vmax、amin=-amax、jmin=-jmax，即運(yùn)動(dòng)約束條件關(guān)于原點(diǎn)對(duì)稱。將雙S形速度軌跡分成如下3個(gè)運(yùn)動(dòng)階段。

第一階段為加速階段：t∈[0,Ta]，加速度從零線性增加到最大值，然后線性減小到零。

在t∈[0,Tj1]階段：

(14)

在t∈[Tj1,Ta-Tj1]階段：

(15)

在t∈[Tj1,Ta-Tj1]階段：

(16)

第二階段為最大速度運(yùn)行階段：t∈[Ta,Ta+Tv]，該階段軌跡的速度為常量。

在t∈[Ta,Ta+Tv]階段：

(17)

第三階段為減速階段：t∈[Ta+Tv,T]，此階段的加速度的變化規(guī)律和第一階段中加速度的變化規(guī)律相同，方向相反。

在t∈[T-Td,T-Td+Tj2]階段：

(18)

在t∈[T-Td+Tj2,Ta-Tj2]階段：

(19)

在t∈[T-Tj2,T]階段：

(20)

3 仿真與分析

為了驗(yàn)證雙S形速度曲線對(duì)碼垛機(jī)器人關(guān)節(jié)空間進(jìn)行軌跡規(guī)劃的有效性，本研究給定軌跡的邊界條件為：q0=0，q1=10和v0=1，v1=0，約束條件為：vmax=10，amax=10和jmax=30。

對(duì)應(yīng)的關(guān)節(jié)運(yùn)動(dòng)仿真曲線如圖3所示。

圖3 雙S形速度軌跡的仿真曲線

從圖3(a～d)的仿真曲線可以看出：給定碼垛機(jī)器人起始點(diǎn)和終止點(diǎn)的關(guān)節(jié)角度值，機(jī)器人關(guān)節(jié)軸在運(yùn)行中滿足位移、速度、加速度、Jerk連續(xù)性的約束條件，并且軌跡曲線首尾兩端的平滑性很好，避免了機(jī)器人在高速運(yùn)行過程中因頻繁快速地啟動(dòng)與停止對(duì)機(jī)械系統(tǒng)產(chǎn)生的沖擊、磨損問題，實(shí)現(xiàn)了機(jī)器人系統(tǒng)高速平穩(wěn)的運(yùn)動(dòng)到指定位置的控制要求。

4 結(jié)束語(yǔ)

結(jié)合碼垛機(jī)器人實(shí)際工作需求，本文設(shè)計(jì)了串—并混聯(lián)式圓柱坐標(biāo)構(gòu)型的碼垛機(jī)器人，該構(gòu)型機(jī)器人在提高機(jī)器人的碼垛能力等方面具有明顯的優(yōu)勢(shì)。

本研究推導(dǎo)了混聯(lián)式圓柱坐標(biāo)型碼垛機(jī)器人正、逆運(yùn)動(dòng)學(xué)，引入雙S型速度曲線完成了該機(jī)器人關(guān)節(jié)空間的軌跡規(guī)劃。Matlab仿真表明：該軌跡規(guī)劃方法保證了機(jī)器人關(guān)節(jié)位移、速度、加速度、Jerk曲線的連續(xù)光滑，能夠減小其在加減速運(yùn)行過程中的沖擊，提高整個(gè)機(jī)器人系統(tǒng)的穩(wěn)定性和可靠性。

[1] 工信部裝備工業(yè)司.《中國(guó)制造2025》推動(dòng)機(jī)器人發(fā)展[J].機(jī)器人技術(shù)與應(yīng)用，2015(3):31-33.

[2] 劉 勇，張 亮.發(fā)展智能制造，促進(jìn)兵器裝備集團(tuán)制造工業(yè)轉(zhuǎn)型升級(jí)——兵器裝備集團(tuán)智能制造技術(shù)與高端裝備產(chǎn)業(yè)發(fā)展思考[J].兵工自動(dòng)化，2017，36(1)：1-6.

[3] 孫 浩.碼垛機(jī)器人的設(shè)計(jì)與研究[D].濟(jì)南：山東理工大學(xué)機(jī)械工程學(xué)院，2013.

[4] 蔡自興.機(jī)器人學(xué)[M].北京:清華大學(xué)出版社，2000.

[5] 張道德，徐 波.機(jī)器人多段插值軌跡規(guī)劃研究與分析[J].湖北工業(yè)大學(xué)學(xué)報(bào)，2016，31(5):1-3.

[6] 涂 孔，張 華.變插值周期五次多項(xiàng)式機(jī)器人關(guān)節(jié)空間規(guī)劃[J].浙江理工大學(xué)學(xué)報(bào)：自然科學(xué)版，2017，37(3)：33-37.

[7] 郭瑞峰，彭光宇，楊 柳，等.基于MDH模型的新型混聯(lián)碼垛機(jī)器人運(yùn)動(dòng)學(xué)分析與仿真[J].機(jī)械傳動(dòng)，2017(2):122-127.

[8] 李亮玉，龍 洋，王天琪，等.基于有理B樣條曲線的焊接機(jī)器人操作空間軌跡規(guī)劃[J].天津工業(yè)大學(xué)學(xué)報(bào)，2015，34(6):80-83.

[9] 趙 皓，劉滿祿，張 華.基于三次非均勻B樣條的6DOF機(jī)械臂軌跡規(guī)劃[J].機(jī)器人技術(shù)與應(yīng)用，2016(6):43-48

[10] 趙 春，甘 戈，蔣哲文.關(guān)節(jié)一體化機(jī)器人運(yùn)動(dòng)控制與B樣條曲線[J].機(jī)械設(shè)計(jì)與制造，2017(1):258-261.