JP2002283276A - Collision detecting-stopping control method in articulated robot - Google Patents

Abstract

PROBLEM TO BE SOLVED: To highly accurately detect the occurrence of a collision in a short time from a fluctuation in joint driving torque generated by the collision, to relieve interference force from an obstacle at stopping processing time after detecting the collision, and to minimize damage. SOLUTION: Torques T1 , T2 and T3 to be generated by respective motors at collision nonoccurrence time are anticipated by an inverse dynamic operation from a position θm, a speed θd m, and acceleration θdd m of the motors detected by servo drivers 21 , 22 and 23 for controlling the respective joint driving motors 11 , 12 and 13 of a robot. A difference ΔT between the torque anticipative value and a torque command value Tm actually generated by the servo drivers or the joint driving torque calculated from a torque response value is determined. When an absolute value of the difference ΔT is larger than a preset determining value ε, it is regarded that the collision is caused. After the collision, the joints are made to perform escape operation to reduce the interference force.

Description

DETAILED DESCRIPTION OF THE INVENTION

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a collision detection / stop control method for an articulated robot. More specifically, the present invention relates to an articulated robot which, when operating, collides with an external object or obstacle such as a workpiece. The present invention also relates to a robot control method that detects accurately and then stops the movement of the robot or releases the force promptly to avoid deformation and damage of the robot body and tools.

[0002]

2. Description of the Related Art A link of a robot composed of a plurality of joints or a tool or a work held at the tip of the robot,
When performing a reproducing operation based on teaching data or moving by a teaching operation using a teach pendant, the object may hit an object to be processed or a nearby obstacle without intention. During the playback operation, the motors that drive each joint of the robot follow the command value.If the robot continues teaching without noticing the collision during teaching, the robot naturally moves even if it breaks through obstacles etc. And As a result, an excessive torque is generated depending on the joint, which may cause deformation of the link or the tool or damage to the movable part of the robot such as the joint.

Each servo driver for controlling each motor for executing the operation of reproducing the work data taught to the robot has a position and speed control system incorporated therein, as shown in FIG. The motor 1 receives the position command given from the control device (robot controller).
Is controlled so as to perform the operation of reproducing the work data. In each servo driver 2, a torque limiter 3 is disposed after the speed control system, so that a torque command generated by the speed control can be limited to an arbitrary value.

As can be seen from this control system, if the error component of the position feedback system, which is the outermost loop, that is, the magnitude (position deviation) obtained by subtracting the position feedback value from the position command, is monitored, the presence or absence of a collision is determined. Can be detected. The most basic one is, as shown in FIG. 15, a position deviation obtained as a result of feedback control of the position of the motor 1 immediately after the adder 4 in the position control loop. In comparison with the determination value 6 for each joint, which is a "threshold value", if it is larger, it is determined that a collision has occurred.

According to such control, when a shock sensor torch collides, a switch is turned on and a servo power supply is turned off, or a conventional detection device such as a detection device equipped with a mechanical collision detection mechanism. By comparison, the provision of even the minimum necessary components of the robot, that is, a robot can detect a collision only by installing collision detection software in a robot controller without having a collision sensor. Therefore, it is possible to prevent the mechanism of the robot body from becoming complicated.

As another method for detecting a collision, there is a collision detection method using a disturbance estimation observer described in Japanese Patent No. 2,749,724. This is because each servo driver has an observer that observes the disturbance torque, estimates the magnitude of the inertia that the motor is actually trying to turn by using an internal model, and caused the collision. This is to detect a disturbance torque and determine the presence or absence of a collision by looking at the magnitude of the estimated value of the disturbance torque.

[0007] That is, by performing an internal modeling, what behavior is to be predicted for a certain command value,
The idea is to compensate for prefetching by matching the predicted behavior with the actual behavior. Therefore, if the inertia can be modeled, the torque can be predicted, and if the inertia does not fluctuate, the collision can be detected with high accuracy. This is because inertia modeling is easy if a single joint is used, regardless of modeling with a plurality of joints.

As described above, the collision can be detected only when there is a change in the servo state variable due to the collision, and the collision is detected by the fluctuation of the torque detected in each motor unit.

[0009] By the way, it is necessary to stop the movement of the robot after detecting the collision. For stopping the movement, it is necessary to generate a torque as large as possible at any joint so that braking can be performed quickly. From the viewpoint, it is general that a maximum torque value is set in the torque limiter. Needless to say, the maximum torque means the maximum torque that can be generated in each motor.

More specifically, the command to the speed control system is forcibly set to 0 by setting the position feedback gain to 0,
Stop processing is performed by performing speed feedback control with speed 0 as the target value. The torque used for deceleration for stopping can always be up to the maximum allowable torque of the motor by a torque limiter arranged after the speed control system.

[0011]

However, in the above-described collision detection system shown in FIG. 15, it is not possible to recognize that a collision has been detected unless error components at any joint position are sufficiently accumulated. For this reason, when the collision is detected, the motor has already generated a large torque, and deformation and damage of the robot body and the tool cannot be reduced as much as possible. That is, although a collision can be detected, it is too late at that time.

[0012] By way of example, the third joint 7 ₃ even trying to hold the third link L ₃ in upright position as indicated by broken line, for example, it if obstacles pressed by the second joint 7 ₂ shown in FIG. 2 Eight. Since the third link L ₃ is about to be moved as shown by the solid line by the obstacle 8 has feeling that feedback control, to the straight position the motor 1 ₃ which drives the third joint L ₃ immediately maximum torque Try to return. Naturally, the interference force increases, and a large force interacts.

As shown in FIG. 15, since the servo driver 2 has a torque control, a speed control on the outside thereof, and a position control on the outermost side, torque is most sensitive to a collision. Next is speed, and position is the least insensitive. Therefore, if an attempt is made to detect a collision based on a positional deviation, the following disadvantages cannot be overcome.

In the feedback control, there is always a position deviation in order to generate a speed and a torque, and it goes without saying that the position deviation becomes large when the operation is performed at a high speed and with a large acceleration. On the other hand, when operating slowly, the positional deviation is small even when the vehicle hits an obstacle.

Therefore, if it is determined that a collision occurs with a small positional deviation, a collision is often erroneously detected because the small positional deviation is immediately exceeded when operating at a high speed and a large acceleration. It just stops and becomes useless. Conversely, if it is determined that the collision is due to a large positional deviation, it is not possible to detect the collision if the vehicle moves at a low speed and a small acceleration without going too far.

As can be seen from the above, even if the most conceivable way of movement is considered, a large set value must be entered so as not to be erroneously recognized as a collision, and the collision detection based on the position deviation is insensitive. It will be something.

When a disturbance estimation observer is used as in Japanese Patent No. 2,749,724, the observer is generally incorporated in a velocity loop. Since this is a story in the servo driver, a disturbance is detected at each joint level. It is easy to model inertia if only one joint is used, but it is extremely difficult to model multiple joints.
It can be said that it is difficult to ensure a sufficiently high collision detection accuracy.

A robot having a plurality of joints has a characteristic that even if an arbitrary motor for driving the joint is operated at the same speed pattern, the torque generated by the motor varies depending on the posture of the robot. This is because load inertia to be driven by the motor and disturbance torque received by the motor vary depending on the joint position of the robot, that is, the posture of the robot and the state of the speed and acceleration of each joint.

Needless to say, the inertia of the robot increases when the arm is extended, and decreases when the arm is shortened. In the case of using a disturbance estimation observer, higher-order control is performed, so that it is practically impossible to define an observer with a plurality of joints. Items required for the accuracy of the observer include interference between joints that is also caused by centrifugal force, Coriolis force, and the like. In the case of a single joint, the squared term of these speeds has no significant effect, but when a plurality of joints are used, the effect becomes large. The disturbance torque due to the gravitational acceleration also changes when the posture of the robot changes. When the radius of gyration changes, the moment due to gravity also changes.

A collision can be detected only when there is a change in the state variable of the servo due to the collision. Since the disturbance estimation observer is built into the servo driver, the detection of the disturbance is at the driver level only, and therefore, it is the fluctuation of the torque detected at each motor unit that detects the collision, which is used for the disturbance detection at each joint level. Will stay.

As a practical problem, it is necessary to know what kind of control object exists at the tip of the motor. However, even if the disturbance torque is detected at one joint level, it is normally detected when there is no inertia fluctuation. Even if you can
In the case of articulated robots with inertia fluctuations or inertia fluctuations that cannot be avoided, it is difficult to distinguish between what happened when the torque fluctuated and what happened due to a collision. .
That is, when there are a plurality of joints, in addition to the above-described fluctuation of the load inertia, there are the following three disturbance torques, and it is difficult to determine the torque.

The following three types of disturbance torque are applied to each joint of a robot having a plurality of joints. one,
Interference torque received from acceleration motion of other joints (changes depending on the joint position and acceleration of the robot). Second, disturbance torque due to centrifugal force and Coriolis force (changes depending on the joint position and speed of the robot). . The third is a disturbance torque due to gravitational acceleration (which varies depending on the joint position of the robot).

As described above, the torque is the sharpest in the detection of a collision. However, even a disturbance estimation observer whose detection is to be detected cannot grasp the inertia fluctuation. Because it works for each joint, it cannot take into account the load inertia and disturbance torque fluctuations linked to the state of the motor driving the other joints, and accurately extracts only the disturbance torque generated by a collision. It will be difficult to do. In view of such circumstances, it must be said that there are still some problems to be solved in order to obtain strictness in the case of an articulated robot.

As described above, when the robot is stopped after the collision, the speed feedback control is performed with the target of the speed 0. However, the torque is still generated due to the function of the integral term, and the torque is large. It is inevitable to become. This is because it is generally assumed that the motor can exert the maximum torque, and if the motor exerts a large force, a large force is necessarily applied between the robot and the obstacle. is there. In other words, the escape operation when interference occurs is hindered by the maximum torque, and there is a problem that the interference force due to the collision cannot be reduced as much as possible.

