工業(yè)機(jī)器人運(yùn)動(dòng)學(xué)與軌跡規(guī)劃仿真

唐新星，王 平，范大川

(長春工業(yè)大學(xué) 機(jī)電工程學(xué)院，吉林 長春 130012)

0 前言

“中國制造2025”提出后，制造業(yè)普遍需要技術(shù)和設(shè)備升級改造，以增強(qiáng)競爭力，提高經(jīng)濟(jì)效益，工業(yè)機(jī)器人作為先進(jìn)制造業(yè)中的重要裝備和手段，具有極大的發(fā)展空間[1-3]。在三維空間內(nèi)對工業(yè)機(jī)器人進(jìn)行軌跡和速度規(guī)劃，可進(jìn)一步提高生產(chǎn)率、提高產(chǎn)品質(zhì)量、降低勞動(dòng)強(qiáng)度、改善勞動(dòng)條件，在實(shí)際工作中具有重要的現(xiàn)實(shí)意義。

工業(yè)機(jī)器人軌跡規(guī)劃以保證其運(yùn)動(dòng)軌跡精度和穩(wěn)定性為前提，各關(guān)節(jié)位移、速度、加速度曲線光滑連續(xù)無突變，減少機(jī)械本體的磨損和振動(dòng)沖擊，提高作業(yè)效率[4]。文獻(xiàn)[5]以M-10iA工業(yè)機(jī)器人為研究對象，選用D-H法則建立機(jī)器人數(shù)學(xué)模型，運(yùn)動(dòng)ADAMS軟件進(jìn)行仿真，研究連桿末端點(diǎn)相對于坐標(biāo)原點(diǎn)的位置變化；文獻(xiàn)[6]以六自由度機(jī)器人為例，運(yùn)用MATLAB對機(jī)器人關(guān)節(jié)運(yùn)動(dòng)狀態(tài)仿真分析；文獻(xiàn)[7]將三次均勻B樣條應(yīng)用于工業(yè)機(jī)器人軌跡規(guī)劃，但該軌跡方法不易得到平滑的加速度曲線，且不合適于空間任意分布的示教點(diǎn)；文獻(xiàn)[8] 運(yùn)用七次樣條曲線進(jìn)行軌跡規(guī)劃，得到了光滑的速度和加速度曲線，但高次插值易發(fā)生“龍格”現(xiàn)象。

針對以上問題，本文根據(jù)自行研發(fā)的六自由度關(guān)節(jié)型工業(yè)機(jī)器人，利用SolidWorks建立工業(yè)機(jī)器人模型，采用五次插值多項(xiàng)式算法，在ADAMS仿真軟件中建立該機(jī)器人的虛擬樣機(jī)，進(jìn)行運(yùn)動(dòng)學(xué)與軌跡規(guī)劃仿真研究。

1 機(jī)器人手臂結(jié)構(gòu)設(shè)計(jì)

運(yùn)用SolidWorks軟件對六自由度關(guān)節(jié)型工業(yè)機(jī)器人進(jìn)行1∶1的三維建模，將各零部件模型進(jìn)行裝配后構(gòu)建虛擬樣機(jī)[9]。所設(shè)計(jì)的六自由度關(guān)節(jié)型工業(yè)機(jī)器人如圖1所示。該機(jī)器人由基座、腰部、大臂、小臂、腕部和手部組成，共有6個(gè)關(guān)節(jié)。各關(guān)節(jié)均為轉(zhuǎn)動(dòng)關(guān)節(jié)，其中關(guān)節(jié)1～5裝有交流伺服電機(jī)，關(guān)節(jié)6裝有步進(jìn)電機(jī)，在機(jī)器人控制系統(tǒng)的作用下，驅(qū)動(dòng)各個(gè)關(guān)節(jié)按照預(yù)定的空間軌跡運(yùn)動(dòng)，從而實(shí)現(xiàn)末端執(zhí)行器的作業(yè)要求。

圖1 機(jī)器人實(shí)物圖

2 機(jī)器人數(shù)學(xué)模型的建立

為了研究機(jī)器人的運(yùn)動(dòng)學(xué)問題，首先要對機(jī)器人建立數(shù)學(xué)模型。

2.1 空間坐標(biāo)系的建立

機(jī)器人共有6個(gè)關(guān)節(jié)，根據(jù)機(jī)器人各關(guān)節(jié)位置與結(jié)構(gòu)關(guān)系可將工業(yè)機(jī)器人空間坐標(biāo)系分為基礎(chǔ)坐標(biāo)系、關(guān)節(jié)坐標(biāo)系、用戶坐標(biāo)系和工具坐標(biāo)系。按照D-H連桿坐標(biāo)系建模方法，分別建立關(guān)節(jié)1～6坐標(biāo)系，建立針對本機(jī)器人的坐標(biāo)系如圖2所示。

