JP2024014341A - Collison detection method of robot and collision detection device of robot - Google Patents

Abstract

PROBLEM TO BE SOLVED: To quickly and surely detect occurrence of collision of a robot, in small calculation loads, even when the robot moves at a low speed.

SOLUTION: A collision detection device, which detects occurrence of collision of a robot 10 whose shafts are driven by a motor 11, comprises: a threshold generation part 53 that generates a threshold (a collision detection limit value); a comparison part 54 that compares the threshold with a torque value of the motor and determines that collision occurs when the torque value surpasses the threshold; and a storage part 55 that stores a collision detection limit table preliminarily storing a threshold for each whole speed, for each stage and for each shaft. When the robot 10 is moved by CP control between an entering position of the stage and a stage position for each stage, the threshold generation part 53 supplies a threshold obtained with reference to the collision detection limit table to the comparison part 54. The whole speed is a speed as a whole of the robot 10 that is operated by CP control.

SELECTED DRAWING: Figure 1

COPYRIGHT: (C)2024,JPO&INPIT

Description

The present invention relates to a method and apparatus for detecting the occurrence of a collision in a robot.

Robots used for transporting workpieces may have their arms interfere with surrounding objects and collide with them during operation. When a collision occurs during operation, it is necessary to immediately detect it and take steps such as stopping the robot's operation immediately. To detect a collision in a robot, a collision sensor attached to the arm or the hand at the tip of the arm can be used. However, providing a collision sensor results in an increase in cost. Furthermore, the mass of the collision sensor may adversely affect the dynamic characteristics of the robot, and depending on the structure of the robot, it may be difficult to provide wiring to the collision sensor. As a method for detecting a collision without using a collision sensor, there is a method of monitoring the torque in the servo motor of each axis of the robot. When a collision occurs and the robot's movement is obstructed, the torque of the servo motor of the corresponding axis increases rapidly, so if a rise in torque is detected by comparing it with a threshold, it can be determined that a collision has occurred. can. In this method, even when the robot is operating normally in response to an external command, the torque may increase significantly depending on the contents of the command, so the threshold value for determining that a collision has occurred is Collision detection limit values need to be set appropriately. If a collision detection limit value is set smaller than the torque that can occur during normal operation, the occurrence of a collision will be erroneously detected during normal operation. Generally, the torque increases as the motor speed increases, so in order to prevent false detection, it is necessary to set the collision detection limit value to a value larger than the torque when the motor is driven at maximum speed.

When the collision detection limit value is set to a value larger than the torque required for driving at the maximum speed, a problem arises in that collisions cannot be accurately detected when the motor speed is small. If the speed is low, the motor torque is small, and the increase in torque when there is a collision or interference is also small. Therefore, when a collision detection limit value corresponding to the maximum speed is used, there is a possibility that a small collision or interference cannot be detected. Alternatively, the collision or interference may be detected only after the collision or interference has progressed to a certain extent, that is, a time lag may occur in the detection.

Patent Document 1 discloses a technology that can quickly and reliably detect the occurrence of a collision in a robot, using a required drive torque command element calculated from at least one of a position command, a speed command, and an acceleration command, and the position and speed of the motor that drives each axis. and acceleration, the required driving torque is calculated without recalculating the equation of motion of the robot, and the occurrence of a collision is determined by comparing the calculated required driving torque and the current of the motor of each axis. The company discloses that it will do the following. Patent Document 2 discloses a state in which the load of the workpiece is not transmitted to the robot and a state in which the load of the workpiece is It discloses that a collision is detected by recognizing the state transition between the states transmitted to and from the state, using a different threshold depending on which state it is in, and comparing the estimated disturbance value with this threshold. . Patent Document 3 discloses that when detecting the occurrence of a collision by estimating a disturbance by performing inverse dynamics calculation, in order to prevent the accuracy of collision detection from decreasing due to vibration caused by the spring component of the reducer, This disclosure discloses that when the command acceleration for robot operation is larger than a predetermined value, the threshold value for collision detection is increased to lower the detection sensitivity.

Although not related to improving the accuracy of collision detection, Patent Document 4 describes a space in which a robot arm moves in a space that is close to the worker in order for the human worker and the robot to work together. It is disclosed that the robot arm is divided into a low-speed motion region and a high-speed motion region that is the other space, and that the detection sensitivity for detecting a collision of a robot arm is made higher in the low-speed motion region than in the high-speed motion region.

Japanese Patent Application Publication No. 2003-25272 Unexamined Japanese Patent Publication No. 2016-16490 Japanese Patent Application Publication No. 2006-116650 International Publication No. 2016/103308

