CN104204977A - 轨迹控制装置 - Google Patents

Abstract

本发明的实施方式的轨迹控制装置(10)是对多个电动机进行控制而控制可动部的轨迹的轨迹控制装置，其具有：插补·加减速运算部(1)，其对指令路径进行插补，并沿该指令路径执行加减速运算而求出加减速后插补路径；轴分配部(3)，其从加减速后插补路径生成各可动轴的位置指令；伺服响应运算部(51、52)，其基于位置指令，运算其伺服响应；切线方向伺服响应运算部(50)，其从加减速后插补路径求出切线方向伺服响应；基准点生成部(2)，其从切线方向伺服响应求出基准点；位置矢量校正部(4)，其通过基于伺服响应以及基准点而运算校正矢量，并以校正矢量对各可动轴的位置指令进行校正，而输出各可动轴的校正后位置指令；以及伺服控制部(6)、(7)，其向各可动轴的电动机分别输出电动机驱动扭矩，以使得各可动轴追随各自的校正后位置指令。

Description

轨迹控制装置

技术领域

本发明涉及一种在工作机械、激光加工机等控制装置中用于抑制轨迹误差并实现高速高精度加工的轨迹控制装置。

背景技术

在使用工作机械、激光加工机等机械进行加工的情况下，进行控制，以使得加工工具相对于工件的位置沿着所指令的路径。该控制被称为轨迹控制，通常通过伺服控制实现，以使机械的各可动轴的实际位置追随根据指令路径求出的各可动轴的位置指令。

作为进行轨迹控制时的问题点，由于各可动轴的控制系统的响应延迟等，存在实际的轨迹从所指令的路径偏离这样的问题。通常由于针对机械的各可动轴进行控制，所以各可动轴的控制系统的响应延迟等引起的误差，会导致各可动轴的伺服系统响应延迟于位置指令而移动。在如直线那样指令路径的移动方向不发生变化的情况下，即使各轴延迟地移动，作为伺服系统响应的轨迹也不会从指令路径上偏离。即，在指令路径的切线方向出现误差，但不会在指令路径的法线方向出现误差。另一方面，在如曲线、角形状等那样指令路径的移动方向发生变化的情况下，由于各轴的伺服控制系统的延迟，在指令路径的法线方向上会出现误差。以下，在伺服系统响应位置相对于位置指令的误差之中，将指令路径的切线方向的成分称为追随误差，将指令路径的法线方向的成分称为轨迹误差。通常，如果发生轨迹误差，则加工形状变为与本来的形状不一致，因此不是优选。

作为用于抑制这些轨迹误差的方法，专利文献1中公开了一种方法，该方法基于预先读取程序而识别出的加工形状，运算出用于将误差抑制为一定值以下的最佳进给速度，并且运算出以该速度进行加工时的误差量，然后将用于消除该误差的校正矢量与原来的指令位置相加而对指令位置进行校正。此处，校正矢量的方向是与移动方向垂直的方向(法线方向)，校正矢量的长度是将法线方向加速度(将速度的平方除以曲率半径而得到的值)乘以规定的系数而得到的值。

此外，在非专利文献1中公开有一种方法，该方法通过将伺服系统的响应误差分解到指令路径的切线方向和法线方向，使用对各个方向成分分别乘以不同的增益而得到的控制输入，进行反馈控制，从而独立地控制追随误差和轨迹误差。

专利文献1：日本特开平6-282321号公报

非专利文献1：精密工学会刊Vol.74.No.11.2008 pp.1193-1198

发明内容

然而，在上述现有技术的方法中，存在下述问题，即，在指令点间的路径上的加速度由于加减速的影响而变化的情况、以微小线段指令对包含有3维形状的复杂形状进行近似并指令这样的情况下，不能对应。在指令路径的起点附近以及终点附近、在指令路径上指令进给速度发生变化的位置附近，如果进给速度突然改变，则会超过作为控制对象的机械的容许加速度，因此通常进行使进给速度逐渐增加或者逐渐减小的加减速。

例如，在专利文献1中，基于进行加减速之前的指令点进行运算，进而运算出的误差量是进给速度处于恒定状态(稳态)下的误差量，所以存在下述问题，即，在如正进行加减速这样的进给速度变化的状态(过渡状态)下，所运算的误差量与实际产生的误差量不同，作为校正结果而得到的响应轨迹与原来的指令轨迹不同，响应轨迹的形状发生变形。

