JP5370127B2 - Robot interference avoidance device - Google Patents

Abstract

<P>PROBLEM TO BE SOLVED: To avoid any contact of a robot with a person as much as possible while the robot is being operated. <P>SOLUTION: When a robot arm is moved in the approaching direction to an object, and if a person and the robot arm are brought close to each other in a predetermined distance, the robot arm is subjected to the emergency stop. If the person and the robot arm are separated from each other over the predetermined distance, the robot arm is decelerated according to the distance therebetween. When the robot arm is moved in the receding direction, the operation of the robot arm according to the operational program is continued without stopping or decelerating the robot arm. <P>COPYRIGHT: (C)2011,JPO&amp;INPIT

Description

  The present invention relates to a robot interference avoidance apparatus that can avoid interference between a robot and a person while avoiding the stop of the robot as much as possible.

  There is a device that prevents a robot from coming into contact with a person by stopping the robot when the person enters a dangerous area of the robot. For example, in the apparatus disclosed in Patent Document 1, the light beam LB is passed near the outside of the stop area (dangerous area), and the light beam LA constituting the warning area is passed a little outside the light beam LB. Then, assuming that a person has entered from the outside to the inside of the warning area, the operation speed of the robot is reduced, and further, when the light beam LB is blocked, the person has entered the stop area through the warning area. And stop the robot.

Utility Model Registration No. 2587584

  In the apparatus of Patent Document 1, regardless of the moving direction of the robot, anyway, when a person enters the warning area, the operation speed of the robot is reduced, and when the person enters the stop area, the robot is stopped. For this reason, depending on the movement direction of the robot, even if there is no fear of contact with a person, the robot decelerates or stops, so that the working efficiency of the robot is reduced.

  The present invention has been made in view of the above circumstances. The purpose of the present invention is to determine the necessity / unnecessity of deceleration or stop in consideration of the position of an object (including a person) and the moving direction of the robot, and stop the robot. In other words, an object of the present invention is to provide a robot interference avoidance device that can avoid contact with an object while operating the robot as much as possible.

  In the present invention, even if an object has entered a predetermined area, if the robot arm is moving in a direction away from the entry position of the object, the operation speed of the robot is not reduced or the robot is not stopped. . For example, even if the distance between the robot arm and the object is short, when the robot arm moves away from the object, the operation according to the operation program is continued without decelerating or stopping the robot arm. . Even when the robot arm approaches the object, if the distance between the tip of the required arm and the object exceeds a certain distance, the robot arm is decelerated according to the distance and the required When the tip of the arm approaches the object within a certain distance considering the braking distance at the time of emergency stop, the robot arm is emergency stopped. For this reason, it is possible to avoid contact (interference) between the robot arm and the object while maintaining a high operation rate of the robot arm.

