CN103080859A - 轨迹控制装置 - Google Patents

Abstract

同时控制多个可动轴的马达来控制可动部的轨迹的轨迹控制装置构成为具备：伺服系应答轨迹计算部（1），根据各可动轴的位置指令，运算伺服系应答轨迹；形状特征判定部（9），从位置指令，根据指令路径的形状是直线还是曲线，输出包括关于路径形状的边界点位置和该边界点附近处的移动方向的信息的形状特征量；位置矢量校正部（2），根据位置指令、伺服系应答轨迹以及形状特征量，校正位置矢量，输出校正后的位置指令；以及伺服控制部（7、8），通过以使各可动轴的位置追踪校正后位置指令的方式，输出马达驱动转矩，而控制各可动轴的马达。由此，能够进行如下校正：能够还包括直线与曲线的边界部分那样的、轨迹误差的大小、朝向过渡性地变化那样的部位，将轨迹误差抑制得充分小。

Description

轨迹控制装置

技术领域

本发明涉及在控制NC工作机械、NC激光加工机等的NC控制装置中管理工具轨迹的轨迹控制装置，特别涉及不论指令路径的形状如何，都抑制轨迹误差来实现高速高精度加工的轨迹控制装置。

背景技术

在使用NC工作机械、NC激光加工机等机械来进行加工的情况下，以使工具（立铣刀等刃物）相对工作物的位置沿着指令的路径上的方式进行控制。该控制被称为轨迹控制，一般通过以使机械的各可动轴的实际的位置追踪根据指令路径求出的各可动轴的位置指令的方式执行伺服控制来进行。

作为进行轨迹控制的问题，以往存在起因于各可动轴的控制系的应答延迟等而实际的轨迹偏离所指令的路径的问题。通常，针对机械的各可动轴的每一个进行控制，所以由于各轴的控制系的应答延迟等所致的误差，各可动轴的伺服系应答比位置指令延迟地移动。在如直线那样指令路径的移动方向不变化的情况下，即使各轴延迟移动，作为伺服系应答的轨迹也不会从指令路径上脱离。即，虽然在指令路径的切线方向上出现误差，但不会出现指令路径的法线方向的误差。另一方面，在指令路径的移动方向如曲线、角形状等那样变化的情况下，由于各轴的伺服控制系的延迟，而在指令路径的法线方向上出现误差。

以下，将伺服系应答位置相对位置指令的误差中的、指令路径的切线方向的分量称为追踪误差，将指令路径的法线方向的分量称为轨迹误差。一般，如果有轨迹误差，则加工形状与本来的形状不一致，所以不优选。另一方面，追踪误差不会对加工形状直接造成影响，所以相比于轨迹误差，被容许的情况较多，但如果追踪误差过大，则加工时间延迟，而不优选。

作为用于抑制这些轨迹误差、追踪误差的方案，例如在下述的专利文献1中，公开了在机器人的卡抓位置的控制中，推测规定的采样时间前方的卡抓的位置，按照从所推测出的卡抓位置落入目标轨道上的垂线矢量的量，校正位置指令，从而容许时间延迟，同时使卡抓位置位于目标轨道上来抑制轨迹误差的方法。即，通过推测在进行多个可动轴的控制的情况下产生的误差中的、与目标轨道即指令路径垂直的方向的误差即轨迹误差，并按照该推测出的轨迹误差的量逆向地校正指令路径，而校正轨迹误差，抑制在移动路径中产生的轨迹误差。

专利文献1：日本特开2006－015431号公报

发明内容

但是，在上述专利文献1公开那样的方法中，虽然在如简单的圆或者圆弧那样稳定地产生相同量的轨迹误差的情况下，能够有效地校正轨迹误差，但在从直线切换为圆弧那样的指令路径的情况下，存在如下问题：在其边界的部分无法充分校正轨迹误差，在校正后的移动路径中也仍残留轨迹误差。即，在直线部分中不产生轨迹误差，所以不发生用于校正轨迹误差的校正量，但在圆弧那样的曲线部分中，如上所述发生指令路径的法线方向的误差、即轨迹误差，所以发生用于校正轨迹误差的校正量。另外，在直线部分与圆弧部分的边界部分中，轨迹误差的大小、朝向过渡性地大幅变化，其结果，用于校正轨迹误差的校正量也过渡性地变化。在这样的情况下，如果原样地使用所推测出的轨迹误差来校正指令路径，则边界部分中的校正量与理想的校正量不同，而无法充分校正轨迹误差。

例如，考虑在沿着XY平面上的指令路径移动的情况下，在X轴方向上直线移动之后，成为圆弧指令的情况。此时，设为直线和圆弧入口的切线的朝向一致，并设为在圆弧部分中Y轴向正的方向移动。在该情况下，为了校正在圆弧的部分中发生的轨迹误差，在与指令路径垂直的方向上加上校正量，校正后的指令路径成为圆弧部分向外侧鼓起来的路径。在这样的情况下，在转移到圆弧指令并经过了一定的时间之后，校正后的路径与指令路径一致，但在刚刚转移到圆弧指令之后，校正后的移动路径相对指令路径过冲（overshoot），而成为向外侧鼓起来的路径。即，在刚刚转移到圆弧指令之后，Y轴暂时向负方向移动，之后向正方向移动。其原因为，为了向相对指令路径垂直的方向校正指令，成为指令路径的圆弧部分向外侧鼓起来的路径，此时由于校正后的指令路径的Y轴分量进行暂时向负方向移动之后向正方向移动那样的变化的原因而产生。