The technology described in Patent Document 1 that improves the accuracy of collision detection in robots requires calculation of the required drive torque, and the technology described in Patent Document 2 requires calculation of an estimated value of disturbance, and the patent The method described in Document 3 requires inverse dynamic calculation to calculate the external force. Therefore, the techniques described in Patent Documents 1 to 3 have a problem in that the calculation load required for collision detection is heavy.

SUMMARY OF THE INVENTION An object of the present invention is to provide a collision detection method and a collision detection device that have a small calculation load and can quickly and reliably detect the occurrence of a collision even when a robot is moving at low speed.

In one aspect of the present invention, a collision detection method for detecting the occurrence of a collision in a robot equipped with a plurality of axes, each of which is driven by a motor, uses a threshold value determined for each axis according to the speed of the motor, and The collision determination step includes comparing the torque value of the motor with the torque value of the motor and determining that a collision has occurred when the torque value exceeds a threshold value. When the robot is a robot that transports a workpiece between a plurality of stages and moves between the entry position of each stage and the stage position by continuous path (CP) control, in the collision determination process, A threshold value obtained by referring to a collision detection limit table in which threshold values for each overall speed are stored in advance for each stage and each axis is used, and the overall speed is the overall speed of the robot operating under CP control.

According to the collision detection method of the above aspect, when performing CP control in which changes in motor speed for each axis cannot be known unless the robot is actually operated, the threshold value for each overall speed is set in advance for each stage and each axis. A stored collision detection limit table is prepared, and when the robot is actually moved using CP control, the collision detection limit table is referenced according to the commanded overall speed to obtain the collision detection limit value, which is a threshold value, and CP control is performed. This enables more optimal collision detection when the robot accesses the stage.

In one aspect of the collision detection method, in a collision detection limit table, the overall speed is divided into a plurality of CP speed regions ranging from zero speed to the maximum speed specified by CP control, and a threshold value is set for each CP speed region. Preferably, it is defined as a single value. By setting the threshold value as a single value for each CP speed region, the size of the collision detection limit table can be reduced and the creation time can be shortened.

In one aspect of the collision detection method, the threshold value may be determined based on the maximum value in the motor torque waveform and stored in the collision detection limit table when the robot's CP control teaching is completed. preferable. By creating a collision detection limit table upon completion of teaching, collision detection can be reliably performed when the robot is actually operated.

In one aspect of the collision detection method, the robot moves between a standby position and an approach position under point-to-point (PTP) control. When moving under PTP control, the speed of that axis is determined based on the speed determined for each axis according to the speed ratio for each of multiple axes, such that the lower the motor speed is, the lower the speed is, and the higher the motor speed is, the higher the speed is. It is preferable that a threshold value for . With this configuration, when the robot is moved under PTP control, it is possible to detect the occurrence of a collision using a more appropriate threshold. In this case, the range from speed zero to the maximum speed of the motor is divided into multiple PTP control speed regions, and the threshold value used during PTP control is determined as a single value for each PTP control speed region. It is preferable to Thereby, the calculation load for threshold generation can be further reduced.

In another aspect of the present invention, a collision detection device for detecting the occurrence of a collision in a robot having a plurality of axes, each of which is driven by a motor, generates a threshold value determined according to the speed of the motor for each axis. a threshold generation section; a comparison section that compares the threshold supplied from the threshold generation section with the torque value of the motor and determines that a collision has occurred when the torque value exceeds the supplied threshold; and a storage unit that stores a collision detection limit table in which threshold values for each overall speed are stored in advance for each axis. In this collision detection device, when the robot is a robot that transports a workpiece between a plurality of stages and moves between the entry position of each stage and the stage position by CP control, the threshold value generation unit supplies a threshold value obtained by referring to the collision detection limit table to the comparator, and the overall speed is the overall speed of the robot operating under CP control.

In the collision detection device of the above aspect, when performing CP control in which changes in motor speed for each axis cannot be known unless the robot is actually operated, a threshold value for each overall speed is stored in advance for each stage and each axis. Prepare a collision detection limit table and store it in the storage unit, and when actually moving the robot under CP control, refer to the collision detection limit table according to the commanded overall speed to obtain the collision detection limit value, which is a threshold value. This makes it possible to perform more optimal collision detection when the robot accesses the stage using CP control.

In one embodiment of the collision detection device, in the collision detection limit table, the overall speed is divided into a plurality of CP control speed regions in the range from zero speed to the maximum speed specified by CP control, and each CP control speed region is divided into a plurality of speed regions. Preferably, the threshold value is determined as a single value. By setting the threshold value as a single value for each speed region for CP control, it is possible to reduce the size of the collision detection limit table and shorten the creation time. Further, a table generation unit may be provided that determines a threshold value based on the maximum value in the torque waveform of the motor and stores it in the collision detection limit table. By providing such a table creation section, it becomes possible to easily create a collision detection limit table.