Incidentally, when a collision occurs, the torque limiter is set to 0.
Cannot be set to. Even if they collide, there is an inertial force, so that only the friction is applied to the inertial force, and the braking force is significantly reduced. In addition to this, it is impossible to brake the motion of the joint due to the impact force due to the collision or the interference torque received from other joints, even for the stopped joint.

SUMMARY OF THE INVENTION The present invention has been made in view of the above problems, and an object of the present invention is to provide a collision detection / stop control method for an articulated robot which can roughly solve the following two problems. The first problem is that even in a robot having a plurality of joints whose joint driving torque fluctuates nonlinearly, it is necessary to detect the occurrence of a collision from the fluctuation of the joint driving torque generated by the collision with high accuracy and in a short time. is there. That is, although the collision is sensed by the fluctuation of the torque detected in each motor unit, the calculation is made as strictly as possible to determine whether or not the collision is caused.

A second object is to reduce the interference force from an obstacle due to a collision and to minimize damage to the robot main body and a hit object in a stop process after the detection of the collision of the robot. In other words, an attempt is made to propose a mechanism in which a joint that does not have a large inertial force or a joint that does not generate velocity escapes in the force direction.

[0028]

According to the present invention, when a link of a robot having a plurality of joints or a tool or a work gripped at the tip of a link collides with a nearby obstacle or the like, the deformation of the link or the like or the movement of the robot is prevented. It is applied to a method of detecting a collision and stopping the robot immediately to avoid damage to the robot. The feature is that, with reference to FIG. 1, the positions of the motors detected by the servo drivers 2 ₁ , 2 ₂ and 2 ₃ for controlling the joint driving motors 1 ₁ , 1 ₂ and 1 ₃ of the robot. from .theta.m · speed theta _d m · acceleration theta _dd m, the collision torque T ₁ each motor should occur if has not occurred, T _2, T _3, · position of each joint of the robot
Based on speed / acceleration and robot mechanism parameters

(Equation 3) Is predicted by the inverse dynamics calculation based on the equation shown by. A difference ΔT between the predicted torque value and the actual joint driving torque Tc is obtained. Then, when the absolute value of the difference ΔT is larger than the preset judgment value ε, it is determined that a collision has occurred.

[0029] As the position, velocity and acceleration of the motor used when the motor for driving each joint when a collision has not occurred to predict a torque to be generated, as shown in FIG. 9, the servo driver 2 _1, employed 2 _2, and 2 ₃ to the joint position calculated by the playback operation of the work data previously taught to sit command theta, its speed joint position calculated based on theta _d and acceleration theta _dd Is also good.

In the inverse dynamics calculation, assuming that the number of joints of the robot is n, the torque generated at each joint by the acceleration of each joint of the robot and the interference torque generated by the acceleration of the other joints are n × n. The matrix includes an inertia matrix, an n × 1 viscosity matrix indicating the influence of centrifugal force and Coriolis force, and an n × 3 gravity matrix indicating torque generated by the influence of gravitational acceleration.

As the diagonal element in the n × n inertia matrix, “the inertia moment indicating the relationship between the acceleration of the joint and the torque exerted on the joint itself” includes the inertia moment of the motor rotor including the reduction gear as the reduction ratio. The sum is multiplied by the square, and the torque generated by the moment of inertia of the motor rotor including the speed reducer in each joint can be considered.

In the equation used for the inverse dynamics calculation,

(Equation 4) Should be added in advance so that the frictional resistance of the motor bearing and the reduction gear in each joint can be considered.

The mechanism parameters of the robot need only include the position, mass, and length of the center of gravity of the link connected to each joint. It is even more preferable if an inertia tensor is added.

The torque calculated from the torque command value actually generated by the servo driver may be used as the actual joint driving torque when calculating the difference from the predicted torque value. If the torque calculated from the torque response value of each motor is adopted, the collision detection accuracy can be further improved.

Among the joints, those which are stopped or operating at low speed or those whose inertia force is small and the torque required for stopping the operation is sufficiently smaller than the maximum torque, the interference force due to the collision after collision. Be prepared to perform the escape operation to soften.

In stopping the robot, the minimum torque value required to stop the operation of the robot from the speed of each joint and the load inertia at the time of collision is calculated for each joint, and the minimum torque is calculated. The value is set in the torque limiter 3 of the servo driver, the position feedback proportional gains Kpp of all the servo drivers are set to 0, and the speed deceleration process is forcibly performed by setting the speed command value to 0.

Necessary for stopping the operation of the robot
When calculating the torque value of each joint, all joints
Deceleration stop time for each joint, assuming that it stops at the same time
tmd ₁, Tmd_Two, Tmd_ThreeAfter calculating, all deceleration
The maximum value of the stop time is the deceleration stop time of any joint
It is preferable that the above is also applied.

[0038]

DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS Hereinafter, a collision detection / stop control method for an articulated robot according to the present invention will be described in detail with reference to the drawings showing an embodiment thereof. First, an outline of the configuration of an arc welding robot as an example to which the present invention is applied, and control of a motor driving a joint connecting a link will be described. In addition, the present invention, when the robot collides with an external object or other obstacles such as a work during the work, detects that there is a collision quickly and accurately, and then immediately stops the movement of the robot The purpose is to release all or a part of the force acting on the link to reduce or avoid deformation, damage, and the like of the robot body, tool, or work.

The details will be described later with reference to the drawings and tables. Referring to FIG. 2, which schematically shows the escaping operation of one of the links in the robot having a plurality of joints, the basic structure of the present invention will be described. Will be described. This example shows three joints 7 ₁ , 7
_2, 7 is a robot 10 having _three, represents the case where the third link L ₃ collides with an obstacle 8 in the first joint 7 ₁ and the second high speed operation of the joint 7 _2. Without even mentioned, as indicated by the arrow 9 in the drawing, if the rotational direction is either less motion third joint 7 ₃ stops to relieve interference power due to a collision, greatly mitigate the impact due to the collision of the entire robot can do.

By the way, as mentioned in the section of the prior art, in the case of a robot having a plurality of joints whose joint driving torque fluctuates nonlinearly, even when a collision does not occur, each joint is considered in consideration of the following factors. It is important to accurately predict the torque value at which the torque occurs. If the fluctuation of the joint driving torque generated due to the collision is detected based on them, the occurrence of the collision can be detected with high accuracy and in a short time.

The factors are (1) fluctuation of load inertia caused by a change in the position of each joint of the robot (referred to as “inertia fluctuation”), and (2) change of the position and acceleration of each joint of the robot. Fluctuation of interference torque received from acceleration motion (referred to as "interference of acceleration"), (3) Fluctuation of disturbance torque consisting of centrifugal force and Coriolis force caused by changes in the position and speed of each robot joint (referred to as "interference of speed") And (4) fluctuation of disturbance torque due to the influence of gravitational acceleration caused by a change in the position of each joint of the robot (referred to as “gravity interference”).

A control block diagram illustrating a first example of the present invention is shown in FIG. The link L ₁ , L ₂ , L _{3 of} the robot 10 shown in FIG. 2 having a plurality of joints and a tool or a work (not shown) held at the link end are
In order to prevent deformation of a link or a tool when colliding with a surrounding obstacle 8 and to avoid damage to a robot movable part such as a joint, the collision detection unit 11 in FIG. The stop processing section 12 can stop the robot. The motor 1 ₁ For each of the three joints, 1 _2, 1 ₃ is equipped, the servo driver 2 ₁ operating in response to a command from the robot controller 13 for each its motor, 2 _2, 2 ₃
Is prepared.

In the collision detection process in the control according to the present invention,
The main steps consist of the following steps. First, the robot
Driver to control each joint drive motor
2₁, 2 _Two, 2_ThreeDetected motor position θm₁, Θm
_Two, Θm_Three, Speed θ_dm₁, Θ_dm_Two, Θ_dm_Three,acceleration
Degree θ_ddm₁, Θ_ddm_Two, Θ_ddm_ThreeFrom the collision
Torque that should be generated by each motor if not
T₁, T_Two, T_ThreeIs calculated by the inverse dynamics calculation described later.
Measure.

Next, the estimated torque value T₁, T_Two, T_Three
And the actual joint driving torque, that is, the servo driver 2
₁, 2_Two, 2_ThreeIs the torque command value Tm actually generated₁,
Tm _Two, Tm_ThreeΔT from the torque calculated from₁,
ΔT_Two, ΔT_ThreeAsk for. And any difference ΔT
_iIs a predetermined judgment value ε, the absolute value of_iGreater than
If possible, consider that a collision has occurred
You.

A collision can be detected only when there is a change in the servo state variable due to the collision.
As already mentioned. He also mentioned that what senses the collision is the fluctuation of the torque detected in each motor unit. It cannot be determined whether the fluctuation of the torque is caused by a collision or by a collision unless the existence of the four disturbance torques described above is determined when there are a plurality of joints. However, exact calculations are required to determine its existence.

Therefore, a torque to be generated at each joint necessary for the robot to perform a desired motion is obtained, and the torque is compared with the torque actually generated by the motor, and the presence or absence of a collision is detected from the difference. To Although the amount of calculation tends to be large, the detection becomes more accurate. In this example, any time after the motor is excited, the current position, speed, and acceleration of the motor are fetched from the servo driver, and the torque value that should be output to each motor is calculated based on them assuming that each joint is moving. Things.

Here, the configuration of the robot system will be described first with reference to FIG. Although the robot has an appropriate number of joints depending on the application, a robot having three joints is taken as an example, as described above, for the sake of simplicity. Motors 1 ₁ , 1 ₂ , which drive each joint of the robot
Each 1 _3, encoder 1e _1, 1e _2, 1e ₃ to provide location information as the forward-reverse amount of rotation is mounted.

