Robot controller and emergency stop method for robot

Abstract

To stop a robot quickly while maintaining coordinates of a hand in a predetermined track when urgently stopping a robot having a plurality of shafts.SOLUTION: A robot is controlled by a robot controller having an all shaft control part 50 for collectively calculating position command values for a plurality of shafts on the basis of a predetermined track, and a motor drive control part 40 provided in each shaft for servo-controlling a motor on the basis of the position command value of each shaft, When an emergency stop signal is input, the position command value used for the servo control is switched to a position command value for stopping each shaft calculated in the motor drive control part 40 from the position command value from the all shaft control part 50. Then, the position command value used for the servo control is returned to the position command value from the all shaft control part 50. When the emergency stop signal is input, the all shaft control part 50 starts the calculation for outputting a position command value for emergency stop for stopping the robot on the predetermined track.SELECTED DRAWING: Figure 9

Description

[Technical field]

[0001] The present invention relates to the control of a robot having multiple axes, and in particular to a robot controller that can bring the robot to an emergency stop while maintaining the coordinates of the hand on a specified trajectory, and a method for bringing the robot to an emergency stop. [Background technology]

[0002] A robot controller that controls a robot having multiple axes, each of which is provided with a motor, is provided with a motor drive control unit that performs servo control based on a position command value for the motor. In order to operate the robot along a specified trajectory, it is necessary to simultaneously move multiple axes of the robot, so the robot controller is provided with an all-axis control unit that collectively calculates and outputs position command values ​​for each axis of the robot, and the position command values ​​for each axis calculated by the all-axis control unit are transmitted to the corresponding motor drive control unit. In addition, when the robot detects an abnormality during its operation or receives an emergency stop command from outside, it is necessary to make the robot stop in an emergency. When making an emergency stop, it is necessary to make the motor speeds of all axes 0 within a predetermined time. For this reason, the robot controller is provided with an emergency stop signal input unit that inputs an emergency stop signal that instructs an emergency stop. When making an emergency stop of the robot, it is preferable that the position and posture of the robot after the emergency stop are on the normal trajectory of the robot. Stopping at a location other than the normal trajectory, i.e., the trajectory of the robot is not maintained at the target value, means that the robot moves to an unexpected position, which may cause a safety problem, and also makes it difficult to control the robot when returning from the emergency stop.

[0003] A control method in a robot controller when an emergency stop signal is input to make an emergency stop of a robot is to supply the emergency stop signal to an all-axis control unit, and the all-axis control unit performs a motion plan and an inverse kinematic calculation of the hand based on the reference position of the hand when the emergency stop signal is received, and outputs a position command value of each axis so that the robot stops on a normal trajectory as much as possible. For example, Patent Document 1 discloses that when making an emergency stop, the all-axis control unit controls the motor of each axis by planning an operation such that the hand moves linearly in the direction of movement of the hand when the emergency stop signal is input while maintaining the orientation of the hand. Another control method in a robot controller when making an emergency stop of a robot is to supply an emergency stop signal to each motor drive control unit, and each motor drive control unit decelerates and stops each motor at an emergency stop acceleration determined for each motor. [Prior art documents] [Patent documents]

[0004] [Patent Document 1] JP 2014-34108 A Summary of the Invention [Problem to be solved by the invention]

[0005] When an emergency stop signal is input to the robot controller, in order to decelerate and stop the robot while maintaining the coordinates and orientation of the hand on the target trajectory, processing time is required for inverse kinematics calculations and motion planning, and delays also occur due to the intervals between the transmission of position command values ​​between the all-axis control unit and the motor drive control unit, making it difficult to stop the robot quickly after an emergency stop signal is issued upon detection of an abnormality. On the other hand, when an emergency stop signal is input to the motor drive control unit to decelerate and stop each motor, the motor is controlled by a speed command value calculated within the motor drive control unit based on the motor coordinate system of each axis, resulting in an independent stopping behavior for each axis, and it is not guaranteed that the robot will stop on the normal trajectory.

[0006] An object of the present invention is to provide a robot controller that can quickly stop a robot while maintaining the coordinates and orientation of the hand on a target trajectory when an emergency stop is required, and a method for such an emergency stop. [Means for solving the problem]

[0007] The robot controller of the present invention is a robot controller for controlling a robot having a plurality of axes and driven by a motor provided for each axis, and includes an all-axis control unit that collectively calculates position command values ​​for the plurality of axes based on a predetermined trajectory of the robot, and a motor control unit that controls the position command values ​​for the plurality of axes based on a predetermined trajectory of the robot. and a motor drive control unit which servo-controls the motor of that axis based on a position command value for that axis from the all-axis control unit, and which is provided with a stop position command calculation unit which calculates an axis stop position command value for each axis based on the motor coordinate system for stopping the corresponding motor, and when an emergency stop signal is input, the motor drive control unit switches the position command value used for servo control from the position command value from the all-axis control unit to the position command value for stopping each axis to servo-control the corresponding motor, and then returns the position command value used for servo control from the position command value for stopping each axis to the position command value from the all-axis control unit to continue the servo control of the corresponding motor, and when an emergency stop signal is input, the all-axis control unit starts calculation to output the emergency stop position command value for stopping the robot on a predetermined trajectory. Furthermore, in the robot controller of the present invention, (A) based on a position command switching command calculated by the all-axis control unit, the position command value used for servo control is returned from the position command value for stopping each axis to the position command value from the all-axis control unit, or (B) an estimated value calculation unit that estimates the movement of the robot resulting from servo-controlling the motor of each axis by the position command value for stopping each axis, and a position command value calculation unit that calculates a position command value for a corrective operation that returns the robot to a specified trajectory, i.e., a target trajectory, based on the result of estimation by the estimated value calculation unit, so that the position command value calculation unit calculates the position command value for emergency stop following the position command value for corrective operation.

