摘要：二自由度的并聯(lián)機(jī)械手由于其高速、高精度的特點，目前被廣泛的應(yīng)用在各種工業(yè)場景中。由于其平行四邊形的結(jié)構(gòu)特點，使其終端執(zhí)行平臺能夠始終保持穩(wěn)定。針對該機(jī)構(gòu)的結(jié)構(gòu)特點和控制要求，主要研究該機(jī)械手的電氣控制方案、軟件實現(xiàn)算法、基于coDeSys 平臺的編程實現(xiàn)等。

關(guān)鍵詞：CoDeSys 平臺；二自由度并聯(lián)機(jī)械手；伺服控制

0 前言

二自由度并聯(lián)機(jī)械手廣泛應(yīng)用于食品、電子、醫(yī)藥等輕工業(yè)領(lǐng)域?？蓪崿F(xiàn)一些輕質(zhì)物料的中近距離搬運(yùn)、分揀、裝卸等各種操作。采用并聯(lián)機(jī)構(gòu)與串聯(lián)機(jī)構(gòu)進(jìn)行對比，具有載荷比低、剛度高、承載能力強(qiáng)等優(yōu)點。因此，廣泛應(yīng)用于各種高速運(yùn)行場合。

1 二自由度并聯(lián)機(jī)構(gòu)

二自由度并聯(lián)機(jī)器人機(jī)械手的機(jī)械結(jié)構(gòu)［1］如圖1 所示。該裝置由伺服軸、主動臂、從動臂和執(zhí)行端組成。在固定平臺上設(shè)置兩個伺服電機(jī)驅(qū)動主動臂，主動臂由從動臂連接，控制末端運(yùn)動。具有二自由度的機(jī)械臂可以從初始位置開始在二維平面上進(jìn)行垂直和水平運(yùn)動。

2 控制系統(tǒng)硬件架構(gòu)方案

2. 1 CoDeSys 平臺簡介

CoDeSys［2］以自動化軟件開發(fā)平臺中間件CoDeSys Automation Platform 和軟PLC 工具包套件為核心，長期專注于工業(yè)智能技術(shù)、PLC 控制器編程開發(fā)工具包套件、復(fù)雜運(yùn)動控制及數(shù)字化工廠解決方案的研發(fā)?；谙冗M(jìn)的.NET 架構(gòu)和IEC61131-3 國際編程標(biāo)準(zhǔn)［3］，遵照 PLCopen 國際組織制定的技術(shù)規(guī)范，提供開發(fā)PLC 控制器（自動化控制系統(tǒng)）的完整解決方案。

本控制系統(tǒng)采用的控制核心為松下PLC，該款PLC 為松下基于CoDeSys 平臺定制開發(fā)的。采用CoDeSys 的統(tǒng)一編程平臺降低了對PLC 開發(fā)人員的要求，當(dāng)開發(fā)人員使用不同廠家的PLC 時，只要這些PLC 都是遵循國際組織制定的規(guī)范，就可以無縫連接，大大減少了熟悉編程軟件的時間。因此，在CoDeSys 環(huán)境下開發(fā)的二自由度并聯(lián)機(jī)器人控制系統(tǒng)具有較高的通用性和實用價值。

2. 2 控制系統(tǒng)拓?fù)浣Y(jié)構(gòu)

該并聯(lián)機(jī)械手控制系統(tǒng)基于CoDeSys 編程平臺，采用EtherCAT 工業(yè)總線構(gòu)建運(yùn)動控制網(wǎng)絡(luò)。從圖2 可以看出，采用CoDeSys 的控制器是整個系統(tǒng)的控制核心，通過ModbusTCP 連接HMI，實現(xiàn)系統(tǒng)參數(shù)配置、系統(tǒng)運(yùn)行控制、運(yùn)行狀態(tài)監(jiān)控等人機(jī)交互操作。另外，通過將松下伺服驅(qū)動器與EtherCAT 總線連接，可以實現(xiàn)對2 軸伺服電機(jī)的精確控制。

3 控制系統(tǒng)軟件設(shè)計

3. 1 運(yùn)動過程逆解

運(yùn)動學(xué)逆解是指已知機(jī)械手末端執(zhí)行機(jī)構(gòu)的位姿，求解各運(yùn)動關(guān)節(jié)的位置，是機(jī)械手運(yùn)動規(guī)劃和軌跡控制的基礎(chǔ)。如圖 1 所示，該機(jī)構(gòu)的運(yùn)動過程逆解是根據(jù)執(zhí)行機(jī)構(gòu)中心點的坐標(biāo)求解電機(jī)的旋轉(zhuǎn)角度。