The perspective view of the robot which shows one Embodiment of this invention Block diagram showing the electrical configuration of the robot Flow chart showing interference avoidance control content Plan view for supplementary explanation of control details of interference avoidance

  Hereinafter, an embodiment of the present invention will be described with reference to the drawings. FIG. 2 shows a six-axis vertical articulated robot 1. The robot arm 2 of the robot 1 includes a base 3, a shoulder portion 4 supported by the base 3 so as to be pivotable in a horizontal direction around the first axis Lc- 1, and a second axis Lc− on the shoulder portion 4. A lower arm 5 supported so as to be pivotable in the vertical direction about the second axis, a first upper arm 6 supported on the lower arm 5 so as to be pivotable in the vertical direction about the third axis Lc-3, A second upper arm 7 is supported at the tip of the first upper arm 6 so as to be rotatable about the fourth axis Lc-4, and a fifth axis Lc-5 is centered on the second upper arm 7. The wrist 8 is supported so as to be pivotable in the vertical direction, and the flange 9 is supported on the wrist 8 so as to be able to rotate by twisting about the sixth axis Lc-6. An end effector (not shown) such as a hand for gripping a workpiece or a camera used for visual inspection is attached to the flange 9 which is the tip of the robot.

  The base 3, the shoulder portion 4, the lower arm 5, the first upper arm 6, the second upper arm 7, the wrist 8, and the flange 9 function as arms in the robot arm 2, except for the base 3 that is a fixed arm. The arms 4 to 9 are fixedly connected to a rotary joint shaft (not shown) rotatably supported by the preceding arm. The rotary joint shafts of the arms 4 to 9 are rotated by the servo motor 10 shown in FIG. Each rotary joint axis corresponds to the first axis Lc-1 to the sixth axis Lc-6.

  The control device 12 as a control means for controlling the operation of the robot arm 2 is composed of a microcomputer and includes a CPU 13, a ROM 14, and a RAM 15 as shown in FIG. A drive circuit 16 of the servo motor 10 and a rotation position detection circuit (rotation position detection means) 17 for detecting the rotation angle of the servo motor 10 are connected to the control device 12. The rotation position detection circuit 17 detects the rotation angle of the servo motor 10 based on a rotation detection signal input from a rotary encoder (rotation sensor) 18 connected to a rotation shaft (not shown) of the servo motor 10. The rotation angle information is given to the control device 12 and the drive circuit 16. In FIG. 2, only one servo motor 10 is shown, but in reality, one servo motor 10 is provided for each arm 4-9.

  Here, three-dimensional coordinates are defined for the base 3 and the arms 4 to 9. Among these, the coordinate system of the base 3 (indicated by coordinate axes XYZ in FIG. 1) installed on the floor surface is a stationary coordinate system and is set as a reference coordinate (robot coordinate). The coordinates of the arms 4 to 9 are set on the rotation center axis of a rotary joint axis (not shown) of each of the arms 4 to 9, and the position and posture on the robot coordinates are changed by the rotation of the arms 4 to 9. The ROM 14 stores the coordinate position of the shoulder section 4 on the robot coordinates, the coordinate position of the lower arm 5 on the coordinates of the shoulder section 4, the coordinate position of the first upper arm 6 on the coordinates of the lower arm 5, and the first upper position. The coordinate position of the second upper arm 7 on the coordinates of the arm 6, the coordinate position of the wrist 8 on the coordinates of the second upper arm 7, the coordinate position of the flange 9 on the coordinates of the wrist 8, and the arms 4 to 9 Various parameters such as length are stored. In the following, the position of the coordinates of the arms 4 to 9 on the robot coordinates is simply referred to as the position of the arms 4 to 9.

  The origin of the coordinates of the flange 9 that is the tip of the robot is determined at the center of rotation of the tip surface of the flange 9. The CPU 13 has a coordinate conversion calculation function. When the position of the robot tip (including the posture; the same applies hereinafter) is given, the rotation of the arms 4 to 8 such that the robot tip takes the given position. The angle can be calculated (reverse conversion), and when the rotation angles of the arms 4 to 9 are given, the positions of the arms 4 to 9 can be calculated (forward conversion).

  The operation program for the robot arm 2 is created by a teaching pendant (operation program creating means) (not shown) and stored in the RAM 15. The CPU 13 calculates the movement trajectory of the robot tip based on the operation program stored in the RAM 15, and the speed pattern of the robot tip is a trapezoid pattern while the robot tip moves from the movement start position to the movement end position. The position of the tip of the robot at every sampling time is calculated so that a predetermined speed pattern such as a triangular pattern is obtained. Note that the sampling time period is set to a very short time, for example, 10 ms. When the position of the tip of the robot for each sampling time is obtained, the CPU 13 obtains the rotation angle for each sampling time of each of the arms 4 to 8 and stores it in the RAM 15 as storage means.

  During the operation control of the robot arm 2, the CPU 13 calculates the rotation angle of each arm 4 to 9 from the rotation angle of each servo motor 10 given from the rotation position detection circuit 17 at each sampling time, The current position is detected by calculating the position of the flange 9 from the rotation angle. Then, the rotation angle of each arm 4-9 at the position next to the current position of the tip of the robot is obtained from the RAM 15, and the rotation angle of each servo motor 10 is calculated from the rotation angle of each arm 4-9, and the rotation angle. Is output to the drive circuit 16 as the target rotation angle of each servo motor 10. In order to obtain the rotation angle of each arm 4 to 9 from the rotation angle of each servo motor 10, the rotation angle of each servo motor 10 is divided by the reduction ratio of each reduction gear 11. Conversely, in order to obtain the rotation angle of each servo motor 10 from the rotation angle of each arm 4-9, the rotation angle of each arm 4-9 is multiplied by the reduction ratio of each reduction gear.

  The drive circuit 16 compares the target rotation angle given from the robot control device 12 with the rotation angle of each servo motor 10 fed back from the rotation position detection circuit 17, and supplies a current corresponding to the difference to each servo motor 10. To supply. Thus, the servo motor 10 is controlled to rotate to the target rotation angle. By performing such control at every sampling time, the tip of the robot moves according to the locus defined by the operation program.

  A CCD camera 19 as an intrusion detection unit is installed above the robot arm 2, for example, on the extension of the Z coordinate axis perpendicular to the robot coordinates. The CCD camera 19 has a predetermined range including the robot arm 2, specifically, a predetermined range centered on the robot arm 2, specifically, a Z coordinate axis of the robot coordinates, and an operation area of the robot arm 2. A range including the image capturing area (detection area). The control unit (not shown) of the CCD camera 19 analyzes the captured image at a predetermined cycle, for example, a cycle equal to or less than the sampling time cycle of the control device (hereinafter simply referred to as robot control device) 12 of the robot 1. Compare the captured image with the current captured image.

  When there is a newly photographed object in the current photographed image, it is determined that the object has entered the detection area. Further, even if the object is captured in both the current captured image and the previous captured image, if the position is different, it is determined that the object has entered the detection area. The control unit of the CCD camera 19 is connected to the robot control device 12 so that when an object is detected, the position of the object on the captured image is given to the robot control device 12. In this case, the position on the image captured by the CCD camera 19 is given as a position on the horizontal two-dimensional coordinates (camera coordinates), and the position on the camera coordinates can be converted into the position on the XY coordinates of the robot coordinates. Calibration is performed in advance between camera coordinates and robot coordinates. The objects detected by the CCD camera 19 include a transport vehicle or a worker on which a work is placed. Since a typical object is a person, the object will be described as a person in the following. .

  The robot control device 12 constitutes an interference avoidance device for controlling the robot arm 2 so as not to interfere with a person using the CCD camera 19 as a human detection sensor. The feature of the function of the robot controller 12 as an interference avoidance device is that the part to be monitored is not the only robot tip (tip of the flange 9) as a moving part that may interfere (contact) with a person. A component in which the monitoring target unit includes a plurality of monitoring target parts including the tips (joints, etc.) of other arms, and the moving direction of the monitoring target part approaches the person even if the distance between the monitoring target part and the person is short If it does not have, there is no risk of contact with humans, and no interference avoidance measures such as emergency stop are taken.

  In this embodiment, two portions, that is, the tip of the flange 9 that is the tip of the robot and the tip of the lower arm 5 are defined as the monitoring target portion. Since the tip of the lower arm 5 functions as an elbow and may move in a direction opposite to the direction of the tip of the flange 9, it can be said that this is a portion that is highly likely to come into contact with a person. The other part, for example, the shoulder part 4 does not change its position in the part that rotates horizontally, and the first upper arm 6, the second upper arm 7, and the wrist 8 are the tip of the lower arm 5 and the flange 9. Since it is considered that the contact with the person can be avoided by monitoring the movement of the tip, it is removed from the monitoring target part.

  Next, interference avoidance by the robot controller 12 will be described with reference to the flowchart of FIG. In FIG. 3, the lower arm 5 is referred to as a J2 arm, and the flange 9 is referred to as a J6 arm.

  The control unit of the CCD camera 19 always acquires a captured image every time a predetermined short time elapses, and compares the previous captured image with the current captured image to determine whether a person has entered the detection area. Is monitored (step S1). When a person enters the detection area, the CCD camera 19 detects that the person has entered the detection area, detects the position of the person who has entered, and indicates that the person has entered the detection area. The control device 12 is notified (“YES” in step S1).

  When the CPU 13 of the robot controller 12 receives a notification that a person has entered the detection area, the CPU 13 acquires all the rotational joint axes at the current sampling time, that is, the rotation angles of all the arms 4 to 9 from the RAM 15 ( Step S2: Arm rotation angle acquisition means), the coordinate conversion calculation function calculates the position of the lower arm 5, which is one of the monitoring target parts, and the position of the lower arm 5 (the coordinate position of the lower arm 5) and the lower From the length of the arm 5 and the rotation angle of the lower arm 5, the position of the tip of the lower arm 5 on the robot coordinates is calculated (step S3: monitoring target portion position detecting means among the motion detecting means). If the tip of the lower arm 5 is the third axis Lc-3, the position of the tip of the lower arm 5 can be obtained as the position of the first upper arm 6.

  When the position of the tip of the lower arm 5 is obtained, the CPU 13 calculates the distance between the tip of the lower arm 5 and a person (step S4: separation distance acquisition means). The distance to the person at this time is a horizontal distance in two-dimensional coordinates defined by the XY coordinates because the position of the person is represented by the XY coordinates of the robot coordinates. Next, the CPU 13 calculates the moving speed vector (direction and speed) of the tip of the lower arm 5 (step S5: speed detecting means and moving direction detecting means of the motion detecting means), and moving the tip of the lower arm 5 The component of the velocity vector in the human direction is calculated (step S6: human direction component detecting means).

  Here, the moving speed vector, that is, the speed and direction of the tip of the lower arm 5 is determined based on the difference between the tip position of the lower arm 5 at the current sampling time and the tip position of the lower arm 5 at the next sampling time. And the speed is obtained by dividing the moving distance by the time interval of the sampling time (speed detecting means among the motion detecting means) and the next time from the position of the tip of the lower arm 5 at the current sampling time. A vector directed to the position of the tip of the sampling time is obtained, and the direction of the vector is set as the movement direction (movement direction detection means among the movement detection means).

  Next, the CPU 13 calculates the position of the tip of the flange 9 that is the other monitoring target portion from the rotation angle acquired in step S2 (step S7: monitoring target portion position detection means of the operation detection means). The distance between the tip of the flange 9 and the person is calculated (step S8: separation distance acquisition means), and the moving speed vector of the tip of the flange 9 is calculated in the same manner as the tip of the lower arm 5 (step S9). : Speed detection means and movement direction detection means of motion detection means), and then the component of the movement speed vector at the tip of the flange 9 in the human direction is calculated (step S10: human direction component detection means). Also at this time, the distance to the person is the horizontal distance.

  About the moving speed vector of the front-end | tip of the lower arm 5 and the front-end | tip of the flange 9, the component of a person direction is calculated as follows. That is, in FIG. 4, R indicates the tip of the lower arm 5 or the tip of the flange 9 that is the monitoring target portion, and V indicates the moving speed vector of the tip of the lower arm 5 or the tip of the flange 9. The component of the moving speed vector V in the human direction is that a perpendicular line Hv is lowered from the tip of the moving speed vector V to a straight line L formed by connecting the monitoring target portion R and the person, and an intersection point m between the perpendicular line Hv and the straight line L is obtained. Ask. Then, if a vector Vm from the monitoring target part R to the intersection point m is obtained, this vector Vm becomes a human component of the moving speed vector V. When the direction of the human-direction component vector Vm is directed toward the person, the human-direction component Vm of the moving speed vector is toward the person.

  When the human direction segment of the moving speed vector of the tip of the lower arm 5 and the tip of the flange 9 which is the monitoring target R is obtained as described above, the CPU 13 then determines the tip of the lower arm 5 and the tip of the flange 9. It is determined whether or not the human direction component of the movement speed vector is toward the person (step S11: approach determination means), and if the human direction component of any movement speed vector is not toward the person ("NO" in step S11) ”,“ NO ”in step S12), the operations in steps S1 to S12 are repeated.

  That is, assuming a straight line Lh that passes through the monitoring target portion R and is orthogonal to the straight line L that connects the person and the monitoring target portion R, the moving speed vector V overlaps with the straight line Lh, or the person is separated from the straight line Lh. When the moving speed vector V is directed to the opposite side of the object and the monitoring target part R moves away from the person, even if the distance between the person and the monitoring target part R is short, the monitoring target part Since there is no fear of contact between R and a person, the operation of the robot arm 2 is continued as it is.

  On the other hand, when both the moving speed vectors of the tip of the lower arm 5 and the tip of the flange 9 are in the human direction component, the CPU 13 risks both the tip of the lower arm 5 and the tip of the flange 9. The target part is set (“YES” in step S11, step S13: danger target part setting means, “YES” in step S14, step S15: danger target part setting means). Further, of the tip of the lower arm 5 and the tip of the flange 9, when the human direction component is directed toward the person by the moving speed vector of the tip of the lower arm 5, only the tip of the lower arm 5 is set as the danger target portion. Set ("YES" in step S11, "NO" in step S13, step S14). Of the tip of the lower arm 5 and the tip of the flange 9, when the human direction component is directed toward the person by the moving speed vector of the tip of the flange 9, only the tip of the flange 9 is set as the danger target part (step) "NO" in S11, "YES" in step S12, step S15).

  When either or both of the tip of the lower arm 5 and the tip of the flange 9 are set as the danger target part, the CPU 13 sets the movement speed of the danger target part at the next sampling time in the same manner as described above. When the emergency stop is performed at this moving speed, the distance (braking distance) that travels from the start of the emergency stop to the stop (braking distance) is calculated (step S16: braking distance acquiring means), and this braking distance is used as a basis. Thus, the safe position S is obtained (step S16). The emergency stop refers to an operation of causing the servo motor 10 to generate the largest reverse torque and rapidly stopping the arms 4 to 9. The emergency stop means is not limited to one that generates reverse torque in the servo motor 10, but may be other means such as operating a mechanical brake.

  Here, the safety position S is set at a position away from the current position of the danger target portion in the speed vector direction by a distance obtained by adding the predetermined distance b to the braking distance B. Then, in FIG. 4, the CPU 13 draws a perpendicular line from the safe position S at the time of emergency stop of the dangerous object part shown in (D) to a straight line L connecting the current dangerous object part (D) and the person, It is set as the danger safety boundary line I (step S17).

  Next, the CPU 13 obtains an intersection n between the danger safety boundary line I and the straight line L, calculates a distance F between the intersection n and the danger target part (D), and calculates this distance F as a person and the danger target part (D ) (The distance between the person determined in step S4 and the tip of the lower arm 5 and / or the distance between the person determined in step S8 and the tip of the flange 9). It is determined whether or not it exists on the danger target part (D) side with respect to the line I (step S18: danger judgment means).

  If a person is on the danger safety boundary line I or on the danger target part (D) side of the danger safety boundary line I (“YES” in step S18), the robot arm 2 is urgently stopped at this point (step S19). Then, in the case of FIG. 4, the danger target part (D) stops before the predetermined distance b of the safe position S. Further, although different from FIG. 4, an emergency stop is started when the danger target part (D) is separated from the person by a distance B + b even in the worst case when the danger target part (D) is moving straight toward the person. Therefore, the robot arm 2 stops immediately before the danger target part (D) contacts the person.

  If the person is not on the danger safety boundary line I or on the danger target part (D) side from the danger safety boundary line I (“NO” in step S18), the emergency stop operation may not be performed yet. Since it is unthinkable to move in a direction approaching the arm 2, the CPU 13 decelerates the robot arm 2 in order to ensure safety in such a case (step S20). The deceleration at this time depends on the distance between the danger target part (D) and the person. For example, the vehicle is decelerated at a deceleration that is proportional to the distance between the danger target part and the danger safety boundary line. By doing so, since the speed of the danger target part (D) decreases as it gets closer to the person, contact with the person can be prevented more reliably. The impact given can be made very small.

  After decelerating in step S20, the process returns to step S1 and the above operations are repeated. Even if the vehicle once decelerates, if the distance to the person does not change thereafter, the robot arm 2 continues to operate at the speed at that time without further decelerating. Further, if the distance between the danger target part (D) and the person increases, the operation speed of the robot arm 2 increases accordingly.

  Thus, according to the present embodiment, when the tip of the lower arm 5 and the tip of the flange 9 move away from the person, the person and the tip of the lower arm 5 and between the person and the tip of the flange 9 are moved. Even if the distance is short (even within the braking distance at the time of emergency stop operation), the operation is continued as it is without performing the emergency stop operation.

  Even if the tip of the lower arm 5 or the tip of the flange 9 moves in a direction approaching a person, the robot arm 2 is not stopped when the distance is within a certain distance, but the person and the tip of the lower arm 5 are stopped. Or since it decelerates according to the distance of a person and the front-end | tip of the flange 9, it can prevent contacting with a person, without making operation | movement of the robot arm 2 as slow as possible.

  Moreover, the CCD camera 19 can detect a newly photographed object or an object with a different position by simply comparing the previous photographed image with the current photographed image, so that it is easy for a person to enter by simple image analysis. Therefore, it is possible to suppress the increase in price.

