JPH03263208A - Servo motor controller - Google Patents

Abstract

PURPOSE:To exactly move a moving body to a target position by controlling the extent of rotation of a servo motor with a prescribed control gain with respect to coordinate axes having no response delay and changing the control gain to control this extent of rotation with respect to coordinate axes having response delay. CONSTITUTION:Preliminarily obtained position loop gains Kpx to Kpz are stored in a data ROM 32 of a gain setting part 9, and an address interface 30 designates a prescribed address of a table to output data of position loop gains corresponding to delay times from the ROM 32 to multipliers 24x to 24z of rotative speed control parts 7x to 7z when data of the delay times of respective coordinate axes is inputted from an interpolating part 3. Data of the prescribed fundamental gain is outputted with respect to the coordinate axis having no time delay. Thus, rotative speeds of servo motors Mx to Mz are controlled to control each coordinate position of the moving body to the target position, and the moving body is exactly moved along a commanded movement locus.

Description

DETAILED DESCRIPTION OF THE INVENTION [Field of Industrial Application] The present invention relates to a servo motor control device that controls the driving of a plurality of servo motors to move a moving body to an arbitrary position in a mechanical coordinate system.

[Conventional technology] Conventionally, multiple servo motors are driven and controlled to drive moving objects such as processing tools and tables in the respective directions of the X-axis, Y-axis, and Z-axis. A machine tool that moves a moving body to a position is known.

This type of machine tool includes an interpolation unit that sets a target position for each of the coordinate axes for each minimum time unit by performing predetermined interpolation on the target trajectory in order to accurately move the movable body along the target trajectory; Each coordinate based on the deviation between the target position and the current position of the moving object? j! A servo motor control device is provided that includes a position control section for each coordinate axis that controls the rotational speed of each servo motor with the same position control gain and moves the movable body to the target position. 1 [Problems to be Solved by the Invention] However, the above device has problems such as the time lag that occurs when the target position is transmitted from the interpolation unit to the position control unit for each coordinate axis in a time-division manner, and the inherent characteristics of each position control unit. There is a delay in response,
If the position control gain is set to be the same for each axis, there is a problem that the moving body cannot be accurately moved to the target position.

Therefore, the present invention has been made with the object of providing a servo motor control device that can accurately move a moving body along a target trajectory.

[Means for Solving the Problems] The gist of the present invention is to provide a servo motor control device for controlling the drive of a plurality of servo motors to move a movable body to an arbitrary position in a machine coordinate system, a position detection means for detecting the current position of the moving object for each of the coordinate axes forming the coordinate system; an interpolation means for performing predetermined interpolation based on an external movement command to set a target position for each of the coordinate axes; Based on the deviation between the current position of the moving body detected by the position detection means for each of the coordinate axes and the target position set by the interpolation means, the amount of rotation of each servo motor is determined by a predetermined control gain set for each of the coordinate axes. a rotation amount control means to control;
A servo motor control device characterized by comprising: control gain changing means for changing each control gain set for each of two coordinate axes based on a response delay in control of each of the coordinate axes by the rotation amount control means.