In one embodiment of the collision detection device, the robot moves between a standby position and an approach position under PTP control, and the threshold value generated by the threshold generation unit while moving under PTP control increases as the speed of the motor decreases. The speed may be determined based on a speed determined for each axis according to a speed ratio to each of a plurality of axes, such that the speed is small and increases as the speed of the motor increases. With this configuration, when the robot is moved under PTP control, it is possible to detect the occurrence of a collision using a more appropriate threshold. In this case, the range from speed zero to the maximum speed of the motor is divided into multiple PTP control speed regions, and the threshold value used during PTP control is determined as a single value for each PTP control speed region. It is preferable to Thereby, the calculation load for threshold value generation can be further reduced.

According to the present invention, the occurrence of a collision can be quickly and reliably detected with a small calculation load even when the robot is moving at low speed.

FIG. 1 is a block diagram showing an example of a robot system to which a collision detection method according to an embodiment of the present invention is applied. FIG. 3 is a diagram showing the principle of collision detection based on torque values. FIG. 3 is a diagram illustrating the principle of collision detection based on the present invention. FIG. 2 is a schematic cross-sectional view showing an example of the configuration of a transport robot. FIG. 3 is a schematic plan view showing an example of the operation of the robot. It is a figure explaining PTP control. It is a figure explaining CP control. It is a figure which shows an example of the content of a collision detection limit table.

Next, embodiments of the present invention will be described with reference to the drawings. A collision detection method based on the present invention detects the occurrence of a collision in a robot such as a transport robot. Collision here includes not only cases where the robot's arm or hand literally collides with another object, but also interference in which the movement of the arm or hand is affected or blocked by another object as the robot moves. shall be provided. First, a robot system to which a collision detection method according to an embodiment of the present invention is applied will be described. FIG. 1 shows an example of the configuration of such a robot system.

The robot system includes a robot 10 used for transporting semiconductor wafers, and a robot control device (robot controller) 50 that drives and controls the robot 10 based on external commands. The robot 10 includes a motor 11 that drives its arm, hand, etc., and an encoder 12 that is mechanically connected to the motor 11 and detects the rotational position of the motor 11. The robot 10 usually has a plurality of axes and is equipped with a motor 11 for each axis, but in FIG. It is.

The robot control device 50 includes a calculation unit 51 that calculates the trajectory of the robot 10 based on commands input from the outside, and outputs position command values for the motors 11 of each axis of the robot 10; It includes a drive section 52 that drives the motor 11 based on a command value. The encoder 12 outputs a signal indicating the rotational position of the motor 11, and this signal is fed back to the drive section 52, so that the drive section 52 executes servo control of the motor 11. When teaching the robot 10, a teaching pendant is connected to the robot control device 50, and commands from the teaching pendant are also input to the calculation unit 51 as external commands. The configuration including the calculation unit 51 and drive unit 52 described here is the same as the configuration of a general robot control device used for controlling industrial robots.

In order to detect the occurrence of a collision in the robot 10, the robot control device 50 shown in FIG. A comparison unit 54 that compares the collision detection limit value (threshold value) supplied from the robot 53 and a collision detection limit table that is referred to by the threshold generation unit 53 when the robot 10 is operating under CP control to be described later are stored. and a table generation section 56 that generates a collision detection limit table. The comparison unit 54 is configured by, for example, a comparator circuit, and determines that a collision has occurred in the robot 10 when the torque value exceeds a collision detection limit value. The determination result of the comparison unit 54 is outputted to the outside as a collision detection result. This collision detection result may be input to the calculation unit 51 in order to make an emergency stop of the robot 10 when a collision occurs. In the collision detection method based on the present invention, as the torque value of the motor 11, a torque command value calculated inside the drive unit 52 for servo control of the motor 11 may be used, or a current value of the motor 11 may be used. It may be used as such, or a torque value actually measured regarding the motor 11 may be used. The torque value actually measured regarding the motor 11 may be a value obtained by performing inverse dynamics calculation or model calculation based on some actual measurement value. The threshold generation section 53 and the comparison section 54 constitute a collision detection device based on the present invention. Details of the threshold generation section 53, table generation section 56, and collision detection limit table will be described later.

