CN1771114A - Method and device for controlling a manipulator - Google Patents

Abstract

By appropriately selecting the command value or the actually measured value as the angular velocity for the friction torque calculation, the friction compensation can be effective in both cases when the operation is actively performed according to the angular velocity command and the passive operation, i.e., when pushed by an external force. Also, after the collision is detected, when the motor rotation direction and the collision direction are opposite, the position control is switched to the current control so that the motor generates a torque in the direction opposite to the motor rotation direction, thereby reducing the motor rotation speed and attenuating the collision energy. Thereafter, when the motor rotation speed becomes a set value or less, the mode is set to the flexible control, thereby eliminating deformation of the speed reduction device or the like caused by the collision. On the other hand, when the motor rotation direction and the collision direction are the same, the position control is directly switched to the flexible control without passing through the current control. By performing the operation in accordance with the collision force, it is possible to attenuate the collision torque.

Description

Method and device for controlling a manipulator

technical field

The invention relates to a method and a device for controlling a manipulator driven by a motor. More specifically, the present invention relates to a compliance servo control technique for controlling a manipulator, i.e. the invention relates to a method for controlling a manipulator stop after a collision with an object has been detected and equipment.

Background technique

Recently, robots have been used not only in industrial fields but also in public consumer fields. Therefore, ensuring security has become important. However, according to the stopper that stops the robot by detecting an external force applied to the robot at the time of collision using a force sensor, manufacturing cost and weight undesirably increase. Thus, it is desirable to enhance the performance of compliant servo control, including collision detection, and to enhance the performance of stop-motion control without the use of sensors.

As for the method for realizing the compliance servo control without using a sensor, a method is often adopted in which even when the position deviation is increased in the position feedback control, by suppressing the increase of the current command (electric current command) without generating excessive torque in the motor.

In accordance with the suppression amount of the current command in the feedback control, the intensity of the torque generated by the motor is also suppressed so that compliance can be enhanced.

A method for suppressing a current command in feedback control is disclosed in JP-A-09-179632 (US Patent No. 5994864), in which the current command is limited. There is also disclosed in JP-A-08-155868 a method for suppressing a current command in feedback control in which the feedback gain is lowered.

As described above, in order to enhance the compliance of the manipulator, it is important to suppress the current command in the feedback control. If the current is not suppressed in the feedback control, the compliance of the manipulator becomes close to the ordinary servo rigidity. Therefore, the compliance of the servo control is lowered.

However, in order to operate the manipulator, it is necessary to generate drive torque from the motor, where inertial torque, frictional torque, and gravitational torque are taken into consideration. Therefore, when the manipulator is operated only by feedback control, it is difficult to suppress the current command of the motor.

FIG. 3 is a block diagram illustrating a conventional method in which actual speed is used to control friction compensation. In said drawings, reference numeral 1 is a motor rotation angle command θ _com , reference numeral 2 is a feedback controller, reference numeral 3 is a current limiting part, reference numeral 4 is a feedback control current command I com , and reference numeral 4 is a feedback control current command I _com . The reference number 5 is the motor current I _m , the reference number 6 is the range of (motor+actual load), the reference number 7 is the motor torque constant K _t , the reference number 8 is the motor generated torque τmm, and the reference number is 9 is the external force τμ+τdyn+τdis provided to the motor, the reference numeral 10 is the transfer function of the motor inertia, the reference numeral 11 is the motor rotation angle θ _fb , the reference numeral 12 is the differential operator, and the reference numeral 13 is the motor Angular velocity command ω _com , reference numeral 14 is a differential operator, reference numeral 15 is a motor angular acceleration command α _com , reference numeral 16 is motor inertia (rotor + reduction gear primary end) J, and reference numeral 17 is an operating robot The required motor current I _ml , the reference number 18 is the reciprocal 1/K _t of the motor torque constant, the reference number 19 is the calculated value of the dynamic torque τdyn, the reference number 20 is the calculated value of the friction force τμ, and the attached Reference numeral 21 is a friction calculation block, reference numeral 22 is a dynamic calculation block, reference numeral 23 is a motor angular velocity ω _fb , reference numeral 24 is a differential operator, and reference numeral 25 is rotation angles of other axes.

The motor generating torque τm generated when operating the robot is expressed by Expression (1) when viewed from the motor driving side. The motor-generating torque τm is expressed by Expression (2) when viewed from the load side.

τmm=K _t *I _m (1)

τml＝J*α+τμ+τdyn+τdis (2)

In this case, the reference numerals used in expressions (1) and (2) are defined as follows.

K _t : motor torque constant

I _m : motor current

α: motor angular acceleration

ω: motor angular velocity

J: Motor inertia (rotor + primary side of reduction gear)

τμ: friction torque (converted to motor shaft end)

τdyn: dynamic torque (dynamic torque is the sum of gravitational torque, inertial force, earth deflection force (Coriolis) and spring force, which is converted to the motor shaft end.)

τdis: Disturbance torque (Disturbance torque is contact torque or parameter error provided from outside. Disturbance torque is converted to motor shaft end.)

When the disturbance torque τdis=0 in expression (2), it is also possible to calculate the motor current I _ml by expressions (1) and (2), which is required to operate the manipulator.

I _ml ＝J*α+τμ+τdyn)/K _t (3)

As shown in Fig. 3, when I _ml calculated by the expression (3) is added to the feedback current command I _com , if the disturbance torque τdis = 0, it becomes possible for the manipulator to reach even when the feedback current is 0 destination location.

In FIG. 3 , the feedback current command I _com ( 4 ) can be obtained when the feedback controller ( 2 ) performs PID calculation from the rotation angle command θ _com ( 1 ) and the actual motor rotation angle θ _fb and performs current limitation ( 3 ). Regarding the means for limiting the current (3), a system in which the limitation is set and a system in which the feedback gain is reduced are provided.

On the other hand, I _ml (17) calculated by Expression (3) can be obtained as follows. The angular velocity θ _com (15) obtained by performing differential operations (12) and (14) twice on the motor rotation command θ _com (1) is multiplied by the motor inertia J (16). Friction torque τμ(20) and dynamic torque τdyn(19) are added to the value thus obtained. By multiplying the obtained value by the reciprocal 1/K _t (18) of the motor torque constant, I _ml (17) calculated by expression (3) can be obtained.

That is, when the motor current I _ml required for the operation of the robot can be accurately calculated from the expression (3), it becomes possible to suppress the current command in the feedback control. Therefore, the compliance of the robot can be enhanced.

However, in reality, a calculation error occurs due to a parameter error of the expression (3). Therefore, when the current is strongly suppressed in the feedback control, it becomes impossible to compensate the error, and the manipulator becomes out of control, and the positional deviation cannot be reduced, that is, there is a possibility of the manipulator out of control.

When the current command is made 0 in the feedback control, if the positional deviation is enlarged due to the contact torque supplied from the outside, no force is generated to return the manipulator to the initial position.

As described above, how much the current command by feedback control can be suppressed depends on the calculation accuracy of expression (3).

The friction torque τμ is one of the main terms of the motor current I _ml calculated by the expression (3) and is required to operate the robot, which includes: the static friction torque τμs and the dynamic friction torque τμm, by the action of force Orientation to determine them; and viscous friction τμd (coefficient D of viscosity), which is proportional to velocity.

τμ＝τμs+τμm+τμd (4)

However, in Expression (4), each term is calculated as follows.

τμs＝τμs0*sgn1(ω)           (5)

τμm＝τμm0*sgn1(ω) (6)

$\mathrm{<span\; class="notranslate">\; sgn</span>}\mathrm{<span\; class="notranslate">\; 2</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; \&omega;</span>}<span\; class="notranslate">\; )</span>\left\{\begin{array}{cc}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}& <span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; \&omega;</span>}<span\; class="notranslate">\; <</span>\mathrm{<span\; class="notranslate">\; 0</span>}<span\; class="notranslate">\; )</span>\\ \mathrm{<span\; class="notranslate">\; 0</span>}& <span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; \&omega;</span>}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; 0</span>}<span\; class="notranslate">\; )</span>\\ \mathrm{<span\; class="notranslate">\; 1</span>}& <span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; \&omega;</span>}<span\; class="notranslate">\; ></span>\mathrm{<span\; class="notranslate">\; 0</span>}<span\; class="notranslate">\; )</span>\end{array}\right.$

τμd＝D*ω (7)

It can be seen from expressions (5)-(7) that all friction torques are calculated from the angular velocity ω.

Regarding the angular velocity ω used to calculate the friction torque, the angular velocity ω _FB subjected to the feedback impedance is used in the above-mentioned conventional example (JP-A-9-179632). In another conventional example (JP-A-10-180663), the angular velocity command ω _com obtained when the position command θ _com is differentiated is used.

However, in the compliance control of the robot, two cases are provided. One is the case where the robot actively moves according to the position command θ _com . The other is the case where the robot is passively moved by an external force.

As shown in Fig. 3, when using the actual angular velocity ω _fb (23) obtained when differentiating the actual motor rotation angle θ _fb (11) as the friction torque τμ( 20) to reflect the velocity fluctuation caused by the external force τdis which is a part of the disturbance torque (9) supplied to the motor, so that the calculation accuracy of the friction torque τμ(20) can be enhanced.

However, when the robot is actively operated from a state where the robot is completely stopped, the direction of operation is unknown until the robot starts to operate. Therefore, it is impossible to calculate the static friction torque τμs.

Until the robot starts to operate, the actual angular velocity ω _FB is 0, and the dynamic friction torque τμm and viscous friction τμd calculated by Expressions (6) and (7) are of course 0. Therefore, the friction torque calculated by Expression (4) is τμ0, and no motor torque for operating the robot is generated at all.

