JPH07104825A - Controller for robot - Google Patents

Abstract

PURPOSE:To decrease the cycle time by enabling move on the operational locus of a straight line or a circular arc at maximum speed to be changed corresponding to a posture. CONSTITUTION:The rotating speed of each axis is calculated from the moving speed on the operational locus at that time at an interpolating point, and the rotating speed ratio of each axis to the maximum rotating speed is calculated (232). When the axis with the maximum rotating speed ratio is decided and the rotating speed ratio of that axis is larger than an upper limit value, the moving speed at the next interpolating point is decelerated just by a prescribed value (234-236) but when the rotating speed ratio is smaller than a lower limit value, the moving speed is accelerated just by a prescribed value (238-240). At the next interpolating point, the rotating angle of each axis is calculated based on the corrected moving speed and according to that rotating angle, each axis is controlled. Since one axis is rotated at the allowable maximum rotating speed at least, high-speed positioning is made possible.

Description

Detailed Description of the Invention

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a robot controller capable of high-speed positioning when positioning a robot hand.

[0002]

2. Description of the Related Art Conventionally, in movement control in linear interpolation, circular interpolation, etc. of a robot, a movement speed on an operation locus is commanded. Then, the rotation speed of each axis is determined so that the commanded movement speed on the operation locus is obtained, and each axis is controlled by the rotation speed. At this time, the maximum movement speed on the motion locus is determined as a value such that the rotation speed of all axes does not exceed the maximum rotation speed determined by the ability of the motor regardless of the posture of the robot.

[0003]

As described above, since the maximum moving speed is given by the speed on the motion locus, even if the moving speed on the motion locus has the maximum value, the rotation of each axis depends on the posture of the robot. The speed is not maximum. That is, the maximum capacity of the robot is not utilized.

The present invention has been made to solve the above problems, and an object thereof is to enable movement on a motion locus such as a straight line and an arc at a maximum possible speed that can be changed depending on the posture. Is. By doing so, the robot is allowed to exert its maximum ability and the cycle time is reduced.

[0005]

According to the present invention, a robot controller for positioning a robot hand at a given teaching point according to a motion locus such as a straight line or a circular arc can move the hand along the motion locus. The maximum possible speed movement command means to set the mode to the maximum speed and the maximum rotation of each axis of the rotation speed of each axis to move the hand at the commanded movement speed on the operation trajectory at the interpolation point on the operation trajectory. Rotational speed ratio computing means for computing a rotational speed ratio with respect to speed, setting means for setting an upper limit value and a lower limit value of the rotational speed ratio, and axis determining means for determining a maximum rotational speed axis at which the rotational speed ratio is maximum. When the rotation speed ratio of the maximum rotation speed axis becomes larger than the upper limit value at the interpolation point, the moving speed at the next interpolation point is decreased by a predetermined value, and the rotation speed ratio of the maximum rotation speed axis decreases. If it becomes smaller than the value, a movement speed control means for increasing the moving speed by a predetermined value, based on the moving speed determined by the movement speed control means,
Angle control means for determining the next interpolation point and controlling each axis based on the determined rotation angle of each axis.

[0006]

The function and effect of the invention The speed control of the present invention is executed by designating the mode in which the movement control on the predetermined motion locus by the straight line, the circular interpolation or the like is performed at the maximum possible speed. At a certain interpolation point, the rotation speed of each axis is calculated from the moving speed on the operation locus at that time, and the rotation speed ratio to the maximum rotation speed of each axis is calculated. Then, the axis having the maximum rotation speed ratio is determined, and when the rotation speed ratio of the axis is larger than the upper limit value, the moving speed on the operation locus at the next interpolation point is reduced by a predetermined value, and the rotation speed ratio If is smaller than the lower limit value, the moving speed on the motion locus is increased by a predetermined value. Then, the next interpolation point is determined based on the corrected moving speed, and the angle of each axis is controlled based on the determined rotation angle of each axis.

As described above, the hand moves on the operation locus such as the instructed straight line or arc, and the moving speed varies unlike the instructed constant moving speed, but the ability concerning the speed of each axis is sufficient. It will move at the maximum possible moving speed that was demonstrated. Therefore, it is possible to maximize the ability of the robot and reduce the cycle time. This control is
It can be used for high-speed positioning and the like.