The collision determination method according to the present invention is based on comparing the torque value of the motor 11 with a collision detection limit value, which is a threshold value. FIG. 2 is a diagram illustrating the principle of this collision determination method, in which the horizontal axis is time and the vertical axis is the torque of the motor 11. As shown in FIG. 2(a), the torque of the motor 11 during normal operation changes from moment to moment, but remains within a certain range. If, for example, the arm of the robot 10 collides with another object at time P while being driven by the motor 11, the movement of the arm will be hindered, so the torque value of the motor 11 will be adjusted to counteract this hindering force. rise rapidly. The occurrence of a collision can be detected by detecting that the rapidly increased torque exceeds a predetermined collision detection limit value. The torque of the motor 11 differs depending on whether the motor 11 is moving at low speed or high speed, as shown in FIG. 2(b). During low speed operation, the torque is relatively small, and during high speed operation, the torque is relatively large. When attempting to detect the occurrence of a collision in the robot 10 using a single collision detection limit value, the collision detection limit value must be a value larger than the torque that can occur in the motor 11 rotating at maximum speed during normal operation. It is necessary to do so.

If the collision detection limit value is set based on the torque during high-speed operation, this collision detection limit value will be too large when the motor 11 is operating at low speed, and will prevent the occurrence of a collision in the robot 10. It is inappropriate for proper detection. FIG. 3 is a diagram similar to FIG. 2(a), but shows changes in torque when the motor 11 is operating at a relatively low speed. The collision detection limit value shown in the figure is set based on the maximum speed of the motor 11. When a collision occurs at time P, the torque value increases rapidly, but does not reach the collision detection limit value, so the occurrence of a collision is not detected at that time. The torque continues to increase and finally reaches the collision detection limit value at time Q, so that the occurrence of a collision is detected at time Q. The time from time P to time Q corresponds to a detection time lag. If a time lag occurs in collision detection, the robot will apply a larger force to the object being collided, or the robot will move further into the object. It is extremely unfavorable. Also, if the severity of the collision is small, for example, the arm collides with a small object, but as a result of the collision the object moves to a position outside the arm's trajectory and the arm can continue to move, then The amount of increase in torque may be so small that it may not reach the collision detection limit value, and the occurrence of a collision may not be detected. Not being able to detect a collision, even if it is small in scale, is extremely undesirable in terms of ensuring safety.

In the collision detection method based on the present invention, in order to quickly and reliably detect the occurrence of a collision even when the robot is moving at low speed, the collision detection limit value is set when the speed of the motor 11 is small. The collision detection limit value, which is a threshold value, is changed in accordance with the speed of the motor 11 so that the collision detection limit value becomes small when the speed is high, and the collision detection limit value becomes large when the speed is high. In FIG. 3, the arrow indicates that the collision detection limit value is made smaller during low-speed operation, so that the occurrence of a collision can be detected without time lag at the time P when the collision actually occurs. The speed of the motor 11 is obtained by differentiating the signal indicating the motor position from the encoder 12, and is constantly calculated in the drive section 52, so the threshold generation section 53 receives the motor speed from the drive section 52. Then, a collision detection limit value, which is a threshold value, is generated according to the motor speed and output to the comparison section 54. Alternatively, if the speed of the motor 11 is calculated in the calculating section 51, the threshold generating section 53 may receive the motor speed from the calculating section 51, as shown by the broken line in the figure. In reality, it is preferable to divide the motor speed range from zero speed to the maximum speed of the motor 11 into a plurality of speed ranges, and to set the collision detection limit value as a single value for each divided speed range. If the collision detection limit value is set for each speed region in this way, it becomes possible to implement the threshold value generation section 53 using a lookup table, and the collision detection limit value can be set by simplifying the configuration of the threshold value generation section 53. It is possible to reduce the amount of calculation for calculating .

Table 1 shows examples of collision detection limit values according to the conventional method. Assuming that the current speed of the motor 11 is expressed as a percentage with the maximum speed of the motor 11 as 100%, in the conventional method, the collision detection limit value is uniformly set when the motor speed is between 0% and 100%. For example, it is set to 200 cN·m. On the other hand, Table 2 shows an example of collision detection limit values used in the collision detection method according to the present invention. In the example shown here, the speed range of the motor 11 is divided into 10 speed regions in 10% increments, such as 0% to 10%, more than 10% to 20%, and more than 20% to 30%. . 83 cN・m is set as the collision detection limit value for the slowest speed region, that is, the speed region of 0% to 10%, and 96 cN・m for the next speed region, that is, the speed region of more than 10% to 20%. m is set, and 200 cN·N is set for the highest speed region, that is, the speed region of more than 90% and less than 100%.

The collision detection limit value for each speed region can be calculated, for example, by driving the motor 11 at different speeds, measuring the torque waveform at that time, and multiplying the maximum torque at that time by an appropriate safety factor. . The collision detection limit value for each motor speed calculated in advance in this manner is set in the threshold generation section 53, but may also be stored in the storage section 55 as a lookup table.

