JPH07200018A - Controller for robot - Google Patents

Abstract

PURPOSE:To improve positioning accuracy by performing backlash correction relating to the positioning of a target position from the relation of gravity torques and the rotation directions of respective axes. CONSTITUTION:In a step 300, the world coordinates of a present position and the target position are transformed to joint coordinates and an axis variable (i) for specifying a robot axis is initialized to '1'. Then, in the step 304, the gravity torques Tci and Toi around an (i) axis of the respective present and target positions are computed. Then, the absolute value of the gravity torque Toi of the target position is compared with a prescribed threshold value Th, whether or not the values of the gravity torque Tci of the present position and the gravity torque Toi of the target position are below '0' is judged in the step 308 when it is equal to or more than the threshold value Th and a backlash correction amount DELTAtheta1 per interpolation cycle is obtained in the step 310 when it is equal to or more than '0'. Then, the arithmetic operation of the gravity torque is repeatedly performed until the axis variable (i) becomes the maximum value of '6' and a program is ended when it becomes the maximum value.

Description

Detailed Description of the Invention

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a control device for a robot which prevents a positioning error due to backlash of a robot axis.

[0002]

2. Description of the Related Art In an articulated robot, a speed reducer is arranged between a motor and an arm rotating shaft, and this speed reducer has a backlash. In order to improve the positioning accuracy due to this backlash, JP-A-4-
The technique described in Japanese Patent No. 233602 is known. In this technique, the joint torque applied to each axis is calculated, the bending angle of each axis is calculated from the joint torque, and the control target position is corrected by the bending angle. Correcting the backlash by using the joint torque in this way, even if the arm moving direction is reversed like a robot,
This is an effective means when backlash may not occur due to the weight of the arm.

[0003]

However, in the above method, the relationship between the bending angle and the joint torque has a very large change rate with respect to the joint torque in the region where the joint torque is close to zero. Therefore, in order to accurately obtain the deflection angle, the accuracy of the joint torque value becomes a problem in the vicinity of zero of the joint torque. However, when the target position is near the gravity torque of 0, the gravity torque is proportional to the sine of the angle formed by the vertical axis and the robot arm. Therefore, it is accurately determined from the sign of the gravity torque that the arm crosses the vertical axis. It will be difficult to do. Further, since the joint torque changes depending on the object held by the hand, it is difficult to obtain a highly accurate calculated value. Also, while the gravity torque is large,
Even if the direction of the rotation angle changes, the backlash error does not occur at that time. However, if the rotation direction is reversed while the gravity torque is near 0, the direction of the gravity torque is actually reversed and a backlash error occurs. Therefore,
The conventional method has a problem in that the positioning accuracy of the articulated robot is not good.

The present invention has been made to solve the above problems, and an object thereof is to improve the positioning accuracy by accurately performing backlash correction in an articulated robot.

[0005]

The configuration of the invention for solving the above-mentioned problems is a teaching device which defines the positions and orientations at a plurality of teaching points in a control device for controlling the position and orientation of the hand tip of an articulated robot. Data and a data storage means for storing an operation program including a movement command word for instructing movement of the tip of the hand, and gravity for calculating a gravity torque of each axis at a current position of the hand tip and a target position defined by the movement instruction word. A size for determining the torque calculation means and whether the absolute value of the gravitational torque of each axis at the target position exists in a first state that is equal to or larger than a predetermined threshold value or in a second state that is smaller than the predetermined threshold value. Determination means, rotation direction determination means for determining whether or not the direction of the rotation angle of each axis is reversed,
The direction of the gravity torque at the current position and the direction of the gravity torque at the target position, the torque direction determination means for determining whether the reverse state is the mutually opposite or the non-inverted state where the direction of the gravity torque is the same, When the magnitude determining means determines that the absolute value of the gravitational torque at the target position is in the first state and the torque direction determining means determines that the gravitational torque at the target position is in the inverted state, or by the magnitude determining means. It is determined that the absolute value of the gravity torque at the target position is in the second state,
And, in the second state, when the rotation direction reversing means determines that the rotation direction is reversed, the backlash correction means for correcting the rotation angle of each axis by the backlash of that axis in positioning with respect to the target position. That is to say.

[0006]

When the tip of the hand of the robot is positioned from the current position to the target position defined by the movement command, the gravity torque at both positions is determined from the position and orientation of the hand tip at the current position and the target position. Is calculated.
In the present invention, backlash correction is performed for positioning the target position based on the relationship between the gravity torque and the rotation direction of each axis.

