周根榮, 姜　平, 鐘永彥

(南通大學(xué) 電氣工程學(xué)院，江蘇 南通　226019)

?

碼垛機器人運動學(xué)分析和控制系統(tǒng)設(shè)計

周根榮, 姜平, 鐘永彥

(南通大學(xué) 電氣工程學(xué)院，江蘇 南通226019)

針對一類關(guān)節(jié)坐標四自由度碼垛機器人，對其機械傳動結(jié)構(gòu)進行分析，構(gòu)建其運動學(xué)坐標體系；根據(jù)碼垛生產(chǎn)特點，運動路徑規(guī)劃采用設(shè)定點位法，其中一些點位的脈沖坐標通過示教法直接確定，其余點位脈沖坐標則通過正、反運動學(xué)求解獲取；為降低運動學(xué)求解復(fù)雜程度，將三維空間坐標體系的求解，先分解到兩個平面坐標中單獨進行，然后再加以合成；控制系統(tǒng)以PLC為核心，各軸驅(qū)動采用高精度交流伺服系統(tǒng)，通過原點回歸確定各軸原點，根據(jù)目標點位實時計算各軸運動脈沖數(shù)和頻率，采用絕對定位驅(qū)動方式實現(xiàn)機器人各軸聯(lián)動；實際運行效果表明，運動學(xué)求解復(fù)雜程度的降低，提高了系統(tǒng)運算速度和實時性，而絕對定位方式可避免相對方式下的累積誤差，從而提高了系統(tǒng)的運動精度。

碼垛機器人；運動學(xué)分析；伺服系統(tǒng)；絕對定位

0　引言

當(dāng)前，伴隨著人力成本上升、裝備產(chǎn)業(yè)升級等因素，具有高度柔性制造能力的工業(yè)機器人已越來越多應(yīng)用在生產(chǎn)過程中。碼垛機器人是一種應(yīng)用在生產(chǎn)、物流自動化領(lǐng)域的工業(yè)機器人，可按照一定模式，實現(xiàn)物料的搬運、存儲和裝卸[1]，本文介紹一種四自由度碼垛機器人的運動學(xué)分析和控制系統(tǒng)設(shè)計。

1　機器人結(jié)構(gòu)及運動軌跡

一類關(guān)節(jié)坐標機器人結(jié)構(gòu)如圖1所示，有腰部、大臂、小臂、腕部四個自由度。大臂電機運動經(jīng)絲桿傳動實現(xiàn)x軸方向前后運動；小臂電機運動經(jīng)連動桿以A為支點實現(xiàn)z軸方向旋轉(zhuǎn)運動；腰部電機則以O(shè)y為軸心實現(xiàn)y軸方向旋轉(zhuǎn)運動；腕部電機通常跟隨腰部電機做反方向同步運動，以保證夾具處于固定不變的角度。機器人原點設(shè)在O點(x軸方向前限位點)，D點為目標點位(夾具中心點)。

圖1　機器人結(jié)構(gòu)簡圖

1.1碼垛路徑規(guī)劃

碼垛機器人一般采用點位控制法，即保證碼垛過程中能準確到達幾個設(shè)定點位，而相鄰點位之間的運動軌跡一般不需要太高精度[2]。一種簡單的碼垛路徑點位示意圖如圖2所示。

機器人開始時和一次碼垛過程結(jié)束后，都處在起始點位P，當(dāng)需要將物品從P2(抓取點)搬運至Q(碼垛點)時，可沿著PP1→P1P2→P2P1→P1P→PQ2→Q2Q1→Q1Q→QQ1→Q1Q2→Q2P運動路線，共6個點位，10個階段。

圖2碼垛路徑點位示意圖

之所以在抓取側(cè)和碼垛側(cè)各設(shè)置一個中間點，是保證機器人抓取、碼垛過程中基本上走垂直線，以圖2中抓取側(cè)為例，如果不設(shè)置中間點，由于起始點

P

和抓取點

P

2

的

x

坐標相同，從

P

→

P

2

過程中，

x

方向電機保持不動，如此在以

A

為支點的

z

方向小臂電機運動下，

P

→

P

2

就會走出圖中所示的一條弧線，基于安全生產(chǎn)考慮、以及在有些空間狹窄的作業(yè)場合，這種運動軌跡是不允許的，因此通常要設(shè)置一個中間點

