機電裝備運動控制軟件功能庫開發

吳繼春， 許 可， 陳凌宇， 范大鵬

（1.湘潭大學 機械工程學院，湖南 湘潭 411105；2.國防科技大學 智能科學學院，湖南 長沙 410073）

1 引 言

運動控制系統開發一直是機電控制領域的關鍵技術，越來越多的用戶在尋求標準化程度高、可移植強、開放性好、能夠快速開發的工業控制系統［1-3］。

目前，可編程控制器（Programmable Logic Controller， PLC）已被廣泛應用，且已成為智能制造系統的載體和核心。通過符合PLCopen規范的編程開發方式，對不同類型組件高效集成［4］，將運動規劃［5］算法、高精度控制［6］算法，以及機電系統的架構［7］均以功能塊的形式表達，因此如何開發功能塊是關鍵。Krzysztof等在基于PLCopen規范的基礎上，通過PLC coder快速生成，將運動軌跡生成與算法實際結合起來，并證明了該方 法的 有效性［8］。Ruiz團 隊開發 了基 于IEC61131-3標準的開發工具，用于評估PLC控制器在不同軌跡下的控制性能［9］。Doaa等針對運動控制內的安全問題進行研究，設計并開發安全功能塊，用于保證運動控制過程中的安全和穩定［10］。國內外自動化生產商廣泛采用PLC規范編程方式，提供了可以嵌入IEC 61131-3編程軟件的機械運動、CNC、機器人等一系列標準運動控制功能庫［11］。但是其開發的產品開放性差，在實際系統的應用有待研究，浙江工業大學徐建明等基于Codesys開發環境和EtherCAT通訊協議設計了PLCopen規范的六軸機器人控制系統［12-13］。針對控制算法的研究，華中科技大學研究團隊提出運動控制系統分層模型和功能塊打斷機制，實現了基于PLCopen標準的基本運動功能塊［14］。王夫康等針對單軸及主/從結構多軸功能塊無法實現復雜多維運動控制的問題，對軸組功能塊的執行方式、參數保存及傳遞方法進行研究，提出結構體隊列法，設計軸組功能塊，完成了多軸協調運動控制［15］。華南理工大學李迪團隊研究了功能塊內部的加減速控制算法以及多軸直線、圓弧插補算法，并在自主設計軟PLC平臺上進行了實驗驗證［16］。

上述研究都是基于特定開發環境使用傳統編程開發方式開發的功能塊，增加了編程要求和難度。本文對運動控制功能塊的設計和開發方式進行研究，在PLCopen規范的基礎上，提出了一種運動控制功能庫架構及其快速生成方法。利用功能塊快速開發技術解決了眾多功能塊的開發問題，降低了編程難度，提高了開發效率；并以旋轉雙棱鏡和CNC運動系統為對象進行實驗，實驗表明，所提出的方法能夠有效減少編程難度、縮短開發周期、增強程序的可復用性，同時也驗證了自主開發的功能庫能夠滿足復雜的運動控制需要。

2 PLCopen規范介紹

PLCopen組織先后制定了符合IEC61131-3標準的運動控制、邏輯控制、安全控制、OPCUA、XML代碼交換等規范。該規范在基于控制安全前提下使得PLC邏輯、機器人控制、CNC控制融合在一個編程平臺中。通過XML規范能夠實現基于不同開發環境中開發的程序或代碼的跨平臺使用。通過OPC-UA規范實現上位機與控制器及HMI組態軟件之間的安全通信。

2.1 PLCopen運動控制規范

在PLCopen規范中，運動控制規范為標準化的運動控制模塊開發提供了框架。該規范基于IEC標準開發環境，在開發、安裝和維護的各個階段協調不同的開發平臺，使上述環節都能夠滿足運動控制功能塊的標準化要求，從而實現“軟件定義機器”的目標。該規范分為6部分，內容和功能定義如表1所示。

表1 PLCopen運動控制規范Tab.1 PLCopen motion control specification

表1中，Part1和Part2對單軸和多軸功能塊的基本功能塊、狀態機、使用規則等進行了定義和擴展。Part3為用戶提高使用引導和功能塊示例。Part4對Part1和Part2在三維空間多軸運動控制功能進行擴展，Part5是針對運動的坐標系變換功能進行擴展。Part6提供液壓元件和液壓泵系統控制規范。上述規范為運動控制功能塊的設計和開發及實現制造裝備的完全自主可控開發生態提供了標準。

2.2 功能塊的實現原理

