JP5473889B2 - Force control device - Google Patents

Description

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

  For example, in Patent Document 1, the difference between the torque command for driving each axis and the estimated value of the driving torque for each axis is calculated as an external torque, the calculated disturbance torque of each axis, the robot work coordinate system, and the joint coordinate system. A technique related to a robot control device that calculates an external force acting on a robot tip position from the Jacobian matrix and performs force control using the calculated external force is disclosed. Specifically, the robot control device stores a compliance model at the tip position, and corrects a position command for driving the robot according to an external force using the compliance model.

JP 2010-105138 A

  According to the above conventional technique, the robot controller calculates the friction torque using the friction coefficient stored in advance when calculating the estimated value of the driving torque of each axis, and the inertial force torque, gravity torque, friction The drive torque estimated value is calculated by adding the force torque. However, since the coefficient of friction of each axis varies depending on conditions such as temperature, it varies even in the season or in the morning and evening, and also varies even if the temperature of each axis rises by continuously operating the robot. Therefore, the actual friction force torque of each axis when calculating the estimated value of the drive torque is different from the friction force torque of each axis when the friction coefficient is calculated, and the estimation accuracy of the drive torque is deteriorated. There is a problem that the accuracy of the estimated value of the external force acting on the robot tip calculated based on the difference between the command and the driving torque is also deteriorated.

  The present invention has been made in view of the above, and obtains a force control device capable of calculating an external force acting on a distal end portion of a robot with high accuracy while realizing cost reduction of a force sensor. With the goal.

In order to solve the above-described problems and achieve the object, the present invention relates to a robot driven by a motor and a robot that generates a current command for driving the motor so that the tip of the robot follows the command position. A control means, a force sensor for detecting an external force applied to each of n (m> n ≧ 1) movement directions among m movement directions of the tip of the robot, and a detection value of the force sensor And identifying the friction coefficient of the robot necessary for calculating the estimated value of the external force applied to the mn operation directions based on the position of the motor and the current command, and identifying the identified friction coefficient. And a force estimation observer that calculates an estimated value of the external force applied to the mn motion directions based on the mn motion directions.

  According to the present invention, even if a 3-axis force sensor, which is less expensive than a 6-axis force sensor, for example, a moment that cannot be measured by the 3-axis force sensor can be estimated, the cost of the force sensor can be reduced. However, the external force acting on the tip of the robot can be calculated with high accuracy.

FIG. 1 is a block diagram illustrating a configuration of the force control device according to the first embodiment. FIG. 2 is a block diagram showing the configuration of the robot control means. FIG. 3 is a block diagram showing a configuration example of each axis position control means. FIG. 4 is a block diagram showing the configuration of the force sense consideration correction means. FIG. 5 is a block diagram showing a configuration of the force estimation observer. FIG. 6 is a block diagram illustrating a configuration of the force control device according to the fifth embodiment. FIG. 7 is a block diagram illustrating a detailed configuration of the robot control unit according to the fifth embodiment.

  Embodiments of a force control device according to the present invention will be described below in detail with reference to the drawings. Note that the present invention is not limited to the embodiments.

