該系統(tǒng)采用雷賽SMC606運動控制器和L5系列交流伺服電機控制、驅(qū)動國產(chǎn)SCARA機械手。如圖1所示，氣動手爪安裝在機械手前段，其中心點不在Z軸的軸線上。

該系統(tǒng)采用嵌入式工控機為主控單元，用C#語言編程，完成機器視覺定位、機械手運動軌跡規(guī)劃的工作；并通過以太網(wǎng)向SMC606運動控制器實時發(fā)出運動控制指令，實現(xiàn)插頭裝配工作。

和普通工控機+運動控制卡的方案相比，該系統(tǒng)具有穩(wěn)定可靠、性價比高等特點。

圖1.  SCARA機械手及手爪

一.SCARA機械手的運動學(xué)方程

1.SCARA機械手的正向運動學(xué)的解正向運動學(xué)是已知各關(guān)節(jié)的參數(shù)，求手爪的位置和姿態(tài)。

即：已知各桿長L1、L2、L3，各關(guān)節(jié)轉(zhuǎn)角θ1、θ2、θ3，求手爪在機座坐標系中的解。

如圖2所示，坐標系{0}是機座坐標系。機座坐標系又稱為全局坐標系，是一個固定坐標系，它是計算機械手運動的基礎(chǔ)。通常其原點設(shè)在機器手底部中心。

坐標系{1}是連桿L1的坐標系。坐標系{2}是連桿L2的坐標系。

坐標系{3}是工具坐標系。工具坐標系是表示工具中心點TCP（ToolCenterPoint）的位置和工具姿態(tài)的直角坐標系。工具坐標系是一個活動的坐標系，使用工具坐標系便于編寫手爪抓取工件的程序。

通過連桿坐標系之間的齊次變換，可得到坐標系{3}與坐標系{0}的變換關(guān)系如下：

2.SCARA機械手的反向運動學(xué)的解

反向運動學(xué)是已知手爪的位置和姿態(tài)，求各關(guān)節(jié)的轉(zhuǎn)角。

即：已知手爪中心點P3在機座坐標系的位置P3（x0，y0）和轉(zhuǎn)角a。求：各關(guān)節(jié)轉(zhuǎn)角θ1、θ2、θ3。如圖3所示。

設(shè)點P2在機座坐標系的位置為P2（M，N）。顯然，

二.最佳路徑的選擇

1.選擇終點的解

如圖4所示，終點位置連桿的參數(shù)有2個解，選用哪一個為好？需要設(shè)計一個判斷準則。

因為連桿L2、Z軸和手爪都安裝在連桿L1上，且L1比L2長，故其負載重、運動覆蓋的面積大，所以，應(yīng)該讓L2多運動、讓L1少運動。

圖4.  SCARA機械手運動路徑的選擇

其中：0.6為連桿L₂的加權(quán)系數(shù)。

比較total1和total2，那個小就用那個解。如圖5所示，選擇解1；如圖6所示，選擇解2。

圖5.  終點選擇解1的示例

圖6.  終點選擇解2的示例

2. 選擇手爪旋轉(zhuǎn)方向

由于手爪上連接有氣管，旋轉(zhuǎn)角度有限制。通常手爪的旋轉(zhuǎn)角θ3小于360度。

如圖4所示，手爪的旋轉(zhuǎn)角可是dθ3也可以是dθ3’。選擇手爪旋轉(zhuǎn)方向的判定準則如下：

1） 在dθ3和dθ3’中選擇絕對值小者a；

2） 如果?asum+a?<360^o，則轉(zhuǎn)動a；然后，asum=asum+a。式中asum為手爪累計旋轉(zhuǎn)角。

3） 如果?asum+a?3360^o，則a為dθ3和dθ3’中的絕對值大者；然后，asum=asum+a。

如圖7所示為正常情況下，dθ3選擇旋轉(zhuǎn)角度小的方向。但圖8所示，dθ3選擇了旋轉(zhuǎn)角度大的方向。因為此時如果選擇角度小的方向，累計轉(zhuǎn)角就會大于360^o。