此外，在非专利文献1的方法中，由于将控制输入设定在与指令路径垂直的方向或者与指令路径相同的方向上，所以在角形状的指令、包含3维形状的指令中，进行校正的方向、校正的大小剧烈地变化或者变得不确定。如果在如上述的状况下进行校正，则会存在对包含机械的驱动系统产生过大的负载、振动这样的问题。此外，在非专利文献1中，由于对伺服系统响应误差进行反馈，所以存在有可能控制系统变得不稳定这样的问题。

本发明就是鉴于上述问题而提出的，其目的在于得到一种能够将轨迹误差抑制得充分小，且能够在高速下实现高精度加工的轨迹控制装置。

为了解决上述课题并实现目的，本发明是一种轨迹控制装置，其通过同时控制多个可动轴的电动机而对可动部的轨迹进行控制，该轨迹控制装置的特征在于，具有：插补·加减速运算部，其通过对被供给的指令路径进行插补，并且沿着所述指令路径执行加减速运算，而求出加减速后插补路径；轴分配部，其根据所述加减速后插补路径，生成各可动轴的位置指令；伺服响应运算部，其基于各可动轴的所述位置指令，运算各可动轴的伺服响应；切线方向伺服响应运算部，其基于所述加减速后插补路径，求出切线方向移动量的切线方向伺服响应；基准点生成部，其根据所述切线方向伺服响应，求出校正基准点；位置矢量校正部，其基于各可动轴的所述伺服响应以及所述校正基准点而对用于抑制轨迹误差的校正矢量进行运算，并利用所述校正矢量对各可动轴的所述位置指令进行校正，由此输出各可动轴的校正后位置指令；以及伺服控制部，其向各可动轴的所述电动机分别输出电动机驱动扭矩，以使得各可动轴的位置分别追随各可动轴的所述校正后位置指令。

发明的效果

根据本发明，能够实现下述效果，即，得到一种即使指令形状是3维形状，或者由微小线段构成，也能够正确地求出校正矢量，能够将轨迹误差抑制得充分小且能够在高速下实现高精度加工的轨迹控制装置。

附图说明

图1是表示出本发明的实施方式1涉及的轨迹控制装置的结构的框图

图2是表示本发明的实施方式1涉及的伺服控制部的结构的框图。

图3-1是对本发明的实施方式1涉及的加减速运算的详细内容进行说明的图。

图3-2是表示出本发明的实施方式1涉及的切线方向速度q’(t)的加减速图案的图。

图3-3是表示出本发明的实施方式1涉及的由加减速后插补点形成的加减速后插补路径的图。

图4是对本发明的实施方式涉及的位置矢量校正部的动作进行说明的图。

图5是表示本发明的实施方式2涉及的轨迹控制装置的结构的框图。

具体实施方式

以下，基于附图，对本发明涉及的轨迹控制装置的实施方式进行详细说明。此外，本发明并不限定于本实施方式。

实施方式1

图1是表示出本发明的实施方式1涉及的轨迹控制装置10的结构的框图。将指令路径以NC程序等形式作为机械的可动部轨迹的轨迹上的座标值而供给至插补·加减速运算部1。此外，也将所指令的座标值间的插补方法(直线·圆弧·样条曲线等)以及沿轨迹方向的移动速度即进给速度利用NC程序等而同时供给至插补·加减速运算部1。在插补·加减速运算部1中，通过用所指定的方法对所指令的座标值间进行插补，并且，进行沿着指令路径以另行指定的规定的加速度或者加减速时间常数进行加速以及减速的运算，从而对加减速后插补路径进行运算。

此外，被供给加减速后插补路径的轴分配部3对机械的各可动轴的位置指令进行运算，以使得机械的可动部经过加减速后插补路径。在本实施方式中设为有2个可动轴，作为对第1轴以及第2轴的位置指令而运算第1轴位置指令以及第2轴位置指令。例如，将机械的X轴设为第1可动轴(第1轴)、将机械的Y轴设为第2可动轴(第2轴)。第1轴伺服响应运算部51以及第2轴伺服响应运算部52分别根据第1轴位置指令和第2轴位置指令运算并输出第1轴伺服响应和第2轴伺服响应。

