四自由度關(guān)節(jié)機(jī)器人碼垛運(yùn)動(dòng)分析與仿真

戴偉 陳峰 周根榮

摘要

以物品碼垛應(yīng)用為背景，設(shè)計(jì)了一種四自由度的關(guān)節(jié)機(jī)器人系統(tǒng)。運(yùn)用正運(yùn)動(dòng)學(xué)及逆運(yùn)動(dòng)學(xué)方法對(duì)碼垛動(dòng)作過程進(jìn)行分析，獲取運(yùn)動(dòng)參數(shù)表達(dá)形式。在此基礎(chǔ)上，利用MATLAB平臺(tái)，按照物理系統(tǒng)的結(jié)構(gòu)參數(shù)，用D-H方法構(gòu)造相應(yīng)的虛擬關(guān)節(jié)機(jī)器人系統(tǒng)。并采用Rotics TooIBox對(duì)其進(jìn)行最短運(yùn)動(dòng)軌跡仿真，得到各關(guān)節(jié)的狀態(tài)參數(shù)與時(shí)間關(guān)系曲線，為后續(xù)實(shí)體關(guān)節(jié)機(jī)器人的控制系統(tǒng)設(shè)計(jì)、動(dòng)力學(xué)分析以及軌跡規(guī)劃等提供理論基礎(chǔ)。

【關(guān)鍵詞】關(guān)節(jié)機(jī)器人 碼垛 四自由度

1 前言

常見的工業(yè)機(jī)器人按照坐標(biāo)可以分成極坐標(biāo)型、圓柱坐標(biāo)型、直角坐標(biāo)型、關(guān)節(jié)坐標(biāo)型等。其中關(guān)節(jié)坐標(biāo)型雖然具有運(yùn)動(dòng)耦合性強(qiáng)，控制較復(fù)雜的特點(diǎn)，但因其運(yùn)動(dòng)靈活性最好，自身占據(jù)空間小，作為一種通用平臺(tái)獲得廣泛應(yīng)用。

關(guān)節(jié)型機(jī)器人與人的手臂非常類似，主要由底座、大臂和小臂三部分構(gòu)成，大臂、小臂在通過底座的垂平面內(nèi)運(yùn)動(dòng)。連接大臂和底座間的關(guān)節(jié)一般稱作肩關(guān)節(jié);連接大、小臂的關(guān)節(jié)稱肘關(guān)節(jié)。要實(shí)現(xiàn)水平面上的旋轉(zhuǎn)運(yùn)動(dòng)，既可由肩關(guān)節(jié)完成，也可繞底座旋轉(zhuǎn)實(shí)現(xiàn)。其通常按照給定程序、軌跡及要求模擬人手動(dòng)作來實(shí)現(xiàn)抓取和搬運(yùn)等特定功能，常應(yīng)用于物品自動(dòng)碼垛場合。

2 關(guān)節(jié)機(jī)器人結(jié)構(gòu)及運(yùn)動(dòng)特性

設(shè)計(jì)的關(guān)節(jié)機(jī)器人結(jié)構(gòu)如圖1所示。其由橫向移動(dòng)的大臂（x軸）、左右旋轉(zhuǎn)的腰部（y軸）、上下移動(dòng)的小臂（z軸）與可用作夾持器具的腕部（w軸）四個(gè)自由度組成。

其中腰部高245mm，且繞腰部關(guān)節(jié)軸心做3600旋轉(zhuǎn)運(yùn)動(dòng);大臂總長480mm，垂直于腰部軸心，利用電機(jī)經(jīng)絲桿傳動(dòng)做橫向運(yùn)動(dòng)，伸縮行程為260mm;小臂臂長為250mm，由小臂電機(jī)經(jīng)連桿驅(qū)動(dòng)，繞大臂一端做旋轉(zhuǎn)運(yùn)動(dòng);腕部由電機(jī)驅(qū)動(dòng)實(shí)現(xiàn)360度自由旋轉(zhuǎn)，其上安裝不同類型的手抓，可以實(shí)現(xiàn)抓取、夾持、吸附等不同的功能，最大承載重量為1.5kg。機(jī)械臂底座、橫杠后端與腕部有四個(gè)步進(jìn)電機(jī)順序安裝，配以減速器，用以完成四自由度驅(qū)動(dòng)，周身還設(shè)有接近傳感器與限位開關(guān)，接近傳感器可以通過探測(cè)特定位置安裝的突出螺絲，用來檢測(cè)關(guān)節(jié)機(jī)器人運(yùn)動(dòng)位置，提供給控制器進(jìn)行決策。