3. 2 門形軌跡規(guī)劃

機(jī)械手的軌跡規(guī)劃需要考慮多個必要的因素，其中包括機(jī)械手的位移、速度、加速度等的限制，軌跡的平滑性，機(jī)械手工作空間的限制，控制系統(tǒng)響應(yīng)速度的限制等。

對于二自由度并聯(lián)機(jī)械手，實際應(yīng)用中的軌跡一般會采用門形軌跡，門形軌跡的含義是，在機(jī)械手末端執(zhí)行機(jī)構(gòu)運(yùn)動過程當(dāng)中，其運(yùn)動的軌跡類似于“門”的形狀，這樣不僅可以使機(jī)械手在空間中的運(yùn)動路徑平滑，還能夠使機(jī)械手的運(yùn)動路徑縮短，從而提高機(jī)械手的運(yùn)行速度和執(zhí)行效率。但是門型軌跡也存在在其轉(zhuǎn)角處沖擊振動較大的問題，所以一般門型軌跡的轉(zhuǎn)角處采用曲線路徑對其進(jìn)行規(guī)劃。

如圖3 所示，將物品從A 點拾起，經(jīng)過B·點、C 點、D 點、E 點、F 點，最終到達(dá)G 點將物品放下，即完成了一次物品的拾取和放置操作。

圖3 中的拾取和放置操作可以分為如下的幾個階段：

上升階段1：機(jī)械手末端執(zhí)行機(jī)構(gòu)在垂直方向上沿著直線從A 點移動到B 點。

曲線階段2：機(jī)械手末端執(zhí)行機(jī)構(gòu)沿著曲線從B 點移動到C 點。

平移階段3：機(jī)械手末端執(zhí)行機(jī)構(gòu)在水平方向上沿著直線從C 點移動到D 點。

平移階段4：機(jī)械手末端執(zhí)行機(jī)構(gòu)在水平方向上沿著直線從D 點移動到E 點。

曲線階段5：機(jī)械手末端執(zhí)行機(jī)構(gòu)沿著曲線從E 點移動到F 點。

下降階段6：機(jī)械手末端執(zhí)行機(jī)構(gòu)在垂直方向上沿著直線從F 點移動到G 點。

3. 3 主程序設(shè)計

控制軟件采用基于CoDeSys 平臺的松下PLC編程軟件進(jìn)行編寫，采用模塊化的程序設(shè)計架構(gòu)。程序中的主要模塊如下：門型軌跡規(guī)劃模塊、運(yùn)行學(xué)逆解模塊、運(yùn)動控制模塊、通訊模塊、異常處理模塊。每個模塊的功能與作用如下。

門型軌跡規(guī)劃模塊：該模塊的作用是根據(jù)用戶輸入的門型軌跡參數(shù)，通過四次多項式（針對直線）、五次多項式（針對曲線）構(gòu)造出一個連續(xù)的函數(shù)。該函數(shù)以時間作為參數(shù)，從而計算出不同時間下的軌跡位置。門型軌跡規(guī)劃模塊的主要參數(shù)為上升段距離、曲線半徑1、平移距離1、平移距離2、曲線半徑2、下降段距離、運(yùn)行速度、運(yùn)行時間等參數(shù)。

運(yùn)動學(xué)逆解模塊：該模塊主要是通過機(jī)械手的末端執(zhí)行機(jī)構(gòu)的中心點坐標(biāo)求解電機(jī)的旋轉(zhuǎn)角度。其主要參數(shù)為機(jī)械手的機(jī)械結(jié)構(gòu)參數(shù)和門型軌跡規(guī)劃模塊給出的執(zhí)行機(jī)構(gòu)的中心點坐標(biāo)。運(yùn)動學(xué)逆解模塊的主要參數(shù)為主動臂長度、從動臂長度、兩個電機(jī)間距離、末端執(zhí)行機(jī)構(gòu)間距、機(jī)械手平衡位置垂直距離、末端執(zhí)行機(jī)構(gòu)坐標(biāo)。

運(yùn)動控制模塊：該模塊主要是將運(yùn)動學(xué)逆解模塊中解算出的電機(jī)旋轉(zhuǎn)角度轉(zhuǎn)換為電機(jī)的實際物理運(yùn)動。該控制系統(tǒng)中，伺服軸采用EtherCAT 總線控制，在IEC 程序中建立兩個伺服軸，并通過CoDeSys 提供的標(biāo)準(zhǔn)的軸控功能塊對伺服軸進(jìn)行回零、點動、定位等基本的控制動作。在該運(yùn)動控制模塊接收到運(yùn)動學(xué)逆解模塊傳來的電機(jī)運(yùn)行角度后，通過MC_Follow Position 功能塊，實時控制電機(jī)跟隨解算結(jié)果運(yùn)動，最終實現(xiàn)連續(xù)的門型軌跡運(yùn)動。