切线方向伺服响应运算部50根据加减速后插补路径的切线方向移动量，运算并输出切线方向伺服响应。基准点生成部2根据切线方向伺服响应，求出并输出基准点的位置。并且，在位置矢量校正部4中，根据第1轴伺服响应、第2轴伺服响应以及基准点的位置求出校正矢量，将校正矢量的各可动轴方向的成分与作为各可动轴的位置指令的第1轴位置指令以及第2轴位置指令相加，从而运算第1轴校正后位置指令以及第2轴校正后位置指令，作为各可动轴的校正后位置指令。第1轴伺服控制部6以及第2轴伺服控制部7分别输出作为第1轴以及第2轴的电动机驱动扭矩的第1轴电动机驱动扭矩以及第2轴电动机驱动扭矩，以使得第1轴的位置以及第2轴的位置分别追随第1轴校正后位置指令以及第2轴校正后位置指令。

第1轴伺服控制部6以及第2轴伺服控制部7为相同结构，在图2中，作为伺服控制部11而示出表示该第1轴伺服控制部6以及第2轴伺服控制部7结构的框图。输入到伺服控制部11的校正后位置指令利用减法器20减去模型位置之后，利用模型增益乘法器21乘以第1模型增益K₁，并利用减法器22减去模型速度，进一步利用模型增益乘法器23乘以第2模型增益K₂，而输出模型加速度。利用积分器24对模型加速度进行积分而输出模型速度，利用积分器25对模型速度进行积分而输出模型位置。将输入校正后位置指令且输出模型位置、模型速度、模型加速度的模块称为标准模型部12。

利用加减法器26从由积分器25输出的模型位置减去由电动机32输出的电动机位置信号而输出位置误差。位置控制部27对位置误差进行比例控制等控制。在加减法器28中，将模型速度与位置控制部27的输出相加后，进一步减去由电动机32输出的电动机速度信号而输出速度误差。速度控制部29对速度误差进行比例·积分控制等控制。利用乘法器30在模型加速度上乘以相当于控制对象的惯量的值而运算模型扭矩，利用加法器31将模型扭矩与速度控制部29的输出相加而输出电动机扭矩信号。电动机扭矩信号输入至电动机32。由电动机32以及负载33构成的机械系统13通过电动机扭矩信号而驱动，将电动机速度信号和电动机位置信号输出到伺服控制部11。

该伺服控制部11是使用了标准模型的2自由度控制器，能够独立地设计针对指令的追随性以及针对扰动的响应性。电动机位置对位置指令的追随性由第1模型增益K₁以及第2模型增益K₂决定，电动机位置对扰动的响应性由位置控制部27以及速度控制部29的设计决定。因此，将电动机位置的响应与实际的控制对象的特性无关地控制为追随标准模型的输出即模型位置。

在轨迹控制时为了不使响应轨迹偏移，需要使各可动轴的位置响应(大致)相同。位置响应是指从伺服控制部11的输入至电动机位置信号为止的响应特性，如果该响应特性在各可动轴之间不同，则在指令出直线形状时响应轨迹从所指令的直线偏离，或者针对圆弧指令的响应轨迹向倾斜方向偏移。在本实施方式中，在第1轴伺服控制部6以及第2轴伺服控制部7中，将标准模型的增益设定为相同值。即，将第1轴伺服控制部6的标准模型的第1模型增益与第2轴伺服控制部7的标准模型的第1模型增益设定为相同值，将第1轴伺服控制部6的标准模型的第2模型增益与第2轴伺服控制部7的标准模型的第2模型增益设定为相同值。由于电动机位置的响应由标准模型的特性决定，所以如果使用完全相同的标准模型就能够得到相同的位置响应。

接下来，对各部分的运算的详细内容进行说明。插补·加减速运算部1用指定的方法对所指令的座标值间进行插补，进一步进行加减速运算，而对指令路径上的每个插补周期的指令位置进行运算。插补的方法有直线插补、圆弧插补、样条曲线插补等。插补周期是作为轨迹控制装置10的规格而确定的恒定周期，通常使用小于或等于数m_s的较短的周期。插补周期较短能够进行高精度的轨迹控制，但用于运算的处理器等的处理负载增大。将作为插补·加减速的结果而得到的每个插补周期的点称为加减速后插补点，将由加减速后插补点形成的路径称为插补路径。加减速运算是用于防止指令加速度超过机械的各可动轴的容许加速度而过大的运算，是以使速度的变化小于或等于由参数等另行指定的加速度的方式对插补路径上的插补点进行重新运算。即，在从起点移动至终点的情况下，在起点之后为了立即使速度逐渐增加而将插补点间的间隔缩短，在终点之后为了立即使速度逐渐减小而将插补点间的间隔缩短。