Embodiment 1 FIG.
FIG. 1 is a block diagram showing the configuration of the force control device according to the first embodiment of the present invention. As shown in the figure, the force control apparatus according to the first embodiment includes a command generation unit 1, a robot control unit 2, a robot 3, and a force estimation observer 4.

  The command generation means 1 gives the robot 3 a position command as a target position every moment. The robot control means 2 controls the robot 3 so that the tip (end effector) of the robot 3 follows the given position command. The robot 3 is a robot that operates under the control of the robot control means 2, and includes a motor that drives one or more axes, for example, a 6-axis vertical articulated robot called an industrial robot, a 4-axis horizontal Applicable to articulated robots.

  A triaxial force sensor 31 for measuring the force acting on the tip is attached to the vicinity of the tip of the robot 3 (here, the wrist of the robot 3). The obtained force is input to the force estimation observer 4 as a force measurement value. Here, in the first embodiment, it is assumed that the triaxial force sensor 31 is of a type that measures forces in the XYZ directions in the sensor coordinate system and does not measure moments around the axes. A position sensor 32 for measuring the motor position of each axis, such as an encoder or resolver, is attached to the motor for each axis that drives the robot 3, and the position of the motor applied to each axis measured by the position sensor 32 is determined by robot control means. 2 is input.

  In addition to the force measurement value measured by the triaxial force sensor 31, the force estimation observer 4 also receives the motor position of each axis detected by the position sensor 32 and the motor current command of each axis calculated by the robot control means 2. Is done. The force estimation observer 4 estimates moments around the XYZ axes that cannot be measured by the force sensor, and outputs them to the robot control means 2 together with force measurement values in the XYZ directions.

  FIG. 2 is a block diagram showing the configuration of the robot control means 2. As shown in the figure, the robot control means 2 includes an adder 6, force sense consideration correction means 7, inverse conversion means 8, and axis position control means 9. The haptic consideration correction means 7 outputs a position command correction amount for correcting the position command based on the motor position and the force estimation value. A detailed configuration of the force sense consideration correction unit 7 will be described later. The adder 6 adds the position command generated by the command generation unit 1 and the position command correction amount output by the force sense consideration correction unit 7. The inverse conversion means 8 performs an inverse conversion calculation for calculating the motor position of each axis that realizes the hand position / posture with respect to the position command corrected by the addition of the position command correction amount. It is converted into a joint position command which is a position command. The converted joint position command is input to each axis position control means 9. Each axis position control means 9 outputs a current command to the robot 3 so that the motor position of each axis follows the input joint position command.

  FIG. 3 is a block diagram showing a configuration example of each axis position control means 9. As shown in the figure, each axis position control means 9 includes a subtractor 91, a proportional means 92, a differentiating means 93, a subtractor 94, and a proportional integration means 95 for each axis. Here, the number of axes is assumed to be n. The subtractor 91 calculates the difference between the position command for each axis and the motor position for that axis, and the calculation result is input to the proportional means 92. The proportional means 92 multiplies the input value by a constant. Differentiating means 93 differentiates the motor position to calculate the motor speed. The subtracter 94 obtains a difference between the output value of the proportional means 92 and the motor speed calculated by the differentiating means 93. The proportional integration means 95 performs a proportional integration operation on the output value of the subtractor 94 to obtain a current command. Each of the shaft position control means 9 has a configuration in which a speed proportional integration control system is provided in the position proportional control system, and is often used as a motor position control system. In the first embodiment, each axis position control means 9 has a configuration of a feedback control system that performs motor position control and speed control for each axis, but control including an observer for each axis. It may have a system configuration, or may have a control system configuration including feedforward. 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.

  FIG. 4 is a block diagram showing the configuration of the force sense consideration correction means 7. As shown in the figure, the force sense correction unit 7 includes a subtractor 71, a first coordinate conversion unit 72, a stiffness matrix unit 73, an adder 74, a subtractor 75, a proportional gain filter 76, a second coordinate conversion unit 77, an integration. Means 78, differentiation means 79, damping matrix unit 80, third coordinate conversion means 81, and forward conversion means 82 are provided.

  The forward conversion means 82 performs forward conversion on each axis motor position, and calculates the hand position / posture at that time. The subtracter 71 calculates a difference between the position command generated by the command generation means 1 and the calculated hand position / posture. The first coordinate conversion means 72 converts the difference value output from the subtractor 71 into a value in the tool coordinate system. The tool coordinate system is a coordinate system fixed to a tool or hand attached to the tip of the robot.

  The stiffness matrix unit 73 obtains the product of the vector of the output value of the first coordinate conversion means 72 and the stiffness matrix, and outputs the obtained product. Differentiating means 79 calculates the differentiation of each element of the output of the first coordinate conversion means 72. The damping matrix unit 80 calculates and outputs the product of the vector composed of each element of the output of the differentiating means 79 and the damping matrix. The adder 74 adds the output value of the stiffness matrix unit 73 and the output value of the damping matrix unit 80.

  The third coordinate conversion means 81 converts the force estimation value output from the force estimation observer 4 into a value in the tool coordinate system. The subtracter 75 calculates and outputs the difference between the output value of the adder 74 and the output value of the third coordinate conversion means 81. The proportional gain filter 76 multiplies the output value of the subtractor 75 by a predetermined proportional gain. The second coordinate conversion unit 77 converts the output value of the proportional gain filter 76 into a rectangular coordinate system. The integrating means 78 integrates the output value of the second coordinate converting means 77 and outputs the obtained value as a position command correction amount (position command correction amount).

  FIG. 5 is a block diagram showing a configuration of the force estimation observer 4. The force estimation observer 4 includes external force calculation means 41, estimated torque calculation means 42, and friction coefficient estimation means 43.

  The friction coefficient estimating means 43 obtains the friction coefficient by system identification using the current command i of each axis as an input value, the motor position q and the force measurement value of each axis as input values, and a parameter for obtaining the friction coefficient. Hereinafter, the arithmetic processing by the friction coefficient estimating means 43 will be specifically described.