3 碼垛運(yùn)動(dòng)過程運(yùn)動(dòng)規(guī)劃

在物品自動(dòng)碼垛過程中，碼垛點(diǎn)往往是事先確定好的，關(guān)節(jié)機(jī)器人只需要在運(yùn)動(dòng)過程準(zhǔn)確有序的到達(dá)即可，對(duì)于相鄰點(diǎn)位間的運(yùn)動(dòng)軌跡可以不需要太精確。

按照自動(dòng)碼垛生產(chǎn)要求，碼垛軌跡規(guī)劃如圖2所示：從起始點(diǎn)P開始，當(dāng)需要將物品從P₂搬運(yùn)至Q點(diǎn)時(shí)，為保證抓取端在運(yùn)動(dòng)過程中基本走直線，規(guī)劃沿PP₁→P₁P₂→P₂P₁→P₁P→PQ₂→Q₂Q₁→Q₁Q→QQ₁→Q₁Q₂→P路線運(yùn)動(dòng)。

在其中設(shè)計(jì)了中間點(diǎn)P₁和Q₁。P₁（z軸坐標(biāo)為0）將原來P→P₂路線分成P→P₁、P₁→P₂兩段。在P→P₁段時(shí)，x軸方向電機(jī)后退（A→A₁），以保證運(yùn)動(dòng)軌跡基本垂直，而P₁→P₂段時(shí)，x軸方向電機(jī)前行（A₁→A₂），以保證此時(shí)軌跡基本垂直。中間點(diǎn)Q₁的設(shè)置同理。

如果沒有中間點(diǎn)進(jìn)行規(guī)范，大臂在x軸坐標(biāo)相同的情況下就不會(huì)運(yùn)動(dòng)，腕部末端的軌跡就是一段弧線，相比于按照P→P₂路線所走的弧線，設(shè)置中間點(diǎn)的軌跡所占用的空間更小。在實(shí)際的一些場合中，例如狹窄的空間搬運(yùn)，各關(guān)節(jié)要同時(shí)運(yùn)作，保證占用空間最少，所以這種路線要更為符合實(shí)際要求。

3.1 碼垛點(diǎn)運(yùn)動(dòng)學(xué)算法分析

在機(jī)械臂控制過程中，需要將目標(biāo)點(diǎn)的距離坐標(biāo)轉(zhuǎn)化為脈沖坐標(biāo)然后進(jìn)行軟件編程設(shè)計(jì)，為此進(jìn)行運(yùn)動(dòng)學(xué)分析。

在碼垛系統(tǒng)運(yùn)行前，需要對(duì)碼垛點(diǎn)和抓取點(diǎn)進(jìn)行定位，一般利用示教法來確定點(diǎn)位坐標(biāo)脈沖。但由于碼垛位置往往不止一處，此方法顯得繁瑣、靈活度差。更多采用示教與運(yùn)動(dòng)在線解算相結(jié)合的方式：首先通過示教法確定碼垛的起始點(diǎn)各軸脈沖坐標(biāo);其次通過反運(yùn)動(dòng)學(xué)解算分析各個(gè)軸脈沖坐標(biāo)，轉(zhuǎn)化為距離坐標(biāo)最后，通過疊加新碼垛點(diǎn)與首個(gè)碼垛點(diǎn)的各個(gè)軸距離偏差形成新的距離坐標(biāo)，然后通過正運(yùn)動(dòng)學(xué)求解得出脈沖坐標(biāo)。

3.2 正運(yùn)動(dòng)學(xué)算法分析