[0008] In the case of an emergency stop of the robot, the emergency stop position command value for stopping the robot on a predetermined trajectory, i.e., while maintaining the coordinates and orientation of the hand on the target trajectory, is calculated after the emergency stop command is input in the all-axis control unit, and is transmitted to the motor drive control unit. Therefore, the time required for the calculation process until the output of the emergency stop position command value starts and the time required for the transmission of the position command value between the all-axis control unit and the motor drive control unit act as a delay time for stopping the robot. In the robot controller of the present invention, when an emergency stop signal is input, the motor drive control unit switches the position command value used for servo control from the position command value from the all-axis control unit to the position command value for stopping each axis, and servo-controls the corresponding motor, and then switches the position command value used for servo control back from the position command value for stopping each axis to the position command value from the all-axis control unit, and continues the servo control of the corresponding motor. Therefore, the delay time until the robot stops can be shortened, and the robot can be quickly stopped while maintaining the coordinates and orientation of the hand on the target trajectory.

[0009] In the robot controller of the present invention, When the configuration (A) is adopted, the position command value used for servo control can be returned to the position command value from the all-axis control unit in accordance with the timing when the all-axis control unit is able to output the position command value for corrective operation or the position command value for emergency stop, so that the robot can be returned to the target trajectory more quickly. Also, when the configuration (A) is adopted, In the motor drive control unit, during a transitional period in which the position command value used for servo control is returned from the position command value for stopping each axis to the position command value from the all-axis control unit, a position command value may be calculated by dividing the position command value for stopping each axis and the position command value from the all-axis control unit while changing the division ratio, and used for servo control. By performing such control, it is possible to return the position command value used for servo control to the position command value from the all-axis control unit with a small amount of calculation while suppressing the occurrence of vibrations and shocks in the robot.

[0010] When the configuration of (B) is adopted, The position command value used for servo control can be returned more smoothly to the position command value from the all-axis control unit.

[0012] In the robot controller of the present invention, the position command value for stopping each axis is, for example, a position command value for decelerating and stopping the motor by an acceleration or deceleration time set for each motor. By using such a position command value for stopping each axis, it becomes easy to estimate the movement of the robot caused by the servo control of the motor of each axis by the position command value for stopping each axis.

[0013] The emergency stop method of the present invention is an emergency stop method for a robot having multiple axes, in which the motors for each axis are servo-controlled based on position command values ​​calculated collectively for the multiple axes by an all-axis control unit so that the robot moves along a predetermined trajectory, and when an emergency stop signal is input, the position command value used for servo control for each axis is switched from the position command value from the all-axis control unit to a position command value for stopping each axis calculated based on a motor coordinate system in order to stop the corresponding motor, and then the position command value used for servo control is returned from the position command value for stopping each axis to the position command value from the all-axis control unit, and when an emergency stop signal is input, a calculation is started in the all-axis control unit to output the emergency stop position command value for stopping the robot on a predetermined trajectory, i.e., a target trajectory. In the emergency stop method of the present invention, (C) based on a position command switching command calculated by the all-axis control unit, the position command value used for servo control is returned from the position command value for stopping each axis to the position command value from the all-axis control unit, or (D) when an emergency stop signal is input, the all-axis control unit estimates the robot movement caused by servo-controlling the motor of each axis using the position command value for stopping each axis, and calculates a position command value for a corrective operation to return the robot to a predetermined trajectory, i.e., a target trajectory, based on the estimated robot movement.

[0014] In the emergency stop method of the present invention, when an emergency stop signal is input, the position command value used for servo control is switched for each axis from the position command value from the all-axis control unit to the position command value for stopping each axis, and then the position command value used for servo control is returned from the position command value for stopping each axis to the position command value from the all-axis control unit.This reduces the effects of the delay time from input of the emergency stop signal to the start of output of the emergency stop position command value and the delay time due to the transmission interval of the position command values ​​from the all-axis control unit, and makes it possible to stop the robot on the target trajectory more quickly.

[0015] Furthermore, in the emergency stop method of the present invention, When the configuration (C) is adopted, the position command value used for servo control can be returned to the position command value from the all-axis control unit in accordance with the timing when the all-axis control unit is able to output the position command value for corrective operation or the position command value for emergency stop, so that the robot can be returned to the target trajectory more quickly. Also, when the configuration (C) is adopted,In a transitional period in which the position command value used for servo control is returned from the position command value for stopping each axis to the position command value from the all-axis control unit, a position command value may be calculated by dividing the position command value for stopping each axis and the position command value from the all-axis control unit while changing the proportional division ratio, and used for servo control. By performing such control, it is possible to return the position command value used for servo control to the position command value from the all-axis control unit with a small amount of calculation while suppressing the occurrence of vibrations and shocks in the robot.

[0016] When the structure of (D) is adopted, The position command value used for servo control can be returned more smoothly to the position command value from the all-axis control unit.

[0018] In the emergency stop method of the present invention, the position command value for stopping each axis is, for example, a position command value for decelerating and stopping the motor by an acceleration or deceleration time set for each motor. By using such a position command value for stopping each axis, it becomes easy to estimate the movement of the robot caused by the servo control of the motor of each axis by the position command value for stopping each axis. Effect of the Invention

[0019] According to the present invention, in the event of an emergency stop of the robot, the robot can be stopped quickly while maintaining the coordinates and orientation of the hand on the target trajectory. [Brief description of the drawings]

[0020] [Figure 1] FIG. 1 is a diagram illustrating an example of a configuration of an articulated robot. [Diagram 2] FIG. 2 is a plan view illustrating the movement of the robot shown in FIG. [Diagram 3] 3 is a diagram illustrating the position, angle, speed, and angular velocity of each part of the robot. FIG. [Figure 4] FIG. 2 is a block diagram showing an example of a configuration of a robot controller. [Diagram 5] FIG. 2 is a block diagram showing the configuration of an all-axis control unit. [Figure 6]FIG. 2 is a block diagram showing a configuration of a motor drive control unit. [Figure 7] 11A and 11B are diagrams illustrating angles and angular velocities of each part of the robot during an emergency stop. [Figure 8] FIG. 13 is a diagram illustrating the trajectory of the robot during an emergency stop. [Figure 9] FIG. 2 is a timing diagram illustrating an emergency stop method according to the first embodiment of the present invention. [Figure 10] FIG. 2 is a block diagram showing a configuration of a motor drive control unit. [Figure 11] FIG. 11 is a timing diagram illustrating an emergency stop method according to the second embodiment. [Figure 12] 11A and 11B are diagrams illustrating changes in the position and posture of the hand accompanying an emergency stop. [Figure 13] 1A to 1C are diagrams illustrating the positions, angles, speeds, and angular velocities of each part of the robot during an emergency stop. [Figure 14] FIG. 2 is a block diagram showing the configuration of an all-axis control unit. [Figure 15] FIG. 2 is a block diagram showing a configuration of a motor drive control unit. DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS

[0021] Next, an embodiment for carrying out the present invention will be described. A robot controller based on the present invention is used to control a robot having multiple axes, but in the following, it is assumed that a horizontal articulated robot having three joints as shown in Fig. 1 is the control target of the robot controller. Of course, the present invention can also be applied to a robot controller that controls a robot other than the robot shown in Fig. 1.

[0022] In FIG. 1, (a) and (b) are a top view and a front view of the horizontal articulated robot, respectively. In the horizontal articulated robot shown in FIG. 1, one end of a link 11 is connected to a base 10 via a joint 1, one end of a link 12 is connected to the other end of the link 11 via a joint 2, and one end of a hand 13 is connected to the other end of the link 12 via a joint 3. The other end of the hand 13 is the hand position of this robot. The axes of the joints 1 to 3 are all oriented vertically. The joint 1 is provided with a motor 21 and a reducer 31 connected to the motor 21, and the link 11 moves in a horizontal plane by driving the motor 21. Similarly, the joint 2 is provided with a motor 22 and a reducer 32, and the link 12 moves in a horizontal plane by driving the motor 22, and the joint 3 is provided with a motor 23 and a reducer 33, and the hand 13 moves in a horizontal plane by driving the motor 23. As shown in FIG. 1(a), the XY Cartesian coordinates are defined in a horizontal plane with the axis position of the joint 1 as the center. This robot is controlled and driven so that the hand 13 moves back and forth along the Y direction (also called the travel direction) on the illustrated straight line L while keeping the longitudinal direction of the hand 13 parallel to the Y axis. The straight line L extends in the Y direction and indicates the target trajectory of the robot's hand position. The straight line L is offset from the axis of the joint 1 by a predetermined distance in the X direction (also called the lateral direction).

[0023] FIG. 2 shows how the positions and postures of the links 11, 12 and the hand 13 change when the robot shown in FIG. 1 is operated on a normal trajectory. As shown in FIG. 2, the robot operates between a state in which the links 11, 12 and the hand 13 are folded in a Z-shape and a state in which they are extended. In the folded state shown in FIG. 2(a), the joint 3 is on a straight line extending from the joint 1 in the X direction. When moving from this state to the extended state, the link 11 rotates counterclockwise around the axis of the joint 1, and reaches a state (shown in FIG. 2(b)) in which the longitudinal direction of the hand 13 and the longitudinal direction of the link 12 are perpendicular to each other. After that, the link 11 rotates clockwise, and reaches the extended state shown in FIG. 2(c). The hand 13 moves in the Y direction so that its longitudinal center line is always on the straight line L.

[0024] 3 is a graph for explaining the changes in the position, angle, speed, and angular velocity of each part of the robot when the robot moves from the state shown in FIG. 2(a) to the state shown in FIG. 2(c), and shows a case in which the robot in the state shown in FIG. 2(a) starts moving at time A, reaches the state shown in FIG. 2(c) at time C, and then stops. In FIG. 3, (a) shows the Y coordinate value of the position of the tip of the hand 13, (b) shows the joint angle of joint 1, (c) shows the joint angle of joint 2, and (d) shows the joint angle of joint 3. Furthermore, in FIG. 3, (e) shows the velocity of the hand 13 along the Y direction, (f) shows the joint angular velocity of joint 1, (g) shows the joint angular velocity of joint 2, and (h) shows the joint angular velocity of joint 3.

[0025] FIG. 4 is a block diagram showing the configuration of a robot controller 15 that controls the horizontal articulated robot shown in FIG. 1. In the robot, a portion consisting of the links 11, 12 and the hand 13 is collectively called a manipulator 14, and this manipulator 14 is shown to be moved by joints 1 to 3 that are driven by motors 21 to 23 via reducers 31 to 33, respectively. The robot controller 15 includes a motor drive control unit 40 that is provided for each of the motors 21 to 23 and servo-controls the corresponding motor according to a position command value for each motor, an all-axis control unit 50 that collectively calculates position command values ​​for the motors 21 to 23, and an emergency stop signal input unit 90 to which an emergency stop signal for emergency stopping the robot is input from the outside. Each of the motors 21 to 23 is equipped with an encoder, and the motor position detected by the encoder is fed back to the motor drive control unit 40 for servo control. The emergency stop signal received by the emergency stop signal input unit 90 is sent to each motor drive control unit 40 and the all-axis control unit 50. Further, in order for the all-axis control unit 50 to know the state of the motor drive control unit 40 , a signal indicating the emergency stop state of that motor drive control unit 40 is sent from each motor drive control unit 40 to the all-axis control unit 50 .

[0026] 5 is a block diagram showing the configuration of the all-axis control unit 50. The all-axis control unit 50 includes a hand position command calculation unit 51 that calculates a hand position command, an inverse kinematics calculation unit 52 that performs inverse kinematics calculation based on the hand position command and calculates a position command (joint angle position command) represented by the joint angle of each axis, and a reverse speed ratio calculation unit 43 that performs inverse speed ratio calculation based on the position command represented by the joint angle of each axis and calculates a position command value for each of the motors 21 to 23.

