JP7424097B2 - Robot control device and robot control method - Google Patents

Description

The present invention relates to an apparatus and method for controlling a robot so that its movement trajectory passes over a teaching point when actually operating a robot.

CP (continuous There is a path) method. Furthermore, the robot's motions are divided into end motions and pass motions, depending on the method of determining the completion of the previous motion when moving from one motion to the next.

For example, as shown in FIG. 9, if there are three teaching points P1 to P3, in the end motion of the CP method, the tip of the robot reaches the teaching point before moving on to the next motion. Therefore, the tip of the robot is moved so that the trajectory between P1→P2 and between P2→P3 is a straight line. Accordingly, the patterns in which the operating speed of the robot changes between P1 and P2 and between P2 and P3 each become trapezoidal, which is a combination of an acceleration period, a constant speed period, and a deceleration period.

On the other hand, in the pass motion of the CP method, the robot moves to the next motion before the tip of the robot reaches the teaching point. Therefore, an acceleration period between P2 and P3 is started in conjunction with a deceleration period between P1 and P2. As a result, the tip of the robot does not pass over the teaching point P2, but passes inside it, that is, on the right side in the figure, and the combined trajectory of these points is shown by the broken line in the figure.

Patent No. 3666341

Here, assume that the robot is used for product assembly work. When the movement from teaching point P2 to teaching point P3 is the final approach movement in assembly work, depending on the shape of the workpiece and the surrounding environment, the tip of the robot may be compared with obstacles placed around it. may have to move through confined spaces. Then, if the movement between P2 → P3 is a straight line, the robot tip can move without any problem, but if the trajectory passes inside the teaching point P2 due to the path movement of the CP method, the robot will be located inside the teaching point P2. There is a risk of interference with obstacles.

The present invention has been made in view of the above-mentioned circumstances, and its purpose is to prevent obstacles in the surroundings when the tip of the robot moves on a trajectory that does not pass through all the teaching points by adopting the CP method path motion. An object of the present invention is to provide a robot control device and a robot control method that can prevent interference with objects.

According to the robot control device according to claim 1, the control unit performs a path operation in which the robot tip continuously moves from a first teaching point where movement starts to a third teaching point where movement ends. The robot is controlled using the teaching data as follows. The teaching point setting unit is configured to set the teaching point from the third teaching point so that the continuous movement locus of the tip of the robot that performs the pass operation overlaps with the second teaching point set between the first teaching point and the third teaching point. A fourth teaching point is set on a straight line extending past the second teaching point. Then, using the teaching data of the fourth teaching point, the control unit is controlled to perform a pass operation from the first teaching point toward the fourth teaching point.

When the tip of the robot is moved by the CP method pass operation for the first to third teaching points set at the beginning, as described above, the deceleration period from the first teaching point to the second teaching point and the second teaching point are changed. As a result of the overlap with the acceleration period set from the point to the third teaching point, the continuous movement locus does not pass through the second teaching point. Then, on the straight line P2-P3 connecting the second teaching point and the third teaching point, the distance that the robot tip moves linearly is shorter than the length of the straight line P2-P3.

Therefore, the teaching point setting section sets the fourth teaching point on a straight line extending from the third teaching point passing through the second teaching point, and uses the teaching data of the fourth teaching point to instruct the control section to If the robot is controlled to perform a path motion from the 1st teaching point to the 4th teaching point, the robot tip will reach the 2nd teaching point, and move in a straight line from the 2nd teaching point to the 3rd teaching point. move in a specific direction. As a result, even when there are obstacles etc. around the path from the second teaching point to the third teaching point and the robot tip moves in a relatively narrow space, the distance between the second and third teaching points is By moving in a straight line, interference with obstacles can be avoided.

Further, according to the robot control device according to claim 1 , the teaching point setting section is configured to set the continuous movement locus of the pass motion to a straight line from the second teaching point to the third teaching point, starting from the second teaching point. After calculating the distance Lpa to the merging point, the fourth teaching point is set on a straight line extending from the second teaching point by the distance Lpa. As mentioned above, when the robot tip moves about the first to third teaching points set at the beginning, the distance the robot tip moves linearly on the straight line P2-P3 is longer than the length of the straight line P2-P3. Since the distance becomes shorter, the distance corresponding to the shorter distance is determined as Lpa. Then, if the fourth teaching point is set at a position equal to the length of the straight line P2-P3 plus the length of the distance Lpa and the robot is made to perform a pass motion, the continuous movement trajectory of the robot tip will pass through the second teaching point. I come to do it.

