JP2012234390A - Trajectory control device - Google Patents

Abstract

PROBLEM TO BE SOLVED: To provide a trajectory control device capable of restraining a trajectory error even when a feeding speed transiently changes due to an effect of acceleration and deceleration.SOLUTION: A trajectory control device controls a trajectory of a movable section of a machine, where the movable section thereof driven by a plurality of movable axes, by concurrently controlling the plurality of the movable axes. The trajectory control device comprises: an interpolation and acceleration/deceleration calculation section which interpolates, accelerates or decelerates a given instruction path; an axis distribution section which generates position instructions for the plurality of the movable axes in accordance with the interpolated, accelerated or decelerated instruction path; a correction vector calculation section which calculates a correction vector for correcting a trajectory error on the basis of the position instructions for the plurality of the movable axes as well as the interpolated, accelerated or decelerated instruction path; a position instruction correction section which corrects the position instructions for the plurality of the axes using the calculated correction vector; and a servo control section which concurrently controls the plurality of the movable axes so that positions of the plurality of the movable axes follows the corrected position instructions for the plurality of the movable axes.

Description

  The present invention relates to a trajectory control device.

  When processing is performed using a machine tool, a laser processing machine, or the like, control is performed so that the position of the tool with respect to the workpiece is along the commanded path. This control is called trajectory control, and the trajectory control is generally performed by performing servo control so that the actual position of each movable axis of the machine follows the position command of each movable axis obtained from the command path. Is.

  When performing trajectory control, the actual trajectory may deviate from the commanded path due to a response delay of the control system of each movable axis. In the trajectory control, control is performed for each movable axis of the machine, and therefore, the servo system response of each movable axis moves behind the position command due to an error caused by the response delay of the control system of each movable axis. If the movement direction of the command path does not change as in a straight line, even if each axis moves with a delay, the locus of the servo system response does not deviate from the command path. That is, an error appears in the tangential direction of the command path, but no error in the normal direction of the command path appears. On the other hand, when the moving direction of the command path changes like a curve or a corner shape, an error appears in the normal direction of the command path due to the delay of the servo control system of each axis. In the following, among the errors with respect to the position command of the servo system response position, a component in the tangential direction of the command path is referred to as a tracking error, and a component in the normal direction of the command path is referred to as a locus error. If there is a locus error, the processed shape does not match the original shape, which is not preferable.

  In Patent Document 1, in a numerically controlled machine tool, an optimum feed for suppressing an error with respect to a target movement locus to a certain value or less based on a shape of a movement locus of a tool tip that is recognized by pre-reading a movement command in an original numerical control program. It is described that the speed is calculated, the error amount with respect to the target movement trajectory when machining is performed at this speed, and the original movement command is corrected based on a correction vector that cancels the error amount. Yes. Thus, according to Patent Document 1, since machining accuracy equivalent to the conventional one can be maintained while machining at a higher feed rate than the conventional one, the machining time can be shortened without lowering the machining accuracy. It is said that.

JP-A-6-282321

  In the technique described in Patent Document 1, the direction of the correction vector is a direction perpendicular to the moving direction (normal direction), and the length of the correction vector is the normal direction acceleration (the square of the velocity divided by the radius of curvature). Value) multiplied by a predetermined coefficient. That is, in the technique described in Patent Document 1, since correction is performed based on the movement command (command point) of the original numerical control program, it is possible to cope with the case where the acceleration in the path between the command points changes due to the influence of acceleration / deceleration. It may not be possible.

  For example, near the start point and end point of the command path, or near the command point where the command feed speed changes on the command path, if the feed speed is changed suddenly, the allowable acceleration of the machine being controlled will be exceeded. It is necessary to perform acceleration / deceleration that gradually increases or decreases. In the technique described in Patent Document 1, an error amount is calculated and corrected based on a command point before acceleration / deceleration, and the calculated error amount is in a state where the feed speed is constant (steady state). Is the error amount. For this reason, in a state where the feed rate changes (transient state) as during acceleration / deceleration, the calculated error amount is different from the error amount actually generated, and the response trajectory obtained as a result of correction is the same as the original command path. Unlikely, the shape of the response trajectory tends to be distorted.

  The present invention has been made in view of the above, and an object of the present invention is to provide a trajectory control apparatus that can suppress trajectory errors even when the feed rate changes transiently due to the influence of acceleration and deceleration.

  In order to solve the above-described problems and achieve the object, a trajectory control device according to one aspect of the present invention is such that a movable part of a machine is driven by a plurality of movable shafts, and the plurality of movable shafts are simultaneously controlled. Is a trajectory control device for controlling the trajectory of the movable part according to the interpolation / acceleration / deceleration calculation unit for performing interpolation / acceleration / deceleration with respect to a given command path, and the command path subjected to the interpolation / acceleration / deceleration. A correction vector for correcting a trajectory error on the basis of the axis distribution unit that generates position commands for the plurality of movable axes, the position commands for the plurality of movable axes, and the interpolated / accelerated and decelerated command paths. A correction vector calculation unit for calculating, a position command correction unit for correcting position commands of the plurality of movable axes using the calculated correction vector, and a plurality of movable axes in which the positions of the plurality of movable shafts are corrected. Follow the position command of As, characterized in that a servo controller for simultaneously controlling the plurality of movable axes.

  According to the present invention, the correction vector is calculated based on the command path that has been interpolated / accelerated, so that even when the curvature or acceleration changes between command points on the command path, the response trajectory is accurate to the command path. It is possible to perform correction so as to follow. Therefore, the trajectory error can be suppressed even when the feed rate changes transiently due to the influence of acceleration / deceleration.