The friction coefficient estimator 43 receives a current command i for each axis, a motor position q for each axis, and a hand external force f as a force measurement value measured by the triaxial force sensor 31. When the robot 3 is a 6-axis vertical articulated robot, the current command i and the motor position q are 6-element vectors, and the external force f is a measurement value of the 3-axis force sensor 31, and thus is a 3-element vector. The friction coefficient estimating means 43 first calculates the torque τ _i (i = 1 to 6) of each axis. Torque of each axis τ _{i (i} = _1~6), a torque constant _kt i for each axis (i = 1 to 6), the reduction ratio gen _i of each axis, when the current command of each axis and _{i i} (i (Subscript) = 1-6),
τ _i = kt _i * i _i / gen _i (1)
Is calculated by

Next, the friction coefficient estimating means 43 obtains the link position ql _i of each axis from the motor position q _i of each axis by the following equation.
ql _i = q _i / gen _i (2)

Further, the friction coefficient estimating means 43 calculates the link speed v _i and the link acceleration a _i by dividing the difference from the previous period value by the sampling time. The vectors composed of the link position, velocity, and acceleration of each axis are ql, v, a, respectively, the inertia matrix is M (ql), the centrifugal Coriolis force is h (ql, v), and the gravity is g (ql), The friction coefficient estimating means 43 obtains an estimated value τr of the drive torque excluding the friction force by the following equation.
τr = M (ql) a + h (ql, v) + g (ql) (3)

At this time, when the frictional force is fr (v) and the force sensor coordinate system is the hand coordinate system, the relationship between the hand speed vx and the joint speed v is expressed using the Jacobian matrix J (q), vx = J (q ) V (4)
If expressed by, a vector Fall composed of external force and moment in each direction in the hand coordinate system is
Fall = (J (q) ^T ) ⁻¹ τd (5)
It is expressed. Here, τd is a vector in which the external force of each axis is arranged,
τd = τ−τr−fr (v) (6)
It is. From equations (5) and (6)
Fall = (J (q) ^T ) ⁻¹ (τ−τr−fr (v)) (7)
Can be derived.

  The first three elements of Fall on the left side of Expression (7) are force measurement values measured by the triaxial force sensor 31. In addition, the first three elements on the right side of Expression (7) indicate force estimation values including a friction coefficient to be calculated. The friction coefficient estimating means 43 calculates the friction coefficient using the correspondence relationship between the right side and the left side of the first three elements in the equation (7).