A control device (robot controller) 13 for driving these motors is provided in a control box separate from the robot body, stores teaching data instructed by the teach pendant 14, and sets the stored data and the preset data. An operation for reproducing the operation is performed based on the stored mechanism parameters unique to the robot.

Then, the controller 13 and the motor 1
In between, servo drivers that control the position and speed of each motor
2₁, 2_Two, 2_ThreeOr determined by position / speed control
Servo that supplies power to the motor based on the command current value
Amplifier 2a₁, 2a_Two, 2a _ThreeIs also provided. By the way
The configuration of the servo driver 2 has been described in the section of the prior art.
As shown in FIG. 14, this is described in the lower half of FIG.
It is mounted in the form.

Each servo driver incorporates a position control loop and a speed control loop for controlling the motor to follow a position command given from the control device. The position control loop performs feedback control using a proportional element, and the speed control loop performs feedback using a proportional element and an integral element. The position feedback proportional gain Kpp can be changed to an arbitrary value according to a request from the robot controller.

[0051] The following of the speed control system is arranged torque limiter 3, which is a torque command value Tm _i generated by the speed control to be able to be limited by any value. The limit value set in the torque limiter 3 can be changed to an arbitrary value according to a request from the controller 13.
Note that Kvp in the figure is a speed feedback proportional gain,
vi is the velocity feedback integral gain, S is the differential element, 1
/ S represents an integral element.

The controller 13 issues control commands for the three joints. When roughly classified, as shown in FIG. 3, a central processing unit CPU, a storage operation unit for storing and operating arithmetic programs and various data, teaching data. It consists of an interface for taking in and outputting calculated values. Here, the operation of reproducing the taught data (hereinafter referred to as work data) will be briefly described below.

The control program is stored in the ROM, and the trajectory planning unit 22 reads out the work data stored in the hard disk 15 as shown in FIG.
A work trajectory of a tool or a work held by the robot tip is planned on orthogonal coordinates in a three-dimensional space.

Next, the interpolation point calculation unit 16 divides the work trajectory for each interpolation cycle of a predetermined time, and calculates the position / posture to be reached by the tool or the like for each interpolation cycle as interpolation point data. Then, the inverse transform unit 17 inversely transforms the position / posture represented by the orthogonal coordinates in the interpolation point data into the position at each joint level necessary for reproduction by the articulated robot, and the respective servo drivers 2 ₁ , 2 _2, and calculates the position command value to 2 _3.

The position command to the servo driver is stored in RAM
The robot joint position is temporarily stored in the servo position.
The synchronization signal is sent from the data conversion unit 18 that converts the data to the command value.
No. S _YNCIs transmitted to each servo driver in synchronization with What
Note that the above processing is for teaching the articulated robot
This is the most basic process for controlling with the
Needless to say, it is well known.

Incidentally, the ROM in FIG. 3 stores a program for executing the operation control of the robot and detecting and stopping the collision, and data such as execution conditions and control constants. The RAM is used as a working area of the CPU, and temporarily stores data being calculated. The hard disk 15 stores not only work data, but also data unique to the robot, such as the length and mass of the link, and data related to a work or tool that is unique in the taught operation.

A serial IO (input / output) 19 is connected to the teach pendant 14 so that work data by operating the teach pendant can be recognized by the CPU through serial communication. Servo I
F (interface) 20 is a servo driver 2 ₁ , 2
_2, is connected to the 2 _3, to allow monitoring of the control state of the transmission and the motor command data. The timer 21 generates a synchronizing signal S _YNC at regular time intervals, and this signal gives an update timing of a command value to the servo driver.

Next, the robot collision detection according to the present invention will be described.
The first form example for performing stop processing after a collision in addition to
A description will be given based on FIG. The stop processing will be described later.
Then, the collision detection will be described. Bases in collision detection
The essential part is that the comparator 29 ₁, 29_Two, 29_ThreeSmell
That the target of comparison is torque instead of position deviation
It is.

When it is intended to detect the presence or absence of a collision based on the torque, it must be noted that even if a large torque is applied, it cannot be said that a collision has occurred. When the joint moves at a large acceleration, a large torque is required. Therefore, it is not possible to regard a collision as a sudden increase in the torque. Therefore, how much torque is required to make the desired motion,
Unless it is strictly determined how far the current torque is, the collision detection cannot be performed with high accuracy.
The present invention has been studied and completed from such a viewpoint.

First, the servo driver controls the driving of the motor.
Not only control but also monitor the status of the servomotor
In this example, the servo driver
Extracts motor information such as motor position, speed, and acceleration
I will decide. And this is the collision of the controller 13
H (θ) · θ which will be described later in detail
_dd+ C (θ ・ θ_d) + G (θ) · g
Inverse dynamics calculation, and if there is no collision,
Torque T that should be generated by the moving motor₁, T _Two, T_Three
Predict.

The inverse dynamics calculation is for calculating the torque generated at each joint of the robot from the state of the position, speed and acceleration of each joint of the robot and the mechanism parameters of the robot. According to this inverse dynamics calculation, it is possible to accurately predict the influence of the torque fluctuation factors (1) to (4) described above.

On the other hand, the “actually generated torque” to be compared with the predicted torque is a torque value picked up just before the torque loop of the servo driver. If there is no collision, the torque value is almost the same as the torque value calculated by the inverse dynamics. If the difference between the two torques is large, it means that there is a collision. Therefore, if the difference has reached a “threshold value” ε _i , which will be described later and is predetermined for each joint, it is determined that a collision has occurred.

T _i = H (θ) · θ _dd + C (θ · θ _d ) +
In calculating G (θ) · g, data unique to the robot, that is, mechanism parameters are also required, and these are read from the hard disk. The torque value may be either a current value or Nm (Newton meter).
If the servo driver has a torque converter with a torque constant (Nm / A), it can output in Nm.
What is necessary is just to match it with the operation value. If the servo driver does not have a torque conversion unit, it goes without saying that it is sufficient to convert the current value into a current value in the inverse dynamics calculation and make a comparison.

A servo IF 20 is provided between each servo driver and the robot controller, and transmits the following data. As shown in FIG. 1, the servo driver 2 sends a motor position response θm to the controller 13.
[Rad], motor speed response θ _dm [rad / se
c], motor acceleration response θ _dd m [rad / sec ² ]
And a torque command value Tm [N · m] to the motor,
The position command value θ is sent from the controller to the servo driver.
[Rad], a position feedback proportional gain Kpp, and a torque limit value Tmdec [N · m] described later.

For the detection of collision, the five steps shown in FIG. 4 are sequentially repeated as long as the feedback control of the servo driver is performed. First, in step 1 (hereinafter referred to as S1 or the like in the drawing), the collision detecting unit 11
From the servo driver 2 via F20, the response values of the position, speed and acceleration of the motor driving each joint of the robot and the torque command value are read. This reading is performed for all motors that drive joints. In the case of a robot having three joints, the response value of the position, speed, and acceleration of the motor and the torque command value are expressed by the equations (1-1) to (1-4) using the symbols in Table 1. Matrix. The numbers appended to the symbols in the formula represent the joint numbers, and [] ^T
Indicates that the matrix described in the column of “number” in Table 1 is “transposed” and displayed.

[Table 1]

(Equation 5)

Incidentally, although the number of joints is set to 3 in the embodiment, n = 2 can be applied for the purpose of the present invention. Even if n = 1, it is not theoretically impossible, but a low-degree-of-freedom robot does not usually have a high speed and does not receive interference from other joints, so there is no need for a matrix operation. In addition, for example, even in the case of a system in which two robots in which a tool is mounted on one side and a workpiece is gripped on the other side are operated in conjunction with each other, the idea of the present invention is applied to each robot. is there.

Incidentally, the motor and the robot joint are generally connected via a speed reducer. for that reason,
The values of Equations (1-1) to (1-4) are calculated by
It is necessary to convert to speed / acceleration. The response values of the position, speed, and acceleration at the joint and the driving torque can be expressed by the equations (1-5) to (1-8) using the symbols in Table 2.

[Table 2]

(Equation 6)

Taking the i-th joint as an example, they are given by the formula (1-10)
Or represented by (1-13). Divided by the reduction ratio zeta _i for position, speed and acceleration become joint conversion value, a value obtained by multiplying the speed reduction ratio is rheumatoid converted value in the torque. Therefore, in step 2, the position, speed, and acceleration of the motor level are changed to values of the joint level of the robot.

(Equation 7)

If the joint level position / velocity / acceleration and torque response values are known, in step 3 these are applied to the inverse dynamics model (H, C, G) described below to calculate the inverse dynamics. The torque that is expected to occur is calculated. The inverse dynamics model includes an H matrix of an inertia matrix, a C matrix of a viscosity matrix, and a G matrix of a gravity matrix. The physical meaning of each matrix will be described later.

Robot mechanism parameters (position of center of gravity, mass, inertia tensor, length, etc. of link connected to joint)
It is known that the torque T generated at each joint can be calculated by the equation (2-1) from the position, velocity, and acceleration of each joint. This is called an inverse dynamics model.

(Equation 8)

The equation (2-1) is expressed in “Robot engineering-” by Shigeo Hirose.
Vector Analysis of Mechanical Systems-"Rokabo, 2nd Edition, 1989
Issued on February 10, 2009 (hereinafter referred to as technical document 1). Section [4] of section 11.3 describes in detail the procedure for calculating the inverse dynamics using the Newton-Euler method, which is one of the rigid body motion analysis techniques, and thus will not be described here. Eventually, the inverse dynamics calculation formula becomes the formula (11.28) in the technical document 1. The last term in the formula, J ^T K (represents the external force applied to the tip of the robot and the moment due to the external force, on the transposed Jacobian matrix) K multiplied by K) does not need to be considered in the case of the present invention because the robot is not colliding, that is, the torque is obtained when the robot is displaced without touching anything in the space. Therefore, by omitting the term, the above equation (2-1) can be obtained.