圖7.  正常情況下手爪旋轉(zhuǎn)選取角度小的方向

圖8.  為避免手爪旋轉(zhuǎn)角累計超過360^o，手爪旋轉(zhuǎn)方向和正常情況相反

三.SCARA機械手結(jié)構(gòu)及參數(shù)

1.Z軸及關(guān)節(jié)J4的結(jié)構(gòu)，SCARA機械手外形如圖9所示。Z軸及關(guān)節(jié)J4由一根絲桿花鍵軸構(gòu)成，如圖10所示。

圖9.  SCARA機械手外形                          圖10.  絲桿花鍵軸的結(jié)構(gòu)

絲桿花鍵軸運動有以下特點：

1）當(dāng)電機3控制絲桿螺母帶輪左右旋轉(zhuǎn)時，Z軸可上下運動。

2）當(dāng)電機4控制花鍵套帶輪左右旋轉(zhuǎn)時，關(guān)節(jié)J₄（手爪）可左右旋轉(zhuǎn)，同時，Z軸上下運動。即：關(guān)節(jié)J₄的旋轉(zhuǎn)運動和Z軸的上下運動是耦合在一起的。

3）關(guān)節(jié)J₄（手爪）要獨立進行旋轉(zhuǎn)運動，電機4和電機3必須聯(lián)動。聯(lián)動方式為：電機3的轉(zhuǎn)角與電機4的轉(zhuǎn)角相同，但方向相反。使Z軸向上和向下的位移量抵消。

2. 計算各軸的脈沖當(dāng)量

已知該機械手的各軸參數(shù)如下：

關(guān)節(jié)J₁和J₂的減速器的減速比均為1:50；關(guān)節(jié)J₄的減速器的減速比為1:20。Z軸減速器的減速比為1:2，絲桿導(dǎo)程為16mm。機械手上的4個電機轉(zhuǎn)動一周的脈沖數(shù)都為5000個。

故：控制J₁電機運動的脈沖當(dāng)量 Equival1 = 5000′50/360 = 694.44444 脈沖/度。

控制J₂電機運動的脈沖當(dāng)量Equival2 = 694.44444 脈沖/度。

控制J₄電機運動的脈沖當(dāng)量Equival4 = 5000′20/360 = 277.77778 脈沖/度。

控制Z電機運動的脈沖當(dāng)量Equival3 = 5000′2/16 = 625.0 脈沖/毫米。

四．  SMC606控制SCARA機械手的程序

1. 控制界面的設(shè)計

SMC606控制SCARA機械手運動分為兩類：手動控制和自動控制。手動控制又分XYZ坐標下的位置控制和3個關(guān)節(jié)旋轉(zhuǎn)軸的位置控制。如圖10所示。

圖10.  SCARA機械手控制界面

手動控制有2種方式：連動和點動。連動即連續(xù)運動，按鍵按下后運動開始，按鍵抬起后運動停止。點動即點擊一次按鍵，相應(yīng)的電機運動一段距離。且每次運動的速度和距離也可選擇為高、低和長、中、短。

2. 程序的初始化

1）在SMC606運動控制器資料光盤中的動態(tài)庫目錄下找到文件LTSMC.CS、LTSMC.dll，并拷貝到C#工程文件目錄中的BIN/DEBUG內(nèi)。

2）用鼠標右鍵點擊“解決方案資源管理器”的工程名，然后點擊“添加”、“現(xiàn)有項”，找到上述目錄中的LTSMC.cs文件，添加到工程中。

3）在Form1.cs頭文件中添加SMC606運動控制器的命名空間 using Leadshine;

4）在Form1_Load函數(shù)中連接SMC606運動控制器。指令如下所示。

注意：與SMC606連接的PC機的IP地址前3段必須為192.168.5。

3. 手動控制的程序

1）單軸運動的點動程序

關(guān)節(jié)J₁、J₂和Z軸的點動過程都是單軸運動，采用點位運動指令Pmove很容易實現(xiàn)此功能?？刂脐P(guān)節(jié)J₁正方向點動的程序如下所示，關(guān)節(jié)J₂和Z軸的點動程序類似。其中運動距離Dist0和運動速度Max_Vel0由界面中的“點動距離”和“速度”確定。