Specifically, when the coulomb friction coefficient of each axis is fci, the viscous friction coefficient is fvi, and sgn (vi) is a function that takes 1 when the speed vi is positive, −1 when negative, and 0 when 0. The frictional force of each axis is represented by fci * sgn (vi) + fvi * vi. The vector obtained by taking out the first three elements of (J (q) ^T ) ⁻¹ (τ−τr) −Fall is taken as fa, and the first three elements of (J (q) ^T ) ⁻¹ (fr (v)) are taken out. If the vector is fb, each element of fb is constructed by multiplying fci and fvi by a function of velocity v and position q. Therefore, fci, fvi, fc1, fv2, fv2, If the vector of 12 elements aligned with ... is p,
fa = y (q, v) p (8)
Call it. Here, y is a 3 × 12 matrix. Assuming that the calculation result of the kth control cycle is represented by [k], the friction coefficient estimating means 43 is:
R [k] = R [k−1] + st * (− k1 * R [k−1] + y [k] ^T y [k]) (9)
r [k] = r [k−1] + st * (− k1 * r [k−1] + fa [k] y [k] ^T ) (10)
R [k] and r [k] are calculated by
p [k] = p [k-1] -st * k2 * (R [k] p [k-1] -r [k]) (11)
To calculate p having a coefficient of friction applied to each axis as a constituent element. Note that st, k1, and k2 are preset system identification parameters.

  By the above calculation, the friction coefficient estimating means 43 can obtain the friction coefficient at that time point based on the current command i of each axis that changes every moment, the motor position q of each axis, and the force measurement value. That is, the friction coefficient estimation means 43 can obtain the friction coefficient even if the friction coefficient changes with temperature or the like.

The estimated torque calculation means 42 calculates the friction torque fr (v) using the friction coefficient estimated by the friction coefficient estimation means 43,
τd = τ−τr−fr (v) (12)
Thus, the estimated value τd of the disturbance torque of each axis is calculated.

The external force calculation means 41 uses the disturbance torque τd calculated by the estimated torque calculation means 42 and the Jacobian matrix J (q),
F = (J (q) ^T ) ⁻¹ τd (13)
Is calculated. Among these, since the first three elements can be measured directly, the measurement values of the three-axis force sensor 31 are replaced and output, and the remaining three elements output the calculation result of the expression (13) as it is.

  In the above description, the friction coefficient is calculated from the relationship of equation (8) using equations (9) to (11), but the least square method or the like is applied to the relationship of equation (8). You may calculate by. In the first embodiment, the motor current command is used as the motor current. However, motor current feedback may be used. Further, although the motor position feedback is used as the motor position, a motor position command finally given to the motor or a motor model position calculated from the motor position command may be used.

As described above, according to the first embodiment of the present invention, the robot 3 driven by the motor and the current command for driving the motor are generated so that the tip of the robot 3 follows the command position. A force sensor 31 that detects an external force applied to each of three movement directions of the robot control means 2 and the six movement directions of the tip of the robot 3 in the x-axis direction, the y-axis direction, and the z-axis direction. Based on the detection value of the force sensor 31, the position of the motor, and the current command, at least the force sensor 31 of the six movement directions of the tip of the robot is not detecting an external force. And a force estimation observer 4 that identifies the friction coefficients in the rotation directions of the three axes and calculates an estimated value of the moments in the rotation directions of the three axes based on the identified friction coefficients. Since it is configured, even if the triaxial force sensor 31 which is less expensive than the six axis force sensor 31 is used, a moment that cannot be measured by the triaxial force sensor 31 can be estimated. i, since it is possible to estimate a moment that cannot be measured using the friction coefficient obtained based on the motor position q and the force measurement value of each axis, it is higher than when the moment is estimated using a preset friction coefficient. The moment can be estimated with accuracy. That is, the external force acting on the tip of the robot can be calculated with high accuracy while realizing cost reduction of the force sensor.

  The robot control unit 2 corrects the position command based on the estimated value of the moment and the motor position, and generates the current command so that the tip of the robot follows the corrected position command. Thus, the force control can be performed with high accuracy.