For a robot having n joints, the inertia matrix H is n
It is a regular symmetric matrix of × n, and means a torque generated at the joint itself by the acceleration of each joint of the robot and an interference torque generated by the acceleration of another joint. The viscosity matrix C is an n × 1 matrix, and indicates the torque generated by the influence of the centrifugal force and Coriolis force generated by the speed of each joint of the robot. The gravity matrix G is an n × 3 matrix, and the torque generated by the influence of the gravitational acceleration. It shows.

The inertia matrix H will be described in some detail.
H of diagonal element of inertia matrix H (see equation (2-9) described later)
jj represents the moment of inertia indicating the relationship between the acceleration of the j-th joint and the torque exerted on the j-th joint itself, that is, H
jj is load inertia that varies depending on the joint position. Hij, which is another component, is a moment of inertia representing the relationship of the interference torque exerted on the i-th joint by the acceleration of the j-th joint.

Inertia matrix H, viscosity matrix C and gravity matrix G
Can be calculated from the robot's mechanism parameters and the position and speed of each joint. Robot mechanism parameters are as follows:
It is composed of one. The numbers in Table 3 are per link connected to the joint. Incidentally, if the tools of the welding torch or the like to the third link L ₃ in the case of the robot illustrated in FIG. 2 are mounted, the parameters of the third link tool would be given in a state of integrally .

[Table 3]

The robot mechanism parameters such as the mass, the center of gravity position, the inertia tensor, and the length of each link are known values that can be obtained in advance from the mechanism, link shape and material of the robot. Since it is clear that it does not change during exercise, it is a fixed value and is stored in the hard disk 15 in advance. If these mechanism parameters are read and applied to equation (2-1), the torque generated at each joint can be calculated from the current three values of the position, velocity, and acceleration of each joint and the gravitational acceleration g. Note that the expression
Each element of (2-1) has the meaning described in Table 4.

[Table 4]

Here, the mechanism parameters of the robot in this example will be described. Mass m _i, the center of gravity position mx
_i, my _i, mz _i, to no inertia tensor Ix _i Iz _i
Indicates the mass of the link connected to the ith joint that moves by the movement of the ith joint and does not move by the movement of the (i + 1) th joint, the center of gravity of the link, and the inertia tensor of the link. The procedure for calculating the inertia tensor is described in detail in section 3.8 of Technical Document 1 (see equation (3.38)), and thus the description is omitted here.

The basis for calculating the position of the center of gravity and the inertia tensor is based on the link coordinate system. The procedure for defining a link coordinate system assigned to each joint is described in Tsuneo Yoshikawa's "Robot Manipulator" Corona, first edition, 1989
This is described in detail in section 2.9 of December 15, 2000 (hereinafter referred to as technical document 2). Here, the definition of the basic link coordinate system will be described with reference to FIG. For example, in a straddle link L ₂ to the second joint 7 ₂ and the third joint 7 _3, link coordinates hit split to the second joint is defined center of rotation of the second joint as the origin, link the x-direction and the length direction of the L _2, determine the z-direction rotational direction of the second joint, the y direction as the right-handed coordinate system.

Next, the inertia tensor I is generally used to indicate the difficulty of rotation of an object, and is represented by a well-known 3 × 3 symmetric matrix equation (2-2). The diagonal component represents the moment of inertia of each joint of the robot mechanism, and the other components represent negative values of the product of inertia.

(Equation 9)

The moment of inertia is obtained by the following equation.
The difficulty around the object (robot link) and the difficulty in stopping it.

(Equation 10)

Next, the product of inertia is obtained from the following equation, and represents a moment that disturbs the rotational motion of the object (robot link) when the object (robot link) performs the rotational motion.

[Equation 11] Here, dm indicates the mass of a minute volume. Also, x in the formula, y, z is represented with reference to a predetermined link coordinate system, the reference coordinates of the inertia tensor of link L ₂ shown in FIG. 5, Link coordinate system assigned to the second joint L
The coordinate system when moved to the position of the center of gravity of ₂ . Other joints are similarly defined.

The method of calculating the matrix values of inertia, viscosity and gravity is described in detail in Technical Document 1. Here, the description is omitted, but in the case of a robot having three joints, the matrix of inertia, viscosity, and gravity is represented by the following equation.

(Equation 12)

Each component of the equations (2-9) to (2-11) is composed of only the parameters shown in Table 3 and the joint position θ and the joint velocity θ _d , and all elements other than θ and θ _d are known. Is the value of Accordingly, the joint position response θr obtained in Step 2, as the joint velocity response theta _d r of the formula theta, I place theta _d, inertia matrix H in the formula (2-1), viscous matrix C and gravity matrix G Can be requested.

(Equation 13)

Since the H, C, and G matrices have been calculated in step 3, the unknown values in the equation (2-1) are obtained by calculating the joint acceleration θ _dd and the gravitational acceleration vector g indicating the direction in which the gravitational acceleration works. There are two. The latter changes depending on the installation posture of the robot, but generally, it can be considered that the installation posture of the robot does not change, and therefore, it can be specified in advance.

In step 4, the only unknown value among the values necessary for finally calculating the inverse dynamics model represented by the equation (2-1) is the joint acceleration _θdd . This is also obtained by _calculating the joint acceleration response θ _ddr obtained in step 2 by the equation (4-1).
Substituting in the form of, each joint θr of the robot in the absence of collision, theta _d r, the torque T generated at each joint necessary for moving at theta _dd r, in equation (4-2) Can be predicted.

[Equation 14]

[0085] Each joint θr of the robot when a collision calculated in step 4 is not occurring, θ _d r, the torque T of each joint necessary for movement theta _dd r, actually calculated in step 2 In step 5, a difference ΔT from the torque command value Tc at which the motor is about to drive the joint is calculated.

(Equation 15)

If there is no collision, the torque command value Tc and the predicted value T are almost equal and ΔT ≒ 0.
On the other hand, if the absolute value of ΔT is larger than the preset determination value ε _i, it is considered that a collision has occurred, and the control program shifts to the stop processing. In particular,
If any one of the conditional expressions (5-3) to (5-5) is satisfied, it is determined that a collision has occurred.

(Equation 16)

[Table 5]

Incidentally, it is often sufficient for ε _i to be, for example, 0.1 Tc _i . Actually, there is a case where it is necessary to be 0.3 Tc _i , but even at that time, if a torque of 30% or more is applied, it is determined that a collision occurs.
If by the position deviation described in the prior art section, as compared to extend to them if for example 0.8Tc _i or more terms of torque, it can be seen that if the collision detection accuracy of the present invention is remarkably improved. Such improvement can be expected to be further improved depending on the joint.

The judgment value ε _i may be simply treated as being fixed, but may be varied according to the magnitude of the acceleration or the speed. The sensitivity can be changed so that it can be raised. Increase the sensitivity during acceleration / deceleration.
This is because if it is set to 1 Tc, erroneous detection of a collision is likely to occur, and if the sensitivity is reduced to 0.3 Tc when acceleration / deceleration is small and the speed is low, detection of the collision will be delayed.

As can be understood from the above description, in order to detect the collision of the robot, information of each motor for driving all joints is collectively processed and integrated to perform inverse dynamics calculation. All interferences (collisions) can be found. It goes without saying that equation (2-1) is indispensable in the calculation. Among the mechanical parameters of the robot, the link mass, the center of gravity of the link, and the link length are also indispensable elements. However, the link inertia tensor is necessary for a robot adopting an elongated link, but may not be considered in the case of a lightweight robot or a short link robot, and may be appropriately selected each time.

The "position command" input to the servo drivers 2 ₁ , 2 ₂ , and 2 ₃ is a command issued as a result calculated by the trajectory planning unit 22 based on the data stored in the hard disk 15 (FIG. 1). ) During teaching, this is a “position command” based on data input by the teach pendant 14 (see FIG. 3). Therefore, the operation of the present invention can be performed even for an unexpected collision during teaching.

According to the collision detection method according to the present invention, the collision detection is performed with high accuracy and speed. Therefore, in combination with a stop processing operation to be described later, at least the mechanical structure of a jig or the like that is broken or is present in the vicinity. Damage to the elements and enormous time required for recovery are reduced as much as possible, and operation and teaching after the collision can be quickly resumed. The collision detection is based on the collision detection software installed in the controller, so there is no need to equip the robot link with a shock sensor or mechanical collision detection device, and the structure and mechanism of the robot link are not complicated and the weight is reduced. .

In FIG. 1, it has been described that the servo driver transmits the motor position response, the motor speed response, the motor acceleration response, and the torque command value to the motor to the controller. However, since the speed response and the acceleration response can be obtained by differentiating the former, the servo driver sends a signal to the controller as shown in FIG.
As described above, the acceleration response of the motor may be obtained by differentiating the speed response value in the collision detection unit 11 without limiting the position response and the speed response of the motor. Further, it is also possible to supply only the position response of the motor to the controller, and to obtain the velocity and acceleration by performing first- and second-order differentiation by the controller (not shown). In either case,
It can be expected that the congestion of information transmission via the servo IF 20 will be reduced.

In the above-described example, when calculating the difference ΔT, the torque prediction value based on the inverse dynamics calculation and the torque command value Tm generated by the speed control by the servo driver are treated as the actual joint driving torque. It was explained that it was adopted. However, a torque calculated from a torque response value of each motor may be employed instead of the torque command value.

As can be seen from FIG. 7, the servo driver 2
₁, 2_Two, 2_ThreeCommand value Tm obtained by_iAccording to
Power from the power line to each brushed DC motor 1₁,
1_Two, 1 _ThreeAMP2a to supply to₁, 2a_Two, 2
a_ThreeCircuit from the current detector 23 ₁, 23_Two, 23_ThreeTo
Provide. A / D conversion of the current picked up by the current detector
After that, the torque value Tn_iMultiplied by the torque constant
・ The torque response value obtained as m (the torque that is actually output)
Luc) Tc_i’. In this case, the above equation (1-1
The following equation will be used instead of 3).

