基于CoDeSys 平臺的二自由度并聯機械手控制系統設計

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

關鍵詞：CoDeSys 平臺；二自由度并聯機械手；伺服控制

0 前言

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

1 二自由度并聯機構

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

2 控制系統硬件架構方案

2. 1 CoDeSys 平臺簡介

CoDeSys［2］以自動化軟件開發平臺中間件CoDeSys Automation Platform 和軟PLC 工具包套件為核心，長期專注于工業智能技術、PLC 控制器編程開發工具包套件、復雜運動控制及數字化工廠解決方案的研發。基于先進的.NET 架構和IEC61131-3 國際編程標準［3］，遵照 PLCopen 國際組織制定的技術規范，提供開發PLC 控制器（自動化控制系統）的完整解決方案。

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

2. 2 控制系統拓撲結構

該并聯機械手控制系統基于CoDeSys 編程平臺，采用EtherCAT 工業總線構建運動控制網絡。從圖2 可以看出，采用CoDeSys 的控制器是整個系統的控制核心，通過ModbusTCP 連接HMI，實現系統參數配置、系統運行控制、運行狀態監控等人機交互操作。另外，通過將松下伺服驅動器與EtherCAT 總線連接，可以實現對2 軸伺服電機的精確控制。

3 控制系統軟件設計

3. 1 運動過程逆解

運動學逆解是指已知機械手末端執行機構的位姿，求解各運動關節的位置，是機械手運動規劃和軌跡控制的基礎。如圖 1 所示，該機構的運動過程逆解是根據執行機構中心點的坐標求解電機的旋轉角度。

3. 2 門形軌跡規劃

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

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

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

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

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

曲線階段2：機械手末端執行機構沿著曲線從B 點移動到C 點。

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

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

曲線階段5：機械手末端執行機構沿著曲線從E 點移動到F 點。

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

3. 3 主程序設計

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

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

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

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

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

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

4 人機界面設計

二自由度并聯機器人控制系統的人機交互部分由維倫通公司生產的觸摸屏實現。人機交互的主界面如圖4 所示。系統分為狀態監控、運行控制和操作菜單三部分。

4. 1 狀態監測

軸1 的實際位置：用于監控二自由度并聯機構第一伺服軸的實際角度位置（對于二自由度并聯機構，左側伺服軸視為第一伺服軸）。

軸2 的實際位置：用于監控二自由度并聯機構第二伺服軸的實際角度位置（對于二自由度并聯機構，將右側伺服軸視為第二伺服軸）。

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

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

報警狀態：報警狀態顯示了整個系統的報警情況，便于用戶根據報警狀態提示處理控制系統出現的異常問題。如伺服報警、通信報警、傳感器報警、參數設置異常報警等報警信息。

4. 2 運行控制

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

啟動：按下啟動按鈕后，整個系統處于運行狀態，將根據預設的軌跡和傳感器信號執行相應的動作。

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

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

復位：按下復位按鈕后，系統告警、錯誤、異常將被復位，控制系統將恢復到初始狀態。

4. 3 操作菜單

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

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

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

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

參數設置：可在參數設置操作子界面下設置二自由度并聯機構的參數信息，包括一些機械信息，如主動臂的長度和從動臂的長度。控制系統根據這些參數信息通過運動學逆解來確定伺服電機的工作角度。

系統設置：系統設置的操作子界面主要用于設置系統時間、告警信息顯示、操作員權限、停止方式等參數。

5 結語

二自由度并聯機械手具有高速、輕負荷的特點，在食品包裝、醫藥、電子等領域得到了廣泛的應用。本文在控制器開發系統的基礎上，給出了二自由度機械臂控制系統的設計。同時，通過數據仿真和聯合調試，驗證了控制系統的控制需求和控制功能。

參 考 文 獻

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

［ 2］ 魏小星，朱建國，張良華，等. 基于CoDeSys 的面向對象PLC編程[J]. 工業控制計算機，2021，34（9）：12-14.

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