Embodiment 2. FIG.
The configuration of the force control device of the second embodiment is the same as that of the first embodiment, except that the force sensor attached to the wrist of the robot 3 is different from the friction coefficient calculation method by the friction coefficient estimating means. Only the configuration of the second embodiment which is different from that of the first embodiment will be described.

The force sensor attached to the robot wrist of the force control device according to the second embodiment is not a sensor capable of measuring forces in the XYZ3 directions (3-axis force sensor 31), but only forces in one axis direction in the Z-axis direction. Can be measured. According to the force control device of the second embodiment, only the third element of the fall of the equation (5) can be measured. Therefore, in the first embodiment, the friction coefficient estimating means 43 is (J (q) ^T ) −1 ( The coefficient of friction was calculated with fa as the vector from which the first three elements of τ-τr) -Fall were extracted, and fb as the vector from which the first three elements of (J (q) ^T ) -1 (fr (v)) were extracted. However, in the second embodiment, the third element of (J (q) ^T ) ⁻¹ (τ−τr) −Fall is fa, and (J (q) ^T ) ⁻¹ (fr (v)) is fb. The coefficient of friction is calculated using only the third element.

  In the description of the second embodiment, a force sensor of a type capable of measuring a force only in the Z-axis direction is employed, but the measurable direction may be the X-axis direction or the Y-axis direction. The external force that can be measured by the force sensor may be any one of moments about the X, Y, and Z axes.

  As described above, according to the second embodiment of the present invention, an inexpensive force sensor having only one measurable direction can be used.

Embodiment 3 FIG.
The configuration of the force control device of the third embodiment is the same as that of the first embodiment except that the calculation method for calculating the friction coefficient by the friction coefficient estimating means is different. Only the configuration of the third embodiment different from that of the first embodiment will be described.

According to the force control device of the third embodiment, the friction coefficient estimation means does not identify the friction coefficient of each axis as an unknown parameter, but calculates the friction force of each axis using the initial value of the friction coefficient. Force fv0 (vi) = fc0i * sgn (vi) + fv0i * vi is calculated as a product of coefficient p1 that is commonly multiplied, that is, fv = p1 * fv0 (14)
P1 is identified as an unknown parameter. Specifically, p1 is calculated from fa = fb, and the friction coefficients are output as p1 * fc0i and p1 * fv0i.

  Thus, according to the third embodiment of the present invention, it is possible to finally use the friction coefficient without using the friction coefficient itself as an unknown parameter as in the first embodiment.

Embodiment 4 FIG.
The configuration of the force control device of the fourth embodiment is the same as that of the first embodiment, except that the force sensor and the calculation method for calculating the friction coefficient by the friction coefficient estimating means are different. Only the configuration of the fourth embodiment that is different from that of the first embodiment will be described.

The force sensor attached to the robot wrist of the force control device according to the fourth embodiment is not a sensor that can measure forces in the XYZ3 directions, but only three components of forces in the X and Y directions and moments around the Z axis. It can be measured. Since only the first, second, and sixth elements of Fall in equation (5) can be measured, the friction coefficient estimating means is the first 3 of (J (q) ^T ) ⁻¹ (τ−τr) −Fall in the first embodiment. The coefficient of friction was calculated using the vector from which the element was extracted as fa and the vector from which the first three elements of (J (q) ^T ) ⁻¹ (fr (v)) were extracted as fb. q) The first, second and sixth elements of ^T ) ^-1 (τ-τr) -Fal are fa, and (J (q) ^T ) ^-1 (fr (v)) is fb, and the first, second and sixth elements The coefficient of friction is calculated using only

  In the above description, the force sensor can measure only the three components of the force in the X direction and the Y direction and the moment around the Z axis direction. However, the third embodiment of the present invention is not limited to the X axis direction. , Y axis direction, Z axis direction, rotation direction with X axis as rotation axis, rotation direction with Y axis as rotation axis, and any three components among rotation directions with Z axis as rotation axis can be measured It can be applied to a force control device employing a force sensor.

  Thus, according to the fourth embodiment of the present invention, the X-axis direction, the Y-axis direction, the Z-axis direction, the rotation direction with the X-axis as the rotation axis, the rotation direction with the Y-axis as the rotation axis, and the Z-axis Even a force control device that employs a force sensor that can measure only three arbitrary components in the rotation direction with the rotation axis as the rotation axis can accurately calculate the external force.

