軌跡控制裝置制造方法
【專利摘要】本發(fā)明的實(shí)施方式的軌跡控制裝置(10)是對(duì)多個(gè)電動(dòng)機(jī)進(jìn)行控制而控制可動(dòng)部的軌跡的軌跡控制裝置，其具有：插補(bǔ)·加減速運(yùn)算部(1)，其對(duì)指令路徑進(jìn)行插補(bǔ)，并沿該指令路徑執(zhí)行加減速運(yùn)算而求出加減速后插補(bǔ)路徑；軸分配部(3)，其從加減速后插補(bǔ)路徑生成各可動(dòng)軸的位置指令；伺服響應(yīng)運(yùn)算部(51、52)，其基于位置指令，運(yùn)算其伺服響應(yīng)；切線方向伺服響應(yīng)運(yùn)算部(50)，其從加減速后插補(bǔ)路徑求出切線方向伺服響應(yīng)；基準(zhǔn)點(diǎn)生成部(2)，其從切線方向伺服響應(yīng)求出基準(zhǔn)點(diǎn)；位置矢量校正部(4)，其通過(guò)基于伺服響應(yīng)以及基準(zhǔn)點(diǎn)而運(yùn)算校正矢量，并以校正矢量對(duì)各可動(dòng)軸的位置指令進(jìn)行校正，而輸出各可動(dòng)軸的校正后位置指令；以及伺服控制部(6)、(7)，其向各可動(dòng)軸的電動(dòng)機(jī)分別輸出電動(dòng)機(jī)驅(qū)動(dòng)扭矩，以使得各可動(dòng)軸追隨各自的校正后位置指令。
【專利說(shuō)明】軌跡控制裝置

【技術(shù)領(lǐng)域】
[0001]本發(fā)明涉及一種在工作機(jī)械、激光加工機(jī)等控制裝置中用于抑制軌跡誤差并實(shí)現(xiàn)高速高精度加工的軌跡控制裝置。

【背景技術(shù)】
[0002]在使用工作機(jī)械、激光加工機(jī)等機(jī)械進(jìn)行加工的情況下，進(jìn)行控制，以使得加工工具相對(duì)于工件的位置沿著所指令的路徑。該控制被稱為軌跡控制，通常通過(guò)伺服控制實(shí)現(xiàn)，以使機(jī)械的各可動(dòng)軸的實(shí)際位置追隨根據(jù)指令路徑求出的各可動(dòng)軸的位置指令。
[0003]作為進(jìn)行軌跡控制時(shí)的問(wèn)題點(diǎn)，由于各可動(dòng)軸的控制系統(tǒng)的響應(yīng)延遲等，存在實(shí)際的軌跡從所指令的路徑偏離這樣的問(wèn)題。通常由于針對(duì)機(jī)械的各可動(dòng)軸進(jìn)行控制，所以各可動(dòng)軸的控制系統(tǒng)的響應(yīng)延遲等引起的誤差，會(huì)導(dǎo)致各可動(dòng)軸的伺服系統(tǒng)響應(yīng)延遲于位置指令而移動(dòng)。在如直線那樣指令路徑的移動(dòng)方向不發(fā)生變化的情況下，即使各軸延遲地移動(dòng)，作為伺服系統(tǒng)響應(yīng)的軌跡也不會(huì)從指令路徑上偏離。S卩，在指令路徑的切線方向出現(xiàn)誤差，但不會(huì)在指令路徑的法線方向出現(xiàn)誤差。另一方面，在如曲線、角形狀等那樣指令路徑的移動(dòng)方向發(fā)生變化的情況下，由于各軸的伺服控制系統(tǒng)的延遲，在指令路徑的法線方向上會(huì)出現(xiàn)誤差。以下，在伺服系統(tǒng)響應(yīng)位置相對(duì)于位置指令的誤差之中，將指令路徑的切線方向的成分稱為追隨誤差，將指令路徑的法線方向的成分稱為軌跡誤差。通常，如果發(fā)生軌跡誤差，則加工形狀變?yōu)榕c本來(lái)的形狀不一致，因此不是優(yōu)選。
[0004]作為用于抑制這些軌跡誤差的方法，專利文獻(xiàn)I中公開了一種方法，該方法基于預(yù)先讀取程序而識(shí)別出的加工形狀，運(yùn)算出用于將誤差抑制為一定值以下的最佳進(jìn)給速度，并且運(yùn)算出以該速度進(jìn)行加工時(shí)的誤差量，然后將用于消除該誤差的校正矢量與原來(lái)的指令位置相加而對(duì)指令位置進(jìn)行校正。此處，校正矢量的方向是與移動(dòng)方向垂直的方向(法線方向)，校正矢量的長(zhǎng)度是將法線方向加速度(將速度的平方除以曲率半徑而得到的值)乘以規(guī)定的系數(shù)而得到的值。
[0005]此外，在非專利文獻(xiàn)I中公開有一種方法，該方法通過(guò)將伺服系統(tǒng)的響應(yīng)誤差分解到指令路徑的切線方向和法線方向，使用對(duì)各個(gè)方向成分分別乘以不同的增益而得到的控制輸入，進(jìn)行反饋控制，從而獨(dú)立地控制追隨誤差和軌跡誤差。
[0006]專利文獻(xiàn)1:日本特開平6-282321號(hào)公報(bào)
[0007]非專利文獻(xiàn)1:精密工學(xué)會(huì)刊 Vol.74.N0.11.2008 pp.1193-1198