The collision detection limit value for the motor 11 of one specific axis in the robot 10 has been described above. In reality, the robot 10 is provided with a plurality of axes, and it is necessary to perform collision detection for each axis. Therefore, it is necessary to change the collision detection limit value for each axis according to the motor speed of that axis. Such an example will be explained below. FIG. 4 shows an example of a robot 10 having multiple axes. The illustrated robot 10 is a three-link horizontally articulated robot that is used for transporting semiconductor wafers and the like. The robot 10 includes a base 21, an elevating section 22 provided on the base 21, a link mechanism 23 whose proximal end is attached to the elevating section 22, and whose proximal end is rotatable with respect to the distal end of the link mechanism 23. An arm 26 is attached to the arm 26, and two hands 27 and 28 are rotatably held at the tip of the arm 26 and hold a workpiece. Hand 27 is the upper hand, and hand 28 is the lower hand. The elevating section 22 is driven by the motor 11Z and moves up and down along the vertical direction (Z direction in the drawing). Here, for the sake of explanation, the movement up and down of the elevating section 22 will be referred to as Z-axis movement. Therefore, the motor 11Z is a Z-axis motor.

The link mechanism 23 is configured such that the movement locus of its tip, that is, the position where the arm 26 is attached, is a straight line. It includes a stand side link 24 and an arm side link 25 located on the arm 26 side, and both links 24 and 25 are rotatably connected to each other by a link joint J. The base side link 24 is connected to the elevating section 22 and is rotatably held by a motor 11A built in the elevating section 22. The base-side link 24 includes a base-side pulley 24a, an arm-side pulley 24b, and a belt 24c, and the belt 24c is stretched between the base-side pulley 24a and the arm-side pulley 24b. The ratio of the diameters of the base pulley 24a and the arm pulley 24b is 2:1. The arm-side pulley 24b is connected to the arm-side link 25, and when the base-side link 24 rotates around the rotation center of the base-side pulley 24a, the rotation angle between the base-side pulley 24a and the arm-side pulley 24b changes. The ratio, that is, the rotation angle ratio between the base side link 24 and the arm side link 25 is configured to be 1:2. Furthermore, the lengths of the base side link 24 and the arm side link 25 are equal. As a result, the tip of the link mechanism 23 moves by driving the motor 11A, but its movement locus is restricted to a predetermined straight line. The movement of the link mechanism 23 is called the A-axis movement. Therefore, the motor 11A is an A-axis motor.

The arm 26 is connected to the tip of the arm-side link 25 and is rotatably held by a motor 11B built in the arm-side link 25. By driving the motor 11B, the arm 26 rotates around its base end, that is, the position where it is connected to the link mechanism 23. This movement of arm 26 is referred to as B-axis movement. Motor 11B is a B-axis motor. The hands 27 and 28 are rotatably held by motors 11C and 11D built into the arm 26, respectively, and are rotated about the tip of the arm 26 by being driven by the motors 11C and 11D. The movement of the hand 27 by the motor 11C is called a C-axis movement, and the movement of the hand 28 by the motor 11D is called a D-axis movement. The motors 11C and 11D are a C-axis motor and a D-axis motor, respectively. As described above, the robot 10 shown in FIG. 4 is a five-axis robot consisting of the A-axis, B-axis, C-axis, D-axis, and Z-axis, and is equipped with five motors 11A, 11B, 11C, 11D, and 11Z. There is. The motors 11A, 11B, 11C, 11D, and 11Z are collectively controlled by a robot control device 50. For collision detection, the robot control device 50 is provided with a threshold generation section 53 and a comparison section 54 for each axis. On the other hand, the storage section 55 and the table generation section 56 are provided in common for a plurality of axes of the robot 10.

By the way, as well-known forms of trajectory control when moving a robot's hand to a target position based on teaching data, there are PTP (point-to-point) control and CP (continuous) control. continuous path) control. PTP control is generally a control that moves a tool or hand by specifying only the start and end points of the trajectory that the tip of the tool or hand attached to the robot should take, while CP control is generally a control that moves a tool or hand in a three-dimensional space. This is a control that specifies a straight line (or curved line in some cases) path and moves the tip of the tool or hand along that path. In PTP control, the starting point and ending point are indicated in the teaching data, but the robot's path between the starting point and the ending point is not specified. Especially for robots with two or more axes, the starting point and ending point are indicated for each axis. After determining how much that axis should move in between, each axis is moved independently according to the amount of movement for each axis. CP control is to control each axis at each instant so as not to deviate from a designated path, and is used, for example, to linearly interpolate movement between teaching points indicated by teaching data. PTP control allows the robot to move faster than CP control, but since the robot's path is not specified, PTP control is more likely to cause interference with walls around the robot. On the other hand, in CP control, the route of the robot can be specified, so although it is slower than PTP control, it can reliably prevent interference with walls and the like.

