JP5915322B2 - Robot device - Google Patents

Description

  The present invention relates to a robot apparatus that detects an intrusion of an object and avoids a collision with a robot arm.

In order to avoid a collision between a robot and a person, a guard fence surrounding the operation area of the robot is generally installed to prevent a person from entering the operation area of the robot.
However, when a protective fence is installed, for example, when a wagon or the like equipped with supply parts is carried into the operation area of the robot, the supply work becomes troublesome.

  As a technique that can be applied to avoid a collision between a person and a robot without providing a protective fence, there is, for example, Patent Document 1. In this patent document 1, a CCD camera is installed above a robot, and when a human intrusion into the operation area of the robot is detected by the CCD camera, an attempt is made to avoid a collision with a person by stopping the robot. To do.

JP 2011-125975 A

  In the technique of Patent Document 1, the CCD camera must be installed on the ceiling of the factory, and an additional work for installing the CCD camera is required in addition to the work of installing the robot on the floor of the factory. Further, since the CCD camera needs to be mounted at a high place such as a factory ceiling, only a small person appears in the photographed image, so that advanced image analysis is required.

  The present invention has been made in view of the above circumstances, and an object of the present invention is to avoid a collision with a moving body (including a person) and to install an apparatus for detecting an intrusion of an object when a robot is installed. It is an object of the present invention to provide a robot apparatus that does not require additional work such as and that can detect the intrusion of an object by a simple method.

According to the first aspect of the present invention, an exploration device for detecting an object is provided at the base of the robot body. Therefore, in addition to the operation of installing the robot body on the floor surface of a factory, etc. No additional work is required.
An exploration device for detecting the intrusion of an object emits an exploration signal, detects an object existing in the emission direction and the distance to the object, and the exploration device moves around the base. Therefore, the position of the object can be easily acquired from the position of the traveling body and the distance from the exploration device to the object.

  Since the exploration device emits the exploration signal obliquely upward, in the case of a person, it will be emitted toward the upper body. Whether or not the detected object is moving is determined by whether or not the position of the object is different between the previous measurement position and the current measurement position. On the other hand, since things such as wagons do not move after installation, it is easy to distinguish between people and things.

  It is determined whether or not the detected object is moving by comparing the current position data obtained by one cycle of the search device with the basic position data. For this reason, for example, when a wagon loaded with supply parts is carried in, it is assumed that the detection object is moving while the wagon is being moved. After the wagon is installed, since the wagon is stationary, there is no difference between the position data from the current one-cycle travel and the position data from the previous one-cycle travel. Then, the position data from the current or previous one-cycle travel is replaced with the basic position data. For this reason, it is not determined that the detected object is moving every time position data is acquired by the exploration device, such as when a new wagon is installed, and the processing can be speeded up. .

  According to the invention of claim 2, when there is a possibility that the detected object and the robot arm collide, first, the robot arm is decelerated, and when the collision cannot be avoided by decelerating, the robot arm is braked and stopped. If the collision cannot be avoided by the stop operation, the movement direction of the robot arm is gradually changed to be the same as the movement direction of the detection object while stopping the robot arm. Can be avoided more reliably.