圖2 機(jī)器人連桿坐標(biāo)系

由D-H連桿坐標(biāo)系可得到相應(yīng)連桿參數(shù)如表1所示，其中，a1=100 mm，a2=222 mm，d4=253 mm，機(jī)器人末端坐標(biāo)系到工具坐標(biāo)系的距離dT=68 mm。

表1 工業(yè)機(jī)器人連桿參數(shù)

2.2 機(jī)器人運(yùn)動(dòng)學(xué)方程的正解

正向運(yùn)動(dòng)學(xué)主要解決機(jī)器人運(yùn)動(dòng)方程建立及手部位姿的求解，即已知各個(gè)關(guān)節(jié)的變量，求解手部的位姿[10]。根據(jù)所建立的D-H連桿坐標(biāo)系，i-1系與i系間變換關(guān)系可用坐標(biāo)系的平移、旋轉(zhuǎn)來實(shí)現(xiàn)，其變換矩陣如式(1)所示。

Ai=Rot(z,θi)Trans(0,0,di)Trans(ai,0,0)Rot(x,αi)=

(1)

末端執(zhí)行器在連桿坐標(biāo)系中的位姿可通過式(2)所示矩陣T表示。

(2)

式中，nx=-c6[c1s5s23-c5(c1c4c23+s1s4)]-

s6(c1s4c23+s1c4)

ny=-c6[s1s5s23-c5(s1c4c23-c1s4)]-

s6(s1s4c23+c1c4)

nz=s4s6s23-c6(s5c23+c4c5s23)

ox=s6[c1s5s23-c5(s1s4+c1c4c23)]-

c6(c1s4c23-s1c4)

oy=s6[s1s5s23-c5(s1c4c23-c1s4)]-

c6(s1s4c23+c1c4)

oz=s23s4c6+s6(c4c5s23+c23s5)

ax=-c1c5s23-s5(c1c4c23+s1s4)

ay=-s1c5s23-s5(s1c4c23-c1s4)

az=c5c23+c4s5s23

px=c1(a1+a2c2-d4s23)-

dT[c1c5s23+s5(c1c4c23+s1s4)]

py=s1(a1+a2c2-d4s23)-

dT[s1c5s23+s5(s1c4c23-c1s4)]

pz=-a2s2-d4c23+d6(-c5c23+c4s5s23)

s1=sinθ1；s23=sin(θ2+θ3)；c1=cosθ1；c23=cos(θ2+θ3)，以此類推。

2.3 機(jī)器人運(yùn)動(dòng)學(xué)方程的反解

工業(yè)機(jī)器人在空間的軌跡是按照機(jī)器人的作業(yè)任務(wù)來規(guī)劃的，軌跡上的各點(diǎn)分別對應(yīng)他們各自關(guān)節(jié)變量，由計(jì)算得到的關(guān)節(jié)變量q=[θ1,θ2,θ3,θ4,θ5]T去控制工業(yè)機(jī)器人的各個(gè)關(guān)節(jié)運(yùn)動(dòng)，實(shí)現(xiàn)對規(guī)劃軌跡的跟蹤。工業(yè)機(jī)器人末端執(zhí)行器坐標(biāo)系原點(diǎn)相對于基座坐標(biāo)系原點(diǎn)的位置關(guān)系式：

(3)

式(3)可進(jìn)一步化簡為，則有

(4)

對式(4)按照一階泰勒級數(shù)展開，則有

(5)

選擇合適的步長不斷進(jìn)行迭代，由式(5)則可以求得

(6)

在工業(yè)機(jī)器人的實(shí)際運(yùn)用中，為了使得到的關(guān)節(jié)角更加合理，根據(jù)各關(guān)節(jié)之間的關(guān)系，可以應(yīng)用約束：θ1∈[-165°～180°]，θ2∈[-132°～71°]，θ3∈[-123°～123°]，θ4∈[-170°～170°]，θ5∈[-120°～120°]。

3 軌跡規(guī)劃與仿真

機(jī)器人軌跡規(guī)劃是根據(jù)作業(yè)任務(wù)的要求，計(jì)算出預(yù)期的運(yùn)動(dòng)軌跡。為了滿足對機(jī)器人運(yùn)動(dòng)軌跡的更高要求，本文設(shè)定機(jī)器人的初始位置和目標(biāo)位置，根據(jù)五次多項(xiàng)式插值算法求解各個(gè)方程的軌跡方程[11]。