In this state, when the torque caused by the feedback-controlled current command I _com (4) is suppressed to enhance compliance and becomes lower than the actual static friction torque τμs, even if the rotation angle command θ _com is generated (1), the robot does not move either.

On the other hand, as shown in Fig. 4, when the angular velocity ω _com (13) obtained when differentiating the motor rotation angle θ _com (1) is used for calculating (21) the angular velocity used for the friction torque τμ (20) When ω, these problems are solved. That is, even when the robot is not operated, when the angular velocity command ω _com (13) is used to calculate (21) the friction torque τμ (20) by the expression (4) and add it to the feedback current command I _com (4) , can compensate the actual friction torque. Even when the current I _com ( 4 ) controlled by feedback is suppressed, the robot can be operated.

However, when the angular velocity command ω _com (13) is used to calculate (21) the angular velocity ω used for the friction torque τμ (20), the robot can be actively operated in accordance with the angular velocity command ω _com (13). However, in the case of angular velocity fluctuation due to disturbance torque τdis in the middle of the operation, a large error is caused between the angular velocity command ω _com (13) and the actual angular velocity ω _fb (reference numeral 23 in FIG. 3 ). Then, the calculation error of the viscous friction torque τμd calculated by the expression (7) increases.

When the robot is pushed by an external force and the robot is stopped while the angular velocity command ω _com (13) is 0 to stop the robot, the friction torque τμ calculated by the expression (4) is always 0. Therefore, the actual friction torque cannot be compensated at all.

Also, even when the robot is not pushed by an external force, in actual operation, a delay in following is caused in the feedback control by the feedback controller. Therefore, when the robot is stopped, the angular velocity command ω _com ( 13 ) becomes 0 before the actual angular velocity ω _fb (reference numeral 23 in FIG. 3 ) becomes 0. Therefore, the friction torque τ calculated by the expression (4) also becomes 0 at this time, and friction compensation is not performed. That is, when the angular velocity command ω _com (13) reaches 0, the robot is suddenly stopped, and cannot reach the target position, and may also cause vibration.

In this case, in the case where the torque generated by the motor in accordance with the current command I _com (4) of the feedback control is suppressed to be lower than the actual friction torque, even when the external force increases the positional deviation, does not operate robot, and it is not possible to reduce the positional deviation.

In other words, although friction compensation is performed, it is impossible to set the feedback current command lower than the actual friction torque. Therefore, the compliance of the robot cannot be enhanced.

As described above, in the case of calculating the friction torque τμ by the expression (4), according to the method in which one of the actual angular velocity ω _fb and the angular velocity command ω _com is used as the angular velocity, even when the calculated friction torque τμ is used When the current I _ml calculated by expression (3) is added to the feedback control current, it cannot compensate the actual friction torque.

As shown in FIG. 3, when the friction torque τμ is compensated 100% by using the actual angular velocity ω _fb (23) for the angular velocity used in calculating (21) the friction torque τμ(20) using (5), the control system The feedback characteristic of the sensor undergoes friction compensation. Therefore, the control system is operated as if there were no friction. Thus, while compliance may be enhanced, the feedback system becomes vibrated.

On the other hand, as shown in FIG. 4 , in the case of using the angular velocity command ω _com ( 13 ) for the angular velocity ω used to calculate ( 21 ) the friction torque τ μ ( 20 ), the feedback characteristics of the control system are not affected. Therefore, in order to improve the target following characteristic, it is desirable to perform 100% compensation.

Next, a second conventional example will be described below.

Regarding the method of obtaining the collision torque without using a sensor, the following method is generally used. The motor generated torque is obtained when the torque loss generated in the motor and reduction gear is subtracted from the torque generated by the driving current of the motor. The collision torque is obtained when the torque necessary for the output of the reduction gear, which is dynamically calculated and called dynamic torque, is subtracted from the motor-generated torque found earlier.

For example, friction torque corresponding to the loss of torque generated by the motor is defined as the sum of a speed-proportional term (viscous friction torque) and a static term (Coulomb's friction torque) and calculated. This is disclosed in, for example, JP-A-2002-283276.

According to JP-A-6-083403 (US Patent No. 6298283), the following technique is proposed. When the fluctuation of the parameters of the robot is calculated by the estimated algorithm and added to the torque (current) command, the fluctuation factor is eliminated. In this conventional example, the friction torque corresponding to the loss induced in the torque produced by the motor is defined as the sum of a speed-proportional term and a static term, and is estimated by an estimation algorithm.

Then, when the collision torque is obtained without using the sensor when subtracting the dynamic torque of the robot from the motor-generated torque or in the case of improving the servo follow-up characteristics by the feed-forward control of the dynamic torque so as to exhibit the maximum motor drive In the case of force, it is necessary to accurately calculate the torque generated by the motor and the torque required for the output of the reduction gear of the robot.

When viewed from the motor drive side, the motor-generated torque τm at the time of operating the robot can be expressed by Expression (8). Also, when viewed from the load side, the motor-generated torque τm at the time of operating the robot can be expressed by Expression (9).

τm＝K _t *I _m -(J*α+D*ω+τμsgn(ω)) (8)

τml＝τdyn++τdis (9)

In this regard, the reference symbols shown in Expressions (1) and (2) are defined as follows.

K _t : motor torque constant

I _m : motor current

α: motor angular acceleration

ω: motor angular velocity

J: Motor inertia (rotor + primary side of reduction gear)

D: Viscous friction coefficient (converted to motor shaft end)

τμ: friction torque (converted to motor shaft end)

τg: gravity torque (converted to motor shaft end)

τdyn: dynamic torque (dynamic torque is the sum of gravitational torque, inertial force, earth deflection force and elastic force, which is converted to the motor shaft end.)

τdis: Disturbance torque (Disturbance torque is collision torque or parameter error. Disturbance torque is converted to motor shaft end.)

$\mathrm{<span\; class="notranslate">\; sgn</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; \&omega;</span>}<span\; class="notranslate">\; )</span>\left\{\begin{array}{cc}\mathrm{<span\; class="notranslate">\; 1</span>}& <span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; \&omega;</span>}<span\; class="notranslate">\; ></span>\mathrm{<span\; class="notranslate">\; 0</span>}<span\; class="notranslate">\; )</span>\\ \mathrm{<span\; class="notranslate">\; 0</span>}& <span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; \&omega;</span>}<span\; class="notranslate">\; \&NotEqual;</span>\mathrm{<span\; class="notranslate">\; 0</span>}<span\; class="notranslate">\; )</span>\\ <span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}& <span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; \&omega;</span>}<span\; class="notranslate">\; <</span>\mathrm{<span\; class="notranslate">\; 0</span>}<span\; class="notranslate">\; )</span>\end{array}\right.$

Since the motor and the manipulator are connected to each other through a reduction gear, it is necessary to use the reduction ratio to convert the terms in Expression (9) other than the motor inertia term J to the motor shaft end.

When it is assumed that τmm=ml in (8) and (9), the collision torque τdis can be obtained by the following modified expression (10).

τdis＝K _t *I _m -(J*α+D*ω+τμ*sgn(ω)+τdyn) (10)

In the conventional example, the dynamic friction term τμ in Expression (10) is calculated as a fixed value. However, when the dynamic friction torque term is calculated as a fixed value, a large calculation error of about 10% of the motor generated torque is caused in the case of high motor generated torque at the time of acceleration and deceleration.

On the other hand, feedback control for the purpose of improving servo control characteristics can be realized as follows. Under the condition of disturbance torque τdis=0, that is, under the condition that the robot is not in contact with the outside and does not cause parameter errors, the motor current I _m is obtained from Expression (10). The current thus obtained is denoted I _ff . Feedback control can be achieved when I _ff is added to the current command.

I _ff (J*α+D*ω+τμ*sgn(ω)+τdyn)/K _t (11)

In the conventional example, the calculation of the expression (11) is not used, but the dynamic friction term τμ is estimated by an estimation algorithm. However, it does not change with time, but the friction torque greatly changes in a short time of acceleration and deceleration. Then, by the estimation of the estimation algorithm, a delay of the phase occurs, and it is impossible to completely compensate.

When the calculation is performed in advance not by the estimation algorithm but by the dynamic torque, no phase delay is caused. However, when the calculation is performed using the expression (11) while setting the dynamic friction torque at a fixed value, about 10% of the torque generated by the motor is caused in the case where the torque generated by the motor is high at the time of acceleration and deceleration large calculation error.

This error will be explained as follows.

FIG. 11 is a view showing motor-generated torque τmm(1), τml(2) and speed (3) calculated by Expressions (8) and (9) when the robot is made to perform the reciprocating motion shown in FIG. 12 .

In this case, the manipulator to be used is a six-axis vertical type multi-joint robot whose portable mass is 6 kg and whose total arm length is about 1.3 meters. In FIG. 12 , three wrist axes are omitted, and three basic axes are shown. When measuring, operate the FA axis as the third axis.

In this case, the measurement is performed under the condition of disturbance torque τdis=0, that is, under the condition that the robot is not in contact with the outside and does not cause parameter errors.

As shown in FIG. 11, in τmm(1) and τml(2), an error (4) of about 4% of the peak torque occurs, that is, it can be understood that expression (8) has an error factor.

When comparing when accelerating and decelerating, the results are as follows.

τmm＞τml

In the acceleration and deceleration of the operation part, the angular acceleration and the angular deceleration are equal in magnitude although their directions are opposite to each other. The manipulator operates in a symmetrical pattern with respect to gravity.

Therefore, in order to reduce the error by reducing τdyn, it is necessary to increase the dynamic friction torque τμ. However, when the dynamic friction torque τμ is increased while being kept at a constant value as shown in FIG. 13, although the error (4) at the peak torque is reduced, it is increased at a constant speed. error (5).