The side view of the robot main body which shows one Embodiment of this invention Side view showing the traveling vehicle attached to the base Schematic perspective view of traveling vehicle Block diagram showing electrical configuration Flow chart showing the processing content of measurement data Flow chart showing the control details while the robot body is stopped Flow chart showing control contents during operation of robot body (A) is a figure which shows the moving direction of a moving object and a robot arm, (b) is a figure which shows the collision avoidance operation | movement of a robot arm. (A) is a speed pattern diagram, (b) is a diagram showing a collision determination area.

  Hereinafter, an embodiment of the present invention will be described with reference to the drawings. FIG. 1 shows a robot body 1 of a six-axis vertical articulated robot apparatus. This robot body 1 is configured by providing a robot arm 3 on a base 2. The robot arm 3 includes a shoulder portion 4 that is supported by the base 2 so as to be able to turn in the horizontal direction, a lower arm 5 that is supported by the shoulder portion 4 so as to be able to turn in the up and down direction, and a lower arm 5 that is turned up and down. The first upper arm 6 supported so as to be able to rotate, the second upper arm 7 supported so as to be able to be twisted and rotated at the distal end portion of the first upper arm 6, and the second upper arm 7 in the vertical direction The wrist arm 8 is rotatably supported, and the flange 9 is supported by the wrist arm 8 so as to be twisted and rotatable. 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 at the tip of the robot arm.

  The shoulder 4, the lower arm 5, the first upper arm 6, the second upper arm 7, the wrist arm 8, and the flange 9 function as an arm (link) in the robot arm 3. Are fixedly connected to a rotary joint shaft (not shown) rotatably supported by the arm. The rotary joint shafts of the arms 4 to 9 are rotated by the servo motor 10 shown in FIG.

  As shown in FIG. 4, the control device 12 as a robot control means for controlling the operation of the robot arm 3 includes a microcomputer, and includes a CPU 13, a ROM 14, and a RAM 15. 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. 4, 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 2 and the arms 4 to 9. Among these, the coordinate system of the base 2 installed on the floor surface (indicated by coordinate axes XYZ in FIG. 1) is a non-moving coordinate system and is set as robot coordinates (reference coordinates). 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 arm 8 on the coordinates of the second upper arm 7, the coordinate position of the flange 9 on the coordinates of the wrist arm 8, Various parameters such as a length of 9 are stored.

  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 a position of the robot tip (including the posture; the same applies hereinafter) is given, the rotation of the arms 4 to 9 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 3 is created by a teaching pendant (operation program creation 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 operation control of the robot arm 3, 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.

  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.

  Now, as shown in FIG. 2, a circular rail 19 surrounding the base 2 is attached to the upper portion of the base 2, and a traveling vehicle (running body) 20 is placed on the rail 19. The traveling vehicle 20 has wheels 21 that roll on the rails 19, and the wheels 21 are rotationally driven by a wheel drive motor 22 mounted on the traveling vehicle 20 to cause the traveling vehicle 20 to travel.

  In order to detect the current position of the traveling vehicle 20, a ring-shaped perforated plate 23 surrounding the base 2 is attached to the base 2. On the other hand, the traveling vehicle 20 is provided with a light projecting element 24 made of, for example, a light emitting diode and a light receiving element 25 made of a photodiode, with the porous plate 23 interposed therebetween.

  As shown in FIG. 3, a large number of small holes 23 a are formed in the perforated plate 23 along the circumferential direction, and light emitted from the light projecting element 24 is received by the light receiving element 25 through the small holes 23 a. When the traveling vehicle 20 moves, the light receiving element 25 intermittently receives the light emitted from the light projecting element 24 and outputs a pulsed light receiving signal. Such a light projecting element 24 and a light receiving element 25 constitute a position detector (position sensor) 26 that detects the position of the traveling vehicle 20.

  The traveling vehicle 20 is equipped with a laser radar 27 as an exploration device. The laser radar 27 includes a laser that emits laser light (exploration signal) in a pulsed manner and a light receiver that receives reflected light of the emitted laser light. The laser radar 27 is disposed so as to emit laser light obliquely upward at a predetermined elevation angle γ as shown in FIG. The laser beam is indicated by L in FIG.

  The traveling vehicle 20 and the base 2 are connected by a flexible cable 28 shown in FIG. The flexible cable 28 supplies power to devices mounted on the traveling vehicle 20 such as the wheel drive motor 22, the position detector 26, and the laser radar 27, and transmits and receives signals between the mounted devices and the control device 12. Is for. A winder 29 that winds up the flexible cable 28 is provided in the traveling vehicle 20. The winder 29 is motor-driven, and the winding amount of the flexible cable 28 corresponding to the position of the traveling vehicle 20 is set. Thereby, the flexible cable 28 is always wound up and rewound with an appropriate slack.

  The perforated plate 23, the light projecting element 24, the light receiving element 25, and the control device 12 constitute a traveling position detecting means for detecting the position of the traveling vehicle 20, and the light reception pulse signal output from the light receiving element 25 is a control device. 12 is input. Then, the control device 12 detects the position of the traveling vehicle 20 by counting the number of received light pulse signals from the light receiving element 25.

  That is, the rail 19 of the traveling vehicle 20 has a circular shape centered on the Z axis of the robot coordinates. In the present embodiment, the Z axis coincides with the rotation center line of the shoulder portion 4. The position of the traveling vehicle 20 is determined by an angle between a vertical line that is lowered from the reference position to the Z axis and a perpendicular line that is lowered from the current position of the traveling vehicle 20 to the Z axis. It shall be expressed. Then, by counting the number of received light pulses from the reference position as described above, it is possible to detect how many times the traveling vehicle 20 is currently rotated around the base 2 from the reference position.

  When the traveling vehicle 20 rotates around the base 2 within a predetermined range of 360 degrees or less, in this embodiment, 360 degrees, that is, one round, the control device 12 reverses the wheel drive motor 22 to move the traveling vehicle 20 in the opposite direction. It is supposed to be moved to. Accordingly, the traveling vehicle 20 repeatedly moves in the forward and reverse directions around the base 2 one by one. This 360-degree movement around the base 2 is assumed to be one cycle of the traveling vehicle 20.

  Incidentally, even when the traveling vehicle 20 moves in the forward / reverse direction, the control device 12 detects how many times the traveling vehicle 20 is rotated from the reference position in which direction depending on the rotation direction of the wheel drive motor 22. Is something that can be done.

  In addition, the emission direction of the laser light of the laser radar 27 is determined to be a predetermined angle, for example, the same direction, with a straight line connecting the Z axis and the laser when viewed from directly above. In addition, the reference position of the traveling vehicle 20 is such that the angle between the X axis and the Y axis of the robot coordinates is a predetermined angle, for example, the angle formed with the X axis when viewed from directly above is 0 degree (matches the X axis) ) Is defined to be 90 degrees with the Y axis.

  When an object is detected by the laser radar 27, the distance from the laser radar 27 to the object is detected at a distance A along the laser beam emission direction as shown in FIG. B = A · cos γ can be obtained. Since the radius d of the movement locus of the laser radar 27 is known, the horizontal distance D from the Z axis of the robot coordinates to the detected object can be obtained by D = (B + d).

  Therefore, when the object is detected by the laser radar 27, the position of the object can be grasped by the XY coordinate values of the robot coordinates based on the position of the traveling vehicle 20 at that time and the distance from the laser radar 27. That is, when the current position of the traveling vehicle 20 is θ degrees from the reference position (the angle formed with the X axis is θ degrees), the XY coordinate values are x = D · cos θ and y = D · sin θ. Can be sought.