使用图3-1～图3-3所示的例子说明该加减速运算的详细内容。考虑下述情况，即如图3-1所示，从起点P₀＝(x₀，y₀)经过中途的点P₁＝(x₁，y₁)以及P₂＝(x₂，y₂)到终点P₃＝(x₃，y₃)为止以速度F移动。将该起点、经过点、终点称为指令点。将指令点在图3-1中以白色圈表示。各指令点的座标值由NC程序等指令。将沿着从起点P₀至任意时刻t的路径上的点P(t)为止的指令路径的距离设为q(t)。将从起点到终点为止的移动长度设为L。L是连接P₀、P₁、P₂、P₃的各线段的长度之和。如果将从P_n-1到P_n为止的线段长度设为l_n(其中n＝1、2、3)，则L＝l₁+l₂+l₃。

接着，将q(t)的时间微分设为切线方向速度q’(t)，将q’(t)的加减速图案设定为如图3-2所示。其是高度为指令进给速度F，面积为从起点到终点为止的移动长度L的梯形的图案，设定为加速部以及减速部的斜率成为由参数等另行指定的加速度。此处，加速以及减速以直线状进行，但有时也设为S字状而使机械的动作更加平稳。通过对切线方向速度q’(t)进行时间积分，而求出加减速后的移动长度q_c(t)。将Δt设为插补周期，而计算t＝N×Δt(N为正整数)的加减速后移动长度q_c(N×Δt)，从起点位置P₀沿指令路径前进了长度q_c(N×Δt)的点成为第N个加减速后插补点。

由加减速后插补点形成的加减速后插补路径如图3-3所示。作为加减速后插补路径的信息，包含：加减速后的切线方向移动长度q_c(t)、插补的种类(直线插补、圆弧插补、样条曲线插补等)、插补中必需的参数(起点位置、终点位置、其它形状参数)。作为其它形状参数，在圆弧插补的情况下有中心位置和半径等参数，在样条曲线插补的情况下有样条曲线函数的系数等参数。

轴分配部3求出加减速后插补路径上的加减速后插补点的各轴的座标值。在图3所示的例子的情况下，例如表示在线段P₁P₂上的加减速后插补点P_c的座标的矢量p_c＝(x_c，y_c)通过如下过程求出，即，在表示P_c所属的线段的起点P₁的矢量p₁＝(x₁，y₁)和表示终点P₂的矢量p₂＝(x₂，y₂)之间，使用由移动长度q_c(t)确定的比率进行直线插补。因此，表示时刻t的位置指令P_c(t)的座标值的位置指令矢量p_c(t)＝(x_c(t)，y_c(t))由下式(1)表示。

【式1】

$p_c\left(t\right)=\frac{q_c\left(t\right)-l_1}{|p_2-p_1|}(p_2-p_1)+p_1...\left(1\right)$

其中，第1轴位置指令为x_c(t)、第2轴位置指令为y_c(t)。

在第1轴伺服响应运算部51以及第2轴伺服响应运算部52中分别运算针对第1轴位置指令以及第2轴位置指令的伺服响应(第1轴伺服响应以及第2轴伺服响应)。伺服响应的运算以下述方式进行。如前述，伺服系统的响应能够用标准模型的响应表示，所以通过求出在第1轴以及第2轴位置指令作为输入而供给的情况下的标准模型的输出即模型位置，而分别求出第1轴以及第2轴的伺服系统响应位置。并且，将第1轴以及第2轴的伺服系统响应位置作为各可动轴方向的成分的矢量设为伺服响应位置矢量，将该位置矢量所绘出的轨迹作为伺服响应轨迹。

标准模型的输出即模型位置，能够通过以微分方程式、差分方程式、传递函数等形式表示图2所示的标准模型部12，并利用数值计算而计算其解而求出。此外，在已知指令形状的情况下，能够通过积分计算而求出解析解。

例如，图2所示的标准模型的传递函数Gm(s)由下式(2)表示。

【式2】

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