在工作机械中，工具（刃物）的移动路径的形状被转印到加工面，所以如上述例子那样，如果从直线在圆弧的边界本来轨迹平滑地连接的部分中移动路径向外侧鼓起来，则其影响在加工面中引起突起、折痕这样的加工不良而被表现，使加工面的质量降低，所以不优选。

本发明是鉴于上述问题而完成的，其目的在于提供一种轨迹控制装置，即使在提供了直线和曲线连续那样的指令路径的情况下，也能够进行还包括直线与曲线的边界部分那样的轨迹误差的大小、朝向过渡性地变化的部位而将轨迹误差抑制得充分小的校正。

为了解决上述课题并达成目的，本发明的轨迹控制装置，通过用多个可动轴驱动机械的可动部，并同时控制所述多个可动轴的马达来控制所述可动部的轨迹，其特征在于，具备：伺服系应答轨迹计算部，根据各可动轴的位置指令，运算伺服系应答轨迹；形状特征判定部，根据所述位置指令，判定指令路径的形状是直线还是曲线，输出形状特征量；位置矢量校正部，根据所述位置指令、所述伺服系应答轨迹以及所述形状特征量，校正位置矢量，输出校正后的位置指令；以及多个伺服控制部，通过以使各可动轴的位置追踪所述校正后位置指令的方式，输出马达驱动转矩，控制各可动轴的马达。

根据本发明，即使在直线和曲线连续那样的指令路径中的直线与曲线的边界部分、拐点这样的、轨迹误差的大小、朝向过渡性地变化那样的情况下，也能够有效地抑制轨迹误差，轨迹精度提高。其结果，起到能够避免加工面的损伤、折痕这样的加工不良，加工面质量提高这样的效果。

附图说明

图1是示出本发明的轨迹控制装置的实施方式1的框图。

图2是详细示出图1的伺服控制部的结构的框图。

图3是示出实施方式1中的轨迹误差矢量的运算动作的图。

图4是示出实施方式1中的形状特征判定部的动作的图。

图5是示出实施方式1中的校正后位置指令的运算动作的图。

图6是示出实施方式1中的校正（轨迹误差抑制）的效果的图。

图7是示出实施方式2中的形状特征判定部的动作的图。

图8是示出实施方式2中的校正后位置指令的运算动作的图。

图9是示出实施方式3中的伺服系应答轨迹计算部的动作的图。

图10是示出实施方式3中的形状特征判定部的动作的图。

图11是示出实施方式3中的校正后位置指令的运算动作的图。

（符号说明）

1：伺服系应答轨迹计算部；2：位置矢量校正部；7：第1轴伺服控制部；8：第2轴伺服控制部；9：形状特征判定部；10：轨迹控制装置；11：伺服控制部；12：规范模型部；13：机械系；20、22、26：减法器；21、23：模型增益乘法器；24、25：积分器；31：加法器；27：位置控制器；28：加减法器；29：速度控制器；30：乘法器；32：马达；33：负载。

具体实施方式

实施方式1.

图1是示出本发明的轨迹控制装置的实施方式1的框图。轨迹控制装置10构成为包括伺服系应答轨迹计算部1、形状特征判定部9、位置矢量校正部2、第1轴伺服控制部7、以及第2轴伺服控制部8。将工具（刃物）的指令路径以NC程序等形式作为机械的第1轴以及第2轴位置指令提供。在伺服系应答轨迹计算部1中，在提供了第1轴位置指令和第2轴位置指令时，运算伺服系的应答轨迹。对于该应答轨迹的运算方法，将后述。形状特征判定部9判定根据第1轴以及第2轴位置指令决定的指令路径是直线还是曲线，将直线与曲线的边界点位置以及直线的方向作为形状特征量而输出。位置矢量校正部2根据上述伺服系应答轨迹和位置指令计算轨迹误差，使用所计算出的轨迹误差和形状特征量运算用于不产生轨迹的过冲地抑制轨迹误差的校正后位置指令。对于校正，通过合成第1轴位置指令以及第2轴位置指令而作为指令位置矢量并对其执行矢量运算来进行校正，通过将校正了的结果的位置矢量的各轴分量作为第1轴以及第2轴的校正后位置指令来进行。第1轴伺服控制部7以及第2轴伺服控制部8以使第1可动轴的位置以及第2可动轴的位置分别追踪第1轴校正后位置指令以及第2轴校正后位置指令的方式，分别输出第1轴以及第2轴的马达驱动转矩。

