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

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

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

圖1.  SCARA機械手及手爪

一.SCARA機械手的運動學方程

1.SCARA機械手的正向運動學的解正向運動學是已知各關節的參數，求手爪的位置和姿態。

即：已知各桿長L1、L2、L3，各關節轉角θ1、θ2、θ3，求手爪在機座坐標系中的解。

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

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

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

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

2.SCARA機械手的反向運動學的解

反向運動學是已知手爪的位置和姿態，求各關節的轉角。

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

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

二.最佳路徑的選擇

1.選擇終點的解

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

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

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

其中：0.6為連桿L₂的加權系數。

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

圖5.  çµ‚點選擇解1的示例

圖6.  çµ‚點選擇解2的示例

2. 選擇手爪旋轉方向

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

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

1） åœ¨dθ3å’Œdθ3’中選擇絕對值小者aï¼›

2） å¦‚æžœ?asum+a?<360^o，則轉動a；然后，asum=asum+a。式中asum為手爪累計旋轉角。

3） å¦‚æžœ?asum+a?3360^o，則a為dθ3å’Œdθ3’中的絕對值大者；然后，asum=asum+a。

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

圖7.  æ­£å¸¸æƒ…況下手爪旋轉選取角度小的方向

圖8.  ç‚ºé¿å…æ‰‹çˆªæ—‹è½‰è§’累計超過360^o，手爪旋轉方向和正常情況相反

三.SCARA機械手結構及參數

1.Z軸及關節J4的結構，SCARA機械手外形如圖9所示。Z軸及關節J4由一根絲桿花鍵軸構成，如圖10所示。

圖9.  SCARA機械手外形                          圖10.  çµ²æ¡¿èŠ±éµè»¸çš„結構

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

1）當電機3控制絲桿螺母帶輪左右旋轉時，Z軸可上下運動。

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

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

2. 計算各軸的脈沖當量

已知該機械手的各軸參數如下：

關節J₁å’ŒJ₂的減速器的減速比均為1:50；關節J₄的減速器的減速比為1:20。Z軸減速器的減速比為1:2，絲桿導程為16mm。機械手上的4個電機轉動一周的脈沖數都為5000個。

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

控制J₂電機運動的脈沖當量Equival2 = 694.44444 脈沖/度。

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

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

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

1. 控制界面的設計

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

圖10.  SCARA機械手控制界面

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

2. 程序的初始化

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

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

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

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

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

3. 手動控制的程序

1）單軸運動的點動程序

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

2）關節J₄（手爪）旋轉的點動程序

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

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

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

4）關節J₁、J₂、Z軸的連動程序

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

5）關節J₄的連動程序

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

6）X、Y軸的連動程序

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

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

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

5. 機械手自動控制程序

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

五．  å°çµ

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

六．  åƒè€ƒæ–‡ç»

[1] 郭洪紅. 工業機器人技術（第三版）. 西安：西安電子科技大學出版社，2016

[2] 張愛紅. 工業機器人應用與編程技術. 北京：電子工業出版社，2015