本發(fā)明涉及在cnc系統(tǒng)中加入一種用于直角坐標型機器人在高速運動過程中實現(xiàn)在線多級調(diào)整的方法，屬于電機運動控制領(lǐng)域，可提供一種實現(xiàn)在高速運動中保證系統(tǒng)實時性及魯棒性的電機高速運動控制方法，適應(yīng)各類需要隨著感知結(jié)果變化要進行在線的實時更新控制領(lǐng)域的需要。
背景技術(shù)：
：：長期以來，我國的大部分帶視覺控制的機器人都由國外引進，核心技術(shù)基本被國外品牌壟斷，并且對于機器人的研究大部分是針對于裝置結(jié)構(gòu)較為復(fù)雜，制造成本相對較高的機械臂，對于直角坐標型機器人的研究相對較少。隨著自動化控制的要求越來越高，對于傳統(tǒng)機器人提出了更高的要求。越來越多的企業(yè)需要精度高、成本低、環(huán)境適應(yīng)能力和可維護性強的機器人投入工業(yè)生產(chǎn)中去。相對于機械臂而言，直角坐標型機器人滿足了成本低以及可維護性強的要求，但一般的直角坐標型機器人主要以定點往復(fù)運動為主，無法實現(xiàn)當(dāng)出現(xiàn)運動終點位置發(fā)生改變時的參數(shù)實時在線調(diào)整，無法滿足日益復(fù)雜的生產(chǎn)環(huán)境需求，同時傳統(tǒng)的直角坐標型機器人在高速運動過程中的穩(wěn)定性也無法保證。解決該問題的方法之一是在直角坐標型機器人的基礎(chǔ)上結(jié)合機器視覺以提高對于動態(tài)環(huán)境的適應(yīng)性，如德國百格拉公司的直角坐標型機器人與德國visioncomponents公司的智能相機相結(jié)合的系統(tǒng)在工業(yè)上的成功運用。但是，用上述的系統(tǒng)需要從國外引進，需要支付高昂的購買費用和授權(quán)費用，無法被我國量大面廣的中小型企業(yè)所使用。因此我們提出了一種用于直角坐標型機器人在高速運動過程中實現(xiàn)在線多級調(diào)整的方法。在現(xiàn)有方法中，傳統(tǒng)的基于時間分割的插補方法以及自適應(yīng)的模糊pid在國內(nèi)外已有論述，但傳統(tǒng)的時間分割插補法無法適應(yīng)在一次運動過程中軌跡的多次在線的實時調(diào)整，而自適應(yīng)的模糊pid主要用于溫控系統(tǒng)，如本文針對直角坐標型機器人在高速運動過程中實現(xiàn)實時在線調(diào)整所運用的以自適應(yīng)的速度曲線規(guī)劃、基于變速的插補控制及自適應(yīng)的模糊pid反饋控制相結(jié)合的控制方法尚未出現(xiàn)。同時該方法的特點在于可以對運動過程的變化進行自適應(yīng)調(diào)節(jié)，提高了直角坐標型機器人的環(huán)境適應(yīng)性和穩(wěn)定性，可衍生并運用在例如工件定位，分類搬運等的工業(yè)生產(chǎn)中。技術(shù)實現(xiàn)要素：本發(fā)明的目的在于針對現(xiàn)有的控制方法的參數(shù)固定，無法做到運動的終點位置發(fā)生突變后的運動狀態(tài)在線實時更新及發(fā)生終點位置突變后系統(tǒng)會產(chǎn)生較大抖動的問題，提供了一種基于機器視覺的，在cnc系統(tǒng)中加入一種用于直角坐標型機器人在高速運動過程中實現(xiàn)在線多級調(diào)整的方法，保證了系統(tǒng)在高速運動過程中發(fā)生了終點位置突變后，可以對運動參數(shù)進行在線的實時更新，并保證系統(tǒng)在突變前后的平穩(wěn)過渡，以達到適應(yīng)不同環(huán)境的控制流程的需要。本發(fā)明的目的是通過以下技術(shù)方案來實現(xiàn)的：在非對稱、多內(nèi)核的控制器存儲空間內(nèi)劃分了一個與工控主機通信所專用的內(nèi)存空間，用于存放實時更新的終點位置坐標。在控制器內(nèi)增加了運動控制算法優(yōu)化模塊，實現(xiàn)了以自適應(yīng)的速度曲線規(guī)劃、基于變速的插補控制及自適應(yīng)的模糊pid反饋控制相結(jié)合的控制方法。用于直角坐標機器人在高速運動中實現(xiàn)在線多級調(diào)整方法，該方法為在傳統(tǒng)cnc系統(tǒng)中以非對稱、多內(nèi)核的控制器為基礎(chǔ)，增加了運動控制算法優(yōu)化模塊，實現(xiàn)了以自適應(yīng)的速度曲線規(guī)劃、基于變速的插補控制及自適應(yīng)的模糊pid反饋控制相結(jié)合的控制方法。所述的非對稱、多內(nèi)核的控制器，劃分一塊與工控主機通信所專用的內(nèi)存空間，用于存放實時更新的終點位置坐標；同時采用了以ipc通信機制、msgram為基礎(chǔ)，結(jié)合了中斷情況判斷、劃分數(shù)據(jù)修改請求動態(tài)優(yōu)先級的擁有時效性保證的變量共享機制，用于在非對稱、多內(nèi)核的核間通信方面確保實時系統(tǒng)的穩(wěn)定性。所述的運動控制算法優(yōu)化模塊，用于獲取實時的運動位置坐標及進行速度曲線規(guī)劃，進行插補運動并實現(xiàn)反饋控制。所述的自適應(yīng)的速度曲線規(guī)劃控制，通過倍頻采樣的方式從指定內(nèi)存中獲取實時的運動終點位置坐標，從而通過經(jīng)優(yōu)化后的速度規(guī)劃方法，得到自適應(yīng)的、實時的速度變化曲線。一旦發(fā)現(xiàn)兩次采樣得到的終點坐標差值大于閾值，則認為發(fā)生了終點變化，需要實時更新速度曲線。所述的優(yōu)化后的速度規(guī)劃方法，具體包括以下步驟：步驟(1)、初始化得到每條軸的電機所處的位置坐標、需要運動到達的終點坐標、目前電機的運動速度vs、電機的最大速度vmax及最大加速度amax；獲得終點位置坐標以及起始坐標后，得到各電機所需運動的距離s；由于目標軸的負載、可運動距離、電機功率的影響，會造成不同目標軸的電機所能達到的最大速度和最大加速度不同，因此需要在每次運動控制開始時進行測量和確定；步驟(2)、獲得每條軸的電機所能達到的最大進給率vmax，求得并初始化s型速度曲線中加加速時間t1，勻加速時間t2，減加速時間t3，加減速時間t5，勻減速時間t6，減減速時間t7為：步驟(3)、在得出各個時間段的初始值后，得到對應(yīng)加加速、勻加速、減加速、勻速、加減速、勻減速、減減速階段點的速度公式為：v2＝v1+jerk*t1*t2t1≤t＜t2公式(6)v4＝v3t3≤t＜t4公式(8)v6＝v5-jerk*t5*t6t5≤t＜t6公式(10)若要求各階段內(nèi)某一時刻的速度只需將對應(yīng)時間改為電機運行時間t；步驟(4)、在求出各階段時間后，得到在理想狀態(tài)下不包括勻速階段的位移s為：步驟(5)、將不包括勻速階段的位移s與實際所需要運動的距離進行比較，判斷是否存在勻速階段；具體情況可分為①完整的七段s型速度曲線；②不含勻速段的s型速度曲線；③不含勻速段和勻減速段的s型速度曲線；④不含勻減速段的s型速度曲線；⑤基本四段速度曲線；⑥不含勻加減速的s型速度曲線；⑦不含勻加速和勻速段的s型速度曲線；步驟(6)、在確定了一次運動周期的速度規(guī)劃曲線后，仍舊對指定存儲內(nèi)存進行定時采樣，若兩次所得的終點位置坐標差值大于設(shè)定閾值，則認為已發(fā)生終點位置變化，重新啟動規(guī)劃任務(wù)，將當(dāng)前的位置及速度作為初始化數(shù)據(jù)輸入，重新規(guī)劃新的運動速度曲線；若相差在閾值范圍內(nèi)，則只需將對應(yīng)時間輸入，可迅速輸出速度，縮短運算時間。所述的自適應(yīng)的模糊pid控制，引入了周期性采樣的環(huán)節(jié)，保證了當(dāng)發(fā)生速度改變時，可以較快地將其引入模糊pid控制器中進行運算。自適應(yīng)的模糊pid方法在保證運算效率的前提下，減少了普通pid方法的超調(diào)現(xiàn)象。本發(fā)明的有益效果如下：(1)通過采用基于非對稱、多內(nèi)核的控制器，實現(xiàn)了將周期性采樣，前饋控制與速度曲線規(guī)劃及模糊pid相結(jié)合的自適應(yīng)方法，達到了毫秒級的自適應(yīng)水平，用于解決現(xiàn)有的控制方法的參數(shù)固定，無法做到運動的終點位置發(fā)生突變后的運動狀態(tài)在線實時更新及發(fā)生終點位置突變后系統(tǒng)會產(chǎn)生較大抖動的問題。(2)非對稱、多內(nèi)核的運動控制器結(jié)構(gòu)設(shè)計新穎，本著利用最少的硬件資源以獲得最高效的、最快速的信息傳輸為目的進行設(shè)計，構(gòu)建多路并行通道以增強控制器交互的實時性，控制器留有多處測試點和拓展引腳使該控制器具有廣泛拓展應(yīng)用性。附圖說明圖1為本發(fā)明一種算法驅(qū)動電路控制器的結(jié)構(gòu)框圖圖2為本發(fā)明一種基于時間分割法的插補示意圖圖3為本發(fā)明一種簡單的模糊pid結(jié)構(gòu)設(shè)計示意圖圖4為本發(fā)明一種針對直角坐標型機器人的新型電機控制算法軟件流程圖具體實施方式以下將結(jié)合附圖對本發(fā)明作進一步說明。為了克服現(xiàn)有技術(shù)的缺陷，本發(fā)明提出了一種支持實現(xiàn)本發(fā)明中所述控制算法的非對稱、雙內(nèi)核的電機控制器。參見圖1，為本發(fā)明所采用控制器的結(jié)構(gòu)框圖，包括供電模塊、穩(wěn)壓模塊，主系統(tǒng)電路，通信模塊、電機驅(qū)動電路以及附屬外設(shè)vga顯示、pc端上位機，其中通信模塊用于數(shù)據(jù)的上傳存儲，主系統(tǒng)電路用于控制調(diào)配硬件資源，電機驅(qū)動電路用于操控電機。所述的電源模塊考慮到要具有足夠大的驅(qū)動電流，優(yōu)選的，我們選用工業(yè)用多路220v轉(zhuǎn)24v開關(guān)電源。該24v輸送至穩(wěn)壓模塊。所述的穩(wěn)壓模塊實現(xiàn)了將直流24v電壓降壓到12v、5v以及3.3v。將各電平輸送至主系統(tǒng)電路中給各自需求的芯片供電。所述的主系統(tǒng)電路目的實現(xiàn)數(shù)據(jù)的交互以及執(zhí)行相關(guān)控制指令，對實時和高速要求較高，優(yōu)選的，采用f28m35為主核心芯片進而搭建主系統(tǒng)電路。并且該系統(tǒng)電路通過rs232與vga顯示屏幕相連，所述vga顯示屏幕用于顯示小球軌跡與控制平臺。該系統(tǒng)電路還通過can總線與通信模塊相連，所述通信模塊為一轉(zhuǎn)換器，并與pc端上位機相連，所述上位機將坐標數(shù)據(jù)通過usb總線傳輸給通信模塊，通信模塊將其格式轉(zhuǎn)換后傳輸給系統(tǒng)電路。系統(tǒng)電路內(nèi)部產(chǎn)生pwm電機驅(qū)動脈沖，通過適當(dāng)接口將其傳輸給電機驅(qū)動電路，所述電機驅(qū)動電路用于驅(qū)動電機。為支撐所述本發(fā)明目的控制算法的實現(xiàn)，構(gòu)建了核心電機驅(qū)動電路，應(yīng)用了一款具有共享內(nèi)存的異步多核主控芯片，實現(xiàn)了基于ipc和msgram的多核協(xié)作機制，多核結(jié)構(gòu)上應(yīng)用了主從拓撲結(jié)構(gòu)，完美結(jié)合控制與計算寄存器目的使代碼執(zhí)行效率得到明顯提高。多核芯片其中的arm核主控制，主要起監(jiān)測和開關(guān)作用，負責(zé)描述此時的電機狀態(tài)并以此為依據(jù)根據(jù)本發(fā)明算法決定進一步的控制命令，另外該核還負責(zé)坐標數(shù)據(jù)的接收緩存。dsp核負責(zé)處理這種控制命令，為實施者，arm核發(fā)出的命令和提供的基礎(chǔ)數(shù)據(jù)通過多核芯片的一塊共享內(nèi)存區(qū)間進行與dsp核數(shù)據(jù)的快速交互，結(jié)合dsp核負責(zé)實現(xiàn)的反饋脈沖的精確計數(shù)，控制脈沖的精確輸出，為本發(fā)明算法的具體實現(xiàn)提供了最優(yōu)的硬件支持。參見圖4，本發(fā)明的另一個目的是針對運動控制的高速自適應(yīng)的速度曲線規(guī)劃、高速自適應(yīng)的運動插補控制及自適應(yīng)的模糊pid反饋控制等三個方面進行了研究，并提供了一種對高速插補運動過程中發(fā)生了終點位置突變時提高系統(tǒng)魯棒性的方法。應(yīng)用上述系統(tǒng)的算法實現(xiàn)包括如下步驟：步驟(1)、初始化得到每條軸的電機所處的位置坐標、需要運動到達的終點坐標、目前電機的運動速度vs以及電機可以達到的最大速度vmax及最大加速度amax等數(shù)據(jù)。獲得終點位置坐標以及起始坐標后，可以得到各電機所需運動的距離s。由于目標軸的負載、可運動距離、電機功率等因素的影響，會造成不同目標軸的電機所能達到的最大速度和最大加速度不同，因此需要在每次運動控制開始時進行測量和確定。步驟(2)、獲得每條軸的電機所能達到的最大進給率vmax，可以求得并初始化s型速度曲線中加加速時間t1，勻加速時間t2，減加速時間t3，加減速時間t5，勻減速時間t6，減減速時間t7為：步驟(3)、在得出各個時間段的初始值后，可以得到對應(yīng)加加速、勻加速、減加速、勻速、加減速、勻減速、減減速階段點的速度公式為(若要求各階段內(nèi)某一時刻的速度只需將對應(yīng)時間改為電機運行時間t即可)：v2＝v1+jerk*t1*t2t1≤t＜t2公式(6)v4＝v3t3≤t＜t4公式(8)v6＝v5-jerk*t5*t6t5≤t＜t6公式(10)步驟(4)、在求出各階段時間后，可以得到在理想狀態(tài)下不包括勻速階段的位移s為：步驟(5)、將不包括勻速階段的位移s與實際所需要運動的距離進行比較，判斷是否存在勻速階段。具體情況可分為①完整的七段s型速度曲線；②不含勻速段的s型速度曲線；③不含勻速段和勻減速段的s型速度曲線；④不含勻減速段的s型速度曲線；⑤基本四段速度曲線；⑥不含勻加減速的s型速度曲線；⑦不含勻加速和勻速段的s型速度曲線。步驟(6)、在規(guī)劃好一條加減速曲線后，以倍頻于插補周期的頻率對指定內(nèi)存進行采樣，若發(fā)現(xiàn)由can總線傳輸至內(nèi)存的終點坐標與上一周期的坐標差值大于閾值，則認為發(fā)生了終點位置的突變，需要將該時刻所處位置、運行速度以及終點坐標重新代入計算，得到新一輪的速度規(guī)劃曲線。步驟(7)、在確定好一個運動周期內(nèi)所需要達到的速度后，dsp核可以通過一個周期的時間和對應(yīng)的速度得到該周期所需運動的步長，對應(yīng)步驟為：首先要將空間曲線投影至三軸，得到三個電機需要移動的最終位移，再根據(jù)指令中的進給速度f(mm/min)計算出每一周期的輪廓步長δl，進而計算出各坐標軸進給量δx和δy，控制x和y坐標軸分別以速度為和進給，即可到達下一插補點，在每次插補判斷是否到達終點前后我們都要對由來自于相機的坐標信號進行采樣，分析判斷插補終點是否改變，采樣頻率約為插補頻率的3倍左右。若分析得終點坐標發(fā)生改變，則調(diào)整插補算法中的終點，若無變動，則以相同終點繼續(xù)插補不做任何改變。在時間分割直線插補法中以(xe,ye)為終點(本文為傳輸至電機控制芯片的落點坐標)，則可得到兩組推導(dǎo)公式：其插補示例如圖2所示，假設(shè)起點為ps(xs,ys,ys),終點為pe(xe,ye，ze)，插補周期為t(ms)，采樣周期與插補周期相同，速度為v(mm/ms)，可得：①程序段各坐標軸增量的計算為：δx＝xe-xs公式(14)δy＝y(tǒng)e-ys公式(15)δz＝ze-zs公式(16)②程序段直線長度的計算為：③插補進給量的計算為：④程序段各坐標軸步長的計算為：slx＝f×δx公式(19)sly＝f×δy公式(20)slz＝f×δz公式(21)⑤初始化終點判斷標識邏輯變量：若xe＝xs，則fx＝1；否則fx＝0若ye＝y(tǒng)s，則fy＝1；否則fy＝0若ze＝zs，則fz＝1；否則fz＝1⑥初始化終點判斷變量：如果xe≥xs，則sx＝1；否則sx＝-1如果ye≥ys，則sy＝1；否則sy＝-1如果ze≥zs，則sz＝1；否則sz＝-1⑦分速度：vx＝v×sinα公式(22)⑧速度比：步驟(8)、在確定了脈沖頻率后，dsp核在一個周期內(nèi)發(fā)送對應(yīng)脈沖個數(shù)，實際發(fā)送的脈沖個數(shù)由arm核進行計數(shù)，實際移動的脈沖個數(shù)由精度為17位的光電編碼器進行計數(shù)并反饋，從而得到在未達到對應(yīng)進給速度時與其的差值。在普通的pid反饋控制中，輸入因子和輸出的量化因子均是固定的，因此難以滿足較大范圍的控制需求，我們考慮到要在保持快速性和穩(wěn)定性的基礎(chǔ)上擴大可控范圍的需求，設(shè)計了將系統(tǒng)誤差e和誤差的變化量δe作為輸入因子的非線性模糊化控制器，同時采用了以查表方式獲得控制指令，進一步提高了處理速度，最后將經(jīng)過了模糊規(guī)則處理后的kp,ki,kd三個參數(shù)輸入普通pid控制器，其中模糊pid控制器的結(jié)構(gòu)設(shè)計如圖3所示。在控制規(guī)則中，偏差e取值為“pb”,“pm”,“ps”,“zo”,“ns”,“nm”,“nb”七值，δe取值范圍與e相同，根據(jù)蘊含式ife＝aiandδe＝bithenkp＝ci,kd＝di可得以下模糊控制規(guī)則表，如表1所示：表1模糊控制規(guī)則表tab.1tableoffuzzycontrolrules其中g(shù)1(e)＝1,g2(e)＝0.7,g3(e)＝0.35。經(jīng)研究驗證我們發(fā)現(xiàn)，在模糊自適應(yīng)機構(gòu)中，uf＝f(e,δe)kue，f的值可通過查模糊控制規(guī)則表確定，再結(jié)合傳統(tǒng)pid算法離散表達式為:ts為采樣周期，因此模糊自適應(yīng)pid輸出為：其中ti,td分別為積分時間常量和微分時間常量。對于比例因子，本文設(shè)置為根據(jù)e及δe進行自適應(yīng)參數(shù)修正以達到提升pid控制器性能的作用，其修正規(guī)則為：若e及δe較大時，應(yīng)取較小的ke和kδe值以及較大的kd和ki值，從而達到降低對偏差以及偏差變化量的分辨率，加快響應(yīng)速度的目的；當(dāng)系統(tǒng)接近穩(wěn)態(tài)后，則需要取較大的ke和kδe值和較小的kd和ki值，從而減小系統(tǒng)的超調(diào)現(xiàn)象，當(dāng)誤差小于閾值后，可轉(zhuǎn)而采用傳統(tǒng)pid算法對誤差進行進一步微調(diào)。根據(jù)以上修正模塊，可以由e及δe的值推理出參數(shù)修正的倍數(shù)n，從而計算得到新的參數(shù)進行控制，積分時間常量又可以由微分時間常量推理得出：ti＝atd公式(27)得到積分時間常量后可得到：最后將uf和式(27),(28)代入式(26)后可以得到如下輸出式:在通過采樣發(fā)現(xiàn)脈沖頻率發(fā)生改變后，則需要重新計算e及δe，從而初始化模糊pid控制器，達到自適應(yīng)的效果。以上實施例的說明只是用于幫助理解本發(fā)明的方法及其核心思想。應(yīng)當(dāng)指出，對于本
技術(shù)領(lǐng)域：
：的普通技術(shù)人員來說，在不脫離本發(fā)明原理的前提下，還可以對本發(fā)明進行若干改進和修飾，這些改進和修飾也落入本發(fā)明權(quán)利要求的保護范圍內(nèi)。對所公開的實施例的上述說明，使本領(lǐng)域?qū)I(yè)技術(shù)人員能夠?qū)崿F(xiàn)或使用本發(fā)明。對這些實施例的多種修改對本領(lǐng)域的專業(yè)技術(shù)人員來說將是顯而易見的，本文中所定義的一般原理可以在不脫離本發(fā)明的精神或范圍的情況下，在其它實施例中實現(xiàn)。因此，本發(fā)明將不會被限制于本文所示的這些實施例，而是要符合與本文所公開的原理和新穎特點相一致的最寬的范圍。當(dāng)前第1頁12當(dāng)前第1頁12