JPH07205075A - Weight compensation method of end effector at force control robot - Google Patents

Abstract

PURPOSE:To compensate accurately the weight element of an end effector by seeking the weight vector and gravity center vectors of the end effector, seeking outer force added to the end effector from the weight of the end effector and force added to a force sense sensor. CONSTITUTION:At the time of the work start of a force control robot 1, respective gravity center vectors of an end effector 3 fitted to the tip portion of the force control robot 1 through a inner force sense sensor 2, at a robot coordinate system and a sensor coordinate system are first sought. Next, at the time of the force control of the force control robot 1, force added to the inner force sense sensor 2 is sought from the output voltage of the inner force sense sensor 2. In addition, force added to the inner force sense sensor 2 due to the weight of the end effector 3 obtained from the gravity center vector Sh and weight vector Rw of the end effector 3 is subtracted from the force added to the force sense sensor 2. As a result, outer force added to the end effector 3 is sought, and the weight element of the end effector 3 is accurately compensated.

Description

Detailed Description of the Invention

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention compensates for the weight of the end effector when measuring the external force applied to the end effector attached to the tip of a force control robot (hereinafter referred to as "force control robot"). Regarding the method.

[0002]

2. Description of the Related Art In order to perform force control by a force control robot, an external force (in three directions x, y, z) applied to an end effector such as a grinder, a gripper, a hand or a sealing cutter attached to the tip of the robot. It is necessary to know exactly the force for each, and the six components consisting of the moment). Therefore,
It is common practice to attach, for example, a strain gauge type 6-axis force sensor between the end effector and the tip of the robot, and measure the external force applied to the end effector from the output voltage of the force sensor. ing. At this time, in order to accurately measure the external force applied to the end effector, the force applied to the force sensor due to the weight of the end effector with respect to the external force obtained from the output voltage of the force sensor is measured. Need to compensate.

As a method of compensating for the weight of the end effector, there is, for example, the method described in Japanese Patent Laid-Open No. 3-184790. In this method, the force measured by the force sensor when an external force is not actually applied to the end effector in a certain posture is written in the memory as an offset value.
Then, when measuring the external force, the offset value is read from the memory, and when the external force is actually applied to the end effector, the offset value read by the force sensor is subtracted from the value of the external force measured by the force sensor to subtract the offset value of the end effector. I try to compensate for the weight.

[0004]

The offset value used in the above method is only a valid value for the posture of the force control robot when the offset value is obtained. However, in general, if the posture of the force control robot changes, the offset value also changes accordingly.
Therefore, in the method described in Japanese Patent Laid-Open No. 3-184790, in the case of a scalar type robot in which the offset value does not change with the change of the posture of the force control robot,
Alternatively, the method is effective only when the posture of the force control robot is not changed in the vertical articulated robot.
That is, the above method cannot be applied to the vertical articulated robot when the posture of the force control robot is changed and used.

Further, the above method has a drawback that it cannot be applied to a mobile robot when the ground is tilted.

In view of the above problems, the present invention accurately determines the weight component of the end effector even when the posture of the force control robot is changed or the force control robot itself is tilted in the vertical articulated robot. The purpose is to be able to compensate.

[0007]

In order to achieve the above object, a weight compensation method for an end effector in a force control robot according to the present invention is an end effector attached to a tip portion of a force control robot via a force sensor. The weight vector in the robot coordinate system and the center of gravity vector in the sensor coordinate system are obtained at the start of the work of the force control robot, and when the force control of the force control robot is performed, the output voltage of the force sensor changes to the force sensor. While determining the applied force, the force applied to the force sensor due to the weight of the end effector obtained from the weight vector and the center of gravity vector is subtracted from the force applied to the force sensor, The external force applied to the end effector is calculated.

Further, when no force is applied to the end effector and no load is applied, the first force control robot of the force control robot is operated.
In this posture, the force applied to the force sensor due to the weight of the end effector is ignored to measure the provisional offset voltage of the force sensor, and in the second posture of the force control robot. , The virtual external force in the second posture is calculated from the output voltage of the force sensor by ignoring the force applied to the force sensor due to the weight of the end effector. A force applied to the force sensor due to the weight of the end effector in the first posture, and a force applied to the force sensor due to the weight of the end effector in the second posture. The weight vector and the center of gravity vector may be obtained from the relationship that holds between

