JPH0233169B2 - - Google Patents

Description

DETAILED DESCRIPTION OF THE INVENTION The present invention relates to an industrial robot, and more particularly to an industrial robot equipped with a control device for preventing transient vibrations thereof.

In order to reduce weight and increase the range of motion, industrial robots have an "open" structure, that is, the ends of movable parts such as arms are free ends. For this reason,
The mechanism of an industrial robot has a low natural frequency and a small damping coefficient.

Therefore, when attempting to perform high-speed trajectory control with an industrial robot, a large change in acceleration occurs in the drive motor in parts of the trajectory with large curvature, which causes undesirable transient vibrations in the mechanism. Causes orbit error.

The basic measure to prevent this transient vibration of the mechanical part is to increase the rigidity and damping rate of the mechanical part, but improvements to the mechanical part can be made by changing the size of the mechanical part,
This is not easy because it increases weight and therefore costs.

The above orbit error can be reduced by reducing the curvature of the orbit, but since the magnitude of the acceleration generated by the drive motor for the same orbit is proportional to the tangential velocity of the orbit, it is necessary to perform high-speed orbit control. It is essentially necessary to prevent transient vibrations of the mechanism.

The present applicant filed a patent application in 1983 with the patent application ``Mechanical Stiffness Compensation Servo Control Device'' published in Japanese Patent Application Laid-Open No. 54-31877 as a method for equivalently increasing the rigidity of a mechanical system having vibrational characteristics or non-vibratory control. -Proposed in 92155 "Control method for electric machines". The former has the effect of mainly reducing transient vibrations, and the latter has the effect of mainly reducing sustained vibrations (forced vibrations). However, all of these methods are basically for single-axis servo systems, and it remains unresolved how to specifically apply them to industrial robots that consist of multi-axis servo systems. It was hot.

In general, the motion of an industrial robot is a combination of the above-mentioned undesirable transient vibrations on a normal motion based on data instructed in advance by teaching or the like. These two motions can be distinguished by the difference in the frequency components of the motion. The motion of is usually at a period of several seconds to several tens of seconds, and the motion of is at a frequency of 5 to 50 Hz or more, which is the resonant frequency of the mechanical system. It is the frequency. Therefore, in terms of the response of a normal servo system, the motion can be considered to be a DC component, and the motion can be considered to be an AC component. In order to dampen this transient vibration, V(t) (a three-dimensional vector as a function of time), which is an alternating current component of the motion of the robot's tip P, should be converged to zero. For a single-axis servo system, if the rotational position and rotational speed as well as the acceleration of transient vibration are fed back to the rotary motor of the servo axis according to the method described in the above-mentioned patent publication, a damping effect will occur against fluctuations in acceleration, and the acceleration will be zero in a steady state. converges to. When acceleration is zero, velocity is constant and the alternating current component of velocity is zero.

However, industrial robots generally have a multi-axis configuration, and each axis often mechanically interferes strongly with each other. Therefore, in industrial robots, vibrations that occur in one axis also cause vibrations in other axes, resulting in beats or complex vibrations that are a combination of vibrations from multiple axes. For this reason, even if the method of the patent publication is applied only to a specific axis of an industrial robot, for example, a shaft with particularly large vibrations, a large effect of damping transient vibrations and sustained vibrations (forced vibrations) cannot be obtained. Although FIG. 5a shows the attenuation of one axis when there is no interference between axes, a beat may occur between two axes that strongly interfere with each other, as shown in FIG. 5b. In such a case, if the method described in the patent publication is applied only to one axis, the vibration damping effect will be effective only during the period when the amplitude of vibration of the applied shaft increases, and the amplitude of vibration of the other axis will increase. Since there is no damping effect during this period, the vibration of the shaft to which the method of the above-mentioned patent publication is applied cannot be rapidly damped. FIG. 5c shows an example of a vibration experiment when the method of the above-mentioned patent publication is applied to the axis l. The broken line D corresponds to the beat (FIG. 5b) when there is no vibration suppression control.