Furthermore, according to the robot control device according to claim 1 , the teaching point setting section calculates the result of recalculating the continuous movement locus when the control section performs the path motion from the first teaching point to the fourth teaching point. The distance Lpa is calculated again for . Then, the difference between the recalculated distance Lpa and the previously calculated distance Lpa is calculated, and if the calculated difference is greater than or equal to the threshold value, the difference is added to the recalculated distance Lpa to update the distance Lpa. Then, the fourth teaching point is set again based on the updated distance Lpa, and the control section is caused to re-execute the control of the pass operation.

With this configuration, since there is an error factor, if the difference between the initially determined distance Lpa and the distance between the second and fourth teaching points is greater than or equal to the threshold, the distance Lpa is updated according to the difference, The error included in the distance Lpa can be reduced by repeatedly re-executing the control of the pass operation. This makes it possible to set the fourth teaching point with high accuracy.

A diagram showing the configuration of a robot system, which is one embodiment Functional block diagram showing the configuration of the controller Flowchart showing the gist of the processing content of the controller A diagram showing the movement locus of the robot tip when performing a pass operation for the initial teaching points P1 to P3. A diagram showing the movement trajectory of the robot tip when performing a pass operation by replacing teaching point P2 with a new teaching point P2_n. Diagram explaining the processing concept in this embodiment using speed patterns (Part 1) Diagram explaining the same processing concept using speed patterns (Part 2) Diagram explaining the same processing concept using speed patterns (Part 3) Diagram explaining end operation and path operation of CP method

An embodiment will be described below. As shown in FIG. 1, the robot system 1 includes an articulated robot 2, a controller 3 for controlling the robot 2, and a teaching pendant 4 connected to the controller 3. This robot system 1 is used for general industrial purposes.

The robot 2 has a well-known configuration as a so-called 6-axis vertically articulated robot, and a shoulder 6 is mounted on a base 5 and rotates in the horizontal direction via a first axis (J1) having an axis in the Z direction. possible to be connected. A lower end portion of a lower arm 7 extending upward is connected to the shoulder 6 so as to be rotatable in the vertical direction via a second shaft (J2) having an axis in the Y direction. A first upper arm 8 is connected to the tip of the lower arm 7 so as to be rotatable in the vertical direction via a third shaft (J3) having an axis in the Y direction. A second upper arm 9 is twistably connected to the tip of the first upper arm 8 via a fourth shaft (J4) having an axis in the X direction. A wrist 10 is vertically rotatably connected to the tip of the second upper arm 9 via a fifth shaft (J5) having an axis in the Y direction. A flange 11 is twistably and rotatably connected to the wrist 10 via a sixth shaft (J6) having an axis in the X direction.

The base 5, shoulder 6, lower arm 7, first upper arm 8, second upper arm 9, wrist 10, and flange 11 function as the arm of the robot 2, and the flange 11 at the tip of the arm is not shown. However, a hand, also called an end effector, is attached. For example, the hand holds and transports a workpiece (not shown), and a tool for processing the workpiece is attached to the hand. Each of the axes J1 to J6 provided on the robot 2 is provided with a motor 12 shown in FIG. 2, which serves as a driving source.

The controller 3 corresponds to a control device for the robot, and controls the robot 2 by executing a computer program in a control unit including a computer including a CPU, ROM, RAM, etc. (not shown). Specifically, the controller 3 includes a drive section configured with an inverter circuit, etc., and uses feedback control, for example, based on the rotational position of the motor detected by an encoder 17 provided corresponding to each motor 12. Drive the motor 12.

The teaching pendant 4 is formed, for example, in the shape of a generally rectangular box, and is formed in a size that allows the user to operate it while holding it. The teaching pendant 4 is provided with various key switches, touch panels, etc., and the user performs teaching using these key switches, touch panels, etc.

As shown in FIG. 2, the controller 3 includes a control section 13 mainly including a CPU, a storage section 14, and a position detection section 15. The control unit 13 performs overall control of the entire industrial robot device 1 . The control unit 13 is connected to a storage unit 14 and a position detection unit 15 as well as a drive circuit 16 for the teaching pendant 4 and the motor 12. The storage unit 14 stores in advance parameters such as maximum acceleration (+α), maximum deceleration (-α), and maximum speed Vmax for the motor 12 that drives each arm 6 to 11, as well as various software for robot control. ing. Furthermore, the storage unit 14 stores therein an operation program set by the teaching pendant 4 and movement trajectory information of the tip of the robot generated prior to the actual operation when the robot performs actual operations. The control section 13 corresponds to a teaching point setting section.