P

1

(

z

坐標為0)，分成

PP

1

、

P

1

P

2

二段運動軌跡，

PP

1

段時，

x

方向電機后退(

A

→

A

1

)，以保證

PP

1

基本垂直，而

P

1

P

2

段時，

x

方向電機前行(

A

1

→

A

2

)，以保證

P

1

P

2

基本垂直。同理，在碼垛側(cè)也設(shè)置一個中間點

Q

1

。

1.2點位坐標確定

在碼垛過程中，抓取點、起始點通常為固定的一點，可采用示教法直接確定；而碼垛點、碼垛上方點均有多點(如圖3所示，每層3*3個物品，共3層，則有27個碼垛點、27個碼垛上方點)。

圖3　碼垛模式示意圖

顯然，實際操作中是不會逐一采用示教法來確定如此多點位的[3]，而通常是按下步驟來確定(以碼垛點為例)：

(1)對1#物品碼垛點采用示教法，直接確定各軸脈沖坐標；

(2)根據(jù)各軸脈沖坐標，通過反運動學(xué)求解，求出1#物品碼垛點的距離坐標；

(3)其余物品則根據(jù)與1#物品之間的x、y、z距離偏差，先由下式確定出其距離坐標：

(1)

其中：n為碼垛物品序號，lx、ly、lz為相鄰碼垛點在x、y、z方向上距離(通常是定值)。

(4)再通過正運動學(xué)求解，由物品距離坐標確定出其脈沖坐標。

之所以先確定距離坐標，再確定脈沖坐標，是由于y、z二軸是旋轉(zhuǎn)運動，且此二軸運動又會影響到x軸坐標(見下文描述)，因而各點的距離坐標和脈沖坐標不是簡單的線性關(guān)系，要確定碼垛側(cè)各點脈沖坐標，必須要通過正運動學(xué)求解。

另外，為避免碼垛各階段運動過程中產(chǎn)生累積誤差，各點位坐標采用絕對坐標體系，即相對于原點的絕對距離位置(距離坐標)，與此對應(yīng)，各軸伺服電機采用絕對定位方式，即相對于原點的絕對脈沖位置(脈沖坐標)。

2　正、反運動學(xué)求解

為確定碼垛側(cè)各點脈沖坐標，需進行正、反運動學(xué)求解，下面詳細介紹求解過程。

2.1正運動學(xué)求解

已知目標點位D的距離坐標(xd,yd,zd)，求解相應(yīng)的脈沖坐標(xpls,ypls,zpls)。

z、y方向均是旋轉(zhuǎn)運動，不受其它方向運動的影響，因而z、y方向的轉(zhuǎn)動角度可方便求出；但z、y方向的旋轉(zhuǎn)運動都會影響到x方向坐標，因此，具體計算x方向運動行進距離時，可分解到x-z和x-y兩個坐標系中，分別計算在x-z和x-y坐標系中的行進距離xz和xy。

另外，為分析方便，目標點D在x-z和x-y兩個坐標系中，分別稱為Dxz、Dxy。

2.1.1x-z坐標計算

圖4為求解示意圖，為分析方便，假設(shè)x、z方向不是同步運動，而是按如下分兩步實現(xiàn)：

圖4　x-z坐標正運動學(xué)求解

第1步：z方向轉(zhuǎn)動角度θ1，顯然：

(2)

其中：ze是計算目標點B’xz的z坐標值。

之所以引入計算目標點B’xz，是由于腕部電機、安裝座及夾具部分(圖中陰影框部分)與地面始終保持垂直，所以實際目標點Dxz到小臂支點A之間距離不是定值，而是隨θ1變化而變化，因此無法作為計算參照，而計算目標點B’xz到A點之間距離是定值(即小臂半徑R)，因而計算時采用B’xz點為參照，B’xz與Dxz點之間關(guān)系如下：

(3)

其中：Rx、Rz分別為實際目標點Dxz和計算目標點B’xz在x、z方向的距離。

由式(2)、(3)得，z方向電機轉(zhuǎn)動角度θ1為：

(4)

第2步：在第1步基礎(chǔ)上，計算x方向運動在x-z坐標系中行進距離xz，顯然：

xz=OE-AE=xe-Rcosθ1，因此：

(5)

2.1.2x-y坐標計算