[0027] Next, a method for emergency stopping the robot shown in FIG. 1 will be described. Before describing the emergency stopping method based on the present invention, a conventional emergency stopping method will be described. As described above, the conventional emergency stopping method includes a method for decelerating and stopping the hand while maintaining the coordinates and orientation of the hand on the target trajectory by inverse kinematics calculation, and a method for decelerating and stopping the motor by generating a command value inside the motor drive control unit based on the motor coordinate system of each axis. In the method for decelerating and stopping the hand while maintaining the hand on the target trajectory, when an emergency stop signal is input while the robot is operating normally and the hand 13 is moving along the straight line L (see FIG. 2), the all-axis control unit 50 switches from the normal operation mode to the emergency deceleration stop mode, and calculates position command values ​​for the motors 21 to 23 by inverse kinematics calculation to stop the robot while moving the robot along the target trajectory. The calculated position command values ​​are sent to each motor drive control unit 40. In this case, due to the processing time required for the inverse kinematics calculation and the transmission interval of the communication between the all-axis control unit 50 and each motor drive control unit 40, it takes some time for the position command value transmitted from the all-axis control unit 50 to each motor drive control unit 40 to be switched from one for normal operation to one for emergency stop. For example, a delay of about several milliseconds occurs from when the all-axis control unit 50 receives the emergency stop signal until the deceleration operation of each of the motors 21 to 23 starts.

[0028] On the other hand, when the motor is decelerated and stopped by calculating a command value within the motor drive control unit based on the motor coordinate system of each axis, each motor drive control unit 40 is configured to be able to receive an emergency stop signal. Figure 6 shows the configuration of the motor drive control unit 40 in this case, where the speed command value is generated independently within the motor drive control unit 40. As described above, the motor position is fed back to the motor drive control unit 40 from the encoder attached to the motor. The motor drive control unit 40 includes a position control unit 41 that calculates a speed command value based on a position command value and the motor position, a speed calculation unit 42 that calculates the motor speed from the motor position, a speed control unit 43 that calculates a torque command value for the motor based on the speed command value and the motor speed input from the speed calculation unit 42, a stop speed command calculation unit 44 that calculates a braking speed command value for decelerating and stopping the motor based on the speed command value input from the position control unit 41 when an emergency stop signal is input, and a selector 45 that switches between the speed command value output by the position control unit 41 and the speed command value for braking calculated by the stop speed command calculation unit 44 depending on the presence or absence of the emergency stop signal and inputs the selected value to the speed control unit 43. The torque command value output by the speed control unit 43 is converted into a current, and the motor is driven by the current obtained by the conversion.

[0029] FIG. 7 is a graph for explaining the change in the angle and angular velocity of each part of the robot when an emergency stop operation is performed using the motor drive control unit 40 shown in FIG. 6. Here, it is assumed that an emergency stop signal is input during the movement of the robot in the normal operation shown in FIG. 3, and the emergency stop operation is started, and the broken lines indicate the joint angles and joint angular velocities in the normal operation shown in FIG. 3. In FIG. 7, (a) indicates the joint angle of joint 1, (b) indicates the joint angle of joint 2, (c) indicates the joint angle of joint 3, (d) indicates the joint angular velocity of joint 1, (e) indicates the joint angular velocity of joint 2, and (f) indicates the joint angular velocity of joint 3. As is clear from the graphs of the angular velocities of each joint shown in (d) to (f), when the emergency stop operation is started, each joint, i.e., each motor, decelerates and stops at the acceleration set for each motor. Accordingly, the angle of each joint changes as shown in the graphs (a) to (c). At this time, the angle of each joint deviates from the angle at which the robot maintains its normal trajectory (the straight line L shown in FIG. 2). FIG. 8 is a diagram for explaining this deviation from the normal trajectory. Fig. 8(a) shows the same state as that shown in Fig. 2(a). If the robot is stopped in an emergency while being operated from this state to the state shown in Fig. 2(c), the robot will stop in the state shown in Fig. 8(b), for example. In the stopped state shown in Fig. 8(b), the position of the robot's hand 13 is significantly deviated from the straight line L indicating the original trajectory, and the longitudinal direction of the hand 13 is also significantly deviated from the direction in which the straight line L extends.

[0030] [First embodiment] As described above, the conventional emergency stop method has a problem that a delay occurs before the motor starts to decelerate, or the coordinates and orientation of the hand cannot be maintained on the target trajectory. Therefore, the emergency stop method of the first embodiment of the present invention combines the above two emergency stop methods, that is, the method of decelerating and stopping the hand by inverse kinematics calculation and the method of generating a command value inside the motor drive control unit based on the motor coordinate system of each axis, to shorten the time from receiving an emergency stop signal to starting deceleration of the motor, while stopping the robot while maintaining the coordinates and orientation of the hand on the target trajectory. Specifically, each motor drive control unit 40 is made capable of calculating a position command value for emergency stop (position command value for stopping each axis), and when an emergency stop signal is input, each motor drive control unit 40 switches the position command value used for servo control of the motor from the position command value from the all-axis control unit 50 to the position command value for stopping each axis, thereby decelerating the motor. In parallel with this, when an emergency stop signal is input, the all-axis control unit 50 starts a calculation for decelerating and stopping the hand by inverse kinematics calculation, and when a predetermined time has elapsed, each motor control drive unit 40 gradually switches from servo control based on the position command value for stopping each axis to servo control for decelerating and stopping the hand by inverse kinematics calculation. Finally, the robot is stopped by control based only on the position command value for decelerating and stopping the hand in inverse kinematics calculation. The start of switching from servo control based on the position command value for stopping each axis to servo control based on the position command value for decelerating the hand calculated by inverse kinematics calculation is given by a position command switching command. Figure 9 explains such an emergency stop operation in the first embodiment.

[0031] In the timing diagram shown in FIG. 9, in the initial state, the robot is performing normal operation, the hand 13 is moving at a predetermined speed, and both the emergency stop signal and the position command switching command are in the OFF state ("0"). The position command sent from the all-axis control unit 50 to each motor drive control unit 40 is for normal operation, and inside the motor drive control unit 40, servo control of the motor is performed based on the position command value from the all-axis control unit 50. Now, assume that at time P, the emergency stop signal is switched from OFF to ON ("1"). At this point, the position command switching command is still OFF, and the position command sent from the all-axis control unit 50 to each motor drive control unit 40 is for normal operation. Even when the emergency stop signal is input, the all-axis control unit 50 continues to output the position command value for normal operation, but starts a motion plan for decelerating the hand.