Next, the operation of the above configuration will be described with reference to the flowcharts of FIGS. When the power of the robot apparatus is turned on and a start operation is performed, first, the control device 12 causes the traveling vehicle 20 to repeatedly perform one cycle traveling around the forward direction and one cycle traveling around the reverse direction from the reference position. While the traveling vehicle 20 is traveling, the laser radar 27 intermittently emits laser light to detect an object.
While the traveling vehicle 20 is moving, the control device 12 stores the time when the light receiving element 25 outputs the light reception signal in, for example, the RAM 15 as storage means for each light reception signal. The time when this light reception signal is output is stored for at least the previous one-cycle running and the current one-cycle running.

  In the one-cycle traveling of the traveling vehicle 20, when the control device 12 detects the presence of an object from the light reception signal of the laser radar 27, the control device 12 calculates the current position from the position of the traveling vehicle 20 and the distance from the laser radar 27 to the object. The position of the detected object is calculated as the position on the XY coordinates of the robot coordinates. And the control apparatus 12 memorize | stores the object detection result containing the position data of all the objects detected in 1 period driving | running | working of the traveling vehicle 20 in RAM15 as 1 period position data (position data acquisition means). The position of the traveling vehicle 20 and the distance of the object from the laser radar 27 may be stored and calculated as a position on the XY coordinates when necessary.

  When the one-cycle position data is acquired as described above, the control device 12 enters the data processing routine of FIG. 5 and compares the one-cycle position data acquired this time with the predetermined basic position data stored in the RAM 15 in advance. (Step A1 in FIG. 5; first comparison means). The basic position data is, for example, one-cycle position data acquired in a state where there is no person around the robot body 1. The basic position data may be the first one-cycle position data after the power is turned on.

  As a result of comparing the one-cycle position data and the basic position data, when there is no difference between the two data (“NO” in step A1), the control device 12 turns off the moving object detection flag (step A4), and returns. Become. Further, when there is a difference between the one-cycle position data and the basic position data (“YES” in step A1), the control device 12 detects an intruding object in the detection area of the laser radar 27. The previous one-cycle position data is compared with the current one-cycle position data (step A2; second comparison means).

  As a result of comparing the previous one-cycle position data with the current one-cycle position data, when there is no difference between the two data (“NO” in step A2), the control device 12 uses the current one-cycle position data as the basic position. The data is replaced (step A5; basic position data conversion means), the moving body detection flag is turned off (step A6), and the process returns.

  When the previous one-cycle position data is compared with the current one-cycle position data and there is a difference between the two data (“YES” in step A2), the control device 12 moves the detected object. As a result, the moving object detection flag is turned on (step A3), and the process returns. Hereinafter, a detected object that is determined to be moving is referred to as a moving object.

  The above will be specifically described. In order to replace a moving body, for example, a wagon (part supply wagon) on which a worker has a supply component, with a wagon that has been emptied as a result of the assembly work by the robot arm 3, it is within the detection area of the laser radar 27. Suppose you have brought it in. Then, since the one-cycle position data acquired this time includes parts supply wagons and workers' position data that did not exist so far, both the basic position data and the previous one cycle This is different from the position data. Therefore, “YES” is determined in step A1, “YES” is determined in step A2, and the moving object detection flag is turned on in step A3.

  An operator pushes the parts supply wagon toward the empty wagon to replace the parts supply wagon with the empty wagon, and pushes the empty wagon to go out of the detection area of the laser radar 27. Since the workers and wagons are moving until they go, the 1-cycle position data acquired one after another is different from the basic position data and the previously acquired 1-cycle position data. Keeps on.

  When the worker pushes the empty wagon and goes out of the detection area of the laser radar 27, the moving object disappears. Immediately thereafter, the one-cycle position data newly acquired is compared with the basic position data. At this time, if the parts supply wagon is placed at the same position as the empty wagon, the newly acquired one-cycle position data matches the basic position data ("NO" in step A1), and therefore the movement The body detection flag is turned off (step A4).

  However, when replacing a parts supply wagon with an empty wagon, the position of the empty wagon is often somewhat different. In this case, the one-cycle position data newly acquired immediately after the worker presses the empty wagon and goes out of the detection area of the laser radar 27 is different from the basic position data. This is also different from the position data (the position data is just before the worker presses the empty wagon and goes out of the detection area of the laser radar 27, so there is position data of the worker and the empty wagon). The moving object detection flag is not turned off and remains on ("YES" in step A2, step A3).

  However, the current one-cycle position data newly acquired is different from the basic position data (“YES” in step A1), but matches the previous one-cycle position data (in step A2). “NO”), the previous basic position data is erased from the RAM 15, and the latest one-cycle position data acquired this time is stored in the RAM 15 as new basic position data (step A5; basic position data conversion means) and moved. The body detection flag is turned off (step A6). Then, since the one-cycle position data acquired next coincides with the new basic position data (“NO” in step A1), the moving object detection flag continues to be turned off.

  Therefore, unlike the case where the moving object detection flag is turned on or off based only on the comparison result between the current one-cycle position data and the basic position data, the actual position of the object is merely changed slightly. In spite of no intrusion, it is possible to prevent the occurrence of a problem such as performing an extra operation of comparing the difference with the previous one-cycle position data. This means that the processing time of the control device 12 is shortened. As a result, the processing of the control device 12 with respect to the operation of the robot arm 3 can be executed with a margin, and the tact time of the robot (one work cycle to be repeated). Can be prevented from becoming longer.

  Laser light is emitted from the laser radar 27 at a predetermined elevation angle γ. As a result, the upper body is irradiated with laser light to a person who has entered the detection area of the laser radar 27. Since humans, particularly those who enter the detection area, are factory workers, their heads and hands are constantly moving even if they remain in place. For this reason, even if a person is standing at the same position, the previous one-cycle position data differs from the current one-cycle position data as the upper body moves, so that the moving object detection flag is kept on. ("YES" in step A1, "YES" in step A2, step A3). For this reason, a person and a thing can be distinguished more correctly and it can prevent judging that a person is not a moving body.

  When the processing of the one-cycle position data as described above is completed, the control device 12 repeatedly executes the data processing routine in FIG. 5 and the in-stop processing routine shown in FIG. 6 alternately. When entering the stop processing routine of FIG. 6, the control device 12 first determines whether or not the moving object detection flag is turned on (step B1). If the moving body detection flag is off (“NO” in step B1), the control device 12 permits the operation (step B7) and causes the robot arm 3 to start the operation according to the operation program.