That is, when the dynamic friction torque τμ is taken as a constant, the error factor caused by τμ cannot be eliminated. Therefore, in Expressions (10) and (11) including τμ, the same error occurs.

Therefore, in the sensorless collision torque detection, when the dynamic friction torque τμ is taken as a constant, although the manipulator does not collide with the object when accelerating or decelerating, expression (11) outputs a current corresponding to the error to as the collision torque. For the above reasons, in order to prevent erroneous detection from occurring, it is necessary to lower the collision detection sensitivity.

On the other hand, in the case of feedforward control of the torque required for the output of the reduction gear, when the dynamic friction torque τμ is made constant, calculation errors increase, and there is a possibility that the feedforward compensation torque becomes insufficient. When an estimation algorithm is used in order to prevent calculation errors from being generated, it is difficult to estimate a sudden change in friction torque at the time of acceleration or deceleration without causing a delay in phase. Then, there is a possibility that the deterioration of the control performance cannot be prevented sufficiently.

Next, a method of stopping the robot after collision detection will be described. The following methods have been proposed: a method of returning the manipulator to a position where a collision is detected (JP-A-2002-117618 (US Patent No. 6429617)); a method of forcibly setting a speed command at 0 in order to stop the manipulator (JP-A-2000 -52286); method of stopping a manipulator by maximum reverse motor torque opposite to the direction of rotation of the motor (Japanese Patent No. 3212571 (US Patent No. 6298283) and Japanese Patent No. 2871993 (US Patent No. 5418440)) .

According to the method of returning the manipulator to the collision detection position, the manipulator returns to the initial position through position control. Therefore, the stop time depends on the response characteristics of the position control. Generally, the response characteristic of the position control is several tens of Hertz at most. Therefore, the response characteristics are not very high at the time of the stop collision, and the stop time is extended, and it is impossible to prevent the occurrence of wear caused by the collision.

According to the method of stopping the manipulator by forcibly setting the speed command at 0, the stop time depends on the response characteristics of the speed control. In this case, the response characteristic is several hundred Hz, which is larger than that in the case of position control. However, it is inferior to the response characteristic of current control (several thousand Hz).

According to this method, the deformation caused by the collision is maintained when the manipulator is stopped because the servo rigidity of the integrated position control through the speed control is high. Therefore, according to JP-A-2000-052286, when the speed integration gain is made 0, the position control rigidity is softened, and thus the deformation problem caused by the collision can be solved. However, in order to enhance the compliance, the gain must be reduced in proportion to the speed, which deteriorates the speed response characteristics and extends the stop time. It is difficult to make stop time and compliance compatible with each other.

According to the method of stopping the manipulator by the maximum reverse motor torque with respect to the rotation direction of the motor, the response characteristic of the current control for generating the reverse torque is high so that the response characteristic can be several thousand Hz, that is, the response characteristic is good. However, according to the method disclosed in Japanese Patent No. 3212571, it is necessary to set the time for applying the reverse torque in advance. When this application time is short, it is impossible to sufficiently reduce the speed, and the damage caused by the collision increases. When this application time is long, redundant movements are performed in the reverse direction, and it is possible that the manipulator again causes a collision. According to the method disclosed in Japanese Patent No. 2871993, a method is proposed in which the maximum reverse torque is applied until the motor is stopped. In this method, it is not necessary to predetermine the application time for applying the reverse torque. Therefore, the above-mentioned problems are solved. However, the problem of deformation caused by collision cannot be solved only when the motor is stopped. Because the generation of the maximum reverse torque itself is the state where the control is producing the maximum output in the open loop, there is a possibility of applying the reverse torque in the case where the speed is so low that the robot cannot be damaged even when the robot collides with an object higher risk.

In either system, in an axis whose collision direction coincides with the rotation direction of the motor, the strength of the collision force is increased when the manipulator returns to the collision detection position or stops suddenly.

FIG. 15 is a view showing this state, in which a two-axis robot is used for explanation. Generally, a general vertical type multi-joint robot consists of 6 axes. However, to simplify the description, a 2-axis model will be described below.

In FIG. 15( a ), the shaft UA ( 41 ) is operated in the direction of angular velocity ω _fb ( 1 ), and the shaft FA ( 42 ) is operated in the direction of angular velocity ω _fb ′ ( 6 ). When time elapses and each manipulator operates in the direction shown in Figure 15(b) and collides with an obstacle (43), a collision force (44) is generated and provides the shaft UA (41) with the direction of rotation of the motor The opposite force, ie in the direction in which the speed can be reduced, provides the shaft UA ( 41 ) with an impact torque τdis. On the other hand, a force is applied to the shaft FA (42) in the same direction as the rotation direction of the motor, that is, a collision torque τdis' (10) is applied to the shaft FA (42) in a direction in which the speed can be increased.

Thereafter, in order to return the shaft FA ( 42 ) to the collision detection position or to stop the shaft FA ( 42 ) abruptly, it is necessary to generate torque by the motor so that the motor rotation can be reduced. However, the direction of this torque is opposite to that of the collision torque τdis'(10). Therefore, the strength of the collision torque is conversely increased.

According to the method of returning the manipulator to the collision detection position, although the axis whose motor rotation direction is opposite to the collision direction (axis UA in Figure 15) returns to the collision detection position, the axis whose motor rotation direction is the same as the collision direction (axis The axis FA) is not reversed, and the operation already carried out is continued until the collision. In this way, the above-mentioned problems are solved.

However, without using sensors to detect the collision, the collision torque is estimated from the information of the robot's mechanical parameters, position, velocity, acceleration and current. Therefore, compared with the case where a collision detection sensor is provided, a detection error increases. For the above reasons, in the case of an axis whose collision detection torque is low, there is a possibility that the direction is erroneously detected, and an appropriate stopping method cannot be selected.

In the case of an axis whose detected collision torque value is low, it is safer not to detect the collision direction and to reduce the motor rotation speed in order to reduce the kinetic energy. However, since the collision direction is unknown, it is better in some cases not to reduce the motor rotation speed, ie, unlike the method described in Japanese Patent No. 2871993, the opposite torque should not be applied until the motor stops. In the case where the rotational speed of the motor is low so that the robot cannot be damaged even when the robot collides with an obstacle, reverse torque should not be applied.

Also, in the case of a vertical type multi-joint robot, it is impossible to ignore the disturbance force between axes. Then, it is possible to provide a speed-reducing force to one shaft whose speed should not be reduced by the disturbing force provided from the shaft to which the reverse torque is applied. In either case, reverse torque should be applied to the required shaft for the minimum period of time.

Contents of the invention

The present invention has been achieved to solve the above-mentioned problems. An object of the present invention is to provide a control method that can enhance compliance of a robot driven by a motor by performing feedback control in compliance control to suppress current limitation below frictional torque.

Another object of the present invention is to provide a control method for controlling a robot that can detect the collision rotation with high accuracy by making the dynamic friction torque of the reduction gear coincide not with a fixed value but with a value corresponding to the actual characteristic. moment.

Another object of the present invention is to provide a control method for controlling a robot capable of enhancing motor rotation before operation by making the dynamic friction torque of the reduction gear coincide not with a fixed value but with a value corresponding to actual characteristics. moment calculation accuracy, and obtain the most appropriate feedback compensation whose delay phase is small.

According to the present invention, there is provided a method of controlling a robot, characterized by: detecting the rotation angle of a motor used to drive the robot; calculating an actual measured value of angular velocity from said rotation angle; One of the angular velocity of the command value and the angular velocity of the actual measurement value is used to calculate the friction torque, wherein the angular velocity with a higher absolute value is used in this calculation; The command value of the motor increases by a value corresponding to the friction torque. Due to this method, current limitation by feedback control can be suppressed below the friction torque. Therefore, it is possible to realize a control method whose compliance is high.

When one of the command value and the actually measured value is appropriately selected as the angular velocity for friction torque calculation while changing the friction compensation rate, the feedback characteristic can be prevented from vibrating, and the target following characteristic can also be improved.

The present invention provides a method of controlling a robot driven by a motor via a reduction gear, characterized in that when the required torque is output by subtracting the reduction gear obtained through the dynamic calculation of the robot from the torque generated by the motor When calculating the external force, the dynamic friction torque of the reduction gear is calculated to increase corresponding to the torque required for the reduction gear output.

The present invention provides a method of controlling a robot driven by a motor via a reduction gear, characterized by following the inverse dynamic calculation of the robot for obtaining the torque required for the output of the reduction gear, and also according to the dynamic friction of the reduction gear Torque calculation, motor output torque compensation is performed through feedback control; and when feedback control is performed, the dynamic friction torque of the reduction gear is calculated to increase in proportion to the torque required for the reduction gear output.

According to the present invention, in the case of an axis whose collision torque direction is opposite to the rotation direction of the motor after a collision is detected, when the robot control mode is changed from position control for generating a current command for making the actual position follow the position command to Motor rotation speed is reduced and crash energy is reduced during current control transitions for commanding current through the motor to produce torque whose direction is opposite to the motor's direction of rotation. Thereafter, when the motor speed is reduced to a value lower than the set value, the control mode is switched to compliance control so that the manipulator can follow the collision force direction and solve the deformation problem caused by the collision in the reduction gear. It is possible to stop and reduce the speed by the current control whose response characteristic is the highest, and when the motor speed is monitored, the application time of the motor torque whose direction is opposite to the motor rotation direction can be determined. Therefore, it is not necessary to set the motor torque application time in advance.

On the other hand, in the case of an axis whose collision torque is in the same direction as the motor rotation direction, current control is not performed, and the control mode is directly switched from position control to compliance control. When the shaft is operated while following a crash force, the crash torque can be weakened.