A rotary encoder 17 connected to the rotating shaft of each motor 12 corresponding to the shoulder 6, arms 7 to 9, etc. is connected to the position detection unit 15. The position detection section 15 detects the rotation angle of the motor 12, that is, the axis value, based on the rotation detection signal input from the rotary encoder 17, and provides the rotation position information to the control section 13. After calculating the rotation angle of the corresponding arm or the like from the rotation angle information of the motor 12, the control unit 13 compares the rotation angle with a target angle and provides the drive circuit 16 with a current command value according to the difference. Then, the drive circuit 16 supplies a current to the motor 12 according to the given current command value. As a result, the rotation of the motor 12 is controlled, and each of the arms 6 to 11 is rotated to a target angle.

Next, the operation of this embodiment will be explained. FIG. 3 is a flowchart showing the gist of the process by which the control unit 13 of the controller 3 calculates the continuous movement locus of the tip of the robot 2. The control unit 13 first determines whether or not it is an approach motion after a pass motion (S1). As shown in FIG. 4, the "approach motion" is the section from teaching point P2 to P3 among the three teaching points P1 to P3 at the end of the continuous movement trajectory, and the teaching point P3 is the section where the robot 2 This is the final position that the tip reaches when performing assembly work, etc. In addition, as shown by the broken line in the same figure, the "pass motion" means that the tip of the robot deviates from the section in which it moves linearly and passes inside the teach point P2 while going from the teaching point P1 to P2, and the approach motion section This is the section until it connects to the straight line trajectory.

If it is an approach motion after a pass motion (S1; YES), a distance Lpa over which the pass motion is performed within the approach motion section is calculated (S2). If the speed of the robot tip at the start of the path motion section is Vm, the deceleration is Dm, and the deceleration time is Tp, the deceleration time Tp is:
Tp=Vm/Dm...(1)
is required. If the acceleration of the robot tip in the approach motion is Aa, the path motion distance Lpa is:
Lpa=Aa×Tp2/ ² ...(2)
is required.

Next, when the path operation distance Lpa_n is initialized by substituting the distance Lpa obtained in step S2 (S3), the starting point Xa of the approach operation, that is, the coordinates of the teaching point P2, is moved by the distance Lpa_n in the approach vector direction. The calculated coordinates are calculated as a new starting point Xa_new (S4). In FIG. 5, the starting point Xa_new is "P2_new". If the approach motion vector is (ax, ay, az) and the starting point Xa = (xa, ya, za), then the new starting point Xa_new is
Xa_new=Xa-Lpa×(ax, ay, az)...(3)
is required. The teaching points P1 to P3 correspond to the first to third teaching points, and the above-mentioned P2_new corresponds to the fourth teaching point.

Then, a pass motion distance Lpa_n is calculated for a pass motion caused by replacing the teaching point P2 with the starting points Xa_new and P2_new (S5). Then, the difference between the absolute value of the distance Lpa found in step S2 and the absolute value of the distance Lpa_n found in step S4 is found, and it is determined whether the absolute value of the difference is less than the threshold delta (S6). The threshold value delta is a value near zero. If the absolute value of the difference is greater than or equal to the threshold delta (NO), the path operation distance Lpa_n is updated by adding the absolute value of the difference to the distance Lpa_n (S8), and then the process returns to step S4.

On the other hand, in step S6, if the absolute value of the difference is less than the threshold delta (YES), the starting point Xa_new at that time is set as the starting point of the approach motion, and the robot 2 moves (S7).

The concept of the above processing will be explained using speed pattern graphs shown in FIGS. 6 to 8. FIG. 6 corresponds to the operation shown in FIG. 4, and is the result of starting an acceleration period when the robot tip moves from teaching point P2 to P3 in conjunction with the start of a deceleration period when the robot tip moves from teaching point P1 to P2. , the composite speed during that period is shown by the broken line. Then, of the distance that the robot tip moves during this period, the movement trajectory turns inward without reaching the teaching point P2 by the distance moved by acceleration, that is, the area of the triangle shown by hatching shown in FIG. 7.

Therefore, as shown in FIG. 8, a new target point P2_new is set so that the pass operation is completed at the teaching point P2, that is, so that both the deceleration period and the acceleration period end. This means setting a new target point P2_new so as to extend the moving distance by an amount equal to the area of the triangle representing the moving distance due to the acceleration described above. As a result, the robot tip moves in a straight line after reaching the original teaching point P2 toward the end point P3.

As described above, according to the present embodiment, the control unit 13 of the controller 3 performs a path operation in which the robot tip continuously moves from the teaching point P1 where the movement starts to the teaching point P3 where the movement ends. The robot 2 is controlled using the teaching data as follows. At that time, the continuous movement locus of the tip of the robot performing the pass motion is on a straight line extending from teaching point P3 passing through teaching point P2 so that it overlaps teaching point P2 set between teaching points P1 and P3. A new teaching point P2_n is set at . Then, using the teaching data of the teaching point P2_n, control is performed to perform a pass operation toward the teaching point P2_n.
As a result, as shown in FIG. 5, even if there are obstacles etc. around the path from teaching point P2 to teaching point P3, and the robot tip moves in a relatively narrow space, teaching point P2, Since the movement between P3 is reliably linear, interference can be avoided.