If the motion of each axis with respect to the stationary coordinate system depends not only on the position of the corresponding rotary motor but also on the position of the rotary motors of the other axes, the acceleration of the point on the corresponding moving mechanism element is This includes not only the acceleration information of the axis relative to the stationary coordinate system, but also the motion acceleration of the axis on which the motion depends.
Therefore, in order to detect the acceleration signal and extract only the acceleration of the corresponding axis relative to the stationary coordinate system, it is necessary to remove other signal components. Removal of this signal generally requires multidimensional matrix calculations (essentially equivalent to coordinate transformation calculations from the robot's coordinate system to the stationary coordinate system) in mechanisms with complex configurations such as industrial robots. The configuration was complex and was not practical for signals that required high-speed processing, such as acceleration.

Therefore, the inventors of the present application have focused on the fact that there is an acceleration correction that allows simple and high-speed calculations without the need for multidimensional matrix calculations as described above, provided that the correction is limited to a specific type of robot.

Here, we will discuss the mechanical configurations of the three specific types of industrial robots mentioned above: (i) Cartesian coordinate type, (ii) Cylindrical coordinate type, and (iii) Joint type with parallelogram links. will be explained based on FIGS. 1 to 4. Figure 1 is a symbol representing the basic mechanism of an industrial robot. Figure 1 shows a slide mechanism in which arm C slides in the X-axis direction by drive unit B, and Figure 2 shows a slide mechanism in which arm C slides in the θ direction by drive unit B. FIG. 3 shows that the arm C is fixed to a stationary coordinate system.

(i) Orthogonal coordinate robot (Figure 2) Moves linearly with respect to the stationary coordinate system O-XYZ,
It is composed of three axes, x-axis, y-axis, and z-axis, which are orthogonal to each other. Each axis has a combination of a rotary motor and a rotary motion/linear motion converter such as a ball screw, and the positions x, y, and z of each axis are controlled by the rotational position of the rotary motor. The position of the robot's tip P is a function of the coordinate values x, y, and z of each axis.

(ii) Cylindrical coordinate robot (Figure 3) The A-axis rotates with respect to the stationary coordinate system O-XYZ, and the z-axis and z move linearly on the θ-axis.
It consists of an r-axis that is orthogonal to the axis and moves in a straight line. The position of the robot's tip P is a function of the coordinate values r, θ, and z of each axis.

(iii) An articulated robot composed of parallelogram links (Fig. 4) It is composed of a θ axis that rotates with respect to the stationary coordinate system O-XYZ, an axis that rotates around the θ axis, and a ψ axis. Ru. The θ-axis, the axis, and the ψ-axis are in the same plane. The ψ axis and axis are nodes A, B,
They constitute parallel links represented by C and D, and are always ABCDDP and ADBC (symbols indicating parallelism). Due to this parallelogram link, the movement of the axes and ψ-axes is independent of the stationary coordinate system O-XYZ.In other words, even if the axes are fixed and the ψ-axis is moved, the movement of the axes and ψ-axes is independent of the stationary coordinate system
The attitude angle of the axis relative to the stationary coordinate system O remains unchanged. Even if the axis is fixed and the ψ axis is moved, the attitude angle of the ψ axis is fixed relative to the stationary coordinate system O.
-Invariant to XYZ. The position of the robot's tip P is a function of each axis θ, , ψ.

The common properties of the above-mentioned Cartesian coordinate type, cylindrical coordinate type, and articulated robots constructed with parallelogram links are that the motion of each axis constituting the robot is independent with respect to the stationary coordinate system O-XYZ. In other words, there is no interference between the axes.

The present invention provides an industrial robot having a specific form as described above, in which the rotary motor of each servo axis is coupled to the movable mechanism element of the axis directly or via a mechanical transmission mechanism, In a multi-axis industrial robot, in which the movable mechanism element of each servo axis makes direct or rotational motion, and the position of the movable mechanism element in motion with respect to the stationary coordinate system is defined only by the rotational position of the rotary motor, the movable mechanism element of each servo axis is A linear acceleration detector for detecting acceleration with respect to a stationary coordinate system is provided on the movable mechanism element and in a direction parallel to the tangential direction of the motion caused by the rotation of the rotary motor, and the linear acceleration detector detects the rotational position signal and rotational speed of the rotary motor. Along with the signal, the acceleration signal detected by the linear acceleration detector is fed back to the control device of the servo axis, and the control device of each servo axis receives the feedback signal and converts it into an AC component of the speed of the movable mechanism element. It has a circuit that adjusts the phase so that the phase is opposite to that of the

Therefore, transient vibrations during high-speed trajectory control of the industrial robot can be prevented, and high-speed and highly accurate trajectory control of the industrial robot can be realized.

The principle of the present invention will be explained below with reference to FIGS. 2 to 4.

(i) In the case of a Cartesian coordinate robot, the coordinate system o-xyz and stationary coordinate system O-XYZ of the orthogonal coordinate robot are such that the directions of the x-axis and the X-axis, the y-axis and the Y-axis, and the z-axis and the Z-axis are the same, respectively. Thinking like this (Figure 2), the coordinates of the robot's tip ^P are
Coordinates of P at (Xo, Yo, Zo) ^T : Coordinates of the origin of the robot in the stationary coordinate system O-XYZ (x, y, z) ^T : Amount of movement of the robot T: Expressed as a symbol representing transposition.

If x〓, y〓, and z〓 are the AC components of the moving speed of the robot on the x, y, and z axes, respectively, then from the above equation, the AC component of the speed of the robot's tip P is X〓 Y〓 Z〓 = 1 0 0 It is expressed as 0 1 0 0 0 1x〓y〓z〓. Therefore, (x〓, y〓, z〓) ^T → (0
,
0, 0) then (X〓, Y〓, Z〓) ^T → (0, 0, 0)
becomes. In other words, if the vibration of each axis of the orthogonal coordinate robot is attenuated, the vibration of the tip P with respect to the stationary coordinate system will be attenuated.