FIG. 1 is a diagram illustrating a configuration of a trajectory control device according to an embodiment. FIG. 2 is a diagram illustrating a configuration of the servo control unit according to the embodiment. FIG. 3 is a diagram illustrating an operation of the interpolation / acceleration / deceleration calculation unit in the embodiment. FIG. 4 is a diagram illustrating temporal changes in tangential velocity and tangential acceleration. FIG. 5 is a diagram illustrating a response locus in the embodiment. FIG. 6 is a diagram illustrating a response locus in the comparative example.

  Embodiments of a trajectory control apparatus according to the present invention will be described below in detail with reference to the drawings. Note that the present invention is not limited to the embodiments.

Embodiment.
A trajectory control apparatus 10 according to an embodiment will be described with reference to FIG. FIG. 1 is a block diagram showing the configuration of the trajectory control device 10.

  When the movable portion of the machine is driven by a plurality of movable shafts, the trajectory control device 10 is a device that controls the trajectories of the movable portions by simultaneously controlling the motors of the plurality of movable shafts. That is, the trajectory control device 10 is a device for realizing high-speed and high-accuracy machining by suppressing a trajectory error regardless of a command path in a control device such as a machine tool or a laser processing machine. The locus error is an error indicating how much the actual response locus has deviated from the command path. The command path is given as a coordinate value on the trajectory of the movable part of the machine in the form of an NC program or the like. Further, the interpolation method (straight line, arc, spline, etc.) between the commanded coordinate values and the moving speed in the direction along the locus, that is, the feed speed are also given simultaneously by the NC program or the like.

  The interpolation / acceleration / deceleration calculation unit 1 is given a command path from the outside. The interpolation / acceleration / deceleration calculation unit 1 interpolates between commanded coordinate values by a specified method, and performs an operation of accelerating and decelerating with a predetermined acceleration or acceleration / deceleration time constant specified separately along the command path. . That is, the interpolation / acceleration / deceleration calculation unit 1 performs interpolation / acceleration / deceleration for a given command path. Thereby, the interpolation / acceleration / deceleration calculation unit 1 calculates the post-acceleration / deceleration interpolation path (that is, the interpolated / accelerated / decelerated command path) and supplies it to the axis distribution unit 3 and the correction vector calculation unit 5.

  The axis distributor 3 calculates the position command of each movable axis of the machine so that the movable part of the machine passes through the interpolation path after acceleration / deceleration. In the present embodiment, it is assumed that there are two movable axes, the first axis and the second axis, and the axis distributor 3 determines the position command for the first axis and the position command for the second axis according to the post-acceleration / deceleration interpolation path. Is supplied to the correction vector calculation unit 5 and the position command correction unit 4.

  The correction vector calculation unit 5 receives the post-acceleration / deceleration interpolation path from the interpolation / acceleration / deceleration calculation unit 1 and receives the first axis position command and the second axis position command from the axis distribution unit 3. The correction vector calculation unit 5 calculates a correction vector for correcting a trajectory error with respect to the command path of the response trajectory based on the post-acceleration / deceleration interpolation path and the position command of each movable axis, and supplies the correction vector to the position command correction unit 4. To do.

  The position command correction unit 4 receives a position command for the first axis and a position command for the second axis from the axis distribution unit 3, and receives a correction vector from the correction vector calculation unit 5. The position command correction unit 4 adds a component in the direction of each movable axis of the correction vector to the position command of each movable axis. That is, the position command correction unit 4 corrects the position command of each movable axis using the correction vector. Thereby, the position command correction unit 4 calculates the corrected position command for each movable axis, and outputs the first axis corrected position command and the second axis corrected position command to the first axis servo control unit 6 and the second axis command, respectively. Output to the axis servo controller 7.

  The first axis servo control unit 6 generates a motor driving torque for the first axis so that the position of the first axis follows the first axis corrected position command, and outputs the generated motor driving torque to the first axis motor. That is, the first axis servo controller 6 controls the first axis motor so that the position of the first axis follows the first axis corrected position command.

  The second axis servo control unit 7 generates a motor driving torque for the second axis and outputs it to the motor for the second axis so that the position of the second axis follows the second axis corrected position command. That is, the second axis servo control unit 7 controls the second axis motor so that the position of the second axis follows the second axis corrected position command.

  The first axis servo control unit 6 and the second axis servo control unit 7 have the same configuration, and a block diagram thereof is shown in FIG. That is, the servo control unit 11 corresponding to the first axis servo control unit 6 and the second axis servo control unit 7 has the following components.