第1轴伺服控制部7以及第2轴伺服控制部8是同一结构，在图2中作为伺服控制部11示出其框图。针对输入到伺服控制部11的校正后位置指令，通过减法器20减去模型位置，通过模型增益乘法器21乘以第1模型增益K₁，通过减法器22减去模型速度，进而通过模型增益乘法器23乘以第2模型增益K₂，而输出模型加速度。通过积分器24对模型加速度进行积分而输出模型速度，通过积分器25对模型速度进行积分而输出模型位置。此处，将输入校正后位置指令，并输出模型位置、模型速度以及模型加速度的模块称为规范模型部12。通过减法器26从模型位置减去马达位置信号而输出位置误差。通过位置控制器27对位置误差进行比例控制等控制。在加减法器28中对位置控制器输出加上模型速度，进而减去马达速度信号而输出速度误差。通过速度控制器29对速度误差进行比例控制或者积分控制等控制。通过乘法器30，对模型加速度乘以与控制对象的惯量相当的值而运算模型转矩，通过加法器31对速度控制器输出加上模型转矩而输出马达转矩信号。具有马达32以及负载33的机械系13通过该马达转矩信号进行驱动，将马达速度信号和马达位置信号作为反馈信号输出到伺服控制部11。

本实施方式的伺服控制部11是使用了规范模型部12的2自由度控制器，能够独立地设计针对指令的追踪性和针对干扰的应答性。马达位置相对位置指令的追踪性由第1模型增益K₁以及第2模型增益K₂决定，马达位置相对干扰的应答性由位置控制器以及速度控制器的设计决定。因此，马达位置的应答不管实际的控制对象的特性如何，都被控制为追踪作为规范模型的输出的模型位置。

如以下那样进行伺服系应答轨迹计算部1中的伺服系应答轨迹的运算。如上所述，伺服系的应答能够通过规范模型的应答来表示，所以通过求出第1轴以及第2轴位置指令作为输入而提供的情况的、规范模型的输出即模型位置，而分别求出第1轴以及第2轴的伺服系应答位置。然后，将以第1轴以及第2轴的伺服系应答位置为各可动轴方向的分量的矢量作为伺服系应答位置矢量，将该位置矢量描绘的轨迹作为伺服系应答轨迹。能够通过用微分方程式、差分方程式、传递函数等形式来表示图2所示的规范模型部12，并利用数值计算来计算其解，而求出作为规范模型的输出的模型位置。另外，在指令形状已知的情况下，能够通过积分计算求出解析解。

例如，图2所示的规范模型的传递函数G_m（s）如下式所示。

[式1]

$G_m\left(s\right)=\frac{K_1{K}_2}{s^2+K_{2}s+K_1{K}_2}\·\·\·\left(1\right)$

通过对规范模型的传递函数与所提供的输入的拉普拉斯变换之积进行逆拉普拉斯变换，而求出对规范模型提供了某一输入时的输出。在分别通过x_c1（t）、x_c2（t）提供了时刻t下的第1轴以及第2轴位置指令时，位置指令矢量如下式所示。

[式2]

$x_c\left(t\right)=\left[\begin{array}{c}{x}_{c1}\left(t\right)\\ x_{c2}\left(t\right)\end{array}\right]\·\·\·\left(2\right)$

此时，第1轴以及第2轴的伺服系应答位置x_r1（t）、x_r2（t）如下式所示。

[式3]

$x_r\left(t\right)=\left[\begin{array}{c}{x}_{r1}\left(t\right)\\ x_{r2}\left(t\right)\end{array}\right]=\left[\begin{array}{c}{L}^{-1}\left[G\left(s\right)L\right[x_{c1}\left(t\right)\left]\right]\\ L^{-1}\left[G\left(s\right)L\right[x_{c2}\left(t\right)\left]\right]\end{array}\right]\·\·\·\left(3\right)$

其中，L[f（t）]表示f（t）的拉普拉斯变换，L^－1[F（s）]表示F（s）的逆拉普拉斯变换。

另外，能够根据规范模型的传递函数求出伺服系的应答延迟时间。此处，将对伺服系提供了一定速度的输入时的相对指令的应答的固定的延迟时间作为伺服系应答延迟时间。在图2所示的规范模型部12中，伺服系应答延迟时间t_d成为第1模型增益的倒数，如下式所示。

[式4]

$t_d=\frac{1}{K_1}\·\·\·\left(4\right)$

图3是说明实施方式1中的位置矢量校正部2中的、轨迹误差的计算部分的动作的图。此处，记述了指令路径是直线和圆弧连续的路径的情况。此时，伺服系应答轨迹呈现图3的虚线那样的路径，在时刻t下处于x_r（t）的位置。在用于推测轨迹误差的运算中，为了去除伺服系的应答延迟所致的追踪误差的影响，并非进行该时刻的伺服系应答位置与位置指令的减法计算，而求出推进了伺服系应答延迟时间的伺服系应答位置与位置指令之差而作为轨迹误差的推测值。因此，轨迹误差e（t）如下式所示。

[式5]

$e\left(t\right)=x_r(t+t_d)-x_c\left(t\right)\·\·\·\left(5\right)$

另外，在针对所有时刻一并进行位置指令的校正的情况下，易于求出伺服系应答延迟时间量前方的伺服系应答位置，但在依次处理每时每刻的位置指令来进行位置指令的校正的情况下，有时需要在伺服系应答延迟时间量前方读位置指令。

