Patent Grant
7102315
The present invention relates to a method and device of controlling a robot arm driven by a motor. More particularly, the present invention relates to a technique of compliance servo control of controlling a robot arm, that is, the present invention relates to a method and device of controlling a stoppage of a robot arm conducted after a collision of the robot arm against an object has been detected.
Recently, robots have been used for not only the industrial field but also the public consumer field. Therefore, it has become important to ensure the safety. However, according to a stoppage means for stopping a robot arm by detecting an external force, which is given to the robot at the time of collision, with a force sensor, the manufacturing cost and the weight are undesirably increased. Accordingly, it is desired to enhance the performance of compliance servo control including the collision detection, in which no sensors are used, and the control of stopping motion.
Concerning the method of realizing compliance servo control without using sensors, it is common to adopt a method in which even when a positional deviation is increased in the positional feed back control, an excessively high torque is not generated in a motor by suppressing the increase in the electric current command with respect to an increase in the positional deviation.
According to a quantity of suppression of the electric current command in the feedback control, an intensity of torque generated by the motor is also suppressed, so that the compliance can be enhanced.
A means for suppressing the electric current command in feedback control is disclosed in JP-A-09-179632 (U.S. Pat. No. 5,994,864) in which the electric current command is limited. A means for suppressing the electric current command in feedback control is also disclosed in JP-A-08-155868 in which the feedback gain is decreased.
As described above, in order to enhance the compliance of a robot arm, it is important to suppress the electric current command in feedback control. Unless the electric current is suppressed in feedback control, the compliance of a robot arm becomes close to the common servo rigidity. Therefore, the compliance of servo control is lowered.
However, in order to operate a robot arm, it is necessary to generate a drive torque by a motor in which consideration is given to the inertia torque, the friction torque and the gravity torque. Therefore, when the robot arm is operated only by feedback control, it is difficult to suppress an electric current command of the motor.
The motor generation torque τm, which is generated at the time of operation of the robot, is expressed by the expression (1) when it is seen from the motor drive side. The motor generation torque cm is expressed by the expression (2) when it is seen from the load side.
τmm=Kt*Im (1)
τml=J*α+τμ+τdyn+τdis (2)
In this case, reference marks used in the expressions (1) and (2) are defined as follows.
Kt: Motor torque constant
Im: Motor electric current
α: Motor angular acceleration
ω: Motor angular velocity
J: Motor inertia (rotor+primary side of reduction gear)
τμ: Friction torque (converted into motor shaft end)
τdyn: Dynamic torque (Dynamic torque is a sum of gravity torque, inertia force, Coriolis force and elastic force, which is converted into a motor shaft end.)
τdis: Disturbance torque (Disturbance torque is a contact torque given from the outside or a parameter error. Disturbance torque is converted into a motor shaft end.)
When the disturbance torque τdis=0 in the expression (2), it is possible to calculate the motor electric current Iml, which is necessary for the operation of the robot arm, by the expressions (1) and (2).
Iml=J*α+τμ+τdyn)/Kt (3)
As shown in
In
On the other hand, Iml (17) calculated by the expression (3) can be obtained as follows. The angular acceleration θcom (15), which is obtained when the motor rotation command θcom (1) is subjected to the differential calculation (12) and (14) twice, is multiplied by the motor inertia J (16). The friction torque τμ (20) and the dynamic torque τdyn (19) are added to the thus obtained value. The obtained value is multiplied by the inverse number 1/Kt (18) of the motor torque constant, and Iml (17) calculated by the expression (3) can be obtained.
That is, when the motor electric current Iml necessary for the operation of a robot can be accurately calculated by the expression (3), it becomes possible to suppress an electric current command in feedback control. Therefore, the compliance of the robot can be enhanced.
However, actually, an error of calculation is caused by the parameter error of the expression (3). Therefore, when the electric current is intensely suppressed in feedback control, it becomes impossible to compensate the error, and the robot arm becomes out of control and the positional deviation can not be decreased, that is, there is a possibility that the robot arm runs away.
When the electric current command is made to be 0 in the feedback control, if the positional deviation is extended by the contact torque given from the outside, a force to return the robot arm to the initial position is not generated.
As explained above, how much the electric current command by feedback control can be suppressed depends on the calculation accuracy of the expression (3).
The friction torque τμ, which is one of the primary items of the motor electric current Iml calculated by the expression (3) and necessary for the operation of the robot, includes: the stationary friction torque τμs and the dynamic friction torque τμm which are determined by the acting direction of a force; and the viscous friction torque τμd (coefficient D of viscosity) which is proportional to the velocity.
τμ=τμs+τμm+τμd (4)
However, in the expression (4), each item is calculated as follows.
As can be seen from the expressions (5) to (7), all friction torque is calculated depending upon the angular velocity ω.
Concerning the angular velocity ω used for the calculation of the friction torque, the angular velocity ωFB, which is subjected to feedback control, is used in the above conventional example (JP-A-9-179632). In another conventional example (JP-A-10-180663), the angular velocity command ωcom, which is obtained when the positional command θcom is differentiated, is used.
However, in the compliance control of the robot, there are provided two cases. One is a case in which the robot acts actively according to the positional command θcom. The other is a case in which the robot acts passively being pushed by an external force.
As shown in
However, when the robot is actively operated from a state in which the robot is completely stopped, the operating direction is unknown until the robot starts operating.
Therefore, it is impossible to calculate the stationary friction torque τμs.
Until the robot starts operating, the actual angular velocity ωFB is 0, and the dynamic friction torque τμm and the viscous friction torque τμd calculated by the expressions (6) and (7) are, of course, 0. Therefore, the friction torque τμ calculated by the 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 electric current command Icom (4) of feedback control is suppressed to enhance the compliance and becomes lower than the actual stationary friction torque τμs, the robot is not moved even when the rotation angle command θcom (1) is generated.
On the other hand, as shown in
However, when the angular velocity command ωcom (13) is used for the angular velocity ω which is used for the calculation (21) of the frictional torque τμ (20), the robot can be actively operated according to the angular velocity command ωcom (13). However, in the case where an angular velocity fluctuation is caused by the disturbance torque τdis in the middle of the operation, a big error is caused between the angular velocity command ωcom (13) and the actual angular velocity ωfb (reference numeral 23 in
When the robot is pushed by an external force at the time of stoppage of the robot when the angular velocity command ωcom (13) is 0, and the robot is stopped, the frictional torque τμ calculated by the expression (4) is 0 at all times. Therefore, the actual friction torque can not be compensated at all.
Further, even when the robot is not pushed by an external force, a delay of follow is caused in the feedback control conducted by the feedback controller in the actual operation. Therefore, at the time of stoppage of the robot, before the actual angular velocity ωfb (reference numeral 23 in
In this case, in the case where the torque generated by the motor according to the electric current command Icom (4) of the feedback control is suppressed to be lower than the actual frictional torque, even when a positional deviation is increased by an external force, the robot is not operated, and it is impossible to reduce the positional deviation.
In other words, although the frictional compensation is being conducted, it is impossible to set the feedback electric current command to be lower than the actual frictional torque. Therefore, the robot compliance can not be enhanced.
As explained above, in the case where the frictional torque τμ is calculated 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 electric current Iml, which is calculated by the expression (3) with the calculated frictional torque τμ, is added to the feedback control electric current, the actual frictional torque can not be compensated.
As shown in
On the other hand, as shown in
Next, the second conventional example will be explained below.
Concerning the method of finding a collision torque without using a sensor, the following method is commonly used. The motor generation torque is found when a loss of torque, which is generated in the motor and the reduction gear, is subtracted from the torque generated by the drive electric current of the motor. When the torque necessary for the output of the reduction gear, which is found by the dynamic calculation and referred to as a dynamic torque, is subtracted from the motor generation torque found before, the collision torque is found.
For example, the frictional torque corresponding to the loss of the torque generated by the motor is defined as a sum of the proportional item to the velocity (viscous friction torque) and the stationary item (Coulomb's frictional torque), and calculated. This is disclosed, for example, in JP-A-2002-283276.
According to JP-A-6-083403 (U.S. Pat. No. 6,298,283), the following technique is proposed. When the fluctuation of a parameter of the robot is calculated by the estimated algorithm and added to the torque (electric current) command, the fluctuation factor is canceled. In this conventional example, the frictional torque corresponding to a loss caused in the torque generated by the motor is defined as a sum of the proportional item to the velocity and the stationary item, and estimated by the estimation algorithm.
Accordingly, in the case where the collision torque is found without using a sensor when the dynamic torque of the robot is subtracted from the motor generation torque or in the case where the servo following characteristic by the feedforward control of dynamic torque is improved in order to exhibit the motor drive force at the maximum, it is required to accurately calculate the necessary torque by the motor generation torque and the reduction gear output of the robot.
The motor generation torque τm at the time of operation of the robot can be expressed by the expression (8) when it is viewed from the motor drive side. Further, the motor generation torque τm at the time of operation of the robot can be expressed by the expression (9) when it is viewed from the load side.
τmm=Kt*Im−(J*α+D*ω+τμsgn(ω)) (8)
τml=τdyn+τdis (9)
In this connection, reference marks shown in the expressions (1) and (2) are defined as follows.
Kt: Motor torque constant
Im: Motor electric current
α: Motor angular acceleration
ω: Motor angular velocity
J: Motor inertia (rotor+primary side of reduction gear)
D: Viscous friction coefficient (converted into motor shaft end)
τμ: Frictional torque (converted into motor shaft end)
τg: Gravity torque (converted into motor shaft end)
τdyn: Dynamic torque (Dynamic torque is a sum of gravity torque, inertia force, Coriolis force and elastic force, which is converted into a motor shaft end.)
τdis: Disturbance torque (Disturbance torque is a collision torque or a parameter error. Disturbance torque is converted into a motor shaft end.)
Since the motor and the robot arm are connected to each other through the reduction gear, the items in the expression (9) except for the item of the motor inertia J must be converted into the motor shaft end with a reduction ratio.
When it is assumed that τmm=ml in (8) and (9), the collision torque τdis can be found by the following deformed the expression (10).
τdis=Kt*Im−(J*α+D*ω+τμ*sgn(ω)+τdyn) (10)
In the conventional example, the dynamic friction item τμ in the expression (10) is calculated as a fixed value. However, when the dynamic friction torque item is calculated as a fixed value, in the case where the motor generation torque is high at the time of acceleration and deceleration, a big calculation error of about 10% of the motor generation torque is caused.
On the other hand, feedforward control, the object of which is to improve the servo control characteristic, can be realized as follows. The motor electric current Im is found by the expression (10) under the condition that the disturbance torque τids=0, that is, under the condition that the robot is not contacted with the outside and no parameter errors are caused. The thus found electric current is represented by Iff. When Iff is added to the electric current command, the feedforward control can be realized.
Iff (J*α+D*ω+τμ*sgn(ω)+τdyn)/Kt (11)
In the conventional example, the calculation of the expression (11) is not used but the dynamic frictional item τμ is estimated by the estimation algorithm. However, it is not a change with age but the frictional torque is greatly changed in a short period of time of acceleration and deceleration. Accordingly, by the estimation of the estimation algorithm, a delay of the phase is generated and it is impossible to completely conduct the compensation.
When the calculation is previously conducted not by the estimation algorithm but by the dynamic torque, the delay of the phase is not caused. However, when the calculation is conducted with the expression (11) while the dynamic frictional torque is set at a fixed value, in the case where the motor generation torque is high at the time of acceleration and deceleration, a big calculation error of about 10% of the motor generation torque is caused.
This error will be explained as follows.
In this case, the robot arm to be used is a six axes vertical type multiple joint robot, the portable mass of which is 6 kg and the total arm length of which is approximately 1.3 m. In
In this case, the measurement is made under the condition that the disturbance torque τdis=0, that is, under the condition that the robot is not contacted with the outside and no parameter errors are caused.
As shown in
When a comparison is made at the time of both the acceleration and the deceleration, the result is as follows.
τmm>τml
In the acceleration and deceleration in the operating section, the angular acceleration and the angular deceleration are the same although the directions are reverse to each other. With respect to the gravity, the robot arm is operated by a symmetrical pattern.
Accordingly, in order to reduce the error by decreasing τdyn, the dynamic frictional torque τμ must be increased. However, when dynamic frictional torque τμ is increased while it is kept as a constant value, as shown in
That is, when the dynamic frictional torque τμ is considered to a constant, the error factor caused by τμ can not be eliminated. Therefore, in the expressions (10) and (11) containing τμ, the same error is generated.
Therefore, in the sensorless collision torque detection, when the dynamic frictional torque τμ is considered to a constant, although the robot arm does not collide with an object at the time of acceleration or deceleration, the expression (11) outputs an electric current corresponding to the error as a collision torque. For the above reasons, in order to prevent the occurrence of an erroneous detection, the collision detecting sensitivity must be lowered.
On the other hand, in the case of feedforward control of torque necessary for the reduction gear output, when the dynamic frictional torque τμ is considered to a constant, the calculation error is increased, and there is a possibility that the feedforward compensation torque becomes insufficient. When the estimation algorithm is used so as to prevent the generation of the calculation error, it is difficult to estimate the frictional torque, which is suddenly changed at the time of acceleration or deceleration, without causing a delay of the phase. Accordingly, there is a possibility that the deterioration of the control performance can not be sufficiently prevented.
Next, explanations will be made into a method of stopping the robot arm after the collision detection. There are proposed a method in which the robot arm is returned to the position where the collision is detected (JP-A-2002-117618 (U.S. Pat. No. 6,429,617)), a method in which the velocity command is forcibly set at 0 so as to stop the robot arm (JP-A-2000-52286) and a method in which the robot arm is stopped by the maximum reverse motor torque which is reverse to the rotating direction of the motor (Japanese Patent No. 3212571 (U.S. Pat. No. 6,298,283)) and (Japanese Patent No. 2871993 (U.S. Pat. No. 5,418,440)).
According to the methods in which the robot arm is returned to the collision detecting position, the robot arm is returned to the initial position by the positional control. Therefore, the stopping time depends upon the responding property of the positional control. In general, the responding property of the positional control is several tens Hz at the highest. Therefore, the responding property is not so high at the time of stopping the collision, and the stopping time is extended and it is impossible to prevent the occurrence of damage caused by the collision.
According to the method in which the robot arm is stopped by forcibly setting the velocity command at 0, the stopping time depends upon the responding property of the velocity control. In this case, the responding property is several hundreds Hz, which is higher than the responding property in the case of the positional control. However, it is inferior to the responding property (several kHz) of the electric current control.
According to this method, since the positional control servo rigidity by the integration of the velocity control is high, the robot arm is stopped while the distortion caused by the collision is remaining. Therefore, according to JP-A-2000-052286, the positional control rigidity is softened when the velocity integration gain is made to be 0, so that the problem of the distortion, which is caused by the collision, can be solved. However, in order to enhance the compliance, it is necessary to reduce a gain proportional to the velocity, which deteriorates the velocity responding property and extends the stopping time. It is difficult to make the stopping time and the compliance compatible with each other.
According to the method in which the robot arm is stopped by the maximum reverse motor torque with respect to the rotating direction of the motor, the responding property of the electric current control for generating the reverse torque is so high that the responding property can be several kHz, that is, the responding property is excellent. However, according to the method disclosed in Japanese Patent No. 3212571, it is necessary to previously set the time at which the reverse torque is impressed. When this impressing time is short, it is impossible to sufficiently reduce the velocity, and the damage caused by the collision is increased. When this impressing time is long, a redundant motion is conducted in the reverse direction, and there is a possibility that the robot arm causes the collision again. According to the method disclosed in Japanese Patent No. 2871993, a method is proposed in which the maximum reverse torque is impressed until the motor is stopped. In this method, it is unnecessary to previously determine the impressing time to impress the reverse torque. Therefore, the aforementioned problems are solved. However, only when the motor is stopped, the problem of the distortion caused by the collision can not be solved. Since the generation of the maximum reverse torque itself is a state in which the control is generating the maximum output in an open loop, in the case where the velocity is so low that the robot can not be damaged even when it collides with an object, there is a higher risk of impressing the reverse torque.
In either system, in the axis, the colliding direction of which coincides with the rotating direction of the motor, when the robot arm is returned to the collision detecting position or suddenly stopped, an intensity of the collision force is increased.
In
After that, in order to return axis FA (42) to the collision detecting position or suddenly stop axis FA (42), it is necessary to generate torque by the motor so that the motor rotation can be reduced. However, this torque is directed reverse to the direction of the collision torque τdis′ (10). Therefore, an intensity of the collision torque is increased on the contrary.
According to the method of returning the robot arm to the collision detecting position, although the axis (axis UA in
However, in the case where the collision is detected without using a sensor, the collision torque is estimated from the pieces of information of the mechanical parameter, position, velocity, acceleration and electric current of the robot. Therefore, compared with the case in which the collision detecting sensor is provided, the detection error is increased. For the above reasons, in the case of an axis, the collision detecting torque of which is low, there is a possibility that the direction is mistakenly detected and the appropriate stopping means can not be selected.
In the case of an axis, the detected collision torque value of which is low, it is safer that the colliding detection is not detected and the motor rotating speed is reduced so as to decrease the kinetic energy. However, since the colliding direction is unknown, it is better not to reduce the motor rotating speed in some cases, that is, different from the method described in Japanese Patent No. 2871993, the reverse torque should not be impressed until the motor is stopped. In the case where the motor rotating speed is so low that the robot can not be damaged even when it collides with an obstacle, the reverse torque should not be impressed.
Further, in the case of a vertical type multiple joint robot, it is impossible to neglect an interference force given between the axes. Accordingly, there is a possibility that a velocity reducing force is given to an axis, the velocity of which should not be reduced, by an interference force given from an axis to which the reverse torque is impressed. In any case, the reverse torque should be impressed upon a necessary axis for a minimum period of time.
The present invention has been accomplished to solve the above problems. It is an object of the present invention to provide a control method, the compliance of which can be enhanced by suppressing the electric current restriction to be lower than the frictional torque by conducting feedback control in the compliance control of a robot driven by a motor.
It is another object of the present invention to provide a control method of controlling a robot capable of highly accurately detecting a collision torque by making a dynamic friction torque of a reduction gear coincide with not a fixed value but a value corresponding to the actual characteristic.
It is still another object of the present invention to provide a control method of controlling a robot capable of enhancing the motor torque calculating accuracy before operation and obtaining the most appropriate feedforward compensation, the delay of the phase of which is small, by making a dynamic friction torque of a reduction gear coincide with not a fixed value but a value corresponding to the actual characteristic.
According to the present invention, a method of controlling a robot is provided which is characterized in that: a rotary angle of a motor to drive a robot is detected; an actually measured value of an angular velocity is calculated from the rotary angle; a frictional torque is calculated by using one of the angular velocity of the command value calculated from the command value given to the motor and the angular velocity of the actually measured value, wherein the angular velocity having a higher absolute value is used in this calculation; and when the motor is driven according to the above command value, a value corresponding to the frictional torque is added to the command value given to the motor. Due to this method, an electric current restriction by feedback control can be suppressed to be lower than the frictional torque. Therefore, it is possible to realize a control method, the compliance of which is high.
When one of the command value and the actually measured value is appropriately selected as an angular velocity used for the frictional torque calculation and the frictional compensation rate is changed at the same time, the feedback characteristic can be prevented from being vibrational and further the target following characteristic can be improved.
The present invention provides a method of controlling a robot driven by a motor via a reduction gear characterized in that: when an external force is calculated by subtracting a necessary torque of the reduction gear output found by the dynamic calculation of the robot from a torque generated by the motor, a dynamic frictional torque of the reduction gear is calculated being increased corresponding to the necessary torque of the reduction gear output.
The present invention provides a method of controlling a robot driven by a motor via a reduction gear characterized in that: according to the inverse dynamic calculation of the robot to find a necessary torque of the reduction gear output and also according to the dynamic frictional torque calculation of the reduction gear, the motor output torque compensation is made by feedforward control; and when the feedback control is conducted, the dynamic frictional torque of the reduction gear is calculated being increased proportional to the necessary torque of the reduction gear output.
According to the present invention, in the case of an axis, the collision torque direction of which is reverse to the rotating direction of the motor after the detection of collision, when the robot control mode is switched from the positional control to generate an electric command for making an actual position follow a positional command to the current control to command an electric current to generate a torque, the direction of which is reverse to the motor rotating direction, by the motor, the motor rotating speed is reduced and the collision energy is alleviated. After that, when the motor velocity is decreased to a value lower than the setting value, the control mode is switched to the compliance control so that the robot arm can follow the collision force direction, and the problem of distortion caused in the reduction gear by the collision is solved. It is possible to stop and decrease the velocity by the electric current control, the responding property of which is the highest, and further when the motor velocity is monitored, the motor torque impressing time, the direction of which is reverse to the motor rotating direction, can be determined. Therefore, it is unnecessary to previously set the motor torque impressing time.
On the other hand, in the case of an axis, the direction of the collision torque of which is the same as the motor rotating direction, without conducting the electric current control, the control mode is directly switched from the positional control to the compliance control. When the axis is operated while following the collision force, the collision torque can be alleviated.
In the case of an axis, the motor rotating speed at the time of collision of which is lower than the setting value, irrespective of the motor rotating direction and the collision torque direction, the control mode is directly switched from the positional control to the compliance control without conducting the electric current control. Therefore, an open loop state of only the electric current control is not generated when it is unnecessary.
Referring to the drawings, the most preferred embodiment of the robot controlling method of the present invention will be explained as follows.
First Embodiment
On the other hand, Iml (17) can be calculated by the expression (3) as follows. The angular acceleration αcom (15), which is found when the motor rotation command θcom (1) is subjected to the differential calculations (12), (14) twice, is multiplied by the motor inertia J (16). The frictional torque τμ (20) and the dynamic torque τdyn (19) are added to the thus obtained value. The thus obtained value is multiplied by the inverse number 1/Kt (18) of the motor torque constant. In this way, Iml (17) can be calculated.
The velocity switching device (26) selects either the angular velocity command ωcom (13), which is obtained when the motor rotation angle command θcom (1) is differentiated (12), or the actual angular velocity ωfb (23), which is obtained when the actual motor rotation angle θfb (11) is differentiated (24), for the angular velocity ω (27) used when the frictional torque τμ (20) is calculated (21) by the expressions (5) to (9).
Velocity is switched by the velocity switching device (26) by the following expressions.
In the 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).
The frictional torque τμ (20) is calculated by the expressions (5) to (7) with this ω (27).
When the selection is made as described above, even when the compliance control becomes valid, the frictional torque τμ (20) can be appropriately calculated without being reduced to 0 either in the case where the positional command θcom (1) is inputted and the robot arm is actively operated or in the case where the robot arm is pushed by the external force τdis and passively operated.
Even when the robot is pushed by the external force τdis and the positional deviation is increased while the robot is being actively operated and a difference is made between the angular velocity command ωcom (13) and the actual angular velocity ωfb (23), if the absolute value of the actual angular velocity ωfb (23) becomes higher than the absolute value of the angular velocity command ωcom (13), the actual angular velocity ωfb (23) is adopted for ω (27). Therefore, the factor of causing the error can be reduced in the calculation of the frictional torque τμ (20).
Further, at the time of stopping 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 higher than the absolute value of the angular velocity command ωcom (13), ωfb (23) is adopted for ω (27). Therefore, the frictional compensation by the frictional torque τ calculated by the expression (4) can be continued from this point of time. That is, it is possible to prevent the occurrence of a sudden stoppage caused by the elimination of the frictional compensation at the point of time when the angular velocity command ωcom (13) has reached 0. Accordingly, it is possible to prevent such a problem that the robot arm can not reach a target position or vibration is generated in the robot arm.
Due to the above constitution, even when the restriction of the electric current Icom (4) by feedback control is suppressed to be lower than the actual frictional torque so as to enhance the compliance of control, it is possible to prevent the occurrence of such a problem that the robot arm can not reach a target position or the robot arm is increasingly vibrated when it is stopped.
Second Embodiment
In the expression (12) showing a switch of velocity in the first embodiment, at least one of the velocity command value and the actually measured value is multiplied by a weighting factor.
When the constitution is made as shown by the expression (13), priority is given to one of the velocity command value and the actually measured value so that it can be adopted as a velocity.
Since the actually measured value ωfb includes a measurement error, for example, when the weighting factor in the expression (13) is set at a value as follows, the velocity command ωcom can be preferentially selected.
kc1>1 and kc2>0 (14)
Third Embodiment
The actual angular velocity ωfb (23) inputted into the velocity switching device (26) is multiplied by the frictional compensation rate kμ.
It is expressed by the expression (15) as follows.
kμ: Frictional compensation rate
When the angular velocity ω (27) obtained by the above expression (15) is used, in the case where the actual angular velocity ωFB (23) is selected for the angular velocity used when the frictional torque τμ (20) is calculated (21) by the expression (5), when the frictional compensation rate kμ is set at a value not more than 1, the frictional torque τμ (20) can not be compensated by 100%. Therefore, it is possible to adjust so that the feedback characteristic can not be vibrational.
On the other hand, in the case where the angular velocity command ωcom (13) is used for the angular velocity ω which is used for the calculation (21) of the frictional torque τμ (20), the frictional torque τμ (20) can be compensated by 100% without affecting the feedback characteristic of the control system. Therefore, the target following characteristic can be improved.
Fourth Embodiment
As can seen from
In the expression (16), reference characters A, B, C, D, E and F are approximate constants, and τth is a setting threshold value.
The above parameters shown in
Then, when the expression (10) is deformed with τμa which has been calculated by the expression (16), the following expression can be obtained.
τdisa=Kt*Im−(J*α+D*ω+τμa*sgn(ω)+τdyn) (17)
When the collision torque τdisa is calculated by this expression (17), it becomes possible to reduce an error especially before the collision compared with the expression (10). Therefore, it is unnecessary to lower the collision detecting sensitivity, and the collision detecting accuracy can be enhanced.
In
In this connection, although the error is increased, the motor electric current torque τmO=Kt×Im may be used instead of the dynamic torque τdyn in the expression (17).
Fifth Embodiment
Next, the fifth embodiment of the present invention will be explained below.
First, concerning the dynamic frictional torque, the expression (16) is used. When the expression (11) is deformed, the following expression (18) can be obtained.
Iff (J*α+D*ω+τμa*sgn(ω)+σdyn)/Kt (18)
In this expression (18), when the angular velocity ω and the angular acceleration α are calculated by conducting the differentiation on the motor rotary angle command θref, the expression (18) can be deformed into the expression (19). Therefore, the electric current Iff to generate the torque necessary for the motor can be calculated without using a feedback signal.
Iff (J*s2(θref)+D*s(θref)+τμa*sgn(ω)+τdyn)/Kt (19)
In
Seventh Embodiment
In
Next, referring to
In the usual positional control conducted before the detection of collision, the electric current command Icom1 (2) is selected as the motor current Im (16) by the control mode switching block (15) and impressed upon (motor)+(actual load) (17).
The motor torque τmm (19), which is obtained when the motor electric current Im (16) is multiplied by the torque constant Kt (18), and the disturbance torque (20) are impressed upon the transfer function (21) described with the motor inertia J.
The disturbance torque (20) is a sum of the frictional torque τμ, the gravity torque τg, the dynamic torque τdyn (the sum of the inertia force, the centrifugal force and Coriolis force) and the collision torque τdis.
The motor rotary angle θfb (22) is outputted from the motor transfer function (21) and usually detected by an optical type encoder or a magnetic type encoder.
In the collision torque detecting block (26), the collision torque detecting value τdisd (27) is found as follows. By using this motor rotary angle (22), the other axis motor rotary angle (29), the angular velocity capable of being led out by conducting time differentiation on them, the angular acceleration and the robot machine parameter, under the condition that the collision torque τdis is not generated, the torque necessary for the motor is found by the inverse dynamic calculation. A value obtained when the actual motor electric current Imm (16) is multiplied by the torque constant Kt (18) is subtracted from the thus obtained value. In this way, the collision torque detecting value τdisd (27) can be found.
The collision judging block (25) judges that a collision has been caused when one of the collision torque detecting value τdisd (27) and the collision torque detecting value (28), which has been found in the other axis in the same manner, has exceeded the predetermined collision detecting threshold value τcth. Then, the collision detecting signal Dco1 (30) is sent to the control mode switching block (15).
In the motor angular velocity detecting block (24), the motor rotary angle θfb (22) is differentiated so as to find the motor angular velocity ωfb (1). The collision direction judging block (23) calculates the collision direction flag Dir (31) from the motor angular velocity ωfb (1) and the collision torque detecting value τdisd (27) by the following expression.
In the expression (20), when the direction of the motor angular velocity ωfb (1) and the direction of the collision torque detecting value τdisd (27) are opposite to each other, the collision direction flag Dir (31) becomes 1. In the other case except for that, the collision direction flag Dir (31) becomes 0.
In the operation shown in
At the point of time when the collision detecting signal Dco1 (30) is inputted, the control mode switching block (15) switches the control mode according to the information of the collision direction flag Dir (31).
Since Dir=1 in axis UA, the electric current Icom2 (3) for generating the torque, the direction of which is opposite to the motor rotating direction, is generated from the motor angular velocity ωfb (1) by the electric current control block (13). Then, as shown in
According to the above constitution, since axis UA (41) is suddenly decelerated, the collision torque τdis (9) after the collision detection can be reduced.
When axis UA (41) is decelerated and the absolute value of the angular velocity ωfb (1) becomes lower than the predetermined deceleration judging threshold value ωfb (5), the motor deceleration judging block (32) outputs the motor deceleration judging signal Dth (33).
Dth=1 (|ωfb|<ωth) (22)
When this motor deceleration judging signal Dth (33) is outputted, the control mode switching block (15) selects Icom3 (4) as the motor electric current Im (6), and the mode is shifted to the compliance control mode shown in
In this connection, in the case where the absolute value of the angular velocity of ωfb (1) is lower than the predetermined threshold value ωth (5) and the condition of the expression (22) is satisfied at the time of collision detection, the mode is not shifted from the usual control mode (shown in
In this case, the compliance control can be realized in such a manner that the compliance control block (14) makes a restriction of the electric current which is commanded by the electric current command Icom1 (2) outputted from the feedback controller (12) and then adds a gravity compensation electric current so as to prevent the robot from dropping down by its own weight.
Due to the foregoing, even when the deviation between the motor rotary angle command θcom (11) and the motor rotary angle θfb (22) is increased, since the motor electric current is restricted, the servo rigidity of the positional control is reduced so that the compliance of control can be enhanced.
Concerning the restriction of the electric current, it is possible to realize the restriction of the electric current by decreasing a gain in the feedback controller (12).
When the motor deceleration judging signal Dth=1 in the expression (22), the motor angular velocity ωfb (1) is lower than the threshold value ωthr, that is, the motor is almost stopped and the inertial energy is low. Therefore, when the mode is shifted to the compliance mode, the problem of distortion in the reduction gear generated at the time of collision can be solved.
On the other hand, in the case of axis FA (42), the collision direction flag Dir′=0 in the expression (2). Therefore, when axis FA (42) is suddenly accelerated or decelerated in the same manner as that of axis UA (41), the collision torque τdis′ (10) is increased on the contrary.
Therefore, in the case where the collision direction flag Dir′=0 at the time of the occurrence of collision, the control mode switching block (15) switches the control mode from the usual control mode (shown in
Due to the above constitution, axis FA (42) is operated by the compliance control while following the collision force. Therefore, the collision torque can be alleviated.
Eighth Embodiment
The characteristics of the eighth embodiment shown in
Referring to
In the seventh embodiment, whether or not the control ode is shifted to the electric current control mode (shown in
However, concerning the collision torque detection value τdisd (27) used in the condition judgment of the expression (20), in the case where the collision torque detection value τdisd (27) is estimated without using a torque sensor as shown in the seventh embodiment, since the collision torque τdis is estimated from pieces of information of the mechanical parameter of the robot, the motor position, the angular velocity, the angular acceleration and the electric current, the detection error is increased as compared with the case in which the collision detecting sensor is provided.
Therefore, in the case of an axis in which the collision torque detection value τdiad (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 the detection error.
In other words, in the case of an axis in which the motor velocity ω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 a possibility that the collision direction flag Dir (31) is erroneously outputted.
When the collision torque detection value τdisd (27) is higher than the predetermined collision judging threshold value τdth in an axis, the collision judging block (25) judges that a collision is caused. Therefore, in the case of an axis, the detection value of which is higher than the collision judging threshold value, no erroneous output of the collision direction flag Dir (31) is made.
In the case of an axis, the collision torque τdis of which is low, except for the axis in which the collision has been detected, since no strong external force is given to the axis, it is safer that the motor is quickly decelerated so as to decrease the inertial energy. However, as long as the collision direction can not be judged, it is better that the motor is not actually decelerated in some cases. Therefore, in the case where the absolute value of the velocity at the time of collision detection is low so that damage can not be given to the robot, the motor is not decelerated by giving an inverse torque to the motor. In the case where the velocity is high, the motor is not decelerated until the rotation is completely stopped but the motor should be decelerated to a velocity at which no damage is caused in the robot.
Therefore, as shown in
τthr: Collision direction judging torque threshold value
0<τthr≦τcth (Collision detection judging threshold value)
As shown by the expression (23), the collision torque threshold value judging signal Dtht (35) becomes 1 when the absolute value of the collision torque detection value τdiad (27) becomes lower than the predetermined collision direction judging torque threshold value τthr.
The collision direction judging torque threshold value τthr in the expression (23) may be set higher than the detection error of the collision torque detection value τdiad (27) under the condition that the collision direction judging torque threshold value τthr is not more than the collision detection judging threshold value τcth.
When the collision torque threshold value judging signal Dtht=1, irrespective of the collision direction flag Dir (31), which is an output signal of the collision velocity-torque direction judging block (23), when the collision is detected, the control mode is shifted from the usual control mode (shown in
At this time, when the threshold value ωth (5) in the expression (22) in the seventh embodiment is set at a value of the velocity so that the robot can not be damaged, the absolute value of the angular velocity ωfb (1) at the time of collision detection is lower than the threshold value ωth (5) so that the robot can not be damaged. In this case, the control mode is not shifted from the usual control mode (shown in
Even when the angular velocity ωfb (1) at the time of collision detection is high, if the motor is decelerated to the threshold value ωth (5), which is a velocity at which the robot is not damaged, the control mode is shifted to the compliance control mode (shown in
When the collision torque threshold value judging signal Dtht=0, the same processing as that of the seventh embodiment is conducted.
Ninth Embodiment
In the seventh embodiment, after the collision detection, in the case of an axis in which the collision torque direction is reverse to the motor rotating direction, the control is switched from the positional control, in which an electric command for making the actual position follow the positional command is generated, to the electric current control which commands an electric current so that the torque, the direction of which is reverse to the motor rotating direction, can be generated by the motor. Due to this switching operation, the motor rotating speed is reduced and the collision energy is alleviated. After that, when the motor rotating speed is decreased to a value not more than the setting value, the control is switched to the compliance control in which th motor rotation follows the direction of the collision force so that the problem of distortion in the reduction gear caused by the collision can be solved.
However, in the case where the velocity at the time of collision detection is high, only when the control is switched to the compliance control at the time when the motor rotation is decelerated, there is a possibility that the problem of distortion in the reduction gear caused by the collision can not be sufficiently solved.
Therefore, in the case of an axis in which the motor rotating direction and the collision torque direction are reverse to each other and the motor rotating speed and the collision torque detecting value are respectively higher than the setting values, when the electric current control is conducted in which the torque, the direction of which is reverse to the direction of motor rotation, is generated by the motor, after the motor rotating speed is decreased, the torque in the reverse direction is continuously impressed until the velocity direction is inverted. After a portion of the distortion in the reduction gear generated by the collision is solved, the inverted velocity is increased to a value not less than the setting value. Then, the control is switched to the compliance control in which the motor rotation follows the collision force direction.
When the above control method is executed, the control system is complicated and the reverse torque impressing time is increased, and further there is a possibility that the robot arm greatly rebounds in the opposite direction to the collision direction. On the other hand, the problem of distortion in the reduction gear can be quickly solved.
The characteristics of the ninth embodiment shown in
Referring to
In the seventh embodiment, the shift to the electric current control mode (shown in
In the ninth embodiment, the collision torque threshold value judging means (34), which is added to the eighth embodiment, is provided. The ninth embodiment is applied to only a case in which Dtht=0 in the expression (23), that is, only a case in which the absolute value of the collision torque detection value τdisd (27) is not less than the predetermined collision direction judging torque threshold value τthr and the collision direction flag Dir=1 (The motor rotating direction and the collision torque direction are opposite to each other.).
In the case except for the above case, control is conducted according to the control method of Embodiments 7 or 8.
In the case where the absolute value of the motor angular velocity ωfb (1) is higher than the predetermined collision velocity judging threshold value ωths (39), the collision velocity judging means (37) outputs the collision velocity judging signal Dths (38).
Dths=1 (|ωfb|<ωths) (24)
ωths≧ωth (Deceleration judging threshold value)
When the collision torque threshold value judging signal Dtht=0 and the collision direction flag Dir=1, this motor deceleration judging signal Dths=1 is outputted. Then, the control mode switching block (15) selects Icom2 (3) as the motor electric current Im (6), and the mode is shifted from the usual control mode (shown in
The motor deceleration means (32) judges that the motor velocity ωfb (1) is reduced and inverted by this impression of the reverse torque. Specifically, operation is conducted as follows. In the expression (22), when the motor deceleration judging signal Dth (33) is shifted by “1 (collision)”→“0 (deceleration)”→→“1 (inversion)”, the control mode switching block (15) selects Icom3 (4) as the motor electric current Im (6), and the mode is shifted to the compliance control mode shown in
Tenth Embodiment
Due to the above constitution, it is possible to conduct the maximum braking operation on the motor. Therefore, the collision energy can be alleviated so that it can be reduced as low as possible.
Of course, the same change can be made in
As described above, the present invention provides a method of controlling a robot characterized in that: a rotary angle of a motor for driving the robot is detected; an actually measured value of the angular velocity is calculated from the rotary angle; when the absolute value of the angular velocity of the command value calculated from the command given to the motor and the absolute value of the angular velocity of the actually measured value are compared with each other, one angular velocity, the absolute value of which is higher than the absolute value of the other angular velocity, is selected and a frictional torque is calculated with that angular velocity; when the motor is driven according to the command value, a value corresponding to the frictional torque is added to the command value given to the motor; and the frictional compensation is validly made at all times in both the case in which the robot arm is actively operated according to the angular command and the case in which the robot arm is passively operated being pushed by an external force. Due to the above characteristic, an electric current restriction by feedback control can be suppressed to be lower than the frictional torque. Therefore, a control method of higher compliance can be realized.
When at least one of the command value and the actually measured value is multiplied or added by a weighting factor at the time of the comparison of the absolute value of the velocity, either the velocity command value or the actually measured value can be preferentially adopted as a velocity. Therefore, for example, a velocity command value, the measurement error of which is small, can be preferentially selected.
Further, a value is used as an angular velocity which is obtained when at least one of the velocity command value and the actually measured value is multiplied by the friction compensation rate. Due to the foregoing, it is possible to prevent the feedback characteristic from becoming vibrational and at the same time the target following characteristic can be improved.
Further, according to the robot control method of the present invention, a dynamic frictional torque of the reduction gear, which corresponds to a loss of the torque generated by the motor, is calculated being increased proportional to the dynamic torque. Due to the foregoing, the accuracy of detecting the collision torque can be enhanced.
When the dynamic torque of the reduction gear is calculated being increased proportional to the dynamic torque, the calculation accuracy of the dynamic frictional torque can be enhanced and the most appropriate feedforward compensation can be realized.
As described above, according to the robot control method of the present invention, after the collision has been detected, when the motor rotating direction and the collision direction are opposite to each other, the control mode is switched from the positional control mode to the electric current control mode, and the motor generates the torque, the direction of which is reverse to the motor rotating direction. Due to the foregoing, the motor is decelerated and the collision energy is alleviated. After that, when the motor rotating speed is lowered to a value not more than the setting value, the control mode is switched to the compliance control and the distortion in the reduction gear generated in the collision is solved. On the other hand, in the case where the motor rotating direction and the collision direction are the same, the control mode is directly switched from the positional control to the compliance control without passing through the electric current control. When the robot arm is operated while following the collision force, the collision torque can be alleviated. When the collision stopping operation is composed as described above, damage of the robot caused by the collision can be suppressed to the minimum.
Further, in the case of an axis in which the absolute value of the collision torque detecting value is lower than the setting value, irrespective of the motor rotating direction and the collision torque direction, the control mode is switched from the positional control to the electric current control, and the torque, the direction of which is reverse to the direction of the motor rotation, is generated by the motor so as to reduce the motor rotating speed. When the motor rotating speed is decreased to a value not more than the setting value, the control mode is switched to the compliance mode. Due to the foregoing, in the case of an axis to which a high intensity of torque is given, an appropriate stopping operation is conducted according to the collision direction and the motor rotating direction. The other axis to which a high intensity of collision torque is not given is quickly stopped, so that the inertial energy can be decreased. Therefore, even in the case of collision torque detection in which a sensor is not used and a large detection error is caused, it is possible to select an appropriate stopping means.
Further, when the electric current control in which the motor generates the torque, the direction of which is reverse to the direction of motor rotation, is conducted in an axis in which the motor rotating direction and the collision torque direction are reverse to each other and the motor rotating speed and the collision torque detecting value exceed the setting values, after the motor rotating speed is reduced, the torque in the reverse direction is kept being impressed until the motor rotating speed direction is inverted. After a portion of the distortion in the reduction gear caused in the collision has been solved, when the inverted speed exceeds the setting value, when the control mode is switched to the compliance control in which the robot arm follows the collision force direction, the distortion in the reduction gear can be quickly solved.
Further, in the case of electric current control in which the motor generates torque, the direction of which is reverse to the direction of motor rotation, a command is given so that the maximum motor torque can be generated. Due to this constitution, the collision energy can be alleviated at the maximum.
| Number | Date | Country | Kind |
|---|---|---|---|
| 2003-281619 | Jul 2003 | JP | national |
| 2004-091461 | Mar 2004 | JP | national |
| 2004-098949 | Mar 2004 | JP | national |
| Filing Document | Filing Date | Country | Kind | 371c Date |
|---|---|---|---|---|
| PCT/JP2004/009781 | 7/2/2004 | WO | 00 | 8/24/2005 |
| Publishing Document | Publishing Date | Country | Kind |
|---|---|---|---|
| WO2005/009692 | 2/3/2005 | WO | A |
| Number | Name | Date | Kind |
|---|---|---|---|
| 4500823 | Walrath | Feb 1985 | A |
| 4925312 | Onaga et al. | May 1990 | A |
| 5134354 | Yamamoto et al. | Jul 1992 | A |
| 5418440 | Sakaguchi et al. | May 1995 | A |
| 5767648 | Morel et al. | Jun 1998 | A |
| 5994864 | Inoue et al. | Nov 1999 | A |
| 6298283 | Kato et al. | Oct 2001 | B1 |
| 6429617 | Sano et al. | Aug 2002 | B1 |
| 6899705 | Niemeyer | May 2005 | B1 |
| 6974449 | Niemeyer | Dec 2005 | B1 |
| Number | Date | Country |
|---|---|---|
| 60-167780 | Aug 1985 | JP |
| 6-83403 | Mar 1994 | JP |
| 8-155868 | Jun 1996 | JP |
| 9-179632 | Jul 1997 | JP |
| 10-180663 | Jul 1998 | JP |
| 2871993 | Jan 1999 | JP |
| 11-143548 | May 1999 | JP |
| 2000-10635 | Jan 2000 | JP |
| 2000-52286 | Feb 2000 | JP |
| 2001-105358 | Apr 2001 | JP |
| 3212571 | Jul 2001 | JP |
| 2002-117618 | Apr 2002 | JP |
| 2002-283276 | Oct 2002 | JP |
| Number | Date | Country | |
|---|---|---|---|
| 20060071625 A1 | Apr 2006 | US |