通訊模塊：該模塊的主要作用為與觸摸屏通訊、與第三方控制軟件通訊。通訊方式可以采用串口通訊、網(wǎng)口通訊。通訊協(xié)議支持Modbus 協(xié)議、EtherNet/IP 協(xié)議、TCP 自由協(xié)議等。該模塊中定義了所需的各種通訊變量，如啟動、暫停、停止、復(fù)位等控制變量，還有機(jī)械手位置、速度、結(jié)構(gòu)參數(shù)等參數(shù)變量，門型軌跡的軌跡參數(shù)設(shè)置變量等。方便用戶應(yīng)對復(fù)雜的工業(yè)應(yīng)用場景。

異常處理模塊：異常處理模塊主要處理程序運(yùn)行過程中的各種異常動作，例如，參數(shù)輸入錯誤、參數(shù)輸入超范圍、電機(jī)運(yùn)行超限、電機(jī)過載等等。該模塊保證了機(jī)械手在運(yùn)行過程中的安全性與穩(wěn)定性。不僅可以保證運(yùn)行過程中人員、設(shè)備等的安全，還能在出現(xiàn)異常時準(zhǔn)確的報出異常代碼，指導(dǎo)維護(hù)人員快速準(zhǔn)確的定位問題，從而快速的恢復(fù)生產(chǎn)。

4 人機(jī)界面設(shè)計

二自由度并聯(lián)機(jī)器人控制系統(tǒng)的人機(jī)交互部分由維倫通公司生產(chǎn)的觸摸屏實現(xiàn)。人機(jī)交互的主界面如圖4 所示。系統(tǒng)分為狀態(tài)監(jiān)控、運(yùn)行控制和操作菜單三部分。

4. 1 狀態(tài)監(jiān)測

軸1 的實際位置：用于監(jiān)控二自由度并聯(lián)機(jī)構(gòu)第一伺服軸的實際角度位置（對于二自由度并聯(lián)機(jī)構(gòu)，左側(cè)伺服軸視為第一伺服軸）。

軸2 的實際位置：用于監(jiān)控二自由度并聯(lián)機(jī)構(gòu)第二伺服軸的實際角度位置（對于二自由度并聯(lián)機(jī)構(gòu)，將右側(cè)伺服軸視為第二伺服軸）。

運(yùn)行速度：運(yùn)行速度表示2-DOF 并聯(lián)機(jī)構(gòu)的運(yùn)行節(jié)拍，即每分鐘完成多少個抓取動作。拍速與被捕捉物體的大小、位置、距離、運(yùn)行軌跡有關(guān)，是一種綜合速度。

運(yùn)行狀態(tài)監(jiān)控：運(yùn)行狀態(tài)監(jiān)控可以監(jiān)控圖形軌跡的畫面，以所見即所得的方式直觀地顯示2-DOF并聯(lián)機(jī)器人的運(yùn)行軌跡和抓取狀態(tài)。如觸摸屏主界面所示，左側(cè)為要捕獲的對象，右側(cè)為放置對象的位置。其操作邏輯為：當(dāng)左側(cè)待抓取物體到達(dá)位置時，觸發(fā)原地檢測傳感器?？刂破飨到y(tǒng)檢測到位置信號后，根據(jù)預(yù)設(shè)的操作軌跡從等待位置移動到抓取位置，抓取物體。當(dāng)抓取物體的動作完成后，機(jī)械手會按照預(yù)設(shè)的操作軌跡移動到物體放置的位置。當(dāng)物體被操作到指定位置后，它將釋放夾持器并將物體放置到位。動作完成后，機(jī)械手返回等待位置，等待下一個捕獲信號。然后它會做這樣一個循環(huán)的動作。借助這個監(jiān)控接口，可以直觀地觀察機(jī)械手的運(yùn)行軌跡是否符合預(yù)期，物體到達(dá)信號是否被觸發(fā)，物體放置后是否被輸送帶帶走。因此，可以很容易地觀察到整個系統(tǒng)的狀態(tài)。

報警狀態(tài)：報警狀態(tài)顯示了整個系統(tǒng)的報警情況，便于用戶根據(jù)報警狀態(tài)提示處理控制系統(tǒng)出現(xiàn)的異常問題。如伺服報警、通信報警、傳感器報警、參數(shù)設(shè)置異常報警等報警信息。