图4是说明形状特征判定部9的动作的图。此处，记述了指令路径是直线和圆弧连续的路径的情况。在形状特征判定部9中，判别指令路径是直线还是曲线，在状态从直线向曲线变化的情况下，将其边界点处的位置坐标值作为形状特征量（边界点坐标）输出，进而将直线部分的移动方向作为形状特征量（直线部分移动方向）输出。即，在该情况下，处于从直线转移到圆弧的情况，将直线与圆弧的接合（边界点）处的坐标x_b、和其跟前的直线部分中的移动方向v_b作为形状特征量输出。

对于指令路径是曲线还是直线的判定、和边界点的判定，考虑以下举出那样的几个方法，但可以使用任意方法。即，例如是如下方法：（1）调查指令路径的曲率是否为0，如果是0则判定为直线，如果非0则判定为曲线，将曲率从0成为非0的值的点作为边界的方法；（2）调查指令路径的法线方向加速度（将通过关于时间对每时每刻的位置指令矢量进行2次微分而求出的加速度矢量在指令路径的法线方向上投影而得到的矢量的长度）是否为0，如果是0则判定为直线，如果非0则判定为曲线，将加速度矢量的长度从0成为非0的值的点作为边界点的方法；（3）对NC程序进行解析，抽出直线指令（在NC控制中通用地使用的EIA代码中的G1指令等）和曲线指令（同样的G2或者G3指令等）的边界，将从直线指令转移到曲线指令时的连接点作为边界点的方法等。另外，在这些方法中判定值是否为0时，为了避免数值运算的舍入误差的影响，也可以以是否为规定的阈值以下的判定来代替其值是否为0的判定。

图5是说明位置矢量校正部2中的、根据形状特征量和位置指令以及轨迹误差来运算校正后的位置指令的部分的动作的图。首先，将对轨迹误差e（t）乘以校正增益α并使正负反转而得到的结果设为临时校正矢量c_tmp（t）。即，

[式6]

$c_{\mathrm{tmp}}\left(t\right)=-\mathrm{\αe}\left(t\right)\·\·\·\left(6\right)$

校正增益α通常设定为0与1之间，其值越接近1，越能够使轨迹误差小。优选为，以通过设定为1来消除法线方向的误差矢量的方式校正位置指令，所以能够有效地抑制轨迹误差。

接下来，根据位置指令、临时校正矢量以及形状特征量的关系，运算校正后位置指令x（t）。将对位置指令x_c（t）加上了临时校正矢量c_tmp（t）之后的位置设为临时校正位置x_tmp（t）。此时，在校正矢量指令路径被包含的平面内，将通过边界点x_b且与移动方向v_b相同的朝向的直线定义为基准直线，在假使位置指令x_c（t）和临时校正位置x_tmp（t）相对该基准直线处于相反的位置关系的情况下，通过将使临时校正位置x_tmp（t）在基准直线上移动后的位置作为校正后位置指令x（t），使位置指令和校正后位置指令不成为相对基准直线而相反的位置关系。另一方面，在位置指令x_c（t）和临时校正位置x_tmp（t）不是相对基准直线而相反的位置关系的情况下，将临时校正位置x_tmp（t）原样地作为校正后位置指令x（t）。

如果用公式来表示用于求出校正后位置指令x（t）的一系列的运算，则如下所述。首先，将位置指令x_c（t）和临时校正位置x_tmp（t）变换为以边界点x_b为始点的矢量，分别设为x_c’、x_tmp’。

[式7]

$x_c^{\′}\left(t\right)=x_c\left(t\right)-x_b\·\·\·\left(7\right)$

[式8]

${x_{\mathrm{tmp}}}^{\′}\left(t\right)=x_{\mathrm{tmp}}\left(t\right)-x_b\·\·\·\left(8\right)$

接下来，求出通过边界点x_b且与基准直线垂直的矢量而作为基准法线矢量n_b。此处，以使基准法线矢量n_b、与位置指令和边界点位置之差x_c’的内积成为正的方式，设定基准法线矢量的方向。另外，以成为1的方式对基准法线矢量的长度进行归一化。

[式9]

$n_b^{\′}=x_c^{\′}\left(t\right)-v_b\·{x_c}^{\′}\left(t\right)$

$n_b={n_b}^{\′}/\left|{n_b}^{\′}\right|\·\·\·\left(9\right)$

接下来，求出边界点x_b至临时校正位置x_tmp（t）的矢量x_tmp’（t）与基准法线矢量n_b的内积，在其值是负的情况下，从边界点x_b如以下那样修正临时校正位置x_tmp（t）来求出校正后的位置x（t）。

[式10]

$x\left(t\right)=x_{\mathrm{tmp}}\left(t\right)-(n_b\·{x_{\mathrm{tmp}}}^{\′}\left(t\right))n_b\·\·\·\left(10\right)$

另一方面，在边界点x_b至临时校正位置的矢量x_tmp’（t）与基准法线矢量n_b的内积是0以上的情况下，将临时校正位置x_tmp（t）原样地作为校正后的位置x（t）。如果取边界点位置至通过式（10）求出的校正后位置指令的矢量、与基准法线矢量的内积，则如下式那样成为0。

[式11]