The case where the backlash correction is performed is as follows. (1) When the magnitude of the absolute value of the gravity torque at the target position is equal to or greater than a predetermined threshold value, the gravity torque at the current position and the gravity torque at the target position have opposite signs. (2) When the magnitude of the absolute value of the gravity torque at the target position is smaller than a predetermined threshold value, the rotation direction is reversed while the gravity torque is smaller than the predetermined threshold value.

As described above, when the absolute value of the gravitational torque at the target position is large, it is considered that there is no error in the sign, so whether or not the direction of the gravitational torque is reversed during the movement process by the movement command. Can be accurately determined. Therefore, in this case, whether or not the rotation angle of the target position is corrected by the amount of backlash can be determined depending on whether or not the sign of the gravity torque changes.

When the absolute value of the gravity torque at the target position is small, that is, when the rotation angle of the target position exists near the axis that reverses the direction of the gravity torque, the sign of the gravity torque is accurately determined. Can not. Further, reversing the direction of the rotation angle while the gravitational torque is small may cause a backlash error. Therefore, in this case, the rotation angle of the axis is corrected by the amount of backlash in the movement process by the movement command. Whether or not the backlash correction is performed on the target position is sequentially executed along the movement route. Therefore, as long as the sign of the gravity torque at the starting point of the path, for example, the origin, can be accurately determined, if the backlash correction is not performed, the same sign as the gravity torque of the previous teaching point, the backlash correction is performed. The sign of the gravitational torque at the target position can be successively determined accurately by using a different sign from the gravitational torque at the previous teaching point.

According to the present invention, since the reversal of the gravitational torque can be accurately determined as described above, the backlash correction becomes accurate and the positioning accuracy of the articulated robot is improved.

[0011]

EXAMPLES The present invention will be described below based on specific examples. FIG. 1 is a mechanism diagram showing the mechanism of a 6-axis articulated robot. 10 is the robot body, and the body 10 is on the floor
A base 13 for fixing the column 12 is disposed, a column 12 is fixedly mounted on the base 13, and a body 14 is rotatably disposed in the column 12. The body 14 rotatably supports the upper arm 15, and the upper arm 15 rotatably supports the forearm 16. Body 14,
The upper arm 15 and the forearm 16 are respectively
Servo motors Sm1, Sm2, Sm3 (see Fig. 2)
It is driven to rotate about axes a, b and c. This rotation angle is detected by the encoders E1, E2, E3. A twist wrist 17 is rotatably supported on the tip of the forearm 16 about the d axis, and a bend list 9 is rotatably supported on the twist wrist 17 about the e axis.
A swivel wrist 18 having a flange 18a at its tip is pivotally supported on the bend wrist 9 so as to be rotatable around the f-axis. A hand 19 is attached to the flange 18a. Servomotors Sm4, Sm for d-axis, e-axis, and f-axis
5, driven by Sm6, its rotation angle is encoder E
4, E5, E6. The opening / closing operation of the hand 19 is controlled by the tool driving circuit 23.

FIG. 2 is a block diagram showing the electrical construction of the robot controller according to the present invention. CPU
A memory 25, servo CPUs 22a to 22f for driving a servo motor, an operation panel 26 for issuing an operation start instruction, a jog operation instruction, a teaching point instruction, and the like are connected to the memory 20. The servo motors Sm1 to Sm6 for driving the axes a to f attached to the robot are respectively the servo CPU 22a.
Driven by ~ 22f.

Each of the servo CPUs 22a to 22f outputs an angle command value θ ₁ to an angle command value of each axis output from the CPU 20.
The output torque of the servomotors Sm1 to Sm6 is controlled based on θ ₆ , the moment of inertia D _i , and the gravity torque T _i .
The detection angles α _{1 to} α ₆ output by the encoders E1 to E6 connected to the respective drive shafts are the CPU 20 and the servo CPU 22.
a to 22f, the CPU 20 calculates the moment of inertia and gravity torque of each axis, and the servo CPU 2
It is used for position feedback control, speed feedback control, and current feedback control by 2a to 22f.

In the memory 25, a PA area in which a program for operating the robot according to the teaching point data is stored, a PDA area in which the teaching point data indicating the position and orientation of the hand 19 is stored, and a heavy object ahead of the flange 18a, That is, the hand 19 or the SDA area for storing the barycentric coordinates (a, b, c) of the object including the hand 19 and the object gripped by the hand 19 and its mass m _L and the ITA area for storing the servo parameters. INA that stores the angle command values θ _{1 to} θ ₆ of each axis at the interpolation point obtained by the interpolation calculation
Area and detection angle α output from encoders E1 to E6
An ANG area for storing _{1 to} α ₆ is formed.