【發(fā)明內(nèi)容】

[0008]然而，在上述現(xiàn)有技術(shù)的方法中，存在下述問(wèn)題，S卩，在指令點(diǎn)間的路徑上的加速度由于加減速的影響而變化的情況、以微小線段指令對(duì)包含有3維形狀的復(fù)雜形狀進(jìn)行近似并指令這樣的情況下，不能對(duì)應(yīng)。在指令路徑的起點(diǎn)附近以及終點(diǎn)附近、在指令路徑上指令進(jìn)給速度發(fā)生變化的位置附近，如果進(jìn)給速度突然改變，則會(huì)超過(guò)作為控制對(duì)象的機(jī)械的容許加速度，因此通常進(jìn)行使進(jìn)給速度逐漸增加或者逐漸減小的加減速。
[0009]例如，在專利文獻(xiàn)I中，基于進(jìn)行加減速之前的指令點(diǎn)進(jìn)行運(yùn)算，進(jìn)而運(yùn)算出的誤差量是進(jìn)給速度處于恒定狀態(tài)(穩(wěn)態(tài))下的誤差量，所以存在下述問(wèn)題，即，在如正進(jìn)行加減速這樣的進(jìn)給速度變化的狀態(tài)(過(guò)渡狀態(tài))下，所運(yùn)算的誤差量與實(shí)際產(chǎn)生的誤差量不同，作為校正結(jié)果而得到的響應(yīng)軌跡與原來(lái)的指令軌跡不同，響應(yīng)軌跡的形狀發(fā)生變形。
[0010]此外，在非專利文獻(xiàn)I的方法中，由于將控制輸入設(shè)定在與指令路徑垂直的方向或者與指令路徑相同的方向上，所以在角形狀的指令、包含3維形狀的指令中，進(jìn)行校正的方向、校正的大小劇烈地變化或者變得不確定。如果在如上述的狀況下進(jìn)行校正，則會(huì)存在對(duì)包含機(jī)械的驅(qū)動(dòng)系統(tǒng)產(chǎn)生過(guò)大的負(fù)載、振動(dòng)這樣的問(wèn)題。此外，在非專利文獻(xiàn)I中，由于對(duì)伺服系統(tǒng)響應(yīng)誤差進(jìn)行反饋，所以存在有可能控制系統(tǒng)變得不穩(wěn)定這樣的問(wèn)題。
[0011]本發(fā)明就是鑒于上述問(wèn)題而提出的，其目的在于得到一種能夠?qū)④壽E誤差抑制得充分小，且能夠在高速下實(shí)現(xiàn)高精度加工的軌跡控制裝置。
[0012]為了解決上述課題并實(shí)現(xiàn)目的，本發(fā)明是一種軌跡控制裝置，其通過(guò)同時(shí)控制多個(gè)可動(dòng)軸的電動(dòng)機(jī)而對(duì)可動(dòng)部的軌跡進(jìn)行控制，該軌跡控制裝置的特征在于，具有:插補(bǔ)?加減速運(yùn)算部，其通過(guò)對(duì)被供給的指令路徑進(jìn)行插補(bǔ)，并且沿著所述指令路徑執(zhí)行加減速運(yùn)算，而求出加減速后插補(bǔ)路徑；軸分配部，其根據(jù)所述加減速后插補(bǔ)路徑，生成各可動(dòng)軸的位置指令；伺服響應(yīng)運(yùn)算部，其基于各可動(dòng)軸的所述位置指令，運(yùn)算各可動(dòng)軸的伺服響應(yīng)；切線方向伺服響應(yīng)運(yùn)算部，其基于所述加減速后插補(bǔ)路徑，求出切線方向移動(dòng)量的切線方向伺服響應(yīng)；基準(zhǔn)點(diǎn)生成部，其根據(jù)所述切線方向伺服響應(yīng)，求出校正基準(zhǔn)點(diǎn)；位置矢量校正部，其基于各可動(dòng)軸的所述伺服響應(yīng)以及所述校正基準(zhǔn)點(diǎn)而對(duì)用于抑制軌跡誤差的校正矢量進(jìn)行運(yùn)算，并利用所述校正矢量對(duì)各可動(dòng)軸的所述位置指令進(jìn)行校正，由此輸出各可動(dòng)軸的校正后位置指令；以及伺服控制部，其向各可動(dòng)軸的所述電動(dòng)機(jī)分別輸出電動(dòng)機(jī)驅(qū)動(dòng)扭矩，以使得各可動(dòng)軸的位置分別追隨各可動(dòng)軸的所述校正后位置指令。
[0013]發(fā)明的效果
[0014]根據(jù)本發(fā)明，能夠?qū)崿F(xiàn)下述效果，S卩，得到一種即使指令形狀是3維形狀，或者由微小線段構(gòu)成，也能夠正確地求出校正矢量，能夠?qū)④壽E誤差抑制得充分小且能夠在高速下實(shí)現(xiàn)高精度加工的軌跡控制裝置。