$(x\left(t\right)-x_b)\·n_b=({x_{\mathrm{tmp}}}^{\′}\left(t\right)-(n_b\·{x_{\mathrm{tmp}}}^{\′}\left(t\right))n_b)\·n_b={x_{\mathrm{tmp}}}^{\′}\left(t\right)\·n_b-n_b\·{x_{\mathrm{tmp}}}^{\′}\left(t\right)=0\·\·\·\left(11\right)$

即，可知通过式（10）求出的校正后位置指令处于基准直线上。

在本实施方式中，可动轴是2轴的情况，所以校正后的位置x（t）的第1要素x₁（t）以及第2要素x₂（t）分别成为第1轴以及第2轴的校正后的位置指令。在第1轴伺服控制部7（11）以及第2轴伺服控制部8（11）中，分别针对校正后位置指令x₁（t）、x₂（t），通过图2所示的伺服控制系11，驱动第1轴以及第2轴的马达，控制机械的可动部的位置。

图6是示出通过本实施方式进行了轨迹控制的情况的轨迹误差抑制的效果的图。图6中，（a）表示不进行位置矢量的校正的情况（将位置指令原样地输入到伺服控制部的情况）、（b）表示虽然进行位置矢量的校正但不进行形状特征判定的情况、即将上述临时校正位置x_tmp（t）始终原样地作为校正后位置指令的情况、（c）表示进行位置矢量的校正、并且进行形状特征判定的情况的轨迹，细实线表示指令路径、粗实线表示校正后位置指令的路径、虚线表示伺服系应答轨迹。指令路径成为从直线转移到圆弧的路径。在（a）的不进行位置矢量的校正的情况下，伺服系应答轨迹成为由于伺服系的应答延迟而在圆弧部分沿着内侧转的轨迹，在内侧产生轨迹误差。在（b）的虽然进行位置矢量的校正但不进行形状特征判定的情况下，校正后的位置指令向外侧鼓起来，其结果，如果位置指令从直线移动到圆弧并过一会儿，则轨迹误差成为大致0，但在直线与圆弧的接合（边界点）的附近，伺服系应答轨迹受到校正后的位置指令向外侧鼓起来的影响而向外侧鼓起来，产生轨迹误差。另一方面，在（c）的进行位置矢量的校正且还进行形状特征判定的情况下，以不从通过边界点且与直线部分的移动方向相同的方向的直线向外侧露出的方式，生成校正后的位置指令，所以在边界点附近不会产生伺服系应答轨迹的鼓起，能够抑制轨迹误差。

如以上那样，根据本实施方式1，根据判定是直线还是曲线的形状特征量判定部输出的形状特征量，运算校正后的位置，所以即使在直线和曲线连续那样的指令形状中的直线与曲线的边界部分、拐点这样的轨迹误差的大小、朝向过渡性地变化那样的情况下，也能够有效地抑制轨迹误差，轨迹精度提高。其结果，得到能够避免加工面的损伤、折痕这样的加工不良，加工面质量提高这样的效果。

另外，根据本实施方式1，把将对伺服系应答轨迹与位置指令之差乘以校正增益而得到的校正矢量加到位置指令而得到的位置作为临时校正位置，进而根据位置指令、临时校正位置、以及形状特征量的位置关系，运算校正后的位置矢量，将该位置矢量的各轴分量作为校正后的位置指令输出，从而不管指令的形状、伺服系的应答如何，都能够抑制轨迹误差。

另外，根据本实施方式1，抽出位置指令的轨迹从直线向曲线变化的部分，将其边界点的位置和即将到达边界点之前的移动方向作为形状特征量输出，位置矢量校正部在边界点的附近处位置指令和临时校正位置成为相对通过即将到达边界点之前的移动方向确定的基准直线而相反时，将使临时校正位置在基准直线上移动后的位置作为校正后的位置指令，从而以使校正前的位置指令和校正后的位置指令不成为相对基准直线而相反的位置关系的方式，运算校正后的位置矢量，所以能够抑制从直线移动到曲线的部分处的轨迹误差。

进而，根据本实施方式1，通过在伺服控制部中使用利用了规范模型的2自由度控制器，无论控制对象的特性如何，都能够根据规范模型的应答准确地计算伺服控制部的应答，进而使用该规范模型求出应答轨迹来校正位置指令，从而能够准确地校正起因于伺服控制部的应答性而产生的轨迹误差。

另外，在本实施方式1中，可动轴的数量是2，但可动轴的数量也可以是3以上。通过使伺服系应答轨迹矢量、误差矢量、校正矢量并非二维而成为3以上的维数的矢量，能够进行同样的校正。

实施方式2.

实施方式2的结构与实施方式1大致相同，其结构如图1所示。与实施方式1的相异点是校正矢量运算部3中的参照点以及校正矢量的决定方式。以下，说明这些相异点。

伺服系应答轨迹运算部1的动作与实施方式1相同。另外，位置矢量校正部2的动作中的、轨迹误差的运算的部分与实施方式1相同。图7是示出本实施方式2的形状特征判定部9的动作的图。在本实施方式中，假设了从圆弧转移到直线的情况的动作。因此，作为形状特征量（边界点坐标），输出圆弧与直线的接合（边界点）的坐标值，作为形状特征量（直线部分移动方向），输出通过了边界点之后的直线部分的移动方向。

