JP2011201009A - Robot system - Google Patents

Abstract

PROBLEM TO BE SOLVED: To reduce a risk of a collision in returning a robot from its stop position to a predetermined standby position when the robot is stopped in a state of having no failure.SOLUTION: In a normal operation, an angle command generating part 27 generates rotation angle commands for each motor M at each relatively-short generation period, and outputs a part of the generated rotation angle commands to a control part 26 at each relatively-long output period. An angle command storing part 28 sequentially stores all the rotation angle commands generated by the angle command generating part 27. In a return operation, the angle command generating part 27 sequentially reads out the rotation angle commands stored in the angle command storing part 28 from the new one to the old one, and outputs the read-out rotation angle commands as return angle commands at each output period. The control part 26 performs the feedback control of the drive of each shaft by the motor M so that the rotation angles of each shaft coincide with the return angle commands.

Description

  The present invention relates to a robot system having a function of performing an operation of returning a robot from a stop position to a predetermined standby position when the robot is stopped without any abnormality.

  For example, when a robot is used in a production line, etc., even if there is no abnormality in the robot, there is an abnormality in other equipment that operates in cooperation with the robot (conveyor stop, parts that become workpieces do not flow, etc.) There are cases where the operation must be stopped. In such a case, the robot must be returned from the stop position to a predetermined standby position for the following reason. In other words, when other equipment is reset (returned to a predetermined standby position), if only the robot has advanced to the stop position, the operation of the other equipment and the robot cannot be matched. End up.

  Currently, as a method of returning the robot from the stop position to the predetermined standby position, a method for moving the robot manually by a user or an operation program for returning the robot from the stop position to the standby position is prepared and stopped when the robot stops. A method of selecting and starting an operation program corresponding to the position and moving the robot is used.

However, in the manual operation method, an accurate operation is required to move the robot to the standby position without colliding with an object such as equipment arranged around the robot. For this reason, a user's work burden increases. Further, in the method using the operation program for returning to the standby position, it is necessary to prepare operation programs as many as the number of assumed robot stop positions in advance. Furthermore, in reality, there may be a case where the robot stops at an unexpected position. In this case, there is a possibility that the robot cannot be returned to the standby position by using an operation program prepared in advance.
On the other hand, Patent Document 1 discloses a technique for automatically returning the hand to a position where it does not collide with a foreign object when it is determined that the robot has collided with the foreign object.