[0032] On the other hand, when an emergency stop signal is input at time P, the motor drive control unit 40 switches the position command value used for servo control of the motor from that from the all-axis control unit 50 to the position command value for stopping each axis. As a result, the motor starts to decelerate, and the speed of the hand position also starts to decrease. The position command value for stopping each axis is a position command value calculated for each motor based on the motor coordinate system reference in order to decelerate and stop the motor by, for example, the acceleration or deceleration time set for each motor. After that, at time Q, the position command switching command is switched from OFF to ON. Then, the all-axis control unit 50 stops outputting the position command value for normal operation and starts outputting the position command value for emergency stop, that is, for decelerating and stopping the hand while maintaining the target trajectory. At this point, the motor drive control unit 40 may switch the position command value used for servo control to the position command value for emergency stop from the all-axis control unit 50, but if this is done, the robot may not be able to operate smoothly, and excessive torque, vibration, impact, etc. may occur. Therefore, in this embodiment, the period from time Q to time R is set as a transition period, and the position command value is gradually switched within the transition period. For example, the position command value used for servo control inside the motor drive control unit 40 is Pc, the position command value for emergency stop output from the all-axis control unit 50 is Pcik, the position command value for stopping each axis generated inside the motor drive control unit 40 is Pcm, and the elapsed time from time Q is t (where t≧0), Pc=r(t)×Pcik+{1-r(t)}×Pcm (1) Here, r(t) is the distribution rate, and if the length of the transition period is T, then If t ≤ T r(t)=a×t If t>T, r(t)=1 a is a parameter that indicates the time rate of change of the proportional ratio, and is expressed as a=1 / T. Even during the transition period, the speed of the hand position continues to decrease. The length of the transition period, T, is determined according to the configuration of the robot, etc.

[0033] When the time R arrives and the transition period ends, the position command value used for servo control inside the motor control drive unit 40 is completely switched to the emergency stop position command value from the all-axis control unit 50, the motor continues to decelerate, and the speed of the hand position also decreases toward 0. The speed of the hand position is also shown in FIG. 9. The hand position speed shown by the dashed line here indicates the hand position speed when only the deceleration and stop of the hand is performed by the inverse kinematics calculation. In the case of only the deceleration and stop of the hand by the inverse kinematics calculation, the motor starts to decelerate after the inverse kinematics calculation in the all-axis control unit 50 is completed, which is considered to be immediately before the time Q. Therefore, according to the emergency stop method of this embodiment described here, the start of the deceleration of the motor is accelerated from time Q to time P, compared to the case where the emergency stop is performed only by the deceleration and stop that maintains the trajectory of the hand at the target value, and the time until the robot's hand stops completely is shortened accordingly.

[0034] The robot controller used to implement the emergency stop method of this embodiment has the same basic configuration as the robot controller 15 described using Fig. 4, but differs in the internal configuration of the motor drive control unit 40. The all-axis control unit 50 shown in Fig. 5 can be used. However, in addition to signals indicating an emergency stop state from each motor drive control unit 40, the all-axis control unit 50 also receives signals indicating whether the position command value used for servo control inside the motor drive control unit 40 is from the all-axis control unit 50 or a position command value for stopping each axis.

[0035] 10 shows the configuration of the motor drive control unit 40 used in the first embodiment. The motor drive control unit 40 includes a position control unit 41 that calculates a speed command value based on a position command value and a motor position, a speed calculation unit 42 that calculates a motor speed from the motor position, a speed control unit 43 that calculates a torque command value for the motor based on the speed command value and the motor speed input from the speed calculation unit 42, a stop position command calculation unit 46 that receives an emergency stop signal and calculates each axis stop position command value based on a position command value from the all-axis control unit 50, a command switching transition calculation unit 47, and a selector 48 that switches between the position command value from the all-axis control unit 50 and the position command value obtained by the calculation in the command switching transition calculation unit 47 based on the emergency stop signal and inputs it to the position control unit 41. Before the position command switching command is input, the command switching transition calculation unit 47 outputs each axis stop position command value output by the stop position command calculation unit 46 to the selector 48, and when the position command switching command is input, outputs the result of calculation according to the above formula (1). The selector 48 selects the position command value from the all-axis control unit 50 when the emergency stop signal is not input, and selects the position command value output by the command position transition calculation unit 47 when the emergency stop signal is input. As a result, the motor drive control unit 40 performs servo control of the motor using the position command value from the all-axis control unit 50 until the emergency stop signal is input, and performs servo control of the motor using the position command value for stopping each axis until the emergency stop signal is input and the position command switching command is input. Furthermore, after the position command switching command is input, the motor drive control unit 40 performs servo control of the transition period based on the formula (1). In particular, after the transition period ends, the command switching transition calculation unit 47 outputs the position command value from the all-axis control unit 50 as it is, so the motor drive control unit 40 performs servo control of the motor using the position command value for emergency stop from the all-axis control unit 50.

[0036] In this embodiment, the position command switching command may be output by the all-axis control unit 50 when it becomes possible for the all-axis control unit 50 to output a position command value for an emergency stop, or may be output by the emergency stop signal input unit 90 when a predetermined time has elapsed since the input of the emergency stop signal. Furthermore, instead of calculating the position command switching command outside the motor drive control unit 40, the position command switching command may be calculated inside the motor drive control unit 40 after a predetermined time has elapsed since the input of the emergency stop signal.

[0037] According to the emergency stop method of this embodiment described above, it is possible, with simple calculations, to shorten the time from when an emergency stop signal is received until the motor starts to decelerate, while stopping the robot while maintaining the coordinates and orientation of the hand on the target trajectory.

[0038] [Second embodiment] Next, an emergency stop method according to a second embodiment of the present invention will be described. In the above-mentioned first embodiment, a transition period is provided, and during the transition period, the position command value for stopping each axis and the internal position command for stopping are proportionally divided according to the passage of time, thereby smoothly switching from the position command value for stopping each axis to the position command value for emergency stop from the all-axis control unit 50. However, by performing a correction calculation on the all-axis control unit 50 side rather than performing a proportional division calculation in the motor drive control unit 40, a smoother switching from the position command value for stopping each axis to the position command value for emergency stop from the all-axis control unit 50 can be realized. In the second embodiment, a case where a correction calculation is performed on the all-axis calculation unit 50 side will be described. FIG. 11 is a timing diagram for explaining the emergency stop method according to the second embodiment. In the timing diagram shown in FIG. 11, the operation before time P is the same as that described using FIG. 9.