The corrected position command input to the servo control unit 11 is subtracted from the model position by the subtracter 20, is multiplied by the _first model gain K1 by the model gain multiplier 21, and the model speed is subtracted by the subtractor 22. Further, the model gain multiplier 23 multiplies the second model gain K ₂ to generate a model acceleration, which is output to the multiplier 30. Further, the model acceleration is integrated by the integrator 24 to generate a model velocity, which is output to the subtractor 22, the integrator 25, and the adder / subtractor 28. The model speed is integrated by the integrator 25 to generate a model position, which is output to the subtracter 20 and the subtractor 26. A block in which the corrected position command is input and the model position / model speed / model acceleration is output is referred to as a reference model 12.

  A motor position signal is subtracted from the model position by the subtractor 26 to generate a position error and output it to the position controller 27. The position controller 27 performs control such as proportional control on the position error, and the result is output to the adder / subtractor 28. In the adder / subtractor 28, the model speed is added to the output from the position controller 27, and the motor speed signal is further subtracted to generate a speed error, which is output to the speed controller 29. The speed controller 29 performs control such as proportional / integral control on the speed error, and outputs the result to the adder 31. The multiplier 30 multiplies the model acceleration by a value corresponding to the inertia to be controlled, calculates the model torque, and outputs it to the adder 31. The adder 31 adds the model torque to the output from the speed controller 29 to generate a motor torque signal, which is output to the motor 32 as a motor drive torque. The mechanical system 13 including the motor 32 and the load 33 is driven by motor driving torque. Further, a motor speed signal and a motor position signal are acquired by an encoder (not shown) or the like and output to the servo control unit 11.