JP-A-5-84681

  When the robot stops due to an abnormality in the equipment, the hand position of the robot can be automatically returned to a predetermined standby position (retreat position) by using the technique described in Patent Document 1. However, if only the technique described in Patent Document 1 is used, there is a risk that a collision occurs during the return operation. This is because the robot has a concept of posture, and the hand position does not simply have to return to the standby position. That is, if the robot is stopped after performing a predetermined motion in the advance direction and then simply performing the predetermined motion in the return direction, the posture of the robot may be different from that when performing the predetermined motion in the forward direction. For example, when performing a predetermined action in the advance direction, the elbow part is folded away from the surrounding equipment, but when performing a predetermined action in the return direction, It is also conceivable that the elbow part protrudes so as to approach the equipment. When the postures are different in this way, there is a risk that the elbow portion of the robot may collide with surrounding equipment when performing a predetermined motion in the return direction.

  The present invention has been made in view of the above circumstances, and its purpose is to perform an operation of returning the robot from the stop position to a predetermined standby position when the robot is stopped without any abnormality in the robot itself. It is an object of the present invention to provide a robot system that can reduce the risk of collisions.

  According to the first aspect of the present invention, when performing a normal operation (advancing operation), the control unit matches the rotation angle of the plurality of axes provided in the robot with the rotation angle command output from the angle command generation unit. In this way, normal control for feedback control of driving of each axis by the drive unit is executed. Here, the rotation angles of the plurality of axes are detected by a rotation angle detector. The angle command generator generates a rotation angle command for each generation cycle, and outputs a part of the rotation angle command for each output cycle longer than the generation cycle. The angle command storage unit sequentially stores all the rotation angle commands generated by the angle command generation unit during the period in which the normal control is being executed.

  The reason for this is as follows. In other words, at present, the rotation angle command is usually generated by software processing (processing that depends only on electrical operation), and can therefore be generated every generation cycle that is an extremely short interval. . On the other hand, the drive unit, the rotation angle detection unit, and the like drive the robot axis and detect the rotation angle of the robot by hardware processing (processing including mechanical operation). The operation interval for matching the rotation angle command must be longer than the generation cycle. For this reason, it is necessary to adopt either to make the generation period of the rotation angle command longer in accordance with the operation interval, or to output the rotation angle command generated in the shorter generation cycle in accordance with the operation interval. is there. This means adopts the latter method because it is necessary to use a rotation angle command generated in a short generation cycle in retrospective control described later.

  Now, as explained in the prior art, if an abnormality occurs in other equipment that operates in cooperation with the robot, even if there is no abnormality in the robot, the normal control is stopped and the robot is stopped. The operation to do is made. When the robot is stopped in this way, it is necessary to perform an operation (backward operation) for returning the robot from the stop position to a predetermined standby position. In such a case, for example, a retroactive start command is input via the retroactive command input unit by a predetermined operation by the user or an operation of a program that automatically starts when the robot stops as described above. Then, the angle command generation unit sequentially reads out the rotation angle commands stored in the angle command storage unit from the new one to the old one, and outputs the read rotation angle commands as the retroactive angle command for each output cycle. That is, a log obtained by rearranging the rotation angle logs of all axes during the advance operation in the reverse order is output as the retroactive angle command.

  The control unit executes retroactive control for feedback control of driving of each axis by the driving unit so that the rotation angle of each axis matches the retrospective angle command. As a result, the backward operation is performed while the rotation angles of all the axes of the robot take the same value as the advance operation. In other words, the backward movement is performed so that the robot traces the movement locus during the advance movement in the opposite direction. That is, since a so-called playback operation is performed, the backward operation is performed while maintaining the same posture as that of the advance operation. By such a retroactive operation, the robot returns to a predetermined standby position, whereby a retroactive end command is input via the retroactive command input unit. Then, the angle command generation unit stops the output of the retroactive angle command, and the control unit stops the execution of the retroactive control. As described above, according to this means, it is possible to reduce the risk of a collision that occurs due to a change in the posture of the robot when a backward operation is performed to return the robot from the stop position to the predetermined standby position.

  Moreover, the retroactive angle command used for control of the retroactive operation is obtained by outputting all the rotational angle commands generated for each generation cycle for each output cycle. In other words, since a rotation angle command for instructing a rotation angle for each relatively short generation cycle is output for each relatively long cycle, the rotation speed of each axis during the backward operation, and hence the robot operation speed, is compared to that during normal operation. Will be late. In this way, the robot operation is slower (slow operation) in the backward operation than in the normal operation, and this is the case when other equipment around the robot moves unexpectedly. However, the user can deal with the movement with a margin. For this reason, it is possible to reduce the risk of a collision that occurs for the first time when the retroactive action is performed.

  When the robot stops due to, for example, collision with peripheral equipment, the following problems occur when trying to execute the retroactive control in the means according to claim 1. That is, when stopping due to a collision, the rotation angle of each axis at the time of stopping is often a value far from the rotation angle command (backward angle command). For this reason, it is difficult to perform retroactive control normally. Further, even if the retroactive control can be executed successfully, the robot will again pass the trajectory when it collides with surrounding equipment. That is, there is a very high possibility that the robot will collide with surrounding equipment again. Therefore, when stopping due to a collision, it is desirable to remove the cause of the collision after completely stopping the operation of each facility including the robot without executing the retroactive control. Therefore, it is conceivable to adopt the means described in claim 2.

  According to the means of claim 2, when the retroactive start command is input, the control unit receives the latest rotation angle command stored in the angle command storage unit and the current rotation angle detected by the rotation angle detection unit. Find the difference between When the absolute value of the difference is equal to or less than a predetermined error value, retroactive control is executed. On the other hand, when the absolute value of the difference exceeds a predetermined error value, retroactive control is not executed. Even when the robot is operating normally, the rotation angle and the rotation angle command may not completely match due to the influence of various errors. On the other hand, when the robot stops due to a collision, as described above, the rotation angle and the rotation angle command are greatly different values. Therefore, when the absolute value of the difference between the rotation angle and the rotation angle command exceeds a predetermined error value, the control unit determines that the robot has stopped due to a collision and does not execute retroactive control. ing. Thereby, it is possible to suppress a risk that the robot re-collises by performing the backward operation.

  According to a third aspect of the present invention, there is provided collision detection means for detecting a robot collision. As the robot collision, a collision during an automatic operation, a collision during teaching (teaching), or an operation confirmation can be considered. In the former case, the robot is likely to be operating at the maximum speed, and the speed at the time of collision (collision speed) is relatively fast. For this reason, there is a high possibility that the mechanical parts and peripheral equipment of the robot will be greatly damaged, and the subsequent automatic recovery becomes difficult. In such a case, since the deviation between the rotation angle of each axis and the rotation angle command at the time of stopping due to the collision is often excessive, it is difficult to execute the retroactive control itself.

  On the other hand, in the latter case, since the robot operates at a low speed, the collision speed is relatively low. For this reason, the mechanical parts and peripheral equipment of the robot are hardly damaged, and the subsequent automatic recovery is possible. In such a case, since the deviation between the rotation angle of each axis and the rotation angle command at the time of stopping due to the collision is often relatively small, the retroactive control can be executed.

  Of the two types of collisions described above, the latter collision with a lower collision speed has a higher occurrence frequency than the former collision with a higher collision speed. This is because during automatic operation, the robot repeatedly performs an action determined based on the action program. The repeated operation is sufficiently verified in advance so as not to collide with peripheral equipment. For this reason, the possibility of collision during automatic operation is extremely low. On the other hand, during teaching and operation confirmation, teaching and operation confirmation are performed also as collision verification with peripheral equipment, and during that time, the operation trajectory is frequently corrected. . For this reason, it is unlikely that the robot repeats the same operation every time during teaching or operation check (reproducibility of the robot operation is low). Therefore, there is a high possibility that a collision with peripheral equipment or the like will occur in a teaching or confirmation operation performed after a new motion trajectory is generated or after the motion trajectory is corrected. In this way, a lighter collision that has a slower collision speed and can execute retroactive control has a higher occurrence frequency than a severe collision that has a high collision speed and cannot perform retroactive control. For this reason, it is desirable to execute retroactive control to move the robot to the standby position even when the robot stops due to a slight collision.

  However, although it is a minor collision, increasing the number of collisions unnecessarily is not preferable for a robot that requires precise movement. This is because even if the collision is slight, there is a risk that the accuracy of the calibration and the frame performed to accurately operate the robot may be distorted by repeating the collision.

  Therefore, in this means, when the robot stops due to a slight collision, retroactive control is executed after adding a device for preventing the occurrence of re-collision as follows. That is, when a collision is detected by the collision detection unit, the control unit specifies a point in time when the collision occurs (collision timing) and a position at the time of the collision (collision position). The control unit sets a spherical entry prohibition space using the identified collision position and collision timing. That is, the center of the entry prohibition space is set to the collision position. The radius of the entry prohibition space corresponds to the distance between the rotation angle command output at the output timing closer to the collision timing and the collision position among the rotation angle command output timings before and after the collision timing. Set to length. And a control part will perform retroactive control, if the retroactive start command is input in the state by which the collision was detected.

  Now, when performing retroactive control after the robot has been stopped due to a collision, if the robot is operated retroactively based on the retroactive angle command output by the angle command generator, the trajectory of the operation must pass through the collision position. become. Therefore, there is a high possibility that the robot will collide again. Therefore, when a retrograde start command is input in a state where a collision has been detected, the angle command generator generates a trajectory of the robot's retroactive motion based on the rotation angle command read from the angle command storage unit. The rotation angle command read so that the motion trajectory is outside the entry prohibition space and along the surface of the sphere of the entry prohibition space between the outer contour position that overlaps first and the outer contour position that overlaps last. And the rotation angle command after the change is output as a retroactive angle command.

  In this way, after stopping due to the collision, the robot performs a backward movement so as to follow the movement locus during the forward movement in the opposite direction until reaching the outline of the entry prohibition space. Then, after avoiding the collision position along the outline of the entry-prohibited space, the backward operation is performed so that the operation locus of the advance operation is traced in the reverse direction. Since the radius of this entry prohibition space is set as described above, it can be said that it is very small. For this reason, when performing an action along the outline of the prohibited entry space, the possibility that a new collision such as another part of the robot's elbow collides with the surrounding equipment increases, that is, another new collision occurs. The robot's posture will not change to a level where it will occur. That is, the posture of the robot when performing a backward movement along the outline of the entry prohibition space is almost the same as the posture at the time of the forward movement in which the collision has occurred, and there is no change in the posture that causes another new collision.

  In this way, in the backward operation after the collision, the collision position is avoided, so that the complete playback operation is not achieved. However, as described above, the entry prohibition space is small. In addition, the route (motion trajectory) through which the robot originally passes is provided with a predetermined margin so as not to collide with peripheral equipment and the like in consideration of the physique of the robot. The physique of the robot is much larger than the entry prohibition space. For this reason, as long as the motion trajectory is changed along the outline of the entry prohibition space such as this means, the possibility that the robot will collide with the peripheral equipment beyond the margin range is extremely low. In other words, it is almost impossible for the robot to cause a new collision due to the slight change of the motion locus described above. Therefore, when the robot moves backward after the collision, it is possible to perform the backward movement while maintaining the same posture as the forward movement while reliably avoiding the collision position and preventing the occurrence of a new collision.

  According to the fourth aspect of the present invention, similar to the third aspect of the present invention, the collision detecting means for detecting the collision of the robot is provided. When a collision is detected by the collision detection unit, the control unit specifies the collision timing and the collision position and sets the entry prohibition space, similarly to the unit according to the third aspect. And a control part will perform retroactive control, if the retroactive start command is input in the state by which the collision was detected. In addition, when a retrograde start command is input in a state in which a collision is detected, the angle command generation unit changes the read rotation angle command and changes the rotation angle command after the change, similarly to the means according to claim 3. Is output as a retroactive angle command. For this reason, also by this means, the same operation and effect as the means described in claim 3 can be obtained.

  However, the entry prohibition space in this means is slightly different from the entry prohibition space in the means described in claim 3. That is, the radius of the entry prohibition space of this means is the distance between the rotation angle command output at the output timing far from the collision timing and the collision position among the rotation angle command output timings before and after the collision timing. Is set to a length equivalent to. For this reason, the radius (size) of the entry prohibition space is slightly larger than that of the means described in claim 3. In this way, for example, even if the surface of the equipment that the robot collides with is somewhat deformed due to the collision, it is possible to perform the backward operation while avoiding the deformed portion. The effect that it can prevent beforehand is acquired.

  According to the means of claim 5, when the retrograde start command is input, the angle command generator generates a new rotation angle command excluding the latest one among the rotation angle commands stored in the angle command storage unit. Read in order of oldness. The latest rotation angle command stored in the angle command storage unit is for commanding the current position when the robot stops. For this reason, when the backward angle command is used in order from the latest one as in the means described in claim 1, the robot actually starts the backward movement only when the backward angle is one before the latest one. It is the time when the command is output. That is, the start of the retroactive operation is slightly delayed. On the other hand, according to the present means, the retroactive operation can be started quickly without such a slight delay, and thus the time required for the entire retroactive operation can be shortened.