【專利附圖】

【附圖說(shuō)明】
[0015]圖1是表示出本發(fā)明的實(shí)施方式I涉及的軌跡控制裝置的結(jié)構(gòu)的框圖
[0016]圖2是表示本發(fā)明的實(shí)施方式I涉及的伺服控制部的結(jié)構(gòu)的框圖。
[0017]圖3-1是對(duì)本發(fā)明的實(shí)施方式I涉及的加減速運(yùn)算的詳細(xì)內(nèi)容進(jìn)行說(shuō)明的圖。
[0018]圖3-2是表示出本發(fā)明的實(shí)施方式I涉及的切線方向速度q’⑴的加減速圖案的圖。
[0019]圖3-3是表示出本發(fā)明的實(shí)施方式I涉及的由加減速后插補(bǔ)點(diǎn)形成的加減速后插補(bǔ)路徑的圖。
[0020]圖4是對(duì)本發(fā)明的實(shí)施方式涉及的位置矢量校正部的動(dòng)作進(jìn)行說(shuō)明的圖。
[0021]圖5是表示本發(fā)明的實(shí)施方式2涉及的軌跡控制裝置的結(jié)構(gòu)的框圖。

【具體實(shí)施方式】
[0022]以下，基于附圖，對(duì)本發(fā)明涉及的軌跡控制裝置的實(shí)施方式進(jìn)行詳細(xì)說(shuō)明。此外，本發(fā)明并不限定于本實(shí)施方式。
[0023]實(shí)施方式I
[0024]圖1是表示出本發(fā)明的實(shí)施方式I涉及的軌跡控制裝置10的結(jié)構(gòu)的框圖。將指令路徑以NC程序等形式作為機(jī)械的可動(dòng)部軌跡的軌跡上的座標(biāo)值而供給至插補(bǔ).加減速運(yùn)算部I。此外，也將所指令的座標(biāo)值間的插補(bǔ)方法(直線?圓弧?樣條曲線等)以及沿軌跡方向的移動(dòng)速度即進(jìn)給速度利用NC程序等而同時(shí)供給至插補(bǔ).加減速運(yùn)算部I。在插補(bǔ).加減速運(yùn)算部I中，通過(guò)用所指定的方法對(duì)所指令的座標(biāo)值間進(jìn)行插補(bǔ)，并且，進(jìn)行沿著指令路徑以另行指定的規(guī)定的加速度或者加減速時(shí)間常數(shù)進(jìn)行加速以及減速的運(yùn)算，從而對(duì)加減速后插補(bǔ)路徑進(jìn)行運(yùn)算。
[0025]此外，被供給加減速后插補(bǔ)路徑的軸分配部3對(duì)機(jī)械的各可動(dòng)軸的位置指令進(jìn)行運(yùn)算，以使得機(jī)械的可動(dòng)部經(jīng)過(guò)加減速后插補(bǔ)路徑。在本實(shí)施方式中設(shè)為有2個(gè)可動(dòng)軸，作為對(duì)第I軸以及第2軸的位置指令而運(yùn)算第I軸位置指令以及第2軸位置指令。例如，將機(jī)械的X軸設(shè)為第I可動(dòng)軸(第I軸)、將機(jī)械的Y軸設(shè)為第2可動(dòng)軸(第2軸)。第I軸伺服響應(yīng)運(yùn)算部51以及第2軸伺服響應(yīng)運(yùn)算部52分別根據(jù)第I軸位置指令和第2軸位置指令運(yùn)算并輸出第I軸伺服響應(yīng)和第2軸伺服響應(yīng)。
[0026]切線方向伺服響應(yīng)運(yùn)算部50根據(jù)加減速后插補(bǔ)路徑的切線方向移動(dòng)量，運(yùn)算并輸出切線方向伺服響應(yīng)?；鶞?zhǔn)點(diǎn)生成部2根據(jù)切線方向伺服響應(yīng)，求出并輸出基準(zhǔn)點(diǎn)的位置。并且，在位置矢量校正部4中，根據(jù)第I軸伺服響應(yīng)、第2軸伺服響應(yīng)以及基準(zhǔn)點(diǎn)的位置求出校正矢量，將校正矢量的各可動(dòng)軸方向的成分與作為各可動(dòng)軸的位置指令的第I軸位置指令以及第2軸位置指令相加,從而運(yùn)算第I軸校正后位置指令以及第2軸校正后位置指令，作為各可動(dòng)軸的校正后位置指令。第I軸伺服控制部6以及第2軸伺服控制部7分別輸出作為第I軸以及第2軸的電動(dòng)機(jī)驅(qū)動(dòng)扭矩的第I軸電動(dòng)機(jī)驅(qū)動(dòng)扭矩以及第2軸電動(dòng)機(jī)驅(qū)動(dòng)扭矩，以使得第I軸的位置以及第2軸的位置分別追隨第I軸校正后位置指令以及第2軸校正后位置指令。
[0027]第I軸伺服控制部6以及第2軸伺服控制部7為相同結(jié)構(gòu)，在圖2中，作為伺服控制部11而示出表示該第I軸伺服控制部6以及第2軸伺服控制部7結(jié)構(gòu)的框圖。輸入到伺服控制部11的校正后位置指令利用減法器20減去模型位置之后，利用模型增益乘法器21乘以第I模型增益K1，并利用減法器22減去模型速度，進(jìn)一步利用模型增益乘法器23乘以第2模型增益K2,而輸出模型加速度。利用積分器24對(duì)模型加速度進(jìn)行積分而輸出模型速度，利用積分器25對(duì)模型速度進(jìn)行積分而輸出模型位置。將輸入校正后位置指令且輸出模型位置、模型速度、模型加速度的模塊稱為標(biāo)準(zhǔn)模型部12。
[0028]利用加減法器26從由積分器25輸出的模型位置減去由電動(dòng)機(jī)32輸出的電動(dòng)機(jī)位置信號(hào)而輸出位置誤差。位置控制部27對(duì)位置誤差進(jìn)行比例控制等控制。在加減法器28中，將模型速度與位置控制部27的輸出相加后，進(jìn)一步減去由電動(dòng)機(jī)32輸出的電動(dòng)機(jī)速度信號(hào)而輸出速度誤差。速度控制部29對(duì)速度誤差進(jìn)行比例?積分控制等控制。利用乘法器30在模型加速度上乘以相當(dāng)于控制對(duì)象的慣量的值而運(yùn)算模型扭矩，利用加法器31將模型扭矩與速度控制部29的輸出相加而輸出電動(dòng)機(jī)扭矩信號(hào)。電動(dòng)機(jī)扭矩信號(hào)輸入至電動(dòng)機(jī)32。由電動(dòng)機(jī)32以及負(fù)載33構(gòu)成的機(jī)械系統(tǒng)13通過(guò)電動(dòng)機(jī)扭矩信號(hào)而驅(qū)動(dòng)，將電動(dòng)機(jī)速度信號(hào)和電動(dòng)機(jī)位置信號(hào)輸出到伺服控制部11。
[0029]該伺服控制部11是使用了標(biāo)準(zhǔn)模型的2自由度控制器，能夠獨(dú)立地設(shè)計(jì)針對(duì)指令的追隨性以及針對(duì)擾動(dòng)的響應(yīng)性。電動(dòng)機(jī)位置對(duì)位置指令的追隨性由第I模型增益&以及第2模型增益K2決定，電動(dòng)機(jī)位置對(duì)擾動(dòng)的響應(yīng)性由位置控制部27以及速度控制部29的設(shè)計(jì)決定。因此，將電動(dòng)機(jī)位置的響應(yīng)與實(shí)際的控制對(duì)象的特性無(wú)關(guān)地控制為追隨標(biāo)準(zhǔn)模型的輸出即模型位置。
[0030]在軌跡控制時(shí)為了不使響應(yīng)軌跡偏移，需要使各可動(dòng)軸的位置響應(yīng)(大致)相同。位置響應(yīng)是指從伺服控制部11的輸入至電動(dòng)機(jī)位置信號(hào)為止的響應(yīng)特性，如果該響應(yīng)特性在各可動(dòng)軸之間不同，則在指令出直線形狀時(shí)響應(yīng)軌跡從所指令的直線偏離，或者針對(duì)圓弧指令的響應(yīng)軌跡向傾斜方向偏移。在本實(shí)施方式中，在第I軸伺服控制部6以及第2軸伺服控制部7中，將標(biāo)準(zhǔn)模型的增益設(shè)定為相同值。即，將第I軸伺服控制部6的標(biāo)準(zhǔn)模型的第I模型增益與第2軸伺服控制部7的標(biāo)準(zhǔn)模型的第I模型增益設(shè)定為相同值，將第I軸伺服控制部6的標(biāo)準(zhǔn)模型的第2模型增益與第2軸伺服控制部7的標(biāo)準(zhǔn)模型的第2模型增益設(shè)定為相同值。由于電動(dòng)機(jī)位置的響應(yīng)由標(biāo)準(zhǔn)模型的特性決定，所以如果使用完全相同的標(biāo)準(zhǔn)模型就能夠得到相同的位置響應(yīng)。
[0031]接下來(lái)，對(duì)各部分的運(yùn)算的詳細(xì)內(nèi)容進(jìn)行說(shuō)明。插補(bǔ)?加減速運(yùn)算部I用指定的方法對(duì)所指令的座標(biāo)值間進(jìn)行插補(bǔ)，進(jìn)一步進(jìn)行加減速運(yùn)算，而對(duì)指令路徑上的每個(gè)插補(bǔ)周期的指令位置進(jìn)行運(yùn)算。插補(bǔ)的方法有直線插補(bǔ)、圓弧插補(bǔ)、樣條曲線插補(bǔ)等。插補(bǔ)周期是作為軌跡控制裝置10的規(guī)格而確定的恒定周期，通常使用小于或等于數(shù)Hls的較短的周期。插補(bǔ)周期較短能夠進(jìn)行高精度的軌跡控制，但用于運(yùn)算的處理器等的處理負(fù)載增大。將作為插補(bǔ).加減速的結(jié)果而得到的每個(gè)插補(bǔ)周期的點(diǎn)稱為加減速后插補(bǔ)點(diǎn)，將由加減速后插補(bǔ)點(diǎn)形成的路徑稱為插補(bǔ)路徑。加減速運(yùn)算是用于防止指令加速度超過(guò)機(jī)械的各可動(dòng)軸的容許加速度而過(guò)大的運(yùn)算，是以使速度的變化小于或等于由參數(shù)等另行指定的加速度的方式對(duì)插補(bǔ)路徑上的插補(bǔ)點(diǎn)進(jìn)行重新運(yùn)算。即，在從起點(diǎn)移動(dòng)至終點(diǎn)的情況下，在起點(diǎn)之后為了立即使速度逐漸增加而將插補(bǔ)點(diǎn)間的間隔縮短，在終點(diǎn)之后為了立即使速度逐漸減小而將插補(bǔ)點(diǎn)間的間隔縮短。
[0032]使用圖3-1?圖3-3所示的例子說(shuō)明該加減速運(yùn)算的詳細(xì)內(nèi)容。考慮下述情況，即如圖3-1所示,從起點(diǎn)P。= (X。，y0)經(jīng)過(guò)中途的點(diǎn)P1 = (X1, Y1)以及P2 = (x2, y2)到終AP3= (x3, y3)為止以速度F移動(dòng)。將該起點(diǎn)、經(jīng)過(guò)點(diǎn)、終點(diǎn)稱為指令點(diǎn)。將指令點(diǎn)在圖3-1中以白色圈表示。各指令點(diǎn)的座標(biāo)值由NC程序等指令。將沿著從起點(diǎn)Ptl至任意時(shí)刻t的路徑上的點(diǎn)P(t)為止的指令路徑的距離設(shè)為q(t)。將從起點(diǎn)到終點(diǎn)為止的移動(dòng)長(zhǎng)度設(shè)為L(zhǎng)。L是連接Pc^PpPyP3的各線段的長(zhǎng)度之和。如果將從Plri到Pn為止的線段長(zhǎng)度設(shè)為 In(其中 n= 1、2、3) JlJL = Wl315
[0033]接著，將q(t)的時(shí)間微分設(shè)為切線方向速度q’(t)Jfq’ (t)的加減速圖案設(shè)定為如圖3-2所示。其是高度為指令進(jìn)給速度F，面積為從起點(diǎn)到終點(diǎn)為止的移動(dòng)長(zhǎng)度L的梯形的圖案，設(shè)定為加速部以及減速部的斜率成為由參數(shù)等另行指定的加速度。此處，加速以及減速以直線狀進(jìn)行，但有時(shí)也設(shè)為S字狀而使機(jī)械的動(dòng)作更加平穩(wěn)。通過(guò)對(duì)切線方向速度q’ (t)進(jìn)行時(shí)間積分，而求出加減速后的移動(dòng)長(zhǎng)度qc;(t)。將At設(shè)為插補(bǔ)周期，而計(jì)算t = NX At (N為正整數(shù))的加減速后移動(dòng)長(zhǎng)度q。(NX Λ t)，從起點(diǎn)位置Ptl沿指令路徑前進(jìn)了長(zhǎng)度qjNX At)的點(diǎn)成為第N個(gè)加減速后插補(bǔ)點(diǎn)。
[0034]由加減速后插補(bǔ)點(diǎn)形成的加減速后插補(bǔ)路徑如圖3-3所示。作為加減速后插補(bǔ)路徑的信息，包含:加減速后的切線方向移動(dòng)長(zhǎng)度1。(0、插補(bǔ)的種類(直線插補(bǔ)、圓弧插補(bǔ)、樣條曲線插補(bǔ)等)、插補(bǔ)中必需的參數(shù)(起點(diǎn)位置、終點(diǎn)位置、其它形狀參數(shù))。作為其它形狀參數(shù)，在圓弧插補(bǔ)的情況下有中心位置和半徑等參數(shù)，在樣條曲線插補(bǔ)的情況下有樣條曲線函數(shù)的系數(shù)等參數(shù)。
[0035]軸分配部3求出加減速后插補(bǔ)路徑上的加減速后插補(bǔ)點(diǎn)的各軸的座標(biāo)值。在圖3所示的例子的情況下，例如表示在線段P1P2上的加減速后插補(bǔ)點(diǎn)P。的座標(biāo)的矢量P。= (X。，yc)通過(guò)如下過(guò)程求出，即，在表示P。所屬的線段的起點(diǎn)P1的矢量P1 = (X1^1)和表示終點(diǎn)P2的矢量P2 = (x2，y2)之間，使用由移動(dòng)長(zhǎng)度qjt)確定的比率進(jìn)行直線插補(bǔ)。因此，表示時(shí)刻t的位置指令P。(t)的座標(biāo)值的位置指令矢量P。(t) = (xc(t), yc(t))由下式(I)表
/Jn ο
[0036]【式I】
/ \ C] ,(I)....1j / %
[0037]Pju= ； I (P^Pt)+P1.-1?