Embodiment 5 FIG.
FIG. 6 is a block diagram illustrating a configuration of the force control device according to the fifth embodiment. As shown in the figure, the force control apparatus according to the fifth embodiment includes command generation means 1, robot control means 10, robot 3, force estimation observer 4, and force information monitor 11. In the figure, the same components as those in the first embodiment are denoted by the same reference numerals. That is, the fifth embodiment is different from the first embodiment in that the output of the force estimation observer 4 is used only for monitoring purposes and is not used for control.

  For example, the force information monitor 11 monitors whether the force and moment in each direction are within an allowable range, and generates an error signal when the allowable range is exceeded. Further, the force information monitor 11 may function as a display device that simply logs a force estimate value or displays and outputs a force estimate value.

  FIG. 7 is a block diagram illustrating a detailed configuration of the robot control unit 10 according to the fifth embodiment. As shown in the figure, in the fifth embodiment, the force control device does not perform compliance control or the like, but only monitors force information (force estimation value). Therefore, the internal configuration of the robot control unit 10 is the same as a generally known configuration for position control that does not include the force sense consideration correction unit 7.

  In this way, the force estimation value may be used not for control but for state monitoring.

  As described above, the force control device according to the present invention is suitable for application to a force control device that controls a robot.

DESCRIPTION OF SYMBOLS 1 Command production | generation means 2 Robot control means 3 Robot 4 Force estimation observer 6 Adder 7 Force sense consideration correction means 8 Inverse conversion means 9 Each axis position control means 10 Robot control means 11 Force information monitor 31 3-axis force sensor 32 Position sensor 41 External force calculation means 42 Estimated torque calculation means 43 Friction coefficient estimation means 71 Subtractor 72 Coordinate conversion means 73 Stiffness matrix part 74 Adder 75 Subtractor 76 Proportional gain filter 77 Coordinate conversion means 78 Integration means 79 Differentiation means 80 Damping matrix part 81 Coordinate conversion means 82 Forward conversion means 91 Subtractor 92 Proportional means 93 Differentiation means 94 Subtractor 95 Proportional integration means

Claims (4)

A robot driven by a motor;
Robot control means for generating a current command for driving the motor so that the tip of the robot follows the command position;
A force sensor for detecting an external force applied to each of n (m> n ≧ 1) motion directions among m motion directions of the tip of the robot;
Based on the detection value of the force sensor, the position of the motor, and the current command, at least the force sensor out of m motion directions of the tip of the robot mn detects no external force. A force estimation observer for identifying each of the friction coefficients necessary for calculating the external force in each of the motion directions, and calculating an estimated value of the external force applied to the mn motion directions based on the identified friction coefficients;
A force control device comprising: The robot control means corrects the position command based on the estimated value of the external force and the motor position, and generates the current command so that the tip of the robot follows the corrected position command.
The force control apparatus according to claim 1.   The force control apparatus according to claim 1, further comprising a force information monitor that monitors the estimated value of the external force. The m is 6, and the m operation directions are an X-axis direction, a Y-axis direction, a Z-axis direction, a rotation direction with the X axis as a rotation axis, a rotation direction with the Y axis as a rotation axis, and Z The direction of rotation with the axis as the rotation axis.
The force control device according to any one of claims 1 to 3, wherein

Images