運動控制功能塊的實現依賴3個基本要素，即運動控制功能塊邏輯控制與算法實現、軸狀態機以及軸信息集合體。其中，軸狀態機表示軸當前狀態及可能存在的狀態轉換關系；軸信息集合體則是包含了當前系統中被控軸的參數信息。當單個功能塊執行時通過讀取軸信息結構體中關于軸的實時信息來改變運行狀態。多個功能塊共同運行時，功能塊之間、功能塊與軸信息結構體之間實時進行參數傳遞。通過參數的實時讀取改變內部函數的輸出，進而得到功能塊的輸出，并將該輸出下發至伺服機構執行。通過實時采集執行機構的參數和理想輸入對比，對軸的狀態進行實時調整。

功能塊實現流程如圖1所示，在PLC編程開發軟件上對功能塊或函數進行開發，通過調用功能塊搭建運動控制模塊；在編譯完成后進行仿真或直接下發指令至PLC實時內核中，與軸信息結構體進行參數傳遞，進而改變軸狀態機。在功能塊執行的過程中，通過對比理想輸出和實際輸出對功能塊參數進行實時修改，為避免由于功能塊內部邏輯算法錯誤導致系統運行錯誤，可預先通過外部指令對系統進行調試。上述流程在PLCopen規范下，使得軟件開發不再受硬件約束，用戶可以使用同一程序控制不同的外設或機電設備，且只需要對功能塊的輸入參數進行修改，有效地提升了程序的復用性和快速移植性。

圖1 PLCopen功能塊實現流程Fig.1 Implementation process of PLCopen function block

功能塊的執行是建立在狀態機上的，且在工作時軸只能處于一種狀態，通過功能塊的執行情況實現軸狀態的轉換。根據PLCopen規范，將狀態機分為單軸和軸組兩類，存在8種狀態，包括禁能、錯誤停止、靜止狀態機等3種非運行狀態機和停止運動、回零運動、同步運動、離散運動和連續運動狀態等5種運行狀態機。軸的狀態改變分為兩種情況：一是功能塊正常執行引起的改變；二是功能塊執行錯誤或者系統發生故障所引起的狀態改變。因此，在功能塊開發時狀態機的正常轉換是實現精確運動控制的前提。

2.3 功能塊的分層結構設計

PLCopen規范只規定到外部定義，對功能塊內部的邏輯框架沒有給出定義，因此需要用戶根據工程實際需要開發程序和邏輯代碼，形成標準化的算法結構和控制流程。如圖2所示，一個標準的功能塊分為內部邏輯和外部接口兩部分，其中外部接口包括輸入輸出變量（包括變量類型）、功能塊名稱和類型。內部邏輯分為接口層、預處理層、軸狀態層和邏輯算法（運動控制核心）。

圖2 功能塊分層結構Fig.2 Functional block hierarchical structure

雖然PLCopen規范為實現運動控制的基本功能塊制定了一套通用標準，詳盡規定了每個功能塊實例所實現的具體功能、輸入輸出接口參數含義和數據類型、完成時序圖等，而對于功能塊的內部執行邏輯和核心控制算法則需要開發者具體實現。根據功能塊的通用模型，本文設計功能塊內部代碼的分層結構，確保功能塊內部邏輯實現與外部接口分開，又能通過外部接口實現與系統硬件驅動的連接，保證控制軟件不再依賴硬件。

3 基于PLCopen規范的功能塊開發方法

3.1 運動控制功能塊的開發

功能塊是控制系統的關鍵，也是“軟件定義機器”的核心一環，隨著一系列研究的進行和標準的提出，其開發方式也逐漸豐富。目前，開發方式主要有兩種，一是傳統的在開發環境中采用IEC標準語言進行程序編寫的方式。由于開發軟件計算能力等因素的限制，開發含大量專業數學運算的功能塊會增加編程難度，如機器人的正逆運動學求解功能塊的開發等。因此，該方法還存在一定的局限性。二是基于PLCopen XML規范，借助其他開發軟件或數學軟件采用PLC coder自動生成的方式生成代碼，將符合軟件兼容性的代碼導入目標開發環境，解決大量復雜的數學運算功能塊的開發問題。該方法能夠通過模型生成代碼，并且可在生成軟件上進行仿真和代碼檢查，其開發效率對比傳統開發方式有著顯著提升，適合復雜功能塊的快速開發。