Therefore, in a Cartesian coordinate robot, x, y,
The vibration of each axis of z relative to the stationary coordinate system O-XYZ may be measured, and the method disclosed in the patent publication may be applied to each axis.

(ii) Cylindrical coordinate robot From the relationship between the coordinate system o-γθz of the cylindrical coordinate robot and the stationary coordinate system O-XYZ (Figure 3), the position of the robot's tip P is expressed as X Y Z = γ cos θ γ sin θ z + h .

If γ〓, θ〓, and z〓 are the AC components of the velocity of γ, θ, and z axes, then from the above equation, the AC component of the velocity of the robot tip P is X〓 Y〓 Z〓=cosθ sinθ 0−γsinθ γcosθ It is expressed as 00 0 1γ〓 θ〓 z〓.

Unlike a Cartesian coordinate robot, X〓, Y〓, Z〓) ^T and (γ〓, θ〓, z〓) ^T are non-interfering, that is, the off-diagonal components of the coordinate transformation matrix are not all 0. , also in this case, if (γ〓, θ〓, z〓) ^T → (0, 0, 0), then (X〓, Y〓, Z〓) ^T → (0, 0, 0) .

Therefore, even in the case of a cylindrical coordinate robot, it is necessary to measure the vibration in the main motion direction for each of the γ, θ, and z axes so that the vibration is attenuated independently of the γ, θ, and z axes. Bye.

(iii) From the relationship between the coordinate system 0-θψ of the articulated robot and the stationary coordinate system 0-XYZ (Fig. 4), the position of the robot's tip P is X Y Z = (Lsin + lsinψ) cos θ (Lsin + lsin ψ) sin θ h + L cos - l cos ψ. expressed.

If θ〓, 〓, ψ〓 are the AC components of the velocity of the θ, , ψ axes, then from the above equation, the AC component of the velocity of the robot's tip P is It is expressed as

Therefore, when (θ〓, 〓, ψ〓) → (0, 0, 0), (X〓, Y〓, Z〓) → (0, 0, 0),
For each of the θ, , ψ axes, the static reference frame 0−
All you have to do is measure the vibrations in XYZ and attenuate the vibrations independently for each axis.

The sixth embodiment of the present invention based on the principle described above is described below.
It is shown in FIGS. 10 to 10 and will be explained below.