In the following description, the position, operation direction, and speed of the robot arm 3 refer to the position, operation direction, and speed of the flange 9 that is the tip of the arm.
When the moving body detection flag is on (“YES” in step B1), the control device 12 refers to the operation program to detect the operation direction at the start of the operation of the robot arm 3, and the direction in which the moving body exists. If the robot arm 3 does not operate ("NO" in step B2), the operation is permitted (step B7), and the robot arm 3 starts the operation according to the operation program.

  If the operation direction at the start of the operation of the robot arm 3 is the direction in which the moving body exists (“YES” in step B2), the control device 12 then refers to the operation program of the robot arm 3 to determine the operation direction. In addition to acquiring the speed, the moving direction and speed of the moving body are acquired, and the position where the robot arm 3 and the moving body collide and the time from the start of the operation to the collision are calculated (step B3).

  The speed of the robot arm 3 is obtained from a speed pattern determined by the operation program. For example, when the speed pattern of the robot arm 3 is a trapezoidal pattern as shown in FIG. 9A, the acceleration α, the maximum speed V, and the deceleration −α are described in the operation program. Then, the speed after t hours from the start of the operation is calculated. The distance (position) moved during the time t from the start of the operation is obtained by integrating the speed so far, that is, the area of the speed pattern up to the time t may be calculated.

  The moving direction and speed of the moving body are obtained by comparing the current one-cycle position data with the previous one-cycle position data. That is, it is assumed that the moving direction of the moving body is a straight line connecting the previous position and the current position of the moving body and moves on the straight line from the previous position toward the current position. In addition, the speed of the moving object is obtained from the distance from the previous position to the current position, and this distance is detected by the moving object from the current one-cycle position data after the moving object is detected from the previous one-cycle position data. It can be obtained by dividing by the time up to when it was done.

  Note that the distance from the previous position of the moving body to the current position is the distance from the Z axis to the position of the previous moving body, the distance from the Z axis to the position of the current moving body, and the distance from the Z axis to the previous position. It can be obtained from the angle formed by the straight line drawn to the position of the moving body and the straight line drawn from the Z axis to the position of the moving body this time. The time from when the moving body is detected based on the previous one-cycle position data to when the moving body is detected based on the current one-cycle position data is the time when the light receiving element 25 outputs the light reception signal at the position of the current moving body. From the time when the light receiving element 25 outputs the light reception signal at the position of the previous moving body, the difference can be obtained.