圖3為功能塊的快速開發流程。在預處理階段，對工程要求進行分析，確定目標被控對象及相關參數。基于數學軟件進行特性分析并建立控制模型和子系統。在建立子系統后需要對它進行兼容性檢測，如不符合標準，則對模型和子系統進行調整；反之，則對目標模型進行仿真以驗證模型是否符合目標工程要求。對模型進行仿真能夠有效地減少開發錯誤，及時對模型和參數進行修改。模型仿真通過后根據目標開發環境生成相應PLC coder。

圖3 功能塊快速開發流程Fig.3 Rapid development process of function blocks

上述方法生成的PLC Coder包括注釋信息和主代碼，注釋信息包括生成軟件和目標軟件的版本信息和代碼生成的時間。主代碼包括功能塊名稱、變量和主程序，各部分都是采用符合IEC標準的ST語言。基于XML規范，用戶在工程中將ST代碼導入即可使用，極大地增加了功能塊的開發效率，減少了開發過程中的錯誤。本文基于上述開發方法，開發了機器人的正逆運動學功能塊以及插補功能塊等復雜的運動控制功能塊。

3.2 運動控制功能庫架構

雖然國外軟件上已形成了一套標準化的功能庫，但是由于功能庫被封裝，用戶不能更改內部程序和算法，這不利于自主可控PAC系統的快速開發和重構。而目前國產軟件的開發工具并沒有專業的運動控制功能庫。因此，對于功能庫的開發是自主可控PAC系統開發及“軟件定義機器”的關鍵環節之一。

本文基于國產PAC軟件建立了標準化的運動控制庫架構，其架構如圖4所示，運動控制功能庫由工程和庫信息模塊、變量和指令模塊和功能塊/函數模塊三部分構成。其中，信息模塊用于庫名稱和庫信息的設置；變量和指令模塊對驅動接口和數據類型等進行定義；此處為PLCopen規范下的自定義數據類型，如全局變量、結構體、枚舉等。功能塊/函數模塊是功能庫的核心，不同的運動控制庫對應不同的功能塊集合。用戶針對目標運動將所需功能塊封裝至同一庫內，可以直接調用。

圖4 功能庫結構樹Fig.4 Structure tree of function library

運動控制功能庫是功能塊和函數的集合，可用于實現單軸運動、多軸運動和插補運動等功能。如圖5所示，庫內的功能塊根據軸數分為單軸、多軸/軸組功能塊，且每一類根據功能屬性又分為軸運動控制類和軸管理類功能塊。其中，運動控制類功能塊用于驅動單軸或多軸運動，如MC_Interpolated（插補）功能塊和Home（回零）功能塊等；管理類功能塊用于軸管理、參數配置和狀態監控，如MC_Power（使能）功能塊和Read-Status（軸狀態讀取）功能塊等。

圖5 功能塊類型Fig.5 Function block types

4 運動控制功能塊實例

4.1 軸管理類功能塊

MC_Power功能塊用于控制運動，控制從站伺服狀態機的切換，使驅動器上電使能，同時監測系統的工作狀態，其接口定義如表2所示。Axis為軸信息結構體，用于讀取軸信息。Enable用于給輸出軸使能，Enable為True時，驅動器就緒且狀態Status顯示為True。同時軸的狀態機由禁能狀態轉換為靜止狀態，等待控制指令輸入。如果從站出現故障狀態機或者驅動器返回伺服報警信號，則狀態Status仍為False，并通過Valid，ErrorID等輸出接口將錯誤信息輸出給用戶，表明系統存在硬件問題。此時，軸將轉換到錯誤停止狀態，需要等待硬件問題修復后才可繼續使用系統。

表2 MC_Power功能塊接口定義Tab.2 Interface definition of MC_ Power function block

4.2 單軸運動控制功能塊

單軸運動控制功能塊是運動控制庫中的基本元素。常用的單軸運動控制功能有MC_Home、速度控制、絕對運動（MC_MoveAbsolute）、相對運動（MC_MoveRelative）等，通過加減速控制實現上述功能。

MC_MoveAbsolute和 MC_MoveRelative運動功能塊是常用的單軸運動控制功能塊，絕對運動功能塊用于以指定參數運動到指定的絕對位置，而相對位置則是運動一段相對距離。不同的參數會得到不同的運動軌跡。因此，速度規劃是此類功能塊的核心，本文針對這類功能塊進行設計和開發。功能塊實例如圖5所示，兩功能塊的輸出和輸入變量與MC_Home相似，其中Distance表示位置增量輸入，Position表示目標位置輸入，Jerk為加加速度輸入。Buffer_Mode用于選擇軌跡類型，本文設計了3種速度類型。輸入為0時系統以輸入的速度勻速運行，1和2分別對應梯形和S型速度模式。

