JP2011235423A - Force control device - Google Patents

Abstract

PROBLEM TO BE SOLVED: To shorten the operating time, and to shorten the startup adjusting time in a range to satisfy the constraint in which the acting force is equal to or smaller than the permissible value even when a robot is stopped when the acting force exceeds the constraint value.SOLUTION: A force control device includes a command generation means (10) for generating the target position command of a robot for an object, a robot control means (20) for performing the follow-up control according to the target position command, a force limit excess discriminating means (30) for generating the stop command when the acting force on the robot from an object exceeds the predetermined force limit value, and a speed optimization means (40) for calculating the optimum speed for maximizing the operating speed in a range for satisfying the constraint that the acting force is equal to or smaller than the predetermined permissible value during the deceleration stop based on the characteristic of the object and the predetermined force limit value taking into consideration the case that the robot is subjected to the deceleration stop. The command generation means (10) generates the target position command according to the optimum speed, and generates the command for executing the deceleration stop if the stop command is received.

Description

  The present invention relates to a force control device applied to a robot that performs assembly, deburring, polishing, substrate mounting, inspection, and the like, an automatic assembly apparatus, a mounting machine, a processing machine, and an inspection apparatus.

  In a general force control device, a force sensor is provided at the tip of a robot that takes out a component from a magazine that stores the component, moves the component to a predetermined position, and performs component assembly work. The robot controller performs compliance control by feedback of the force detected by the force sensor, compares the detected force with a preset force threshold, and determines the next robot operation and determines whether the work is successful or not based on the comparison result. Make a decision.

  For example, when performing a series of operations such as gripping a round bar-shaped part with a hand, lowering it, and inserting it, and the force detected during insertion (descent) exceeds a threshold value, the robot controller Is determined to have failed, and the descent operation is stopped. As a result, generation of excessive force can be prevented. Then, after the hand stops, the work is performed again or an abnormal alarm is generated.

Japanese Patent Laid-Open No. 7-24665

However, the prior art has the following problems.
When the insertion operation is detected and stopped, the robot moves down from the detected point until it actually stops. For this reason, it is conceivable that a force greater than the threshold value used for detection acts on the robot and the object during the descent. The force that acts when the insertion failure is detected and stopped (in the following description, such “acting force” may be simply expressed as acting force) is the allowable value of the robot and the object. In order to satisfy the following condition, it is necessary to lower the speed below the speed at which it can be ensured that the acting force at the time of stoppage is less than the allowable value.

  However, the conventional force control device does not have a function of calculating a speed at which the acting force at the time of stopping is equal to or less than an allowable value. For this reason, it is necessary to take measures such as lowering the descending speed more than necessary, or obtaining in advance trial and error a speed at which the acting force at the time of stopping is less than an allowable value. However, in the former case, there is a problem that the operation time becomes long because the speed is low. In the latter case, there is a problem that time is required for the pre-adjustment.

  The present invention has been made in order to solve the above-described problems. When the force limit is exceeded due to an insertion failure or the like, even when the robot is decelerated and stopped in the middle of the operation, the acting force is less than the allowable value. A force control device capable of shortening the operation time within a range satisfying the constraints and shortening the adjustment time for realizing the shortening of the operation time within a range satisfying the constraints where the acting force is less than an allowable value is obtained. For the purpose.

  A force control device according to the present invention includes: a command generation unit that generates a target position command for a robot with respect to an object; a robot control unit that performs tracking control of the robot in accordance with the target position command; Generated by the command generation means when the acting force acting on the robot exceeds a predetermined force limit value defined as a value smaller than a predetermined allowable value corresponding to the upper limit value of the acting force determined by the object or robot. Considering the case where the force limit excess discriminating means for generating a stop command for decelerating and stopping the robot by the target position command and the case of decelerating and stopping the robot by generating the stop command by the force limit excess discriminating means Based on the characteristics of the object and the predetermined force limit value, the operating speed is maximized within a range that satisfies the constraint that the applied force is below the predetermined allowable value during deceleration stop. A speed optimization unit that calculates an optimum speed, and the command generation unit generates a target position command corresponding to the optimum speed calculated by the speed optimization unit and a stop command generated by the force limit excess determination unit Is received, a command to decelerate and stop the robot is generated.

  According to the force control device of the present invention, when the acting force acting on the robot from the object exceeds a predetermined force limit, the acting force during the deceleration stop is an allowable value when it is necessary to decelerate and stop the robot. By calculating the maximum speed in the range that satisfies the following constraints and generating the position command based on the calculated maximum speed and operating the robot, the robot operates within the range that satisfies the constraint that the applied force is less than the allowable value. It is possible to obtain a force control device that can shorten the time and shorten the startup adjustment time in advance for shortening the robot operation time.

