基于PLCopen的軸組協(xié)調(diào)運(yùn)動(dòng)功能塊的設(shè)計(jì)與實(shí)現(xiàn)

（合肥工業(yè)大學(xué)，合肥 230009）

0 引言

隨著工業(yè)化的發(fā)展，一大批運(yùn)動(dòng)控制器涌現(xiàn)出來(lái)。許多運(yùn)動(dòng)控制器中都集成有PLC的功能，可以實(shí)現(xiàn)一些簡(jiǎn)單的控制邏輯。但是在擁有眾多I/0點(diǎn)的復(fù)雜運(yùn)動(dòng)控制系統(tǒng)中，這些簡(jiǎn)單的PLC功能無(wú)法滿足控制要求。因此，PLC和運(yùn)動(dòng)控制往往是獨(dú)立的兩套系統(tǒng)，兩者之間可以通過(guò)I/O接口進(jìn)行數(shù)據(jù)交互實(shí)現(xiàn)同步。這就導(dǎo)致了系統(tǒng)設(shè)計(jì)十分復(fù)雜。

PLCopen組織將PLC與運(yùn)動(dòng)控制結(jié)合起來(lái)，實(shí)現(xiàn)了PLC編程語(yǔ)言標(biāo)準(zhǔn)化，并推出運(yùn)動(dòng)控制功能塊標(biāo)準(zhǔn)。標(biāo)準(zhǔn)化的功能塊僅僅定義到接口和數(shù)據(jù)結(jié)構(gòu)層面，功能塊內(nèi)部算法實(shí)現(xiàn)由各個(gè)廠商自行定義，采用不同功能塊的組合，可以滿足眾多應(yīng)用場(chǎng)合的需求。目前PLCopen的MC運(yùn)動(dòng)控制規(guī)范定義的功能塊主要分為用于單軸運(yùn)動(dòng)的Part1部分，用于電子齒輪、電子凸輪的Part2部分以及軸組協(xié)調(diào)運(yùn)動(dòng)的Part4部分。

由于控制單軸的運(yùn)動(dòng)控制系統(tǒng)已被廣泛實(shí)現(xiàn)，而針對(duì)軸組協(xié)調(diào)運(yùn)動(dòng)的軌跡規(guī)劃描述較少，本文利用菲尼克斯提供的SoftPLC，所調(diào)用功能塊的邏輯關(guān)系滿足PLCopen定義的軸組狀態(tài)機(jī)狀態(tài)的切換，并通過(guò)編寫(xiě)軸組直線插補(bǔ)與圓弧插補(bǔ)算法，驗(yàn)證所設(shè)計(jì)的功能塊可以被應(yīng)用于傳統(tǒng)工業(yè)機(jī)器人和CNC領(lǐng)域內(nèi)，具有高移植性。

1 PLCopen軸組功能塊的設(shè)計(jì)

1.1 功能塊模型

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

圖1 功能塊模型

對(duì)于功能塊而言，只有其輸入和輸出變量是可訪問(wèn)的。換句話說(shuō)，功能塊內(nèi)部變量、數(shù)據(jù)結(jié)構(gòu)、邏輯控制是不透明的，用戶只需要根據(jù)功能塊內(nèi)部算法所需要輸入的速度、加速度、位移等參數(shù)進(jìn)行輸入配置即可，既能方便操作，也能保護(hù)運(yùn)動(dòng)控制功能塊開(kāi)發(fā)人員的知識(shí)產(chǎn)權(quán)。

1.2 軸組狀態(tài)機(jī)的設(shè)計(jì)

在運(yùn)動(dòng)控制系統(tǒng)中的運(yùn)動(dòng)命令總是按照順序給出，并會(huì)根據(jù)狀態(tài)機(jī)作用于運(yùn)動(dòng)軸上，而運(yùn)動(dòng)軸始終處于狀態(tài)機(jī)所定義的某種狀態(tài)。單軸控制系統(tǒng)中軸狀態(tài)機(jī)的狀態(tài)切換是以單個(gè)軸為控制對(duì)象，而軸組也擁有自身的狀態(tài)機(jī)，并且軸組內(nèi)的單個(gè)軸也有自身的狀態(tài)機(jī)，兩者相互獨(dú)立卻也有關(guān)聯(lián)。