The servo control unit 11 is a two-degree-of-freedom controller using the reference model 12, and can be designed independently of followability to a command and responsiveness to a disturbance. The followability of the motor position with respect to the position command is determined by the first model gain K ₁ and the second model gain K ₂ , and the response of the motor position to the disturbance is determined by the design of the position controller 27 and the speed controller 29. . Therefore, the response of the motor position is controlled so as to follow the model position, which is the output of the reference model 12, regardless of the characteristics of the actual control target (mechanical system 13).

  Next, details of the calculation of each unit will be described. The interpolation / acceleration / deceleration calculation unit 1 interpolates between the commanded coordinate values by a designated method, further performs acceleration / deceleration calculation, and calculates a command position for each interpolation cycle on the command path. Examples of interpolation methods include linear interpolation, circular interpolation, and spline interpolation. The interpolation cycle is a fixed cycle determined as the specification of the trajectory control device, and generally a short cycle of several ms or less is used. The shorter the interpolation cycle, the more accurate trajectory control is possible, but the processing load on the processor or the like used for the calculation is increased. A point for each interpolation cycle obtained as a result of the interpolation / acceleration / deceleration is called an interpolation point after acceleration / deceleration, and a path formed by the interpolation points after acceleration / deceleration is called an interpolation path after acceleration / deceleration. The acceleration / deceleration calculation is to prevent the commanded acceleration from exceeding the allowable acceleration of each movable axis of the machine, and the interpolation path so that the change in speed is less than the acceleration specified by a separate parameter. The upper interpolation point is recalculated. That is, when moving from the start point to the end point, the interval between the interpolation points is shortened so that the speed gradually increases immediately after the start point, and the interval between the interpolation points is shortened so that the speed gradually decreases immediately after the end point. . Details of this acceleration / deceleration calculation will be described using the example shown in FIG.

  Consider a case where circular interpolation is performed counterclockwise from the start point P1 = (R, 0) to the end point P2 = (0, R) as shown in FIG. First, let q (t) be the distance along the command path from the start point P1 to the point P (t) on the path at an arbitrary time t. If the movement length from the start point to the end point is L, L = πR / 2. Next, an acceleration / deceleration pattern of q ′ (t) is determined as shown in FIG. 3B with the time derivative of q (t) as a tangential velocity q ′ (t). This is a trapezoidal pattern in which the height is the command feed speed F and the area is the movement length L from the start point to the end point, and the inclinations of the acceleration unit and the deceleration unit are accelerations separately specified by parameters or the like. It is determined as follows. Here, acceleration and deceleration are performed in a straight line, but there are also cases where the movement of the machine is further smoothed by using an S shape. The movement length q (t) after acceleration / deceleration is obtained by time-integrating the tangential speed q '(t). Using Δt as an interpolation cycle, a post-acceleration / deceleration movement length q (NΔt) at t = NΔt (N is a positive integer) is obtained, and a point advanced by a length q (NΔt) from the start point position P1 along the command path is It becomes the interpolation point after the Nth acceleration / deceleration. The post-acceleration / deceleration interpolation path formed from the post-acceleration / deceleration interpolation points is as shown in FIG. The post-acceleration / deceleration interpolation path information includes post-acceleration / deceleration movement length q (t), interpolation type (linear interpolation, circular interpolation, spline interpolation, etc.), parameters required for interpolation (start point position, end point position, etc. Shape parameters). As other shape parameters, there are parameters such as a center position and a radius in the case of circular interpolation, and a coefficient of a spline function in the case of spline interpolation.

The axis distribution unit 3 obtains the coordinate value of each axis of the post-acceleration / deceleration interpolation point on the post-acceleration / deceleration interpolation path. In the example shown in FIG. 3, the deflection angle θ from the starting point of the interpolation point on the command arc path is a value obtained by dividing the movement length q (t) by the command radius R (θ = q (t) / R). _Therefore , the position command vector x _c (t) having the first axis position command x _c1 (t) and the second axis position command x _c2 (t) as components is expressed by the following equation.

  The correction vector calculation unit 5 calculates a trajectory error vector for the command path of the servo system response trajectory in accordance with the position command of each axis and the change in the tangential speed obtained by the acceleration / deceleration calculation. The correction vector is calculated so as to cancel. The trajectory error vector has a property that occurs in a direction perpendicular to the traveling direction of the command path in proportion to the magnitude of the normal direction component of the command acceleration. The proportional coefficient k is obtained by measuring the relationship between the commanded acceleration and the trajectory error amount, and analytically obtaining the ratio between the theoretical value of the trajectory error amount in the steady state at the time of the arc command and the normal direction acceleration. Set by the method. In the latter method, the trajectory error amount at the time of circular arc command is first obtained by subtracting the command radius from the command radius and the absolute value of the frequency response transfer function of the servo system, and this trajectory error amount is divided by the command acceleration. To find out.

ここで、ｊは虚数単位である。図２に示したサーボ系では、前述のようにモータ位置の応答は、実際の制御対象（機械系１３）の特性にかかわらず、規範モデル１２の出力であるモデル位置に追従するように制御される。そのため、サーボ系の伝達関数Ｇ（ｓ）は規範モデル１２の伝達関数Ｇ_ｍ（ｓ）に一致する。規範モデル１２の伝達関数Ｇ_ｍ（ｓ）は次の式（３）で表される。

When expressed by an equation, the following equation (2) is obtained. In the case of an arc command with a radius R and each speed ω, the proportional coefficient k of the trajectory error amount with respect to the commanded acceleration is given by the following equation (2) using the transfer function G (s) of the servo system.