即根據(jù)目標(biāo)點(diǎn)D的距離坐標(biāo)（x_d，y_d，z_d）后，解算出相應(yīng)的脈沖坐標(biāo)（x_pls，y_pls，z_pls）。

系統(tǒng)中y軸和z軸是通過腰部電機(jī)和小臂電機(jī)控制做旋轉(zhuǎn)運(yùn)動(dòng)的，但當(dāng)y軸和z軸做旋轉(zhuǎn)運(yùn)動(dòng)時(shí)都會(huì)對(duì)x軸的坐標(biāo)產(chǎn)生影響，所以要計(jì)算關(guān)節(jié)機(jī)器人在x軸行進(jìn)的脈沖坐標(biāo)，就必須要采用分解坐標(biāo)系的方法，把其在x軸的行進(jìn)情況分解到x-z和x-y兩個(gè)坐標(biāo)系中，計(jì)算其在這兩個(gè)坐標(biāo)系的坐標(biāo)x_z和x_y。兩者相加得到x軸坐標(biāo)。為分析方便，將目標(biāo)點(diǎn)D在x-z和x-y的映射點(diǎn)稱為D_xa和D_xy。

3.2.1 x-z坐標(biāo)下運(yùn)動(dòng)分析

如圖3所示，在x-z坐標(biāo)系中，目標(biāo)點(diǎn)D的運(yùn)動(dòng)分解為沿z軸的轉(zhuǎn)向運(yùn)動(dòng)和沿x軸的直線運(yùn)動(dòng)。

（1）z軸旋轉(zhuǎn)運(yùn)動(dòng)分析。

z軸為旋轉(zhuǎn)運(yùn)動(dòng)，設(shè)z軸轉(zhuǎn)動(dòng)的角度為θ1。在圖3.2中，引入計(jì)算目標(biāo)點(diǎn)B_xz以簡化計(jì)算過程。將腕部電機(jī)及和夾具整體視作一個(gè)固定大小矩形，根據(jù)工藝其始終與平臺(tái)底面平行，因此計(jì)算的實(shí)際目標(biāo)點(diǎn)D_xz到A點(diǎn)的距離并非一個(gè)固定值，會(huì)隨轉(zhuǎn)動(dòng)角度變化而變化。引入計(jì)算目標(biāo)點(diǎn)B'_xz后，B'_xz點(diǎn)與A點(diǎn)之間的距離就是固定值且等于小臂長度R_oz_e是引入的計(jì)算目標(biāo)點(diǎn)B_xz的z軸坐標(biāo)，根據(jù)上述分析：

由引入的計(jì)算目標(biāo)點(diǎn)B'_xz的坐標(biāo)可以得到實(shí)際目標(biāo)點(diǎn)D_xz的坐標(biāo)：

R_x和R_z是R在坐標(biāo)系上的投影，為可以測(cè)量的固定值。

由上式（1）、（2）可以得到：

（2）x軸直線運(yùn)動(dòng)分析。

設(shè)x軸的運(yùn)動(dòng)在x-z坐標(biāo)系中行進(jìn)距離為x_z，由圖3.2可得：

3.2.2 x-y坐標(biāo)系下運(yùn)動(dòng)分析

在x-y坐標(biāo)系中，目標(biāo)點(diǎn)D的運(yùn)動(dòng)類似分解為沿y軸的旋轉(zhuǎn)運(yùn)動(dòng)和沿x軸的直線運(yùn)動(dòng)。

（1）y軸的旋轉(zhuǎn)運(yùn)動(dòng)。

關(guān)節(jié)機(jī)器人在y軸方向做旋轉(zhuǎn)運(yùn)動(dòng)，設(shè)腰部電機(jī)旋轉(zhuǎn)θ₂角度后，到達(dá)新的實(shí)際目標(biāo)點(diǎn)D_xy，由圖4可知：

（2）x軸直線的運(yùn)動(dòng)。