The figure which shows schematically the structure of the robot system which shows the 1st Embodiment of this invention. Electrical configuration of robot system Diagram showing generation and output timing of rotation angle command during normal operation The figure which shows an example of the rotation angle command memorize | stored in an angle command memory | storage part The figure which shows the output timing of the rotation angle command at the time of retroactive operation Block diagram equivalently showing the contents of motor control The figure which shows the state of the robot which operates according to the predetermined operation program Diagram showing an example of an operation program FIG. 1 equivalent diagram showing a second embodiment of the present invention 6 equivalent diagram Diagram showing rotation angle command, rotation angle and current value before and after collision The figure which shows the movement state of the robot before and after the collision The figure which shows the hand of the robot and its movement locus when passing the collision position The figure which shows each movement locus at the time of advance operation and backward operation near the collision position

(First embodiment)
Hereinafter, a first embodiment of the present invention will be described with reference to FIGS.
FIG. 1 shows a system configuration of a general industrial robot. A robot system 1 shown in FIG. 1 includes a robot 2, a controller 3 that controls the robot 2, and a teaching pendant 4 connected to the controller 3.

  The robot 2 is configured as, for example, a 4-axis horizontal articulated robot. The robot 2 includes a base 5 fixed to the installation surface, a first arm 6 connected to the base 5 so as to be pivotable about a vertical axis (first axis), and a tip of the first arm 6. The second arm 7 is connected to be pivotable about a vertical axis (second axis), and a shaft 8 is provided at the tip of the second arm 7 so as to be movable up and down and rotatable. ing. The axis when the shaft 8 is moved up and down corresponds to the third axis, and the axis when the shaft 8 is rotated corresponds to the fourth axis. A flange 9 is positioned and attached to the tip (lower end) of the shaft 8 so as to be detachable.

  The base 5, the first arm 6, the second arm 7, the shaft 8, and the flange 9 function as an arm of the robot 2, but the end effector (hand) is not shown on the flange 9 that is the tip of the arm, although not shown. It is attached. A plurality of shafts provided in the robot 2 are driven by motors (indicated by reference numeral M in FIG. 2) provided corresponding to the respective axes. In the vicinity of each motor, a position detector (not shown) for detecting the rotation angle of each rotation shaft is provided.

  The teaching pendant 4 is, for example, a size that can be operated by being carried by a user or carried by a hand, and is formed in, for example, a thin, substantially rectangular box shape. The teaching pendant 4 has a display unit 10 made of, for example, a liquid crystal display at the center of the surface portion. Various screens are displayed on the display unit 10. The display unit 10 is configured with a touch panel. The teaching pendant 4 is provided with various key switches 11 around the display unit 10, and the user performs various input operations using the key switches 11 and touch switches provided on the touch panel. The key switch 11 or the touch switch includes a backward start switch for instructing the start of a backward operation described later. The teaching pendant 4 is connected to the controller 3 via a cable, and performs high-speed data transfer with the controller 3 via a communication interface. The teaching pendant 4 is input by operating the key switch 11 or the like. Information such as the operation signal is transmitted from the teaching pendant 4 to the controller 3.

  FIG. 2 is a block diagram schematically showing the electrical configuration of the robot system 1. The robot 2 is provided with a plurality of motors M (only one is shown in FIG. 2) for driving each axis. The motor M (corresponding to the drive unit) is, for example, a brushless DC motor. The controller 3 includes a direct current power supply device 22 that rectifies and smoothes alternating current supplied from the alternating current power supply 21, an inverter device 23 that drives the motor M, a current detection unit 24, a rotation angle detection unit 25, A control unit 26 that performs control and the like, an angle command generation unit 27, and an angle command storage unit 28 are provided.

  The DC power supply device 22 includes a rectifier circuit 30 and a smoothing capacitor 31. The rectifier circuit 30 has a known configuration in which a diode is connected in the form of a bridge. For example, each phase output of the three-phase 200 V AC power supply 21 is connected to the AC input terminal of the rectifier circuit 30. DC output terminals of the rectifier circuit 30 are connected to DC power supply lines 32 and 33, respectively. A capacitor 31 is connected between the DC power supply lines 32 and 33.

  The inverter device 23 includes an inverter main circuit configured by three-phase full-bridge connection of six switching elements such as IGBTs (only two are shown in FIG. 1) between the DC power supply lines 32 and 33, and a drive circuit thereof. Six sets are provided (only one set is shown in FIG. 2). A free-wheeling diode is connected between the collector and emitter of the IGBT. The gate signal is given to the gate of the IGBT from the drive circuit. The drive circuit drives each IGBT by outputting a gate signal that is pulse-width modulated based on a command signal (energization command Sc) given from the control unit 26.

  The control unit 26 is configured mainly with a microcomputer including a CPU, a ROM, a RAM, an I / O, and the like. The current detector 24 converts a detection signal from a current detector (not shown) that detects a current flowing through the motor M into data that can be input to the controller 26 and outputs the data. The rotation angle detector 25 converts a detection signal from a position detector (not shown) that detects the rotation angle of the motor M into data that can be input to the controller 26 and outputs the data.

  The control unit 26 acquires the value of the current flowing through the motor M based on the data output from the current detection unit 24, and the rotation angle and rotation speed of the motor M based on the data output from the rotation angle detection unit 25. To get. Although details will be described later, the control unit 26 feedback-controls the driving of the motor M by the inverter device 23 so that the acquired rotation angle matches the command value (rotation angle command pc) given from the angle command generation unit 27. To do. Note that the current value and the rotational speed acquired in the feedback control are also used.

  In general, the industrial robot 2 performs a normal operation (advance operation) that operates in accordance with a predetermined operation program created by performing teaching or the like in advance. In addition to this, when the robot 2 according to the present embodiment stops due to a cause other than its own abnormality (cause of equipment abnormality or the like), each axis moves so as to follow the motion trajectory during the forward movement in the reverse direction. It is possible to perform a retroactive action.

  The angle command generation unit 27 corresponds to a higher-level control device of the control unit 26, and is configured mainly with a microcomputer as with the control unit 26. The angle command generation unit 27 interprets the operation program when normal operation is performed, and generates a rotation angle command for controlling each motor M so that the robot 2 performs an operation according to the operation program. Generate every period. The angle command generation unit 27 outputs a part of the generated rotation angle command to the control unit 26 for each output cycle as a rotation angle command pc. The output cycle is longer than the generation cycle.

  FIG. 3 shows the generation timing and output timing of the rotation angle command by the angle command generation unit 27 during normal operation. As shown in FIG. 3, the angle command generation unit 27 converts a part of the rotation angle command generated at a relatively short generation cycle (1 ms in the present embodiment) into a relatively long output cycle (8 ms in the present embodiment). Output every time. In other words, the angle command generation unit 27 does not output most of the rotation angle commands generated for each generation cycle (thinning out). In the present embodiment, the ratio between the rotation angle command to be output and the rotation angle command not to be output (thinned out) is set to 1: 7. The reason for adopting such a configuration is as follows.

  That is, since the rotation angle command is generated by processing by the software of the angle command generation unit 27, it can be generated for each generation cycle that is a relatively short interval. On the other hand, the motor M, the inverter device 23, the rotation angle detection unit 25, and the like drive each axis of the robot 2 and detect the rotation angle of each axis by processing using hardware. Therefore, the operation interval for making the rotation angle coincide with the rotation angle command must be longer than the generation cycle. For this reason, it is necessary to either lengthen the generation cycle of the rotation angle command in accordance with the operation interval or output the rotation angle command generated in a short generation cycle by thinning it out in accordance with the operation interval. In the present embodiment, the latter method is adopted because it is necessary to use a rotation angle command generated in a short generation cycle in retrospective control described later.

  The angle command storage unit 28 sequentially stores all the rotation angle commands generated by the angle command generation unit 27 during the period in which the normal operation is performed (= both the output rotation angle command and the thinned rotation angle command). To do. For example, when the normal operation is performed only for the period shown in FIG. 3, the generated rotation angle commands pc1 'to pc12' are stored in this order. FIG. 4 shows an example of the rotation angle command stored in the angle command storage unit 28. As shown in FIG. 4, the angle command storage unit 28 stores rotation angle commands for the motors M for driving all the axes (first axis to fourth axis) of the robot 2 (when normal operation is started). Is stored). Of the stored rotation angle commands, the first rotation angle command corresponds to the standby position of the robot 2, and the last rotation angle command corresponds to the stop position of the robot 2. That is, it can be said that the rotation angle command stored in the angle command storage unit 28 is a log for each generation period of the rotation angle (trajectory) of each axis during normal operation.

  The control unit 26 includes a function (corresponding to a retroactive command input unit) for inputting a retroactive start command and a retroactive end command during a period in which normal operation is not performed. That is, during the period when the normal operation is performed, the control unit 26 ignores those commands even if a retroactive start command or retroactive end command is given. The retroactive start command is given when the robot 2 is stopped due to equipment abnormality or the like. In the present embodiment, the retrograde start command is given by operating the retrograde start switch provided in the teaching pendant 4. The retrogression start command is not limited to the configuration given by manual operation of the switch, but may be given automatically. For example, a program for outputting a retroactive start command may be implemented and set so that the program is automatically executed when the robot 2 stops due to the above-described cause. Further, the backward end command is given when the robot 2 moves to a predetermined standby position by the backward operation.

  When the retroactive start command is input, the control unit 26 updates the latest rotation angle commands (No. N rotation angle command in FIG. 4) stored in the angle command storage unit 28 and the rotation angle detection unit 25. Using the current rotation angle of each motor M acquired through the process, a retrogressibility determination process is performed. This retrogressibility determination process confirms whether or not the latest position of the robot 2, that is, the stop position of the robot 2 matches the latest command position (details will be described later). When the control unit 26 determines that the stop position of the robot 2 is coincident with the command position by the retrogression possibility determination process, the control unit 26 outputs a retrograde start command to the angle command generation unit 27 to cause the robot 2 to perform a retrospective operation. Perform retroactive control. Further, when the control unit 26 determines that the stop position does not coincide with the command position, the control unit 26 does not output the retrograde start command to the angle command generation unit 27 and does not execute the retrospective control. When the retroactive end command is given, the control unit 26 outputs the retroactive end command to the angle command generating unit 27 and ends the execution of the retroactive control.

  The angle command generator 27 receives the backward start command from the control unit 26 during a period when the normal operation is not performed, and the second time when the rotation angle command stored in the angle command storage unit 28 is stored is the second. The data is read in order from the new one (No. N-1 in FIG. 4) to the oldest (No. 1 in FIG. 4). The angle command generator 27 outputs all the read rotation angle commands as the rotation angle command pc in the order of reading, that is, in order from the newest stored time to the oldest. The rotation angle command pc in this case corresponds to a retroactive angle command. FIG. 5 shows the output timing of the rotation angle command pc by the angle command generation unit 27 during the backward operation. As shown in FIG. 5, the angle command generator 27 outputs all of the read rotation angle commands for each output cycle. Therefore, the rotation angle command generated for each relatively short generation cycle is output for each relatively long output cycle in the reverse order of the generated sequence without being thinned out. And the angle command production | generation part 27 will stop the output operation | movement of the said retrogression angle command, if the retrogression completion command is given from the control part 26. FIG.

  FIG. 6 is a block diagram equivalently showing the contents of motor control in the robot system 1. As shown in FIG. 6, the control unit 26 includes a position control unit 41, a speed control unit 42, and a current control unit 43. In FIG. 6, only the configuration related to the control of one motor M is shown, but the same configuration is actually provided for each of all the motors M.

  The position control unit 41 has a subtractor 44 for obtaining a deviation of the current rotation angle p * with respect to the rotation angle command pc given from the angle command generation unit 27, and a speed so that the output (deviation) of the subtractor 44 approaches zero. The position control amplifier 45 is configured to output a command vc. The gain of the position control amplifier 45 is Kp.

  The speed controller 42 includes a differentiator 46, a subtractor 47, and a speed control amplifier 48. The differentiator 46 differentiates the current rotation angle p * and converts it to the current rotation speed v *. The subtractor 47 obtains a deviation of the current rotational speed v * from the speed command vc. The speed control amplifier 48 outputs a current command ic so that the output (deviation) of the subtractor 47 approaches zero. The gain of this speed control amplifier 48 is Kv.

  The current control unit 43 obtains a deviation of the current i * flowing through the current motor M from the current command ic, and a command signal (inverter) for the inverter device 23 so that the output (deviation) of the subtractor 49 approaches zero. And a current control amplifier 50 that outputs an energization command Sc). The gain of the current control amplifier 50 is Ki. With such a configuration, the control unit 26 performs current feedback control, speed feedback control, and position feedback control, and controls the operation of the arm of the robot 2 by feedback controlling the driving of the motor M.