Since the robot 10 shown in FIG. 4 is a transport robot, it is controlled to move from one position to another in order to transport a workpiece. At this time, the robot 10 is controlled using a combination of PTP control and CP control. The calculations necessary for PTP control and CP control are executed by the calculation unit 51 in the robot control device 50.

FIG. 5 shows an example of the operation of the robot 10 when transporting a semiconductor wafer as a workpiece. When the semiconductor wafer is a workpiece 60, the robot 10 is used to transport the workpiece 60 between a plurality of stages 61, as shown in FIG. Here, the cassette that stores the workpiece 60 and the workpiece processing device that processes the workpiece 60 are collectively referred to as a stage 61. The plurality of stages 61 are arranged so as to face a work area that is a space surrounding the robot 10, and the hands 27 and 28 of the robot 10 can be inserted into the stage 61 through the opening of the stage 61. It is configured so that it can be done. The robot 10 installed in the work area is normally in a standby position shown in FIG. 5(a) with its links 24, 25, arm 26, and hands 27, 28 folded. When the hand 27 is used to take out a workpiece 60 from a certain stage 61, the hand 27 is positioned in front of the opening of the target stage 61 using PTP control as shown in FIG. 5(b). The robot 10 is moved to the entry position shown. Thereafter, by CP control, the robot 10 is moved to the stage position shown in FIG. 5(c) so that the hand 27 enters the inside of the stage 61. Once the workpiece 60 is placed on the hand 27 at the stage position, the robot 10 returns to the standby position via the approach position, and then moves in the same manner as above to another stage 61 to which the workpiece 60 is to be transported. . The stage position is a position where the workpiece 60 in the stage 61 is placed on the hand 27, and is also a position where the workpiece 60 already placed on the hand 27 is unloaded onto the stage 61. On the other hand, the approach position is a position pulled back toward the work area from the stage position of the stage 61, and is a part of the robot 10 or a workpiece placed on the robot 10 when the robot 10 is in the approach position. 60 is located inside the stage 61. In the illustrated example, when the robot 10 is at the entry position, the tip of the hand 27 is inside the stage 61.

FIG. 6(a) is a schematic perspective view showing an example in which the robot 10 shown in FIG. 4 is moved by PTP control, and shows the posture before movement (posture R) and the posture after movement (posture S). It shows. FIG. 6(b) shows changes in the axial speed of each axis when the robot is moved from posture R to posture S under PTP control. In PTP control, when controlling so that each axis starts moving at the same time at the start point of movement and stops simultaneously at the end point of movement, the ratio of motor speeds between different axes, including periods of acceleration and deceleration, is Always constant. The ratio of motor speeds between different axes is determined by the amount of movement of the motor of each axis from the start point to the end point, and this can be calculated from the coordinate information of the start point and end point, so the PTP control operation command is input. It can be calculated in stages. Furthermore, when performing PTP control, an overall speed indicating the overall speed of movement under the PTP control is generally commanded. Each motor has a specified maximum speed, but 100% overall speed means that the robot can move at the fastest speed using PTP control. The motor is operated at the maximum speed specified, and the motors on other axes are operated at a speed lower than the maximum speed specified for the axis. Since we know the specified maximum speed for each motor and the ratio of motor speeds between different axes during PTP operation, when the overall speed is 100%, the motor of each axis It is possible to determine the percentage of the specified maximum speed at which the device is operating (speed ratio).

As described above, in this embodiment, the collision detection limit value for each motor is changed depending on what percentage of the maximum speed specified for that motor is the current speed of that motor. . On the other hand, when performing PTP control, the motor speed is not individually commanded for each axis, but the overall speed is commanded. When performing teaching, the robot 10 is not operated at 100% overall speed, but at a lower speed. For example, when operating the robot 10 at X% of the overall speed, the speed ratio of each axis motor to the maximum speed of that motor is the speed ratio when the overall speed is 100% multiplied by X/100. Become something. Then, as the collision detection limit value for each axis, a value corresponding to the speed ratio of each motor obtained in this manner may be used. In the robot control device 50, information regarding the target position and the percentage of the overall speed at which the robot should be operated regarding PTP control is given as a command to the calculation section 51, so that the calculation section 51 can calculate the operating speed of the motor of each axis. In the robot control device 50, a threshold generation unit 53 provided for each axis is given the motor speed calculated for that axis. At this time, when the speed range of the motor is divided into a plurality of speed regions as shown in Table 2, the collision detection limit value is determined depending on which speed region the calculated motor speed is included. By assigning a collision detection limit value to each axis according to the speed of that axis in this manner, collision detection can be performed using a more appropriate collision detection limit value.