3.1 五次多項(xiàng)式插值算法

五次多項(xiàng)式樣條中，兩節(jié)點(diǎn)之間的曲線段由五次多項(xiàng)式擬合生成，整個(gè)曲線由一段五次多項(xiàng)式組成。五次多項(xiàng)式函數(shù)中共有6個(gè)未知系數(shù)，對其求一階、求二階導(dǎo)，可求出機(jī)器人末端的速度、加速度關(guān)于時(shí)間t的函數(shù)式為

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

(7)

(8)

(9)

其中與起始和終止時(shí)間點(diǎn)t0、tf相對應(yīng)的角位移θ0和θf為已知，且滿足條件：

(10)

在每段多項(xiàng)式起始點(diǎn)和終止點(diǎn)處滿足二階連續(xù)，設(shè)置始末點(diǎn)的速度、加速度且滿足條件：

(11)

每個(gè)點(diǎn)的角位移值為已知量，設(shè)定其角速度、角加速度為任意值，兩點(diǎn)處共可得6個(gè)已知條件，將其代入式(7)、(8)、(9)聯(lián)立方程組可分別解出6個(gè)未知系數(shù)a0～a5。

3.2 軌跡規(guī)劃

關(guān)節(jié)1軌跡方程為

θ1(t)=1.932-1.667t3+1.250t4-0.250t5

(12)

關(guān)節(jié)2軌跡方程為

θ2(t)=0.289+1.252t3-0.939t4+0.188t5

(13)

關(guān)節(jié)3軌跡方程為

θ3(t)=-1.109+2.094t3-1.571t4+0.314t5

(14)

關(guān)節(jié)4軌跡方程為

θ4(t)=0.922+1.690t3-1.268t4+0.254t5

(15)

關(guān)節(jié)5軌跡方程為

θ5(t)=0.444+0.679t3-0.509t4+0.102t5

(16)

關(guān)節(jié)6軌跡方程為

θ6(t)=0.197+2.411t3-1.808t4+0.362t5

(17)

3.3 仿真分析

將建好的SolidWorks模型導(dǎo)入ADAMS中，根據(jù)機(jī)器人構(gòu)件的相對運(yùn)動(dòng)關(guān)系在ADAMS中添加相應(yīng)的約束和運(yùn)動(dòng)副，如圖3所示[12]。檢查機(jī)器人的自由度后，根據(jù)機(jī)器人的運(yùn)動(dòng)軌跡要求，設(shè)定各個(gè)關(guān)節(jié)的軌跡程。

圖3 機(jī)器人虛擬樣機(jī)模型

將上述關(guān)節(jié)軌跡方程分別輸入到對應(yīng)的Motion驅(qū)動(dòng)中，設(shè)置終止時(shí)間是2 s，設(shè)置輸出步數(shù)是200步，設(shè)置仿真類型為運(yùn)動(dòng)學(xué)仿真。

利用ADAMS仿真得到0～2 s內(nèi)各關(guān)節(jié)及末端執(zhí)行器的運(yùn)動(dòng)狀態(tài)，如圖4～ 10所示。按照軌跡規(guī)劃要求，由圖4可以看出關(guān)節(jié)1轉(zhuǎn)動(dòng)了76.41°，關(guān)節(jié)2轉(zhuǎn)動(dòng)了-57.38°，關(guān)節(jié)3轉(zhuǎn)動(dòng)了-96°，關(guān)節(jié)4轉(zhuǎn)動(dòng)了-77.47°，關(guān)節(jié)5轉(zhuǎn)動(dòng)了-31.12°，關(guān)節(jié)6轉(zhuǎn)動(dòng)了-110.52°。由圖5、圖6可以看出機(jī)器人在起始時(shí)間各關(guān)節(jié)角速度、角加速度均為0，其中關(guān)節(jié)1的最大角速度為71.63 rad/s，關(guān)節(jié)2的最大角速度為-33.63 rad/s，關(guān)節(jié)3的最大角速度為-25.44 rad/s，關(guān)節(jié)4的最大角速度為-57.46 rad/s，關(guān)節(jié)5的最大角速度為27.46 rad/s，關(guān)節(jié)6的最大角速度為-65.43 rad/s，在終止時(shí)間機(jī)器人各關(guān)節(jié)角速度、角加速度均為0。末端執(zhí)行器的位移、速度和加速度變化直接影響著工業(yè)機(jī)器人的平穩(wěn)性。圖7～圖9給出了機(jī)器人末端執(zhí)行器的重心對機(jī)器人基坐標(biāo)系分別在x、y、z方向的位移、速度和加速度變化曲線，圖10給出了工業(yè)機(jī)器人末端執(zhí)行器的重心的軌跡曲線。從圖7～10中可以看出，機(jī)器人運(yùn)動(dòng)速度和加速度比較平穩(wěn)，均無突變，從而保證了機(jī)器人工作時(shí)的平穩(wěn)連續(xù)性。