It is a block diagram which shows the structure of the force control apparatus in Embodiment 1 of this invention. It is an internal block diagram of the robot control means in Embodiment 1 of this invention. It is an example of the block diagram of each axis position control means in Embodiment 1 of this invention. It is an internal block diagram of the force sense consideration correction | amendment means in Embodiment 1 of this invention. It is a block diagram which shows the structure of the force control apparatus in Embodiment 2 of this invention. It is a block diagram which shows the structure of the force control apparatus in Embodiment 3 of this invention. It is a block diagram which shows the structure of the force control apparatus in Embodiment 4 of this invention. It is a block diagram which shows the structure of the force control apparatus in Embodiment 5 of this invention.

  Hereinafter, preferred embodiments of a force control device of the present invention will be described with reference to the drawings.

Embodiment 1 FIG.
FIG. 1 is a block diagram showing the configuration of the force control device according to Embodiment 1 of the present invention. The force control apparatus according to the first embodiment includes a command generation unit 10, a robot control unit 20, a force limit excess determination unit 30, and a speed optimization unit 40, and performs follow-up control of the robot 1 that is a control target. ing. The robot 1 includes a single-axis or multi-axis motor, and examples thereof include a 6-axis vertical articulated robot called an industrial robot or a 4-axis horizontal articulated robot.

  The command generation means 10 gives the robot 1 a target position every moment. The robot control unit 20 performs follow-up control on the robot 1 in accordance with the position command given from the command generation unit 10.

  A force sensor 2 for measuring the acting force and moment is attached to the vicinity of the tip of the robot 1 or an object on which the robot 1 works. The force and moment measured by the force sensor 2 are input to the robot control unit 20 as a force detection value and also input to the force limit excess determination unit 30.

  The force detection value is composed of a force in three axes and a moment around each axis when the six-axis force sensor 2 is used, and when a force sensor 2 having five or less axes is used. Of the forces in the three axis directions and the moments around each axis, it consists only of data that can be measured.

  In addition, a position sensor 3 for measuring the motor position of each axis such as an encoder or a resolver is attached to each axis motor that drives the robot 1, and the motor position of each axis measured by the position sensor 3 is as follows. Is input to the robot control means 20.

  The force limit excess determining means 30 compares a predetermined force threshold stored in advance for each axis with the force detection value transmitted from the force sensor 2, and at least one axial force or moment exceeds the force threshold. If it is, a stop command is transmitted to the command generation means 10. In the first embodiment, the determination of exceeding the force limit is performed by comparing each of the detected force and moment components with a predetermined force threshold. However, for the combined force and moment, May be implemented.

  The speed optimization means 40 is set in advance in the force limit value (force threshold) input from the force limit excess determination means 30, the compliance control parameter input from the robot control means 20, and the speed optimization means 40. The optimum speed is obtained based on the allowable force value and the parameter relating to the rigidity of the object (the spring constant of the object), and is output to the command generation means 10.

  Here, the force allowance value is a value larger than the force limit value, and is the upper limit value of the acting force that must not be exceeded, which is determined by the lower of the object restrictions or robot restrictions, and exceeds this value. It corresponds to a value for controlling the robot so that there is no. On the other hand, the force limit value is a limit value that is set to satisfy the constraint that is less than or equal to the force limit value. When the force limit value is exceeded, the robot is decelerated and stopped to prevent the applied force from exceeding the force limit value. To do.

  The “optimum speed” output from the speed optimization means 40 means that the force and moment acting on the object and the robot 1 when the force limit excess determination means 30 determines that the force limit has been exceeded and stops, Means an upper limit speed that is equal to or less than the force allowable value set in the internalizing means 40.

  The command generation means 10 sets the optimum speed input from the speed optimization means 40 as the maximum speed, and generates a position command for each command generation period based on the acceleration / deceleration parameters stored in the command generation means 10. To the robot control means 20. In addition, when receiving a stop command from the force limit excess determination unit 30, the command generation unit 10 generates a command for decelerating and stopping from the current position / speed and transmits the command to the robot control unit 20.

  Next, the function of the robot control means 20 will be described in detail. FIG. 2 is an internal configuration diagram of the robot control means 20 according to the first embodiment of the present invention. The robot control unit 20 in FIG. 2 includes a force sense consideration correction unit 21, an inverse conversion unit 22, and each axis position control unit 23.

  The haptic consideration correcting unit 21 generates a position command correction amount based on the position command generated by the command generating unit 10. Details of the generation of the position command correction amount will be described later. Then, the position command is corrected by adding the position command correction amount which is the output of the force sense consideration correction means 21.

  Next, the inverse conversion means 22 performs an inverse conversion calculation for calculating the motor position of each axis that realizes the hand position / posture with respect to the added value of the position command and the position command correction amount, and the position of each axis of the robot. It is converted into a joint position command which is a command.

  The converted joint position command is input to each axis position control means 23. Each axis position control means 23 performs control so that the motor position of each axis follows the input joint position command. FIG. 3 is an example of a configuration diagram of each axis position control means 23 in Embodiment 1 of the present invention.

  In each axis position control means 23 shown in FIG. 3, each axis (1 axis to n axis) is constituted by a proportional calculation unit 23a, a differential calculation unit 23b, and a proportional integration calculation unit 23c. The number in parentheses after the symbol corresponds to the axis number.

  The proportional calculation unit 23a performs proportional calculation on the difference between the position command of each axis and the motor position of the axis. On the other hand, the differential calculation unit 23b calculates the motor speed by differentiating the motor position. Then, the proportional-plus-integral calculation unit 23c performs a proportional-integral calculation on a value obtained by subtracting the output of the differential calculation unit 23b from the output of the proportional calculation unit 23a.

  The control system has a configuration in which a speed proportional integration control system is provided inside the position proportional control system, and is often used as a motor position control system. In the example shown in FIG. 3, each axis position control means 23 is configured as a feedback control system that performs motor position control and speed control for each axis. However, the control system of the present invention is not limited to such a configuration. A control system including an observer for each axis may be used, or a control system including feedforward may be used. In addition, the configuration is not limited to independent of each axis, and may be a configuration in which interference torque between the axes is corrected by feedforward or feedback.

  Next, the internal configuration of the force sense consideration correction unit 21 will be described in detail. FIG. 4 is an internal configuration diagram of the haptic consideration correction unit 21 according to Embodiment 1 of the present invention. As shown in FIG. 4, the haptic consideration correction unit 21 according to the first embodiment includes a tool coordinate system coordinate conversion unit 211, a stiffness matrix unit 212, a differential calculation unit 213, a damping matrix unit 214, a proportional gain calculation unit 215, An orthogonal coordinate system coordinate conversion unit 216, an integral calculation unit 217, a tool coordinate system coordinate conversion unit 218, and a forward conversion unit 219 are provided.

  First, the forward conversion unit 219 performs forward conversion for calculating the hand position / posture at that time from each axis motor position, and calculates the hand position / posture. The tool coordinate system coordinate conversion unit 211 receives the difference between the position command (hand position / posture command) generated by the command generation unit 10 and the hand position / posture calculated by the forward conversion unit 219 as an input to the tool coordinate system. Perform coordinate transformation of. Here, the “tool coordinate system” means a coordinate system fixed to a tool or a hand attached to the tip of the robot.

  Next, the stiffness matrix unit 212 receives a vector configured from the output of the tool coordinate system coordinate conversion unit 211, obtains a vector product with the stiffness matrix, and outputs the product.

  On the other hand, the differential calculation unit 213 calculates the differential of each element of the output of the tool coordinate system coordinate conversion unit 211. Then, the damping matrix unit 214 calculates a vector product of the vector composed of the differentiation result calculated by the differentiation calculation unit 213 and the damping matrix, and outputs the product. Further, the output of the stiffness matrix unit 212 and the output of the damping matrix unit 214 are added.

  Next, the tool coordinate system coordinate conversion unit 218 converts the force detection value detected by the force sensor 2 into a tool coordinate system. The force detection value converted by the tool coordinate system coordinate conversion unit 218 is subtracted from the addition result of the output of the stiffness matrix unit 212 and the output of the damping matrix unit 214 and is input to the proportional gain calculation unit 215. .

  The proportional gain calculation unit 215 multiplies each element of the input subtraction result by a proportional gain. Further, the orthogonal coordinate system coordinate conversion unit 216 converts the multiplication result of the proportional gain into the orthogonal coordinate system. Further, the integration calculation unit 217 performs an integration process on the conversion result obtained by the orthogonal coordinate system coordinate conversion unit 216 to calculate and output a position command correction amount (position command correction amount). Through such a series of processing, the force sense consideration correction unit 21 can generate the position command correction amount based on the position command generated by the command generation unit 10.

  Next, the internal configuration of the speed optimization unit 40 will be described in detail. In the first embodiment, the case where the force limit value (force threshold value) and the force allowable value are set for each axis will be described as an example. However, the combination of the force and moment in each axis direction is considered. It doesn't matter. In the following, the Z-axis direction will be described as a representative, but the same applies to the case of considering forces in the X-axis direction and the Y-axis direction.

  The force threshold (force limit value) in the Z-axis direction is Fs, the allowable force value in the Z-axis direction is Fp, the rigidity of the object in the Z direction is K1, and the compliance control parameter in the Z direction is K2. Here, Fs is a force threshold value input from the force limit excess determining means 30. When the acting force exceeds Fs, a stop command is input from the force limit excess determining means 30 to the command generating means 10. The robot 1 decelerates and stops.