FIG. 7(a) is a schematic perspective view showing an example in which the robot 10 shown in FIG. 4 is moved by CP control that performs linear interpolation so that the trajectory of the hand 27 becomes a straight line. The posture R) and the posture after movement (posture S) are shown. FIG. 7(b) shows changes in the axis speed of each axis when the robot is moved from posture R to posture S under CP control. As is clear from FIG. 7(b), the manner in which the shaft speed of the motor increases or decreases greatly differs from motor to motor. The ratio of the speeds of the motors on different axes is not constant, but varies greatly from the start to the end of the movement. The motor speed ratio and its changes cannot be obtained simply by providing coordinate information of the start and end points of movement, but cannot be known unless the robot 10 is actually operated under CP control. Therefore, unlike the case of PTP control, in CP control, if a CP control operation command is input with a specification of the overall speed of the robot 10, a collision detection limit value for each axis can be assigned on the spot. I can't. Note that in CP control, the speed change of the motor of each axis is not known in advance, so the speed of the motor of any axis will be the maximum speed specified for that motor, and the speed of the motors of the remaining axes will be the speed of those motors. It is difficult to specify 100% of the overall speed under the condition that the maximum speed is less than the maximum speed specified for each. Instead, in the robot control device 50, the maximum value of the overall speed of the robot 10 in CP control is determined in consideration of the margin, and the maximum value of the speed is set as 100% of the overall speed in CP control.

By the way, since CP control cannot move the robot as quickly as PTP control, the situations in which CP control is used in transportation robots are limited. In the case of the transport robot 10 shown in FIG. 4, as explained using FIG. CP control is used only for movement between and PTP control is used for other movement. The location of the stage 61 is generally fixed, and if the location of the stage 61 is fixed, the movement trajectory of the robot 10 under CP control with respect to the stage position of each stage 61 does not change. it is conceivable that. In other words, after the teaching of the robot 10 to the stage 61 is completed, the robot 10 passes through the same trajectory every time it moves relative to the stage 61 under CP control.

Therefore, in this embodiment, a collision detection limit table is prepared for each stage 61 and stored in the storage unit 55, and during CP control, the threshold value generation unit 53 outputs the threshold value obtained by referring to the collision detection limit table to the comparison unit 54. do. In FIG. 8, it is assumed that there are two stages 61, stage #1 and stage #2. An example of the contents of a collision detection limit table is shown. As shown in the figure, the collision detection limit table provided for each stage 61 indicates a collision detection limit value, which is a threshold value, for each axis and for each speed region at the overall speed. The speed region at the overall speed is called the CP (or continuous path control) speed region. On the other hand, as explained using Table 2, the speed range determined based on the maximum speed specified for the motor is called the PTP (or point-to-point) speed range. Similar to the case shown in Table 2, in the table, "10%" of the overall speed corresponds to the case where the overall speed is between 0% and 10%, and "20%" corresponds to the case where the overall speed is over 20% and 30%. It corresponds to the following cases. For example, if the overall speed of CP control in stage #1 is 45%, the threshold value generation unit 53 corresponding to the A-axis selects the column for the overall speed of "50%" in the collision detection limit table for stage #1. 240 cN·m is obtained as a collision detection limit value for the A-axis and outputted to the comparator 54 as a threshold value, and similarly, the threshold generating unit 53 corresponding to the B-axis acquires 200 cN·m as a collision detection limit value for the B-axis. Get m. The same applies to other axes.

The collision detection limit table for each stage 61 is created by measuring the speed ratio of each axis under CP control in the stage 61 when the robot 10 completes teaching for that stage 61 under CP control, and detecting collision for each axis. It can be created by calculating the limit value. Specifically, during movement to the corresponding stage 61, the torque correction value of each axis within the drive unit 52 is measured to obtain a torque waveform, the maximum torque correction value for each axis is obtained from the torque waveform, and the maximum torque correction value for each axis is calculated. The collision detection limit value for each axis is determined by adding a margin for absorbing individual differences between robots to the maximum torque correction value. The table generation unit 56 provided in the robot control device 50 executes processing from acquiring the torque compensation value from the drive unit 52 to calculating the collision detection limit value for each axis, and is the target at that time. The calculated collision detection limit value is written into the collision detection limit table in the storage unit 55 based on the stage and the overall speed at that time. By calculating such a collision detection limit value for all stages 61 and all speed regions (all speed regions used in the collision detection limit table regarding the overall speed) accessed by the robot 10, A collision detection limit table for each stage 61 is completed. Once the collision detection limit table is completed, from then on, when the robot 10 accesses the stage 61 under CP control, the collision detection limit value read from the collision detection limit table is assigned to each axis. When operating the robot 10, more optimal collision detection can be performed.