In the case of an axis whose motor rotation speed at the time of collision is lower than the set value, regardless of the motor rotation direction and collision torque direction, the control mode is directly switched from position control to compliance control without current control. Therefore, an open-loop state of only current control is not generated when unnecessary.

Description of drawings

FIG. 1 is a block diagram showing a control method for controlling friction compensation in the first and second embodiments.

FIG. 2 is a block diagram showing a control method for controlling friction compensation in the third embodiment.

FIG. 3 is a block diagram showing a control method for controlling friction compensation in a conventional example in which an actual speed is used.

FIG. 4 is a block diagram showing a control method for controlling friction compensation in a conventional example, in which a speed command is used.

5 is a block diagram showing a collision torque detection method of Embodiment 1 of the present invention.

FIG. 6 is a view showing one example of dynamic friction torque characteristics required for a reduction gear output of a harmonic reduction gear.

FIG. 7 is a view showing one example of dynamic friction torque characteristics required for reduction gear output of the RV reduction gear.

FIG. 8 is a view showing parameters in the dynamic friction torque approximate expression.

FIG. 9 is a view showing a torque error required for a reduction gear output according to a dynamic friction torque calculation method of the present invention.

Fig. 10 is a block diagram of torque feed-forward control required for reduction gear output in Embodiment 2 of the present invention.

FIG. 11 is a view showing a torque error required for a reduction gear output according to a dynamic friction torque calculation method of a conventional example.

FIG. 12 is a view showing an operation in measuring the torque required for the output of the reduction gear.

FIG. 13 is a view showing a torque error required for reduction gear output in the case of increasing dynamic friction torque.

FIG. 14 is a timing chart showing the collision stop control method in the first embodiment.

Fig. 15 is a robot operation diagram showing the direction of velocity and the direction of collision torque at the time of collision.

Fig. 16 is a block diagram showing the collision stop control means (position control mode) in the first embodiment.

FIG. 17 is a block diagram showing the crash stop control device (current control mode) in the first embodiment.

Fig. 18 is a block diagram showing the crash stop control means (compliance control mode) in the first embodiment.

Fig. 19 is a block diagram showing the collision stop control means (position control mode) in the second embodiment.

FIG. 20 is a block diagram showing a crash stop control device (current control mode) in the second embodiment.

FIG. 21 is a block diagram showing a crash stop control device (current control mode) in the fourth embodiment.

Fig. 22 is a block diagram showing the collision stop control means (position control mode) in the third embodiment.

FIG. 23 is a block diagram showing a crash stop control device (current control mode) in the third embodiment.

Fig. 24 is a block diagram showing the crash stop control means (compliance control mode) in the third embodiment.

FIG. 25 is a timing chart showing a collision stop control method in the third embodiment.

Detailed ways

Referring to the drawings, preferred embodiments of the robot control method of the present invention will be explained as follows.

first embodiment

FIG. 1 is a block diagram showing a control method of the present invention. In FIG. 1, reference numeral 26 is a velocity converting device, and reference numeral 27 is an angular velocity ω selected by the velocity converting device. The feedback current command I _com (4) can be obtained by performing PID calculation from the rotation angle command θ _com (1) and the actual motor rotation angle θ _fb by the feedback controller (2), and performing current limitation (3). Regarding the means for current limitation (3), there are provided a system in which the limitation is set and a system in which the feedback gain is reduced.

On the other hand, I _ml can be calculated by Expression (3) as follows (17). The angular acceleration α _com (15) obtained when the motor rotation command θ _com (1) is differentially calculated twice (12), (14) is multiplied by the motor inertia J (16). Friction torque τμ(20) and dynamic torque τdyn(19) are added to the value thus obtained. Multiply the value thus obtained by the inverse of the motor torque constant 1/K _t (18). In this way, 1 _ml can be calculated (17).

For the angular velocity ω(27) used when calculating (21) friction torque τμ(20) by expressions (5)-(9), the speed conversion device (26) _selects The angular velocity command ω _com (13) obtained when differentiating, or the actual angular velocity _ω fb (23) obtained when differentiating (24) the actual motor rotation angle θ _fb (11).

The speed is converted by the speed conversion device (26) according to the following expression.

$\mathrm{<span\; class="notranslate">\; \&omega;</span>}<span\; class="notranslate">\; =</span>\left\{\begin{array}{cc}\mathrm{<span\; class="notranslate">\; \&omega;\; fb</span>}& <span\; class="notranslate">\; (</span><span\; class="notranslate">\; |</span>\mathrm{<span\; class="notranslate">\; \&omega;com</span>}<span\; class="notranslate">\; |</span><span\; class="notranslate">\; \&le;</span><span\; class="notranslate">\; |</span>\mathrm{<span\; class="notranslate">\; \&omega;\; fb</span>}<span\; class="notranslate">\; |</span><span\; class="notranslate">\; )</span>\\ \mathrm{<span\; class="notranslate">\; \&omega;com</span>}& <span\; class="notranslate">\; (</span><span\; class="notranslate">\; |</span>\mathrm{<span\; class="notranslate">\; \&omega;com</span>}<span\; class="notranslate">\; |</span><span\; class="notranslate">\; ></span><span\; class="notranslate">\; |</span>\mathrm{<span\; class="notranslate">\; \&omega;\; fb</span>}<span\; class="notranslate">\; |</span><span\; class="notranslate">\; )</span>\end{array}\right.<span\; class="notranslate">\; .</span><span\; class="notranslate">\; .</span><span\; class="notranslate">\; .</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 12</span>}<span\; class="notranslate">\; )</span>$

In expression (12), the absolute value of the angular velocity command ω _com (13) and the absolute value of the actual angular velocity ω _fb (23) are compared with each other, and the higher value is selected as ω (27).

Using this ω, the friction torque τμ(20) is calculated by Expressions (5)-(7).

When selection is made as described above, even when compliance control becomes effective, in the case where the position command θ _com (1) is input and the manipulator is actively operated or in the case where the manipulator is pushed by the external force τdis and the manipulator is passively operated , it is also possible to properly calculate the friction torque τμ(20) without being reduced to zero.

Even when the robot is pushed by an external force τdis and the position deviation increases while there is a difference between the angular velocity command ω _com (13) and the actual angular velocity ω _fb (23) while the robot is being actively operated, if the actual angular velocity ω _fb (23) becomes larger than the absolute value of the angular velocity command ω _com (13), the actual angular velocity ω _fb (23) is adopted as ω (27). Therefore, factors causing errors can be reduced in the calculation of the friction torque τμ(20).

Also, at the time of stop operation, even when the angular velocity command ω _com (13) becomes 0 before the actual angular velocity ω _fb (23) becomes 0, if the absolute value of the actual angular velocity ω _fb (23) becomes larger than the angular velocity command The absolute value of ω _com (13), then use ω _fb (23) as ω (27). Therefore, the friction compensation by the friction torque τ calculated by the expression (4) can be continued from this point of time. That is, it is possible to prevent sudden stop caused by cancellation of friction compensation when the angular velocity command ω _com ( 13 ) has reached 0. Therefore, it is possible to prevent such a problem that the manipulator cannot reach the target position or vibration is generated in the manipulator.

Due to the above constitution, even when the limitation of the current I _com (4) by the feedback control is suppressed to be lower than the actual friction torque in order to enhance the compliance of the control, it is possible to prevent the occurrence of the problem that the manipulator cannot reach the target position or Vibrates extra when the robot is stopped.

second embodiment

In the expression (12) showing the conversion of the speed in the first embodiment, at least one of the speed command value and the actual measurement value is multiplied by a weighting coefficient.

$\mathrm{<span\; class="notranslate">\; \&omega;</span>}<span\; class="notranslate">\; =</span>\left\{\begin{array}{cc}\mathrm{<span\; class="notranslate">\; \&omega;\; fb</span>}& <span\; class="notranslate">\; (</span><span\; class="notranslate">\; |</span>\mathrm{<span\; class="notranslate">\; kcl</span>}<span\; class="notranslate">\; *</span>\mathrm{<span\; class="notranslate">\; \&omega;com</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; <figure-callout\; id="2"\; label="kc"\; filenames="A200480009285.X00311.png,A200480009285.X00321.png"\; state="\{\{state\}\}">kc</figure-callout></span>}\mathrm{<span\; class="notranslate">\; 2</span>}<span\; class="notranslate">\; |</span><span\; class="notranslate">\; \&le;</span><span\; class="notranslate">\; |</span>\mathrm{<span\; class="notranslate">\; \&omega;\; fb</span>}<span\; class="notranslate">\; |</span><span\; class="notranslate">\; )</span>\\ \mathrm{<span\; class="notranslate">\; \&omega;com</span>}& <span\; class="notranslate">\; (</span><span\; class="notranslate">\; |</span>\mathrm{<span\; class="notranslate">\; kcl</span>}<span\; class="notranslate">\; *</span>\mathrm{<span\; class="notranslate">\; \&omega;com</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; <figure-callout\; id="2"\; label="kc"\; filenames="A200480009285.X00311.png,A200480009285.X00321.png"\; state="\{\{state\}\}">kc</figure-callout></span>}\mathrm{<span\; class="notranslate">\; 2</span>}<span\; class="notranslate">\; |</span><span\; class="notranslate">\; ></span><span\; class="notranslate">\; |</span>\mathrm{<span\; class="notranslate">\; \&omega;\; fb</span>}<span\; class="notranslate">\; |</span><span\; class="notranslate">\; )</span>\end{array}\right.<span\; class="notranslate">\; .</span><span\; class="notranslate">\; .</span><span\; class="notranslate">\; .</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 13</span>}<span\; class="notranslate">\; )</span>$

When the composition is established as shown by expression (13), priority is given to one of the speed command value and the actual measurement value so that it can be adopted as the speed.

Since the actually measured value ω _fb includes a measurement error, for example, when the weighting coefficient in Expression (13) is set at a value as follows, the speed command ω _com can be preferentially selected.