Next, the operation of the above configuration will be described with reference to FIGS.
FIG. 7 shows a state of the robot that operates according to a predetermined operation program. FIG. 8 shows an outline of the predetermined operation program. The outline of the operation program (pro2) shown in FIG. 8 is as follows. That is, the 07th line is a command to move the hand of the robot 2 to the position P1 (standby position). The 09th line is a branch instruction to be shifted to the 10th or 15th line depending on the part number of the part related to the operation of the robot 2. When the 10th line is entered, the instructions on the 11th to 13th lines are executed. The 11th to 13th lines are commands for moving the hand of the robot 2 in the order of position P2, position P3, and position P4. On the other hand, in the case of shifting to the 15th line, the commands on and after the 16th line are executed.

  FIG. 7 shows the state of the robot 2 when the branch instruction on the 09th line shifts to the 10th line. Therefore, when a normal operation according to this operation program is performed, the hand of the robot 2 moves in the order of position P1, → position P2, → position P3, and position P4. In FIG. 7, the robot 2 when the hand is at the position P1 is indicated by a solid line, and the robot 2 when the hand is at the positions P2, P3, and P4 is indicated by a two-dot chain line.

  At this time, the angle command generation unit 27 generates a rotation angle command for driving each motor M so that the robot 2 sequentially moves from the position P1 to the position P4, and a part of the rotation command is output cycle. Output every time. Therefore, the angle command generation unit 27 generates 25 (= 1 × 4 + 7 × 3) rotation angle commands, and four rotation angle commands (commands for positions P1, P2, P3, and P4, respectively). Is output. At this time, the angle command storage unit 28 sequentially stores the 25 rotation angle commands generated by the angle command generation unit 27. The control unit 26 performs feedback control of driving of the motor M based on the rotation angle command pc given from the angle command generation unit 27 (normal control).

  When such a normal operation is performed, for example, when the robot 2 is stopped at the position P4 due to an abnormality in equipment or the like, in this embodiment, it is possible to execute a retroactive operation in the following procedure. ing. That is, when the retroactive start switch provided on the teaching pendant 4 is operated, the control unit 26 executes the retrospective possibility determination process. In this retrogressibility determination process, the control unit 26 calculates the difference between the latest rotation angle command (in this case, a command indicating the position P4) stored in the angle command storage unit 28 and the current rotation angle of the motor M. Find the axis. The control unit 26 outputs a retroactive start command to the angle command generation unit 27 when the largest value among the absolute values of the differences is equal to or less than a predetermined error value. This error value may be a value sufficiently larger than an error between the rotation angle command and the rotation angle that occurs when the robot 2 is operating normally (deviation of the rotation angle with respect to the rotation angle command).

  When the retrograde start command is given, the angle command generator 27 indicates the position one position before the position P4 (closer to the position P3) out of the 25 rotation angle commands stored in the angle command storage unit 28. The command is sequentially read from the oldest one (position P1 = command indicating the standby position). The angle command generator 27 outputs the read 24 rotation angle commands in the order of reading, that is, in order from the newest stored time to the oldest, as the rotation angle command pc (backward angle command) for each output cycle. Output to.

  The control unit 26 performs feedback control of driving of the motor M based on the rotation angle command pc (backward angle command) given from the angle command generation unit 27 (backward control). As a result, the hand of the robot 2 moves in the order of position P4 → position P3 → position P2 → position P1 (standby position). Moreover, at this time, the robot 2 moves while maintaining the same posture as in the normal operation while each axis passes through the same position as in the normal operation (advance operation). Further, since the rotation angle command generated every generation cycle of 1 ms is output as the retroactive angle command every output cycle of 8 ms, the moving speed of the robot 2 during the backward operation is the movement speed of the robot 2 during the normal operation. About 1/8.

  When the hand of the robot 2 has moved to the position P1 (standby position), a retroactive end command is given to the control unit 26. Accordingly, the control unit 26 outputs a retroactive end command to the angle command generating unit 27 and ends the retroactive control. Further, the angle command generation unit 27 stops the output operation of the retroactive command when the retrospective end command is given from the control unit 26.

