本发明涉及一种在加工机的控制装置中用于控制轨迹而实现高速高精度加工的轨迹控制装置。
背景技术：
在使用工作机械、激光加工机等加工机进行加工的情况下，控制为，搭载有与作为加工的对象物的工作物(工件)对应的刀具或激光喷嘴的加工头的位置沿所指定的路径进行动作。该控制被称为轨迹控制，通常通过进行伺服控制而实现，以使得机械的各可动轴的实际的位置追随根据指令路径求出的各可动轴的指令位置。
在轨迹控制中同时控制的可动轴的轴数根据加工机的用途而不同，但通常为2轴至5轴。另外，并不限定于此，也存在可动轴的轴数大于或等于6轴的情况。作为2轴的例子，已知金属板激光加工机。这种金属板激光加工机在板状的材料的表面之上使搭载有激光喷嘴的加工头使用2个直线进给轴进行2维扫描，在由加工程序所指定的定时使激光束进行照射，从而对工作物构建出规定的形状。
接下来，作为3轴的代表性的例子，可列举立式加工中心。这种立式加工中心沿铅垂方向把持搭载有立铣刀等刀具的加工头，一边通过主轴电动机使其进行旋转、一边使用3个直线进给轴使其在3维空间内进行移动，从而构建模具、部件等的规定的3维形状。并且，5轴加工机是在3轴的立式加工中心中添加了2个旋转轴的加工机，在刀具的3维空间内的位置的基础上，还能够控制刀具的姿态。
在任意轴数的加工机中，均为数控装置读取加工程序，在由加工程序所指定的各点之间进行插补而生成每个控制周期的各可动轴的指令位置。通过各可动轴所分别具有的伺服控制装置进行伺服控制，以使得各可动轴的实际的位置追随各个可动轴的指令位置。下面，将数控装置和各可动轴的伺服控制装置统称为轨迹控制装置。
作为进行轨迹控制时的问题点，可列举由于各可动轴的伺服控制系统的响应延迟而导致实际的路径从所指定的路径偏离的问题点。通常对机械的每个可动轴进行控制，因此由于由各可动轴的控制系统的响应延迟等引起的误差，导致各可动轴的伺服系统响应相比于指令位置发生延迟而移动。在例如像直线那样指令路径的移动方向不发生变化的情况下，即使各轴延迟地进行移动，作为伺服控制系统响应的轨迹也不会从指令路径上偏离。即，虽然在指令路径的切线方向出现误差，但不会出现指令路径的法线方向的误差。另一方面，在如曲线、角形状等那样指令路径的移动方向发生变化的情况下，由于各轴的伺服控制系统的延迟，在指令路径的法线方向会出现误差。
下面，在伺服控制系统中的响应位置的相对于指令位置的误差之中，将指令路径的切线方向的成分称为追随误差，将指令路径的法线方向的成分称为轨迹误差。通常，如果存在轨迹误差，则加工形状变得与原本的形状不一致，因此不优选。
为了抑制这些轨迹误差，在专利文献1中，基于预读取程序而识别出的加工形状，运算用于将误差抑制为小于或等于固定值的最佳进给速度。对以该速度进行加工时的误差量进行运算，然后将用于消除该误差的校正矢量与原来的指令位置相加而对指令位置进行校正。校正矢量的方向是与移动方向垂直的方向(法线方向)，校正矢量的长度是将法线方向加速度(将速度的平方除以曲率半径所得出的值)乘以规定的系数所得出的值。
另外，在专利文献2中，在机器人的指尖位置的控制中，对规定的采样时间前的指尖的位置进行推定，按照从推定出的指尖位置下降至目标轨道上的垂线矢量的大小而对指令位置进行校正。由此，控制为一边容许时间延迟、一边使指尖位置在目标轨道上进行移动。
专利文献1：日本特开平6-282321号公报(第4页～第5页，图1)
专利文献2：日本特开2006-15431号公报(第5页～第6页，图3)
技术实现要素：
然而，在专利文献1的方法中，校正矢量的运算仅基于对移动进行指示的指令值的法线方向加速度等移动指令的信息，未考虑实际的伺服响应。因此，在如直线与圆弧相连续的情况那样移动指令的法线方向加速度非稳态地发生变化的情况下，或者在通过微小线段指令对还包含3维形状在内的复杂的形状进行近似并进行指定这样的情况下，存在不能正确地运算出校正矢量这样的问题。在移动指令中的指令值的法线方向加速度固定的情况下，能够根据法线方向加速度推定轨迹误差量。另外，在法线方向加速度发生变化的状态(非稳态的状态)下，在基于移动指令的法线方向加速度推定的轨迹误差量与实际产生的轨迹误差量之间会产生差异。因此，在使用了基于推定出的轨迹误差量而运算出的校正矢量时的响应轨迹与原来的指令轨迹之间会产生轨迹误差，作为结果，响应轨迹的形状发生畸变。
接下来，在专利文献2的方法中，利用实际的指尖的位置即响应位置对指令位置进行校正，但响应延迟时间设为固定，将固定时间前的响应作为基准而决定校正量。因此，在如指令的移动方向时时刻刻发生变化那样的情况下，存在下述问题，即，未正确地对响应延迟时间非稳态地变化进行校正，响应轨迹从指令轨迹偏离。
另外，在专利文献2中，需要对向目标轨道的垂线进行运算，但在如移动方向时时刻刻发生变化那样的复杂的指令形状中，存在下述问题，即，根据目标轨道与响应位置的关系而存在多条向目标轨道的垂线，无法唯一地确定校正量。例如，想到构成为长方形中的角部等、2个边形成规定的角度的指令形状。在该情况下，在控制对象的位置处在上述规定的角度的平分线上的时刻，垂足分别存在于2个边，从而不能唯一地决定校正量。另外，在垂线的方向或长度急剧地变化的情况下，存在下述问题，即，校正量急剧地变化，对机械造成冲击而引起振动。
并且，在专利文献2中，为了对响应位置进行运算而需要运算出全部的各可动轴的伺服响应，存在运算量变多这样的问题。这个问题在5轴加工机等可动轴的数量多的情况下特别地显著。
另外，在专利文献1及2中，为了对指令位置的校正量进行运算而需要预读取移动指令，运算处理变得复杂。并且，因为由数控装置逐次地处理加工程序，所以存在下述课题，即，为了进行预读取，需要使动作停止预读取所需的时间，读取所需量的移动指令，加工时间会相应地延长。
本发明就是为了解决上面所述的课题而提出的，其目的在于获得一种轨迹控制装置，该轨迹控制装置即使在移动指令的法线方向加速度非稳态地发生变化的情况下、或者在移动指令的移动方向时时刻刻发生变化的情况下，也控制加工机的可动部的轨迹以使得在所指定的路径上准确地进行追随。
本发明涉及的轨迹控制装置对由多个轴驱动的加工机的可动部的轨迹进行控制，其特征在于，具有：合成长度运算部，其对沿指令路径从开始位置起累加的长度即指令合成长度进行运算，该指令路径是作为分别针对多个轴的指令位置的履历而确定的；模拟伺服响应滤波器部，其使模拟出多个轴的伺服响应的第1滤波器作用于指令合成长度而对响应合成长度进行运算；基准响应运算部，其对从开始位置起沿指令路径的长度与响应合成长度相等的指令路径上的位置即基准响应位置进行运算；响应补偿部，其使具有多个轴的伺服响应的反向特性的第2滤波器作用于基准响应位置处的多个轴各自的坐标值，对多个轴的各个校正后指令位置进行运算；以及伺服控制部，其通过输出驱动扭矩以使得多个轴的各个位置追随校正后指令位置，从而分别控制所述多个轴。
发明的效果
根据本发明，即使在移动指令的法线方向加速度非稳态地发生变化的情况下、或者在如移动指令的移动方向时时刻刻发生变化这样的情况下，也能够基于响应合成长度而求出基准响应位置及校正后指令位置，控制可动部的轨迹以使得校正后的响应位置在所指定的路径上准确地进行追随。
另外，能够唯一地求出校正后指令位置，能够稳定地进行校正后指令位置的运算。
附图说明
图1是表示本发明的实施方式1涉及的轨迹控制装置的框图。
图2是表示本发明的实施方式1中的控制对象的示意图。
图3是表示本发明的实施方式1中的伺服控制部的框图。
图4是表示本发明的实施方式1中的合成长度运算部的动作的说明图。
图5是表示本发明的实施方式1中的模拟伺服响应滤波器部的动作的说明图。
图6是表示本发明的实施方式1中的基准响应运算部的动作的说明图。
图7是表示本发明的实施方式1中的响应补偿部的框图。
图8是表示本发明的实施方式1中的未进行指令位置的校正的情况下的响应位置的轨迹的图。
图9是表示本发明的实施方式1中的进行指令位置的校正后的情况下的轨迹的图。
图10是表示本发明的实施方式1中的指令合成长度和响应合成长度的时间变化的图。
图11是表示本发明的实施方式2涉及的轨迹控制装置的框图。
图12是表示本发明的实施方式2中的控制对象的示意图。
图13是表示本发明的实施方式2中的未进行指令位置的校正的情况下的轨迹的图。
图14是表示本发明的实施方式2中的进行指令位置的校正后的情况下的轨迹的图。
图15是表示本发明的实施方式2中的指令合成长度和响应合成长度的时间变化的图。
图16是表示本发明的实施方式3涉及的轨迹控制装置的框图。
图17是表示本发明的实施方式3中的控制对象的示意图。
图18是表示本发明的实施方式3中的未进行指令位置的校正的情况下的各轴坐标值的时间变化的图。
图19是表示本发明的实施方式3中的进行指令位置的校正后的情况下的各轴坐标值的时间变化的图。
图20是表示本发明的实施方式3中的未进行指令位置的校正的情况下的轨迹的图。
图21是表示本发明的实施方式3中的进行指令位置的校正后的情况下的轨迹的图。
图22是表示本发明的实施方式3中的指令合成长度和响应合成长度的时间变化的图。
具体实施方式
下面，使用附图对本发明涉及的轨迹控制装置详细地进行说明。此外，下面所示的实施方式为例示，本发明不限定于下面的实施方式。
实施方式1.
图1是表示本发明的实施方式1中的轨迹控制装置的框图。另外，图2是表示在本实施方式中作为控制的对象的2维的加工机的示意图。
在图2中，加工头63安装于Y轴可动部62，由Y轴的电动机(未图示)沿Y轴方向进行驱动。另外，由Y轴可动部62及Y轴电动机构成的Y轴驱动机构安装于X轴可动部61，由X轴的电动机(未图示)沿X轴方向进行驱动。由X轴可动部61及X轴电动机构成的X轴驱动机构安装于加工机主体(未图示)。假设X轴及Y轴相互正交。
X轴及Y轴的电动机由图1所示的轨迹控制装置1进行驱动控制。在轨迹控制装置1包含有：合成长度运算部2、模拟伺服响应滤波器部3、基准响应运算部4、X轴响应补偿部6、Y轴响应补偿部7、X轴伺服控制部8、以及Y轴伺服控制部9。
在轨迹控制装置1，作为移动指令而输入X轴及Y轴的指令位置。在轨迹控制装置1的内部，X轴及Y轴的指令位置被输入至合成长度运算部2，输出指令合成长度。指令合成长度被输入至模拟伺服响应滤波器部3。模拟伺服响应滤波器部3的输出与X轴及Y轴的指令位置一起被输入至基准响应运算部4，对X轴基准响应位置及Y轴基准响应位置进行运算。X轴基准响应位置及Y轴基准响应位置被分别输入至X轴响应补偿部6及Y轴响应补偿部7，分别输出X轴校正后指令位置及Y轴校正后指令位置。X轴校正后指令位置及Y轴校正后指令位置被分别输入至X轴伺服控制部8及Y轴伺服控制部9，分别输出X轴电动机驱动扭矩及Y轴电动机驱动扭矩。
X轴伺服控制部8及Y轴伺服控制部9为相同的结构，将该结构的框图与机械系统13的框图一起在图3中示出。机械系统13由电动机32及负载33构成。下面，将X轴伺服控制部8、Y轴伺服控制部9统称为伺服控制部11。
在图3中，通过第1减法器20，从向伺服控制部11所输入的校正后指令位置减去模型位置。通过第1模型增益乘法器21，对该减法所得到的值乘以第1模型增益(控制增益)K1。通过第2减法器22，从该乘法所得到的值减去模型速度。通过第2模型增益乘法器23，对该减法所得到的值乘以第2模型增益(控制增益)K2，输出模型加速度。接下来，通过第1积分器24对模型加速度进行积分而输出模型速度。并且，通过第2积分器25对模型速度进行积分，输出模型位置。将上面所述的、从校正后指令位置至模型位置为止的模块称为标准模型12。
在X轴伺服控制部8和Y轴伺服控制部9分别对第1模型增益K1及第2模型增益K2设定相同值。因此，X轴伺服控制部8和Y轴伺服控制部9的标准模型具有相同的传递函数。
如图3所示，通过第3减法器26从模型位置减去电动机位置信号而输出位置误差。在位置控制器27对该位置误差进行比例控制。在加减法器28中，将模型速度与位置控制器27的输出相加，并减去电动机速度信号的值而输出速度误差。在速度控制器29对该速度误差进行比例及积分控制。另外，通过比例乘法器30，对模型加速度乘以相当于控制对象的惯量的值(J)，对模型扭矩进行运算。接下来，通过第4加法器31将模型扭矩与速度控制器29的输出相加而输出电动机扭矩信号。
机械系统13由该电动机扭矩信号驱动，将电动机速度信号和电动机位置信号向伺服控制部11进行输出。此外，在本实施方式中，如图1所示，Y轴的负载是安装了加工头63的Y轴可动部62，X轴的负载是X轴可动部61以及在其之上被驱动的Y轴可动部62。
伺服控制部11是使用了标准模型的2自由度控制器，能够独立地设计针对指令的追随性和针对干扰的响应性。针对指令的追随性由第1模型增益K1及第2模型增益K2决定，针对干扰的响应性由位置控制器27及速度控制器29的设计决定。
伺服系统的响应与实际的控制对象的特性无关，被控制为追随标准模型12的输出即模型位置。在本实施方式中，由于在X轴的伺服控制系统和Y轴的伺服控制系统使用相同的传递函数的标准模型，因此由X轴伺服控制部8控制的伺服控制系统的响应和由Y轴伺服控制部9控制的伺服控制系统的响应变得相同，该响应能够通过标准模型的传递函数进行表示。
合成长度运算部2对于由各可动轴的指令位置的履历确定的指令路径，对从开始位置算起的沿指令路径进行累加而得出的指令合成长度进行运算。指令合成长度的运算以下述方式进行。假设指令位置每隔固定的控制周期T进行更新，将第n个(n为零或自然数)指令位置表示为Pcn。Pcn是具有X轴方向成分及Y轴方向成分共2个要素的坐标，将该坐标值表示为(xcn，ycn)。另外，将移动的开始位置设为Pc0，将其坐标设为(xc0，yc0)。第n个指令位置时的指令合成长度Lcn是通过从开始位置起累加从该指令位置的前次值(前次指令位置)即第(n-1)个指令位置Pc(n-1)至本次指令位置Pcn为止的矢量的几何范数而求出的。几何范数为矢量的各要素的平方和的平方根。如果将这些通过公式进行表示，则成为下面的式(1)。
【式1】
在图4示意性地示出n＝0～3时的指令位置Pcn与指令合成长度Lcn的关系。此外，与n＝0(开始位置)对应的指令合成长度Lc0为零。另外，在图4中，“sqrt(式A)”表示式A的平方根。
模拟伺服响应滤波器部3使模拟出伺服响应的滤波器作用于指令合成长度Lcn。伺服响应通常以传递函数的形式进行表示。在本实施方式中，模拟伺服响应滤波器部3构成为，由伺服控制部11控制的伺服系统的伺服响应和模拟伺服响应滤波器部3的伺服响应变得相同。在伺服控制部11对使用了标准模型的2自由度控制进行了应用的情况下，伺服控制部11的伺服响应成为通过标准模型的传递函数进行表示的响应。如上所述，由于X轴伺服控制部8和Y轴伺服控制部9为相同的结构，使用相同的控制增益(第1模型增益K1及第2模型增益K2)，因此X轴标准模型和Y轴标准模型成为相同结构的滤波器。在图3所示的伺服控制部11的情况下，标准模型12的传递函数Gm(s)以2阶的低通滤波器的形式表示为下面的式(2)。
【式2】
该低通滤波器的输出能够通过下述方式而求出，即，以微分方程式、差分方程式、传递函数等形式表示式2的传递函数，通过数值计算而计算其解。另外，在指令形状已知的情况下，能够通过积分计算而求出解析解。具体地说，通过对传递函数与被赋予的输入的拉普拉斯变换之积进行拉普拉斯逆变换而求出解析解。
对于时刻t，假设表示为t＝nT(T为控制周期)。在将时刻t处的指令合成长度设为Lc(t)时，指令合成长度Lc(t)被输入时的模拟伺服响应滤波器部3的输出、即响应合成长度Lr(t)表示为下面的式(3)。
【式3】
Lr(t)＝F-1[Gm(s)F[Lc(t)]] (3)
在这里，F[g(t)]表示时间函数g(t)的拉普拉斯变换，F-1[G(s)]表示s个区域(多个区域)的函数G(s)的拉普拉斯逆变换。
在图5示意性地示出指令合成长度Lc(t)和响应合成长度Lr(t)的时间变化。在这里，t1＝T，t2＝2T，t3＝3T。在图5中，将T＝t1、t2以及t3处的响应合成长度分别表示为Lr1、Lr2以及Lr3，将指令合成长度分别表示为Lc1、Lc2以及Lc3。如该图所示，通过模拟伺服响应滤波器部3，响应合成长度Lr(t)相比于指令合成长度Lc(t)发生延迟地对其进行追随。
接下来，在基准响应运算部4中对基准响应位置进行运算，该基准响应位置是位于指令路径上且从开始位置算起的沿指令路径的长度与响应合成长度Lr(t)相等的位置。在将第n个基准响应位置设为Prn时，其坐标(xrn，yrn)通过下面的方式而求出。
首先，将第n个响应合成长度Lrn与包含第n个在内的过去的指令合成长度Lc1～Lcn进行比较，决定响应基准点位于指令路径的第几个线段上。即，求出如Lrn大于或等于Lc(k-1)而小于Lck那样的k(k为自然数)。由于Lck随k的增加而单调增加，因此能够唯一地求出k。接下来，通过使用由响应合成长度Lrn确定的比率在基准响应位置所属的线段的起点和终点之间进行直线插补，从而决定基准响应位置的坐标。基准响应位置的坐标(xrn，yrn)可通过下面的式(4)而求出。
【式4】
对于n＝1～3的情况，将指令路径与响应合成长度Lrn与基准响应位置Prn的关系在图6中示意性地示出。在图6中，指令路径表示为按照编号n的顺序连结指令位置Pcn的线段。此外，Pc0表示开始位置。
接下来，X轴响应补偿部6及Y轴响应补偿部7分别使伺服响应的反向特性的滤波器作用于基准响应位置Prn的X轴成分及Y轴成分。假设在这里考虑的伺服响应是与由伺服控制部11控制的伺服控制系统相同的响应。因此，使标准模型12的反向特性的响应的滤波器进行作用。
在图7示出X轴响应补偿部6及Y轴响应补偿部7的框图。在X轴及Y轴，各个响应补偿部均为相同的结构，在该图中作为响应补偿部40而示出。输入至响应补偿部40的信号(X轴基准响应位置或Y轴基准响应位置)被输入至第1微分运算器50，该第1微分运算器50的输出被进一步输入至第2微分运算器51。在这里，假设微分运算通过差分近似法而进行，将前次值与本次值之差除以控制周期T所得出的值作为微分值而进行输出。接下来，在第1系数乘法器52中对第2微分运算器51的输出乘以第2模型增益K2的倒数，将该第1系数乘法器52的输出与第1微分运算器50的输出在第1加法器53中进行相加。进而在第2系数乘法器54中对该第1加法器53的输出乘以第1模型增益K1的倒数，将该第2系数乘法器54的输出与向响应补偿部40的输入信号在第2加法器55中进行相加，将该结果设为响应补偿部40的输出。
响应补偿部40的输入输出间的传递函数表示为下面的式(5)。
【式5】
如式(5)所示，该式为式(2)的标准模型的传递函数的逆传递函数，响应补偿部40具有标准模型的反向特性。
在图1中，使用上述的响应补偿部40的输出计算方法，X轴响应补偿部6及Y轴响应补偿部7的输出分别作为X轴及Y轴的校正后指令位置，而输入至X轴伺服控制部8及Y轴伺服控制部9。X轴伺服控制部8及Y轴伺服控制部9进行所述的伺服控制，运算出X轴及Y轴的电动机驱动扭矩而进行输出，分别驱动X轴电动机及Y轴电动机。
如本实施方式中所示，通过在使响应补偿部40作用于基准响应位置之后进行伺服控制，从而作为伺服控制的结果而获得的各可动轴的响应位置追随基准响应位置。由于基准响应位置处在指令路径上，因此各可动轴的响应位置也被控制为在指令路径上无误差地进行追随。另外，响应补偿部40的输出即校正后指令位置在移动方向不变化的情况(指令路径呈直线状的情况)下与原来的指令位置成为相同的位置。与此相对，在移动方向发生变化的情况(指令路径呈曲线状的情况)下，为了抑制轨迹误差，通过响应补偿部40的作用而输出与原来的指令位置不同的位置。
实施例1.
接下来，使用实施例表示本实施方式中的效果。作为指令路径，使用圆弧部分的半径为1mm的R角形状(即，由半径1mm的圆弧和将其夹着的2个直线部分构成的形状)。将本实施例中使用的指令位置的轨迹即指令路径、未进行指令位置的校正的情况下的响应位置的轨迹在图8中示出。(X，Y)＝(0mm，0mm)的点为起点(开始位置)，(X，Y)＝(2mm，2mm)的点为终点。此外，图8中的箭头表示指定路径上的指令位置的移动方向。该图中的箭头所代表的含义在以后的图9、图13、图14、图20以及图21中也相同。
在图8的图表中，横轴取X轴位置，纵轴取Y轴位置，无圆点的实线为指令位置，有圆点的实线为未进行指令位置的校正的情况下(在图1中，将各可动轴的指令位置直接输入至各个轴的伺服控制部的情况下)的伺服响应位置的模拟结果。作为模拟的条件，假设进给速度为2m/min，X轴及Y轴的伺服控制部的标准模型12中的第1模型增益(控制增益)K1为40rad/s，假设第2模型增益(控制增益)K2为第1模型增益K1的4倍。另外，假设控制周期T为2ms。
接下来，将进行指令位置的校正后的情况下的、校正后指令位置的轨迹和校正后响应位置的轨迹的模拟结果在图9中示出。在图9中，无圆点的实线为原来的指令位置的轨迹，虚线表示校正后指令位置的轨迹，有圆点的实线表示响应位置的轨迹。进给速度、伺服控制部的第1模型增益K1及第2模型增益K2的值使用与在未进行指令位置的校正的情况下相同的值。在进行指令位置的校正后的情况下，原来的指令位置的轨迹和响应位置的轨迹几乎一致，为了区分两者，图8及图9的响应位置的波形带有圆点。
另外，在图10中示出本实施例中的模拟伺服响应滤波器部3的输入输出、即指令合成长度和响应合成长度的时间变化。
根据图8可知，在未进行指令位置的校正的情况下，由于各轴的伺服响应延迟的影响，在圆弧部分发生了约0.2mm的轨迹误差。关于在该实施例使用的指令形状，直线部分与圆弧部分相连续，在直线部分和圆弧部分处法线方向加速度非稳态地发生变化，并且在圆弧部分处移动方向时时刻刻发生变化，是容易发生轨迹误差的形状。
根据图10可知，在进行指令位置的校正后的情况下，沿合成长度方向(图10中的纵轴的方向)相对于指令发生延迟而进行响应。另一方面，根据图9的有圆点的实线与图9的无圆点的实线一致，可知相对于指令路径的轨迹误差被充分地抑制。这是由于使伺服响应追随根据响应合成长度和指令位置而求出的基准响应位置，通过各轴的响应补偿部如图9的虚线那样对校正后指令位置运算出的结果。
如上所述，根据本实施方式，即使在移动指令的法线方向加速度非稳态地发生变化的情况下、或者在如移动指令的移动方向时时刻刻发生变化这样的情况下，也能够基于响应合成长度而求出基准响应位置及校正后指令位置，控制可动部的轨迹以使得校正后的响应位置在所指定的路径上准确地进行追随。
因此，能够稳定地进行校正后指令位置的运算。
另外，由于能够运算出校正后指令位置而无需求出各轴的伺服响应，因此能够实现运算量的削减。
另外，由于仅基于指令位置的过去的履历就能够运算出校正后指令位置，而无需预读取移动指令，因此运算处理变得简单，能够避免为了预读取而延长加工时间。
并且，通过将使用了标准模型的2自由度控制器用于伺服控制部，从而能够与控制对象的特性无关地使伺服控制部的响应与标准模型的响应一致。
另外，通过将由标准模型的传递函数的逆传递函数表示的滤波器用于响应补偿部，从而能够使各可动轴的实际的位置准确地追随沿着指令路径的位置即基准响应位置，能够控制轨迹以使得不从指令路径发生轨迹误差。
如上所述，根据本实施方式，能够在2维平面内进行轨迹控制的情况下抑制轨迹误差而进行高精度的加工。
实施方式2.
图11是表示本发明的实施方式2中的轨迹控制装置101的框图。另外，图12是表示在本实施方式2中作为控制对象的3轴加工机的示意图。
在图12中，加工头74安装于Z轴可动部73，由Z轴的电动机(未图示)沿Z轴方向进行驱动。另外，由Z轴可动部73及Z轴电动机构成的Z轴驱动机构安装于Y轴可动部72，由Y轴的电动机(未图示)沿Y轴方向进行驱动。并且，由Y轴可动部72及Y轴电动机构成的Y轴驱动机构安装于X轴可动部71，由X轴的电动机(未图示)沿X轴方向进行驱动。由X轴可动部71及X轴电动机构成的X轴驱动机构安装于加工机主体。假设X轴、Y轴以及Z轴相互正交。
图12中的加工头74、X轴可动部71以及Y轴可动部72分别与图2中的加工头63、X轴可动部61以及Y轴可动部62对应。
X轴、Y轴以及Z轴的电动机由图11所示的轨迹控制装置101进行驱动控制。
本实施方式与实施方式1不同的点在于成为控制的对象的可动轴的数量变为3轴、新追加了Z轴，其他的结构(X轴及Y轴的结构)与实施方式1相同。另外，Z轴响应补偿部108及Z轴伺服控制部111是与X轴及Y轴相同的结构。
下面，对与实施方式1的不同点进行叙述。如果使用X轴、Y轴、Z轴的坐标值将第n个指令位置Pcn的坐标设为(xcn，ycn，zcn)，则合成长度运算部102中的指令合成长度Lcn可通过下面的式(6)而求出。
【式6】
接下来，基准响应运算部104中的基准响应位置的坐标(xrn，yrn，zrn)可通过下面的式(7)而求出。
【式7】
如上所述，本实施方式中的其他的结构与实施方式1相同。
X轴响应补偿部106、Y轴响应补偿部107以及Z轴响应补偿部108均构成为与图7所示的响应补偿部40相同。即，关于X轴、Y轴以及Z轴，响应补偿部的输入输出间的传递函数以式(5)示出。
另外，Z轴伺服控制部111是与X轴伺服控制部109及Y轴伺服控制部110相同的结构，该结构的框图如图3所示。
X轴响应补偿部106、Y轴响应补偿部107以及Z轴响应补偿部107的输出分别作为X轴、Y轴以及Z轴的校正后指令位置，而输入至X轴伺服控制部109、Y轴伺服控制部110以及Z轴伺服控制部111。X轴伺服控制部109、Y轴伺服控制部110以及Z轴伺服控制部111进行与在实施方式1中所述的伺服控制相同的伺服控制，运算出X轴、Y轴以及Z轴的电动机驱动扭矩而进行输出，分别驱动X轴电动机、Y轴电动机以及Z轴电动机。
实施例2.
接下来，使用实施例表示本实施方式中的效果。作为指令路径而示出使用如果投影于XY平面、则成为半径5mm的圆弧的螺旋状的形状后的情况。假设X轴和Y轴在XY平面内进行1周的圆周运动期间，沿Z方向移动10mm。
将本实施例中使用的指令位置的轨迹即指令路径、未进行指令位置的校正的情况下的响应位置的轨迹在图13中示出。图13(a)为投影于XY平面的轨迹，图13(b)为投影于XZ平面的轨迹。(X，Y，Z)＝(0mm，-5mm，0mm)的位置为起点，(X，Y，Z)＝(0mm，-5mm，10mm)的位置为终点。另外，在图13(a)所示的XY平面中，圆周轨迹是沿逆时针旋转而成的。
在图13(a)及(b)的各图表中，无圆点的实线为指令位置，有圆点的实线为未进行指令位置的校正的情况下(在图11中，将各可动轴的指令位置直接输入至各个轴的伺服控制部的情况下)的伺服响应位置的模拟结果。作为模拟的条件，假设指令路径方向的进给速度为7.9m/min。另外，假设起点处的初速度为0m/s，假设指令路径的切线方向的速度以固定的加速度加速至指令进给速度即7.9m/min。并且，假设在终点附近为下述模式，即，指令路径的切线方向的进给速度从指令进给速度即7.9m/min以固定的加速度进行减速，在终点处变为0m/min。假设加速及减速时的切线方向的加速度的绝对值为10m/s2。
假设X轴、Y轴以及Z轴的伺服控制部的标准模型中的第1模型增益(控制增益)K1为40rad/s，假设第2模型增益(控制增益)K2为第1模型增益(控制增益)K1的4倍。另外，假设控制周期T为2ms。
将进行指令位置的校正后的情况下的、校正后指令位置的轨迹和校正后响应位置的轨迹的模拟结果在图14中示出。图14(a)为投影于XY平面的轨迹，图14(b)为投影于XZ平面的轨迹。在图14中，无圆点的实线为原来的指令位置的轨迹，虚线表示校正后指令位置的轨迹，有圆点的实线表示响应位置的轨迹。进给速度、伺服控制部的第1模型增益K1及第2模型增益K2的值使用与在未进行指令位置的校正的情况下相同的值。
在进行指令位置的校正后的情况下，原来的指令位置的轨迹和响应位置的轨迹几乎一致，为了两者的识别，图13及图14的校正后响应位置的波形带有圆点。
另外，在图15中示出本实施例中的模拟伺服响应滤波器部的输入输出、即指令合成长度和响应合成长度的时间变化。
根据图13可知，在未进行指令位置的校正的情况下的伺服响应位置的轨迹，在整体上沿作为指令轨迹的圆弧形状的内侧的方向发生了轨迹误差，该轨迹误差最大约为0.2mm。关于在该实施例使用的指令形状，移动方向时时刻刻3维地进行变化，是容易发生轨迹误差的形状。另外，在起点和终点处一边沿切线方向进行加减速、一边移动方向发生变化，法线方向加速度非稳态地发生变化，因此是难以进行轨迹误差的准确的校正的指令路径。
另外，根据图15可知，在进行指令位置的校正后的情况下，沿合成长度方向相对于指令发生延迟而进行响应。另一方面，根据图14的有圆点的实线与图14的无圆点的实线一致，可知相对于指令路径的轨迹误差被充分地抑制。这是由于使伺服响应追随根据响应合成长度和指令位置而求出的基准响应位置，通过各轴的响应补偿部如图14的虚线那样对校正后指令位置运算出的结果。
如上所述，根据本实施方式，如3轴的立式加工中心等那样在3维空间内进行轨迹控制的情况下，也可获得与实施方式1相同的效果。在该情况下也能够抑制轨迹误差而进行高精度的加工。
实施方式3.
图16是表示本发明的实施方式3涉及的轨迹控制装置的框图。另外，图17是表示在本实施方式中作为控制的对象的5轴加工机的示意图。
在图17中，加工头86、Z轴可动部83、Y轴可动部82以及X轴可动部81是与实施方式2相同的结构。即，加工头86、Z轴可动部83、Y轴可动部82以及X轴可动部81分别与图12中的加工头74、Z轴可动部73、Y轴可动部72以及X轴可动部71对应。
在本实施方式中还具备旋转台，该旋转台具有绕Y轴旋转的B轴和绕Z轴旋转的C轴，通过在旋转台之上放置工件，从而能够控制与工件对应的刀具的朝向(刀具姿态)。C轴可动部85作为旋转台而起作用，由C轴电动机(未图示)进行驱动。由C轴可动部85和C轴电动机构成的C轴驱动机构安装于B轴可动部84，由B轴的电动机(未图示)进行驱动。由B轴可动部84及B轴电动机构成的B轴驱动机构安装于加工机主体。假设B轴的旋转中心轴与Y轴平行，在B轴的旋转角度为0°时，C轴的旋转中心轴与Z轴平行。
X轴、Y轴、Z轴、B轴以及C轴的电动机由图16所示的轨迹控制装置201进行驱动控制。与实施方式2不同的点在于成为控制的对象的可动轴的数量变为5轴、新追加了B轴和C轴，其他的结构与实施方式2相同。另外，B轴及C轴的响应补偿部及伺服控制部是与X轴、Y轴以及Z轴相同的结构。
下面，对与实施方式2的不同点进行叙述。如果使用X轴、Y轴、Z轴、B轴、C轴的各轴的坐标值将第n个指令位置Pcn的坐标设为(xcn，ycn，zcn，bcn，ccn)，则合成长度运算部202中的指令合成长度Lcn可通过下面的式(8)而求出。
【式8】
接下来，基准响应运算部204中的基准响应位置的坐标(xrn，yrn，zrn，brn，crn)可通过下面的式(9)而求出。
【式9】
如上所述，本实施方式中的其他的结构与实施方式2相同。X轴响应补偿部206、Y轴响应补偿部207、Z轴响应补偿部208、B轴响应补偿部209以及C轴响应补偿部210均构成为与图7所示的响应补偿部40相同。另外，关于X轴、Y轴、Z轴、B轴以及C轴，响应补偿部的输入输出间的传递函数以式(5)示出。
另外，B轴伺服控制部214及C轴伺服控制部215是与X轴伺服控制部211、Y轴伺服控制部212以及Z轴伺服控制部213相同的结构，该结构的框图如图3所示。
X轴响应补偿部206、Y轴响应补偿部207、Z轴响应补偿部208、B轴响应补偿部209以及C轴响应补偿部210的输出分别作为X轴、Y轴、Z轴、B轴以及C轴的校正后指令位置，而输入至X轴伺服控制部211、Y轴伺服控制部212、Z轴伺服控制部213、B轴伺服控制部214以及C轴伺服控制部215。X轴伺服控制部211、Y轴伺服控制部212、Z轴伺服控制部213、B轴伺服控制部214以及C轴伺服控制部215进行与实施方式1相同的伺服控制，运算出X轴、Y轴、Z轴、B轴以及C轴的电动机驱动扭矩而进行输出，分别驱动X轴电动机、Y轴电动机、Z轴电动机、B轴电动机以及C轴电动机。
实施例3.
接下来，使用实施例表示本实施方式中的效果。作为指令路径而设为如下路径，即，从工件侧观察时刀具前端的路径为直线，其间刀具姿态进行变化。将机械的X轴、Y轴以及Z轴的轴向分别为X、Y以及Z的坐标轴方向的坐标系定义为机械坐标系。另外，将在B轴和C轴同时为0°时坐标轴方向与机械坐标系一致、与工作台的转动联动的坐标系定义为工作台坐标系。
作为一个例子，假设B轴旋转台的旋转中心的坐标值在机械坐标系为(100mm，200mm，300mm)，C轴旋转台的旋转中心的坐标在机械坐标系为(100mm，200mm，400mm)。将C轴旋转台的旋转中心的位置设为工作台坐标系的原点。在工作台坐标系中，假设刀具前端位置的坐标值从(0mm，200mm，0mm)移动至(-200mm，0mm，0mm)，其间，刀具姿态以矢量表示，假设从(0，1，1)的方向向(-1，0，1)的方向进行变化。
为了实现这样的刀具前端的路径，机械的各轴需要以复杂的路径进行运动。因此，将工作台坐标系中的刀具前端位置及刀具姿态矢量每隔控制周期换算为机械坐标系中的刀具位置(X、Y以及Z的各轴的位置的坐标值)和旋转轴角度(B及C的各旋转轴的坐标值)，将作为换算出的结果而获得的X、Y、Z、B以及C的各轴坐标值用作各轴的指令位置。该换算能够使用几何关系而唯一地进行。
将本实施例中使用的指令位置、未进行指令位置的校正的情况下的响应位置的时间变化在图18中示出。在图18的图表中，实线为指令位置，虚线为未进行指令位置的校正的情况下(在图16中，将各可动轴的指令位置直接输入至各个轴的伺服控制部的情况下)的伺服响应位置的模拟结果。作为模拟的条件，假设路径方向的进给速度为2m/min。假设X轴、Y轴、Z轴、B轴以及C轴的伺服控制部的标准模型中的第1模型增益(控制增益)K1为40rad/s，假设第2模型增益(控制增益)K2为第1模型增益(控制增益)K1的4倍。另外，假设控制周期T为2ms。
另外，将进行指令位置的校正后的情况下的、校正后指令位置和校正后响应位置的时间变化的模拟结果在图19中示出。在图19中，无圆点的实线为原来的指令位置，有圆点的实线表示校正后指令位置的轨迹，虚线表示响应位置的轨迹。另外，在图19中，原来的指令位置的时间变化与校正后指令位置的时间变化几乎一致，为了区分两者，校正后指令位置的波形带有圆点。进给速度、伺服控制部的第1模型增益K1及第2模型增益K2的值使用与在未进行指令位置的校正的情况下相同的值。
根据图18及图19可知，为了实现工作台坐标系中的刀具前端位置和刀具姿态的移动，机械的各可动轴的指令位置的时间变化呈复杂的形状。另外，无论是未进行指令位置的校正的情况还是进行指令位置的校正后的情况，响应位置均相对于指令位置发生延迟而进行追随，但该延迟方式在两者之间不同。
在各可动轴位置的时间变化中，由于不能进行轨迹误差的评价，因此将响应位置及校正后指令位置从机械坐标系换算为工作台坐标系而进行评价。
将工作台坐标系中的、指令位置的轨迹和未进行指令位置的校正的情况下的响应位置的轨迹在图20中示出。在图20的各图表中，无圆点的实线为指令位置，有圆点的实线为未进行指令位置的校正的情况下(在图16中，将各可动轴的指令位置直接输入至各个轴的伺服控制部的情况下)的伺服响应位置的模拟结果。
接下来，将进行指令位置的校正后的情况下的、工作台坐标系中的校正后指令位置的轨迹和校正后响应位置的轨迹在图21中示出。在图21中，无圆点的实线为原来的指令位置的轨迹，虚线表示校正后指令位置的轨迹，有圆点的实线表示响应位置的轨迹。进给速度、伺服控制部的第1模型增益K1及第2模型增益K2的值使用与在未进行指令位置的校正的情况下相同的值。在进行指令位置的校正后的情况下，原来的指令位置的轨迹和响应位置的轨迹几乎一致，为了区分两者，对图20及图21的响应位置的波形附带圆点。
另外，在图22中示出本实施例中的模拟伺服响应滤波器部的输入输出、即指令合成长度和响应合成长度的时间变化。
根据图20可知，在未进行指令位置的校正的情况下，由于各轴的伺服响应延迟的影响，在直线的中央部分发生了约5mm的轨迹误差。关于在该实施例使用的指令形状，为了实现工作台坐标系中的刀具前端位置和刀具姿态的按照指令的移动，机械的各可动轴一边使加速度时时刻刻进行变化、一边进行移动，机械的各可动轴的追随误差容易作为工作台坐标系中的轨迹误差而出现。
根据图22可知，在进行指令位置的校正后的情况下，沿合成长度方向相对于指令发生延迟而进行响应。另一方面，根据图21的有圆点的实线与图21的无圆点的实线一致，可知相对于指令路径的轨迹误差被充分地抑制。这是由于使伺服响应追随根据响应合成长度和指令位置而求出的基准响应位置，通过各轴的响应补偿部如图19的有圆点的实线及图21的虚线那样对校正后指令位置运算出的结果。
另外，如图19所示，同时刻的校正后指令位置和指令位置之差是比追随误差(图18中的指令位置和响应位置之差)小的值。这是因为基于响应合成长度而容许追随误差地对基准响应位置进行运算，能够通过小的校正量而有效地抑制轨迹误差。
如上所述，根据本实施方式，在如5轴加工机那样进行刀具位置和姿态的轨迹控制的情况下，也可获得与实施方式1相同的效果。在该情况下也能够抑制轨迹误差而进行高精度的加工。
此外，在本实施方式中对在工作台侧具有2个旋转轴的情况进行了说明，但也可以是在刀具侧具有2个旋转轴的情况、或者是在刀具侧和工作台侧各具有1个旋转轴的情况。如果适当地对机械坐标系中的坐标值与工作台坐标系中的坐标值之间的换算式进行选择而应用，则能够获得相同的效果。
另外，在本实施方式中将2个旋转轴设为绕Y轴旋转的B轴和绕Z轴旋转的C轴，但也可以是绕其他方向的轴旋转的轴。如果在机械坐标系中的坐标值与工作台坐标系中的坐标值之间的换算式应用适当的值，则能够获得相同的效果。
在上面所述的实施方式1至3中，对可动轴的数量为2、3、5的情况进行了说明，但如果可动轴的数量为4或者大于或等于6也是同样的。在该情况下，在合成长度运算部对与可动轴的数量对应的合成长度进行运算，并按照与可动轴的数量对应的量而设置响应补偿部及伺服控制部即可。
标号的说明
1、101、201 轨迹控制装置，
2、102、202 合成长度运算部，
3、103、203 模拟伺服响应滤波器部，
4、104、204 基准响应运算部，
6、106、206 X轴响应补偿部，
7、107、207 Y轴响应补偿部，
8、109、211 X轴伺服控制部，
9、110、212 Y轴伺服控制部，
11 伺服控制部，
12 标准模型，
40 响应补偿部，
61、71、81 X轴可动部，
62、72、82 Y轴可动部，
73、83 Z轴可动部，
84 B轴可动部，
85 C轴可动部，
108、208 Z轴响应补偿部，
111 Z轴伺服控制部，
209 B轴响应补偿部，
210 C轴响应补偿部，
213 Z轴伺服控制部
214 B轴伺服控制部，
215 C轴伺服控制部。