[Equation 17]

When the motor is a brushless DC motor, not all currents actually flowing become effective torques. Particularly, when the motor rotates at high speed, the reactive current that is not converted to torque increases, and the error between the command and the output increases. The torque actually output by the motor detects the current flowing in two of the three phases of the motor (hereinafter referred to as phase current), and performs coordinate conversion by the coordinate conversion unit 24 shown in FIG. Thus, an effective current _iqa , which is a current converted into torque, can be obtained.

In the coordinate conversion process, the reactive current is discarded and only the effective component is taken out. This is described in Hidehiko Sugimoto et al., "Theory and Design of AC Servo Systems," Sogo Denshi Publishing Co., 2nd edition, 1990. This is described in detail in section 4.2 of May 8, 2003 (hereinafter referred to as technical reference 3). Finally, it is represented by the formula (4.11) in this document. In which section i _qa in the formula corresponds to active current, the phase current i _ua, i _va a motor electric angle theta _re than the effective current i _qa are calculated by the equation (6-1).

(Equation 18)

Note that there are many methods for detecting the electrical angle θ _re , but when an absolute value encoder is used, it can be obtained as follows. First, the encoder pulse displacement θe _rev corresponding to the electrical angle of 360 ° is calculated. θe
_rev is the pulse displacement θm _rev per encoder rotation.
Is divided by the number of pole pairs Pn of the motor, and is calculated by equation (6-2). The number of pole pairs means u, v,
The number of three pole pairs of w is arranged per one rotation of the motor, and the rotation amount of the motor corresponding to one pole pair is an electrical angle of 360 °. The pulse displacement θm _rev per rotation of the encoder and the number of pole pairs Pn of the motor are also values that can be specified in advance from the specifications of the encoder and the motor.

[Equation 19]

The electrical angle θ _re is obtained by dividing the output pulse θe from the encoder by the pulse displacement θe _rev of the encoder corresponding to the electrical angle of 360 ° and dividing the remainder by θe _rev by 2π. Desired. Specifically, Equation (6-3) is obtained. The operator MOD in the expression indicates that the remainder of the division is calculated.

(Equation 20)

By multiplying the effective current _iqa calculated from the above equations (6-1) to (6-3) by the torque constant Kt, the torque Tn actually output by the motor is calculated. The torque constant Kt is the generated torque T with respect to the effective torque.
It is a value that can be specified in advance by the ratio of n from the specifications of the motor.

(Equation 21)

The symbols appearing above are listed in Table 6 for reference. FIG. 8 is a block diagram of a servo driver having an output torque detecting function. The phase currents i _ua and i _va detected by the current detectors 23 _u and 23 _v are A
It is read via the D converter. The output pulse of the encoder required for calculating the electrical angle can also be read.

[Table 6]

When the motor is a brushed DC motor, the coordinate conversion shown in FIG. 8 is not necessary, and the motor need only be immediately multiplied by the torque constant. If the output torque of the motor is detected in this way and used for collision detection, the detection accuracy is further improved. In particular, this is an extremely effective means in a brushless DC motor or the like in which a reactive current which is not converted to a torque at the time of high-speed rotation increases and a deviation between a torque command value and a motor output torque is likely to occur.

In FIG. 1, the inverse dynamics calculation is performed using the position / velocity / acceleration response values picked up from the servo driver. There is a slight delay. FIG. 9 shows an attempt to improve the situation. The work data is used as the position, speed, and acceleration of the motor used to predict the torque to be generated by the motor driving each joint when no collision occurs. And a velocity and an acceleration calculated based on the joint position calculated for the reproducing operation.

It should be known at the stage of trajectory planning at what position, speed and acceleration the motor is moving. If this is used, it is possible to calculate the predicted torque without looking at the servo state. It is based on the findings of
Therefore, the position, velocity,
The torque which is expected to be generated in each motor from the acceleration is obtained by inverse dynamics calculation, and the torque is compared with the torque actually flowing to the motor to detect the presence or absence of a collision.

For this purpose, the control program in the controller 13 shown in FIG. 9 includes, in addition to the trajectory planning unit 22, the interpolation point calculation unit 16 and the inverse conversion unit 17 shown in FIG. A speed / acceleration calculation unit 25 for calculating the speed / acceleration of the robot joint from the robot is added.

It is necessary to adjust the time between the output of the position command signal from the data converter 18 to the servo driver 2 and the output of the data signal 26 for the inverse dynamics calculation. In this case, consideration should be given to delaying the latter from the former by, for example, the calculation time of the servo driver. With the configuration shown in FIG. 9, the amount of data exchange between the servo driver 2 and the controller 13 is reduced, and the communication load is reduced. Incidentally, even during teaching by the teach pendant or during manual operation, the work trajectory is generated by the trajectory planning unit 22 and the position of the joint and
Since the speed / acceleration can be predicted, this method can be applied even at the time other than the operation reproduction, as in the previous example (see FIG. 1).

Detection of a collision is realized by sequentially repeating the six steps shown in FIG. 10 as long as the feedback control of the servo driver 2 is performed on the interpolation point data of the work trajectory of the robot. The collision detection performed by the collision detection unit 11 which is a program in the controller 13 is not different from the example of FIG.

First, in step 11, the position, speed and acceleration of each joint of the robot are calculated in the course of performing the operation of reproducing the work data stored in the hard disk 15. Specifically, the speed / acceleration calculator 25 calculates the speed θ _d and the acceleration θ _{dd of} each joint of the robot from each joint position θ of the robot for each interpolation cycle.

Here, a method for calculating the speed and acceleration of each joint of the robot will be described. When the position θ of each joint of the robot is calculated for each interpolation cycle Smpt, the joint position calculated in the current cycle is set as θcurr, the joint position in the immediately preceding interpolation is set as θprev, and the velocity θ _d of each joint at the current interpolation point is set.
Is calculated as follows.

(Equation 22)

Assuming that the joint speed calculated in the current cycle is θ _d curr and the joint speed at the time of the immediately preceding interpolation is θ _d prev, the acceleration θ _dd of each joint at the current interpolation point can be obtained in a similar manner. Note that the speed and acceleration of each joint at time 0 are both set to 0.

(Equation 23)

From the position θ of each joint of the robot calculated in step 11, the position command θc to the servo driver is calculated by the equation (8-
1) and output to the servo driver 2 via the servo IF 20. Since the motor drives the joints via the speed reducer, a value obtained by multiplying the position θ of each joint of the robot by the reduction ratio is a command value to the servo driver. In this way,
The transmission of the position command value to the servo driver is performed in step 12. Note that θc is the position response of the robot joint [ra
d], and the number is an n (= 3) × 1 matrix.

(Equation 24)

At step 13, the state of the torque is read. That is, the collision detection unit 11 reads the torque command values Tm ₁ , Tm ₂ , and Tm ₃ of the motors that drive the joints of the robot from the servo driver 2 via the servo IF. Also, the coupling from the motor to the robot joint so takes place via a reduction gear, the value obtained by multiplying the speed reduction ratio of the torque command value Tm _i of the servo driver is provided as the torque Tc _i for driving the joint.

(Equation 25)

Thereafter, in step 14, the inverse dynamics model (H, C, G) is calculated, and in step 15, the predicted torque value by the inverse dynamics calculation is calculated. In step 16, the collision determination processing is performed according to the procedure described above.
Of course, thereafter, a stop process described later is also performed.

In the example of FIG. 9, the torque is predicted on the basis of the position, speed and acceleration commands. It is clear that the accuracy of the torque prediction increases if the error between the actual response value and the command value is small. . Therefore, the collision detection accuracy is also improved. Therefore, if the speed command is feed-forward compensated for the servo control system in the servo driver 2 in the configuration of FIG. 9, the command following performance of the servo driver can be improved and the collision detection accuracy of the robot can always be kept high. Can be kept. FIG. 11 shows an embodiment in which feedforward compensation of a speed command is added to the configuration of FIG.

More specifically, in the case of FIG. 9, since the joint speed is also calculated after the trajectory planning, the speed can be fed forward from the data converter 27 as shown in FIG. If you feed forward,
The extent to which the relationship between speed and acceleration differs from the actual response can be made smaller than in the case of FIG. That is, in the case of FIG. 9, the position / speed / acceleration and the actually used position / speed / acceleration tend to be different from each other. Cause a problem in terms of calculation accuracy such that the two do not match.
However, according to the feedforward compensation, this can be effectively solved.

FIG. 12 shows how the speed response differs depending on the presence / absence of feedforward compensation of the speed command. When the feedforward compensation of the speed command is performed, as can be seen by comparing (a) and (b), the tracking performance (speed response) is improved as indicated by the solid line with respect to the speed command indicated by the broken line. As can be seen, the feed lag reduces the time lag, and as a result, the relationship among position, velocity, and acceleration is actually very close.

When the feedforward compensation is performed, the effectiveness of the position feedback proportional gain Kpp is reduced, but the speed is effective in a feedforward manner, so that the speed command is promptly issued. Since speed alone does not provide sufficient positional accuracy, speed control based on position is also left, and this should be made to function interpolatively with respect to speed feedforward.

By the way, it has been explained that the equation (2-1) omits J ^T K because it is applied to, for example, an arc welding robot which always works without touching anything outside and displaces the space. This is because the case was considered. Therefore, when the present invention is applied to a kind of robot that works while touching a workpiece, it is effective only when the tool is simply moved spatially from the current position to the next target position. However, if previously added to J ^T K in the equation (2-1), but will be accompanied by an increase some processing time inverse dynamics operation to contact with the workpiece while exercising force sensor or the like While working, the idea of the present invention can be reflected.