The servo CPUs 22a to 22f for the respective axes achieve the functions of the circuit shown in FIG. 3 by sequential digital control. That is, it has a current feedback loop, a velocity feed-forward loop, and an acceleration feed-forward loop with a feedback loop at each rotational angle position and a velocity feedback loop.

Next, the operation of this apparatus will be described. FIG. 5 shows an operation program stored in the PA area of the RAM 25. Line numbers 10, 50 and 90 are command words for setting the barycentric coordinates (a, b, c) of the heavy object forming the data setting means and the mass m _L thereof. By executing this command, the barycentric coordinates (a, b, c) and the mass m _L data are stored in the SDA area of the RAM 25. Command word area A
Represents the barycentric coordinate r, and the region B represents the mass m _L. The center of gravity coordinates (a, b, c) are the flange 18 of the swivel list 18.
It is given by the coordinates viewed from the link coordinate system fixed at the center of a, that is, the normal, oriental, and approach coordinates (in mm). In addition, mass is expressed in kg. Line numbers 10 and 90 are command words related to data setting when the heavy object ahead of the flange 18a is only the hand 19, and line numbers 90 are command words related to data setting when an object is gripped by the hand 19. is there. Also, line number 20
The HAND OFF command of 80 and 80 is a command for opening the hand 19, and the HAND ON command of line number 40 is a command for closing the hand 19. In addition, line number 3
MOVE command words of 0, 60, 70, 100, 110 are command words for moving to the designated teaching point Pn.

According to the operation program shown in FIG. 5, the robot opens the hand 19 and moves to the point P1, then closes the hand 19 to hold the object, and passes through the point P2 to P3.
Move to the point, open the hand 19, leave the object, P
It is possible to move to P5 via 4 points.

FIG. 4 is a flow chart of a main program for decoding the operation program by the CPU 20. When the MOVE command is decoded in step 100, an interpolation calculation for moving the hand 19 from the current position to the designated teaching point is executed in step 102. Then, the angle command values θ _{1 to} θ ₆ of the respective axes obtained by the interpolation calculation are the servo CPUs 22a to 22f.
Is output to. In step 104, LOAD
When the command word is decoded, in step 106, the data of the barycentric coordinates and mass of the previous heavy object from the hand described in the command word is stored in the SDA area of the RAM 25. When the HAND OFF command is decoded in step 108, the tool drive circuit 2 is read in step 110.
3 is given an instruction to open the hand 19. Also, step 11
When the HAND ON command is decoded in step 2, step 1
At 14, the tool drive circuit 23 is instructed to close the hand 19.

The interpolation calculation between the teaching points in step 102 is executed according to the flowchart shown in FIG.
As well known, the interpolation calculation can be performed by using the rotation spindle method or the like (for example, Japanese Patent Laid-Open No. 62-15400).
No. 6). In step 200, the backlash correction amount calculation program shown in FIG. 7 is started.

In step 300 of FIG. 7, the world coordinates of the current position and the target position (O-xy fixed on the floor).
z system coordinates) are converted into joint coordinates (coordinates of rotation angles of each axis of the robot). Next, in step 302, an axis variable i that identifies the axis of the robot is initialized to one.

Next, at step 304, the gravity torque Tc _i about the i-axis at the current position and the target position,
To _i is calculated. The torque τ _i around the i-axis is
As is well known by the following equation, it can be obtained from the derivative of the Lagrangian function with respect to the robot arm.

[0022]

[Formula 1] τ _i = Σ _{j = i} ⁿ Σ _{k = i} ^j Tr (U _jk J _j U _ji ^T ) D ² θ _k-
Σm _j gU _ji ^j r _j However, the components of the Coriolis force and the centrifugal force are small and are excluded from the above equation. J _j is the link inertia matrix only by the members forming the j axis, Tr is the sum of the diagonal components of the determinant, m _j is the mass of the j axis link, g is the gravitational acceleration, and ^j r _j is the j axis link coordinate system. It is the vector of the center of gravity of the j-axis link represented, and D ² θ
_k is the second derivative of the angle with respect to time, that is, the angular acceleration. In the expansion equation of the first term of the above equation, the coefficient of D ² θ _i becomes the inertia matrix D _i , and the second term becomes the gravity torque T _i around the i axis. Also, U _jk is

[0023]

2 (0) (when j <i)