对于指令路径是曲线还是直线的判定、和边界点的判定，考虑以下举出那样的几个方法，但可以使用任意方法。即，例如是如下方法：（1）调查指令路径的曲率是否为0，如果是0则判定为直线，如果非0则判定为曲线，将曲率从非0的值成为0的点作为边界点的方法；（2）调查指令路径的法线方向加速度（将通过关于时间对每时每刻的位置指令矢量进行2次微分而求出的加速度矢量在指令路径的法线方向上投影而得到的矢量的长度）是否为0，如果是0则判定为直线，如果非0则判定为曲线，将加速度矢量的长度从非0的值成为0的点作为边界点的方法、（3）对NC程序进行解析，抽出直线指令（在NC控制中通用地使用的EIA代码中的G1指令等）与曲线指令（同样的G2或者G3指令等）的边界，将从曲线指令转移到直线指令时的连接点作为边界点。另外，在这些方法中判定值是否为0时，为了避免数值运算的舍入误差的影响，也可以以是否为规定的阈值以下的判定来代替其值是否为0的判定。

图8是说明本发明的实施方式2中的位置矢量校正部2中的、根据形状特征量和位置指令以及轨迹误差运算校正后位置指令的部分的动作的图。运算校正后位置指令的步骤与实施方式1相同，但在本实施方式中，假设了从圆弧转移到直线的情况的动作。因此，在与边界点相比靠前的圆弧部分中，根据轨迹误差使用式（6）运算临时校正矢量c_tmp（t），进而根据临时校正矢量、边界点坐标x_b、以及移动方向v_b的位置关系，运算校正后位置指令x（t）。在求出校正后的位置时，使用式（9）来运算基准法线矢量，求出边界点x_b至临时校正位置x_tmp（t）的矢量x_tmp’（t）与基准法线矢量n_b的内积，在其值是负的情况下，使用式（10）修正临时校正位置x_tmp（t）而求出校正后的位置x（t）。另一方面，在边界点x_b至临时校正位置的矢量x_tmp’（t）与基准法线矢量n_b的内积是0以上的情况下，将临时校正位置x_tmp（t）原样地作为校正后的位置x（t）。另外，第1轴伺服控制部7以及第2轴伺服控制部8的动作与实施方式1相同。

如以上那样，根据本实施方式2，根据判定是直线还是曲线的形状特征量判定部输出的形状特征量来运算校正后的位置，所以即使在直线和曲线连续那样的指令形状中的直线与曲线的边界部分、拐点这样的轨迹误差的大小、朝向过渡性地变化那样的情况下，也能够有效地抑制轨迹误差，轨迹精度提高。其结果，得到能够避免加工面的损伤、折痕这样的加工不良，加工面质量提高这样的效果。

另外，根据本实施方式2，把将对伺服系应答轨迹与位置指令之差乘以校正增益而得到的校正矢量加到位置指令而得到的位置作为临时校正位置，进而根据位置指令、临时校正位置、以及形状特征量的位置关系，运算校正后的位置矢量，将该位置矢量的各轴分量作为校正后的位置指令输出，从而无论指令的形状、伺服系的应答如何，都能够抑制轨迹误差。

另外，根据本实施方式2，抽出位置指令的轨迹从曲线变化为直线的部分，将其边界的位置、和即将到达边界之前的移动方向作为形状特征量输出，位置矢量校正部在边界点的附近处位置指令和临时校正位置成为相对通过即将到达边界点之前的移动方向确定的基准直线而相反时，将使临时校正位置在基准直线上移动后的位置作为校正后的位置指令，从而以使校正前的位置指令和校正后的位置指令不成为相对基准直线而相反的位置关系的方式，运算校正后的位置矢量，所以能够抑制从曲线转移到直线的部分处的轨迹误差。

进而，根据本实施方式2，通过在伺服控制部中使用利用了规范模型的2自由度控制器，无论控制对象的特性如何，都能够根据规范模型的应答准确地计算伺服控制部的应答，进而通过使用该规范模型求出应答轨迹来校正位置指令，能够准确地校正起因于伺服控制部的应答性而产生的轨迹误差。

另外，在本实施方式2中，可动轴的数量是2，但可动轴的数量也可以是3以上。通过使伺服系应答轨迹矢量、误差矢量、校正矢量并非二维而成为3以上的维数的矢量，能够进行同样的校正。

实施方式3.

实施方式3的结构与实施方式2的结构大致相同，其整体结构如图1所示。与实施方式2的相异点是伺服系应答轨迹运算部的动作。另外，作为指令路径的形状，提供朝向不同的连续的圆弧指令。