Here, j is an imaginary unit. In the servo system shown in FIG. 2, as described above, the response of the motor position is controlled so as to follow the model position, which is the output of the reference model 12, regardless of the characteristics of the actual control target (mechanical system 13). The Therefore, the transfer function G (s) of the servo system matches the transfer function G _m (s) of the reference model 12. The transfer function G _m (s) of the reference model 12 is expressed by the following equation (3).

すなわち、軌跡誤差ベクトルは、法線方向加速度ベクトルａ_ｎ（ｔ）にサーボ系のゲインの二乗に反比例するような係数ｋをかけた値となる。式で表すと、次の式（５）のようになる。

法線方向加速度ベクトルａ_ｎ（ｔ）は、指令位置ベクトルの２回微分である指令加速度ベクトルを、指令経路に垂直な方向に射影することにより求められる。式で表すと、次の式（６）となる。

ただし、ａ（ｔ）は指令加速度ベクトルｄ^２ｘ_ｃ／ｄｔ^２であり、ｖは指令速度ベクトルｄｘ_ｃ／ｄｔである。 When the second model gain K ₂ is four times the first model gain K ₁ , that is, when K ₂ = 4 × K ₁ , the proportionality coefficient k with respect to the commanded acceleration of the trajectory error amount is obtained. Equation (4) is obtained.

That is, the trajectory error vector is a value obtained by multiplying the coefficient k as inversely proportional to the square of the gain of the servo system in the normal direction acceleration vector a _{n (t).} When expressed by an equation, the following equation (5) is obtained.

The normal direction acceleration vector a _n (t) is obtained by projecting a command acceleration vector which is a second derivative of the command position vector in a direction perpendicular to the command path. This is expressed by the following equation (6).

However, a (t) is a command acceleration vector d ² x _c / dt ² , and v is a command speed vector dx _c / dt.

When the feed rate does not change, a vector obtained by inverting the sign of the trajectory error vector may be calculated as a correction vector in order to cancel the trajectory error vector. This trajectory error vector is a first correction vector c ₁ and is expressed by the following equation (7).

接線方向加速度に対する比例係数α（所定係数）は調整パラメータとし、第１の補正ベクトルのみで補正を行った場合の接線方向加速度と軌跡とのずれ量の比率に基づいて調整を行う。さらに、応答軌跡の指令経路に対する軌跡誤差量が小さくなるように調整を行っても良い。ここで、加減速に伴う補正ベクトルの長さの理想的な長さのずれは、加速時と減速時とで異なった大きさとなる。そこで、接線方向加速度に対する比例係数αは、加速時と減速時とで異なった値を有するように設定する。加速時か減速時かは、接線方向加速度ｑ’’（ｔ）が正のときは加速、負のときは減速として判断することができる。また、注意点として、第１の補正ベクトルの大きさが０の場合、第２の補正ベクトルの計算式である式（８）は、分母が０となり計算不能となるが、この場合は法線方向加速度が０、すなわち軌跡誤差ベクトルｅ（ｔ）が０である場合であり、本来補正を行う必要がない。そこで、第１の補正ベクトルの大きさが０の場合は、第２の補正ベクトルは０とする。以上で得られた第１の補正ベクトルと第２の補正ベクトルとの和を補正ベクトルとする。 On the other hand, when the feed rate changes, the length of the correction vector is different from the ideal length. This deviation has a property that is approximately proportional to the rate of change of the tangential velocity, that is, the tangential acceleration. Therefore, the tangential speed q ′ (t) calculated by the interpolation / acceleration / deceleration calculation unit 1 is differentiated to obtain the tangential acceleration q ″ (t) = d ² q / dt ² , and the length is the tangential acceleration. And a second correction vector whose direction matches the first correction vector is obtained. When the second correction vector is expressed by an equation, the following equation (8) is obtained.

