基于PLCopen的軸組協調運動功能塊的設計與實現

（合肥工業大學，合肥 230009）

0 引言

隨著工業化的發展，一大批運動控制器涌現出來。許多運動控制器中都集成有PLC的功能，可以實現一些簡單的控制邏輯。但是在擁有眾多I/0點的復雜運動控制系統中，這些簡單的PLC功能無法滿足控制要求。因此，PLC和運動控制往往是獨立的兩套系統，兩者之間可以通過I/O接口進行數據交互實現同步。這就導致了系統設計十分復雜。

PLCopen組織將PLC與運動控制結合起來，實現了PLC編程語言標準化，并推出運動控制功能塊標準。標準化的功能塊僅僅定義到接口和數據結構層面，功能塊內部算法實現由各個廠商自行定義，采用不同功能塊的組合，可以滿足眾多應用場合的需求。目前PLCopen的MC運動控制規范定義的功能塊主要分為用于單軸運動的Part1部分，用于電子齒輪、電子凸輪的Part2部分以及軸組協調運動的Part4部分。

由于控制單軸的運動控制系統已被廣泛實現，而針對軸組協調運動的軌跡規劃描述較少，本文利用菲尼克斯提供的SoftPLC，所調用功能塊的邏輯關系滿足PLCopen定義的軸組狀態機狀態的切換，并通過編寫軸組直線插補與圓弧插補算法，驗證所設計的功能塊可以被應用于傳統工業機器人和CNC領域內，具有高移植性。

1 PLCopen軸組功能塊的設計

1.1 功能塊模型

PLCopen標準將運動控制的編程語言定義為功能塊的形式，功能塊作為程序組織單元的一種，執行后能夠生成一個或多個輸出值。并且功能塊能夠生成多個自身的拷貝，被稱為實例。如圖1所示，每個實例都擁有相關的實例名、一個包含輸出和內部變量的數據結構、輸入變量的值。數據結構中的所有輸出變量和某些內部變量的值可能是保持的，這意味著在上一次功能塊的執行過程之后的這些變量的值在下一次執行中也是有效的。因此，實例化出來的幾個相同的功能塊之間依舊存在著復雜的邏輯關系。

圖1 功能塊模型

對于功能塊而言，只有其輸入和輸出變量是可訪問的。換句話說，功能塊內部變量、數據結構、邏輯控制是不透明的，用戶只需要根據功能塊內部算法所需要輸入的速度、加速度、位移等參數進行輸入配置即可，既能方便操作，也能保護運動控制功能塊開發人員的知識產權。

1.2 軸組狀態機的設計

在運動控制系統中的運動命令總是按照順序給出，并會根據狀態機作用于運動軸上，而運動軸始終處于狀態機所定義的某種狀態。單軸控制系統中軸狀態機的狀態切換是以單個軸為控制對象，而軸組也擁有自身的狀態機，并且軸組內的單個軸也有自身的狀態機，兩者相互獨立卻也有關聯。

如圖2所示為PLCopen定義的軸組狀態機。PLCopen將軸組狀態機分為6個部分，在系統上電后，系統處于軸組禁用狀態，通過調用MC_GroupEnable或者MC_GroupDisable狀態機可以在軸組禁用以及軸組待機相互切換，并且在這兩種狀態下，都可以執行將運動軸加入軸組或者移出軸組的操作。軸組功能塊只要是執行運動類功能塊都將進入軸組運動中狀態。軸組回零狀態展示的是一系列的回零操作被執行的過程。在軸組運動或者回零過程中，通過使能MC_GroupStop功能塊，運動或者回零過程會被立即打斷，狀態機會切換至軸組停止過程，并且在軸組停止的過程中任何功能塊都不可以打斷它，直到停止完成后，狀態機自行切換至軸組待機。當軸組內任何一根軸出錯時，系統狀態會進入軸組出錯停止，只有調用MC_GroupReset功能塊才能離開該狀態。

圖2 軸組狀態機

1.3 運動功能塊的設計

通過C/C++將直線插補與圓弧插補算法寫入軸組直線運動與圓弧運動功能塊，并且調用相關功能塊便可實現相對應的功能。本文功能塊以絕對位置為例，基于PLCopen設計的功能塊如圖3、圖4所示。在圖3所示的直線插補功能塊中，輸入輸出引腳外部為變量數據類型，內部為變量名稱。輸入變量有儲存數據狀態的軸組號、使能端、多維數組構成的目標位置、速度、加速度、減速度、加加速度、參考坐標系、緩沖模式、過渡模式、過渡參數。其中過渡模式與過渡參數并不常用，本文沒有針對這兩個輸入作用進行多余闡述。輸出變量主要是展示功能塊運行狀態的標志位，包括完成位、忙碌位、激活位、打斷位、錯誤位以及錯誤ID號。

圖3 直線運動功能塊

圖4所示的圓弧插補功能塊與直線插補功能塊在輸入接口定義上有稍許不同，增加了圓弧模式、輔助點、終止點、圓弧方向選擇。在不同圓弧模式下輔助點的定義不同，圓心模式下輔助點為圓心點而邊界模式下輔助點為邊界點，在圓心模式下需要選擇插補方向而在邊界點模式下圓弧插補方向在功能塊初始化狀態下即被確定。

圖4 圓弧運動功能塊

2 軸組協調運動算法

2.1 直線插補

為了滿足待插補線段具有任意性的特點，插補首點一般不是軸組的起始點。軸組需要先執行起始點到插補首點的直線插補，再執行待插補線段的插補操作如圖5所示。以插補首點到插補終點的過程為例，在開始插補前，需要將坐標系的參考點轉換為a點。以插補首點為原點建立直角坐標系，如果插補終點b在坐標系的第一、四象限，由式(1)算出ab與X軸正方向的夾角θ1；如果插補終點b在a點第二、三象限，θ為ab與X軸負方向的夾角。