图9是说明本实施方式3中的伺服系应答轨迹运算部1的动作的图。在本实施方式3中，对于伺服系的应答轨迹，并非使用伺服系的模型来求出，而仅使用位置指令的信息来求出。在指令形状是圆弧等平滑地变化的形状的情况下，具有在指令路径的加速度方向上与加速度的大小成比例地产生轨迹误差的性质。该比例系数由伺服系的应答性确定。通过对时刻t下的位置指令加上误差矢量e（t）来求出该时刻t下的伺服系应答的位置。通过对位置指令的加速度矢量d²x_c/dt²乘以系数a来求出轨迹误差矢量e（t）。以与伺服系应答延迟时间t_d的平方成比例的方式，确定该系数a。对于该关系式，除了对指令加速度和轨迹误差量的关系进行实际测量而求出的方法以外，还可以通过解析地求出圆弧指令时的轨迹误差量的逻辑值与指令加速度之比的方法来设定。在后者的方法中，首先，通过从指令半径减去对伺服系的频率应答传递函数的绝对值乘以指令半径而得到的值，求出圆弧指令时的轨迹误差量，并将该轨迹误差量除以指令加速度而求出。如果用公式来表示，则如下所述。在半径R、各速度ω的圆弧指令时的情况下，使用伺服系的传递函数G（s），如以下提供轨迹误差量与指令加速度之比。

[式12]

$a=\frac{R(1-|G\left(\mathrm{j\ω}\right)\left|\right)}{{\mathrm{R\ω}}^2}=\frac{(1-|G\left(\mathrm{j\ω}\right))}{{\mathrm{\ω}}^2}\·\·\·\left(12\right)$

此处，j是虚数单位。

对于图2所示的伺服控制系，传递函数如式（1）所示，伺服系应答延迟时间如式（4）所示。如果使用式（12）来求出第2模型增益是第1模型增益的4倍的情况、即是K₂=4K₁的情况的、轨迹误差量与伺服系应答延迟时间的关系，则如下式所示。

[式13]

$a=\frac{1}{4}\frac{1}{{K_1}^2}=\frac{1}{4}{t_d}^2\·\·\·\left(13\right)$

图10是示出本实施方式3的形状特征判定部9的动作的图。在本实施方式中，假设了连续指令了朝向不同的圆弧的情况的动作。因此，作为形状特征量（边界点坐标），输出圆弧与圆弧的接合（边界点）的坐标值，作为形状特征量（直线部分移动方向），输出边界点处的移动方向。此时，移动方向成为边界点处的曲线的切线方向。其能够解释为在第一个圆弧与第二个圆弧之间存在微小的直线部分的情况。

对于边界点的判定，考虑以下举出那样的几个方法，但可以使用任意方法。即，例如是如下方法：（1）调查指令路径的曲率是否为0，如果是0则判定为直线，如果非0则判定为曲线，并且调查曲率中心的方向朝向行进方向处于右侧还是处于左侧，将曲率中心的方向变化的点作为边界点的方法；（2）调查指令路径的法线方向加速度（将通过关于时间对每时每刻的位置指令矢量进行2次微分而求出的加速度矢量在指令路径的法线方向上投影而得到的矢量的长度）是否为0，如果是0则判定为直线，如果非0则判定为曲线，进而，调查加速度矢量与速度矢量的外积，将其符号变化的点作为边界点的方法；（3）对NC程序进行解析，抽出旋转的朝向不同的曲线指令（在NC控制中通用地使用的EIA代码中的G2指令和G3指令等）的边界方法。另外，在这些方法中判定值是否为0时，为了避免数值运算的舍入误差的影响，也可以以是否为规定的阈值以下的判定来代替其值是否为0的判定。

图11是说明本实施方式3中的位置矢量校正部2中的、根据形状特征量、位置指令以及轨迹误差运算校正后位置指令的部分的动作的图。运算校正后位置指令的步骤与实施方式1相同，但在本实施方式中，假设了连续指令了朝向不同的圆弧的情况的动作。因此，在与边界点相比靠前的圆弧部分、和超过了边界点之后的圆弧部分的各个中，根据轨迹误差使用式（6）来运算临时校正矢量c_tmp（t），进而根据临时校正矢量、边界点坐标x_b、以及移动方向v_b的位置关系，运算校正后位置指令x（t）。在求出校正后的位置时，使用式（9）来运算基准法线矢量，求出边界点x_b至临时校正位置x_tmp（t）的矢量x_tmp’（t）与基准法线矢量n_b的内积，在其值是负的情况下，使用式（10）修正临时校正位置x_tmp（t）来求出校正后的位置x（t）。另一方面，在边界点x_b至临时校正位置的矢量x_tmp’（t）与基准法线矢量n_b的内积是0以上的情况下，将临时校正位置x_tmp（t）原样地作为校正后的位置x（t）。另外，第1轴伺服控制部7以及第2轴伺服控制部8的动作与实施方式1相同。

如以上那样，根据本实施方式3，根据判定是直线还是曲线的形状特征量判定部输出的形状特征量，运算校正后的位置，所以即使在直线和曲线连续那样的指令形状中的直线与曲线的边界部分、拐点这样的轨迹误差的大小、朝向过渡性地变化那样的情况下，也能够有效地抑制轨迹误差，轨迹精度提高。其结果，得到能够避免加工面的损伤、折痕这样的加工不良，加工面质量提高这样的效果。

另外，根据本实施方式3，把将对伺服系应答轨迹与位置指令之差乘以校正增益而得到的校正矢量加到位置指令而得到的位置作为临时校正位置，进而根据位置指令、临时校正位置、以及形状特征量的位置关系运算校正后的位置矢量，将该位置矢量的各轴分量作为校正后的位置指令输出，从而无论指令的形状、伺服系的应答如何，都能够抑制轨迹误差。