[0008]

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 of 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 θ ₁ for 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 in accordance with the teaching point data is stored, a PDA area in which teaching point data representing the position and orientation of the hand 19 is stored, and an acceleration (deceleration) command value. And an SDA area for storing the target speed, an ITA area for storing the upper limit value Th and the lower limit value Tl of the rotation speed, and an INA for storing the angle command values θ _{1 to} θ ₆ of each axis at the interpolation points obtained by the interpolation calculation. An area and an ANG area for storing the detected angles α _{1 to} α ₆ output from the encoders E1 to E6 are formed.

Next, the operation of this apparatus will be described. FIG. 6 shows an operation program stored in the PA area of the RAM 25. By this operation program, the tip of the robot hand moves as shown in FIG. Line number 1
Acceleration and deceleration in the acceleration process of moving from the stop position (start position) to the target speed and in the deceleration process of moving from the target speed to the stop position (target position) (ratio to the maximum acceleration A that can be output by the machine 100 c% (Fig. 6 Then 100c =
It is a command word that commands 100%)). Line number 2 indicates the target speed (ratio 10 to the maximum speed V that can be output by the machine).
0b% (100b = 100% in FIG. 6). Line number 3 is a command word for positioning the hand tip at the machine origin, line number 4 is a MOVESP command word for positioning to teaching point P2 in the maximum possible speed movement mode, and line number 5 is a teaching point P.
It is a MOVE command for positioning by linear interpolation to 1.
Line number 6 is a command for closing the hand and gripping the article. Line number 7 is a MOVE command word for positioning to teaching point P2 by linear interpolation, line number 8 is a MOVESP command word for positioning to teaching point P3 in the maximum possible speed movement mode as in line number 4, and line number 9 is MOVE to position at teaching point P4 by linear interpolation in the same way as line number 5
It is a command, and line number 10 is a command for opening the hand and leaving the article. The line number 11 is a MOVE command word for positioning to the teaching point P3 by linear interpolation similarly to the line number 7, and the line number 12 is a command word for positioning the hand tip to the machine origin, similarly to the line number 3.

As shown in FIG. 7, the operation program of FIG. 6 causes the robot to move from the origin to P1 via point P2.
Stop at the point, then close the hand 19 to grasp the object, move to the point P4 via the points P2 and P3, open the hand 19, leave the object, and leave the origin via the point P3. It is possible to move to. In FIG. 7, P1 to P4
The points are teaching points.

FIG. 3 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. Also, in step 104, MOVE
When the SP command is decoded, in step 106,
The processing in the possible maximum speed movement mode described later is executed.

When the HAND OFF command is decoded in step 108, a command for opening the hand 19 is given to the tool driving circuit 23 in step 110. When the HAND ON command is decoded in step 112, the tool drive circuit 23 is instructed to close the hand 19 in step 114.

In step 116, ACCEL or SPEED
If it is determined that the command word is the command word, the acceleration command value c and the target speed command value b are stored in the RAM in step 118.
25 SDA areas.

As is well known, the interpolation calculation between the teaching points in steps 102 and 106 can be performed by using the rotating spindle method or the like (for example, Japanese Patent Laid-Open No. 62-154).
No. 006).

Next, the processing procedure of the move command (MOVESP) in the maximum possible speed move mode will be described with reference to FIG. When the MOVESP command is given, the program of FIG. 4 starts the process from step 200, and thereafter repeatedly executes from step 208 every interpolation cycle ΔT until the movement to the teaching point designated by the MOVESP command is completed. To be done. When the movement to the teaching point designated by the MOVESP command is completed, the process returns to the main program shown in FIG.

Next, in step 200, the direction vector of the rotary spindle is calculated from the position and orientation of the starting point and the position and orientation of the next positioning target point given as teaching data expressed in the world coordinate system, and in step 200 202
At, the rotation angle Θ around the rotation main axis is calculated.
Next, in step 204, the moving speed Vm at that time
Is 0, that is, it is determined whether or not it is in a stopped state. If it is in the stopped state, in step 206, the moving speed Vm on the operation locus is the acceleration A commanded at that time by the following equation.
It is increased by a unit amount using m.

[0020]

[Formula 1] Vm = Vm + Am.ΔT If the movement command words are continuously given, continuous movement is performed without decelerating or positioning the teaching point designated by the previous movement command word. It is necessary to process as. Therefore, in this case, the acceleration process of step 206 is not performed.

Next, at step 208, the interpolation distance ΔL from the section start teaching point on the motion locus is updated by the following equation.

## EQU2 ## ΔL = ΔL + VmΔT VmΔT represents the movement distance on the operation locus in one interpolation cycle.