在x-y坐標(biāo)系中，由于腰部電機(jī)旋轉(zhuǎn)θ₂角度后，目標(biāo)點(diǎn)在y軸的坐標(biāo)會(huì)發(fā)生變化，所以為了能夠滿足實(shí)際目標(biāo)點(diǎn)D_xy在x軸的坐標(biāo)不變，x軸方向要前行。設(shè)目標(biāo)點(diǎn)D沿x軸方向再次前行x_y，直到目標(biāo)點(diǎn)從位置D_xz行進(jìn)到C點(diǎn)，由圖4可知：

其中R_xy為測(cè)量原點(diǎn)到腰部軸心的距離。

3.2.3 目標(biāo)點(diǎn)D的脈沖坐標(biāo)

根據(jù)上述分析，得到沿x軸方向總行進(jìn)距離x_dis：

x_dis=x_z+x_y（7）

則目標(biāo)點(diǎn)D的脈沖坐標(biāo)（x_pls，y_pls，z_pls）為：

其中x_div、y_div分別為x軸、y軸電機(jī)行進(jìn)分度值（pls/mm，每毫米對(duì)應(yīng)脈沖數(shù)），z_div為z軸電機(jī)行進(jìn)的分度值CPUs/rad，每弧度對(duì)應(yīng)脈沖數(shù)）。

3.3 逆運(yùn)動(dòng)學(xué)算法分析

逆運(yùn)動(dòng)學(xué)求解方法與正運(yùn)動(dòng)學(xué)求解方法相反，是根據(jù)已知的目標(biāo)點(diǎn)位D的脈沖坐標(biāo)（x_pls，y_pls，z_pls），求解相應(yīng)的距離坐標(biāo)（x_d，y_d，z_d）。

3.3.1 在z軸的距離坐標(biāo)z_d

由目標(biāo)點(diǎn)位D在z軸轉(zhuǎn)動(dòng)角度：

得到：

3.3.2 在x軸的距離坐標(biāo)x_d

由目標(biāo)點(diǎn)位D在y軸轉(zhuǎn)動(dòng)的角度：

由圖4可知：

將上式（14）代入式（6），可得：

得出x軸行進(jìn)總距離：

化簡之后可以求出：

3.3.3 目標(biāo)點(diǎn)在y軸的距離坐標(biāo)y_d

將式（17）代入到式（14）中可得：

4 基于MATLAB的運(yùn)動(dòng)仿真

在Matlab環(huán)境下結(jié)合Robotics Toolbox，可以模擬關(guān)節(jié)機(jī)器人的運(yùn)動(dòng)過程，規(guī)劃運(yùn)動(dòng)軌跡，反映各關(guān)節(jié)的位置、速度和加速度變化，為其控制算法設(shè)計(jì)提供參考依據(jù)。

4.1 關(guān)節(jié)坐標(biāo)系建立

關(guān)節(jié)機(jī)器人常由一系列連桿和關(guān)節(jié)任意連接而成，關(guān)節(jié)運(yùn)動(dòng)可為滑動(dòng)（線性）或旋轉(zhuǎn) （轉(zhuǎn)動(dòng)），位置關(guān)系上可處在不同平面并允許轉(zhuǎn)軸間存在偏差。連桿長度可以是任意的（包括零），可被扭曲或彎曲。因此利用任何一組關(guān)節(jié)和連桿都可以構(gòu)成機(jī)器人系統(tǒng)。對(duì)這樣的系統(tǒng)進(jìn)行結(jié)構(gòu)描述，表示每個(gè)關(guān)節(jié)與連桿的關(guān)系，首先就需要建立相應(yīng)的坐標(biāo)系。

Denavit和Hartenberg 1955年提出了一種通用方法，用D-H參數(shù)描述多連桿結(jié)構(gòu)中連桿之間的關(guān)系。其在機(jī)器人系統(tǒng)的每根連桿上都固定一個(gè)坐標(biāo)系，然后用一個(gè)4階齊次變換矩陣表示相鄰兩連桿空間轉(zhuǎn)換關(guān)系。通過依次遞推，最終得到末端執(zhí)行器相對(duì)于基坐標(biāo)系的位姿變換矩陣，建立出機(jī)器人的運(yùn)動(dòng)學(xué)方程。