The method of detecting a collision has been described above, but a stop process after a collision is detected will be described below. The purpose of the process is as follows. It goes without saying that when a collision occurs, it should be detected as soon as possible. However, when a collision can be detected, a little time has already passed since the collision, and it is very difficult to stop the robot immediately before the collision.

Therefore, in the stopping operation of the robot,
The ultimate goal should be to minimize damage to the robot body, bumped objects and obstacles in the event of a collision. Simply put, each motor must be able to generate enough torque to stop the link being moved through its joints, absorbing the moment of inertia that it has now, so that the Stop processing is performed from the viewpoint of enabling the escape operation and stopping damage to a minimum.

As described above with reference to FIG. 2, this stop processing is performed for a joint that is stopped or moving at a low speed.
This is to perform a control that allows a rotation in a direction to reduce the interference force due to a collision as indicated by an arrow 9. As described in the section of the related art, a torque as large as possible is generated at any joint and quickly. When attempting to brake, it is intended to avoid that a large force acts between the robot and the obstacle.

Specifically, after a collision is detected, the generated torque of the motor is limited so as not to generate a torque exceeding the minimum torque required for setting each motor speed to zero. For example, if the third joint is stopped and no gravity is acting, the torque is almost zero.
If so, the joint will rotate easily and the link will escape from the obstacle. If it is possible to escape without a load, there is still room for coasting to stop at the second joint and the third joint, so that the interference force and impact force of the entire robot are softened and the occurrence of damage is minimized. . As described above, in the present invention, it is intended to propose a mechanism in which a joint that does not have a large inertial force or does not generate a high speed escapes without opposing the force.

The gist of the stop processing is that a joint that is stopped or operating at a low speed among joints or a joint whose inertia force is small and the torque required to stop the operation is sufficiently smaller than the maximum torque has no collision. After that, an escape operation is performed on the obstacle so that the interference force due to the collision is reduced.

Further, the minimum torque value necessary for stopping the operation of the robot is calculated for each joint from the speed of each joint and the load inertia at the time of collision, and the minimum torque value is calculated in the servo driver. The torque limiter 3 is set, and the position feedback proportional gains Kpp in all servo drivers are replaced with zero. Thus, the deceleration process by the speed feedback control with the speed command value set to 0 is forcibly performed.

Further, in calculating the torque value of each joint necessary to stop the operation of the robot, it is assumed that all joints stop at the same time, and after calculating the deceleration stop time for each joint, By assigning the maximum value of the deceleration stop time to the deceleration stop time of any joint,
The above-described relief operation can be realized in some joints.

Even if a collision is detected, the speed becomes 0
Therefore, the joint speed θ _dr at the time of occurrence of a collision can be known from the motor speed θ _dm which is a state variable of the servo driver. The joint position can also be known from the motor position θm which is a state variable of the servo driver. If the joint position at the time of the collision is known, the calculation of the H matrix, which is the load inertia, becomes possible. This is, of course, inertia that changes depending on the posture of the robot. From the inertia and the speed at the time of the collision, the torque required to reduce the speed to 0 is calculated, and the motor output torque is limited so as not to generate a torque exceeding this value. This torque output limiting process is performed via the torque limiting value transmission path 28 shown in FIGS. 1, 9, and 11.

[0126] the third joint 7 ₃ as in Figure 2 when it is not moving and the first joint 7 ₁ and the second joint 7 ₂ is moving,
The first joint and the second joint require a relatively large torque for controlling the inertial force, but the third joint has a small inertia force, so that the torque required for deceleration is also applied to the first joint and the second joint. It may be small in comparison. Therefore, the torque limiter of the motor for driving the third joint 7 ₃ is set a small value, the third joint connecting link first joint, be impacted by the movement of the second joint, third joint Can easily escape in a direction to reduce the interference force due to the collision, and can absorb the change due to the collision.

A specific example of the stop processing will be described below. After detecting a collision by one of the collision detection processes described above,
A process including the four steps shown in FIG. 13 is performed. First, in step 21, a torque value that can be used to stop the operation of the robot is calculated.

The torque value Tusab of each joint which can be used for the stop processing at the time of collision occurrence can be calculated from the equation (10-1) obtained by modifying the equation (4-2). This torque value Tusab can be used for a deceleration motion for stopping the operation, and is regarded as a torque generated by acceleration. The torque due to gravity and the torque due to viscosity depend on speed, and must be secured separately from the torque due to acceleration.

If the torque (viscous torque) that disappears by speed and the torque that disappears by gravity are subtracted from the equation (4-2), it becomes a torque that can be used for deceleration. That is, the torque that can be actually used for the deceleration motion for stopping is limited by the disturbance torque that affects the centrifugal force and the gravitational acceleration.

(Equation 26)

[0130] Incidentally, sign in the formula (10-1) (θ _d r) is decided symbols. Tmax is the maximum torque that can be generated by each motor, and is set to a positive value. However, in some cases, you may want to use a negative value, so sign
(Θ _d r) is to determine it. If the current speed is positive, a negative acceleration is required, and a negative torque is given. When the speed is positive, Tmax is positive, but the negative sign indicates negative torque. Since a positive acceleration is required if a negative speed is present, the negative x negative = positive is set. That means that to invert the sign relative velocity (θ _d r).

[0131] -sign (θ _d r) · Tmax is a maximum of deceleration torque of the torque to each joint can be output. The deceleration torque required to bring the current speed to zero is the torque minus the torque lost by speed and the torque lost by gravity, which can be used for deceleration ( This is the torque Tusab given in 10-1).

Equation (10-1) is obtained by calculating H (θr) · θ _{dd of} equation (4-2).
and Tusab the r, -sign the T of formula _{(4-2) (θ d r)} · Tma
Replaced with x. The maximum output torque value Tmax _i of the _i-th joint is calculated by taking the reduction ratio into consideration, where Tmmax is the maximum torque that can be output by the motor driving the joint.
It can be calculated by (10-4). Needless to say, Tm
max is a value that can be specified in advance from the specifications of the motor, and the viscosity matrix C and the gravity matrix G can be calculated from the position and velocity of each joint at the time of collision.

[Equation 27]

[0133] Tm max _i also because the reduction ratio zeta _i also a known value Tmax _i can also keep give advance fixedly. The C matrix and the G matrix are also calculated in the process of detecting a collision, and need not be newly calculated. The current one is acceptable, so just bring the one used in equation (4-2). Actually, there is very little time lag, but it is not enough to calculate again from the time of collision. Of course, it may be calculated, but it takes time and there is no significant difference in the calculated value. The symbols appearing above are described in Table 7 for reference.

[Table 7]

In step 22, the torque value of each joint required to stop the operation is calculated. The torque generated by the deceleration is the product of the inertia matrix H and the deceleration of each joint, and is represented by the following equation (11-1). Although θ _dd is unknown,
If it is known, Tdec can be obtained by this equation.

[Equation 28]

In the control of the stop processing, it is assumed that all the joints decelerate to a speed of 0 at time tmd and stop. Then, the deceleration θ _dd of each joint is calculated by the equation (11-2)
Can be expressed as It should be noted that, in the formula (11-2) θ _d r
Is the velocity of each joint at the time of collision. However, at this stage, tmd is unknown.

(Equation 29)

The reason why the control condition is that all the joints decelerate and stop at the time tmd in equation (11-2) is that control cannot be executed unless some condition is imposed. In addition, the above-described escape operation can be performed in some joints.

Therefore, as the time tmd, an expression described later
The longest one of a plurality of tmds obtained by the number of robot joints in (11-4) or (11-6) is used (see formula (11-7)). This is to prevent the torque generated by the deceleration movement of the robot after the collision at all the joints of the robot from exceeding the torque Tusab usable for deceleration. Can be stopped at the same time.

Incidentally, for reference, the selected tmd
Are often the joints that are moving fastest or that receive the greatest moment. By the way, according to the time selected in this way, the slowest moving joint has enough time until the speed becomes zero and the unnecessary coasting is strengthened, but the speed is originally low. So even if you coast a little, the distance does not increase much.

Here, since the torque that can be used for deceleration in the stop processing is calculated as Tusab from the equation (10-1), the equation (1)
Replace the right side of 1-1) with Tusab, and replace all joints with Equation (11-2)
At the time tmd. Θ in equation (11-1)
Substituting the right side of equation (11-2) into _dd , equation (11-1) becomes equation (11-3)
It is transformed as follows. The method of calculating the inertia matrix H in the equation (11-3) is as described above.

[Equation 30]

From this equation (11-3), a deceleration stop time tmd at which the speed of each joint of the robot can be set to 0 can be calculated within the range of the torque Tusab which can be used for deceleration of the stop processing. This is because, in equation (11-3), H (θr) can be calculated from each joint position θr at the time of collision occurrence, and θr can be monitored via the servo IF 20. Also, the theta _d r a can be monitored via is servo IF a respective joint velocity during collision, Tusab is also a pre-calculated by Equation (10-1).

Finally, the only unknown value is tmd. t
md is a determinant, and in the case of a robot having three joints, three relational expressions are obtained. In practice, equation (11-3) is
Decomposed into the following three formulas for each joint, and deceleration stop time tmd
Is calculated. Specifically, in this embodiment, the number of joints n = 3
Therefore, the deceleration stop time tmd ₁ to tmd for each joint
₃ After calculating the, tmd _1, tmd _2, devote maximum value among the tmd ₃ to deceleration stop time of all joints. MAX in the equation (11-7) means that the maximum value in the element is extracted.

(Equation 31)