In this case, when the control unit 13 calculates the distance Lpa from the teaching point P2 to the point where the continuous movement locus of the pass operation merges with the straight line from the teaching point P2 to the teaching point P3, the control unit 13 calculates the teaching point P2_n as follows. It is set on a straight line extending a distance Lpa from the teaching point P2. As described above, when moving the robot tip about the initially set teaching points P1 to P3, the distance that the robot tip moves linearly on the straight line P2-P3 is shortened by Lpa. Therefore, if the teaching point P2_n is set at a position equal to the length of the straight line P2-P3 plus the length of the distance Lpa, and the robot 2 is made to perform a path motion, the continuous movement locus of the robot tip will move back to the original teaching point P2. It starts to pass.

Further, the control unit 13 recalculates the distance Lpa based on the result of recalculating the continuous movement locus when performing the path motion from the teaching point P1 to the teaching point P2_n, and sets it as the distance Lpa_n. Then, the difference between the recalculated distance Lpa_n and the previously calculated distance Lpa is determined, and if the determined difference is greater than or equal to the threshold delta, the distance Lpa_n is updated by adding the difference to the recalculated distance Lpa_n. Then, the teaching point P2_n is set again based on the updated distance Lpa_n, and the control of the pass operation is re-executed. With this configuration, it becomes possible to reduce the error included in the distance Lpa_n and set the teaching point P2_n with high accuracy.

In the drawings, 1 is a robot system, 2 is a robot, 3 is a controller, 4 is a teaching pendant, 13 is a control section, and 14 is a storage section.

Claims (2)

In a robot control device that is equipped with a plurality of rotating arms and each arm is driven by its own drive motor,
a storage unit that stores, as teaching data, the positions and postures of three or more teaching points taught for passing the robot tip;
Controlling the robot using the teaching data so that the robot tip performs a path motion in which the tip of the robot continuously moves from a first teaching point where movement starts to a third teaching point where movement ends among the teaching points. a control unit to
from the third teaching point to the third teaching point so that the continuous movement locus of the tip of the robot that performs the pass operation overlaps with the second teaching point set between the first teaching point and the third teaching point. A fourth teaching point is set on a straight line extending past the second teaching point, and using the teaching data of the fourth teaching point, the controller is instructed to move from the first teaching point to the fourth teaching point. a teaching point setting unit that controls the pass operation to be performed using the teaching point setting unit ;
The teaching point setting unit calculates a distance Lpa from the second teaching point to a point where the continuous movement locus of the pass motion joins a straight line from the second teaching point to the third teaching point. ,
setting the fourth teaching point on a straight line extending from the second teaching point by the distance Lpa;
The control unit recalculates the distance Lpa based on the result of recalculating a continuous movement trajectory when performing a path motion from the first teaching point to the fourth teaching point,
Find the difference between the recalculated distance Lpa and the previously calculated distance Lpa,
If the difference is greater than or equal to the threshold, the difference is added to the recalculated distance Lpa to update the distance Lpa, the fourth teaching point is set again based on the updated distance Lpa, and the control unit is instructed to perform the pass operation. A robot control device that re-executes control . When operating a robot that is equipped with a plurality of rotatable arms, each arm being driven by its own drive motor, and controlling the robot using teaching data about three or more teaching points taught to pass the tip of the robot. To,
Among the teaching points, a continuous movement locus of the robot tip performs a path motion in which the robot continuously moves from a first teaching point where movement starts to a third teaching point where movement ends. setting a fourth teaching point on a straight line extending from the third teaching point passing through the second teaching point so as to overlap the second teaching point set between the third teaching point;
performing the pass operation for movement from the first teaching point to the fourth teaching point using the teaching data of the fourth teaching point;
Calculating a distance Lpa from the second teaching point as a starting point to a point where the trajectory of the pass motion joins a straight line from the second teaching point to the third teaching point;
setting the fourth teaching point on a straight line extending from the second teaching point by the distance Lpa;
recalculating the distance Lpa with respect to the result of recalculating the continuous movement locus in order to perform a path motion from the first teaching point to the fourth teaching point;
Find the difference between the recalculated distance Lpa and the previously calculated distance Lpa,
If the difference is greater than or equal to the threshold, the difference is added to the recalculated distance Lpa to update the distance Lpa, the fourth teaching point is set again based on the updated distance Lpa, and the pass operation is re-executed. How to control the robot.

Images