Further, the posture of the force control robot is set to N (N
≧ 2) times, the virtual external force in each posture is obtained, and the virtual external force in each posture and the force applied to the force sensor due to the weight of the end effector in the first posture, The weight vector and the center-of-gravity vector may be obtained by the least squares method from N relationships that are respectively established between the posture and the force applied to the force sensor due to the weight of the end effector in each of the postures.

Further, when the end effector is in an unloaded state, the force control robot is applied to the force sensor due to the weight of the end effector obtained from the weight vector and the center of gravity vector in an arbitrary posture of the robot. The true offset voltage of the force sensor is measured in consideration of the force applied to the force sensor due to the output voltage of the force sensor, the true offset voltage, and the weight of the end effector. The external force applied to the end effector may be determined based on the applied force.

[0011]

Since the present invention comprises the above technical means, only the external force applied to the end effector can be accurately measured. At this time, if the offset voltage of the force sensor is taken into consideration, it is possible to more accurately measure only the external force applied to the end effector.

Further, when the weight control vector and the center of gravity vector are obtained at the start of work such as when the force control robot is turned on or after the movement, the inclination of the force control robot itself or the force sensor is generated. It becomes possible to eliminate the influence of the offset and to reliably measure only the external force applied to the end effector.

[0013]

DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS An embodiment of a weight compensation method for an end effector in a force control robot according to the present invention will be described below with reference to the drawings.

First, the center of gravity vector and the weight vector of the end effector will be described with reference to FIGS. 2 and 3. In the present embodiment, the center of gravity vector of the end effector 3 attached to the tip of the force control robot 1 via the force sensor 2 means the center of gravity 4 of the force sensor 2 to the end effector 3 as shown in FIG. The vector ^S h in the sensor coordinate system up to the center of gravity 5 of. Here, as shown in FIG. 3, the sensor coordinate system is a coordinate system in which the coordinate origin is located at the center of gravity 4 of the force sensor 2. Further, the weight vector of the end effector 3 is a vector ^R in the robot coordinate system that is oriented vertically from the center of gravity 5 of the end effector 3.
w. Here, as shown in FIG. 3, the robot coordinate system is a coordinate system in which the coordinate origin is located at a point 6 on the robot rotation axis of the base of the force control robot 1.

On the other hand, the formula for calculating the external force ^R F applied to the end effector from the output voltage of the force sensor is generally given by the following (Formula 1). Note that the subscripts on the left shoulder of the English letters (W, f, m, w, etc. described below) representing the physical quantity indicate the coordinate system displaying the physical quantity.

^R F = ^R R _S ^′ · F ₀ (V−V _OS ) ^−R W (Formula 1)

Here, ^R F is an external force (6 components) applied to the end effector in the robot coordinate system, ^R R _S ^'
Is the coordinate transformation matrix ^R from the sensor coordinate system to the robot coordinate system
A matrix having diagonal elements of R _S (3 rows and 3 columns) (6 rows and 6 columns),
F ₀ is a matrix (6 rows and 6 columns) for converting the output voltage of the force sensor into a force in the sensor coordinate system, V is the output voltage of the force sensor (6 components), V _OS is the offset voltage of the force sensor ( 6 components). Also, ^R W is the force applied to the force sensor due to the weight of the end effector in the robot coordinate system (6 component consisting weight vector ^R w and moment vector ^R m _W 3-component, respectively).

This ^R W and its constituents ^R w and
^R m _W is represented by the following [Equation 1], [Equation 2] and [Equation 3], respectively.

[0019]

[Equation 1]

[0020]

[Equation 2]

[0021]

[Equation 3]

Therefore, when the (Equation 1) is written as a component, it is expressed by the following [Equation 4]. In [Formula 4], ^R
f _x, ^R f _y, the ^R f _z are three components of the force applied to the end _{^{effector, R m x, R m y}} , the ^R m _z is 3 components moment applied to the end effector.

[0023]

[Equation 4]

Here, the weight vector ^R w and the center of gravity vector ^S h are, by definition, vectors that do not change even if the posture of the force control robot 1 changes. Further, the coordinate transformation matrix ^R R _S from the sensor coordinate system to the robot coordinate system can be known from the movement of the force sensor 2 with respect to the base body of the force control robot 1. Therefore, the weight vector ^R w of the end effector 3 in a certain posture of the force control robot 1
And the center of gravity vector ^S h are obtained, the coordinate transformation matrix
Depending on ^R R _S , only the force ^R W exerted on the force sensor due to the weight of the end effector in the robot coordinate system can always be obtained.