kc1>1 and kc2>0 (14)

third embodiment

Fig. 2 is a block diagram showing a control method of the third embodiment.

The actual angular velocity ω _fb (23) input to the velocity conversion device (26) is multiplied by the friction compensation rate kμ.

It is expressed by Expression (15) as follows.

$\mathrm{<span\; class="notranslate">\; \&omega;</span>}<span\; class="notranslate">\; =</span>\left\{\begin{array}{cc}\mathrm{<span\; class="notranslate">\; k</span>}\mathrm{<span\; class="notranslate">\; \&mu;</span>}<span\; class="notranslate">\; *</span>\mathrm{<span\; class="notranslate">\; \&omega;\; fb</span>}& <span\; class="notranslate">\; (</span><span\; class="notranslate">\; |</span>\mathrm{<span\; class="notranslate">\; kcl</span>}<span\; class="notranslate">\; *</span>\mathrm{<span\; class="notranslate">\; \&omega;com</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; <figure-callout\; id="2"\; label="kc"\; filenames="A200480009285.X00311.png,A200480009285.X00321.png"\; state="\{\{state\}\}">kc</figure-callout></span>}\mathrm{<span\; class="notranslate">\; 2</span>}<span\; class="notranslate">\; |</span><span\; class="notranslate">\; \&le;</span><span\; class="notranslate">\; |</span>\mathrm{<span\; class="notranslate">\; \&omega;\; fb</span>}<span\; class="notranslate">\; |</span><span\; class="notranslate">\; )</span>\\ \mathrm{<span\; class="notranslate">\; \&omega;com</span>}& <span\; class="notranslate">\; (</span><span\; class="notranslate">\; |</span>\mathrm{<span\; class="notranslate">\; kcl</span>}<span\; class="notranslate">\; *</span>\mathrm{<span\; class="notranslate">\; \&omega;com</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; <figure-callout\; id="2"\; label="kc"\; filenames="A200480009285.X00311.png,A200480009285.X00321.png"\; state="\{\{state\}\}">kc</figure-callout></span>}\mathrm{<span\; class="notranslate">\; 2</span>}<span\; class="notranslate">\; |</span><span\; class="notranslate">\; ></span><span\; class="notranslate">\; |</span>\mathrm{<span\; class="notranslate">\; \&omega;\; fb</span>}<span\; class="notranslate">\; |</span><span\; class="notranslate">\; )</span>\end{array}\right.<span\; class="notranslate">\; .</span><span\; class="notranslate">\; .</span><span\; class="notranslate">\; .</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 15</span>}<span\; class="notranslate">\; )</span>$

kμ: Friction compensation rate

When using the angular velocity ω(27) obtained by the above expression (15), in selecting the actual angular velocity ωFB(23) for the angular velocity used when calculating (21) the friction torque τμ(20) by the expression (5) In some cases, when the friction compensation rate kμ is set at a value not larger than 1, the friction torque τμ(20) cannot be compensated 100%. Therefore, it is possible to adjust so that the feedback characteristic does not vibrate.

On the other hand, in the case of using the angular velocity command ω _com (13) for the angular velocity ω used for the calculation (21) of the friction torque τμ (20), the friction torque τμ (20) can be compensated 100% without Affects the feedback characteristics of the control system. Therefore, target following characteristics can be improved.

Fourth embodiment

6 and 7 are views showing measurement results in which dynamic friction torque in a typical reduction gear used in a robot is measured with respect to fluctuations in dynamic torque τdyn under the condition of disturbance torque τdis=0. FIG. 6 is a view showing characteristics in the case of a harmonic reduction gear, and FIG. 7 is a view showing characteristics in the case of an RV reduction gear which is an eccentric differential type reduction gear type.

It can be seen from FIGS. 6 and 7 that the dynamic friction torque is increased in accordance with the increase of the dynamic torque τdyn. The dynamic friction torque can be approximated by Expression (16).

$\mathrm{<span\; class="notranslate">\; \&tau;\&mu;a</span>}<span\; class="notranslate">\; =</span>\left\{\begin{array}{cc}\mathrm{<span\; class="notranslate">\; A</span>}<span\; class="notranslate">\; *</span>\mathrm{<span\; class="notranslate">\; \&tau;dyn</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; B</span>}& <span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; \&tau;dyn</span>}<span\; class="notranslate">\; \&Greater\; Equal;</span>\mathrm{<span\; class="notranslate">\; \&tau;th</span>}<span\; class="notranslate">\; )</span>\\ \mathrm{<span\; class="notranslate">\; C</span>}<span\; class="notranslate">\; *</span>\mathrm{<span\; class="notranslate">\; \&tau;</span>}{\mathrm{<span\; class="notranslate">\; <figure-callout\; id="2"\; label="dyn"\; filenames="A200480009285.X00311.png,A200480009285.X00321.png"\; state="\{\{state\}\}">dyn</figure-callout></span>}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; D.</span>}& <span\; class="notranslate">\; (</span><span\; class="notranslate">\; |</span>\mathrm{<span\; class="notranslate">\; \&tau;dyn</span>}<span\; class="notranslate">\; |</span><span\; class="notranslate">\; <</span>\mathrm{<span\; class="notranslate">\; \&tau;th</span>}<span\; class="notranslate">\; )</span>\\ \mathrm{<span\; class="notranslate">\; E.</span>}<span\; class="notranslate">\; *</span>\mathrm{<span\; class="notranslate">\; \&tau;dyn</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; f</span>}& <span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; \&tau;dyn</span>}<span\; class="notranslate">\; \&le;</span><span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; \&tau;th</span>}<span\; class="notranslate">\; )</span>\end{array}\right..<span\; class="notranslate">\; .</span><span\; class="notranslate">\; .</span><span\; class="notranslate">\; .</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 16</span>}<span\; class="notranslate">\; )</span>$

In Expression (16), reference numerals A, B, C, D, E, and F are approximate constants, and τth is a set threshold.

The above parameters shown in FIGS. 6 and 7 are described in FIG. 8 .

FIG. 9 is a view showing calculation results in which the dynamic friction torque approximate value τμa is calculated according to the expression (16), and the rotations necessary for calculation and comparison are calculated and compared at the same reduction gear output as that shown in FIG. 11 . moment. It can be seen in Fig. 9 that the error (5) at constant speed does not increase, while the error (3) at peak torque is reduced compared to Fig. 11 .

Then, when expression (10) is transformed using τμa calculated by expression (16), the following expression can be obtained.

τdisa＝K _t *I _m -(J*α+D*ω+τμa*sgn(ω)+τdyn) (17)

When the collision torque τdisa is calculated by the expression (17), it becomes possible to reduce the error especially before the collision as compared with the expression (10). Therefore, it is not necessary to lower the collision detection sensitivity, and the collision detection accuracy can be enhanced.

Figure 5 is a block diagram illustrating this method.

In FIG. 5 , the motor rotation angle command value θ _ref ( 11 ) and the motor rotation angle θ _M ( 13 ) obtained from the region ( 12 ) surrounded by a dashed line showing (motor) + (actual load) are compared to are compared, and the controller (14) provides current _Im (15) to the motor. In a motor, torque is produced, which is obtained when multiplying the current I _m (15) by the torque constant K _t (16). The sum of dynamic torque τyn, collision torque τdis and friction torque τμ×sgn(ω) is subtracted from this torque (17). The torque obtained from said subtraction is used to drive a single motor body represented by box (18). The collision torque calculation section (19) calculates the collision torque detection value τdisa (20) from the motor rotation angle θ _M (13) and the current I _m (15) by the expression (17).

In this regard, although the error is increased, the motor current torque τmO=K _t ×I _m may be used in the expression (17) instead of the dynamic torque τdyn.

fifth embodiment

Next, a fifth embodiment of the present invention will be described below.

First, regarding the dynamic friction torque, Expression (16) is used. When expression (11) is transformed, the following expression (18) can be obtained.

I _ff (J*α+D*ω+τμa*sgn(ω)+τdyn)/K _t (18)

In this expression (18), when the angular velocity ω and the angular acceleration α are calculated by differentiating the motor rotation angle command θ _ref , the expression (18) can be transformed into the expression (19). Therefore, the current I _ff for generating the torque required by the motor can be calculated without using the feedback signal.

I _ff (J*s ² (θ _ref )+D*s(θ _ref )+τμa*sgn(ω)+τdyn)/K _t (19)

FIG. 10 is a block diagram showing an embodiment in the case of performing feed-forward compensation by this feed-forward current I _ff .

In FIG. 5, the motor rotation angle command value θ _ref (11) and the motor rotation angle θ _M (13) obtained from the region (12) surrounded by the dotted line showing (motor)+(actual load) are compared, and the feedback control (21) outputs the current command I _com (22) flowing in the motor. When the feed-forward current command I _ff (24) obtained by the block (23) showing expression (9) is added to the current command I _com (22) obtained by this feedback control, it is possible to realize wherein the estimation Feedback control with small errors and delays.

Seventh embodiment

Fig. 16 is a view showing a seventh embodiment of the present invention.