Using the deceleration stop time tmd calculated from the equations (11-4) to (11-7), the torque value Tdec _i of each joint required to stop the operation is calculated by the equation (11-8). ). Note that Tdec _i is the joint torque value [N
[M], and the number is an n (= 3) × 1 matrix. tmd
Is the deceleration stop time [sec] at all joints, and the number is 1
It is.

(Equation 32)

The Tdec calculated in this manner takes into account the load inertia and the fluctuation of the disturbance torque which change depending on the position and speed of each joint of the robot at the time of collision.
The required minimum torque value does not cause a delay in the stop time.

[0144] H matrix of Equation (11-1), from the current joint velocity theta _d r and deceleration stop time tmd of formula (11-2), deceleration torque Tdec required for each joint by formula (11-8) I understand. It is equal to Tusab for any one of the joints, that is, a joint which is considered to require the maximum time for the deceleration operation, but Tdec <Tusab for the other joints. This is because Tdec becomes smaller for a motor having a small speed or a small inertial force (= H). When this is set as a torque limit value, a relief operation can be performed for the interference force.

In step 23, the set value of the torque limiter 3 in the servo driver 2 is changed. That is, Tdec calculated in step 2 is the torque generated at the joint, and the servo driver performs the process of limiting the torque actually generated by the motor. Normally, since the motor drives the joint via a speed reducer, Tdec is changed to a motor shaft converted value Tmdec, and Tmdec is transmitted to the torque limiter 3 of each servo driver through the transmission path 28 via the servo IF 20. Is set to

The minimum necessary torque value Tm dec which does not cause a delay in the stop time of the motor driving the ith joint.
i can be calculated as in equation (12-1). Note that Tm dec is the motor torque value [N ·
m], and the number becomes an n (= 3) × 1 matrix.

[Equation 33]

Incidentally, although Tdec of the equation (11-8) enters the servo driver as the torque limit value, Tdec cannot be entered as it is. Since the dynamics calculation is generally obtained at the joint level, the obtained Tdec is also the torque at the joint level, and it is the motor that actually limits the torque. Since a reduction gear is interposed between the motor and the joint, a value divided by the reduction ratio must be set. Thus practice would Tm dec _i is placed.

[0148] As described just in case, H matrix and θ_dr is
Because it is different for each joint, tmd₁= Tmd _Two= Tmd_ThreeToshii
Nevertheless, Tm dec_iIs the servo driver 2₁, 2_Two,
2_ThreeDifferent for each. The obtained Tmdec_iAs it is
Tmdec_iFor each joint
It may be convenient if you add eight. Such a place
In each case, a different coefficient is assigned to each Tmdec_iMultiply and adopt
Just fine.

In step 24, the position feedback proportional gain Kpp in each servo driver 2 is set to the servo IF2
It is changed to 0 through 0. Through the processing in steps 21 to 24 described above, the deceleration processing by the speed feedback control in which the speed command value is forcibly set to 0 is performed.

Since the minimum torque value required for stopping is set in the torque limiter 3 in each servo driver,
If the interference force due to the collision exceeds this, the joint operates in a direction to release the interference force (collision force), and the impact is reduced.
As compared with the conventional case where the maximum torque value is set in the torque limiter, the effect of reducing the interference force is remarkably improved.

As described above, Kpp is set to 0 in addition to replacing the torque limit value, because the replacement of the torque limit value alone does not stop the joint. No matter what value comes in the position command or position feedback,
If Kpp is set to 0, the speed command immediately becomes 0. This is because it is easy to set Kpp to 0 in order to simply set the speed command to 0.

As can be seen from the above description, the motion of the joint is attempted to be stopped only by the speed loop. However, since the torque is appropriately changed and limited, the joint that is moving fast is stopped at the maximum torque. Is made. On the other hand, in other joints, it is possible to keep the torque generated only to maintain its own speed at 0, that is, to maintain the current posture.

As can be seen from the above, according to the above-described stop processing, when an interference force or an impact exceeding the torque limit value is applied, it is possible to cause the joint to perform a force-releasing behavior by using the strength of the force. The occurrence of damage due to the collision is suppressed as much as possible. Even in the stopped state, the integral term in the velocity loop will work forever, regardless of whether it is a joint that has hit an obstacle or a joint that has not hit, so each link maintains its posture by the torque within the limit value exhibited by the joint. Keep doing.

By the way, for example, in the case of a link that is displaced only in the horizontal plane and does not require a force against gravity to maintain the posture, or a link that is unfavorable when the torque is not applied, the speed becomes zero. The torque should be released when it becomes. In that case, the function of the integral term containing Kvi may be eliminated, and an operation of clearing the integral term may be added when Kpp is set to 0 or thereafter.

Having described the stop processing after the collision detection, a measure for improving the collision detection accuracy will be described below.
As described above, one of the methods is to adopt a torque response value instead of the torque command value.However, here, the moment of inertia of the motor rotor or the frictional resistance of the motor bearing or reduction gear must be considered. Improvement of collision detection accuracy due to Any of them can function alone or in an overlapping manner.

First, the improvement of the collision detection accuracy by considering the moment of inertia of the motor rotor will be described. This is, as a diagonal element in the inertial matrix of n × n described above, "inertia moment indicating the relationship of the torque exerted on the joint itself by the acceleration of the joint", that is, "load inertia that varies depending on the joint position", This is to add the moment of inertia of the motor rotor including the speed reducer to the square of the speed reduction ratio. Thereby, the collision detection accuracy can be improved in consideration of the torque generated by the inertia moment of the motor rotor including the reduction gear in each joint.

First, the torque generated by the inertia moment of the motor rotor including the speed reducer in each joint is taken into consideration by modifying the inertia matrix H of the equation (2-9) as follows. Here, Im is the moment of inertia [kg · m ² ] of the motor rotor, and the number thereof is n (= 3) × 1 matrix.

(Equation 34)

As can be said in common with the examples shown in FIGS. 1, 9 and 11, the inertia matrix H only calculates the inertia of the link connected to each joint. Although the joint moves because it is driven by the motor, the motor is also driven before the joint is driven, and attention should also be paid to the inertia of the rotation of the rotor itself at that time.

Here, Im _i is the moment of inertia of the motor rotor including the speed reducer. The moment of inertia of the rotating body that moves in tandem is multiplied by one square of its reduction ratio to affect the moment of inertia of the input shaft. Therefore, the effect of the moment of inertia of the input shaft on the output shaft is the opposite, and must be multiplied by the square of the reduction ratio. This is why the moment of inertia of the motor rotor including the speed reducer is multiplied by the square of the speed reduction ratio and added.

The diagonal elements of the inertia matrix H indicate the influence of torque on itself, and the other elements indicate the influence of interference torque on other joints. Since the moment of inertia of the motor rotor including the reduction gear does not interfere with other joints, only the diagonal element needs to be corrected. Since both Im _i and ξ _i are known, which can be set in advance, collision detection and stop processing can be performed in the case of Expression (13-1) in the same manner as in the example described above.

Next, a description will be given of how to improve the accuracy of collision detection in consideration of frictional resistance. By changing the robot dynamics model represented by equation (2-1) as shown in equation (14-1), we will try to take into account the frictional resistance of motor bearings and reduction gears in each joint. Things. The symbols that appear here are listed in Table 8 for reference.

(Equation 35)

[Table 8]

The reduction gear itself has considerable friction, which cannot be ignored. That the speed terms rather viscous matrix, but it is Cm · θ _d. However, this is not intended for friction when objects are rubbed. When a moving object at a certain speed overlaps with a moving object at a certain speed, the speeds interfere with each other to generate a torque in another direction, which means the torque. This is represented by four items in equation (14-1).

Cm is a kinematic viscosity coefficient, which is multiplied by a speed to obtain a disturbance torque. │Fcou │ is Coulomb friction (static friction: resistance before starting to move)
It is a torque component that has a constant magnitude independent of speed but always acts in the direction opposite to the motion. Cm · theta and _d m | Fcou | sum of is the frictional torque to be considered here. The equation (14-1) is obtained by adding four items to the equation (2-1), and is intended to improve the accuracy by calculation in consideration of friction.

Here, Cm is a kinematic viscosity coefficient n × 1 matrix of a motor bearing or a speed reducer, and the kinematic viscosity coefficient is a constant representing a proportional relationship between speed and kinetic friction torque. The dynamic friction torque is a torque component having a magnitude proportional to the speed. Furthermore, the theta _d m is the speed n × 1 matrix of the motor. Incidentally, sgn in the formula (14-1) (θ _d m) on the basis of the speed of the motor, which is a sign function which is determined by the following equation.

[Equation 36]

The relationship between the joint speed and the motor speed is represented by the following equation (14-5) from the reduction ratio ξ.

(37) Here, ξ is the reduction ratio of the reducer in the joint. Since the parameters Cm, Fcou, and ξ are also known values that can be set in advance, it goes without saying that the collision detection and the stop processing can also be performed by the above (14-1).

[0166]

According to the present invention, the torque to be generated by each motor when the collision does not occur is predicted from the position, speed and acceleration of the motor detected by the servo driver. Find the difference from the joint drive torque,
If the absolute value is larger than the preset judgment value, it is considered that a collision has occurred, so any link or tool of the robot collides with an object such as a work or other obstacles Can be quickly and accurately detected through torque.

The torque to be generated by the motor driving each joint when a collision does not occur is calculated based on the position, speed, acceleration of each joint of the robot and the mechanical parameters of the robot as T = H (θ ) · Θ _dd + C (θ, θ _d ) + G
(Θ) · g is predicted by the inverse dynamics calculation, so that the torque prediction value can be obtained with high accuracy, and the torque can be quickly calculated without using a mechanical component such as a collision sensor. And a collision can be detected accurately.

The position, speed, and acceleration of the motor used in predicting the torque are calculated based on the joint position calculated in the operation of reproducing the work data to instruct the servo driver to perform the position command, and the joint position. If the speed and the acceleration are applied, it is possible to promptly perform the collision detection calculation only for the time for acquiring the information on the position, speed, and acceleration of the motor from the servo driver.

In the inverse dynamics calculation, an nxn inertia matrix indicating the torque generated at each joint by the acceleration of each joint of the robot and the interference torque generated by the acceleration of the other joints, and the influence of centrifugal force and Coriolis force And an n × 3 gravitational matrix indicating the torque generated by the influence of the gravitational acceleration, the calculation accuracy is extremely high.

The inertia moment of the motor rotor including the reduction gear is calculated by adding the inertia moment of the motor rotor including the reduction gear to the “inertia moment indicating the relationship of the torque exerted on the joint itself by the acceleration of the joint” as the diagonal element in the n × n inertia matrix. By multiplying and adding, it is possible to perform collision detection in consideration of the torque generated by the inertia moment of the motor rotor including the speed reducer in each joint.

If factors for the kinematic viscosity coefficient and Coulomb friction torque of the speed reducer are reflected in the expression used for the inverse dynamics calculation, the frictional resistance of the motor bearing in each joint and the speed reducer is considered. Accurate collision detection is realized.

As the robot mechanical parameters used for the inverse dynamics calculation, the position of the center of gravity of the link connected to the joint,
If the mass and length are given, the calculation will be in accordance with the individual robot. Even more, if the inertia tensor is added, more accurate torque prediction will be possible.

If the torque calculated from the torque command value actually generated by the servo driver is used as the actual joint driving torque for which the difference from the predicted torque value is obtained, the calculation speed can be kept high. When the torque is calculated from the torque response value of each motor, the accuracy for collision detection is further improved by comparison with the actual torque.

In the joints that are stopped or operating at low speed, or the joints whose inertia force is small and the torque required for stopping the operation is sufficiently smaller than the maximum torque, the joints are made to perform the escape operation after a collision. By doing so, it is possible to reduce the interference force due to the collision and suppress the occurrence of property damage.

The minimum torque value required to stop the operation of the robot is calculated for each joint from the speed of each joint and the load inertia at the time of collision, and the minimum torque value is set to the torque limit in the servo driver. If the position feedback proportional gain Kpp in all servo drivers is set to 0 and the speed command value is forcibly set to 0 to perform deceleration processing by speed feedback control, position commands and position feedback No matter what value comes, the speed command can be simply set to 0,
In addition, when the interference force due to the collision exceeds the torque limit value, the joint operates in a direction to release the interference force, and the collision force is reduced.

In calculating the torque value of each joint required to stop the operation of the robot, it is assumed that all joints stop at the same time, and after calculating the deceleration stop time for each joint, all deceleration stops are calculated. If the maximum value of the stop time is used for the deceleration stop time of any joint, there is room for escape operation even if a collision occurs, absorbing the force due to the collision and minimizing damage. It can be kept low.

[Brief description of the drawings]

FIG. 1 is a block diagram showing an entire robot control system to which a collision detection / stop control method in an articulated robot according to the present invention is applied.

FIG. 2 is a schematic diagram of a robot having a simplified structure in which the number of joints is set to 3 and showing an escape operation when a link hits an obstacle.

FIG. 3 is a control block diagram showing an outline of signal transmission between a robot controller (control device), a teach pendant, and a joint driving motor.

FIG. 4 is a flowchart showing a processing procedure for determining the presence or absence of a collision.

FIG. 5 is a second diagram showing reference coordinates of an inertia tensor.
FIG.

FIG. 6 is a block diagram showing an entire control system including an example of changing a response value transmitted from a servo driver to a controller.

FIG. 7 is a block diagram of a servo driver when a torque response value is used instead of a torque command value.

FIG. 8 is a block diagram of a servo driver when a brushless DC motor is used.

FIG. 9 is a block diagram showing an entire robot control system for performing inverse dynamics calculation based on speeds and accelerations obtained from joint positions generated by trajectory planning.

FIG. 10 is a flowchart in the case where the processing procedure based on FIG. 9 is employed in determining the presence / absence of a collision.

FIG. 11 is a block diagram of the entire robot control system when the speed is fed forward to the servo driver.

FIGS. 12A and 12B show the degree of delay of a speed response to a speed command. FIG. 12A is a response diagram when the speed is not feedforward, and FIG. 12B is a response diagram when the feedforward is performed.

FIG. 13 is a flowchart illustrating a procedure of a stop process.

FIG. 14 is a schematic configuration diagram of a servo driver in which various control loops for driving a motor are incorporated.

FIG. 15 is a block diagram of a prior art showing that a position deviation is taken out from a servo driver to determine the presence or absence of a collision, and that a stop process can be subsequently performed.

[Explanation of symbols]

1, 1 _1, 1 _2, 1 ₃ ... motor, 2, 2 _{1, 2} 2, 2 ₃
... servo driver, 3 ... torque limiter, 7 ₁ ... first joint, 7 ₂ ... second joint, 7 ₃ ... third joint, 8 ... obstacle, 1
0: robot, 11: collision detection unit, 12: stop processing unit,
13 ... controller, 15 ... hard disk, 22 ... trajectory planning unit, 23 ... current detector, 25 ... speed and acceleration computing unit 29 _1, 29 _2, 29 ₃ ... comparator, L ₁ ... first link, L ₂ ... Second link, L ₃ ... Third link, θm ₁ , θ
m _2, θm ₃ ... motor position servo driver _{detects, θ d m 1, θ d} m 2, θ d m 3 ... motor speed servo driver _{detects, θ dd m 1, θ dd} m 2, θ _dd m ₃
... of the motor servo driver has detected acceleration, T _1, T
_2, T ₃ ... torque prediction value each motor is generated when a collision has not _{occurred, Tm 1, Tm 2, Tm} 3 ... torque command value servo driver is actually generated, ΔT _1, Δ
T ₂ , ΔT ₃ ... difference between torque predicted value and torque calculated from torque command value, ε ₁ , ε ₂ , ε ₃ ... collision judgment value,
Kpp: Position feedback proportional gain.

 ──────────────────────────────────────────────────続 き Continued on the front page F term (reference) 3C007 AS11 BS10 KS16 KS22 KS23 KS35 KS37 LU03 LV15 LV19 MS03 MS14 5H269 AB12 AB33 BB11 CC09 DD05 EE01 EE14 GG01 MM05 NN04 PP03

Claims (12)

[Claims] When a link of a robot having a plurality of joints or a tool or a work gripped at the tip of the link collides with a nearby obstacle or the like, the collision collides to avoid deformation of the link or the like and damage to the movable part of the robot. In the method of detecting the fact and stopping the robot immediately, the position, speed and acceleration of the motor detected by the servo driver for controlling each joint driving motor of the robot are
The torque to be generated by each motor when no collision occurs is based on the position, speed, acceleration of each joint of the robot and the robot mechanism parameters. A prediction is made by an inverse dynamics calculation based on the equation shown below, and a difference between the predicted torque value and the actual joint driving torque is obtained. If the absolute value of the difference is larger than a predetermined determination value, a collision occurs A collision detection / stop control method for an articulated robot, characterized in that it is regarded as a control method. 2. The position, speed, and acceleration of a motor used when predicting a torque to be generated by a motor driving each joint when a collision does not occur are determined by the position, speed, and acceleration of the motor detected by the servo driver. In place of the speed and acceleration, a joint position calculated by a reproduction operation of work data previously taught to instruct a position to a servo driver, and a speed and acceleration calculated based on the joint position are characterized in that: The collision detection / stop control method for an articulated robot according to claim 1. 3. The inverse dynamics calculation includes: n × n representing the torque generated at each joint by the acceleration of each joint of the robot and the interference torque generated by the acceleration of another joint, where n is the number of joints of the robot. 3. An inertia matrix, an n.times.1 viscous matrix indicating the influence of centrifugal force and Coriolis force, and an n.times.3 gravity matrix indicating torque generated by the influence of gravitational acceleration. 2. A collision detection / stop control method for an articulated robot according to the above. 4. The “inertia moment indicating the relationship between the acceleration exerted by the joint and the torque exerted on the joint itself” as a diagonal element in the n × n inertia matrix includes the inertia moment of the motor rotor including the speed reducer. 4. The multi-joint according to claim 3, wherein a torque generated by an inertia moment of a motor rotor including a speed reducer in each joint is added by multiplying by a square of a reduction ratio. Collision detection / stop control method for robots. 5. The formula used for the inverse dynamics calculation includes: The multi-joint robot according to any one of claims 1 to 4, wherein the frictional resistance of the motor bearing and the reduction gear in each joint can be considered. Collision detection / stop control method. 6. The robot according to claim 1, wherein the mechanism parameters of the robot are a position of a center of gravity, a mass, and a length of a link connected to each joint. Collision detection and stop control method for articulated robots. 7. The robot according to claim 6, wherein an inertia tensor is added to the mechanism parameters of the robot.
2. A collision detection / stop control method for an articulated robot according to the above. 8. The method according to claim 1, wherein the actual joint driving torque is a torque calculated from a torque command value actually generated by a servo driver. Detection and stop control method for an articulated robot. 9. The articulated robot according to claim 1, wherein the actual joint driving torque is a torque calculated from a torque response value of each motor. Of collision detection and stop control in a vehicle. 10. Among a plurality of joints, a joint operating at a low speed or at a low speed or a joint whose inertia force is small enough to stop operation due to a small inertia force is smaller than a maximum torque. The collision detection / stop control method for an articulated robot according to any one of claims 1 to 9, wherein a relief operation can be performed to reduce interference force. 11. A minimum torque value required to stop the operation of the robot for each joint from the speed of each joint and the load inertia at the time of a collision is calculated for each joint, and the minimum torque value is calculated as the torque of the servo driver. 11. The deceleration process according to claim 10, wherein the limiter is set, and the position feedback proportional gains of all the servo drivers are set to 0, and the speed feedback control is forcibly performed with the speed command value set to 0. A collision detection / stop control method for the described articulated robot. 12. In calculating the torque value of each joint necessary to stop the operation of the robot, it is assumed that all the joints stop at the same time, and after calculating the deceleration stop time for each joint, all the joints are stopped. 12. The collision detection / stop control method for an articulated robot according to claim 10, wherein the maximum value of the deceleration stop time is used for the deceleration stop time of any joint.