A method of actually compensating the weight of the end effector of the force control robot in this embodiment will be described below with reference to the flowchart of FIG.

In this embodiment, first, when the work of the force control robot is started, the weight vector ^R w and the sensor in the robot coordinate system of the end effector attached to the tip of the force control robot via the force sensor are detected. The center of gravity vector ^S h in the coordinate system is obtained (step S1). Hereinafter, the content of step S1 will be described more specifically.

In step S1, first, a certain posture (basic posture) of the force control robot when the end effector is unloaded (nothing touches the end effector).
The output voltage of the force sensor is measured by and is set as a temporary offset voltage (step S11). At this time, the force applied to the force sensor due to the weight of the end effector is ignored. That is, in (Equation 1) described above, the force applied to the force sensor due to the weight of the end effector.
^{Let R} W ₀ be zero. Then, in the unloaded state, the actual external force ^R F = 0 applied to the end effector is
The following (Equation 2) holds from the same idea as (Equation 1).

0 = ^R R _{S, 0} ^′ · F ₀ [(V _OS + ΔV ₀ ) − (V _OS + ΔV ₀ )] (Equation 2)

In (Equation 2),^RR_{S, 0} ^'Is the basic posture
Transformation from sensor coordinate system to robot coordinate system
A matrix having a matrix with diagonal elements, V_OSIs the true offset voltage
Pressure, ΔV₀Is the true offset voltage and the output voltage in this position.
Pressure V_OS+ ΔV₀Error with^RW ₀Was set as zero
(Caused by) and V_OS+ ΔV₀Is the above provisional off
Represents the set voltage. Therefore, the end in the robot coordinate system
Force applied to the force sensor due to the weight of the effector
^RW₀And error ΔV₀Is expressed by the following (Equation 3) between
Relationship is established.

^R W ₀ = ^R R _{S, 0} ^′ · F ₀ · ΔV ₀ (Formula 3)

Next, the posture of the force control robot is changed from the basic posture to another posture in which the end effector is in an unloaded state (step S12).

After that, the output voltage of the force sensor in this posture is measured. Also at this time, the force ^R W ₁ applied to the force sensor due to the weight of the end effector in this posture is ignored. Then, the output voltage of the force sensor changes due to the influence of the weight of the end effector, and the following (Equation 4) is established. Therefore, the external force ^R F ₁ applied to the end effector at this time is obtained from this (Equation 4) ( Step S13).

^R F ₁ = ^R R _{S, 1} ^′ · F ₀ [(V _OS + ΔV ₁ ) − (V _OS + ΔV ₀ )] (Equation 4)

In (Equation 4), ^R F ₁ is essentially the external force applied to the end effector in this posture, but since this state is a no-load state, ^R F ₁ is ^R W
It is a virtual external force resulting from placing ₁ as zero and the offset voltage as V _OS + ΔV ₀ . In (Equation 4), ^R R _{S, 1} ^' is a matrix having a coordinate transformation matrix from the sensor coordinate system to the robot coordinate system in this posture as diagonal components, and ΔV ₁ is the output of the force sensor in this posture. Voltage V _OS +
It represents the error between ΔV ₁ and the true offset voltage V _OS (caused by placing ^R W ₁ at zero). Therefore, the relationship represented by the following (Formula 5) is established between the force ^R W ₁ applied to the force sensor due to the weight of the end effector in the robot coordinate system and the error ΔV ₁ .

^R W ₁ = ^R R _{S, 1} · F ₀ · ΔV ₁ (Equation 5)

From (Equation 2) to (Equation 5), the following (Equation 6)
Holds.

^R F ₁ = ^R W ₁ ^−R R _{S, 1} ^′ · ^S _{RR, 0} ^′ · ^R W ₀ (Equation 6)

When this (Equation 6) is written as a component, the following [Equation 5] is obtained. Note that ^S RR _{, 0} ^′ is a matrix (6 rows and 6 columns) having a diagonal transformation element ^S _RR (3 rows and 3 columns) from the robot coordinate system to the sensor coordinate system.

[0039]