The proportional coefficient α (predetermined coefficient) with respect to the tangential acceleration is used as an adjustment parameter, and adjustment is performed based on the ratio of the amount of deviation between the tangential acceleration and the locus when correction is performed using only the first correction vector. Further, adjustment may be performed so that the amount of trajectory error with respect to the command path of the response trajectory becomes small. Here, the ideal deviation of the length of the correction vector accompanying acceleration / deceleration has different magnitudes during acceleration and deceleration. Therefore, the proportionality coefficient α with respect to the tangential acceleration is set so as to have different values during acceleration and deceleration. Whether the vehicle is accelerating or decelerating can be determined as accelerating when the tangential acceleration q ″ (t) is positive, and decelerating when negative. Also, as a cautionary note, when the magnitude of the first correction vector is 0, equation (8), which is the calculation formula for the second correction vector, cannot be calculated because the denominator is 0. In this case, the normal line This is a case where the directional acceleration is 0, that is, the trajectory error vector e (t) is 0, and originally no correction is necessary. Therefore, when the magnitude of the first correction vector is zero, the second correction vector is zero. The sum of the first correction vector and the second correction vector obtained as described above is used as a correction vector.

  In the position command correction unit 4, the correction vector is set to 0 when the post-acceleration / deceleration interpolation path shape is a straight line. That is, when the type of interpolation is linear interpolation and the movement length is sufficiently long, the correction vector is set to zero. This is because, when accelerating / decelerating in the straight line portion, a trajectory error does not naturally occur, and therefore a trajectory error occurs conversely when a correction amount proportional to the tangential acceleration is added. Also, due to the influence of rounding errors in interpolation and acceleration / deceleration calculations, even if the normal acceleration is actually 0, it is determined that the normal acceleration is not 0, and the second correction vector is added. Although there is a possibility that the correction becomes excessive, it is possible to prevent the excessive correction by using the original route information, and it is possible to perform the correction more accurately.

  The position command correction unit 4 adds the first axis component and the second axis component of the correction vector to the first axis position command and the second axis position command, respectively, so that the first axis corrected position command and the second axis component are added. The corrected position command is calculated.

  Next, the effect by this Embodiment is demonstrated using the result of a numerical simulation.

A response locus when a counterclockwise arc command having a radius of 5 mm and a feed speed of 6 m / min (= 0.1 m / s) is given is obtained by simulation. The tangential acceleration was 2 m / s ^2, and the normative model gain K ₁ of the servo control unit 11 was 100 rad / s. FIG. 4A shows a time waveform of the tangential velocity at that time, and FIG. 4B shows a time waveform of the tangential acceleration. As shown in FIG. 4B, the tangential acceleration has a positive value during acceleration near the start point, 0 in a steady state near the middle, and a negative value during deceleration near the end point.

  FIG. 6 is a response trajectory when correction is performed by the method of the comparative example. In this comparative example, interpolation / acceleration / deceleration is performed after correcting the command position by adding a correction amount that cancels the locus error to the command position before acceleration / deceleration. The locus error is enlarged and plotted in the radial direction, and one scale corresponds to a locus error of 10 μm. The trajectory error in the steady state excluding the vicinity of the start point and the end point is almost zero, but a trajectory error of 40 μm is generated near the start point and the end point.