如圖2所示為PLCopen定義的軸組狀態(tài)機(jī)。PLCopen將軸組狀態(tài)機(jī)分為6個(gè)部分，在系統(tǒng)上電后，系統(tǒng)處于軸組禁用狀態(tài)，通過(guò)調(diào)用MC_GroupEnable或者M(jìn)C_GroupDisable狀態(tài)機(jī)可以在軸組禁用以及軸組待機(jī)相互切換，并且在這兩種狀態(tài)下，都可以執(zhí)行將運(yùn)動(dòng)軸加入軸組或者移出軸組的操作。軸組功能塊只要是執(zhí)行運(yùn)動(dòng)類(lèi)功能塊都將進(jìn)入軸組運(yùn)動(dòng)中狀態(tài)。軸組回零狀態(tài)展示的是一系列的回零操作被執(zhí)行的過(guò)程。在軸組運(yùn)動(dòng)或者回零過(guò)程中，通過(guò)使能MC_GroupStop功能塊，運(yùn)動(dòng)或者回零過(guò)程會(huì)被立即打斷，狀態(tài)機(jī)會(huì)切換至軸組停止過(guò)程，并且在軸組停止的過(guò)程中任何功能塊都不可以打斷它，直到停止完成后，狀態(tài)機(jī)自行切換至軸組待機(jī)。當(dāng)軸組內(nèi)任何一根軸出錯(cuò)時(shí)，系統(tǒng)狀態(tài)會(huì)進(jìn)入軸組出錯(cuò)停止，只有調(diào)用MC_GroupReset功能塊才能離開(kāi)該狀態(tài)。

圖2 軸組狀態(tài)機(jī)

1.3 運(yùn)動(dòng)功能塊的設(shè)計(jì)

通過(guò)C/C++將直線插補(bǔ)與圓弧插補(bǔ)算法寫(xiě)入軸組直線運(yùn)動(dòng)與圓弧運(yùn)動(dòng)功能塊，并且調(diào)用相關(guān)功能塊便可實(shí)現(xiàn)相對(duì)應(yīng)的功能。本文功能塊以絕對(duì)位置為例，基于PLCopen設(shè)計(jì)的功能塊如圖3、圖4所示。在圖3所示的直線插補(bǔ)功能塊中，輸入輸出引腳外部為變量數(shù)據(jù)類(lèi)型，內(nèi)部為變量名稱。輸入變量有儲(chǔ)存數(shù)據(jù)狀態(tài)的軸組號(hào)、使能端、多維數(shù)組構(gòu)成的目標(biāo)位置、速度、加速度、減速度、加加速度、參考坐標(biāo)系、緩沖模式、過(guò)渡模式、過(guò)渡參數(shù)。其中過(guò)渡模式與過(guò)渡參數(shù)并不常用，本文沒(méi)有針對(duì)這兩個(gè)輸入作用進(jìn)行多余闡述。輸出變量主要是展示功能塊運(yùn)行狀態(tài)的標(biāo)志位，包括完成位、忙碌位、激活位、打斷位、錯(cuò)誤位以及錯(cuò)誤ID號(hào)。

圖3 直線運(yùn)動(dòng)功能塊

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

圖4 圓弧運(yùn)動(dòng)功能塊

2 軸組協(xié)調(diào)運(yùn)動(dòng)算法

2.1 直線插補(bǔ)

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

在ab段的插補(bǔ)過(guò)程中，可以將插補(bǔ)線段分割成一系列的小段，每小段分別對(duì)應(yīng)軸組每個(gè)運(yùn)動(dòng)周期內(nèi)的位移，每段位移量由每個(gè)運(yùn)動(dòng)周期的速度控制，通過(guò)將速度V沿X軸與Y軸分解，由式(2)算出Vx與Vy來(lái)分別執(zhí)行每一軸的運(yùn)動(dòng)插補(bǔ)，通過(guò)判斷插補(bǔ)終點(diǎn)在插補(bǔ)首點(diǎn)的方向，分解軸將向判斷后的位置進(jìn)行運(yùn)動(dòng)，軸組最終停在插補(bǔ)終點(diǎn)。所以只需要通過(guò)規(guī)劃速度即可實(shí)現(xiàn)ab段的插補(bǔ)。

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

圖5 直線插補(bǔ)示意圖

2.2 圓弧插補(bǔ)

相比較直線插補(bǔ)，圓弧插補(bǔ)的實(shí)現(xiàn)比較復(fù)雜，需要將一段圓弧轉(zhuǎn)化成若干微小線段，在每個(gè)運(yùn)動(dòng)周期內(nèi)，軸組進(jìn)行直線運(yùn)動(dòng)，每個(gè)細(xì)小線段的最終組合逼近理論圓弧。常用的圓弧插補(bǔ)算法有逐點(diǎn)比較法、弦線法、DDA算法。本文采用改進(jìn)的DDA算法，即軸組沿著圓弧切線方向進(jìn)給，插補(bǔ)速度由角度控制，并在插補(bǔ)終點(diǎn)通過(guò)角度判別來(lái)判斷是否插補(bǔ)結(jié)束。

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

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

4）當(dāng)軸組切線速度V=VMax時(shí)，得到加速過(guò)程中的角度θAcc，如果進(jìn)行逆時(shí)針插補(bǔ)，當(dāng)插補(bǔ)角度時(shí)，進(jìn)行減速運(yùn)動(dòng)；如果進(jìn)行順時(shí)針插補(bǔ)，當(dāng)插補(bǔ)角度進(jìn)行減速運(yùn)動(dòng)。

圖6 圓弧插補(bǔ)示意圖

3 軸組功能塊的驗(yàn)證