向标准模型供给某个输入时的输出，通过对标准模型的传递函数和被供给的输入的拉普拉斯变换的积进行拉普拉斯逆变换而求出。针对位置指令矢量p_c(t)的伺服响应位置P_s(t)的矢量p_s(t)＝(x_s(t)，y_s(t))由下式(3)表示。

【式3】

$p_s\left(t\right)=\left[\begin{array}{c}{x}_s\left(t\right)\\ y_s\left(t\right)\end{array}\right]=\left[\begin{array}{c}{L}^{-1}\left[G_m\left(s\right)L\right[x_c\left(t\right)\left]\right]\\ L^{-1}\left[G_m\left(s\right)L\right[y_c\left(t\right)\left]\right]\end{array}\right]...\left(3\right)$

其中，L[f(t)]表示f(t)的拉普拉斯变换，L^-1[F(s)]表示F(s)的拉普拉斯逆变换。第1轴的伺服响应为x_s(t)，第2轴的伺服响应为y_s(t)。

与第1轴伺服响应运算部51以及第2轴伺服响应运算部52同样地，在切线方向伺服响应运算部50中求出针对加减速后的切线方向移动长度q_c(t)的伺服响应即切线方向伺服响应q_s(t)。但是，在求出位置的伺服响应的情况下将矢量用作输入，但在切线方向移动长度的情况下输入是标量值。切线方向伺服响应q_s(t)能够通过对标准模型的传递函数和被供给的输入q_c(t)的拉普拉斯变换的积进行拉普拉斯逆变换而求出，由下式(4)表示。

【式4】

q_s(t)＝L^-1[G_m(s)L[q_c(t)]]       …(4)

接下来，在基准点生成部2中求出从起点沿着加减速后插补路径前进了切线方向伺服响应的长度的点P_r(t)，并将该点作为基准点(校正基准点)。基准点的座标p_r(t)以下述方式求出。首先，根据切线方向伺服响应q_s(t)和各指令点间的线段长度l_n，确定基准点属于哪个线段。即，如果q_s(t)小于l₁，则确定为在P₀P₁上，如果q_s(t)大于或等于l₁而小于l₁+l₂，则确定为在P₁P₂上。此处，设为q_s(t)在P₁P₂上。接着，通过使用由切线方向伺服响应的长度q_r(t)确定的比率在基准点所属于的线段的起点和终点之间进行直线插补，而确定基准点的座标。基准点的座标p_r(t)＝(x_r(t)，y_r(t))由下式(5)求出。

【式5】

$p_r\left(t\right)=\frac{q_r\left(t\right)-l_1}{|p_2-p_1|}(p_2-p_1)+p_1...\left(5\right)$

在位置矢量校正部4中将从伺服响应位置P_s(t)到基准点P_r(t)为止的矢量作为校正矢量，将加减速后的插补点P_c(t)的位置矢量与校正矢量相加而得到的矢量作为校正后的位置矢量。即，通过将第1轴位置指令x_c(t)以及第2轴位置指令y_c(t)分别与校正矢量的第1轴成分和第2轴成分相加，而分别运算第1轴校正后位置指令和第2轴校正后位置指令。

在图4中示出位置矢量校正部中的校正矢量的关系。校正矢量是从伺服响应位置P_s(t)朝向考虑切线方向伺服响应而求出的基准点P_r(t)的矢量，其是使伺服响应的从指令路径的偏移反向而得到的矢量，是与所指令的轨迹大致垂直的方向。因此，能够通过将该校正矢量与位置指令(第1轴位置指令x_c(t)以及第2轴位置指令y_c(t))相加，而对伺服响应的从指令路径的偏移进行校正。

在不使用考虑了切线方向伺服响应的基准点的情况下，例如，如果将从伺服响应P_s(t)到位置指令的插补点P_c(t)为止的矢量用作校正矢量，则不仅包含与指令路径垂直的方向的误差成分，还包含与指令路径平行的方向的误差成分。此外，也考虑从伺服响应P_s(t)向指令路径作出垂线的方法，但如果采用这种方法，在如伺服响应经过锐角的角部内侧这样的情况下，会产生多个可作出垂线的方向的候选，不能唯一地确定校正矢量。