[0039] In the timing diagram shown in FIG. 11, it is assumed that the emergency stop signal is switched from OFF ("0") to ON ("1") at time P. At this time, the position command switching command is still OFF, and the position command sent from the all-axis control unit 50 to each motor drive control unit 40 is for normal operation. Even when the emergency stop signal is input, the all-axis control unit 50 continues to output position command values ​​for normal operation, but starts an operation plan for deceleration stop that maintains the coordinates and orientation of the hand on the target trajectory. In particular, the all-axis control unit 50 estimates the robot position when the motor drive control unit 40 performs servo control based on the position command value for stopping each axis, and starts a calculation (correction calculation) for a correction operation that corrects the position and posture of the robot based on the estimation result. When the emergency stop signal is input at time P, the motor drive control unit 40 switches the position command value used for servo control of the motor from the one from the all-axis control unit 50 to the position command value for stopping each axis. This causes the motor to start decelerating, and the speed of the hand position also starts to decrease.

[0040] After that, at time Q, the position command switching command is switched from OFF to ON. Then, the all-axis control unit 50 stops outputting the position command value for normal operation, and outputs a position command value for correction operation to correct deviation from the original trajectory that occurs when the robot is controlled based on the position command value for stopping each axis. Then, when the robot returns to the original trajectory, i.e., the target trajectory, at time R, it continues to output a position command value for emergency stop. When the position command switching command is switched ON, the motor drive control unit 40 switches the position command value used for servo control inside it from the position command value for stopping each axis to the position command value output by the all-axis control unit 50. Through the above process, the hand position speed is decelerated and quickly reaches 0, similar to the case shown in FIG. 9.

[0041] FIG. 12 is a diagram showing the change in the position and posture of the hand 13 of the robot when an emergency stop is performed in the second embodiment, and shows how the hand 13 moves in the XY plane. When an emergency stop signal is input and servo control based on the internal position command for stopping in each motor drive control unit 40 is started, the position of the hand 13 deviates from the original trajectory, and the longitudinal direction of the hand 13 also becomes oblique to the Y direction. Then, when servo control based on the position command value of the correction operation from the all-axis control unit 50 is started, the position and posture of the hand 13 are aligned with the original trajectory, and the hand 13 is stopped while maintaining the position and posture on the original trajectory by the subsequent position command value for emergency stop from the all-axis control unit 50. Here, the amount of deviation between the position of the joint 3 in the hand 13 and the original trajectory is defined as the abscissa error Δx, and the angle that the longitudinal direction of the hand 13 makes with the Y direction is defined as the azimuth error Δθ.

[0042] FIG. 13 is a graph showing changes in the position, angle, speed, and angular velocity of each part of the robot when an emergency stop operation of the robot is performed in the second embodiment. In FIG. 13, (a) shows the Y coordinate value of the hand position, (b) shows the X-direction position of the joint 3, i.e., the abscissa error Δx, (c) shows the azimuth angle error Δθ, (d) shows the Y-direction velocity of the hand position, (e) shows the X-direction velocity of the hand position, and (f) shows the time change in the azimuth angle error Δθ, i.e., the angular velocity for the azimuth angle. The period from the timing (time P) when the emergency stop signal is input to the timing (time Q) when the position command switching command is input is a period during which each motor is servo-controlled based on the position command value for stopping each axis calculated in the motor drive control unit 40, and FIG. 13 does not show changes in the position, angle, speed, and angular velocity during this period. Until the emergency stop signal is input at time P, the robot moves along a predetermined trajectory. When an emergency stop signal is input, each motor is servo-controlled based on the position command value for stopping each axis calculated in the motor drive control unit 40, so that the robot deviates from the trajectory. The amount of deviation can be determined from the abscissa error Δx and the azimuth error Δθ at time Q. During the period from time Q to time R, each motor is servo-controlled based on the position command value for the corrective operation output by the all-axis control unit 50, and the hand posture, etc. are corrected so that the robot returns to the original trajectory. The motors have already started decelerating at time Q due to control based on the position command value for stopping each axis, and the position command value for the corrective operation is planned to achieve a smooth correction operation with the coordinates and speed of the hand at time Q as initial values. As a result, at time R, the robot has returned to the original trajectory. After that, each motor is controlled based on the position command value for emergency stop for deceleration stop that maintains the coordinates and orientation of the hand on the target trajectory, and the robot finally stops on the target trajectory.

[0043] The robot controller for performing the emergency stop operation of the second embodiment includes an all-axis control unit 50 and a motor drive control unit 40 for each motor, similar to that used in the first embodiment. In the case of the second embodiment, as described with reference to Figs. 12 and 13, the all-axis control unit 50 needs to output a position command value for a corrective operation at time Q. On the other hand, it takes time for the all-axis control unit 50 to start outputting a position command for decelerating and stopping the hand (i.e., a position command for an emergency stop). Therefore, the all-axis control unit 50 estimates the position and speed of each motor at time Q, and estimates the coordinates and speed of the hand at time Q based on the position and speed. Fig. 14 shows the configuration of the all-axis control unit 50 used in this embodiment.

[0044] The all-axis control unit 50 shown in Fig. 14 is obtained by adding an estimated value calculation unit 56 for estimating the position and speed of the robot at time Q, and a position command value calculation unit 57 for calculating a position command value for corrective operation and an emergency stop position command value for decelerating and stopping to maintain the coordinates and orientation of the hand on the target trajectory based on the estimated value obtained by the estimated value calculation unit 56. The reverse speed ratio calculation unit 53, which outputs a position command value for normal operation, outputs a position command differential value in addition to the position command value. An emergency stop signal is also input to the estimated value calculation unit 56.