(3) δ ( ⁰ A ₁ ¹ A ₂ … ^j-1 A _j ) / δθ _i = Π _{s = 1} ^{j s-1} A _s
Δ ^i-1 A _i / δ θ _i (when j ≧ i) where s ≠ i. Also, ^j-1 A _j is the j-axis link coordinate j
-1 is a homogeneous coordinate transformation matrix that transforms to link coordinates,
⁰ A ₁ is a homogeneous coordinate transformation matrix that transforms uniaxial link coordinates into world coordinates.

J _j can be expressed as follows using the inertia tensor I _ij .

[Equation 4]

However, r _ix , r _iy , and r _iz are the coordinates of the center of gravity of the j-axis link viewed from the j-axis link coordinates, and m _i is its mass. or,

## EQU5 ## I _ij = ∫ [δ _ij [Σx _k ² ] -x _i y _i ] dm.

Note that J ₆ changes depending on the barycentric coordinates (a, b, c) and the mass m _L set by the LOAD command. J ₆ is (r _ix , r _iy , r _iz ) =
It can be obtained by setting (a, b, c) and m _i = m _L.

From Equation 1, the gravity torque T _i around each axis at the current position and the target position is calculated. In this embodiment, in order to reduce the calculation time, only the (i, i) diagonal component is considered, assuming that the interaction with other axes is small.

When the gravity torque is calculated, step 30
At 6, it is determined whether the absolute value of the gravity torque To _{i at} the target position is smaller than a predetermined threshold Th. To
_If the absolute value of _i is greater than or equal to the predetermined threshold value Th, it is determined in step 308 whether the product of the gravity torque Tc _{i at} the current position and the gravity torque To _{i at} the target position is 0 or less. That is, it is determined whether the gravity torque Tc _{i at} the current position and the gravity torque To _{i at} the target position have different signs. If the signs are different, in step 310, the backlash correction amount Δθ _i per interpolation cycle is calculated by A _i / n. However, A _i is the backlash amount of the i-axis, and n is the number of interpolations.

On the other hand, if it is determined in step 308 that the gravity torque Tc _i at the current position and the gravity torque To _{i at} the target position have the same sign, backlash correction is not necessary, so step In step 312, the backlash correction amount Δθ _i per interpolation cycle is set to 0.

On the other hand, if it is determined in step 306 that the absolute value of the gravitational torque To _{i at} the target position is smaller than the predetermined threshold value, then in step 314 a flag F _i is stored to store that fact. It is set to "1", and in this case, the backlash correction per interpolation is not performed, so the backlash correction amount Δ per interpolation is
θ _i is set to 0. Then, the process proceeds to step 316.

Next, in step 316, the axis variable i
Has reached the maximum value 6, and if it has not reached that value, in step 318, the axis variable i is incremented by 1, and the process returns to step 304 to determine the gravity torque of the next axis. The calculation is repeatedly executed. When the axis variable i reaches the maximum value of 6, this program ends.

When the above calculation is completed, the process returns to step 202 in FIG. In step 202, the direction vector of the rotary spindle is calculated from the position and posture of the start point and the position and posture of the next positioning target point given as the teaching data expressed in the world coordinate system, and in step 204, the rotary spindle is rotated. The rotation angle Θ around is calculated. Next, in step 206, the interpolation angle ΔΘ is calculated based on the position and orientation of the starting point, and the next step 208
At, the posture conversion matrix R for converting the position and posture of the start point into the position and posture at the interpolation point is calculated using the interpolation angle ΔΘ.

Then, in step 210, the attitude transformation matrix is applied to the homogeneous coordinate matrix representing the position and orientation of the starting point, and the homogeneous coordinate matrix representing the position and orientation at the interpolation point is calculated. Next, in step 212, the value in the joint coordinate system from the homogeneous coordinate transformation matrix of the position and orientation expressed in the world coordinate system at the interpolation point,
That is, the rotation angle θ _{i of} each axis is calculated.

Next, at step 214, the flag F
Whether _i is 1 or not is determined, and if F _i is not 1, the process proceeds to step 216, and the backlash correction amount Δθ per interpolation calculated in step 310 from the rotation angle θ _i at that interpolation point. _i is subtractively corrected. That is, the corrected rotation angle θ _i = θ _i −Δθ _i . The corrected angle command value θ _i of these interpolation points is stored in the INA area of the RAM 25.