(i) Example 1 for a Cartesian coordinate robot (Fig. 6) In this example, the tip P (y
Acceleration detectors A for detecting the linear accelerations αx, αy, αz of the orthogonal axes x, y, and z with respect to the stationary coordinate system are arranged on the axis movable mechanism element (Fig. 6a),
The detected linear accelerations αx, αy, αz are applied to the rotation motors Mx, My, and servo system of the corresponding axes, respectively.
The rotational speed Vx, Vy, Vy from the rotational speed detector TGx, TGy, TGz,
(Fig. 6b) (ii) Example of implementation for a Cartesian coordinate robot (Fig. 7) ) In this embodiment, instead of the acceleration detector A in the above-described embodiment 1 (FIG. 6), x, y, and
Acceleration detector that detects linear acceleration αx, αy, αz with respect to the stationary coordinate system in the main motion direction of the y and z axes
Ax, Ay, Az are arranged (Figure 7a),
The detected linear accelerations αx, αy, and αz are fed back to the control devices Gx, Gy, and Gz of the rotary motors Mx, My, and Mz of the servo system of the corresponding axes in the same manner as in (i) (Fig. 7b).

(iii) Example 1 for a cylindrical coordinate robot (Fig. 8) In this example, for the θ-axis, linear acceleration αθ with respect to the stationary coordinate system in the tangential direction of rotation is applied at a distance l from the rotation center of the θ-axis. An acceleration detector Aθ is arranged to detect the linear accelerations αz and αr relative to the stationary coordinate system in the main movement direction, and acceleration detectors Az and Ar are arranged on the movable mechanism elements for the Z-axis and the r-axis, respectively (see Fig. 8). a) The detected accelerations αθ, αr, αz are sent to the control devices Gθ, Gr, Gz of the servo system motors Mθ, Mr, Mz of the corresponding axes,
Rotational speed Vθ, Vr, Vz from rotational speed detectors TGθ, TGr, TGz and rotational position detector PGθ,
Feedback is provided along with the rotational positions θ, γ, and z from PGr and PGz (Fig. 8b).

(iv) Embodiment 2 for a cylindrical coordinate robot (Fig. 9) In this embodiment, the tip P of the robot (on the r-axis movable mechanism element) is oriented in the tangential direction of the θ-axis and r
An acceleration detector A is arranged to detect linear accelerations αθ, αr, αz in orthogonal three-axis directions that coincide with the main motion directions of the axis and z-axis, and the detected linear accelerations αθ, αr,
As in case (iii), αz is the motor of the corresponding axis.
Feedback is provided to the control devices Gθ, Gr, and Gz for Mθ, Mr, and Mz. However, in this case, the linear acceleration αθ is proportional to the arm length r, so in order to keep the feedback gain constant with respect to the rotational angular velocity of the θ axis (there is an appropriate gain that gives a favorable response), the arm It is necessary to perform gain correction on the linear acceleration αθ in proportion to the reciprocal of the length r.