另外，根据本实施方式3，抽出位置指令的轨迹的拐点的部分、即连续指令了朝向不同的圆弧的情况的连接点，将其边界的位置、和边界处的移动方向作为形状特征量输出，位置矢量校正部在边界点的附近处位置指令和临时校正位置成为相对通过即将到达边界点之前的移动方向确定的基准直线而相反时，通过将使临时校正位置在基准直线上移动后的位置作为校正后的位置指令，以使校正前的位置指令和校正后的位置指令不成为相对基准直线而相反的位置关系的方式，运算校正后的位置矢量，所以能够抑制指令曲线的拐点的部分中的轨迹误差。

进而，根据本实施方式3，不使用伺服控制部的模型，而仅通过位置指令轨迹的信息求出伺服系应答轨迹，从而能够减轻用于求出伺服系应答轨迹的运算负载。

另外，在本实施方式3中，可动轴的数量是2，但可动轴的数量也可以是3以上。通过使伺服系应答轨迹矢量、误差矢量、校正矢量并非二维而成为3以上的维数的矢量，能够进行同样的校正。

产业上的可利用性

如以上那样，本发明的轨迹控制装置适用于通过用多个可动轴驱动机械的可动部、并同时控制多个可动轴的马达来控制可动部的轨迹的轨迹控制装置。

Claims (7)

1.一种轨迹控制装置，通过用多个可动轴驱动机械的可动部，并同时控制所述多个可动轴的马达来控制所述可动部的轨迹，其特征在于，具备：

伺服系应答轨迹计算部，根据各可动轴的位置指令，运算伺服系应答轨迹；

形状特征判定部，根据所述位置指令，判定指令路径的形状是直线还是曲线，输出形状特征量；

位置矢量校正部，根据所述位置指令、所述伺服系应答轨迹以及所述形状特征量，通过矢量运算校正位置指令，输出校正后的位置指令；以及

多个伺服控制部，通过以使各可动轴的位置追踪所述校正后位置指令的方式，输出马达驱动转矩，控制各可动轴的马达。

2.根据权利要求1所述的轨迹控制装置，其特征在于，

所述位置矢量校正部把将对所述伺服系应答轨迹与所述位置指令之差乘以校正增益而得到的校正矢量加到所述位置指令而得到的位置作为临时校正位置，进而根据所述位置指令、所述临时校正位置以及所述形状特征量的位置关系，运算校正后的位置矢量，将该位置矢量的各轴分量作为所述校正后的位置指令输出。

3.根据权利要求2所述的轨迹控制装置，其特征在于，

所述形状特征判定部抽出所述位置指令的轨迹从直线变化为曲线的部分，将其边界点的位置和即将到达边界点之前的移动方向作为所述形状特征量输出，

所述位置矢量校正部在所述边界点的附近处所述位置指令和所述临时校正位置成为相对通过即将到达所述边界点之前的移动方向确定的基准直线而相反时，将使所述临时校正位置在所述基准直线上移动后的位置作为所述校正后的位置指令，从而以使校正前的位置指令和校正后的位置指令不成为相对所述基准直线而相反的位置关系的方式，运算校正后的位置矢量。

4.根据权利要求2所述的轨迹控制装置，其特征在于，

所述形状特征判定部抽出所述位置指令的轨迹从曲线变化为直线的部分，将其边界点的位置和紧接着边界点之后的移动方向作为所述形状特征量输出，

所述位置矢量校正部在所述边界点的附近处所述位置指令和所述临时校正位置成为相对通过紧接着所述边界点之后的移动方向确定的基准直线而相反时，将使所述临时校正位置在所述基准直线上移动后的位置作为所述校正后的位置指令，从而以使校正前的位置指令和校正后的位置指令不成为相对所述基准直线而相反的位置关系的方式，运算校正后的位置矢量。

5.根据权利要求2所述的轨迹控制装置，其特征在于，

所述形状特征判定部抽出所述位置指令的轨迹的法线矢量的朝向反转的部分，将其边界点的位置和边界点处的移动方向作为所述形状特征量输出，

所述位置矢量校正部在所述边界点的附近处所述位置指令和所述临时校正位置成为相对通过所述边界点处的移动方向确定的基准直线而相反时，将使所述临时校正位置在所述基准直线上移动后的位置作为所述校正后的位置指令，从而以使校正前的位置指令和校正后的位置指令不成为相对所述基准直线成为相反的位置关系的方式，运算校正后的位置矢量。

6.根据权利要求1所述的轨迹控制装置，其特征在于，

所述伺服控制部具有使用了规范模型部的2自由度控制器，

所述伺服系应答轨迹计算部输出对与在所述伺服控制部中使用的所述规范模型部相同的模型提供了所述位置指令时的输出轨迹，而作为所述伺服系应答轨迹。

7.根据权利要求1所述的轨迹控制装置，其特征在于，

所述伺服系应答轨迹计算部输出从所述位置指令的轨迹减去对各时刻下的所述位置指令的加速度矢量乘以由所述伺服控制部的应答性决定的规定的系数而得到的矢量而得到的轨迹，而作为所述伺服系应答轨迹。

Images