,,
(浙江工業(yè)大學(xué) 信息工程學(xué)院, 杭州 310023)
近幾年來,隨著德國“工業(yè)4.0”和“中國制造2025”的先后提出,“智能制造”已經(jīng)成為全球工業(yè)發(fā)展新的代名詞。工業(yè)機器人作為智能制造的重要組成部分,將在“智慧工廠”、“智能物流”、“全自動化流水線”等智能制造領(lǐng)域扮演著重要角色。而運動控制技術(shù)作為工業(yè)機器人的核心技術(shù),直接影響著工業(yè)機器人技術(shù)的發(fā)展,因此,發(fā)展和推進工業(yè)機器人運動控制技術(shù)具有十分重要的實踐意義[1-3]。
目前,工業(yè)機器人的運動控制基本分為兩種方式。一是采用專用的控制器,專用的編程語言,即采用原始設(shè)備制造商(original equip ment manufactures,OEM)方式。各個機器人廠商有自己專用的編程語言[4-5]。例如,瑞士的ABB工業(yè)機器人采用RAPID編程語言,S?ubli機器人使用VAL3語言,德國的庫卡機器人采用KRL編程語言,日本的安川機器人采用INFORM語言,發(fā)那科機器人則使用Karel編程語言。
另一種是基于PLC的運動控制方式。采用符合PLCopen標準的運動控制功能塊作為編程控制基礎(chǔ)。這種控制方式不受各種軟硬件平臺的限制,具有開放性、易編程性、可移植性、通用性和可擴展性等特點[4-5]。如3S公司的CodeSys、Beckhoff公司的TwinCAT、KW公司的MultiProg、固高公司的OtoStudio等開發(fā)環(huán)境均是基于此種編程方式開發(fā)的。目前,南京航空航天大學(xué)張得禮教授團隊采用工控機+PMAC運動控制卡的架構(gòu)在Codesys環(huán)境下基于PLCopen軸組方式對運動控制功能塊進行研究與開發(fā)[6-7],華中科技大學(xué)唐小琦教授團隊結(jié)合IEC61131-3語言及PLCopen標準,基于Beremiz開源平臺研發(fā)了應(yīng)用于數(shù)控機床控制系統(tǒng)的運動控制功能塊[8-9]。華南理工大學(xué)李迪教授團隊采用DSP+FPGA為架構(gòu)的方案基于PLCopen對單軸及多軸運動控制功能塊進行了研究[10-12]。
基于以上兩種運動控制編程方式的各自特點,本文選擇采用通用性和可擴展性更好的基于PLCopen標準的運動控制編程方式,采用ARM+Linux+RunTime System的硬件控制方案,以3S公司的CodeSys作為機器人控制系統(tǒng)的軟件開發(fā)環(huán)境,以PLCopen標準的運動控制功能塊為基礎(chǔ),結(jié)合SoftMotionCNC運動控制庫開發(fā)出六軸工業(yè)機器人運動控制功能塊,包含:坐標變換、正逆運動變換、直線與圓弧插補運動等,并在此基礎(chǔ)上開發(fā)出系列運動控制指令,包括運動控制類、位置偏移類、回零、急停和復(fù)位等指令,同時設(shè)計出用于人機交互的可視化編程控制界面。最后,以埃夫特ER50機器人為實驗對象,采用研制的機器人控制器,對開發(fā)的運動控制功能塊及指令進行實驗驗證,結(jié)果表明,功能塊及指令執(zhí)行正確,滿足六軸工業(yè)機器人運動控制的要求。
整個六軸工業(yè)機器人控制系統(tǒng)采用一主多從的控制模式,主站采用ARM+Linux控制方案,從站采用帶EtherCAT接口的清能德創(chuàng)CoolA8系列伺服驅(qū)動,主站與從站、從站與從站之間采用工業(yè)以太網(wǎng)EtherCAT技術(shù)進行通訊,軟件平臺以Codesys 3.5sp10作為開發(fā)環(huán)境,以ST和CFC作為主要編程語言進行機器人運動控制功能塊的開發(fā)以及機器人控制算法的開發(fā),機器人控制系統(tǒng)結(jié)構(gòu)圖如圖1所示。
圖1 機器人控制系統(tǒng)結(jié)構(gòu)圖
PLCopen組織自成立以來共發(fā)布了六個部分控制標準,分別為運動控制庫、擴展、用戶導(dǎo)則、運動控制協(xié)調(diào)、回零功能、液壓驅(qū)動擴展。這六個部分構(gòu)成了PLCopen運動控制的基礎(chǔ)。第一部分主要介紹了定義的運動控制功能塊庫,包括單軸功能塊和部分多軸功能塊,并定義了功能塊的狀態(tài)圖,第二部分對第一部分的功能塊庫進行了補充,添加了部分單軸功能塊,并進一步完善了功能塊的狀態(tài)圖,第三部分列舉了數(shù)個使用PLCopen功能塊做控制系統(tǒng)的實例,并對其做了詳細說明,第四部分介紹了軸組功能塊及其狀態(tài)圖,第五部分詳細介紹了單個運動軸的回零原理及常見回零方式,并定義了部分回零功能塊,第六部分介紹了流體液壓驅(qū)動設(shè)備功能塊。
本文以一、二、四部分功能塊為基礎(chǔ),結(jié)合SMC_CNC庫中的數(shù)組、結(jié)構(gòu)體和功能塊,開發(fā)六軸工業(yè)機器人運動控制功能塊及系列運動控制指令,通過從可視化界面輸入控制指令調(diào)用運動控制功能塊達到控制機器人運動的目的,最后,在CodeSys開發(fā)環(huán)境中開發(fā)可視化編程界面,用于實現(xiàn)指令的輸入和對機器人運動控制的操作,其軟件結(jié)構(gòu)框圖如圖2所示。
圖2 機器人運動控制軟件結(jié)構(gòu)圖
由圖2可知,機器人運動控制軟件結(jié)構(gòu)可分為四層,包括PLCopen基礎(chǔ)庫SMC_CNC、六軸工業(yè)機器人運動控制功能塊、機器人運動控制指令以及可視化編程界面?;A(chǔ)庫SMC_CNC中,包含數(shù)組、結(jié)構(gòu)體和功能塊,其中,結(jié)構(gòu)體SMC_OUTQUEUE包含多個路徑對象結(jié)構(gòu)體SMC_GEOINFO、SMC_GEOINF O包含四個數(shù)組,分別為SMC_ POSINFO、SM C_DIRECTION、SMC_MOVTYP、SMC_VE CTOR3D,庫中的功能塊對結(jié)構(gòu)體和數(shù)組進行調(diào)用,如示教變換類功能塊調(diào)用SMC_VECT OR3D,插補控制類功能塊調(diào)用結(jié)構(gòu)體SMC_OUTQUEUE,正逆運動學(xué)變換功能塊調(diào)用結(jié)構(gòu)SMC_Trafo_Articulated Robot_6DOF數(shù)組SMC_POSINFO。六軸工業(yè)機器人功能塊層
包含位置變換類、正逆運動變換類、直線和圓弧插補類運動控制功能塊。機器人運動控制指令層包含運動控制類、位置偏移類、回零、復(fù)位等指令,通過對上層輸入的文本解析,并與指令系統(tǒng)中的指令匹配后,調(diào)用下層對應(yīng)的功能塊實現(xiàn)對機器人的控制??梢暬幊探缑鎸佑糜谥噶畛绦虻妮斎牒腿藱C交互。
在CodeSys開發(fā)環(huán)境下,添加SMC_CNC功能塊庫到庫管理器中,作為設(shè)計六軸工業(yè)機器人運動控制功能塊的基礎(chǔ)庫,庫中常用數(shù)組、結(jié)構(gòu)體如表1所示,基礎(chǔ)功能塊如表2所示。
表1 SMC_CNC庫數(shù)據(jù)結(jié)構(gòu)
本文以上述數(shù)組、結(jié)構(gòu)體、功能塊為基礎(chǔ),開發(fā)了符合PLCopen標準的、適用于六軸工業(yè)機器人運動控制的功能塊,如表3所示,其中PCS表示工件坐標系,WCS表示世界坐標系。
表2 SMC_CNC庫功能塊
表3 六軸工業(yè)機器人功能塊介紹
下面以點到點運動控制功能塊和圓弧插補運動控制功能塊為例詳細介紹六軸工業(yè)機器人運動控制功能塊的設(shè)計流程。
機器人點到點運動控制功能塊的作用是控制機器人的工具中心點(tool center point,TCP)在空間中以最快的速度從起始點運動到目標點,它主要應(yīng)用于不需考慮TCP運動的軌跡形狀,而要求在最短時間內(nèi)到達的運動場合。設(shè)計的六軸工業(yè)機器人點到點運動控制功能塊流程如圖3所示。
圖3 點到點運動控制功能塊流程圖
(1)參照PLCopen功能塊標準,確定功能塊的輸入變量、輸出變量以及輸入輸出變量,然后對其進行定義和初始化,變量如圖4中接口所示。(2)通過調(diào)用正運動學(xué)功能塊MC_ Positive_kinematics_6Axis計算出當前機器人TCP的位姿值,將當前TCP的位姿作為機器人運動的起始點的位姿。接著根據(jù)起始位置和目標位置的數(shù)值,以及初始和結(jié)束時機器人TCP的速度均為零等約束條件,即可計算出機器人各軸運動的三次多項式函數(shù)的各項系數(shù),共12個數(shù)據(jù)。(3)將前面計算出的起始位姿、三次多項式參數(shù)、輸入的目標位姿、速度、加速度、 減加速度等數(shù)據(jù)分別賦值到路徑信息對象SMC_ GEOINFO對應(yīng)的參數(shù)piStartPos、dP1-dP12、piDestPos、dVel、dAcel、dDecel中,完成之后調(diào)用SMC_AppendObj()函數(shù)將該對象添加到結(jié)構(gòu)體SMC_OUTQUEUE中,將其作為路徑預(yù)處理對象。(4)調(diào)用SMC_CheckVelocities功能塊和SMC_Smooth Path功能塊分別對路徑對象的速度和位置進行平滑化處理,使得運動軌跡更加平滑,運動速度更加平穩(wěn)。(5)調(diào)用SMC_Interpolator功能塊對預(yù)處理之后的路徑對象進行路徑插補,通過插補功能塊可以將初始位姿和目標位姿之間的路徑插補出單獨的位置點。(6)調(diào)用設(shè)計的逆運動學(xué)功能塊MC_Inve rse_kinematics_6Axis,將(5)插補出來的位置點通過逆變換,得到一組關(guān)節(jié)角。(7)將關(guān)節(jié)角的值相對應(yīng)的輸入到功能塊SMC_ControlAxisBy Pos,從而控制各個關(guān)節(jié)軸的運動,最終實現(xiàn)點到點的插補運動,到達目標點的位置。
下面通過一個實例來描述功能塊在執(zhí)行過程中TCP的位置和速度、加速度、的變化,以及功能塊輸入輸出變量的狀態(tài)變化關(guān)系。調(diào)用兩個點到點運動控制功能塊,記為FB1和FB2, 將目標點位姿P1(1200, 100,-1000, 40, 50, 60)作為FB1的位姿輸入、P2(1400, 200,-1300, 70, 50, 30)作為FB2的輸入,然后將FB1.bDone與FB2.bExecute連接,使得功能塊FB1執(zhí)行完成后觸發(fā)FB2的執(zhí)行,從而實現(xiàn)機器人TCP從當前位置按點到點運動的方式運動到P1點,再到P2點。
通過使用CodeSys中數(shù)據(jù)跟蹤插件對運動過程中變量數(shù)據(jù)的變化進行實時跟蹤,程序運行中功能塊變量狀態(tài)時序圖及速度、加速度、位置曲線如圖4所示。
圖4 功能塊各變量數(shù)據(jù)曲線
當t=1.2 s時,F(xiàn)B1.bExecute由0變1,功能塊FB1開始執(zhí)行,F(xiàn)B1.bBusy狀態(tài)為真,機器人開始加速運動,TCP的位置開始變化;在t=1.7 s時,機器人到達第一個目標位置點P1,功能塊FB1執(zhí)行完成,此時FB1.bBusy狀態(tài)由1變0,F(xiàn)B1.bDone狀態(tài)由0變1,使得FB2. bExecute為1,F(xiàn)B2開始執(zhí)行,F(xiàn)B2.bBusy為1;直到t=2.4 s時,F(xiàn)B2.bDone為1,F(xiàn)B2.bBusy由1變0,機器人TCP到達第二個目標點P2,此時,速度、加速度等均減小至零,標志著功能塊FB2執(zhí)行完成。
機器人圓弧插補運動控制功能塊的作用是控制機器人TCP在空間中根據(jù)給定的中間點和目標點,以圓弧插補軌跡方式從起始點經(jīng)過中間點運動到目標點。根據(jù)空間中三點能唯一確定一個圓的數(shù)學(xué)原理,將機器人起始位置、輸入到功能塊的中間點位置和目標點位置坐標采用數(shù)學(xué)方法,唯一確定機器人在空間運動的圓弧軌跡,經(jīng)過插補算法,即可實現(xiàn)控制機器人TCP在空間中按照規(guī)劃好的圓弧軌跡方式運動。圓弧插補功能塊的流程如圖5所示。
圖5 圓弧插補運動控制功能塊流程圖
(1)定義功能塊的輸入變量、輸出變量以及輸入輸出變量。由于在空間中三點才能唯一確定一個圓,所以不僅要有目標點作為輸入,還要一個中間點作為輸入,其他變量與點到點運動控制功能塊保持一致。(2)調(diào)用正運動學(xué)功能塊MC_Positive_kinematics_6Axis,計算出機器人TCP當前所處位置的位姿,作為機器人運動的起始點。(3)根據(jù)輸入的中間點位置和目標點位置的數(shù)值,確定圓弧的圓心和半徑。由數(shù)學(xué)原理可知,空間三點可以構(gòu)成唯一的一個圓,該圓也稱三點連接的三角形的外接圓,而圓心就是該三角形的外心,半徑則是圓心與任意一點的距離,根據(jù)數(shù)學(xué)公式可求得圓心和半徑。(4)調(diào)用三點示教功能塊TeachFrame_6Axis,以圓心為新坐標系原點,圓心到起始點為ox軸方向,圓心、起始點、中間點、目標點所在平面為xoy平面,建立新的坐標系。(5)將新建立的坐標系記為PCS,則調(diào)用不同坐標系下位置坐標轉(zhuǎn)換功能塊WCSToPCS_6Axis,可以確定起始點、中間點、目標點在新坐標系下的坐標值。(6)根據(jù)起始點、中間點、目標點在新坐標系下的坐標值,可以確定其在新坐標系下的位置關(guān)系,從而確定圓弧軌跡的方向是順時針還是逆時針,根據(jù)運動方向,再確定圓弧所對應(yīng)的圓心角的大小,即圓弧運動的旋轉(zhuǎn)角度。(7)將以上步驟求得的圓心坐標、半徑值、軌跡方向、旋轉(zhuǎn)角度分別賦值給路徑對象SMC_GEO INFO中的參數(shù)(dP1、dP2、dP4)、dP3、iMoveType、dT2,將輸入的速度、加速度、減加速度參數(shù)分別賦值給dVel、dAccel、dDecel,將示教得出的坐標系列向量分別賦值給Vx、Vy、Vz,完成之后調(diào)用SMC_AppendObj()函數(shù)將該對象添加到結(jié)構(gòu)體SMC_ OUTQUEUE中,然后將其作為路徑預(yù)處理對象。接下來的步驟與點到點運動控制功能塊的設(shè)計步驟(4)~(7)完全一致,這樣整個圓弧插補運動控制功能塊的設(shè)計流程基本完成。
通過實例驗證設(shè)計的圓弧插補功能塊在控制機器人運動過程中,各輸入輸出狀態(tài)變量的變化情況,以及位置、速度、加速度等變化關(guān)系。通過調(diào)用兩個圓弧插補運動控制功能塊,分別為FB1和FB2,將FB1的執(zhí)行完成輸出變量bDone連接到FB2的輸入行變量bExecute,當FB1執(zhí)行完后,觸發(fā)FB2的執(zhí)行,從而控制機器人完成兩段圓弧軌跡的運行。
通過使用Codesys中數(shù)據(jù)跟蹤插件對運動過程中各變量數(shù)據(jù)進行實時跟蹤,得出各功能塊輸入輸出變量的時序圖和位置等變量的曲線如圖6所示,在t=1 s時,F(xiàn)B1.bExecute觸發(fā),F(xiàn)B1開始執(zhí)行,F(xiàn)B1.bBusy由0變1,機器人開始運動,速度、加速度,位置開始變化;在t=14 s時,機器人運動到第一個目標點P1,標志著功能塊FB1執(zhí)行完成,F(xiàn)B1.bDone變?yōu)?,同時觸發(fā)FB2執(zhí)行,此時FB2.bBusy由0變?yōu)?,速度、加速度等再次從零變?yōu)檎?,機器人開始加速運動;在t=20 s時,機器人運動到終點P2,F(xiàn)B2.bDone由0變?yōu)?,表示功能塊FB2執(zhí)行完成;在t=22 s時刻,將FB1.bExecute復(fù)位,所有變量全部復(fù)位為0,功能塊被重新初始化。
圖6 功能塊各變量變化曲線圖
開發(fā)機器人的運動控制指令及可視化編程界面是為了能夠直接而簡易的控制機器人的運動,同時也是為了能夠驗證設(shè)計的運動控制功能塊的可行性。通過在編程界面上輸入控制指令,在指令解析程序中將指令進行解析,調(diào)用開發(fā)的機器人運動控制功能塊,實現(xiàn)對機器人的運動控制。
機器人運動控制指令主要包括控制機器人進行點到點運動、直線插補運動、圓弧插補運動、TCP位置偏移、坐標系變換、回原點等。通過對機器人功能需求進行分析,開發(fā)了如表4所示控制指令。
表4 運動控制指令及功能
首先,通過調(diào)用CodeSys開發(fā)環(huán)境下的文件讀取函數(shù)SysFileRead(),讀取出從編程界面輸入到文本框中的指令字符,然后將字符以ASCII碼的形式存入到聲明的指定內(nèi)存中,通過指令解析程序,將內(nèi)存中的ASCII碼與設(shè)計的指令的ASCII碼一一匹配,一旦與設(shè)計的指令匹配成功,則調(diào)用指令對應(yīng)的運動控制功能塊,將指令后面的位置參數(shù)賦值給功能塊的輸入位置變量,然后執(zhí)行該功能塊,從而控制機器人運動到指定位置。
以movej指令為例,首先調(diào)用SysFileRead()函數(shù),讀取編程界面文件中的字符,以ASCII碼形式存入內(nèi)存Buffer中,調(diào)用指令解析程序,將內(nèi)存中的ASCII碼與movej每一個字符對應(yīng)的ASCII碼匹配,若匹配成功,則將指令后面括號中的參數(shù)賦值給點到點運動控制功能塊的目標位置輸入值fDesPos_dX、fDesPos_dY、fDesPos_dZ等,然后執(zhí)行功能塊MC_MovePTP _6Axis,控制機器人執(zhí)行點到點運動。
為了通過實驗驗證設(shè)計的運動控制指令及運動控制功能塊的可行性,開發(fā)了可視化編程界面,該界面由文本輸入框、文本顯示框、按鍵等可視化元素排列組成。文本輸入框用于運動控制指令程序的輸入,通過調(diào)用SysFileRead()函數(shù),讀取出指令程序字符,用于程序解析。界面右上角五個按鍵用于對文本文件的操作,分別關(guān)聯(lián)到文本輸入框的newfile、openfile、sa vefile、closefile、deletefile變量,右下角為文件顯示欄,顯示控制器機器人控制程序的文件名。底部五個按鍵中,編譯按鍵關(guān)聯(lián)指令解析程序的開始執(zhí)行變量start,運行按鍵關(guān)聯(lián)指令程序執(zhí)行變量run,用于執(zhí)行指令程序控制機器人運動,停止按鍵關(guān)聯(lián)程序停止變量stop,用于停止機器人運動。示教和返回按鍵用于切換可視化顯示界面。
以埃夫特ER50六軸工業(yè)機器人為實驗對象,驗證六軸工業(yè)機器人運動控制功能塊及運動控制指令功能。ER50機器人的D-H參數(shù)表如表5所示,依照ER50機器人模型建立連桿坐標系模型,如圖7所示。
表5 ER50機器人D-H參數(shù)表
圖7 六軸機器人連桿坐標系
通過在可視化編程界面上輸入指令程序movej(1200,100,-1000,30,40,50);movel(1400,300,-1500,60,30,20);movec(1000,200,-1000,30,50,60,1200,100,-1200,50,20,30); 即圖7界面中的程序段來控制機器人運動,通過在CodeSys開發(fā)環(huán)境下記錄的TCP位置信息數(shù)據(jù)通過Matlab合成的三維運動軌跡如圖8所示。
圖8 TCP運動軌跡合成曲線圖
如圖8所示,首先,機器人TCP以點到點方式從P0運動至P1點,接著以直線插補方式運動至P2點,最后機器人開始以圓弧插補的方式運動,到達P4點,軌跡曲線跟設(shè)計的程序控制機器人的功能完全一致。通過對機器人TCP的運動軌跡曲線圖的分析,驗證了設(shè)計的運動控制功能塊及控制指令對機器人的控制功能。
本文在研究PLCopen運動控制功能塊的基礎(chǔ)上,在CodeSys軟件開發(fā)環(huán)境下,采用符合IEC61131-3編程語言標準的ST和CFC編程語言,針對六軸工業(yè)機器人設(shè)計了系列運動控制功能塊,設(shè)計了系列運動控制指令,調(diào)用運動控制功能塊,實施對機器人的控制,并開發(fā)了可視化編程界面。以ER50六軸工業(yè)機器人作為實驗對象,搭建了運動控制系統(tǒng)測試實驗平臺,在可視化編程界面輸入運動控制指令,調(diào)用運動控制功能塊,實施對機器人的運動控制,通過機器人TCP軌跡運動控制實驗驗證了運動控制功能塊能夠?qū)αS工業(yè)機器人實現(xiàn)運動控制,達到了設(shè)計的目的,同時該運動控制功能塊具有良好的通用性和可擴展性,具有重要的應(yīng)用價值。