As a result of calculating the position where the robot arm 3 and the moving body collide and the time from the start of the operation to the collision, if there is no collision (“NO” in step B4), the control device 12 permits the operation of the robot arm 3. Start the operation.
If there is a collision, the control device 12 sets the speed of the robot arm 3 at a safe speed lower than that, for example, 25 mm / sec or less recognized by ISO 10218, regardless of the speed pattern determined by the operation program. When the arm 3 is operated, it is determined whether or not it collides with a moving body. If the collision can be avoided at a safe speed (“YES” in step B5), the speed of the robot arm 3 is set to the safe speed. The operation is permitted (step B6).

If the collision cannot be avoided even if the safe speed is set (“NO” in step B5), the control device 12 prohibits the operation of the robot arm 3 (step B8).
When the operation of the robot arm 3 is started, the control device 12 repeatedly executes the data processing routine of FIG. 5 and the in-operation processing routine of FIG. 7 alternately. In the in-operation processing routine of FIG. 7, the control device 12 sets various collision avoidance measures (collision avoidance measure setting means) in order to avoid a collision with the moving body according to the situation.

In the flowchart of FIG. 7, the control device 12 determines whether or not the moving object detection flag is turned on (step C1). If the moving object detection flag is turned off (“NO” in step C1). The operation becomes a return and the operation of the robot arm 3 is continued.
When the moving body detection flag is ON (“YES” in step C1), the control device 12 acquires the operation direction and speed of the robot arm 3 in the same manner as described above, and moves and moves the moving body. And the position where the robot arm 3 and the moving body collide and the time until the collision are calculated (step C2).