As described above, according to the present embodiment, the following effects can be obtained.
When the backward motion of the robot 2 is performed, the angle command generation unit 27 sequentially reads the rotation angle commands stored in the angle command storage unit 28 from the newest ones to the old ones, and goes back the read rotation angle commands. An angle command is output every output cycle. That is, a log obtained by rearranging the rotation angle log of each axis during the advance operation in the reverse order is output as a retroactive angle command. The control unit 26 executes retroactive control for feedback control of driving of each axis by the motor M so that the rotation angle of each axis matches the retrospective angle command. As a result, the backward operation is performed while the rotation angles of all the axes of the robot 2 take the same values as the advance operation. In other words, the robot 2 performs a backward movement so as to follow the movement locus during the forward movement in the reverse direction. That is, since a so-called playback operation is performed, the backward operation is performed while maintaining the same posture as that of the advance operation. Therefore, when a backward operation is performed to return the robot 2 from the stop position to a predetermined standby position, it is possible to reduce the risk of a collision that occurs when the posture of the robot 2 changes compared to the advance operation.

  For this embodiment, the robot 2 is moved from the stop position by simply executing the operation program from the line instructing the movement to the stop position to the line instructing the movement to the predetermined standby position. It is also conceivable to adopt a method of returning to a predetermined standby position. However, the above method cannot be adopted for the following reasons. That is, the operation program includes a branch instruction (for example, the 09th line in FIG. 7) in which the subsequent operation changes for each condition. Considering that the operation program is simply executed in reverse, it is not possible to determine how to branch in such a branch portion. Therefore, there is a high possibility that the operation program cannot be executed in reverse. Further, in the normal operation of the robot 2 (in the advance operation 9, a pass operation may be performed when moving from a predetermined operation point to the next operation point. According to this embodiment, each axis during the advance operation Since the rotation angle logs are output in the reverse order as the retroactive angle command, the rotation angle of each axis associated with the path operation is also accurately reproduced. It is impossible to obtain the rotation angle of each axis associated with such a path operation simply by executing this, so that the same effect as the present embodiment can be obtained by simply executing the operation program in reverse. Cannot be obtained.

  Further, the retroactive angle command is obtained by outputting all the rotation angle commands generated for each generation cycle for each long output cycle. In other words, since a rotation angle command for instructing a rotation angle for each relatively short generation cycle is output for each relatively long cycle, the rotation speed of each axis during the retroactive operation, and thus the operation speed of the robot 2 is the normal operation time. It is slower than that. As described above, during the backward operation, the operation of the robot 2 is slower than the normal operation (becomes a slow operation). Therefore, if other equipment around the robot 2 moves unexpectedly by the user. Even so, the user can deal with the movement with a margin. For this reason, it is possible to reduce the risk of a collision that occurs for the first time when the retroactive action is performed.

  When the robot 2 stops due to collision with, for example, peripheral equipment, the rotation angle of each axis at the time of stopping is often a value far from the rotation angle command. For this reason, it is difficult to perform retroactive control normally. Further, even if the retroactive control can be executed successfully, the robot 2 passes through the locus when it collides with surrounding equipment. That is, there is a very high possibility that the robot 2 will collide with surrounding equipment again. Accordingly, when stopping due to a collision, it is desirable to remove the cause of the collision after completely stopping the operation of each facility including the robot 2 without performing retroactive control. Therefore, when the retroactive start command is given, the control unit 26 of the present embodiment executes the following retrospective possibility determination process.

  That is, the control unit 26 obtains the difference between the latest rotation angle command stored in the angle command storage unit 28 and the current rotation angle of the motor M for each axis, and the largest value among the absolute values of the differences is predetermined. When the error value is equal to or less than the error value, retroactive control is executed. On the other hand, retroactive control is not executed when the largest value among the absolute values of the differences is equal to or smaller than a predetermined error value. Even when the robot 2 is operating normally, the rotation angle and the rotation angle command may not completely match due to the influence of various errors. On the other hand, when the robot 2 stops due to a collision, the rotation angle and the rotation angle command are greatly different values. From this, when the absolute value of the difference between the rotation angle and the rotation angle command exceeds a predetermined error value, the control unit 26 determines that the robot 2 is in a stopped state due to a collision, and performs retroactive control. Do not run. Thereby, the risk that the robot 2 will collide again by performing the retroactive action can be suppressed.

  Further, when the retrograde start command is given, the angle command generation unit 27 sequentially reads the rotation angle commands stored in the angle command storage unit 28 from the second most recently stored time to the oldest one. The latest rotation angle command stored in the angle command storage unit 28 is for commanding the current position when the robot 2 stops. For this reason, when the backward angle command is used in order from the latest one, the robot 2 actually starts the backward operation when the previous backward angle command is output before the latest one. That is, the start of the retroactive operation is slightly delayed. On the other hand, according to the configuration of the present embodiment, the retroactive operation can be started quickly without such a slight delay, and thus the time required for the entire retroactive operation can be shortened.

(Second Embodiment)
Hereinafter, a second embodiment of the present invention will be described with reference to FIGS.
FIG. 9 and FIG. 10 are views corresponding to FIG. 1 and FIG. 6 in the first embodiment, respectively, and the same parts as those in the first embodiment are denoted by the same reference numerals and description thereof is omitted. The robot system 61 of this embodiment shown in FIG. 9 differs from the robot system 1 of the first embodiment shown in FIG. 1 in that a robot 62 is provided instead of the robot 2.

  The robot 62 is configured as, for example, a 6-axis vertical articulated robot. The robot 62 includes a base 63, a shoulder portion 64 supported by the base 63 so as to be rotatable in the horizontal direction, a lower arm 65 supported by the shoulder portion 64 so as to be rotatable in the vertical direction, and the lower arm 65. A first upper arm 66 supported so as to be rotatable in the vertical direction, a second upper arm 67 supported by the first upper arm 66 so as to be able to rotate, and a vertical direction supported by the second upper arm 67 The wrist 68 is rotatably supported on the wrist 68 and a flange 69 is rotatably supported on the wrist 68 by twisting.

  The base 63, the shoulder portion 64, the lower arm 65, the first upper arm 66, the second upper arm 67, the wrist 68, and the flange 69 function as an arm of the robot 62. Although not, an end effector (hand) is attached. The plurality of axes provided in the robot 62 are driven by motors (indicated by reference numeral M in FIG. 2) provided corresponding to each of the axes as in the robot 2 of the first embodiment. Further, in the vicinity of each motor, a position detector (not shown) for detecting the rotational position of each rotating shaft is provided.

  Although not shown, the angle command storage unit 28 of the present embodiment sequentially stores the value of the current i * (or the value of the current command ic) every generation cycle (for example, 1 ms) during the period in which the normal operation is performed. It has become. That is, the angle command storage unit 28 of this embodiment stores log data of current values for each generation cycle.

  FIG. 10 is a block diagram equivalently showing the contents of motor control in the robot system 61. As shown in FIG. 10, the control unit 71 of the present embodiment includes a torque value estimation unit 72, a generated torque value calculation unit 73, and a disturbance torque value calculation in addition to the configuration provided in the control unit 26 of the first embodiment. A portion 74 is provided.

  A torque value estimation unit 72 (corresponding to a torque value estimation means) is a torque value (estimated torque value) that must be generated by the motor M from the start to the end of the operation performed by the robot 62 based on a predetermined operation program. ) Is collectively estimated using the rotation angle command pc. As this estimation method, for example, a method of estimating a torque value that needs to be generated by the motor M in order to rotate the axis of the robot 62 from a predetermined rotational position to the next rotational position using an equation of motion is given. It is done. The torque value estimation unit 72 may be configured to sequentially estimate the torque value that the motor M needs to generate in order for the robot 62 to move from the current command position to the next command position. A generated torque value calculation unit 73 (corresponding to a torque value calculation means) multiplies the current current i * flowing through the motor M by a torque constant of the motor M, thereby generating a current torque value (generated by the motor M) ( Calculate the generated torque value.

The disturbance torque value calculation unit 74 (corresponding to the disturbance calculation means) obtains a torque value (disturbance load torque value) applied from the outside to the shaft to be driven by the motor M from the difference between the estimated torque value and the generated torque value. That is, unless a load is applied to the robot 62 from the outside, torque generated by the motor M is generated on each axis of the robot 62 in accordance with the rotation angle command pc. Therefore, the estimated torque value and the generated torque value Matches. On the other hand, when a predetermined load is applied to the robot 62 from the outside due to a collision or the like, a difference corresponding to the load is generated between the estimated torque value and the generated torque value. . Therefore, the disturbance torque value calculation unit 74 calculates the disturbance load torque value Td based on the following equation (1) using the estimated torque value Te and the generated torque value Tg.
Td = | Tg−Te | (1)

  The control unit 71 (corresponding to a collision determination unit) detects a collision with the robot 62 based on the disturbance load torque value Td applied to each axis. That is, the control unit 71 obtains a disturbance load torque value Td applied to each axis for each predetermined collision detection cycle, and when the disturbance load torque value Td applied to at least one of the axes exceeds a predetermined threshold value Tth. Judge that a collision has occurred. The threshold value Tth can be set in advance by the user, and is, for example, a value in the range of 1 to 500 [kgf · cm] (the initial value is 100 [kgf · cm]). Although the sensitivity of collision detection can be increased by reducing the set value of the threshold value Tth, the possibility of erroneous detection due to various calculation errors increases. On the other hand, when the set value of the threshold value Tth is increased, the sensitivity of collision detection is lowered, but the possibility of erroneous detection is reduced. The threshold value Tth may be set to an optimum value as a system to be applied in consideration of such a trade-off relationship.

  Thus, in the robot system 61 of this embodiment, when the robot 62 collides with a peripheral device, it is possible to detect the collision instantaneously without a sensor. That is, in the present embodiment, the control unit 71 functions as a collision detection unit. Further, when detecting the collision of the robot 62, the control unit 71 performs stop control for immediately stopping the operation of each arm of the robot 62. In the stop control, the control unit 71 causes the motor M that drives each axis of the robot 62 to generate reverse torque while stopping the operation of the arm along the subsequent movement locus.

  Further, when detecting the collision of the robot 62, the control unit 71 specifies the collision position based on the rotation angle of each axis at the time of the collision. FIG. 11 shows the rotation angle command pc, the rotation angle p *, and the current i * on a predetermined axis before and after the collision. A vertical solid line in FIG. 11 indicates an operation command output point (sampling timing). The interval between the operation command output points (robot operation command interval) is the same as the output cycle (for example, 8 ms) in this embodiment. Further, the vertical dotted line in FIG. 11 indicates the execution timing of the above-described collision detection operation. The interval of the execution timing (collision detection cycle) is shorter than the output cycle, and is the same as the generation cycle (for example, 1 ms) in this embodiment.

  When a collision with the robot 62 occurs at time t1, the rotation angle command pc continues to increase toward the value (target value) of the next operation command output point. On the other hand, the detected actual rotation angle p * (encoder value) is substantially constant without changing since the collision occurs. For this reason, the deviation (positional deviation) of the current rotation angle p * with respect to the rotation angle command pc increases rapidly, and the current i * also increases rapidly. Thereafter, when a collision is detected by the above-described collision detection operation at time t2, the robot 62 is controlled to stop so that the rotation angle command pc matches the rotation angle p *. At this time, the current i * also decreases toward zero, and becomes zero when the robot 62 is completely stopped (time t3).

  After detecting the collision and stopping the robot 62, the control unit 71 traces the log data of the current value stored in the angle command storage unit 28 from the value at the time of stopping, and the value increases rapidly. Find the starting point. In the case of FIG. 11, the current i * starts to increase rapidly at time t1. For this reason, the control unit 71 specifies the time t1 as the timing when the collision occurs (collision timing) and specifies the collision position based on the rotation angle p * of each axis at the time t1. Examples of the method for specifying the collision position include a method for determining the collision position by determining that a collision has occurred with respect to the axis having the largest disturbance load torque value Td applied. In that case, the disturbance load torque value applied to the hand of the robot 62 can be obtained from the disturbance load torque value Td of each axis using the Jacobian matrix, and the collision with the hand can be specified.

  Moreover, in this embodiment, the control part 71 can perform retroactive control even after collision detection. When detecting a collision, the control unit 71 sets an entry prohibition space for use in retroactive control after the collision. The entry prohibition space has a spherical shape, and its center is set at the identified collision position (position at time t1). For example, when the identified collision position is the tip of the robot 62, the radius of the entry prohibition space is the timing closer to the collision timing among the operation command output points before and after the collision timing, that is, immediately before the collision timing. Is set to a length corresponding to the distance between the position of the hand at the time of the operation command output point (time ta) and the position of the hand at the time of collision (collision position). Note that the position of the hand at the time ta is obtained based on the rotation angle command of each axis output at the time ta.

Next, the operation and effect of this embodiment will be described with reference to FIGS.
As a collision of the robot 62, a collision during automatic driving, a collision during teaching or operation confirmation, and the like can be considered. In the former case, there is a high possibility that the robot 62 is operating at the maximum speed, and the collision speed is relatively fast. For this reason, there is a high possibility that the mechanical parts and peripheral equipment of the robot will be greatly damaged, and the subsequent automatic recovery becomes difficult. In such a case, since the positional deviation at the time of stopping due to a collision is often excessive, it is difficult to execute the retroactive control itself. In this case, before the collision is detected, an error such as an excessive deviation, an overcurrent, or an acceleration limit value is generated and the robot 62 is urgently stopped. Therefore, the retroactive control is not executed also in the robot system 61 of the present embodiment. .

  On the other hand, in the latter case, since the robot 62 operates at a low speed, the collision speed is relatively low. For this reason, the mechanical parts and peripheral equipment of the robot are hardly damaged, and the subsequent automatic recovery is possible. In such a case, since the position deviation at the time of stopping due to a collision is often relatively small, retroactive control can be executed. Of the above-mentioned collisions, the latter collision with a low collision speed is more frequently generated than the former collision with a high collision speed. This is because the robot 62 repeatedly performs an operation determined based on the operation program during the automatic operation. The repeated operation is sufficiently verified in advance so as not to collide with peripheral equipment. For this reason, the possibility of collision during automatic operation is extremely low.

  On the other hand, during teaching and operation confirmation, teaching and operation confirmation are performed also as collision verification with peripheral equipment, and during that time, the operation trajectory is frequently corrected. . For this reason, the reproducibility of the operation of the robot 62 is low during teaching or operation confirmation. Therefore, there is a high possibility that a collision with peripheral equipment or the like will occur in a teaching or confirmation operation performed after a new motion trajectory is generated or after the motion trajectory is corrected. In this way, a lighter collision that has a slower collision speed and can execute retroactive control has a higher occurrence frequency than a severe collision that has a high collision speed and cannot perform retroactive control. For this reason, even when the robot 62 stops due to a slight collision, it is desirable to execute retroactive control to move the robot to the standby position.

  However, although it is a minor collision, increasing the number of collisions unnecessarily is not preferable for the robot 62 that requires precise movement. This is because even if the collision is slight, there is a risk that the accuracy of the calibration and the frame performed for precisely operating the robot 62 may be distorted by repeating the collision. Therefore, in the present embodiment, when the robot 62 stops due to a slight collision, retroactive control is executed after adding a device for preventing the occurrence of re-collision as follows.

  FIG. 12 shows an operation state of the robot before and after the collision, for example, during teaching or during operation confirmation. First, as shown in FIG. 12 (1), the hand of the robot 62 moves from a position slightly away from the equipment 81 toward the equipment 81. Then, as shown in FIG. 12 (2), the hand of the robot 62 collides with the equipment 81. At this time, the control unit 71 detects the collision and starts the stop control of the robot 62. Thereafter, as shown in (3) of FIG. 12, the hand of the robot 62 stops by the stop control while moving to a position slightly away from the equipment 81 due to the reaction of the collision.

  The control unit 71 executes the retroactive control when the retroactive start command is input in a state where the robot 62 is stopped due to the occurrence of the collision as described above. However, when the retroactive control is executed after the robot 62 is stopped due to the collision, based on the rotation angle command pc (the retroactive angle command) output by the angle command generating unit 27 as in the retrospective control of the first embodiment. When the robot 62 is moved backward, the movement locus always passes through the collision position. Therefore, there is a high possibility that the robot 62 will collide again.

  Therefore, when a retrograde start command is input in a state where a collision is detected, the angle command generation unit 27 of the present embodiment makes the following changes to the rotation angle command pc read from the angle command storage unit 28. That is, the angle command generation unit 27 of the present embodiment, when a retroactive command is input in a state where a collision is detected, shows the trajectory of the retrospective movement of the robot 62 based on the rotation angle command pc read from the angle command storage unit 28. , Between the outer contour position that first overlaps the outer contour of the sphere in the entry prohibition space set by the control unit 71 and the outer contour position that overlaps last, the movement locus is outside the entry prohibition space, and the sphere in the entry prohibition space The read rotation angle command pc is changed so as to be a locus along the surface, and the changed rotation angle command pc is output as a retroactive angle command. In short, in this embodiment, without using the rotation angle command pc as an operation trajectory that passes through the inside of the entry prohibition space as a retroactive angle command, instead, an operation trajectory that crawls the surface of the sphere in the entry prohibition space. A new rotation angle command pc is generated and used as a retroactive angle command.

  When the retroactive control is executed by the control unit 71 in a state where the operation trajectory is changed in accordance with the change of the rotation angle command pc by the angle command generation unit 27 as described above, the hand of the robot 62 is moved to (3 in FIG. ) From the stop position shown in FIG. 12 to the position shown in (1) of FIG. 12 via the collision position. However, when going through the collision position in the retroactive control, as shown in FIG. 13, the retroactive action is performed so that the hand of the robot 62 avoids the collision position. Hereinafter, the collision position avoidance operation during the backward operation will be described in detail with reference to FIG.

  FIG. 14 shows the motion trajectory of the hand at the time of the forward motion near the collision position and the motion trajectory of the hand at the time of the backward motion after the collision. In FIG. 14, the motion trajectory during the forward motion is indicated by a solid line, and the motion trajectory during the backward motion is indicated by a one-dot chain line. Further, the collision position is indicated by a point denoted by a symbol a, and the set entry prohibition space is denoted by a broken line and denoted by a symbol b. Further, the position where the movement trajectory during the forward movement first overlaps the entry prohibition space b (escape outline position) is indicated by a point denoted by reference numeral x1, and the position where the movement locus finally overlaps the entry prohibition space b (entrance outline position) This is indicated by a point with x2. Note that the times t1, ta, and tb shown in FIG. 14 correspond to the times indicated by the same reference numerals in FIG.

  As shown in FIG. 14, after stopping due to the collision, the hand of the robot 62 performs a backward operation so as to follow the motion trajectory during the forward motion in the reverse direction in the section from the stop position to the entry outline position x2. . In the section from the entry outline position x2 to the escape outline position x1, the collision position a is avoided along the surface (outline) of the sphere of the entry prohibition space b. Then, in the section after the escape outline position x1, the backward movement is performed so that the movement locus of the advance movement is traced in the opposite direction. In FIG. 14, for the convenience of explanation, in the section from the stop position to the entry outline position x2 and the section after the exit outline position x1, the movement trajectories during the forward operation and the retroactive operation are drawn slightly shifted from each other. Actually, however, the movement trajectories are the same.

  Since the radius of the entry prohibition space b is set as described above, it can be said that the radius is very small. For this reason, when performing a backward movement along the outline of the entry-prohibited space b, the possibility that a new collision such as a part other than the hand of the robot 62 such as the elbow collides with surrounding equipment is increased, that is, The posture of the robot 62 does not change to a level at which another new collision occurs. That is, the posture of the robot 62 when performing the backward movement along the outline of the entry prohibition space b is almost the same as the posture at the time of the forward movement in which the collision has occurred, and the posture change that causes another new collision is as follows. Absent.

  In this way, in the backward operation after the collision, the collision position a is avoided, so that the complete playback operation is not achieved. However, as described above, the entry prohibition space b is small. In addition, a path (motion trajectory) through which the robot 62 originally passes is provided with a predetermined margin so as not to collide with peripheral equipment and the like in consideration of the physique of the robot 62. The physique of the robot 62 is much larger than the entry prohibition space b. For this reason, as long as the motion trajectory is changed along the outline of the entry prohibition space b, the possibility that the robot 62 collides with the peripheral equipment beyond the margin range is extremely low. That is, the robot 62 is unlikely to cause a new collision due to a slight change in the motion trajectory in the retroactive control. Therefore, when the robot 62 moves backward after the collision, it is possible to perform the backward movement while maintaining the same posture as the forward movement while reliably avoiding the collision position a and preventing the occurrence of a new collision. .

(Other embodiments)
The present invention is not limited to the embodiments described above and illustrated in the drawings, and the following modifications or expansions are possible.
When the retrograde start command is input, the angle command generation unit 27 reads the rotation angle commands stored in the angle command storage unit 28 in order from the newest to the oldest stored time. Good. Further, the angle command generation unit 27 may read out the rotation angle commands stored in the angle command storage unit 28 in order from the newest to the oldest when the retrograde start command is input. Good.
The generation cycle and the output cycle can be appropriately changed so as to set the operation speed of the robot 2 during the backward operation to a necessary speed. That is, the ratio (1: x) of the rotation angle command output by the angle command generation unit 27 and the rotation angle command not output (thinned out) (1: x) can be appropriately changed within a range satisfying the condition of “x> 1”.
In the retrogressibility determination process, the control unit 26 may change the difference between the latest rotation angle command stored in the angle command storage unit 28 and the current rotation angle of the motor M so as to obtain a predetermined axis. . Moreover, the control part 26 does not need to perform a retrogression possibility determination process, as long as the retroactive start command is not given when the robot 2 stops due to a collision.

  The standby position at which the execution of the retroactive control is terminated is not limited to the position represented by the first rotation angle command stored in the angle command storage unit 28, and may be an arbitrary position. For example, the arbitrary position may be a position (or a position before or after) where there is a move instruction (an instruction to move the hand to perform a predetermined work) in the operation program. At the position of the move instruction and the positions before and after it, there is a high possibility that a command such as grasping the work is issued to the hand. If the backward movement is continued while the robot 2 holds the workpiece, the workpiece may collide with peripheral equipment (for example, a narrow passage). Therefore, the possibility of the above-mentioned collision can be reduced by ending the backward movement before and after the move command and waiting the robot 2.

  In the second embodiment, the entry prohibition space set by the control unit 71 avoids re-collision at the position where the robot 62 first collided when the robot 62 is moved backward after the collision occurs, and a new Any space that can prevent the occurrence of another collision can be appropriately changed. For example, when the identified collision position is the tip of the robot 62, the radius of the entry prohibition space is the operation command output point before and after the collision timing, that is, the timing far from the collision timing, that is, the operation immediately after the collision timing. A length corresponding to the distance between the position of the hand at the time of the command output point (time tb in FIGS. 11 and 14) and the position of the hand at the time of collision (collision position) may be set. Note that the position of the hand at time tb is obtained based on the rotation angle command for each axis output at time tb. The radius of the entry prohibition space set in this way is slightly larger than that set in the second embodiment. For this reason, for example, even when the surface of the equipment on which the robot 62 collides is deformed to some extent due to the collision, it is possible to perform the backward operation while avoiding the deformed portion. The effect that it can prevent is acquired.

  Further, the radius of the entry prohibition space may be set based on an accuracy error (for example, ± 0.075 mm) of the robot 62. In that case, in order to take a margin on the safe side, an accuracy error N times (for example, N = 2) may be set as the radius of the entry prohibition space. Alternatively, the radius of the entry prohibition space may be set based on the speed at the time of collision (collision speed). In that case, in order to take a margin on the safe side, if the distance moved at the collision speed during the collision detection period (for example, 1 ms) is multiplied by N (for example, N = 2), the radius of the entry prohibition space is set. Good. Alternatively, the larger one of the radius set based on the accuracy error of the robot 62 and the radius set based on the collision speed may be set as the radius of the entry prohibition space.

  What is necessary is just to obtain | require the said collision speed based on the rotational speed of each axis | shaft at the time of a collision, for example. That is, after detecting the collision, the control unit 71 is between the operation command output point immediately before the collision timing (time ta in FIG. 11) and the previous operation command output point (time tc in FIG. 11). , I.e., the difference between the immediately preceding rotation angle command pc and the immediately preceding rotation angle command pc, the moving distance (movement amount) in the output cycle immediately before the collision is obtained. And the control part 71 should just calculate the rotational speed in the output period immediately before a collision from the value (for example, 8 ms) of output period, and the calculated | required moving distance, and should just acquire the value as a collision speed. In this way, it is possible to accurately obtain the speed immediately before the occurrence of the collision, and thus the speed at the time of the collision.

The collision detection means is not limited to the configuration described in the second embodiment, and any apparatus that can detect a collision with the robot can be applied. For example, another sensorless type collision detection unit or a collision detection unit using a sensor such as an acceleration sensor may be used.
In the first embodiment, an example in which the present invention is applied to a four-axis horizontal articulated robot 2 will be described. In the second embodiment, the present invention is applied to a six-axis vertical articulated robot 62. However, the present invention is applicable to all multi-axis robots having a plurality of axes.

  In the drawings, 1 and 61 are robot systems, 2 and 62 are robots, 25 is a rotation angle detection unit, 26 is a control unit (backward command input unit), 27 is an angle command generation unit, 28 is an angle command storage unit, and 71 is A control unit (collision detection means) and M represents a motor (drive unit).

Claims (5)

A plurality of axes provided on the robot;
A driving unit for driving each of the plurality of shafts;
A rotation angle detector that detects rotation angles of the plurality of axes;
An angle command generation unit that generates a rotation angle command for each generation cycle, and outputs a part of the rotation angle command for each output cycle longer than the generation cycle;
A control unit that performs normal control for controlling the operation of the drive unit so that the rotation angle detected by the rotation angle detection unit matches the rotation angle command output from the angle command generation unit;
An angle command storage unit that sequentially stores all of the rotation angle commands generated by the angle command generation unit during a period in which the normal control is performed by the control unit;
A retroactive command input unit for inputting a retroactive start command and a retroactive end command in a period in which the normal control is not executed by the control unit;
The angle command generator is
When the retroactive start command is input, the rotation angle commands stored in the angle command storage unit are sequentially read from new to old, and the read rotation angle commands are set as retroactive angle commands for each output cycle. Output to
When the backward end command is input, the output of the backward angle command is stopped,
When the retroactive start command is input, the controller controls the operation of the drive unit so that the rotational angle detected by the rotational angle detector matches the retroactive angle command output from the angle command generator. Start the retroactive control to control,
Execution of the retroactive control is terminated when the retroactive end command is input.   When the retroactive start command is input, the control unit obtains a difference between the latest rotation angle command stored in the angle command storage unit and the current rotation angle detected by the rotational position detection unit. When the absolute value of the difference is equal to or less than a predetermined error value, the retroactive control is executed, and when the absolute value of the difference exceeds the error value, the retroactive control is not executed. Item 2. The robot system according to Item 1. A collision detection means for detecting a collision of the robot;
The controller is
When a collision is detected by the collision detection means, a collision timing at which the collision occurs and a collision position which is a position at the time of the collision are specified, the collision position is the center and before and after the collision timing Set an entry prohibition space having a spherical shape with a radius between the rotation angle command output at the output timing closer to the collision timing and the collision position among the output timings of the rotation angle command at
The angle command generator is
When the backward start command is input in a state where the collision is detected, the robot's motion trajectory based on the rotation angle command read from the angle command storage unit has two contour positions that overlap the contour of the entry prohibition space. In between, the movement angle command of the robot is outside the entry prohibition space and the read rotation angle command is changed so as to be a locus along the surface of the sphere of the entry prohibition space. The robot system according to claim 1, wherein: A collision detection means for detecting a collision of the robot;
The controller is
When a collision is detected by the collision detection means, a collision timing at which the collision occurs and a collision position which is a position at the time of the collision are specified, the collision position is the center and before and after the collision timing Set an entry prohibition space having a spherical shape with a radius between the rotation angle command output at the output timing far from the collision timing and the collision position among the output timings of the rotation angle command at
The angle command generator is
When the backward start command is input in a state where the collision is detected, the robot's motion trajectory based on the rotation angle command read from the angle command storage unit has two contour positions that overlap the contour of the entry prohibition space. In between, the movement angle command of the robot is outside the entry prohibition space and the read rotation angle command is changed so as to be a locus along the surface of the sphere of the entry prohibition space. The robot system according to claim 1, wherein:   When the retrograde start command is input, the angle command generation unit sequentially reads out rotation angle commands excluding the latest one among the rotation angle commands stored in the angle command storage unit from newest to oldest. The robot system according to any one of claims 1 to 4, wherein:

Images