Further, the rigidity K1 and the force allowable value Fp of the object are stored in advance in the speed optimization means 40. The speed optimizing means 40 corresponds to the force threshold Fs input from the force limit excess determining means 30 and the compliance control parameter K2 input from the robot control means 20 (corresponding to the Z axis of the stiffness matrix) immediately before the operation of each axis. The optimum speed Vs is calculated by the following equation (1) from the pre-stored allowable force Fp of the object, the rigidity K1 of the object, and the deceleration time gt.
Vs = sqrt ((Fp−Fs) × (K1 + K2) × 2 × gt / K1 / K2) (1)

  That is, the speed optimization means 40 satisfies the constraint that the acting force is not more than a predetermined allowable value during the deceleration stop based on the characteristics of the object and the predetermined force limit value using the above equation (1). In the range, the optimum speed Vs that maximizes the operating speed is calculated.

  Then, the speed optimization unit 40 outputs the calculated optimum speed Vs to the command generation unit 10. In the above formula (1), sqrt means a square root. The command generation unit 10 generates a position command for each command generation cycle so that the maximum speed becomes Vs, and outputs the position command to the robot control unit 20.

  By performing such control, when an excessive force is applied to the robot compared to the normal case due to misalignment (corresponding to the occurrence of an error), even when the robot is decelerated and stopped, The robot can be decelerated and stopped at the maximum operating speed as long as it can be ensured that the force acting on the robot does not exceed a predetermined allowable value. As a result, it is possible to minimize the operation time under the constraint of ensuring an appropriate stop operation at the time of displacement. Furthermore, even when the occurrence of an error is detected and the vehicle is decelerated and stopped, there is no need to obtain the maximum speed at which the acting force is below the allowable value by trial and error, and the startup adjustment time can be shortened.

  As described above, according to the first embodiment, it is determined that the force limit has been exceeded immediately before the operation, and the maximum speed in a range satisfying the constraint that the acting force when stopped is less than the allowable value is calculated. The position command is generated based on the calculated maximum speed, and the robot is operated. As a result, when it becomes necessary to decelerate and stop the robot when an error exceeding the force limit occurs, it is possible to shorten the operation time while keeping the acting force below an allowable value.

  Furthermore, since the operation speed can be automatically set based on the above equation (1), it is not necessary to optimize the operation speed by trial and error, and the startup time can be shortened.