As a result of the calculation, when the robot arm 3 and the moving body do not collide (“NO” in step C3), the control device 12 continues the operation of the robot arm 3 as it is.
When the robot arm 3 and the moving body collide (“YES” in Step C3), the control device 12 determines whether or not the collision with the moving body can be avoided by decelerating the robot arm 3 (Step S3). C4).

  For example, when the movement direction of the robot arm 3 and the moving direction of the moving body intersect and a result of collision between the two at the intersecting position is obtained, the moving body intersects by reducing the speed of the robot arm 3. If the robot arm 3 reaches the crossing position after passing through the position, the two will not collide. However, if the moving direction of the robot arm 3 and the moving direction of the moving body overlap, they will collide even if the speed of the robot arm 3 is reduced.

  If the collision can be avoided by decelerating the robot arm 3 (“YES” in step C4), the control device 12 calculates a speed at which the collision can be avoided and decelerates the robot arm 3 until the speed is reached (step C8; First collision avoidance measure). The deceleration is performed by generating a reverse torque in the servo motor 3 to brake the robot arm 3 (braking means). The braking means is not limited to generating reverse torque in the servo motor 3, but may be other means such as operating a mechanical brake.

  If the collision cannot be avoided even by decelerating the robot arm 3 (“NO” in step C5), the control device 12 determines whether or not the collision can be avoided by an emergency stop (step C5). The emergency stop is, for example, an operation of causing the servo motor 3 to generate a maximum reverse torque and rapidly stopping the robot arm 3 at an allowable maximum deceleration. This allowable maximum deceleration is stored in advance in storage means, for example, the ROM 14.

  Whether or not a collision can be avoided by an emergency stop is first determined by detecting the current speed of the robot arm 3 and moving to a stop (braking distance) when decelerating at the maximum allowable deceleration at this speed. Find s. Then, if the moving object exists within the braking distance s, it is determined to collide, and if it is outside the braking distance s, it is determined not to collide. The braking distance s can be obtained from the area of a triangle with diagonal lines when the current speed is V and the allowable maximum deceleration is −α in FIG. 9A. In order to more reliably avoid the collision, the braking distance s may be obtained on the assumption that the current speed is not the actual speed but the maximum allowable speed.

If the collision can be avoided by the emergency stop (“YES” in step C5), the control device 12 causes the robot arm 3 to stop urgently (step C9).
When the collision cannot be avoided even by the emergency stop (“NO” in Step C5), the control device 12 changes the trajectory in a safe direction to avoid the collision (Step C6), and performs an emergency while operating in the safe direction. Stop (step C7).

  Here, the safe direction means the same direction as the moving direction of the moving body in the present embodiment. Specifically, as shown in FIG. 8A, the direction in which the robot arm 3 moves toward the target position Q indicated by the operation program is indicated by a straight line L1, and the moving direction of the moving body R is indicated by a straight line L2. 8B, as shown in FIG. 8B, an arbitrary position Q1 further away from the moving body R than the straight line L1 is set as an avoidance target position, and a trajectory that reaches the avoidance target position Q1 is calculated. The robot arm 3 is moved along the trajectory while making an emergency stop. In this case, in order to make the moving direction closer to the moving direction of the moving body R earlier in order to avoid the collision of the robot arm 3, the avoidance target position Q1 is set closer to the robot arm 3 than the straight line L2 including the straight line L2. It is preferable.