(v) Example of an articulated robot configured with a parallelogram link (Fig. 10) In this example, the axis is set so that the θ axis coincides with the linear direction of rotation at a distance l from the center of rotation. on the movable mechanism element (A
Coincident with the tangential direction of rotation (so that it does not coincide with the point D), i.e. perpendicular to the ψ axis, on the movable mechanism element (so that it does not coincide with the point D), i.e.
Acceleration detectors A〓, A〓, A〓 for detecting linear accelerations αθ, α, αψ in the main motion direction of each axis with respect to the stationary coordinate system are arranged in the vertical direction of CD (10th
Figure a), the detected linear accelerations αθ, α, αψ are applied to the servo system motors (Mθ, M, Mψ
A rotation speed detector is installed in the control device Gθ, G, Gψ.
Rotation speed Vθ, V from TGθ, TG, TGψ,
Feedback is performed with Vψ and rotational position detectors PGθ, PG, PGψ (Fig. 10b).

The rotary motor control device in the above embodiment implements the technology disclosed in the above-mentioned patent publication, and an embodiment thereof is shown in FIG. Reference numeral 1 denotes a position command generator that outputs command pulses for commanding the rotational position of the rotary motor. Reference numeral 2 denotes an operational amplifier that compares the command pulse output from the position command generator 1 and the rotational position signal from the rotational position detector of the rotary motor, and amplifies the difference. Reference numeral 3 denotes an operational amplifier that compares the speed command signal output from the operational amplifier 2 with the rotational speed signal from the rotational speed detector of the rotary motor and amplifies the difference. 4 is a phase adjustment circuit for compensating the phase characteristics of the rotary motor, which inputs the acceleration signal detected by the aforementioned linear acceleration detector to obtain a correction signal having a phase opposite to the alternating current component of the speed. 5 is a gain adjustment circuit that determines the magnitude of a correction signal for damping vibrations. The phase adjustment circuit 4 and gain adjustment circuit 5 described above are detailed in the aforementioned Japanese Patent Application No. 56-92155. 6 is a power amplifier including an operational amplifier.

In addition, there are many versatile types of linear acceleration detectors on the market, such as piezoelectric types and servo types.
It is inexpensive and easily available.

As described above, by inputting to the motor control device an acceleration correction signal whose phase is adjusted to be in opposite phase to the alternating current component of the velocity of the movement of the movable mechanism element for each axis, mutual interference can be prevented. Transient vibration between shafts can be prevented.

The present invention detects acceleration signals relative to a stationary coordinate system by providing a linear acceleration detector on a movable mechanical element connected to a rotary motor in a direction parallel to the tangential direction of the motion caused by the rotation of the rotary motor, and The acceleration signal for the servo axis is fed back to the control device for each servo axis, and the phase of the acceleration signal is adjusted so that the phase is opposite to the AC component of the speed of the movable mechanism element, thereby increasing the speed of the industrial robot. It enables high-speed and highly accurate trajectory control by preventing transient vibrations of industrial robots due to changes in the drive torque of the rotating motor, torque ripples of the rotating motor, or resonance phenomena in the mechanical system that occur during trajectory control. . In other words, the same effect as increasing the rigidity of the mechanical part of an industrial robot several times can be obtained. The present invention can be realized easily and inexpensively by simply arranging an acceleration detector in an existing industrial robot and does not require any changes to the mechanism or control device of the industrial robot.

[Brief explanation of drawings]

Figure 1 is a symbolic diagram of the basic mechanism of an industrial robot, and Figures 2, 3, and 4 are diagrams of articulated robots with orthogonal coordinates, cylindrical coordinates, and parallelogram links, respectively. Diagram showing the mechanism configuration, 5th
The figure shows vibrations between two interfering axes, FIGS. 6 to 10 show embodiments of the present invention, and FIG. 11 is a block diagram of a control device. Mx, My, M ₂ , Mθ, Mγ, M, Mψ……rotating motor, TGx, TGy, TGz, TGθ, TGγ,
Tg, TGψ...Rotation speed detector, PGx, PGy,
PGz, PGθ, PGγ, PG, PGψ...Rotational position detector, A, Ax, Ay, Az, Aθ, Aγ, A, Aψ...
...Acceleration detector, Gx, Gy, Gz, Gθ, Gγ, G,
Gψ...control device, 4...phase adjustment circuit, 5...
Gain adjustment circuit.

Claims (1)

[Claims] 1 Consisting of a plurality of servo axes, the rotation motor of each servo axis is coupled directly or via a mechanical transmission mechanism to a movable mechanism element of the servo axis, and the rotary motor of each servo axis is coupled to a movable mechanism element of the servo axis, and In a multi-axis industrial robot that performs linear or rotational movement, and in which the position of the movable mechanism element relative to a stationary coordinate system during the movement is defined only by the rotational position of the rotation motor of the servo axis, the movability of each servo axis A linear acceleration detector for detecting acceleration of the mechanical element with respect to a stationary coordinate system is provided on the movable mechanical element and in a direction parallel to the tangential direction of the motion caused by the rotation of the rotary motor, and the linear acceleration detector detects the rotational position signal of the rotary motor. The acceleration signal detected by the linear acceleration detector is fed back to the control device of the servo axis together with the rotational speed signal, and the feedback signal is sent back to the control device of each servo axis to determine the speed of the movable mechanism element. An industrial robot characterized by having a circuit that adjusts the phase to be in opposite phase to the alternating current component of the robot.