4. 2 運(yùn)行控制

運(yùn)行控制是人機(jī)交互過程中的主要控制部分，通過界面中間的按鈕控制整個系統(tǒng)的啟動和停止。主要包括以下幾個部分。

啟動：按下啟動按鈕后，整個系統(tǒng)處于運(yùn)行狀態(tài)，將根據(jù)預(yù)設(shè)的軌跡和傳感器信號執(zhí)行相應(yīng)的動作。

暫停：當(dāng)按下暫停按鈕時，整個系統(tǒng)將處于暫停狀態(tài)，機(jī)械手將懸停在當(dāng)前位置，控制系統(tǒng)將記錄當(dāng)前狀態(tài)。當(dāng)再次按下啟動按鈕時，暫停狀態(tài)將被釋放，控制系統(tǒng)將從暫停前的狀態(tài)恢復(fù)到運(yùn)行狀態(tài)。

停止：按下停止按鈕后，整個系統(tǒng)將進(jìn)入停止?fàn)顟B(tài)，機(jī)械手將返回等待位置，不再響應(yīng)外部傳感器信號。為了增加安全性，機(jī)械手的停止動作分為幾種情況。第一種情況：操縱器立即停止，不做任何判斷，所有狀態(tài)都回到初值。第二種情況：機(jī)械手立即停止，然后判斷當(dāng)前位置，夾爪內(nèi)是否有物體。如果有對象存在，則保持當(dāng)前狀態(tài)，運(yùn)行到安全位置等待。第三種情況：機(jī)械手不會立即停止，機(jī)械手將在最后一個周期后執(zhí)行停止動作。用戶可以在系統(tǒng)設(shè)置中設(shè)置停止動作。

復(fù)位：按下復(fù)位按鈕后，系統(tǒng)告警、錯誤、異常將被復(fù)位，控制系統(tǒng)將恢復(fù)到初始狀態(tài)。

4. 3 操作菜單

可通過操作菜單進(jìn)入相應(yīng)觸摸屏的功能子界面。

手動模式：在該操作子接口下，整個系統(tǒng)處于手動模式，主要用于系統(tǒng)調(diào)試、維護(hù)、異常處理等場景。

自動模式：在自動模式的操作子界面下，可以設(shè)置自動運(yùn)行參數(shù)，如軌跡、拍子等。

軌跡教學(xué)：在軌跡教學(xué)操作子界面下，可以通過系統(tǒng)教學(xué)的方式確定系統(tǒng)運(yùn)行軌跡，為用戶設(shè)置機(jī)械手運(yùn)行軌跡提供更方便。

參數(shù)設(shè)置：可在參數(shù)設(shè)置操作子界面下設(shè)置二自由度并聯(lián)機(jī)構(gòu)的參數(shù)信息，包括一些機(jī)械信息，如主動臂的長度和從動臂的長度?？刂葡到y(tǒng)根據(jù)這些參數(shù)信息通過運(yùn)動學(xué)逆解來確定伺服電機(jī)的工作角度。

系統(tǒng)設(shè)置：系統(tǒng)設(shè)置的操作子界面主要用于設(shè)置系統(tǒng)時間、告警信息顯示、操作員權(quán)限、停止方式等參數(shù)。

5 結(jié)語

二自由度并聯(lián)機(jī)械手具有高速、輕負(fù)荷的特點，在食品包裝、醫(yī)藥、電子等領(lǐng)域得到了廣泛的應(yīng)用。本文在控制器開發(fā)系統(tǒng)的基礎(chǔ)上，給出了二自由度機(jī)械臂控制系統(tǒng)的設(shè)計。同時，通過數(shù)據(jù)仿真和聯(lián)合調(diào)試，驗證了控制系統(tǒng)的控制需求和控制功能。

參 考 文 獻(xiàn)

［ 1 ］ 張卓然，黃慶久. 二自由度工業(yè)機(jī)器人位置力的混合控制[J]. 農(nóng)業(yè)機(jī)械與車輛工程，2020，58（11）：23-28.

［ 2］ 魏小星，朱建國，張良華，等. 基于CoDeSys 的面向?qū)ο驪LC編程[J]. 工業(yè)控制計算機(jī)，2021，34（9）：12-14.

［ 3 ］ 立信馬. 基于IEC61131 國際標(biāo)準(zhǔn)的開放式控制系統(tǒng)編程技術(shù)[M]. 北京：郵電出版社，2015.