Embodiment 2. FIG.
FIG. 5 is a block diagram showing a configuration of the force control device according to Embodiment 2 of the present invention. Compared with the configuration of FIG. 1 in the first embodiment, the configuration of FIG. 5 in the second embodiment further includes the object characteristic identification means 50 and the internal configuration of the speed optimization means 40 is different. . Therefore, the following description will be made focusing on the inside of the speed optimization means 40 and the object characteristic identification means 50.

  In the first embodiment, it is necessary to previously store the object characteristics (object rigidity) in the speed optimization unit 40. On the other hand, in the second embodiment, the object characteristic identification means 50 is newly provided to identify the object rigidity from the operation data at the time of start-up adjustment, and the identified value is stored. . On the other hand, the speed optimization means 40 reads the object rigidity stored in the object characteristic identification means 50 and uses it for calculation when calculating the optimum speed.

  As a specific identification method by the object characteristic identification means 50, the following can be considered. For example, the characteristic of the object is assumed to be a linear spring. In this case, the object characteristic identification means 50 uses time series data of the moving distance and the pushing force from the contact start position, or two or more moving distances and the pushing force from the action data when the pushing action is performed as the identifying action. From the combination of the data, the spring constant can be identified using linear identification theory.

  Therefore, the object characteristic identification unit 50 according to the second embodiment identifies in advance the rigidity K1 of the object from the time series data of the moving distance from the contact start position and the pushing force in the identification operation performed for identification. And remember. Then, the speed optimization unit 40 can calculate the optimum speed based on the identification result obtained by the object characteristic identification unit 50. As a result, the speed optimizing means 40 can be configured so as not to store the object characteristics in advance.

  As described above, according to the second embodiment, it is determined that the force limit has been exceeded immediately before the operation, and the maximum speed in the range satisfying the constraint that the acting force when stopped is less than the allowable value is calculated. The position command is generated based on the calculated maximum speed, and the robot is operated. As a result, when it becomes necessary to decelerate and stop the robot when an error that exceeds the force limit occurs, the operating time can be shortened while keeping the acting force below an allowable value.

  Furthermore, since the operation speed can be automatically set based on the above equation (1), it is not necessary to optimize the operation speed by trial and error, and the startup time can be shortened. Further, by providing the object characteristic identification means, even if the characteristic of the object is unknown, the object characteristic can be identified by executing one identification operation, and the startup adjustment time including the identification time can be adjusted. Shortening is possible.