,Pj...P?I
[0038]其中,第I軸位置指令為X Jt)、第2軸位置指令為ye(t)。
[0039]在第I軸伺服響應(yīng)運(yùn)算部51以及第2軸伺服響應(yīng)運(yùn)算部52中分別運(yùn)算針對(duì)第I軸位置指令以及第2軸位置指令的伺服響應(yīng)(第I軸伺服響應(yīng)以及第2軸伺服響應(yīng))。伺服響應(yīng)的運(yùn)算以下述方式進(jìn)行。如前述，伺服系統(tǒng)的響應(yīng)能夠用標(biāo)準(zhǔn)模型的響應(yīng)表示，所以通過(guò)求出在第I軸以及第2軸位置指令作為輸入而供給的情況下的標(biāo)準(zhǔn)模型的輸出即模型位置，而分別求出第I軸以及第2軸的伺服系統(tǒng)響應(yīng)位置。并且，將第I軸以及第2軸的伺服系統(tǒng)響應(yīng)位置作為各可動(dòng)軸方向的成分的矢量設(shè)為伺服響應(yīng)位置矢量，將該位置矢量所繪出的軌跡作為伺服響應(yīng)軌跡。
[0040]標(biāo)準(zhǔn)模型的輸出即模型位置，能夠通過(guò)以微分方程式、差分方程式、傳遞函數(shù)等形式表示圖2所示的標(biāo)準(zhǔn)模型部12，并利用數(shù)值計(jì)算而計(jì)算其解而求出。此外，在已知指令形狀的情況下，能夠通過(guò)積分計(jì)算而求出解析解。
[0041]例如，圖2所示的標(biāo)準(zhǔn)模型的傳遞函數(shù)Gm(S)由下式⑵表示。
[0042]【式2】
[0043]:…⑵