2）關(guān)節(jié)J₄（手爪）旋轉(zhuǎn)的點動程序

如本文三.2節(jié)所述，關(guān)節(jié)J₄做旋轉(zhuǎn)運動時，Z軸也會運動。為了實現(xiàn)J₄獨立運動，必須同時讓Z軸反向運動。采用2軸插補運動可實現(xiàn)此功能。詳見如下程序。

3）X和Y方向的點動程序

如本文一.2節(jié)所述，當(dāng)手爪僅向X方向運動時，關(guān)節(jié)J₁、J₂、J₄都會運動；由本文三.2節(jié)所述，關(guān)節(jié)J₄運動時，Z軸也會運動。故X方向的點動，實際上涉及到4個軸的運動，采用4軸插補運動可實現(xiàn)該功能。X和Y方向的點動程序類似。僅給出X正方向點動程序如下。

4）關(guān)節(jié)J₁、J₂、Z軸的連動程序

關(guān)節(jié)J₁、J₂和Z軸的連動過程都是單軸運動，采用速度控制指令Vmove很容易實現(xiàn)此功能?？刂脐P(guān)節(jié)J₁正方向點動的程序如下所示，關(guān)節(jié)J₂和Z軸的連動程序類似。

5）關(guān)節(jié)J₄的連動程序

當(dāng)按鍵按下后，用連續(xù)運動指令vmove控制關(guān)節(jié)J₄按一定的速度持續(xù)運動；同時，用點位運動指令pmove控制Z軸跟隨關(guān)節(jié)J₄運動；并且打開一個定時器，每0.2秒自動檢查一次位置誤差，然后用在線變位指令調(diào)整Z軸的目標位置。當(dāng)按鍵抬起后，關(guān)節(jié)J₄運動停止；然后，檢查Z軸位置，最后一次在線調(diào)整Z軸的目標位置，使其誤差為零。程序如下所示。

6）X、Y軸的連動程序

以X軸正方向連動為例：當(dāng)按鍵按下后，計算位移量dx，然后計算終點處4軸的參數(shù)，調(diào)用4軸插補運動函數(shù)，且有加速過程；指令脈沖發(fā)送完后，繼續(xù)向前運動dx，但4軸插補運動為勻速過程。當(dāng)按鍵抬起后，再向前運動最后一個dx，但4軸插補運動有減速過程。這樣連動過程就比較連續(xù)、平穩(wěn)。程序如下所示。

4. 機械手位置動畫顯示程序

如圖10所示，程序由一個定時器控制，每隔0.2秒在控制界面上顯示一次機械手連桿L₁、L₂和手爪的位置。為提高動畫顯示質(zhì)量，避免畫面閃爍，坐標軸及網(wǎng)格、機械手運動范圍不是每次重畫一遍，而是直接調(diào)用事先畫好的一個圖片。動畫程序如下所示。

5. 機械手自動控制程序

根據(jù)機械手動作要求，編寫自動控制程序并不復(fù)雜。點位運動的指令可參照本文三.2節(jié)中介紹的“X和Y方向的點動程序”內(nèi)容。對于復(fù)雜的軌跡控制，可調(diào)用連續(xù)插補、樣條差值（PVT函數(shù)）等指令。重點是機器視覺與運動控制的混合編程。因篇幅受限，相關(guān)內(nèi)容將在后續(xù)文章中介紹。

五．  小結(jié)

SMC606運動控制器的運動控制指令的種類多、功能強。只要充分理解SCARA機械手的運動學(xué)正解和反解的算法、靈活應(yīng)用SMC606的指令，控制SCARA機械手達到快、穩(wěn)、準的要求并不難。

六．  參考文獻

[1] 郭洪紅. 工業(yè)機器人技術(shù)（第三版）. 西安：西安電子科技大學(xué)出版社，2016

[2] 張愛紅. 工業(yè)機器人應(yīng)用與編程技術(shù). 北京：電子工業(yè)出版社，2015