Embodiment 3 FIG.
FIG. 6 is a block diagram showing the configuration of the force control device according to Embodiment 3 of the present invention. The speed optimizing means 40 in the previous first and second embodiments calculates the optimum speed Vs by the above equation (1) using the compliance control parameter K2. On the other hand, the speed optimization means 40 in the third embodiment calculates the optimum speed Vs by the following equation (2) without using the compliance control parameter K2.
Vs = sqrt ((Fp−Fs) × 2 × gt / K1) (2)

  That is, the speed optimization means 40 satisfies the constraint that the acting force is not more than a predetermined allowable value during the deceleration stop based on the characteristics of the object and the predetermined force limit value using the above equation (2). In the range, the optimum speed Vs that maximizes the operating speed is calculated.

  Further, the speed optimization unit 40 outputs the calculated optimum speed Vs to the command generation unit 10. Other processes are the same as those in the first embodiment. In the configuration of FIG. 5 provided with the object characteristic identification unit 50 described in the second embodiment, the speed optimization unit 40 can calculate the optimum speed Vs using the above equation (2). it can.

  As described above, according to the third embodiment, it is determined that the force limit has been exceeded immediately before the operation, and the maximum speed in the range satisfying the constraint that the acting force when stopped is less than the allowable value is calculated. The position command is generated based on the calculated maximum speed, and the robot is operated. As a result, when it becomes necessary to decelerate and stop the robot when an error exceeding the force limit occurs, it is possible to shorten the operation time while keeping the acting force below an allowable value.

  Furthermore, since the operation speed can be automatically set based on the above equation (2) without using the compliance control parameter K2, it is not necessary to optimize the operation speed by trial and error, and the start-up time can be shortened. .

