基于CoDeSys 平臺的二自由度并聯(lián)機械手控制系統(tǒng)設(shè)計

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

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

0 前言

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

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

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

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

2. 1 CoDeSys 平臺簡介

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

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

2. 2 控制系統(tǒng)拓撲結(jié)構(gòu)

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

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

3. 1 運動過程逆解

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

4 人機界面設(shè)計

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

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

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

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

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

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

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

4. 2 運行控制

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

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

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

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

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

4. 3 操作菜單

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

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

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

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

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

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

5 結(jié)語

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

參 考 文 獻

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

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

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