In FIG. 16, item (26) is a collision torque detecting part for detecting a collision torque τdisd(27) applied to the manipulator so as to drive the arm by the collision force supplied to the driving manipulator. , Item (25) is a collision judging component, which is used to judge a collision by comparing the collision torque detection value τdisd (27) with the already set collision torque threshold, and is used to output a collision detection signal D _col (30), with figures Number (24) is a motor rotation detection part (23), which is used to detect the angular velocity ω _fb of the motor from the motor rotation angle θ _fb (22), and the reference number (23) is a collision direction judgment part, which is used for comparing the collision torque The detection direction and the motor rotation direction output the collision direction sign D _ir (31), and the reference numeral (32) is a motor deceleration judging part, which is used for comparing the motor angular velocity ω _fb (1) with the threshold value that has been set, and by Confirming the deceleration of the motor to output the motor deceleration judgment signal _Dth (33), reference numeral (15) is a control mode switching part, which will be described later. In the case of an axis in which the motor rotation direction and the collision torque direction are opposite to each other, when the position control part (12) for generating a current command so that the motor rotation angle θ _fb follows the rotation angle command θ _com (11) is switched to The rotation speed of the motor is reduced when the current control part (13) is used to provide a command to generate current so that the motor can generate a torque whose direction is opposite to the rotation direction of the motor. When the rotational speed of the motor is reduced to a value not greater than the set value, the control mode switching means switches the mode to the compliance control means (14) following the direction of the collision force. In the case of an axis where the direction of motor rotation is the same as the direction of the crash torque, the control mode switching means switches the mode from the position control means (12) to the compliance control means (14).

Next, referring to FIG. 16, the stop control method performed after detection of a collision will be described in detail as follows. The current command I _com1 (2) for position control is obtained from the motor rotation angle command θ _com (11) and the actual motor rotation angle θ _fb (22) by the feedback controller (12) as a position control part. The feedback controller (12) usually consists of PID control.

In normal position control before collision detection, the current command I _com1 (2) is selected as the motor current I _m (16) by the control mode switching block (15), and is applied at (motor) + (actual load ) (17).

The motor torque τ _mm (19) and the disturbance torque (20) obtained when the motor current I _m (16) is multiplied by the torque constant K _t (18) are applied in the transfer function described using the motor inertia J ( 21) on.

Disturbance torque (20) is the sum of friction torque τμ, gravitational torque τg, dynamic torque τdyn (sum of inertial force, centrifugal force and earth deflection force) and collision torque τdis.

The motor rotation angle θ _fb (22) is output from the motor transfer function (21), and is usually detected by an optical type encoder or a magnetic type encoder.

In the collision torque detection block (26), the collision torque detection value τdisd (27) is obtained as follows. By using this motor rotation angle (22), the other axis motor rotation angle (29), angular velocity, angular acceleration and robot machine parameters that can be derived by time differentiating them without generating a collision torque τdis, it is possible to The torque required by the motor is obtained through inverse dynamic calculation. The value obtained by multiplying the actual motor current I _mm (16) by the torque constant K _t (18) is subtracted from the value thus obtained. In this way, the collision torque detection value τdisd(27) can be obtained.

When one of the collision torque detection value (28) and the collision torque detection value τdisd (27) obtained at another axis in the same manner has exceeded a predetermined collision detection threshold _τcth , the collision judgment block (25) judges that a collision. Then, a collision detection signal D _col (30) is sent to the control mode switching block (15).

In the motor angular velocity detection block (24), the motor rotation angle θ _fb (22) is differentiated to obtain the motor angular velocity ω _fb (1). The collision direction judgment block (23) calculates the collision direction flag _Dir (31) from the motor angular velocity ω _fb (1) and the collision torque detection value τdisd (27) by the following expression.

$\mathrm{<span\; class="notranslate">\; Dir</span>}<span\; class="notranslate">\; =</span>\left\{\begin{array}{cc}\mathrm{<span\; class="notranslate">\; 1</span>}& <span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; \&omega;\; fb</span>}<span\; class="notranslate">\; *</span>\mathrm{<span\; class="notranslate">\; \&tau;disd</span>}<span\; class="notranslate">\; <</span>\mathrm{<span\; class="notranslate">\; 0</span>}<span\; class="notranslate">\; )</span>\\ \mathrm{<span\; class="notranslate">\; 0</span>}& <span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; \&omega;\; fb</span>}<span\; class="notranslate">\; *</span>\mathrm{<span\; class="notranslate">\; \&tau;disd</span>}<span\; class="notranslate">\; \&Greater\; Equal;</span>\mathrm{<span\; class="notranslate">\; 0</span>}<span\; class="notranslate">\; )</span>\end{array}\right.<span\; class="notranslate">\; .</span><span\; class="notranslate">\; .</span><span\; class="notranslate">\; .</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 20</span>}<span\; class="notranslate">\; )</span>$

In Expression (20), the collision direction flag D _ir ( 31 ) becomes 1 when the direction of the motor angular velocity ω _fb ( 1 ) and the direction of the collision torque detection value τdisd ( 27 ) are opposite to each other. In other cases than this, the collision direction flag D _ir ( 31 ) becomes 0.

In the operation shown in FIG. 15 , D _ir =1 in axis UA ( 41 ), and D _ir′ =0 in axis FA ( 42 ).

When the collision detection signal D _col (30) is input, the control mode switching block (15) switches the control mode according to the information of the collision direction flag D _ir (31).

Since D _ir =1 in axis UA, the current I _com2 (3) for generating torque is generated by the current control block (13) from the angular velocity of the motor ω _fb (1) in the direction corresponding to the direction of rotation of the motor on the contrary. Then, as shown in Fig. 17, the control mode switching block (15) selects I _com2 (3) as the motor current I _m (16), that is, the mode switches to the current control mode.

According to the above configuration, since the shaft UA ( 41 ) is suddenly decelerated, the collision torque τdis ( 9 ) can be reduced after collision detection.

When the shaft UA (41) decelerates and the absolute value of the angular velocity ω _fb becomes smaller than a predetermined deceleration judgment threshold ω _th (5), the motor deceleration judgment block (32) outputs a motor deceleration judgment signal D _th (33).

D _th ＝1(|ω _fb |＜ω _th ) (22)

When this motor deceleration judgment signal D _th (33) is output, the control mode switching block (15) selects I _com3 (4) as the motor current I _m (6), and the mode switches to the compliance control mode shown in FIG. 18 .

In this regard, when the absolute value of the angular velocity ω _fb (1) is smaller than the predetermined threshold ω _th (5) and the condition of Expression (22) is satisfied at the time of collision detection, the mode is not changed from the usual control mode (shown in FIG. 16 ) to a current control mode (shown in Figure 17), but a mode switch to a compliance control mode (shown in Figure 18), but without deceleration by applying reverse torque to the motor.

In this case, compliance control can be achieved in such a way that the compliance control block (14) limits the current controlled by the current command I _com1 (2) output from the feedback controller (12) and then increases Gravity compensation current in order to prevent the robot from falling due to its own weight.

Due to the above, even when the deviation between the motor rotation angle command θ _com (11) and the motor rotation angle θ _fb (22) is increased, since the motor current is limited, the servo rigidity of the position control is lowered so that it can be Enhanced control compliance.

Regarding current limitation, it is possible to achieve current limitation by reducing the gain in the feedback controller (12).

When the motor deceleration judgment signal D _th =1 in expression (22), the motor angular velocity ω _fb (1) is lower than the threshold ω _thr , ie the motor is almost stopped and the inertial energy is low. Therefore, when the mode is shifted to the compliance mode, it is possible to solve the problem of deformation of the reduction gear generated at the time of collision.

On the other hand, in the case of the axis FA ( 42 ), the collision direction flag D _ir′ =0 in the expression (2). Therefore, when the shaft FA ( 42 ) is suddenly accelerated or decelerated in the same manner as the shaft UA ( 41 ), the collision torque τ _dis' ( 10 ) increases inversely.

Therefore, when the collision direction flag D _ir' =0 when the collision occurs, the control mode switching block (15) switches the control mode from the normal control mode (shown in FIG. 16 ) to the compliance control mode (shown in FIG. 18 ), without going through the current control mode (shown in Figure 17).

Due to the above configuration, the shaft FA ( 42 ) is operated by compliance control while following the collision force. Therefore, the collision torque can be weakened.

FIG. 14 is a timing chart showing the above-described control method in time series.

Eighth embodiment

Fig. 19 is a view showing an eighth embodiment of the present invention.

The characteristics of the eighth embodiment shown in Fig. 19 are described as follows. Referring to Fig. 16 showing the seventh embodiment, in the embodiment shown in Fig. 19, a collision torque threshold judging part (34) is provided, which combines the collision torque detection value τ _disd (27) with the collision direction The judgment torque threshold is compared. In the case of an axis where the collision torque detection value τ _disd (27) is lower than the collision direction judgment torque threshold value, regardless of the motor rotation direction and the collision torque direction, the position control part (12) is switched to the current control part ( 13) so that the motor can generate a torque whose direction is opposite to the motor rotation direction, and the motor rotation speed ω _fb (1) can be reduced. The control mode switching section (15) switches the mode to the compliance control section (14) when the motor rotation speed is reduced to a value not greater than the set value.

Referring to Fig. 19, the operation and function of the collision torque threshold judgment block (34) as an added function will be explained.

In the seventh embodiment, whether the control mode is switched to the current control mode (shown in FIG. 17 ) is determined only by the collision direction flag D _ir (31) determined by the expression (20).

However, regarding the collision torque detection value τ _disd (27) used in the condition judgment of the expression (20), the collision torque detection value τ _disd is estimated without using the torque sensor shown in the seventh embodiment In the case of (27), since the collision torque τ _dis is estimated from the information of the robot's mechanical parameters, motor position, angular velocity, angular acceleration, and current, the detection error increases compared to the case where the collision detection sensor is provided .

Therefore, in the case of an axis where the collision torque detection value τ _disd ( 27 ) is low and close to 0, there is a possibility that an error is caused in the sign of the collision torque detection value τ _disd ( 27 ) by a detection error.

In other words, in the case of an axis in which the motor speed ω _fb (1) is high, that is, in the case of an axis in which the inertial energy is high, when the collision torque detection value τ _disd (27) is low, there is The collision direction flag D _ir ( 31 ) may be output incorrectly.

