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

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

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

圖1.  SCARA機(jī)械手及手爪

一.SCARA機(jī)械手的運(yùn)動(dòng)學(xué)方程

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

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

    如圖2所示，坐標(biāo)系{0}是機(jī)座坐標(biāo)系。機(jī)座坐標(biāo)系又稱為全局坐標(biāo)系，是一個(gè)固定坐標(biāo)系，它是計(jì)算機(jī)械手運(yùn)動(dòng)的基礎(chǔ)。通常其原點(diǎn)設(shè)在機(jī)器手底部中心。

    坐標(biāo)系{1}是連桿L1的坐標(biāo)系。坐標(biāo)系{2}是連桿L2的坐標(biāo)系。坐標(biāo)系{3}是工具坐標(biāo)系。工具坐標(biāo)系是表示工具中心點(diǎn)TCP（ToolCenterPoint）的位置和工具姿態(tài)的直角坐標(biāo)系。工具坐標(biāo)系是一個(gè)活動(dòng)的坐標(biāo)系，使用工具坐標(biāo)系便于編寫手爪抓取工件的程序。通過連桿坐標(biāo)系之間的齊次變換，可得到坐標(biāo)系{3}與坐標(biāo)系{0}的變換關(guān)系如下：

2.SCARA機(jī)械手的反向運(yùn)動(dòng)學(xué)的解

    反向運(yùn)動(dòng)學(xué)是已知手爪的位置和姿態(tài)，求各關(guān)節(jié)的轉(zhuǎn)角。即：已知手爪中心點(diǎn)P3在機(jī)座坐標(biāo)系的位置P3（x0，y0）和轉(zhuǎn)角a。求：各關(guān)節(jié)轉(zhuǎn)角θ1、θ2、θ3。如圖3所示。

設(shè)點(diǎn)P2在機(jī)座坐標(biāo)系的位置為P2（M，N）。顯然，

二.最佳路徑的選擇

1.選擇終點(diǎn)的解

    如圖4所示，終點(diǎn)位置連桿的參數(shù)有2個(gè)解，選用哪一個(gè)為好？需要設(shè)計(jì)一個(gè)判斷準(zhǔn)則。因?yàn)檫B桿L2、Z軸和手爪都安裝在連桿L1上，且L1比L2長，故其負(fù)載重、運(yùn)動(dòng)覆蓋的面積大，所以，應(yīng)該讓L2多運(yùn)動(dòng)、讓L1少運(yùn)動(dòng)。

圖4.  SCARA機(jī)械手運(yùn)動(dòng)路徑的選擇

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

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

圖5.  終點(diǎn)選擇解1的示例

圖6.  終點(diǎn)選擇解2的示例

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

    由于手爪上連接有氣管，旋轉(zhuǎn)角度有限制。通常手爪的旋轉(zhuǎn)角θ3小于360度。如圖4所示，手爪的旋轉(zhuǎn)角可是dθ3也可以是dθ3’。選擇手爪旋轉(zhuǎn)方向的判定準(zhǔn)則如下：

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

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

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

    如圖7所示為正常情況下，dθ3選擇旋轉(zhuǎn)角度小的方向。但圖8所示，dθ3選擇了旋轉(zhuǎn)角度大的方向。因?yàn)榇藭r(shí)如果選擇角度小的方向，累計(jì)轉(zhuǎn)角就會(huì)大于360^o。

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

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

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

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

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

絲桿花鍵軸運(yùn)動(dòng)有以下特點(diǎn)：

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

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

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

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

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

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

故：控制J₁電機(jī)運(yùn)動(dòng)的脈沖當(dāng)量 Equival1 = 5000´50/360 = 694.44444 脈沖/度。

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

控制J₄電機(jī)運(yùn)動(dòng)的脈沖當(dāng)量Equival4 = 5000´20/360 = 277.77778 脈沖/度。

控制Z電機(jī)運(yùn)動(dòng)的脈沖當(dāng)量Equival3 = 5000´2/16 = 625.0 脈沖/毫米。

四．  SMC606控制SCARA機(jī)械手的程序

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

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

圖10.  SCARA機(jī)械手控制界面

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

2. 程序的初始化

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

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

3）在Form1.cs頭文件中添加SMC606運(yùn)動(dòng)控制器的命名空間 using Leadshine;

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

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

3. 手動(dòng)控制的程序

1）單軸運(yùn)動(dòng)的點(diǎn)動(dòng)程序

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

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

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

3）X和Y方向的點(diǎn)動(dòng)程序

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

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

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

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

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

6）X、Y軸的連動(dòng)程序

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

4. 機(jī)械手位置動(dòng)畫顯示程序

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

5. 機(jī)械手自動(dòng)控制程序

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

五．  小結(jié)

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

六．  參考文獻(xiàn)

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

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