Embodiment 4 FIG.
FIG. 7 is a block diagram showing the configuration of the force control device according to Embodiment 4 of the present invention. Compared with the configuration of FIG. 1 in the first embodiment, the configuration of FIG. 7 in the fourth embodiment is further provided with a force estimation observer 60, and the force sensor 2 is not used. .

  That is, in the fourth embodiment, the force and moment detected values are not obtained from the force sensor 2, but the force and moment acting on the robot tip are estimated and estimated using the force estimation observer 60. Force estimation value is used instead of force detection value.

  The force estimation observer 60 inputs the motor position of each axis and the current of the motor that drives each axis, and estimates the estimated force value. More specifically, the force estimation observer 60 calculates motor speed and motor acceleration from the motor position of each axis, calculates drive torque required for the operation from the calculated motor position, speed, and acceleration, and calculates motor current. Is calculated as the disturbance torque of each axis.

  Further, the force estimation observer 60 converts the disturbance torque into the force and moment acting on the robot tip using the Jacobian matrix calculated based on the motor position of each axis, and the converted force and moment are estimated. Output as a value.

  As described above, according to the fourth embodiment, the same effect as in the first and second embodiments can be obtained without using a force sensor, and the force sensor is not required. System cost can be reduced.

Embodiment 5 FIG.
FIG. 8 is a block diagram showing a configuration of the force control device according to Embodiment 5 of the present invention. The speed optimization means 40 in the previous fourth embodiment calculates the optimum speed Vs by the above equation (1) using the compliance control parameter K2 as in the first and second embodiments. On the other hand, the speed optimization unit 40 according to the fifth embodiment calculates the optimum speed Vs by the above equation (2) without using the compliance control parameter K2 as in the third embodiment. Other processes are the same as those in the fourth embodiment.

  As described above, according to the fifth embodiment, the same effect as in the fourth embodiment can be obtained without using a force sensor, and the system cost by eliminating the need for a force sensor. Can be reduced. Further, as in the third embodiment, since the operation speed can be automatically set based on the above equation (2) without using the compliance control parameter K2, it is not necessary to optimize the operation speed by trial and error. Shortening the startup time can be realized.

  It should be noted that the position control can be performed without using the force estimation value estimated by the force estimation observer 60 in the previous fourth and fifth embodiments inside the robot control means.

  DESCRIPTION OF SYMBOLS 1 Robot, 2 Force sensor, 3 Position sensor, 10 Command generation means, 20 Robot control means, 21 Force sense consideration correction means, 22 Inverse conversion means, 23 Each axis position control means, 23a Proportional calculation part, 23b Differential calculation part , 23c proportional integral calculation unit, 30 force limit excess determination unit, 40 speed optimization unit, 50 object characteristic identification unit, 60 force estimation observer, 211 tool coordinate system coordinate conversion unit, 212 stiffness matrix unit, 213 differentiation calculation unit, 214 Damping matrix unit, 215 Proportional gain calculation unit, 216 Cartesian coordinate system coordinate conversion unit, 217 Integration calculation unit, 218 Tool coordinate system coordinate conversion unit, 219 Forward conversion unit.

Claims (3)

Command generating means for generating a target position command of the robot with respect to the object;
Robot control means for performing follow-up control of the robot in response to the target position command;
A predetermined force limit value that is defined as a value that is smaller than a predetermined allowable value corresponding to an upper limit value of the applied force determined by the target object or the robot, from the target object or the robot acting force following the tracking operation Force limit excess determining means for generating a stop command for decelerating and stopping the robot according to the target position command generated by the command generating means,
In consideration of a case where the robot is decelerated and stopped by generating the stop command by the force limit excess determining means, based on the characteristics of the object and the predetermined force limit value, A speed optimizing unit that calculates an optimum speed that maximizes the operating speed within a range that satisfies a constraint that an acting force is equal to or less than the predetermined allowable value; and
The command generation means generates a target position command corresponding to the optimum speed calculated by the speed optimization means, and when receiving the stop instruction generated by the force limit excess determination means, A force control device that generates a command to decelerate and stop a robot. The force control device according to claim 1,
Based on time-series data obtained by the robot performing an identification operation, further comprising an object characteristic identification means for identifying the characteristic of the object,
The robot control means acquires the action force and position information of each axis motor of the robot in accordance with the identification operation performed in advance to identify the characteristics of the object, and the robot applies the object to the object. Based on the movement distance from the start of contact and time-series data of the acting force corresponding to the movement distance, the characteristics of the object are identified,
The force control device characterized in that the speed optimization means calculates the optimum speed by using the characteristic of the object identified by the object characteristic identification means. The force control device according to claim 1 or 2,
A force estimation observer for estimating the acting force based on the current value and position information of each axis motor for driving the robot;
The force limit excess determining means generates the stop command when the acting force estimated by the force estimation observer exceeds the predetermined force limit value.

Images