By determining the avoidance target position in this manner, the robot arm 3 gradually stops in an emergency direction while changing its direction in the same direction as the moving direction of the moving body R, so that a collision with the moving body can be avoided.
That is, since the robot arm 3 can be urgently stopped while being moved away from the moving body, a collision with the moving body can be avoided.
For example, when the robot arm 3 collides with a moving body, the moving direction of the moving body is assumed to be a linear direction connecting the position in the previous one-cycle position data and the position in the current one-cycle position data. Actually, however, the direction may be changed to move away from the robot arm 3 afterwards, or the direction may be changed in a direction approaching the robot arm 3. If the direction is changed in the direction away from the robot arm 3, in FIG. 8 (a), when the robot arm 3 is moved in the direction closer to the robot arm 3 on the side of the robot arm 3 than the straight line L2, There is a risk that the robot arm 3 and the moving body collide.

  On the other hand, in this embodiment, since the robot arm 3 changes direction so as to move away from the moving body R toward the avoidance target position Q1, the moving body moves in the direction of the robot arm 3 in FIG. Even if the robot arm 3 is changed, the robot arm 3 moves in a direction away from the moving body, so that it is possible to more reliably avoid a collision with the moving body.

  As described above, according to the present embodiment, the laser radar 27 for detecting the moving body is mounted on the traveling vehicle 20, and the traveling vehicle 20 is movably provided on the rail 19 attached to the base 2. Therefore, if the robot body 1 is installed on the floor, the installation of the laser radar 27 can be completed at the same time. Therefore, in addition to the installation of the robot body 1, additional work such as installing the laser radar 27 at a required position in the factory. Do not need.

  Moreover, since the moving body can be detected by mounting the laser radar 27 on the traveling vehicle 20 and emitting the laser beam from the laser radar 27 while moving the traveling vehicle 20, a CCD camera attached to the ceiling of the factory can be used. The detection of a moving body can be easily performed without requiring advanced image analysis as in the case of detection.

  Further, in the present embodiment, even if the current one-cycle position data is different from the basic position data, if the current one-cycle position data matches the previous one-cycle position data, the movement and other detection flag is not turned on. As a result, the current one-cycle position data is adopted as the basic position data and replaced with the previous basic position data ("YES" in step A1, and in step A2). “NO”, step A5).

  On the other hand, when the moving object detection flag is turned on only because there is a difference between the current one-cycle position data and the basic position data, for example, when the parts supply wagon is replaced with an empty wagon, If the parts supply wagon is not placed exactly at the position where there was an empty wagon, all subsequent 1-cycle position data will be different from the basic position data. As a result, even after the parts supply wagon is placed, it will move. The body detection flag is kept on.

  In such a case, in the present embodiment, even if the current one-cycle position data is different from the basic position data, it matches the previous one-cycle position data. Then, the current one-cycle position data is replaced with basic position data. For this reason, since the next one-cycle position data coincides with the basic position data, the moving object detection flag is not turned on, and the moving object detection flag is not kept on forever.

  In the present embodiment, when the moving body is detected while the robot arm 3 is stopped, it is a matter of course that the operation is not started when there is a risk of collision if the robot arm 3 is operated. If there is no fear of a collision, the robot arm 3 starts its operation. If it is at a safe speed, the robot arm 3 starts its operation at a safe speed. It is possible to prevent collision while preventing the operation prohibition of 3 as much as possible.

  In addition, when a moving body is detected during the operation of the robot arm 3, first, the collision is avoided by deceleration, and when the collision cannot be avoided by deceleration, the collision is avoided by braking and stopped, and the braking is applied. When the collision cannot be avoided by stopping, the moving direction of the robot arm 3 is changed so as to be the same as the moving direction of the moving body while stopping the robot arm 3, so that the robot arm 3 can move more reliably. Can be prevented from colliding with.