Next, in step 210, the MOVE is executed.
The remaining distance R on the motion locus with respect to the target position given by the SP command is calculated by the following equation.

## EQU00003 ## R = L-.DELTA.L However, L is the section distance on the motion locus to be moved by the movement command. Next, in step 212, it is judged whether or not the interpolation distance ΔL is larger than the section distance L, and if it is larger, the interpolation point exceeds the target position, so in step 214, the interpolation distance ΔL is set to the section distance L. Equal to.

Next, at step 216, the interpolation angle ΔΘ around the rotation main axis is calculated by the following equation.

## EQU00004 ## .DELTA..THETA. =. THETA..DELTA.L / L That is, the ratio of the length of the interpolation point on the operation locus is made equal to the ratio of the angle around the rotation main axis.

Next, in step 218, the posture conversion matrix R (4 × 4 of 4 × 4) for converting the position and posture of the start point into the position and posture at the interpolation point using the interpolation angle ΔΘ and the interpolation ratio ΔL / L. A homogeneous coordinate matrix) is calculated. Then, in step 220, the attitude transformation matrix is applied to the homogeneous coordinate matrix representing the position and orientation of the start point to calculate the homogeneous coordinate matrix representing the position and orientation at the interpolation point. Next, in step 222, the value in the joint coordinate system, that is, the rotation angle (θ _{1 to} θ ₆ ) of each axis is calculated from the homogeneous coordinate matrix of the position and orientation expressed in the world coordinate system at the interpolation point. This value is the IN of RAM25
It is stored in the area A.

Next, at step 224, the rotation angle θ _{i of} each axis is output to the servo CPUs 22a-22f.
As a result, each axis of the robot rotates to the target rotation angle during the interpolation cycle ΔT, and the tip of the hand moves to the interpolation point on the movement locus at the moving speed Vm.

MOVES executing this program
If the instruction word next to the P instruction word is a move type instruction word,
Positioning to the target position is not performed. Therefore, in step 226, it is determined whether or not the route is a continuous route, and if it is not a continuous route, the interpolation distance ΔL is the section distance L in step 228.
Is determined. If they are not equal, in step 230, the remaining distance R and the required stop distance (Vm ² /
2 Am) size is compared. The required stop distance is the distance required to decelerate at a deceleration commanded from the speed Vm at that time and stop at the target position. When the remaining distance R is equal to or greater than the stop distance, it is not necessary to decelerate yet, so the speed control in the maximum possible speed movement mode from step 232 is executed. Even if it is determined in step 226 that the route is the continuous route, it is not necessary to position the target position, so that the control in the maximum possible speed movement mode of step 232 and subsequent steps is continuously executed.

If the remaining distance R is smaller than the required stop distance in step 230, the moving speed Vm is subtracted by the following equation in step 246 in order to perform deceleration processing for positioning with respect to the target position. To be done.

## EQU5 ## Vm = Vm-Am..DELTA.T

On the other hand, if the interpolation distance ΔL is equal to the section distance L in step 228, it means that the motion locus is not a continuous path and the interpolation point has reached the target teaching point. Therefore, in step 248, Execution of this MOVESP command is completed, the moving speed Vm is set to 0, and the process returns to the main program of FIG.

In the control step 232 according to the maximum possible speed movement mode , the rotation speed ω _{i of} each axis and the rotation speed ratio β _i are calculated by the following equation.