通過對(duì)基座和連桿進(jìn)行編號(hào)，按照下面的規(guī)則建立連桿連接的坐標(biāo)系，

（1）沿關(guān)節(jié)i+1的軸的方向選定軸Z_i。

（2）原點(diǎn)O_i定位于軸Z_i和軸Z_i-1公垂線的交點(diǎn)。

（3）沿軸Z_i和Z_i-1的公垂線方向選擇X_i，方向由關(guān)節(jié)i指向關(guān)節(jié)i-1。

（4）按照右手定則確定Y_i的方向。

相應(yīng)的連桿參數(shù)定義如表1所示。

建立好的坐標(biāo)系如圖5所示。

連桿L₁～L₄分別代表腰部、大臂、小臂、手腕四個(gè)機(jī)構(gòu)的運(yùn)動(dòng)。連桿坐標(biāo)系上獲得相應(yīng)的D-H參數(shù)如表2所示。

4.2 建立關(guān)節(jié)機(jī)器人模型

在D-H參數(shù)表建立好之后，利用RoboticsToolbox中的Link函數(shù)，描述各連桿關(guān)節(jié)關(guān)系，其中形參為前面分析得到的D-H參數(shù);使用SerialLink函數(shù)就可以實(shí)現(xiàn)關(guān)節(jié)之間的連接組合，連接完成的關(guān)節(jié)機(jī)器人如圖6所示。

搭建完關(guān)節(jié)機(jī)器人的模型后，給其提供兩組不同的位姿或者關(guān)節(jié)狀態(tài)，就可以進(jìn)行軌跡規(guī)劃。其中需要注意，系統(tǒng)中設(shè)計(jì)的關(guān)節(jié)機(jī)器人的大臂和小臂運(yùn)動(dòng)范圍不是無限制的，是受到機(jī)械結(jié)構(gòu)約束，經(jīng)測(cè)試可知，大臂的可移動(dòng)坐標(biāo)范圍為4～30cm，小臂的旋轉(zhuǎn)范圍為45～135度，需要在仿真中將這一運(yùn)動(dòng)約束納入。

4.3 軌跡規(guī)劃

在Robotics Toolbox中，對(duì)于關(guān)節(jié)機(jī)器人的軌跡規(guī)劃，可以采用兩種方式。一種是調(diào)用ctraj函數(shù)，根據(jù)已知初始和終止的末端關(guān)節(jié)位姿，采用勻加、減速運(yùn)動(dòng)來規(guī)劃軌跡，在笛卡兒空間下表現(xiàn)出關(guān)節(jié)最末端的運(yùn)動(dòng)軌跡。第二種是調(diào)用jtraj函數(shù)，在已知初始和終止的關(guān)節(jié)角度下，利用五次多項(xiàng)式來規(guī)劃軌跡，適用于關(guān)節(jié)空間下的軌跡規(guī)劃，輸出的是各個(gè)關(guān)節(jié)的運(yùn)動(dòng)參數(shù)，可用于各個(gè)關(guān)節(jié)驅(qū)動(dòng)。

將構(gòu)建的關(guān)節(jié)機(jī)器人系統(tǒng)，置于一個(gè)200*200*200cm的空間中，底座所處位置為笛卡爾坐標(biāo)原點(diǎn)。利用Robotics Toolbox，對(duì)于給定的初始點(diǎn)和終止點(diǎn)，進(jìn)行手爪安裝端最短運(yùn)動(dòng)軌跡規(guī)劃。

4.3 仿真結(jié)果分析

設(shè)置各個(gè)關(guān)節(jié)的初始位置qz=[04pi/40]，終止位置qr=[pi/430pi/20]。手爪安裝端關(guān)節(jié)結(jié)結(jié)算出相應(yīng)的齊次變換矩陣為：

運(yùn)行仿真獲得如圖7所示的四個(gè)關(guān)節(jié)空間運(yùn)動(dòng)仿真圖。

第1列上圖是關(guān)節(jié)機(jī)器人的運(yùn)行過程動(dòng)態(tài)演示，下圖是其末端關(guān)節(jié)（手爪安裝端）在笛卡爾空間下的軌跡，顯示為一條直線。