為驗證兩功能塊是否能夠完成目標運動，對兩功能塊進行仿真，在Beremize軟件中調用兩功能塊并將相對運動功能塊與絕對運動功能塊相連，運動參數設置如表3所示。

表3 MC_MoveAbsolute和MC_MoveRelative的參數Tab.3 Parameter of MC_MoveAbsolute and MC_ Moverelative

仿真結果如圖6所示，兩功能塊都是使用S型加減速方式。在運行至3 s左右時，對相對功能塊的執行狀態進行打斷并使絕對功能塊運行，可以看到兩功能塊能夠完成預期運動，表明兩功能塊能夠實現軸的相對運動和絕對運動。

圖6 功能塊執行結果Fig.6 Result of function block execution

4.3 多軸運動控制功能塊

多軸運動的實現形式包括多軸同步協同或者實現空間任意軌跡，本文針對多軸運動開發了一系列運動控制功能塊，如兩軸和三軸直線插補、圓弧插補等。圖5所示為MC_Interpolator插補功能塊，其輸入輸出與相對運動功能塊相似，通過上升沿觸發，可以實現圓弧和直線插補，以用戶指定參數進行插補運動控制。其中，輸入變量Mode為插補模式，其值為0，1和2，分別代表勻速插補、梯形速度模式和S型速度模式。多軸圓弧插補算法采用在Matlab編程仿真，然后通過PLC Coder自動生成代碼的方式實現，完成了從當前位置點P1（15，0，5）經過中間邊界點P2（14，14，10）到 達 終 點P3（3，1，-10）的 空間圓弧插補，采用梯形加減速算法，插補速度為30 (°)/s，加速度為45 (°)/s2。插補結果如圖7所示。

圖7 插補功能塊測試曲線Fig.7 Test curves of interpolation function block

5 仿真與實驗

本文以旋轉雙棱鏡和CNC系統為控制對象，開發了運動控制功能庫，并通過自主搭建的實驗平臺對上述功能庫進行驗證。

5.1 自主可控PAC系統實驗平臺

如圖8所示，系統由主站、從站、實時以太網總線、上位機軟件和用戶監控終端等構成。實時以太網采用高性能NCUC 2.0總線協議，通信周期為1 ms，邏輯編程軟件和HMI監控軟件運行于PC上位端，與系統主站進行Modbus或TCP/IP通信，完成控制程序代碼下載和數據實時交互。

圖8 國產PLC控制測試系統架構Fig.8 Architecture of domestic PLC control test system

主站是整個PLC系統的核心與“大腦”，主要用于完成系統實時任務調度、編譯環境的邏輯信息運算、對各節點從站的指令下達與通信控制、HMI數據交互等功能。本文使用的主站為基于國產處理器M7的嵌入式主站，嵌入式主站基于“FPGA+ARM架構”，無操作系統，程序執行代碼直接運行于嵌入式芯片上；從站節點模塊主要是根據主站的指令進行外設操作，包含傳感器的采集和輸出，并通過以太網總線控制器與主站實現數據交換和通信。以太網總線通信控制器位于各從站模塊中，主要實現各節點之間的高帶寬、高效率、高速度的數據傳輸通信；伺服驅動裝置和外設是系統完成運動控制的執行機構；系統軟件主要包括IEC-61131-3標準軟件集成開發環境和界面監控軟件兩部分，軟件集成開發環境主要用于實現系統硬件組態、可視化的系統功能設計和邏輯編程、邏輯代碼的編譯、程序可執行文件的生成和下載等。本文采用的開發環境為國產Beremize軟件，監控軟件為Qtouch軟件。界面監控軟件主要用于人機交互界面的設計和開發，完成與主站的通信，保證對主站數據的實時監控。

5.2 旋轉雙棱鏡控制實驗

典型旋轉雙棱鏡成像系統原理如圖9（a）所示，兩棱鏡Π1，Π2的橫截面為直角三角形，兩直角邊面相互平行且垂直于共同旋轉軸Z軸。為實現一定空間角度內所有指向的全覆蓋，兩棱鏡必須達到完全匹配，故通常讓兩棱鏡具有相同的折射系數和頂角，即n1=n2=n，α1=α2=α。兩棱鏡旋轉角θ1，θ2以X軸為測量起始。視軸方向以極坐標中的偏轉角Φ和方位角Θ描述。通過對點光源進行折射，改變兩個楔鏡的相對旋轉角度，光的傳播路徑隨之改變，從而達到對光束方向的控制。因此，根據上述原理，本實驗以兩棱鏡光源指向控制為目標，驗證所開發的功能塊以及國產軟硬件對在旋轉雙棱鏡上的應用效果，控制系統如圖9（b）所示。