When the collision torque detection value τ _disd (27) is greater than a predetermined collision judgment threshold τdth in one axis, the collision judgment block (25) judges that a collision has occurred. Therefore, in the case of an axis whose detection value is higher than the collision judgment threshold value, erroneous output of the collision direction flag D _ir ( 31 ) is not performed.

In the case of an axis whose collision torque τdis is low, except for the axis in which a collision has been detected, it is safer to rapidly decelerate the motor to reduce inertial energy because no strong external force is applied to the axis. However, as long as the collision direction cannot be judged, it is better not to actually decelerate the motor in some cases. Therefore, in the case where the absolute value of the speed at the time of collision detection is low so as not to damage the robot, the motor is not decelerated by supplying reverse torque to the motor. At high speeds, the motor is not decelerated until the rotation has come to a complete stop, but the motor should be decelerated to a speed that does not cause damage in the robot.

Therefore, as shown in FIG. 19, the collision torque threshold judgment block (34) which has been newly added outputs the collision torque threshold judgment signal D _tht (35) to the control mode switching block (15).

$\mathrm{<span\; class="notranslate">\; Dtnt</span>}<span\; class="notranslate">\; =</span>\left\{\begin{array}{cc}\mathrm{<span\; class="notranslate">\; 1</span>}& <span\; class="notranslate">\; (</span><span\; class="notranslate">\; |</span>\mathrm{<span\; class="notranslate">\; \&tau;disd</span>}<span\; class="notranslate">\; |</span><span\; class="notranslate">\; <</span>\mathrm{<span\; class="notranslate">\; \&tau;thr</span>}<span\; class="notranslate">\; )</span>\\ \mathrm{<span\; class="notranslate">\; 0</span>}& <span\; class="notranslate">\; (</span><span\; class="notranslate">\; |</span>\mathrm{<span\; class="notranslate">\; \&tau;disd</span>}<span\; class="notranslate">\; |</span><span\; class="notranslate">\; \&Greater\; Equal;</span>\mathrm{<span\; class="notranslate">\; \&tau;thr</span>}<span\; class="notranslate">\; )</span>\end{array}\right.<span\; class="notranslate">\; .</span><span\; class="notranslate">\; .</span><span\; class="notranslate">\; .</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; twenty\; three</span>}<span\; class="notranslate">\; )</span>$

τ _thr : Torque threshold for collision direction judgment

0<τ _thr ≤τ _cth (collision detection threshold)

As shown in expression (23), when the absolute value of the collision torque detection value τ _disd (27) becomes smaller than the predetermined collision direction judgment threshold τ _thr , the collision torque threshold judgment signal D _tht (35) becomes 1 .

Under the condition that the collision direction judgment torque threshold τ _thr is not greater than the collision detection judgment threshold τ _cth , the collision direction judgment torque threshold τ _thr in expression (23) can be set to be larger than the collision torque detection value τ _disd ( 27) detection error.

When the collision torque threshold judgment signal D _tht = 1, regardless of the collision direction flag D _ir (31) as the output signal of the collision speed torque direction judgment block (23), when a collision is detected, the control mode changes from the usual The control mode (shown in Figure 19) switches to the current control mode (shown in Figure 20), and the motor decelerates. Thereafter, the same processing as in the seventh embodiment is performed.

At this time, when the threshold value ω _th (5) in the expression (22) in the seventh embodiment is set at a velocity value that can not damage the robot, the absolute value of the angular velocity ω _fb (1) at the time of collision detection is smaller than the threshold ω _th (5) in order not to damage the robot. In this case, the control mode is not switched from the normal control mode (shown in Figure 16) to the current control mode (shown in Figure 17), but the control mode is switched to the compliance control mode (shown in Figure 18), but Deceleration is not performed by applying reverse torque to the motor.

Even when the angular velocity ω _fb (1) at the time of collision detection is high, if the motor is decelerated to the threshold ω _th (5) as a speed that does not damage the robot, the control mode shifts to the compliance control mode (shown in Fig. 18 ).

When the collision torque threshold judgment signal D _tht =0, the same processing as in the seventh embodiment is performed.

Ninth embodiment

In the seventh embodiment, after collision detection, in the case of an axis in which the direction of collision torque is opposite to the direction of rotation of the motor, the control is switched from position control to current control, in which a generator for making the actual position A current command following the position command, the current control commands the current so that a torque can be produced by the motor whose direction is opposite to the direction of rotation of the motor. Due to this switching operation, the motor rotation speed is reduced, and the collision energy is attenuated. Thereafter, when the motor rotation speed decreases to a value not greater than the set value, the control shifts to compliance control in which the motor rotation follows the direction of the collision force so that the damage caused by the collision in the reduction gear can be resolved. deformation problem.

However, when the speed at the time of collision detection is high, the problem of deformation of the reduction gear caused by the collision may not be sufficiently resolved only when the control is switched to the compliance control when the rotation of the motor is decelerated.

Therefore, in the case of an axis where the motor rotation direction and the collision torque direction are opposite to each other and the motor rotation speed and the collision torque detection value are respectively higher than the set value, when the current control is performed after reducing the motor rotation speed (wherein its direction The torque in the opposite direction to the motor rotation is generated by the motor), and the torque in the opposite direction is continuously applied until the direction of the speed is reversed. After solving a part of the deformation of the reduction gear caused by the collision, the speed of the reverse is increased to a value not smaller than the set value. Then, the control transitions to compliance control in which the motor rotation follows the direction of the impact force.

When the above-mentioned control method is carried out, the control system is complicated, and the reverse torque application time is increased, and there is a possibility that the manipulator rebounds greatly in the opposite direction to the collision direction. On the other hand, the deformation problem in the reduction gear can be quickly solved.

Fig. 22 is a view showing a ninth embodiment of the present invention.

The characteristics of the ninth embodiment shown in Fig. 22 are described as follows. Referring to FIG. 16 showing the seventh embodiment, the motor deceleration judgment part (32) is changed to a motor deceleration and reverse reversal judgment part (39). In addition to the collision torque threshold judging part (34) provided in the eighth embodiment, a collision speed judging part (37) is newly provided for judging the motor speed at the time of collision detection. Due to the above, in the case of an axis where the motor rotation direction and the collision torque direction are opposite to each other and it is judged that the motor rotation speed and the collision torque detection value exceed the set value, the mode transitions to where the motor is generated in the direction opposite to the motor rotation direction Torque in current control mode. Even after the motor rotational speed has been reduced, torque in the opposite direction remains applied until the speed direction is reversed. After a part of the deformation of the reduction gear caused by the collision has been resolved, when the speed of the reverse is increased to not less than the set value, the control mode is switched to compliance control in which the motor rotation follows the direction of the collision force. The ninth embodiment includes the control mode switching section (15) operating as described above.

Referring to Fig. 22, the operation and function of the collision speed judging part (37) as an increasing part and the motor deceleration and reverse judging part (39) as a changing part will be explained.

In the seventh embodiment, transition to the current control mode (shown in FIG. 17 ) is determined only by the collision direction flag D _ir (31) determined by the expression (20).

In the ninth embodiment, there is provided the collision torque threshold judgment means (34) added to the eighth embodiment. The ninth embodiment is applied only to the case where D _tht = 0 in Expression (23), that is, only to where the absolute value of the collision torque detection value τ _disd (27) is not smaller than the predetermined collision direction judgment torque The case where the threshold τ _thr and the collision direction flag D _ir =1 (the motor rotation direction and the collision torque direction are opposite to each other).

In cases other than the above cases, control is performed according to the control method of Embodiment 7 or 8.

In case the absolute value of the motor angular velocity ω _fb (1) is greater than a predetermined collision speed judgment threshold ω _ths (39), the collision speed judging part (37) outputs a collision speed judgment signal D _ths (38).

D _ths ＝1(|ω _fb |＜ω _ths ) (24)

ω _ths ≥ω _th (deceleration judgment threshold)

When the collision torque threshold judgment signal D _tht =0 and the collision direction flag D _ir =1, the motor deceleration judgment signal D _ths =1 is output. Then, the control mode switching block (15) selects I _com2 (3) as the motor current I _m (6), and the mode switches from the usual control mode (shown in FIG. 22 ) to one in which the Current control mode for motor rotation deceleration (shown in Figure 23).

The motor decelerating part (32) judges that the motor speed ω _fb (1) is reduced and reversed by applying the reverse torque. Specifically, the operation is performed as follows. In the expression (22), when the motor deceleration judgment signal D _th (33) is converted by "1 (collision)" → "0 (deceleration)" → "1 (reverse)", the control mode conversion block (15 ) selects I _com3 (4) as the motor current I _m (6), and the mode transitions to the compliance control mode shown in FIG. 24 .

FIG. 25 is a timing chart showing the above-described control method in time series.

Tenth embodiment

Fig. 21 is a view showing a tenth embodiment of the present invention.

Fig. 21 shows an arrangement in which the current control means is changed from (13) to (36) relative to Fig. 19 which shows an eighth embodiment. The tenth embodiment includes a current control part (36) for controlling a current that produces the maximum rotation of the motor in a direction opposite to the rotation direction of the motor when the current control part is selected in the control mode switching part (15). moment.

Due to the above constitution, it is possible to perform a maximum braking operation on the motor. Therefore, the collision energy can be weakened so that it can be reduced as low as possible.

Of course, the same changes can be made in FIG. 16 showing the seventh embodiment and FIG. 22 showing the ninth embodiment.

Industrial application