第2～4列為為四個(gè)關(guān)節(jié)的空間規(guī)劃圖，分別顯示其位置（角位移）、速度（角速度）和加速度（角加速度）在關(guān)節(jié)運(yùn)動(dòng)過程中的相應(yīng)變化。相應(yīng)的運(yùn)動(dòng)曲線可以為設(shè)計(jì)各關(guān)節(jié)驅(qū)動(dòng)電機(jī)的控制參數(shù)提供參考。其中關(guān)節(jié)4主要用于安裝手爪，因在當(dāng)前碼垛工藝中沒有考慮手爪運(yùn)動(dòng)，因此該關(guān)節(jié)在整個(gè)過程實(shí)際并不運(yùn)動(dòng)。

根據(jù)圖2中碼垛的路線規(guī)劃要求，從位置P→P2、P2→Q2、Q2→Q均需要保證為直線運(yùn)動(dòng)。因此只要將P、P2、Q2、Q作為各段起始和終止位姿，測(cè)量相應(yīng)參數(shù)，通過仿真就可以獲得整個(gè)過程各關(guān)節(jié)的運(yùn)動(dòng)曲線。

通過觀察運(yùn)動(dòng)仿真過程還可以發(fā)現(xiàn)，笛卡爾軌跡與軌跡規(guī)劃結(jié)果與關(guān)節(jié)空間規(guī)劃是相對(duì)應(yīng)的，通過規(guī)劃各關(guān)節(jié)協(xié)同動(dòng)作，實(shí)現(xiàn)了自主堆垛運(yùn)行所要求的最短路徑，滿足了設(shè)計(jì)要求。

5 結(jié)論

以物品碼垛應(yīng)用為背景，設(shè)計(jì)了一種四自由度的關(guān)節(jié)機(jī)器人系統(tǒng)，建立了基于D-H法的關(guān)節(jié)機(jī)器人模型。結(jié)合碼垛工藝，利用正、逆運(yùn)動(dòng)學(xué)方法分析自主碼垛動(dòng)作過程，得到關(guān)節(jié)坐標(biāo)系下和笛卡爾坐標(biāo)系下的轉(zhuǎn)換關(guān)系。在MATLAB中，運(yùn)用Rotics ToolBox構(gòu)建仿真模型，研究了在最短運(yùn)動(dòng)軌跡下，各關(guān)節(jié)運(yùn)動(dòng)參數(shù)曲線，為實(shí)體機(jī)器人的運(yùn)動(dòng)控制、軌跡規(guī)劃提供了重要依據(jù)。

參考文獻(xiàn)

[1]吳振彪，王正家，熊有倫.工業(yè)機(jī)器人（第二版）[M].武漢：華中科技大學(xué)出版社，2006.

[2]孫樹棟.工業(yè)機(jī)器人技術(shù)基A [M].西安：西北工業(yè)大學(xué)出版社，2007.

[3]葉辰雷，劉曉平，負(fù)超.碼垛機(jī)器人的軌跡規(guī)劃與仿真分析[ J].機(jī)械研究與應(yīng)用，2013，26（05）：26-30.

[4]周根榮，姜平，鐘永彥.碼垛機(jī)器人運(yùn)動(dòng)學(xué)分析和控制系統(tǒng)設(shè)計(jì)[J].計(jì)算機(jī)測(cè)量與控制，2016（1）：81-84.

[5]Swaraj Zodey，Sharad K.Pradhan.Matlab Toolbox for KinematicAnalysis and Simulation of DexterousRobotic Grippers[J].ProcediaEngineering，2014，97.

[6]Singh Amanpreet，Singla Ekta，SoniSanjeev，Singla Ashish.Kinematicmodeling of a 7-degree of freedomspatial hybrid manipulator formedical surgery.[J].Proceedingsof the Institution of MechanicalEngineers.Part H，Journal ofengineering in medicine，2018，232（1）.

[7]任崇軒.五自由度機(jī)械臂運(yùn)動(dòng)和控制仿真分析[D].華南理工大學(xué)，2012.