(合肥工業(yè)大學(xué),合肥 230009)
隨著工業(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),具有高移植性。
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)。
在運(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ī)
通過(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)功能塊
為了滿足待插補(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ǔ)示意圖
相比較直線插補(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ǔ)示意圖
本文通過(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)曲線
本文將運(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ù)方便、可靠性高。