[Effect] The above configuration of the present invention (two-fold I!

When movement is commanded from the outside, the interpolation means performs predetermined interpolation based on the movement command I to set target positions for each of the coordinate axes. Then, the rotation amount control means controls each servo motor at a predetermined control gain set for each of the coordinate axes based on the deviation between the target position and the current position of the moving body for each of the coordinate axes detected by the position detection means. control the amount of rotation. At this time, if there is a response delay in controlling each of the coordinate axes by the rotation amount control means, the control gain changing means changes each control gain set for each of the coordinate axes based on the response delay. and,
The rotation amount control means controls the rotation amount of each servo motor using the changed control gain.

In other words, by keeping the predetermined control gain unchanged for the coordinate axes without a response delay, and changing the control gain for the coordinate axes with a response delay, the amount of rotation of each servo motor is controlled and the movable body is moved to the target position.

[Example] Embodiments of the present invention will be described below with reference to the drawings.

First, FIG. 1 is a block diagram of a servo motor control device to which the present invention is applied.

The servo motor control device 1 constitutes a part of a machine tool (path shown), and drives and controls three servo motors Mx, My, and Mz to move a machining tool (path shown) along the X axis, Y axis, and Z axis. It is used to move to an arbitrary position in a mechanical coordinate system consisting of axes.

In a machine tool (i), the machining tool moves in the direction of each coordinate axis by the rotation of each servo motor Mx-Mz, and the mechanical A transmission mechanism (path shown) is configured.

As shown in the figure, the servo motor control device 1 includes an interpolation section 3 that calculates, for each coordinate, a target position for moving the processing tool along a movement locus commanded by a higher order command device (W & shown in the figure); A distribution section 5 outputs a pulse train signal to each coordinate axis based on the calculation result of the section 3, and a distribution section 5 is provided for each coordinate axis to output a pulse train signal from the distribution section 5 and a feedback pulse (
(to be described later), and a gain setting unit 9 that sets the position loop gain of each of the rotation speed control units 7x to 7z. , a speed command unit 11 that outputs a rotation speed command based on the rotation speed control of the rotation speed control units 7X to 7z, and a speed command of the speed command unit 1 and the servo motor M.
Based on the actual rotational speed of x-Mz, servo motor Mx-
The main components are drive units 13x, 13y, and 13z that drive and control Mz.

In this embodiment, the rotation speed control sections 7x to 7z, the speed command section 11, and the drive sections 13x to 13z correspond to rotation amount control means, and the gain setting section 9 corresponds to control gain changing means.

The interpolation unit 3 includes a well-known CPU 3a, ROM 3b, and RAM 3.
c. Constructed as a logic operation circuit centered around the backup RAM 3d and input/output port 3e. When a movement command for moving the processing tool in a straight line or in an arc is input from the upper command device, the trajectory of the commanded movement is Accordingly, arithmetic processing is executed to obtain data for linear interpolation or circular interpolation, and target positions and movement amounts for each of the X, Y, and Z axes are calculated for each predetermined minimum time unit. Furthermore, in the process of moving the processing tool along the movement trajectory, the servo motor M
In order to perform slow-up/slow-down operation of 'x-Mz with a predetermined acceleration/deceleration time constant, a process is executed to obtain speed data for each axis from a speed data table stored in advance in the ROM 3b. Then, the calculated target position data and speed data are output to the distribution section 5.

Furthermore, at startup, the interpolation section 3 outputs response delay time data (described later) for each coordinate axis to the gain setting section 9 as part of the initialization process.

The response delay time data is input in advance from the machine tool's data input device (as shown in the diagram), and is stored in the backup RAM.
It is stored in a predetermined area of 30d.

Note that the calculation process for linear interpolation or circular interpolation and the process for obtaining speed data are well known, so the details will be omitted.

The distribution unit 5 includes pulse distributors 5x, 5, each consisting of a clock pulse generator, a counter, a gate (all shown in the figure), etc.
When target position data and speed data are input from the interpolation section 3, a number of pulses (hereinafter referred to as distribution pulses) Px, Py, Pz corresponding to those data are inputted.
It outputs to the rotational speed control sections 7x to 7z, respectively.

The rotational speed control units 7x to 7z control the servo motors Mx to 7z based on the distribution pulses Px-Pz input from the distribution unit 5.
It controls the rotational speed of Mz, and together with the speed command section 11 and drive sections 13x to 13z, forms a closed loop control system that controls the servo motors Mx to Mz to orient the processing tool to the target position. .

That is, in the rotational speed control units 7x to 7z, the comparators 20x,
20y and 20z are rotation angle detection pulses (hereinafter referred to as feedback pulses) PxFB and PyFB input from the shaft encoder E attached to the servo motors Mx to Mz.
, PzFB and the distributed pulses Px to Pz, and output pulses corresponding to the deviation to the integrators 22x, 22Y, and 22z. The integrators 22x to 22z add the pulses corresponding to the deviation to the accumulated bone so far, thereby obtaining the accumulated pulses εX,
εy and ε2 are calculated and output to multipliers 24×, 24Y, and 24z. The servo motor M The rotation speed of x to Mz is determined and outputted to the speed command section 11 as a rotation command signal (digital signal).

In this embodiment, the shaft encoder E corresponds to the position detection means.

The gain setting unit 9 includes an address interface 30 and a data ROM 32 that are composed of logic elements, counters, etc.
In a predetermined area of the data ROM 32, positions are preset according to the delay time of the response of the control system between the respective coordinate axes to the position command (for example, the delay time associated with the time-sharing sequence of the multiplexer 40, which will be described later). A group of loop gain data is stored as a table for each coordinate axis. When the delay time data for each coordinate axis is input from the interpolation unit 3, the address interface 30 specifies a predetermined address in the table and inputs the position loop gain data corresponding to the delay time stored therein. It is output from the ROM 32 to the multipliers 24x to 24z. For coordinate axes with no delay time, data of a predetermined basic control gain KpO is output.

From a speed command unit 111, a multiplexer 40, a D/A converter (hereinafter referred to as D/AC) 42, and sample and hold circuits (hereinafter referred to as S/H circuits) 44x, 44y, and 442 provided for each coordinate axis. The rotation command signals input from the configuration tongue rotation speed control units 7x to 7z are converted into command signals for the rotation speed of the servo motors Mx to Mz, and output to the drive units 13x to 13z.

That is, in the speed command unit 1], the multiplexer 40
in a predetermined time-sharing sequence (for example, E
At intervals of Δt, in the order of X-axis - Y-axis - Z-axis), one of the rotational speed control units 7x to 7z is connected to the D/AC 34, and a latch command is output to the S/H circuit 34x to 34z of the corresponding coordinate axis. do. Then, the D/AC 34 converts the rotation command signal (digital) into a voltage signal (analog) and outputs it as a speed command. Then, the S/port circuits 34x to 342 hold the voltage signals as they are and output them as speed command signals to the drive units 13x to 13z of the coordinate axes. Therefore, the speed command signal is delayed by Δt for the Y-axis and by 2×Δt for the Z-axis, and the speed command signal is delayed by Δt for the Y-axis with respect to the position command.
3Y and 13z, a time delay of Δt occurs in the Y-axis control system, and a time delay of 2×Δt occurs in the Z-axis control system.

The drive units 13x to 13z are constructed using a well-known pulse width modulation circuit (P
WM circuit) and an energizing circuit, and the servo motor Mx=
The rotation speed is controlled to the command speed by increasing or decreasing the energizing current according to the deviation between the actual rotation speed of the servo motors Mx to My detected by the tacho generator TG attached to My and the speed command. In addition, the driving parts 13x to 13y
Since this is well known as a servo motor drive unit, the details will be omitted.

Next, the operation of the servo motor control device 1 will be explained.

Speed F [mm/min] (Pulse conversion speed f [pu
lse/see]), when a command to move the machining tool along the linear trajectory L1m with an inclination θ on the
Linear interpolation is performed to calculate the target position and movement amount to be reached for each of the X-axis, Y-axis, and Z-axis for each predetermined minimum time unit, and the distribution unit 5 generates a pulse train signal based on the calculation results. It is output to the rotational speed controllers 7x to 7z for each coordinate axis.

The rotational speed control units 7x to 7z determine the rotational speeds of the servo motors Mx to Mz by multiplying the residual pulses εX to ε2 by the position loop gain Kpx-Kpz.

Here, as shown in Fig. 2 (A), if the target position on the straight line trajectory at time t is point A, then if there is no time delay in the Y-axis control system ( The droop pulses εX and Ey of the axis and Y axis are respectively expressed by the following equations. Note that the Z axis servo motor Mz is stopped.

εx= f X cos θ/K pO[pu I se
]εy=fXsinθ/K pO
Distance between point and point 8 is 1ft, l=f/KpO[pul
It becomes sel. At this time, rotation speed control units 7x and 7y
The rotational speeds of servo motors Mx and My determined by
K pxX e x= f X cosθ[pulse
/sec] and KpyX t: y= f X5inθ
[Since pulse/5ecl, servo motor Mx
and My rotate based on the rotation speed command from the speed command unit 11 to move the processing tool to the target position.

However, as mentioned above, the Y-axis control system has a time delay Δ.
Because of t, the actual position of the machining tool is the 0 point delayed by Δy (=fXsinθ×Δt) with respect to the Y axis, which deviates from the straight line trajectory and causes a position error. Therefore, in this embodiment, the position loop gain of the Y-axis rotational speed control section 7y is set according to the time delay Δt to increase the rotational speed of the servo motor My and increase the amount of movement in the Y-axis direction. to correct the position error.

That is, if the set position loop gain is Kpy and the droop pulse at time t is Ey, then in order for the machining tool to be located on a straight line trajectory, E y=Ey −f X5inθXΔt.

(fXsinθ/Kpy)=(fX5inθ×Δt
m) fXsinθ×Δt Therefore, Kpy=1/((1/KpO)−Δt)−・
-The condition (1) is necessary. Therefore, Fig. 2 (B
), if the position loop gain Kpy is set based on equation (1), the droop pulse in the rotational speed control section 7y becomes 1#y, and the position of the processing tool is moved to 8 points on the target trajectory. be able to.

Also, when a command is given to move along the straight line locus Ll with an inclination θ on the Y-axis plane, the time delay about the li Z-axis is 2×Δt, so in the same way as in the above case, K pz = 1 / ((1/K pO)-2XΔt)
- Since the condition (2) is required, the position loop gain Kpz is set based on equation (2).

In this embodiment, the position loop gain Kpx-Kpz obtained in advance in this way is stored in the data ROM 32 of the gain setting section 9, and the time delay Δt, 2×Δt
Position loop gain Kpx (: K po), K
Since py and Kpz are set, the droop pulses εX to εz in the rotational speed control units 7y to 7z of each coordinate axis
& Same as when there is no time delay. In other words, the output of the speed command signal from the speed command unit 11 to the drive units 13y, 13z is delayed by Δt, 2×Δt, and the position loop gain is corrected to increase in the rotational speed control units 7y, 7z to increase the rotational speed. The speed will be increased. Then, the speed command unit 1] converts it into a speed command, and the drive units 14x, 14y actuate the servo motors Mx, My according to the deviation between the speed command and the actual speed.
Controls the energizing current.

In this way, the rotational speed of the servo motors Mx-Mz is controlled, and the machining tool moves along the linear trajectory Ll:.

As mentioned above, in this embodiment, the position loop gain is set to the basic gain KpO for the X-axis without response delay,
For the Y-axis or Z-axis with a response delay, set the position loop gain according to the response delay Δ or 2 × Δt (Kp
y, Kpz) and the rotational speed of the servo motors M can be done.

Here, in this embodiment, the rotation speed control sections 7x to 72 are configured as a closed loop control system, but they may also be configured as a feed forward control system. That is, the third
As shown in the figure, in the rotational speed control sections 7x to 7z,
Adders 26x, 26y, 26z that add a feedforward amount determined from a predetermined feedforward rate α to the outputs of the multipliers 24x to 242 (the product of the accumulated pulses εX to ε2 multiplied by the position loop gain Kpx−Kpz); may be added to form a feedforward position control system.

In this case, the droop pulse Ey decreases by the addition of the feedforward amount, so in order to move the machining tool to the target position, (YEY=(fXsinθ/Kpy)X(1-cr)εy=
(fXsinθ/KpO)X(1-a>E V” e
y −f X5inθ×Δt Therefore, (fXsinθ/Kpy)X(1-a>=(fXsin
θ/KpO)
...(3) Similarly, K pz = 1 / ((1/K pO) -2XΔ
The following condition is required: t/(1-α))...(4).

Therefore, if the position loop gains Kpy and Kpzt are set based on equations (3) and (4), the droop pulses in the rotational speed control sections 7y and 7z will be εy and ε2, as in the case of feedback control. The processing tool can be moved along the target trajectory.

As described above, the present invention can be applied to both feedback control and feedforward control, so it can contribute to improving accuracy in a wide range of servo motor position control.

In this example, linear interpolation was explained as an example, but in the case of circular interpolation, the processing tool can be commanded by changing and setting the position loop gain according to the response delay of each coordinate. It can be moved along a movement trajectory.

Furthermore, in the above embodiment, the response delay Δt, 2×Δt of the position control system for each coordinate axis is inputted in advance from the data input device, but in addition to this, the output timing of interpolated data from the interpolation unit 3 and the S/ The configuration may be such that the time lag between the output timing of the speed command from the output circuits 44X to 44z is detected and the position loop gain is set in accordance with the detected time lag.

In this case, more precise and accurate position control becomes possible.

Further, in this embodiment, the machining tool is moved by a servo motor, but it may be configured to move the work table or the machining head in addition to this, and it is not limited to machine tools. The present invention can be applied to any device that moves a movable body to an arbitrary position in a mechanical coordinate system by controlling.

[Effects of the Invention] As detailed above, according to the present invention, the predetermined control gain is maintained as is for coordinate axes without response delay, and the control gain is changed for coordinate axes with response delay.
Since the amount of rotation of each servo motor is controlled, the position of the movable body can be accurately controlled to the target position at each coordinate point, and the movable body can be moved in accordance with the movement command.

[Brief explanation of drawings]

Figure 1 is a configuration diagram of the servo motor control device of the embodiment, Figure 2
Figure (A) is an explanatory diagram of position error due to time delay, and Figure 2 (
B) is an explanatory diagram of position error correction, and FIG. 3 is a configuration diagram of an embodiment in which position control is performed by feedforward. 1... Servo motor control device 3... Interpolation section 5...
・Distribution section 7X, 7'+', 7Z...Rotation speed control section 9
... Gain setting section 1] -- Speed command section 1
3X, 13Y, 132...-Drive section E...Shaft encoder

Claims (1)

[Scope of Claim] A servo motor control device that controls the driving of a plurality of servo motors to move a moving body to an arbitrary position in a mechanical coordinate system, wherein the moving body a position detection means for detecting the current position of the coordinate axis; an interpolation means for performing predetermined interpolation based on a movement command from the outside to set a target position for each of the coordinate axes; and a position detection means for each of the coordinate axes. Rotation for controlling the amount of rotation of each of the servo motors with a predetermined control gain set for each of the coordinate axes based on the deviation between the detected current position of the moving body and the target position set by the interpolation means. and control gain changing means for changing each control gain set for each of the coordinate axes based on a response delay in control of each of the coordinate axes by the rotation amount control means. servo motor control device.