According to the collision detection method of the present embodiment described above, when determining that a collision has occurred when the motor torque value exceeds the collision detection limit value, the collision detection limit value is changed according to the motor speed. To quickly and reliably detect the occurrence of a collision even when the robot is moving at low speed. In particular, a collision detection limit table is prepared in which threshold values for each overall speed are stored for each stage and each axis, and when the robot is actually moved using CP control, the collision detection limit table is referred to based on the commanded overall speed. By obtaining the collision detection limit value, which is a threshold value, more optimal collision detection can be performed when the robot is accessed to the stage by CP control. Even when the robot is operating in PTP mode, the occurrence of a collision can be reliably detected.

10... Robot; 11, 11A, 11B, 11C, 11D, 11Z... Motor; 12... Encoder; 22... Base; 23... Link mechanism; 26... Arm; 27... Upper hand; 28... Lower hand; 29... Lifting 50... Robot control device; 51... Calculation unit; 52... Drive unit; 53... Threshold generation unit; 54... Comparison unit; 55... Storage unit; 56... Table generation unit.

Claims (10)

A collision detection method for detecting the occurrence of a collision in a robot equipped with a plurality of axes, each of which is driven by a motor, the method comprising:
A collision determination step of comparing a threshold value determined according to the speed of the motor for each axis with a torque value of the motor of the axis, and determining that a collision has occurred when the torque value exceeds the threshold value. ,
In the collision determination step, when the robot is a robot that transports a workpiece between a plurality of stages, and the robot moves between the entry position of the stage and the stage position for each stage by continuous path control. , the threshold value obtained by referring to a collision detection limit table in which the threshold value for each overall speed is stored in advance for each stage and each axis is used,
The collision detection method, wherein the overall speed is the overall speed of the robot operating under the continuous path control. In the collision detection limit table, the overall speed is divided into a plurality of speed regions for continuous path control in a range from zero speed to the maximum speed specified in the continuous path control, and the threshold value is set for each speed region for continuous path control. The collision detection method according to claim 1, wherein: is determined as a single value. 3. The threshold value is determined based on a maximum value in a torque waveform of the motor and stored in the collision detection limit table when teaching of the robot in the continuous path control is completed. Collision detection method described. The robot moves between a standby position and the approach position under point-to-point control,
When moving under the point-to-point control, the speed ratio is determined for each axis in accordance with the speed ratio for each of the plurality of axes, such that the lower the speed of the motor is, the smaller the speed is, and the higher the speed of the motor is, the higher the speed is. The collision detection method according to claim 1 or 2, wherein the threshold value for the axis is determined based on the speed determined by the axis. The range from speed zero to the maximum speed of the motor is divided into a plurality of point-to-point control speed regions, and each point-to-point control speed region is used during the point-to-point control. 5. The collision detection method according to claim 4, wherein the threshold value is determined as a single value. A collision detection device for detecting the occurrence of a collision in a robot equipped with a plurality of axes, each of which is driven by a motor,
a threshold value generation unit that generates a threshold value determined according to the speed of the motor for each axis;
a comparison unit that compares the threshold value supplied from the threshold generation unit and the torque value of the motor, and determines that a collision has occurred when the torque value exceeds the supplied threshold value;
a storage unit that stores a collision detection limit table in which the threshold value for each overall speed is stored in advance for each stage and each axis;
has
When the robot is a robot that transports a workpiece between a plurality of stages, and when the robot moves between the entry position of the stage and the stage position for each stage by continuous path control, the threshold value generation unit , supplying the threshold value obtained by referring to the collision detection limit table to the comparison unit;
The collision detection device, wherein the overall speed is the overall speed of the robot operating under the continuous path control. In the collision detection limit table, the overall speed is divided into a plurality of speed regions for continuous path control in a range from zero speed to the maximum speed specified in the continuous path control, and the threshold value is set for each speed region for continuous path control. The collision detection device according to claim 6, wherein is determined as a single value. The collision detection device according to claim 6 or 7, further comprising a table generation unit that determines the threshold value based on the maximum value in the torque waveform of the motor and stores it in the collision detection limit table. The robot moves between a standby position and the approach position under point-to-point control,
The threshold value generated by the threshold value generation unit during movement under the point-to-point control is set for each of the plurality of axes such that the lower the motor speed is, the smaller the threshold value is, and the higher the motor speed is, the larger the threshold value is. The collision detection device according to claim 6 or 7, wherein the collision detection device is determined based on a speed determined for each axis according to a speed ratio. The range from zero speed to the maximum speed of the motor is divided into a plurality of point-to-point control speed regions, and each point-to-point control speed region is used during the point-to-point control. The collision detection device according to claim 9, wherein the threshold value is determined as a single value.

Images