As described above, the present invention provides a method of controlling a robot, characterized by: detecting the rotation angle of a motor used to drive the robot; calculating an actual measured value of angular velocity from the rotation angle; When the absolute value of the angular velocity of the command value is compared with the absolute value of the angular velocity of the actually measured value, an angular velocity whose absolute value is greater than the absolute value of the other angular velocity is selected, and that angular velocity is used to calculate the friction torque; when following the command When the motor is driven by a certain value, a value corresponding to the friction torque is added to the command value supplied to the motor; and friction compensation is always effective in the following two cases: In the case of actively operating the manipulator according to the angular command, by The case where the manipulator is passively operated by an external force. Due to the above characteristics, current limitation by feedback control can be suppressed to be smaller than the friction torque. Therefore, a higher compliance control method can be realized.

When at least one of the command value and the actual measurement value is multiplied or added to a weighting coefficient when comparing the absolute value of the speed, the speed command value or the actual measurement value may preferably be employed as the speed. Therefore, for example, a speed command value whose measurement error is small can be preferentially selected.

Also, one value is used as the angular velocity obtained when at least one of the velocity command value and the actual measurement value is multiplied by the friction compensation rate. Due to the above, it is possible to prevent the feedback characteristic from becoming oscillating, while the target following characteristic can be improved.

Also, according to the robot control method of the present invention, the dynamic friction torque of the reduction gear corresponding to the loss of torque generated by the motor is calculated to increase in proportion to the dynamic torque. Due to the above, the detection accuracy of the collision torque can be enhanced.

When the dynamic torque of the reduction gear is calculated to increase in proportion to the dynamic torque, the calculation accuracy of the dynamic friction torque can be enhanced, and the most appropriate feed-forward compensation can be realized.

As described above, according to the robot control method of the present invention, after a collision is detected, when the motor rotation direction and the collision direction are opposite to each other, the control mode is switched from the position control mode to the current control mode, and the motor generates its direction and the motor rotation direction opposite torque. Due to the above reasons, the motor is slowed down and the collision energy is weakened. Thereafter, when the motor rotation speed is reduced to a value not greater than the set value, the control mode is switched to compliance control, and deformation in the reduction gear generated in the collision is resolved. On the other hand, in the case where the motor rotation direction and the collision direction are the same, the control mode is directly switched from position control to compliance control without going through current control. When operating the manipulator while following the collision force, the collision torque can be weakened. When the collision stop operation is constituted as described above, damage to the robot caused by collision can be suppressed to a minimum.

Also, in the case of an axis where the absolute value of the collision torque detection value is smaller than the set value, regardless of the motor rotation direction and the collision torque direction, the control mode is switched from position control to current control, and the motor produces its direction that is the same as the motor rotation direction. Torque in the opposite direction in order to slow down the motor rotation speed. When the rotation speed of the motor decreases to a value not greater than the set value, the control mode shifts to the compliance mode. For the above reasons, in the case of a shaft to which a high-strength torque is supplied, an appropriate stopping operation is performed in accordance with the collision direction and the motor rotation direction. The other axis, which is not supplied with a high impact torque, is quickly stopped so that the inertial energy can be reduced. Therefore, even in the case of collision torque detection in which no sensor is used and which causes a large detection error, it is possible to select an appropriate stopping method.

Also, when the current control is performed in which the motor rotation direction and the collision torque direction are opposite to each other and the motor rotation speed and the collision torque detection value exceed the set value, the motor generates a current whose direction is opposite to the motor rotation direction. With torque, after reducing the motor rotation speed, keep the torque applied in reverse until the motor rotation speed is reversed. After having resolved part of the deformation in the reduction gear caused in the collision, when the reverse speed exceeds the set value, when the control mode is switched to compliance control in which the manipulator follows the direction of the collision force, the reduction in the speed reduction can be quickly resolved Deformation in gears.

Also, in the case of current control in which the motor generates torque whose direction is opposite to the rotation direction of the motor, a command is provided so that the maximum motor torque can be generated. Due to this constitution, collision energy can be attenuated to the maximum.

Claims (13)

1. A method of controlling a manipulator, comprising: The step of detecting the rotation angle of the motor for driving the manipulator; a step of calculating an actual measured angular velocity value from said rotational angle; the step of calculating a commanded angular velocity from a command value supplied to the motor; A step of calculating the friction torque according to one of the command angular velocity and the actually measured angular velocity, wherein the absolute value of the command angular velocity and the absolute value of the actually measured angular velocity are compared, and the friction torque is calculated using the angular velocity having a higher absolute value moments; and A step of adding a value corresponding to the friction torque to a command value supplied to the motor when the motor is driven according to the command value. 2. The method for controlling a manipulator according to claim 1, further comprising the step of: multiplying at least one of said command angular velocity and an actually measured angular velocity by a weighting coefficient, or adding said weighting coefficient to said command angular velocity and an actual measured angular velocity At least one of the measured angular velocities. 3. The method for controlling a manipulator according to claim 1, further comprising the step of multiplying at least one of said command angular velocity and an actually measured angular velocity by a friction compensation rate. 4. A method of controlling a motor-driven manipulator via a reduction gear, comprising: the step of calculating the torque produced by the motor; a step of calculating a friction torque comprising at least a dynamic friction torque of the reduction gear required for the output of the reduction gear from the torque calculated by inverse dynamics; the step of increasing the dynamic torque of the reduction gear according to the frictional torque required for the output of the reduction gear; and The step of calculating the external force by subtracting the increased frictional torque from the generated torque. 5. A method of controlling a motor-driven manipulator via a reduction gear, comprising: According to the inverse dynamic calculation of the robot for obtaining the torque required for the output of the reduction gear, and also according to the calculation of the dynamic friction torque of the reduction gear, the step of compensating the motor output torque through feedback control, However, when feedback control is performed, the dynamic friction torque of the reduction gear increases in proportion to the torque required for the reduction gear output. 6. A method of controlling a robot in a stop operation performed after detection of a collision of a robot driven by a motor, in an axis in which the direction of rotation of the motor and the direction of the collision torque are opposite to each other, when the control mode changes from position control to current control When switching, the motor rotation speed is reduced, and when the motor rotation speed is reduced to a value not greater than the set value, the control mode is switched to compliance control in which the robot follows the direction of the collision force, wherein in the position control, a method for making the actual position follows the current command of the position command, in current control, controls a current through the motor that produces a torque whose direction is opposite to the direction of rotation of the motor; and In an axis in which the motor rotation direction is the same as the collision torque direction, the control mode is switched from position control to compliance control. 7. The method of controlling a robot according to claim 6, wherein, in an axis whose collision detection torque is smaller than a set value, regardless of the direction of rotation of the motor and the direction of the collision torque, the control mode is switched from position control to current control, and when the motor When a torque whose direction is opposite to the motor rotation direction is generated, the motor rotation speed is reduced, and when the motor rotation speed is reduced to a value not greater than the set value, the control mode shifts to compliance control. 8. The method of controlling a robot according to claim 6, wherein when the following current control is performed: an axis in which the motor rotation direction and the collision torque direction are opposite to each other and the motor rotation speed and the collision torque detection value exceed the set value When the motor generates a torque whose direction is opposite to the motor rotation direction, after reducing the motor rotation speed, keep applying the reverse torque until the speed direction is reversed, and when the electrode rotation speed increases to not less than the set value value, the control mode transitions to compliance control where the robot follows the direction of the collision force. 9. The method of controlling a robot according to claim 6, wherein, when the current control for generating torque by the motor in a direction opposite to the rotation direction of the motor is performed after the collision detection, the current for generating the maximum torque of the motor is controlled. 10. A device for controlling a robot, comprising: a collision torque detection part for detecting the strength and direction of the torque provided to the motor driving the robot by the collision force supplied to the robot; a collision judging component for judging a collision by comparing the detected value of the collision torque with the threshold value of the collision torque that has been set; The motor rotation detection component is used to detect the rotation speed and rotation direction of the motor; a collision direction recognition component for comparing the detection direction of the collision torque with the rotation direction of the motor; and a motor deceleration judging section for confirming deceleration of the motor by comparing the electrode rotation speed with a threshold value that has been set, wherein, in an axis in which the motor rotation direction and the collision torque direction are opposite to each other, when the control mode changes from position control to When the current control is switched, the motor rotation speed is reduced, and when the motor rotation speed is reduced to a value not greater than the set value, the control mode is switched to the compliance control where the robot follows the direction of the collision force, where in the position control, the generated To make the actual position follow the current command of the position command, in current control, control a current through the motor to produce a torque whose direction is opposite to the direction of rotation of the motor; The apparatus for controlling the robot further includes control mode switching means for switching from the position control means to the compliance control means in an axis in which the motor rotation direction is the same as the collision torque direction. 11. The apparatus for controlling a robot according to claim 10, further comprising: a collision torque threshold judging component for comparing the collision torque detection value with the collision direction identification torque threshold; and a control mode switching section for switching the control mode from the position control section to the current control section in the shaft in which the collision torque detection value is smaller than the collision direction recognition torque threshold regardless of the motor rotation direction and the collision torque direction, and For switching the control mode to the compliance control part when the motor rotation speed is reduced by generating a torque whose direction is opposite to the motor rotation direction by the motor, so as to reduce the motor rotation speed, and to reduce the motor rotation speed to not more than the set The value of the value. 12. The apparatus for controlling a robot according to claim 10, wherein when the following current control is performed: the axis in which the motor rotation direction and the collision torque direction are opposite to each other and the motor rotation speed and the collision torque detection value exceed the set value When the motor generates a torque whose direction is opposite to the rotation direction of the motor, after reducing the rotation speed of the motor, keep applying the reverse torque until the direction of the speed is reversed, and when the rotation speed of the electrode increases to not less than the set value, the control mode switches to compliance control where the robot follows the direction of the collision force. 13. The apparatus for controlling a robot according to claim 10, further comprising: a current control part for commanding a current to pass through the motor to generate its direction and motor rotation when the current control part is selected by the control mode switching part after the collision has been detected Maximum torque in opposite direction.

Images