[0045] The estimated value calculation unit 56 includes a post-deceleration angle calculation unit 61 that receives the position command value output by the reverse speed ratio calculation unit 53 at the time when the emergency stop signal is input and estimates the motor position after deceleration by the position command value for stopping each axis, a speed ratio calculation unit 62 that performs a speed ratio calculation based on the motor position estimated by the post-deceleration angle calculation unit 61, and a kinematics calculation unit 63 that performs a kinematics calculation based on the calculation result by the speed ratio calculation unit 62 to calculate a hand position estimated value. The estimated value calculation unit 56 further includes a post-deceleration angular velocity calculation unit 64 that receives the position command differential value output by the reverse speed ratio calculation unit 53 at the time when the emergency stop signal is input and estimates the motor speed after deceleration by the position command value for stopping each axis, a speed ratio calculation unit 65 that performs a speed ratio calculation based on the motor speed estimated by the post-deceleration angular velocity calculation unit 61, and a Jacobian calculation unit 66 that performs a Jacobian calculation based on the calculation result by the speed ratio calculation unit 62 to calculate a hand speed estimated value. When each motor drive control unit 40 decelerates the motor based on the position command value for stopping each axis, control is executed to decelerate the motor with the acceleration (or deceleration time) set for each motor. Since the acceleration for deceleration at this time is known, the post-deceleration angle calculation unit 61 and the post-deceleration angular velocity calculation unit 64 can estimate the position and velocity of each motor at time Q based on the position command value and position command differential value output by the reverse speed ratio calculation unit 53 at the time when the emergency stop signal is input. Then, the hand position and hand velocity can be estimated by performing a speed ratio calculation on the estimated values ​​of the motor position and motor velocity, and further performing a kinematic calculation and a Jacobian calculation. The hand position estimate value includes the Y-direction coordinate of the hand position, the X-direction coordinate of the hand position (or the horizontal coordinate error Δx), and the azimuth angle error Δθ. The hand velocity estimate value includes the Y-direction velocity of the hand position, the X-direction velocity of the hand position, and the time change of the azimuth angle error Δθ (i.e., the azimuth angle angular velocity).

[0046] The position command value calculation unit 57 includes a hand end position command calculation unit 71 that calculates a hand end position command for deceleration based on the Y-direction coordinate and Y-direction velocity of the hand end position calculated by the estimated value calculation unit 56, an inverse kinematics calculation unit 74 that performs inverse kinematics calculation on the hand end position command calculated by the hand end position command calculation unit 71, and an inverse speed ratio calculation unit 75 that performs inverse speed ratio calculation on the calculation result of the inverse kinematics calculation unit 74 to output a position command value. If the hand end position command is calculated based only on the Y-direction coordinate and velocity, deviation from the robot's trajectory cannot be corrected. Therefore, the position command value calculation unit 57 further includes an error correction amount calculation unit 72 that calculates an error correction amount based on the X-direction coordinate of the hand end position, the azimuth error Δx, the X-direction velocity of the hand end position, and the azimuth velocity, and an adder 73 that adds the error correction amount to the hand end position command calculated by the hand end position command calculation unit 71. The hand end position command to which the error correction amount is added in the adder 73 is the target of the inverse kinematics calculation in the inverse kinematics calculation unit 74. By using such a position command value calculation unit 57, it is possible to calculate not only an emergency stop position command value for decelerating and stopping while maintaining the trajectory of the hand at a target value, but also a position command value for a corrective operation.

[0047] FIG. 15 shows the configuration of the motor drive control unit 40 that can be used in the second embodiment. The motor drive control unit 40 includes a position control unit 41 that calculates a speed command value based on a position command value and a motor position, a speed calculation unit 42 that calculates a motor speed from the motor position, a speed control unit 43 that calculates a torque command value for the motor based on the speed command value and the motor speed input from the speed calculation unit 42, and a stop position command calculation unit 46 that receives an emergency stop signal and calculates a position command value for stopping each axis based on a position command value from the all-axis control unit 50. In addition, selectors 48 and 49 are provided to switch the position command value input to the position control unit 41. The selector 48 switches between the position command value from the all-axis control unit 50 and the position command value output by the selector 49 in response to the emergency stop signal and inputs it to the position control unit 41. The selector 49 switches between the position command value from the all-axis control unit 50 and the position command value for stopping each axis calculated by the stop position command calculation unit 46 in response to a position command switching command and outputs it. Selectors 48, 49 input the position command values ​​for stopping each axis to position control unit 41 during the period from when an emergency stop signal is input to when a position command switching command is input, and during other periods, the position command values ​​from all-axis control unit 50 are input to position control unit 41.

[0048] According to the emergency stop method of the second embodiment described above, the calculation processing in the all-axis control unit 50 is more complicated than in the first embodiment, but the robot can be brought to an emergency stop more smoothly on the target trajectory.

[0049] The above describes an example of an emergency stop method according to the present invention applied to a three-axis horizontal articulated robot. The robots to which the present invention can be applied are not limited to those described here, and the present invention can also be applied to robots other than horizontal articulated robots. In general, with a robot having many axes, it is often difficult to quickly stop the robot while keeping the coordinates and orientation of the hand on the target trajectory. However, by applying the present invention, it becomes possible to quickly stop such a robot while keeping the coordinates and orientation of the hand on the target trajectory. [Explanation of symbols]

[0050] 1~3...joints; 10...base; 11, 12...links; 13...hand; 14...manipulator; 21~23...motors; 31~33...reduction gear; 40...motor drive control unit; 41...position control unit; 42...speed calculation unit; 43...speed control unit; 44...stop speed command calculation unit; 45, 48, 49...selector; 46...stop position command calculation unit; 47...command switching transition calculation unit; 50...all-axis control unit; 51, 71...hand position command calculation unit; 52, 74...inverse kinematics calculation unit; 53, 75...inverse speed ratio calculation unit; 56...estimated value calculation unit; 57...position command value calculation unit; 61...post-deceleration angle calculation unit; 62, 65...speed ratio calculation unit; 63...kinematics calculation unit; 64...post-deceleration angular velocity calculation unit; 66...Jacobian calculation unit; 72...error correction amount calculation unit; 73...addition unit.

Claims