On the other hand, in step 214, the flag F
_{If i} is determined to be 1, it is determined in step 218 whether or not the backlash correction calculation by reversing the rotation direction is completed. If the backlash correction calculation is not completed, step 218 is performed. At 220, it is determined whether or not the rotation direction is reversed at the joint coordinate of the interpolation point within the angle range where the gravity torque of the axis is smaller than the predetermined threshold with respect to the joint coordinate corresponding to the target position. It If the rotation direction is reversed at the interpolation point between them, step 222
At, the backlash correction amount A _i is subtractively corrected with respect to the rotation angle θo _i of the target position. That is, the corrected rotation angle θo _i = θo _i −A _i . The corrected angle command values θo _i of these interpolation points are stored in the INA area of the RAM 25. Further, in this case, since the correction is not performed for each unit interpolation, the correction amount Δθ _i per interpolation is set to 0.

On the other hand, when it is determined in step 218 that the backlash correction calculation by reversing the rotation direction is completed, in step 224, the angle correction of the target position is not performed in duplicate. , Flag F _i is set to 0.

Next, in steps 226 and 228, the angle command values θ _{1 to} θ _{6 of} the interpolation points stored in the INA area of the RAM 25, the moment of inertia and the gravitational torque are set in the ITA area of the RAM 25. The servo parameters D _i, T _i are output to the servo CPUs 22a to 22f. As a result, backlash correction is performed on each axis, and accurate positioning control is possible.

Then, in step 230, it is judged whether or not the interpolation point has reached the target position given as the teaching data, and if the interpolation is not completed, the position and orientation of the next interpolation point is calculated. In order to do so, steps 208 and onward are executed. On the other hand, when the interpolation is completed, all flags F _i are returned to the initial value of 0 in step 232, and the process returns to the main program of FIG. 4 to decode the next command word.

In the embodiment described above, the magnitude of the gravity torque is directly obtained, and the case where the backlash correction is performed by the gravity torque and the case where the backlash correction is performed by the moving direction are discriminated. However, when the magnitude of the gravitational torque can be determined by the angle of the arm of the robot, the angle may be determined.

[0040]

[Brief description of drawings]

FIG. 1 is a configuration diagram showing a robot used in an apparatus according to a specific embodiment of the present invention.

FIG. 2 is a block diagram showing a configuration of a robot control device.

FIG. 3 is a block diagram showing a process of a servo CPU in FIG.

FIG. 4 is a flowchart showing a main program for controlling the position and attitude of the robot.

FIG. 5 is an explanatory diagram showing an operation program for controlling the position and orientation of the robot.

FIG. 6 is a flowchart showing a procedure of interpolation calculation.

FIG. 7 is a flowchart showing a procedure for calculating a backlash correction amount.

[Explanation of symbols]

10 ... Robot 18 ... Swivel list 18a ... Flange 19 ... Hand 20 ... CPU (gravitational torque calculation means, size determination means,
Rotation direction reversal means, torque direction determination means, backlash correction means) 22a to 22f ... Servo CPU 25 ... RAM (data storage means) E1 to E6 ... Encoder step 304 ... Gravity torque calculation means step 306 ... Size determination means step 222 Rotation direction reversing means Step 308 Torque direction determining means Steps 310, 216, 222 ... Backlash correcting means

─────────────────────────────────────────────────── ─── Continuation of the front page (51) Int.Cl. ⁶ Identification code Internal reference number FI technical display location B25J 13/08 G05B 19/18 G05D 17/02

Claims (1)

[Claims] 1. A control device for controlling the position and posture of a hand tip of an articulated robot, the teaching data defining the positions and postures at a plurality of teaching points, and an operation including a movement command word for instructing movement of the hand tip. Data storage means that stores a program, gravity torque calculation means that calculates the gravity torque of each axis at the current position of the hand tip and the target position defined by the movement command, and gravity torque of each axis at the target position And a magnitude determining means for determining whether the absolute value of the present value exists in a first state that is equal to or larger than a predetermined threshold value or a second state that is smaller than the predetermined threshold value, and the direction of the rotation angle of each axis is reversed. The direction of the gravity torque at the current position and the direction of the gravity torque at the target position are opposite to each other. The direction of torque is the same as the non-inverted state, the absolute value of the gravity torque at the target position is determined to be in the first state, and When the gravity torque at the target position is determined to be in the inverted state by the torque direction determination means, or when the absolute value of the gravity torque at the target position is determined to be the second state by the magnitude determination means. And the rotation direction determining means determines that the direction of the rotation angle is reversed during the second state, regarding the positioning with respect to the target position, the rotation angle of each axis is equal to the backlash of the axis. A control device for a robot, comprising: a backlash correction means for correction.