S τ' ISk.> S 卞 ISk.1 ISl I

細(xì).I餘
[0044]向標(biāo)準(zhǔn)模型供給某個(gè)輸入時(shí)的輸出，通過(guò)對(duì)標(biāo)準(zhǔn)模型的傳遞函數(shù)和被供給的輸入的拉普拉斯變換的積進(jìn)行拉普拉斯逆變換而求出。針對(duì)位置指令矢量P。(t)的伺服響應(yīng)位置？3(1:)的矢量 Ps (t) = (xs(t), ys(t))由下式(3)表示。
[0045]【式3】
“..χ(ιΓ !.;[(iir(s)1.[x ΙΟΙΓ
[0046]pM= ' s:: , ' , ' J…⑶
[0047]其中，L[f(t)]表示f(t)的拉普拉斯變換，L-1LF(S)]表示F(S)的拉普拉斯逆變換。第I軸的伺服響應(yīng)為xs(t)，第2軸的伺服響應(yīng)為ys(t)。
[0048]與第I軸伺服響應(yīng)運(yùn)算部51以及第2軸伺服響應(yīng)運(yùn)算部52同樣地，在切線方向伺服響應(yīng)運(yùn)算部50中求出針對(duì)加減速后的切線方向移動(dòng)長(zhǎng)度qc;(t)的伺服響應(yīng)即切線方向伺服響應(yīng)qs(t)。但是，在求出位置的伺服響應(yīng)的情況下將矢量用作輸入，但在切線方向移動(dòng)長(zhǎng)度的情況下輸入是標(biāo)量值。切線方向伺服響應(yīng)qs(t)能夠通過(guò)對(duì)標(biāo)準(zhǔn)模型的傳遞函數(shù)和被供給的輸入ια)的拉普拉斯變換的積進(jìn)行拉普拉斯逆變換而求出，由下式(4)表
/Jn ο
[0049]【式4】
[0050]qs (t) = L-1 [Gm (s) L [qc (t)]]...⑷
[0051]接下來(lái)，在基準(zhǔn)點(diǎn)生成部2中求出從起點(diǎn)沿著加減速后插補(bǔ)路徑前進(jìn)了切線方向伺服響應(yīng)的長(zhǎng)度的點(diǎn)PJt)，并將該點(diǎn)作為基準(zhǔn)點(diǎn)(校正基準(zhǔn)點(diǎn))。基準(zhǔn)點(diǎn)的座標(biāo)Pjt)以下述方式求出。首先，根據(jù)切線方向伺服響應(yīng)qs(t)和各指令點(diǎn)間的線段長(zhǎng)度In，確定基準(zhǔn)點(diǎn)屬于哪個(gè)線段。即，如果qs(t)小于I1，則確定為在PtlP1上，如果qs(t)大于或等于I1而小于IJl2，則確定為在P1P2上。此處，設(shè)為qs(t)在P1P2上。接著，通過(guò)使用由切線方向伺服響應(yīng)的長(zhǎng)度qjt)確定的比率在基準(zhǔn)點(diǎn)所屬于的線段的起點(diǎn)和終點(diǎn)之間進(jìn)行直線插補(bǔ)，而確定基準(zhǔn)點(diǎn)的座標(biāo)?；鶞?zhǔn)點(diǎn)的座標(biāo)Pr(t) = (xr(t), yr(t))由下式(5)求出。
[0052]【式5】
[0053]