[Equation 5]

Further, from [Equation 5], ^R f ₁ is ^R f ₁ = ^R w− ^R R _{S, 1} · ^S R _{R, 0} · ^R w = (where I is a unit matrix of 3 rows and 3 columns). _{^{I- R R S, 1 · S}} R R, 0) is expressed as ^R w (equation 7). When this is converted into the sensor coordinate system, the following (Equation 8) is established. ^S f ₁ = ( ^S R _{R, 1} ^−S R _{R, 0} ) ^R w (Equation 8)

On the other hand, from [Equation 5], ^R m ₁ is ^R m ₁ = ( ^R R _{S, 1} · ^S h) × ^R w − ( ^R R _{S, 1} · ^S R _{R, 0} ) [( ^R R _{S, 0} · ^S h) × ^R w] (Equation 9). If this is converted into the sensor coordinate system, the following (formula 10) is established. _{^{S m 1 = S h × (}} S R R, 1 · R w) - S h × (S R R, 0 · R w) = S h × S w 1 - S h × S w 0 = (- ^[S w ₁ ×] + [ ^S w ₀ ×]) ^S h (Equation 10)

Here, the symbol [ ^S w ×] is a matrix represented by the following [Equation 6].

[0043]

[Equation 6]

Therefore, the weight vector ^R w and the center of gravity vector ^S h can be obtained by solving the simultaneous three-dimensional linear equations of (Expression 8) and (Expression 10). However, in this embodiment, the posture of the force control robot is changed N-1 times (N ≧ 2) in order to reduce the influence of the measurement error. That is, step S12 and step S1
3 is executed N times in total, the output voltage of the force sensor is measured in N postures other than the basic posture, and N three-dimensional simultaneous linear equations of (Equation 8) and (Equation 10) are obtained. . Then, the weight vector ^R w and the center of gravity vector ^S h
For, the relational expressions shown in the following [Equation 7] and [Equation 8] are obtained.

[0045]

[Equation 7]

[0046]

[Equation 8]

Next, using the method of least squares, for example, [Equation 7]
The value of the weight vector ^R w can be estimated by solving the simultaneous linear equation of 1 to obtain the weight vector ^R w (step S14).

Subsequently, for example, by using the method of least squares, the simultaneous linear equations of [Equation 8] are solved to obtain the center of gravity vector ^S h (step S15), and the center of gravity vector ^S h is calculated.
The value of can be estimated.

The number N of times of changing the posture of the force control robot may be any number as long as it is 1 or 2 or more, but may be appropriately determined in consideration of the balance between calculation accuracy and load.

Next, in the present embodiment, when the force control robot 1 controls the force, the force sensor 2 outputs the output voltage from the force sensor 2.
The force applied to the force sensor 2 is calculated from the force applied to the force sensor 2 due to the weight of the end effector 3 obtained from the weight vector ^R w and the center of gravity vector ^S h. The external force applied to the end effector 3 is obtained by subtraction (step S2). Hereinafter, the content of step S2 will be described more specifically.

In step S2, first, the true offset voltage of the force sensor is taken in an arbitrary posture of the force control robot in which the end effector is in an unloaded state (step S21). At this time, from the weight vector ^R w and the center of gravity vector ^S h obtained in step S1 (Equation 1)
The force calculated by (Equation 3), that is, the force ^R W applied to the force sensor due to the weight of the end effector in this posture is considered. That is, when (Equation 1) is modified in consideration of ^R F = 0 in the no-load state, the following (Equation 11) is established. Therefore, by solving this equation, the true offset voltage V _OS can be obtained.

F ₀ · V _OS = F ₀ · V− ^S _RR ^′ · ^RW (Formula 11)

Next, in order to actually control the force of the end effector, the output voltage of the force sensor is measured in the load state where the end effector is in contact with the object (step S22).

Next, from the output voltage of the force sensor measured in step S22, the offset voltage V _OS obtained as described above and the posture at this time obtained from the weight vector ^R w and the center of gravity vector ^S h The external force applied to the end effector from (Equation 1) using the force ^R W applied to the force sensor due to the weight of the end effector.
^{Find R} F (step S23)

Then, while the force control is being performed, step S22 and step S23 are repeatedly executed to obtain the force applied to the end effector over time. As a result, the force applied to the end effector can always be known accurately, so that reliable force control can be performed.