在ab段的插補過程中，可以將插補線段分割成一系列的小段，每小段分別對應軸組每個運動周期內的位移，每段位移量由每個運動周期的速度控制，通過將速度V沿X軸與Y軸分解，由式(2)算出Vx與Vy來分別執行每一軸的運動插補，通過判斷插補終點在插補首點的方向，分解軸將向判斷后的位置進行運動，軸組最終停在插補終點。所以只需要通過規劃速度即可實現ab段的插補。

直線插補中需要注意的是由于插補點位置的數據類型為整形，而規劃速度算法的精確性決定了插補脈沖的數據類型為浮點型。這就導致了在插補過程中會遺漏掉許多小數位的脈沖當量，造成插補算法的不精確。本文采用的解決辦法為記錄每一次的插補脈沖，分別提取整數位與小數位，將整數位的脈沖作為插補點位置脈沖進行發送而小數位的數據則加入下一次的插補脈沖中一同發送，可以保證插補數據基本不丟失。

圖5 直線插補示意圖

2.2 圓弧插補

相比較直線插補，圓弧插補的實現比較復雜，需要將一段圓弧轉化成若干微小線段，在每個運動周期內，軸組進行直線運動，每個細小線段的最終組合逼近理論圓弧。常用的圓弧插補算法有逐點比較法、弦線法、DDA算法。本文采用改進的DDA算法，即軸組沿著圓弧切線方向進給，插補速度由角度控制，并在插補終點通過角度判別來判斷是否插補結束。

在二維坐標系下，插補圓弧的選擇通常可分為中心點模式與邊界點模式。中心點模式即給定圓弧的中心點與插補終點，同時需要規定圓弧的插補方向，考慮到中心點可能與物體發生碰撞，所以一般情況下不能用于示教。而邊界點模式則需要給定圓弧的邊界點與插補終點，圓弧插補方向沿著起始點到邊界點再到插補終點，由于圓弧可以抵達邊界點，所以這種模式可以用于示教。本文采用邊界點模式進行圓弧插補。

如圖6所示為圓弧插補示意圖，插補的主要過程為：1）分別算出起始點F與插補終點B與變換坐標系后原點O相對于X′正方向的角度，分別記為θF、θB，并且始終保證角度控制在[0,2π]，通過FA與AB的矢量積確定圓弧插補方向。2）插補過程中，每進行一次插補操作都需要由式(3)算出運動點此時的插補角度θ，其中：XAct為軸組實際橫坐標，XO為圓弧中心點的橫坐標位置。

4）當軸組切線速度V=VMax時，得到加速過程中的角度θAcc，如果進行逆時針插補，當插補角度時，進行減速運動；如果進行順時針插補，當插補角度進行減速運動。

圖6 圓弧插補示意圖

3 軸組功能塊的驗證

本文通過德國KW公司提供的編程環境Multiprog平臺來調用所編寫的軸組功能塊，驗證所開發的直線插補與圓弧插補的準確性。整個實驗平臺如圖7，PC機與運動控制器通過以太網連接，運動控制器采用ARMCortex A9開發板，底層執行WinCE的操作系統，在嵌入式系統運行時，應用層會執行eCLR系統，這種系統可以將微軟通用的中間語言代碼轉換成嵌入式CPU所能執行的機器碼，縮短了應用程序的執行時間。運動控制器作為EtherCAT主站，與作為從站的伺服驅動器的通訊周期為2ms。并且將EtherCAT通訊周期作為事件任務與Multiprog相關聯，即EtherCAT每進行一次數據傳輸，Multiprog內與這個事件任務相關聯的實例化功能塊被執行一次。實現了通訊周期與運動控制器的執行周期同步。

圖7 實驗平臺

為了配合軸組進行插補運動，還需要調用一些管理類功能塊，如MC_ReadAxisInfo、MC_WriteAxisInfo、MC_GroupEnable、 MC_GroupStop，分別代表讀取軸信息、寫軸信息、軸組使能、軸組停止。本文通過調用一個軸組直線插補功能塊與兩個圓弧插補功能塊，來實現功能塊之間的協同工作，功能塊主要參數設置如表1、表2、表3所示，其中位置點的單位為unit，速度單位為unit/s，加、減速度的單位為unit/s2。

軸組功能塊坐標系CoordSystem選擇MCS代表機器坐標系，功能塊之間的打斷模式BufferMode設置為Aborting，表示想要執行的運動功能塊在使能后可以打斷現有的運動功能塊。Multiprog內置邏輯分析儀，可以實時獲取運動數據，并將數據保存在文件內并用MATLAB繪制插補軌跡來分析所設計的功能塊是否滿足設計標準。單軸運動曲線及運動空間曲線分別如圖8、圖9所示，插補算法可以準確到達插補位置。

表1 圓弧功能塊1參數

表2 直線功能塊參數

表3 圓弧功能塊2參數

圖8 單軸運動曲線

圖9 軸組空間運動曲線

4 結束語

本文將運動控制算法模塊化，設計了軸組運動控制功能塊模型。在Visual Studio 2015中通過C#開發功能塊的輸入輸出接口以及功能塊類型，再利用C++將運動控制算法封裝進功能塊內，在上位界面Multiprog中調用功能塊并在ARM-Cortex A9開發板內執行運動控制程序，最終將軸組運動數據導入MATLAB中繪制運動軌跡。實驗結果表明所設計的軸組運動功能塊符合PLCopen標準的要求，這種以功能塊的形式來進行運動控制可以被應用于傳統工業機器人和CNC領域內，并且維護方便、可靠性高。