如上述，根据本实施方式1，由于使用切线方向的伺服响应求出校正的基准点，并基于进行了插补以及加减速处理之后的插补路径信息而运算校正矢量，所以即使指令形状为三维形状或由微小线段构成，也能够正确地求出校正矢量。由此，能够得到下述轨迹控制装置，即，其将轨迹误差抑制得充分小，并且能够在高速下实现高精度加工。

此外，通过使用具有与实际的可动轴的位置响应(大致)相同的响应特性的滤波器进行伺服响应的运算，能够正确地推测伺服响应。

此外，通过使用具有与实际的可动轴的位置响应(大致)相同的响应特性的滤波器进行可动轴的位置响应伺服响应的运算，能够正确地推测基准点的位置。

此外，通过考虑(校正)指令路径和切线方向长度的伺服响应而设定基准点，从而能够正确地确定校正矢量的方向和长度。

并且，通过将从至伺服响应位置为止的矢量起至基准点为止的矢量作为校正矢量而与位置指令相加，从而能够正确地校正从基准点至伺服响应位置为止的轨迹误差。

实施方式2

图5是表示本发明的实施方式2涉及的轨迹控制装置15的结构的框图。与实施方式1的不同点在于，机械的可动轴有3个，针对各个可动轴设置伺服控制部(第1轴伺服控制部6、第2轴伺服控制部7、以及第3轴伺服控制部8)和伺服响应运算部(第1轴伺服响应运算部51、第2轴伺服响应运算部52、以及第3轴伺服响应运算部53)。除此以外的结构与实施方式1相同。

在本实施方式中，对具有X轴、Y轴、Z轴这3个可动轴的机械进行包含3维形状的指令。将X轴设为第1轴，将Y轴设为第2轴，将Z轴设为第3轴。伺服控制部(第1轴伺服控制部6、第2轴伺服控制部7、以及第3轴伺服控制部8)各自的标准模型增益对于第1轴·第2轴·第3轴设定为相同。

以下，对与实施方式1不同的部分进行叙述。

在轴分配部3中求出加减速后插补路径上的加减速后插补点的各轴的座标值。在本实施方式中，在图3-1～图3-3中，对各点添加Z轴的座标。即，在图3-1中，设为从起点P₀＝(x₀，y₀，z₀)经过中途的点P₁＝(x₁，y₁，z₁)以及P₂＝(x₂，y₂，z₂)至终点P₃＝(x₃，y₃，z₃)为止以速度F移动。表示在线段P₁P₂上的加减速后插补点P_c的座标的矢量p_c＝(x_c，y_c，z_c)通过如下过程求出，即，在表示P_c所属的线段的起点P₁的矢量p₁＝(x₁，y₁，z₁)和表示终点P₂的矢量p₂＝(x₂，y₂，z₂)之间，使用由移动长度q_c(t)确定的比率进行直线插补。因此，表示时刻t的位置指令P_c(t)的座标值的位置指令矢量p_c(t)＝(x_c(t)，y_c(t)，z_c(t))由下式(6)表示。

【式6】

$p_c\left(t\right)=\frac{q_c\left(t\right)-l_1}{|p_2-p_1|}(p_2-p_1)+p_1...\left(6\right)$

其中，第1轴位置指令为x_c(t)，第2轴位置指令为y_c(t)，第3轴位置指令为z_c(t)。

在第3轴伺服响应运算部53中进行与第1轴响应运算部51以及第2轴伺服响应运算部52相同的运算，对针对第3轴位置指令的伺服响应进行运算。

针对位置指令矢量p_c(t)的伺服响应位置P_s(t)的矢量p_s(t)＝(x_s(t)，y_s(t)，z_s(t))由下式(7)表示。

【式7】

$p_s\left(t\right)=\left[\begin{array}{c}{x}_s\left(t\right)\\ y_s\left(t\right)\\ z_s\left(t\right)\end{array}\right]=\left[\begin{array}{c}{L}^{-1}\left[G_m\left(s\right)L\right[x_c\left(t\right)\left]\right]\\ L^{-1}\left[G_m\left(s\right)L\right[y_c\left(t\right)\left]\right]\\ L^{-1}\left[G_m\left(s\right)L\right[z_c\left(t\right)\left]\right]\end{array}\right]...\left(7\right)$

第1轴伺服响应为x_s(t)、第2轴伺服响应为y_s(t)、第3轴伺服响应为z_s(t)。

接下来，基准点生成部中的基准点的座标p_r(t)＝(x_r(t)，y_r(t)，z_r(t))由下式(8)求出。