圖10所示為旋轉雙棱鏡運動控制系統，系統主要由國產PAC系統、旋轉雙棱鏡、視覺采集系統組成以及平行光源組成。所使用光源波長為650 nm，視覺采集系統由海康攝像頭及視圖處理軟件組成。

圖9 旋轉雙棱鏡運動模型與控制系統Fig.9 Motion model and control system of rotating double prism

圖9（b）中，軸管理和從站參數設置功能塊用于對電機使能以及從站內部控制器的參數設置；雙棱鏡運動學功能塊用于進行光束正向問題和逆向問題的解算；運動指令轉換功能塊用于對指令進行轉換，以便從站執行；傳感器反饋模塊用于接收電機編碼器反饋的實際運動參數；從站狀態反饋功能塊用于監視和調整從站的狀態。

實驗結果如圖11所示，通過視覺處理系統可以獲得光點的軌跡，由光點軌跡可以看出經過雙棱鏡系統的點射光源在經過反射后始終保持一個橢圓軌跡，表明所開發的功能塊能夠實現雙棱鏡光束指向的正、逆向問題求解，且所開發的軟硬件系統能夠完成雙棱鏡的光束指向控制。因此，該系統可以應用于自由空間光通信、紅外對抗、激光指示器、激光雷達和光纖光開關等精密光學系統中。

圖10 旋轉雙棱鏡實驗平臺Fig.10 Rotating double prism experimental platform

圖11 光點軌跡Fig.11 Light path

5.3 CNC運動控制實驗

本文搭建了CNC運動控制實驗平臺和控制模塊，如圖12所示。圖12（b）中，MC_power為軸使能功能塊，MC_Interpolator為插補運動控制功能塊，MC_Home功能塊為復位功能塊，負責軸的急停和軸的回零運動等。

本文以圖13鏇輪機璇修軌跡為插補對象進行實驗，并以S型加減速模式運動，通過各段的曲率閾值，可以得到各段參數配置如下：Jerk=1.6×103mm/s3，Acceleration=20 mm/s2，Velocity=10 mm/s，各段的起始速度為vs1=0 mm/s，vs2=2 mm/s，vs3=4 mm/s，vs4=3 mm/s，vs5=5 mm/s，vs6=4 mm/s，vs7=3 mm/s，vs8=2 mm/s，vs9=2 mm/s；運動終點速度為ve9=0 mm/s，Mode=2。

圖12 CNC功能庫實驗系統Fig.12 CNC functional library experimental system

圖13 目標璇修軌跡Fig.13 Target repair trajectory

圖14 加速度與速度曲線Fig.14 Acceleration and velocity curves

實際的加速度和速度曲線如圖14所示。X和Y軸的實際位移曲線如圖15（a）所示，兩軸同步運動，可以實現多段的S型加減速運動。HMI反饋軌跡如圖15（b）所示，該功能塊可以正常完成圓弧插補和直線插補運動，與實際軌跡相比，在無負載的情況下兩者之間的誤差在0.3 mm。上述結果表明，自主開發的CNC運動控制功能庫能夠實現多種運動規劃，模擬簡單的CNC直線插補和圓弧插補運動，實現多電機的同步運動，初步驗證了該運動控制庫的有效性。

圖15 插補軌跡Fig.15 Interpolation trajectory

6 結 論

本文針對PLC上位機軟件中功能塊和庫不開放以及國產軟件內部功能塊缺乏等問題，基于XML規范，采用PLC coder技術自動生成符合IEC標準的結構化文本的方式快速開發功能塊，能夠解決上位機軟件中大量含復雜計算的功能塊的開發需要。基于功能塊的基本模型設計功能塊的分層結構，針對功能塊在國產開發環境中的關鍵技術進行了研究，通過快速開發方法開發一系列運動控制功能庫，如常用的機器人和CNC功能庫等。通過功能庫快速搭建了旋轉雙棱鏡和CNC運動控制模塊，能夠有效節約開發時間。在PLC上位機軟件中，利用快速生成的旋轉雙棱鏡和CNC運動控制庫搭建控制系統，通過自主搭建的實驗平臺，驗證了該控制系統的有效性。實驗結果表明，該方法能夠快速生成基于PLCopen的運動控制功能庫，解決了傳統PLC軟件功能庫不完善的問題。該控制系統能夠實現自主開發和控制，滿足工業控制的需求。