The present invention is not limited to the embodiment described above and shown in the drawings, and can be expanded or modified as follows.
The deceleration rate of the robot arm 2 in step S19 may be a deceleration rate proportional to the distance between the person and the danger target portion, or may be a stepwise change in the deceleration rate.
The robot is not limited to a vertical articulated robot, and may be a horizontal articulated robot.
The CCD camera 19 (imaging means) may be one that can acquire the position of an object in three dimensions.
The intrusion detection means is not limited to the CCD camera 19.
The control part of the interference avoidance device may be constituted by a microcomputer different from the robot control device 12.

  In the drawings, 2 is a robot arm, 5 is a lower arm (monitoring target part), 9 is a flange (monitoring target part), 10 is a servo motor, 12 is a control device (interference avoidance device, control means, motion detection means, approach judgment). Means, separation distance acquisition means, braking distance acquisition means, danger judgment means), 19 indicates a CCD camera (intrusion detection means).

Claims (1)

In a robot interference avoidance device comprising a robot arm configured by connecting a plurality of arms by a plurality of joints, and operating the robot arm according to a predetermined operation program,
An intrusion detection means for detecting a position of an object that has entered the detection area, with a predetermined range including the robot arm as a detection area;
An operation detecting means for detecting a position, a moving direction, and a moving speed of the monitoring target portion based on the operation program, with a tip of a required arm selected from the plurality of arms as a monitoring target portion;
An approach determining means for determining whether or not the moving speed vector of each of the monitoring target parts has a speed component in a direction toward the object;
The distance from the risk target part to the object is detected using the monitor target part determined to have a speed component in the direction toward the object among the monitor target parts. Separating distance acquisition means
Braking distance acquisition means for acquiring the braking distance at the time of emergency stop of the danger target part from the moving speed of the danger target part;
Assuming a safe position away from the current position of the danger target portion in the direction of the moving speed vector by a distance exceeding the braking distance, a perpendicular line drawn from the safety position to a straight line connecting the danger target portion and the object is drawn. A risk judging means for judging whether or not the object is closer to the danger target part than the dangerous safety boundary line,
Control means for controlling the robot arm according to the judgment result of the danger judgment means,
When it is determined that the object is closer to the danger target part than the dangerous safety boundary line, the control means stops the robot arm urgently, and the object is more dangerous than the dangerous safety boundary line. An interference avoidance device for a robot, comprising: decelerating the robot arm according to a separation distance between the danger target portion and the object when it is determined that the robot arm does not exist on a side close to the target portion.

Images