[Equation 6] ω _i = ((θ _i (k) −θ _i (k-1)) / ΔT

Β _i = ω _i / ω _imax where ω _imax is the maximum rotation speed that can be output on the i-th axis.
Then, the maximum value of the rotation speed ratio β _i of the six axes is set to β m.

Next, at step 234, it is judged whether or not the maximum rotation speed ratio βm is not less than the upper limit value TH. If it is above, at step 236, the moving speed Vm is decelerated and corrected by the following equation.

## EQU00008 ## Vm = Vm-Am..DELTA.T If the determination result in step 234 is No, it is determined in step 238 whether the maximum rotation speed ratio .beta.m is less than or equal to the lower limit value TL. In step 240, the moving speed Vm is increased and corrected by the following equation.

[Equation 9] Vm = Vm + Am.ΔT

Next, at step 242, it is judged if the interpolation distance ΔL is equal to the section distance L. In the case of Yes, it means that the interpolation point has reached the target teaching point in the continuous route. Therefore, in this case, the velocity control by this MOVESP command is ended while the movement velocity Vm corrected in steps 236 and 240 is held, and the process returns to the main program shown in FIG. If the next command is MOVESP, the maximum possible speed control by the moving speed Vm is executed.

In step 242, the judgment result is
If No, in step 244, the interpolation cycle ΔT,
That is, it waits until the next control time. Then, at the next control time, the processing is repeated from step 208. Thus, step 23 of the previous control cycle
The interpolation point in the next control cycle is determined based on the moving speed Vm corrected in 6 and 240, and the command rotation angle of each axis is determined.

As a result of such processing, as shown in FIG. 8A, the maximum rotation speed ratio βm has a lower limit value TH and an upper limit value T.
Moving speed Vm on the motion locus so as to take a value between L and
Is controlled. However, the axis that takes the maximum rotation speed ratio may differ for each interpolation point. As a result, when acceleration is completed,
At least one of the six axes will be rotating at the maximum permissible speed. Therefore, even if the maximum speed (100%) is commanded, the movement speed on the motion locus is as shown in FIG.
As shown in (b) of (1), depending on the posture, the speed can exceed the maximum speed. In this way, it is possible to perform high-speed movement control along a predetermined motion trajectory that maximizes the capabilities of the robot.

In the above embodiment, the case of linear interpolation has been described, but circular interpolation can be carried out by the same procedure. That is, in the arc, the central angle Γ,
When the radius is r, and when, in the above equation,

## EQU10 ## L = .GAMMA..DELTA.L = .DELTA..GAMMA. Vm = r.multidot..OMEGA.m Am = r.PI. Here, Ωm is the angular velocity of the central angle of circular interpolation, and Π is the angular acceleration of the central angle. In the embodiment described above, the measured rotation angle θ _i of each axis is detected, the rotation speed ratio β _i is calculated, and the rotation speed ratio β _i is set to the upper limit value T.
The moving speed Vm is changed in comparison with H and the lower limit value TL. That is, the rotation speed ratio is actually the upper limit value,
Although the speed is changed when the lower limit value is exceeded, the speed may be changed by predicting the next moving speed from the target rotation angle.

[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 flowchart showing a main processing procedure of instruction word decoding by the CPU.

FIG. 4 is a flowchart showing a processing procedure for determining an interpolation angle by a CPU.

FIG. 5 is a flowchart showing a speed control processing procedure in a maximum possible speed movement mode by a CPU.

FIG. 6 is an explanatory diagram showing an operation program.

FIG. 7 is an explanatory diagram showing an operation path.

FIG. 8 is an explanatory diagram showing changes in the maximum rotation speed ratio and the moving speed.

[Explanation of symbols]

10 ... Robot 18 ... Swivel list 18a ... Flange 19 ... Hand 20 ... CPU (rotational speed ratio calculation means, axis determination means, moving speed control means, angle control means) 22a-22f ... Servo CPU (angle control means) 25 ... RAM (Maximum possible speed movement command means, setting means) Step 104 ... Maximum speed movement command means Step 232 ... Rotation speed ratio calculation means, axis determination means Steps 234-240 ... Movement speed control means Steps 200-224 ... Angular speed control means

─────────────────────────────────────────────────── ─── Continuation of the front page (51) Int.Cl. ⁶ Identification code Internal reference number FI technical display location G05D 13/62 H 9132-3H

Claims (1)

[Claims] 1. A controller for a robot that positions a robot hand at a teaching point given according to a motion trajectory such as a straight line or an arc, and a mode for moving the hand along the motion trajectory at a maximum possible speed. Possible maximum speed movement command means, and at the interpolation point on the movement locus, the rotation speed of the rotation speed of each axis for moving the hand at the movement speed commanded on the movement locus with respect to the maximum rotation speed of each axis Rotating speed ratio calculating means for calculating a ratio, setting means for setting an upper limit value and a lower limit value of the rotating speed ratio, axis determining means for determining a maximum rotating speed axis at which the rotating speed ratio is maximum, At the interpolation point, when the rotation speed ratio of the maximum rotation speed axis becomes larger than the upper limit value, the moving speed at the next interpolation point is decreased by a predetermined value to obtain the maximum rotation speed. When the rotational speed ratio is smaller than the lower limit value,
Based on the moving speed control means for increasing the moving speed by a predetermined value, and the moving speed determined by the moving speed control means, the next interpolation point is determined, and based on the determined rotation angle of each axis. A controller for a robot, comprising: angle control means for controlling each axis.