The present invention is not limited to the embodiment described above and shown in the drawings, and can be expanded or changed as follows.
The exploration device is not limited to the laser radar 27, and may be a radar that measures the distance and direction to an object by transmitting a radio wave (exploration signal) and measuring the reflected wave.
The robot arm is not limited to a six-axis type, and is not limited to a horizontal articulated type.
The driving source of the moving body may be a linear motor.
The traveling vehicle 20 is not limited to reciprocating around the base 2 and may be one that moves in one direction.
Depending on the operating range of the robot arm 3, the moving range of the traveling vehicle 20 may be less than 360 degrees around the base 2.
The position data replaced with the basic data in step A5 is not limited to the current one-cycle position data, but may be the previous one-cycle position data.

  In the drawings, 1 is a robot body, 2 is a base, 3 is a robot base, 12 is a control device (position data acquisition means, movement position detection means, first comparison means, second comparison means, basic position data conversion means, Collision avoidance measure setting means, robot control means), 23 is a perforated plate (moving position detecting means), 24 is a light projecting element, 25 is a light receiving element, 26 is a position detector (moving position detecting means), and 27 is a laser radar ( Exploration device).

Claims (2)

A robot body which is configured by providing a robot arm on a base, and the base is fixed to an installation floor;
A traveling body that is provided so as to be able to travel around the base, and that repeats the one-cycle traveling as a one-cycle traveling within a predetermined range of 360 degrees or less around the base;
Traveling position detecting means for detecting the position of the traveling body;
An exploration device that is mounted on the traveling body, emits an exploration signal obliquely upward, detects an object existing in the emission direction of the exploration signal, and measures a distance to the detected object;
The position data of the object existing around the base is acquired from the position of the traveling object detected by the traveling position detection means and the distance to the detected object by the exploration device for each cycle of the traveling object. Position data acquisition means;
First comparing means for comparing the position data acquired by the current one-cycle traveling of the traveling body and predetermined predetermined basic position data;
When the first comparison means determines that there is a difference between the position data and the basic position data from the current one-cycle travel, the position data from the current one-cycle travel and the previous one cycle Second comparison means for comparing the position data obtained by running;
When it is determined by the second comparison means that there is no difference between the position data from the current one-cycle travel and the position data from the previous one-cycle travel, the current or previous one-cycle travel is performed. Basic position data conversion means for replacing position data as the basic position data;
When it is determined by the second comparison means that there is a difference between the position data from the current one-cycle driving and the position data from the previous one-cycle driving, the detection object is detected as moving. A collision avoidance measure setting means for setting a collision avoidance measure for avoiding a collision with an object;
When the first comparison means determines that there is no difference between the position data and the basic position data from the current one-cycle travel, the robot arm is caused to execute an operation according to an operation program, and the collision avoidance measure A robot apparatus comprising: robot control means for causing the robot body to execute the collision avoidance measure when the setting means sets the collision avoidance measure. The collision avoidance measure setting means includes:
The moving speed and moving direction of the detected object are calculated from the difference between the position data obtained from the current one-cycle running and the position data obtained from the previous one-cycle running, and the detected movement direction and moving speed are then detected. When the object moves, when the detected object collides with the robot arm that operates according to the operation program, a first collision avoidance measure for reducing the speed of the robot arm is set.
If a collision cannot be avoided by the first collision avoidance measure, a second collision avoidance measure is set to perform a stop operation for braking until the robot arm is stopped.
When the second collision avoidance measure cannot avoid a collision, the third collision avoidance measure is configured to gradually change the moving direction of the robot arm to be the same as the moving direction of the detected object while stopping the robot arm. The robot apparatus according to claim 1, wherein:

Images