圖4 關(guān)節(jié)角度變化曲線

圖5 關(guān)節(jié)角速度變化曲線

圖6 關(guān)節(jié)角加速度變化曲線

圖7 末端執(zhí)行器位移變化曲線

圖8 末端執(zhí)行器速度變化曲線

圖9 末端加速度變化曲線

圖10 機(jī)器人末端運(yùn)動(dòng)軌跡

4 結(jié)束語

通過Solidworks三維建模軟件構(gòu)造工業(yè)機(jī)器人的虛擬樣機(jī)，將三維模型導(dǎo)入ADAMS多體動(dòng)力學(xué)軟件中，根據(jù)設(shè)定機(jī)器人的初始位置和目標(biāo)位置，經(jīng)運(yùn)動(dòng)學(xué)反解獲得各個(gè)關(guān)節(jié)的運(yùn)動(dòng)參數(shù)，利用五次多項(xiàng)式插值對工業(yè)機(jī)器人運(yùn)動(dòng)軌跡進(jìn)行規(guī)劃仿真，得到工業(yè)機(jī)器人各個(gè)關(guān)節(jié)角度、角速度和角加速度的變化曲線，末端執(zhí)行器的運(yùn)動(dòng)軌跡、位置、速度和加速度變化曲線。仿真結(jié)果表明，機(jī)器人運(yùn)動(dòng)速度和加速度比較平穩(wěn)，加速度無突變，能夠保證工業(yè)機(jī)器人平穩(wěn)連續(xù)運(yùn)行，為進(jìn)一步工業(yè)機(jī)器人實(shí)時(shí)控制提供了重要參考。

參考文獻(xiàn):

[1] 《中國制造2025》推動(dòng)機(jī)器人發(fā)展[J]. 機(jī)器人技術(shù)與應(yīng)用，2015 (03):31-33.

[2] 王田苗，陶永. 我國工業(yè)機(jī)器人技術(shù)現(xiàn)狀與產(chǎn)業(yè)化發(fā)展戰(zhàn)略[J]. 機(jī)械工程學(xué)報(bào)，2014，50(09):1-13.

[3] 駱敏舟，方健，趙江海. 工業(yè)機(jī)器人的技術(shù)發(fā)展及其應(yīng)用[J]. 機(jī)械制造與自動(dòng)化，2015，44(01):1-4.

[4] 樂英，岳艷波.六自由度機(jī)器人運(yùn)動(dòng)學(xué)仿真及軌跡規(guī)劃[J].組合機(jī)床與自動(dòng)化加工技術(shù)，2016(04)：89-92.

[5] 朱華炳，張娟，宋孝炳. 基于ADAMS的工業(yè)機(jī)器人運(yùn)動(dòng)學(xué)分析和仿真[J]. 機(jī)械設(shè)計(jì)與制造，2013(05):204-206.

[6] 王想實(shí)，周薇，高雅. 焊接機(jī)器人控制設(shè)計(jì)與仿真研究[J]. 機(jī)床與液壓，2017 (17):32-35.

[7] 李林峰，馬蕾. 三次均勻B樣條在工業(yè)機(jī)器人軌跡規(guī)劃中的應(yīng)用研究[J]. 科學(xué)技術(shù)與工程，2013，13(13):3621-3625，3646.

[8] 劉松國，朱世強(qiáng)，吳文祥. 具有運(yùn)動(dòng)時(shí)間約束的機(jī)械手最優(yōu)平滑軌跡規(guī)劃[J]. 電機(jī)與控制學(xué)報(bào)，2009，13(06):897-902.

[9] 詹迪維.SolidWorks機(jī)械設(shè)計(jì)教程2012[M].機(jī)械工業(yè)出版社，2012.

[10] 蔡自興.機(jī)器人學(xué)基礎(chǔ)[M],機(jī)械工業(yè)出版社，2015.

[11] 張強(qiáng). 基于UG和ADAMS的五自由度機(jī)器人手臂的空間軌跡優(yōu)化設(shè)計(jì)[D].沈陽：沈陽理工大學(xué)，2016.

[12] 宋少云，尹芳.ADAMS在機(jī)械中的應(yīng)用[M].國防工業(yè)出版社，2015.