1. A robot controller for controlling a robot having a plurality of axes and driven by a motor provided for each axis, comprising: an all-axis control unit that collectively calculates position command values ​​for the plurality of axes based on a predetermined trajectory of the robot; a motor drive control unit provided for each of the axes, which servo-controls a motor of the corresponding axis based on the position command value for the corresponding axis from the all-axis control unit; having the motor drive control unit includes a stop position command calculation unit that calculates an axis stop position command value for stopping the corresponding motor based on a motor coordinate system; When an emergency stop signal is input, the motor drive control unit switches a position command value used for servo control from the position command value from the all-axis control unit to the position command value for stopping each axis to servo-control the corresponding motor, and then returns the position command value used for servo control from the position command value for stopping each axis to the position command value from the all-axis control unit to continue the servo control of the corresponding motor; when the emergency stop signal is input, the all-axis control unit starts a calculation for outputting an emergency stop position command value for stopping the robot on the predetermined trajectory; A robot controller in which, based on a position command switching command calculated by the all-axis control unit, the motor drive control unit returns the position command value used for the servo control from the position command value for stopping each axis to the position command value from the all-axis control unit.

2. 2. The robot controller according to claim 1, wherein, during a transition period in which the position command value used for the servo control is returned from the position command value for stopping each axis to the position command value from the all-axis control unit, the motor drive control unit calculates a position command value by apportioning the position command value for stopping each axis and the position command value from the all-axis control unit while changing an apportionment ratio, and uses the position command value for the servo control.

3. The all-axis control unit includes: an estimated value calculation unit that estimates a motion of the robot caused by servo-controlling the motor of each axis based on the each axis stop position command value; a position command value calculation unit that calculates a position command value for a corrective operation to return the robot to the predetermined trajectory based on a result of the estimation by the estimated value calculation unit; having 2. The robot controller according to claim 1, wherein the position command value calculation unit calculates the emergency stop position command value subsequent to the corrective operation position command value.

4. A robot controller for controlling a robot having multiple axes and driven by a motor provided for each axis, comprising: an all-axis control unit that collectively calculates position command values ​​for the plurality of axes based on a predetermined trajectory of the robot; a motor drive control unit provided for each of the axes, which servo-controls a motor of the corresponding axis based on the position command value for the corresponding axis from the all-axis control unit; having the motor drive control unit includes a stop position command calculation unit that calculates an axis stop position command value for stopping the corresponding motor based on a motor coordinate system; The all-axis control unit includes: an estimated value calculation unit that estimates a motion of the robot caused by servo-controlling the motor of each axis based on the each axis stop position command value; a position command value calculation unit that calculates a position command value for a corrective operation to return the robot to the predetermined trajectory based on a result of the estimation by the estimated value calculation unit; having When an emergency stop signal is input, the motor drive control unit switches a position command value used for servo control from the position command value from the all-axis control unit to the position command value for stopping each axis to servo-control the corresponding motor, and then returns the position command value used for servo control from the position command value for stopping each axis to the position command value from the all-axis control unit to continue the servo control of the corresponding motor; when the emergency stop signal is input, the all-axis control unit starts a calculation for outputting an emergency stop position command value for stopping the robot on the predetermined trajectory; The position command value calculation unit calculates the emergency stop position command value following the corrective operation position command value.

5. 5. The robot controller according to claim 1, wherein the position command value for stopping each axis is a position command value for decelerating and stopping the motor according to an acceleration or deceleration time set for each motor.

6. 1. An emergency stop method for a robot having a plurality of axes, in which a motor for each axis is servo-controlled based on a position command value calculated collectively for the plurality of axes by an all-axis control unit so that the robot moves along a predetermined trajectory, comprising: when an emergency stop signal is input, a position command value used for servo control is switched for each of the axes from the position command value from the all-axis control unit to a position command value for stopping each axis calculated based on a motor coordinate system in order to stop the corresponding motor, and thereafter, the position command value used for servo control is returned from the position command value for stopping each axis to the position command value from the all-axis control unit; when the emergency stop signal is input, a calculation is started in the all-axis control unit for outputting an emergency stop position command value for stopping the robot on the predetermined trajectory; An emergency stop method in which a position command value used for the servo control is returned from the position command value for stopping each axis to the position command value from the all-axis control unit based on a position command switching command calculated by the all-axis control unit.

7. 7. The emergency stop method according to claim 6, wherein, during a transition period in which the position command value used for the servo control is returned from the position command value for stopping each axis to the position command value from the all-axis control unit, a position command value is calculated by apportioning the position command value for stopping each axis and the position command value from the all-axis control unit while changing a proportioning ratio, and the position command value is used for the servo control.

8. 7. The emergency stop method according to claim 6, wherein, when the emergency stop signal is input, the all-axis control unit estimates the movement of the robot caused by servo-controlling the motors of each axis using the position command values ​​for stopping each axis, and calculates position command values ​​for a corrective operation to return the robot to the specified trajectory based on the estimated robot movement.

9. A method for emergency stopping a robot having a plurality of axes and in which a motor for each axis is servo-controlled based on position command values ​​calculated collectively for the plurality of axes by an all-axis control unit so that the robot moves along a predetermined trajectory, comprising: when an emergency stop signal is input, a position command value used for servo control is switched for each of the axes from the position command value from the all-axis control unit to a position command value for stopping each axis calculated based on a motor coordinate system in order to stop the corresponding motor, and thereafter, the position command value used for servo control is returned from the position command value for stopping each axis to the position command value from the all-axis control unit; when the emergency stop signal is input, a calculation is started in the all-axis control unit for outputting an emergency stop position command value for stopping the robot on the predetermined trajectory; An emergency stop method in which, when the emergency stop signal is input, the all-axis control unit estimates the movement of the robot caused by servo-controlling the motors of each axis using the position command values ​​for stopping each axis, and calculates position command values ​​for corrective operation to return the robot to the specified trajectory based on the estimated robot movement.

10. 10. The emergency stop method according to claim 6, wherein the position command value for stopping each axis is a position command value for decelerating and stopping the motor according to an acceleration or deceleration time set for each motor.

Images