【權(quán)利要求】
1.一種軌跡控制裝置，其通過(guò)同時(shí)控制多個(gè)可動(dòng)軸的電動(dòng)機(jī)而對(duì)可動(dòng)部的軌跡進(jìn)行控制， 該軌跡控制裝置的特征在于，具有: 插補(bǔ)?加減速運(yùn)算部，其通過(guò)對(duì)被供給的指令路徑進(jìn)行插補(bǔ)，并且沿著所述指令路徑執(zhí)行加減速運(yùn)算，而求出加減速后插補(bǔ)路徑； 軸分配部，其根據(jù)所述加減速后插補(bǔ)路徑，生成各可動(dòng)軸的位置指令； 伺服響應(yīng)運(yùn)算部，其基于各可動(dòng)軸的所述位置指令，運(yùn)算各可動(dòng)軸的伺服響應(yīng)； 切線方向伺服響應(yīng)運(yùn)算部，其基于所述加減速后插補(bǔ)路徑，求出切線方向移動(dòng)量的切線方向伺服響應(yīng)； 基準(zhǔn)點(diǎn)生成部，其根據(jù)所述切線方向伺服響應(yīng)，求出校正基準(zhǔn)點(diǎn)； 位置矢量校正部，其基于各可動(dòng)軸的所述伺服響應(yīng)以及所述校正基準(zhǔn)點(diǎn)而對(duì)用于抑制軌跡誤差的校正矢量進(jìn)行運(yùn)算，并利用所述校正矢量對(duì)各可動(dòng)軸的所述位置指令進(jìn)行校正，由此輸出各可動(dòng)軸的校正后位置指令；以及 伺服控制部，其向各可動(dòng)軸的所述電動(dòng)機(jī)分別輸出電動(dòng)機(jī)驅(qū)動(dòng)扭矩，以使得各可動(dòng)軸的位置分別追隨各可動(dòng)軸的所述校正后位置指令。
2.根據(jù)權(quán)利要求1所述的軌跡控制裝置，其特征在于， 所述伺服響應(yīng)運(yùn)算部針對(duì)每個(gè)所述可動(dòng)軸具有獨(dú)立的伺服響應(yīng)運(yùn)算部，各所述獨(dú)立的伺服響應(yīng)運(yùn)算部是輸入輸出之間的響應(yīng)與各自相對(duì)應(yīng)的所述可動(dòng)軸的位置響應(yīng)大致相同的濾波器。
3.根據(jù)權(quán)利要求1所述的軌跡控制裝置，其特征在于， 所述切線方向伺服響應(yīng)運(yùn)算部是輸入輸出之間的響應(yīng)與所述可動(dòng)軸的位置響應(yīng)大致相同的濾波器，其將沿著從所述指令路徑的起點(diǎn)至加減速后指令位置為止的指令路徑的長(zhǎng)度的標(biāo)量值作為輸入。
4.根據(jù)權(quán)利要求2或3所述的軌跡控制裝置，其特征在于， 所述伺服控制部針對(duì)每個(gè)所述可動(dòng)軸具有獨(dú)立的伺服控制部，所述可動(dòng)軸的所述位置響應(yīng)是從該可動(dòng)軸的所述獨(dú)立的伺服控制部的輸入起至該可動(dòng)軸的電動(dòng)機(jī)的位置信號(hào)為止的響應(yīng)特性。
5.根據(jù)權(quán)利要求2、3或4所述的軌跡控制裝置，其特征在于， 每個(gè)所述可動(dòng)軸的所述位置響應(yīng)均大致相同。
6.根據(jù)權(quán)利要求1至5中任一項(xiàng)所述的軌跡控制裝置，其特征在于， 所述基準(zhǔn)點(diǎn)生成部求出從所述指令路徑的起點(diǎn)沿所述加減速后插補(bǔ)路徑前進(jìn)了根據(jù)所述切線方向伺服響應(yīng)而得到的長(zhǎng)度的點(diǎn)，作為所述校正基準(zhǔn)點(diǎn)。
7.根據(jù)權(quán)利要求1至6中任一項(xiàng)所述的軌跡控制裝置，其特征在于， 所述位置矢量校正部，將從根據(jù)所述伺服響應(yīng)確定的點(diǎn)至所述校正基準(zhǔn)點(diǎn)為止的矢量作為所述校正矢量，將所述校正矢量與根據(jù)所述位置指令確定的點(diǎn)相加而得到的結(jié)果作為所述校正后位置指令。
【文檔編號(hào)】G05D3/12GK104204977SQ201280071568
【公開日】2014年12月10日 申請(qǐng)日期:2012年12月11日 優(yōu)先權(quán)日:2012年3月21日
【發(fā)明者】長(zhǎng)岡弘太朗 申請(qǐng)人:三菱電機(jī)株式會(huì)社