The present invention is intended to accurately perform force control by compensating for the weight of the end effector of the force control robot, but by knowing the weight vector ^R w of the end effector in the robot coordinate system. Since the degree of tilt of the force control robot itself can be known, it can be used as a method for detecting the posture and tilt of the robot.

[0057] Further, the present invention is to previously obtain the weight vector ^R w of the end effector in advance the robot coordinate system, the weight vector ^R of the end effector including the object in a state of lifting the object by the end effector
By further obtaining w ′, the weight of the object can be known from the difference between the two weight vectors, and thus the method can be used as a method for detecting the weight of the object lifted by the robot.

[0058]

As described above, according to the present invention, the weight vector and the center of gravity vector of the end effector are obtained at the start of work, and the force applied to the force sensor is obtained from the output voltage of the force sensor during the force control. At the same time, the force applied to the force sensor due to the weight of the end effector obtained from the weight vector and the center of gravity vector is subtracted from the force applied to the force sensor to obtain the external force applied to the end effector. Therefore, even if the posture of the vertical articulated robot is changed or the force control robot itself is tilted, it is possible to reliably compensate the weight component of the end effector. It becomes possible to accurately measure only the external force applied to. Therefore, the force control of the force control robot can be reliably performed. Further, since the weight vector and the center of gravity vector are obtained by a simple procedure, it is possible to save the trouble of previously measuring and calculating the weight vector and the center of gravity vector of the end effector. Further, by measuring the true offset voltage of the force sensor in consideration of the force applied to the force sensor due to the weight of the end effector obtained from the weight vector and the center of gravity vector, the force It is possible to eliminate the influence of the offset generated in the sense sensor.

[Brief description of drawings]

FIG. 1 is a flow chart for explaining an embodiment of the present invention.

FIG. 2 is a diagram for explaining a weight vector and a center of gravity vector of an end effector.

FIG. 3 is a diagram for explaining a sensor coordinate system and a robot coordinate system.

[Explanation of symbols]

 1 force control robot 2 force sensor 3 end effector 4 center of gravity of force sensor 5 center of gravity of end effector 6 point on robot rotation axis of base of force control robot

Claims (4)

[Claims] 1. A weight vector in a robot coordinate system and a center of gravity vector in a sensor coordinate system of an end effector attached to a tip of a force control robot via a force sensor are obtained at the start of work of the force control robot. During the force control of the force control robot, the force applied to the force sensor is obtained from the output voltage of the force sensor, and due to the weight of the end effector obtained from the weight vector and the center of gravity vector. A weight compensation method for an end effector in a force control robot, comprising: subtracting a force applied to the force sensor from a force applied to the force sensor to obtain an external force applied to the end effector. 2. The force applied to the force sensor due to the weight of the end effector in the first posture of the force control robot in an unloaded state in which no external force is applied to the end effector. Ignoring this, the provisional offset voltage of the force sensor is measured, and in the second posture of the force control robot, the force sensor is caused by the output voltage of the force sensor due to the weight of the end effector. The virtual external force in the second posture is calculated by ignoring the force applied to the virtual external force, and the force applied to the force sensor due to the virtual external force and the weight of the end effector in the first posture. When,
The weight vector and the center-of-gravity vector are obtained from a relationship that holds between the force applied to the force sensor due to the weight of the end effector in the second posture. A method for compensating the weight of an end effector in the force control robot described. 3. The posture of the force control robot is set to N (N ≧
2) changing the number of times and obtaining the virtual external force in each posture, the virtual external force in each posture, the force applied to the force sensor due to the weight of the end effector in the first posture, and It is characterized in that the weight vector and the center of gravity vector are obtained by the least squares method from N relationships that are respectively established between the force applied to the force sensor due to the weight of the end effector in each posture and the force applied to the force sensor. The weight compensation method for an end effector in a force control robot according to claim 2. 4. When the end effector is in a no-load state, in any posture of the force control robot, in addition to the force sensor due to the weight of the end effector obtained from the weight vector and the center of gravity vector. The true offset voltage of the force sensor is measured in consideration of the force applied to the force sensor due to the output voltage of the force sensor, the true offset voltage, and the weight of the end effector. The external force applied to the end effector is obtained based on the applied force.
4. The weight compensation method for an end effector in the force control robot according to any one of 3 above.