【式8】

$p_r\left(t\right)=\frac{q_r\left(t\right)-l_1}{|p_2-p_1|}(p_2-p_1)+p_1...\left(8\right)$

在本实施方式2中，仅是矢量的要素从2要素变为3要素，其它运算与实施方式1完全相同地进行，从而能够校正伺服响应与指令路径之间的误差。

由以上，根据本实施方式2，即使是包含3维形状的指令，也能够通过使用切线方向的伺服响应求出校正的基准点，并基于进行了插补以及加减速处理之后的插补路径信息而运算校正矢量，而正确地求出校正矢量。

此外，本发明申请不限于上述实施方式，能够在实施阶段在不脱离其要旨的范围内进行各种变形。此外，在上述实施方式中包含各种阶段的发明，能够从所公开的多个技术特征的适当的组合提取出各种发明。例如，即使从上述实施方式所示出的全部技术特征中删除一些技术特征，在能够解决发明内容中所记载的课题，能够得到发明的效果中所记载的效果的情况下，也能够将删除了该技术特征的结构作为发明而提出。此外，也可以对不同实施方式所涉及的结构要素进行适当组合。

工业实用性

如以上，本发明涉及的轨迹控制装置在工作机械、激光加工机等控制装置中是有用的，尤其适用于要求抑制轨迹误差的高速高精度加工的控制装置中。

标号的说明

1插补·加减速运算部，2基准点生成部，3轴分配部，4位置矢量校正部，6第1轴伺服控制部，7第2轴伺服控制部，8第3轴伺服控制部，10、15轨迹控制装置，11伺服控制部，12标准模型部，13机械系统，20减法器，21模型增益乘法器，22减法器，23模型增益乘法器，24、25积分器，26、28加减法器，27位置控制部，29速度控制部，30乘法器，31加法器，32电动机，33负载，50切线方向伺服响应运算部，51第1轴伺服响应运算部，52第2轴伺服响应运算部，53第3轴伺服响应运算部。

Claims (7)

1.一种轨迹控制装置，其通过同时控制多个可动轴的电动机而对可动部的轨迹进行控制，

该轨迹控制装置的特征在于，具有：

插补·加减速运算部，其通过对被供给的指令路径进行插补，并且沿着所述指令路径执行加减速运算，而求出加减速后插补路径；

轴分配部，其根据所述加减速后插补路径，生成各可动轴的位置指令；

伺服响应运算部，其基于各可动轴的所述位置指令，运算各可动轴的伺服响应；

切线方向伺服响应运算部，其基于所述加减速后插补路径，求出切线方向移动量的切线方向伺服响应；

基准点生成部，其根据所述切线方向伺服响应，求出校正基准点；

位置矢量校正部，其基于各可动轴的所述伺服响应以及所述校正基准点而对用于抑制轨迹误差的校正矢量进行运算，并利用所述校正矢量对各可动轴的所述位置指令进行校正，由此输出各可动轴的校正后位置指令；以及

伺服控制部，其向各可动轴的所述电动机分别输出电动机驱动扭矩，以使得各可动轴的位置分别追随各可动轴的所述校正后位置指令。

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

所述伺服响应运算部针对每个所述可动轴具有独立的伺服响应运算部，各所述独立的伺服响应运算部是输入输出之间的响应与各自相对应的所述可动轴的位置响应大致相同的滤波器。

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

所述切线方向伺服响应运算部是输入输出之间的响应与所述可动轴的位置响应大致相同的滤波器，其将沿着从所述指令路径的起点至加减速后指令位置为止的指令路径的长度的标量值作为输入。

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

所述伺服控制部针对每个所述可动轴具有独立的伺服控制部，所述可动轴的所述位置响应是从该可动轴的所述独立的伺服控制部的输入起至该可动轴的电动机的位置信号为止的响应特性。

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

每个所述可动轴的所述位置响应均大致相同。

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

所述基准点生成部求出从所述指令路径的起点沿所述加减速后插补路径前进了根据所述切线方向伺服响应而得到的长度的点，作为所述校正基准点。

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

所述位置矢量校正部，将从根据所述伺服响应确定的点至所述校正基准点为止的矢量作为所述校正矢量，将所述校正矢量与根据所述位置指令确定的点相加而得到的结果作为所述校正后位置指令。