圖5為求解示意圖，為分析方便，假設(shè)x、y方向不是同步運動，而是按如下順序分兩步實現(xiàn)。

圖5　x-y坐標正運動學(xué)求解

第3步：在上述第1、2步基礎(chǔ)上，x方向運動再前行距離xy，使之從Dxz點前行到C點，之所以如此，目的是經(jīng)第4步y(tǒng)軸沿腰部軸心Oy旋轉(zhuǎn)θ2后，最終滿足實際目標點Dxy的y坐標要求。顯然，由xy=OyC-OyDxz=OyDxy--OyDxz，可得：

(6)

其中：Rxy是原點O與腰部軸心Oy之間距離。

第4步：y方向電機轉(zhuǎn)動角度θ2，到達目標點Dxy，可得：

(7)

2.1.3x軸運動總行進距離xdis計算

解出xz、xy后，即可得出：

(8)

2.1.4目標點位D脈沖坐標計算

經(jīng)上述步驟，求得z、y方向電機轉(zhuǎn)動角度θ1、θ2(rad)，及x方向運動的行進距離xdis(mm)后，即可求出目標點D脈沖坐標(xpls,ypls,zpls)：

(9)

其中：zdiv、ydiv為z、y軸電機行進分度值(pls/rad，每弧度對應(yīng)脈沖數(shù))；xdiv為x軸電機行進分度值(pls/mm，每mm對應(yīng)脈沖數(shù))。

2.2反運動學(xué)求解

已知目標點位D的脈沖坐標(xpls,ypls,zpls)，求解相應(yīng)的距離坐標(xd,yd,zd)。按如下步驟計算(公式中各符號含義見2.1節(jié))：

2.2.1目標點z軸位置坐標

先求出z方向電機行進角度θ1：

(10)

再由圖4和式(3)，可得：

(11)

2.2.2目標點x軸位置坐標

先求出y方向電機行進角度θ2：

(12)

再由圖5，可知：

(13)

將上式代入式(6)，可得：

(14)

將式(5)、式(14)代入式(8)，有：

即：

因此：

(15)

由式(9)求出xdis，代入式(15)，即可求得目標點x軸位置坐標：

(6)

2.2.3目標點y軸位置坐標

求得xd后，由式(13)即可求出yd。

2.3實現(xiàn)指令

系統(tǒng)控制核心采用日本三菱電機的FX3U-48MT小型PLC，具有豐富的浮點運算功能指令，如三角函數(shù)、反三角函數(shù)、及浮點加減乘除等，另外還有浮點數(shù)→整數(shù)、整數(shù)→浮點數(shù)的轉(zhuǎn)換指令，非常方便編程實現(xiàn)上述的正、反運動學(xué)求解過程。

3　控制系統(tǒng)

系統(tǒng)控制核心FX3U-48MT小型PLC，本體具有3路最高為100 k的高速輸出I/O口；各軸驅(qū)動采用三菱的MR-J4-A系列交流伺服系統(tǒng)[4]，其內(nèi)部配置22位高分辨率編碼器(4194304 ppr)，具有位置控制、速度控制和轉(zhuǎn)矩控制三種控制模式，本系統(tǒng)中使用位置控制模式。系統(tǒng)主要輸入輸出I/O信號如圖6所示。

大臂x、小臂z、腰部y軸這三軸伺服系統(tǒng)的脈沖輸入配置為“脈沖串+符號”形式(通過伺服驅(qū)動器PA13參數(shù)設(shè)置)，PP為脈沖串輸入引腳，NP為脈沖方向引腳，直接由FX3U-48MT本體的Y0、Y1、Y2高速輸出口輸出脈沖串、Y10、Y11、Y12則控制脈沖方向。

圖6　主要輸入輸出I/O信號