On the other hand, FIG. 5 shows a response trajectory when correction is performed in the present embodiment. Here, the coefficient α for the tangential acceleration is 3 × 10 ⁻⁶ during acceleration and 2 × 10 ⁻⁶ during deceleration. From FIG. 6, it can be seen that the trajectory error in the vicinity of the start point / end point is suppressed to about 5 μm.

  As described above, in the embodiment, the interpolation / acceleration / deceleration calculation unit 1 performs interpolation / acceleration / deceleration on the command path, and the correction vector calculation unit 5 performs the interpolation / acceleration / deceleration command path (interpolation after acceleration / deceleration). The correction vector is calculated based on (path). Thereby, even when the curvature and acceleration change between command points on the command path, it is possible to perform correction so that the response trajectory accurately follows the command path. Therefore, the trajectory error can be suppressed even when the feed rate changes transiently due to the influence of acceleration / deceleration.

  In the embodiment, the correction vector calculation unit 5 calculates the trajectory error vector based on the first axis position command and the second axis position command and the change in the tangential direction movement amount in the post-acceleration / deceleration interpolation path. Then, the first correction vector is calculated so as to cancel the trajectory error vector, and the correction vector is calculated using the first correction vector. As a result, even when the curvature or acceleration changes between command points on the command path, correction can be performed so as to cancel the trajectory error, and correction is performed so that the response path accurately follows the command path. be able to.

  In the embodiment, in addition to the first correction vector, the correction vector calculation unit 5 has the same direction as the first correction vector, and the tangential acceleration in the post-acceleration / deceleration interpolation path is multiplied by a predetermined coefficient. A correction vector is calculated by calculating a second correction vector having a length and adding the first correction vector and the second correction vector. For this reason, even when the feed rate changes due to acceleration / deceleration, it is possible to perform correction so that the response locus accurately follows the command path.

  In the embodiment, the length of the second correction vector is set to a predetermined coefficient times the tangential acceleration. Thereby, it is possible to correct more accurately in the acceleration / deceleration portion, and it is possible to suppress distortion generated in the locus.

  Furthermore, in the embodiment, when calculating the second correction vector, the coefficient by which the tangential acceleration is multiplied is changed between acceleration and deceleration. As a result, even when the method of distortion of the trajectory differs depending on the characteristics of the servo system during adjustment and deceleration, correct correction can be performed.

  In addition, when accelerating / decelerating in a straight line portion, a trajectory error is not originally generated, so adding a correction amount proportional to the tangential acceleration results in a trajectory error. In the embodiment, in this case, Do not make any corrections. That is, the position command correction unit 4 determines whether the command path is a curve or a straight line based on the interpolated / accelerated / decelerated command path (interpolated path after acceleration / deceleration). To do. Thereby, the trajectory accuracy can be maintained even in the straight line portion. Further, the second correction vector may be output excessively due to the influence of rounding error of interpolation or acceleration / deceleration calculation, but the second correction vector becomes excessive by using the original path information. Can be corrected, and correction can be performed more accurately.

  In the embodiment, the number of movable axes is two (first axis and second axis), but the number of movable axes may be three or more. Similar correction can be performed by setting the servo system response trajectory vector, error vector, and correction vector to vectors of three or more dimensions instead of two dimensions.

  In this embodiment, the trajectory error vector is calculated from the normal direction command acceleration. However, the response trajectory may be obtained by simulation, and the trajectory error may be obtained from a deviation from the command path. By doing so, although the calculation load of the processor is required, it is possible to obtain the trajectory error vector more accurately.

  As described above, the trajectory control device according to the present invention is useful for controlling the trajectory of an operating part of a machine such as a machine tool or a laser processing machine.

DESCRIPTION OF SYMBOLS 1 Interpolation / acceleration / deceleration calculation part 3 Axis distribution part 4 Position command correction part 5 Correction vector calculation part 6 1st axis servo control part 7 2nd axis servo control part 10 Trajectory control apparatus 11 Servo control part 12 Reference model 13 Mechanical system 20 Subtractor 21 Model gain multiplier 22 Subtractor 23 Model gain multiplier 24 Integrator 25 Integrator 26 Subtractor 27 Position controller 28 Adder / subtractor 29 Speed controller 30 Multiplier 31 Adder 32 Motor 33 Load

Claims (5)

A trajectory control device for controlling a trajectory of the movable part by driving a movable part of a machine by a plurality of movable axes and simultaneously controlling the plurality of movable axes,
An interpolation / acceleration / deceleration calculation unit that performs interpolation / acceleration / deceleration for a given command path;
An axis distributor that generates position commands for the plurality of movable axes according to the interpolated / accelerated and decelerated command paths;
A correction vector calculation unit for calculating a correction vector for correcting a trajectory error based on the position commands of the plurality of movable axes and the command path subjected to the interpolation / acceleration and deceleration;
A position command correction unit that corrects position commands of the plurality of movable axes using the calculated correction vector;
A servo control unit that simultaneously controls the plurality of movable axes such that the positions of the plurality of movable axes follow the corrected position commands of the plurality of movable axes;
A trajectory control device characterized by comprising: The correction vector calculation unit calculates a trajectory error vector based on a position command of the plurality of movable axes and a change in a tangential movement amount in the interpolated / accelerated / decelerated command path, and the trajectory error vector The trajectory control device according to claim 1, wherein a first correction vector is calculated so as to cancel and the correction vector is calculated using the first correction vector. The correction vector calculation unit adds a second correction vector having the same direction as the first correction vector and a length obtained by multiplying a tangential acceleration by a predetermined coefficient to the first correction vector, The trajectory control device according to claim 2, wherein the correction vector is calculated. The trajectory control device according to claim 3, wherein the predetermined coefficient has different values between acceleration and deceleration. The position command correction unit determines whether the command path is a curve or a straight line based on the interpolated / accelerated / decelerated command path, and sets the correction vector to 0 when it is determined to be a straight line. Item 5. The trajectory control device according to any one of Items 1 to 4.

Images