本文通過(guò)德國(guó)KW公司提供的編程環(huán)境Multiprog平臺(tái)來(lái)調(diào)用所編寫(xiě)的軸組功能塊，驗(yàn)證所開(kāi)發(fā)的直線插補(bǔ)與圓弧插補(bǔ)的準(zhǔn)確性。整個(gè)實(shí)驗(yàn)平臺(tái)如圖7，PC機(jī)與運(yùn)動(dòng)控制器通過(guò)以太網(wǎng)連接，運(yùn)動(dòng)控制器采用ARMCortex A9開(kāi)發(fā)板，底層執(zhí)行WinCE的操作系統(tǒng)，在嵌入式系統(tǒng)運(yùn)行時(shí)，應(yīng)用層會(huì)執(zhí)行eCLR系統(tǒng)，這種系統(tǒng)可以將微軟通用的中間語(yǔ)言代碼轉(zhuǎn)換成嵌入式CPU所能執(zhí)行的機(jī)器碼，縮短了應(yīng)用程序的執(zhí)行時(shí)間。運(yùn)動(dòng)控制器作為EtherCAT主站，與作為從站的伺服驅(qū)動(dòng)器的通訊周期為2ms。并且將EtherCAT通訊周期作為事件任務(wù)與Multiprog相關(guān)聯(lián)，即EtherCAT每進(jìn)行一次數(shù)據(jù)傳輸，Multiprog內(nèi)與這個(gè)事件任務(wù)相關(guān)聯(lián)的實(shí)例化功能塊被執(zhí)行一次。實(shí)現(xiàn)了通訊周期與運(yùn)動(dòng)控制器的執(zhí)行周期同步。

圖7 實(shí)驗(yàn)平臺(tái)

為了配合軸組進(jìn)行插補(bǔ)運(yùn)動(dòng)，還需要調(diào)用一些管理類(lèi)功能塊，如MC_ReadAxisInfo、MC_WriteAxisInfo、MC_GroupEnable、 MC_GroupStop，分別代表讀取軸信息、寫(xiě)軸信息、軸組使能、軸組停止。本文通過(guò)調(diào)用一個(gè)軸組直線插補(bǔ)功能塊與兩個(gè)圓弧插補(bǔ)功能塊，來(lái)實(shí)現(xiàn)功能塊之間的協(xié)同工作，功能塊主要參數(shù)設(shè)置如表1、表2、表3所示，其中位置點(diǎn)的單位為unit，速度單位為unit/s，加、減速度的單位為unit/s2。

軸組功能塊坐標(biāo)系CoordSystem選擇MCS代表機(jī)器坐標(biāo)系，功能塊之間的打斷模式BufferMode設(shè)置為Aborting，表示想要執(zhí)行的運(yùn)動(dòng)功能塊在使能后可以打斷現(xiàn)有的運(yùn)動(dòng)功能塊。Multiprog內(nèi)置邏輯分析儀，可以實(shí)時(shí)獲取運(yùn)動(dòng)數(shù)據(jù)，并將數(shù)據(jù)保存在文件內(nèi)并用MATLAB繪制插補(bǔ)軌跡來(lái)分析所設(shè)計(jì)的功能塊是否滿足設(shè)計(jì)標(biāo)準(zhǔn)。單軸運(yùn)動(dòng)曲線及運(yùn)動(dòng)空間曲線分別如圖8、圖9所示，插補(bǔ)算法可以準(zhǔn)確到達(dá)插補(bǔ)位置。

表1 圓弧功能塊1參數(shù)

表2 直線功能塊參數(shù)

表3 圓弧功能塊2參數(shù)

圖8 單軸運(yùn)動(dòng)曲線

圖9 軸組空間運(yùn)動(dòng)曲線

4 結(jié)束語(yǔ)

本文將運(yùn)動(dòng)控制算法模塊化，設(shè)計(jì)了軸組運(yùn)動(dòng)控制功能塊模型。在Visual Studio 2015中通過(guò)C#開(kāi)發(fā)功能塊的輸入輸出接口以及功能塊類(lèi)型，再利用C++將運(yùn)動(dòng)控制算法封裝進(jìn)功能塊內(nèi)，在上位界面Multiprog中調(diào)用功能塊并在ARM-Cortex A9開(kāi)發(fā)板內(nèi)執(zhí)行運(yùn)動(dòng)控制程序，最終將軸組運(yùn)動(dòng)數(shù)據(jù)導(dǎo)入MATLAB中繪制運(yùn)動(dòng)軌跡。實(shí)驗(yàn)結(jié)果表明所設(shè)計(jì)的軸組運(yùn)動(dòng)功能塊符合PLCopen標(biāo)準(zhǔn)的要求，這種以功能塊的形式來(lái)進(jìn)行運(yùn)動(dòng)控制可以被應(yīng)用于傳統(tǒng)工業(yè)機(jī)器人和CNC領(lǐng)域內(nèi)，并且維護(hù)方便、可靠性高。