腕部w軸伺服系統(tǒng)的脈沖輸入配置也是“脈沖串+符號”形式(PA13參數(shù))，由FX2N-1PG脈沖發(fā)生器模塊控制，可以實現(xiàn)1路最大100 k高速輸出，1PG模塊脈沖輸出配置為“帶方向的脈沖”(BFM#3單元的b8位=1)，即FP輸出脈沖串，RP則為方向輸出。

3.1原點回歸

系統(tǒng)采用絕對脈沖坐標，即各點脈沖值是相對于原點的絕對脈沖位置，因而系統(tǒng)上電運行后，必須先對各軸執(zhí)行原點回歸[5]，以獲得各軸原點位置。以x軸為例，結(jié)合圖7說明原點回歸過程。

圖7　原點回歸示意圖

從A點開始執(zhí)行原點回歸，按設(shè)定加速時間加速到設(shè)定的回歸速度(B點)后，按回歸速度朝原點運動；當(dāng)檢測到C點的DOG信號(近原點信號)后，按設(shè)定減速時間減速到設(shè)定的爬行速度(D點)后，按爬行速度緩慢朝原點運動；當(dāng)檢測到E點的原點信號后，在1 ms之內(nèi)從F點開始減速到零(O點)，電機停止，脈沖當(dāng)前值寄存器(D8341-D8340)清零，原點回歸完成。

具體指令格式如下[6]：

[DSZR X000 X001 Y000 M200]

其中：DSZR是帶DOG搜索的原點回歸，X0為DOG信號輸入I/O口，X1為零點/原點信號輸入I/O口，Y0為高速脈沖輸出I/O口，M200由PLC自身判斷旋轉(zhuǎn)方向，PLC程序再根據(jù)M200狀態(tài)控制伺服系統(tǒng)方向輸入信號。

加減速時間、最高速度、回歸速度和爬行速度等通過D8342～D8348特殊數(shù)據(jù)寄存器設(shè)定。

另外從示意圖可知，通常要使原點回歸的開始點(A點)，位于DOG信號的右邊，而當(dāng)機器人遇到斷電或緊急停止等特殊情況時，并不能保證當(dāng)前位置在DOG信號的右邊。為此，原點回歸開始前，程序先執(zhí)行PLSY脈沖輸出指令，控制電機運動，使當(dāng)前位置處在x軸后限位，然后再執(zhí)行原點回歸。PLSY具體指令格式如下[6]：

[DPLSY D0 D10 Y000]

其中：DPLSY是32位脈沖輸出指令，D1-D0為輸出脈沖頻率，D11-D10為輸出脈沖數(shù)，因為上電后無法得知當(dāng)前位置距x軸后限位的具體脈沖數(shù)值，所以此單元設(shè)為0(此時Y0以設(shè)定脈沖頻率一直輸出脈沖)。運動過程中檢測到x軸后限位時，即可停止本指令執(zhí)行，轉(zhuǎn)而進行原點回歸。

3.2各軸聯(lián)動脈沖數(shù)、脈沖頻率確定

在運動學(xué)求解過程中，假設(shè)各軸電機不是同步運動的，但這僅僅是為了計算分析方便，實際運動時各軸當(dāng)然需聯(lián)動控制：即從當(dāng)前點位向目標點位運動時，各軸電機需同時啟動、且同時到達目標點位、并同時停止。顯然各軸距目標點位的行進脈沖數(shù)是不同的，為了使各軸運動能同時到達目標點位，必然要求各軸電機的行進速度要匹配，計算過程如下：

(1)計算目標點位的脈沖坐標，標記如下：

(2)根據(jù)當(dāng)前點位脈沖坐標，標記如下：

先確定最大行進脈沖數(shù)，然后按下式確定到達目標點位所需時間：

(17)

其中：fmax是系統(tǒng)設(shè)定的各軸伺服系統(tǒng)脈沖輸入的最大頻率。

確定到達目標點位所需時間t后，即可按下式確定各軸伺服系統(tǒng)脈沖輸入頻率：

(18)

3.3各軸絕對定位驅(qū)動

各軸行進脈沖數(shù)、脈沖頻率確定后，即可執(zhí)行絕對定位指令，控制各軸伺服電機行進到目標點位。絕對定位示意圖如圖8所示。

圖8　原點回歸示意圖

假設(shè)起點1、起點2當(dāng)前脈沖坐標值為100、375，目標點坐標值均為225，顯然對于起點1情形下，行進方向應(yīng)向右，而對于起點2情形下，行進方向應(yīng)向左。FX3UPLC的絕對定位指令具有自動判斷運動方向的功能，因而系統(tǒng)使用絕對坐標體系和絕對定位驅(qū)動，通過絕對定位指令自動判斷運動方向，極大降低了程序復(fù)雜程度。

以x方向伺服為例，指令格式如下[6]：

[DDRVA D100 D200 Y000 M100]

其中：DDRVA是雙字絕對定位指令，D101-D100存放經(jīng)計算所得的脈沖坐標，D201-D200存放脈沖頻率，Y0為高速脈沖輸出I/O口，本指令根據(jù)起點和目標點坐標，自動判斷運動方向，并對M100置位或清零，PLC程序再根據(jù)M100狀態(tài)控制伺服系統(tǒng)方向輸入信號。

4　結(jié)論

介紹了一類關(guān)節(jié)坐標碼垛機器人的機械結(jié)構(gòu)和控制系統(tǒng)，根據(jù)碼垛生產(chǎn)特點，軌跡規(guī)劃采用設(shè)定點位法，對起始點、抓取點、1#物品碼垛點等點位采用示教法直接確定，其余點位則根據(jù)正、反運動學(xué)求解獲取。各軸驅(qū)動采用交流伺服系統(tǒng)，為避免相對定位方式下產(chǎn)生累積誤差，機器人運動采用絕對坐標體系，為此需先通過原點回歸確定各軸原點。碼垛過程中，根據(jù)目標點位實時計算各軸聯(lián)動所需的脈沖數(shù)和頻率，采用絕對定位驅(qū)動方式實現(xiàn)機器人各軸聯(lián)動，同時利用伺服自身配備的高精度編碼器檢測各軸是否到位，以提高系統(tǒng)運動精度。

[1] 李成偉, 朱秀麗, 贠超. 碼垛機器人機構(gòu)設(shè)計與控制系統(tǒng)研究[J]. 機電工程, 2008,25(12):81-84.

[2] 葉辰雷, 劉曉平, 贠超. 碼垛機器人的軌跡規(guī)劃與仿真分析[J]. 機械研究與應(yīng)用, 2013,26(5): 26-30.

[3] 張明, 何慶中, 王志鵬,等. 關(guān)節(jié)型碼垛機器人的工作空間分析[J]. 組合機床與自動化加工技術(shù), 2013(7):68-70.

[4] 三菱電機自動化(中國)有限公司. MR-J4-A伺服放大器技術(shù)資料集，SH030103-A[EB/OL]. (2012-01)[2015-06].http://cn.mitsubishielectric.com/fa/zh/download/dwn_idx_manual.asp.

[5] 三菱電機自動化(中國)有限公司. FX3U系列微型可編程控制器用戶手冊[定位控制篇]，JY997D19501G [EB/OL]. (2014-04)[2015-06] . http://cn.mitsubishielectric.com/fa/zh/download/dwn_idx_manual.asp.

[6] 三菱電機自動化(中國)有限公司. FX3U系列微型可編程控制器編程手冊，JY997D19401M[EB/OL].(2014-03)[2015-06].http://cn.mitsubishielectric.com/fa/zh/download/dwn_idx_manual.asp.

Kinematics analysis and control system design of palletizing robot

Zhou Genrong，Jiang Ping，Zhong Yongyan

(School of Electrical Engineering, Nantong University, Nantong226019, China)

This paper analyzes the transmission structure of a kind of four DOF palletizing robot, and constructs the kinematics coordinate system. According to the production characteristics of palletizing, the point-setting method is used for trajectory planning, that some points are determined through teaching method directly, the remaining points are obtained by the forward and reverse kinematics solving. In order to reduce the complexity of solving, the solution in the three-dimensional coordinate system is first decomposed into two plane coordinate system, and then solved separately, finally integrated together. PLC is used as controller in this system, each axis is driven by AC servo system, and the origin of each axis is determined by return-to-zero-point instruction. According to the target point coordinate, the driving pulse numbers and frequency of each axis are calculated in real time, the linkage of all axises is realized by the absolute positioning method. The practical results show that the calculation speed and the real-time performance of the system are improved due to the reduction of the complexity of the solution. Compared with the relative positioning method, absolute positioning method avoids the accumulated error and improves the accuracy of motion.

palletizing robot; kinematics analysis; AC servo system; absolute positioning

2015-10-09；

2015-11-12。

江蘇省產(chǎn)學(xué)研合作前瞻性聯(lián)合研究項目(BY2015047-06)。

周根榮(1971-)，男，浙江金華人，副教授，主要從事自動控制、檢測與傳感技術(shù)方向的研究。

1671-4598(2016)01-0081-04

10.16526/j.cnki.11-4762/tp.2016.01.022

TP249

A