JPH09174466A - Arm tip position calibration device - Google Patents

Abstract

PROBLEM TO BE SOLVED: To reduce a model error and allow calibration by obtaining gravitational torque with an arm model inputting an articulate angle command and outputting the gravitational torque, and obtaining the articulate rigidity constant in response to the gravitational torque. SOLUTION: This calibration device 1 is constituted of an arm simulator 2, a gravitational torque deflection angle guiding unit 3, a deflection angle correcting unit 4, an adder 5, and an arm tip position/attitude guiding unit 6. The arm tip position/attitude guiding unit 6 is connected to a controller 8 and an arm tip position/attitude guiding unit 9 via a subtracter 7 outside the calibration device 1. The controller 8 is connected to an arm 12 via an articulate angular velocity command guiding unit 10 and an articulate driving amplifier 11. The arm 12 is connected to the subtracter 7 via the arm tip position/attitude guiding unit 9, and it is connected to an articulate angular velocity guiding unit 10 via a Jacobian matrix, etc.

Description

Detailed Description of the Invention

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to an arm hand end position calibration device applied to arm hand end position control.

[0002]

2. Description of the Related Art FIG. 3 is a diagram showing the configuration of a conventional arm hand position calibration device of this type. In FIG. 3, reference numeral 20 denotes a calibration device, and the calibration device 20 inputs the arm target position / orientation S1 to the arm simulator 2. The arm simulator 2 gives the input arm target position / orientation S1 to the internal arm model and obtains the joint angle of the arm model. Then, the obtained joint angle is output to the adder 5 and the gravity torque deflection angle derivation device 30 as the joint angle command S2.

FIG. 4 shows the above-described gravity torque deflection angle deriving device 3.
FIG. 3 is a diagram showing a configuration of a zero. The gravity torque deflection angle deriving unit 30 inputs the joint angle command S2 and supplies it to the joint gravitational torque deriving unit 31 to obtain the gravitational torque S3 applied to each joint. Then, the obtained gravitational torque S3 and the joint stiffness constant 34 that holds the stiffness of each joint as a constant are input to the divider 32, and the gravitational torque corresponding to the stiffness constant of each joint is input as the joint deflection angle due to the gravitational torque. A certain gravitational torque deflection angle S5 is calculated and output.

The adder 5 in the calibration device 20 adds the joint angle command S2 and the gravitational torque deflection angle S5 and outputs a corrected joint angle command S7. The arm hand position / posture deriving unit 6 inputs the corrected joint angle command S7, performs correction, and outputs the arm hand position / posture command S8 to the subtractor 7.

In this way, the calibration device 20 outputs the arm hand position / orientation command S8, and the subtractor 7,
The controller 8, the arm hand position / posture derivation unit 9, the joint angular velocity command derivation unit 10, the Jacobian matrix derivation unit 13, the inverse Jacobian matrix derivation unit 14, the joint angle S9, the joint angular velocity S10, the joint drive amplifier 11, and the joint drive current S11. Each joint angle of the arm 12 is driven by using the arm 12, and the arm hand position is adjusted to the arm target position / posture S1.

[0006]

In the conventional arm hand position calibration method, the deflection angle due to the gravitational torque is obtained from the gravitational torque applied to each joint and the stiffness constant of each joint, and the deflection angle is canceled. , Was added to the joint angle command S2. The gravitational torque used to calculate the deflection angle is calculated using model parameters of the arm link length, link center of gravity length, and link weight.
However, since the model parameter and the stiffness constant of the joint include a model error, there is a problem that it is not possible to perform accurate calibration with the obtained deflection angle due to the gravitational torque.

It is an object of the present invention to provide a calibration device capable of reducing the model error included in the model parameter for obtaining the gravitational torque and the stiffness constant of the joint and accurately calibrating the arm hand position.

[0008]

In order to solve the above problems and achieve the object, the arm hand position calibration device of the present invention is configured as follows. The arm hand position calibration device of the present invention inputs an arm target position / posture which is a target of a hand position of an arm, and outputs an joint angle command using an arm model, and an arm simulator which inputs the joint angle command and outputs gravity. Gravity torque is obtained using an arm model that outputs torque, the joint stiffness constant is obtained according to the joint stiffness function that outputs the joint stiffness constant according to this gravity torque, and the obtained gravity torque and joint stiffness constant are obtained. , A deflection angle deriving device for deriving a deflection angle for deriving a deflection angle, a deflection angle compensator for reducing a model error included in the deflection angle due to the gravitational torque, and outputting a corrected deflection angle, the joint angle command and the correction. An adder that adds a deflection angle and outputs a corrected joint angle command, and an adder that inputs the corrected joint angle command and outputs the arm end target position And the no-hand position and orientation derivation unit, and is configured from.

[0009] As a result of taking the above measures, the following effects occur. According to the arm hand position calibration device of the present invention, the stiffness constant of the joint is obtained according to the gravity torque obtained from the joint angle command related to the arm target position / posture, and the deflection angle is obtained from the gravity torque and the stiffness constant of the joint. Derived, the corrected deflection angle reduced by the model error included in the deflection angle and the joint angle command are added, and the arm tip target position is output, so that the arm is driven and the known point position and the arm tip position are set. In order to match the deflection angle obtained from the error between the gravity torque and the deflection angle obtained from the joint stiffness constant, a correction coefficient is set for each joint in the deflection angle corrector, and is obtained from the gravity torque and the joint stiffness constant. Model error can be reduced by correcting the deflection angle.

[0010]

1 is a diagram showing a configuration of an arm hand position calibration device according to an embodiment of the present invention. In FIG. 1, the same parts as those in FIG. 3 are designated by the same reference numerals.

In FIG. 1, reference numeral 1 denotes a calibration device. The calibration device 1 includes an arm simulator 2, a gravitational torque deflection angle derivation device 3, a deflection angle correction device 4, an adder 5 and an arm hand position / posture derivation device 6. It consists of The arm simulator 2 is connected to the gravity torque deflection angle derivation device 3 and the adder 5, and the gravity torque deflection angle derivation device 3 is connected to the addition device 5 via the deflection angle correction device 4. The adder 5 is connected to the arm hand position / posture derivation device 6.

The arm hand position / posture deriving device 6 is connected to a controller 8 and an arm hand position / posture deriving device 9 via a subtracter 7 outside the calibration device 1.
The controller 8 is connected to the arm 12 via a joint angular velocity command derivation device 10 and a joint drive amplifier 11.
The arm 12 is connected to the subtractor 7 via the arm hand position / posture derivation device 9 and is also connected to the joint angular velocity command derivation device 10 via the Jacobian matrix derivation device 13 and the inverse Jacobian matrix derivation device 14. .

FIG. 2 shows the gravitational torque deflection angle deriving device 3 described above.
FIG. 3 is a diagram showing the configuration of FIG. In FIG. 2, reference numeral 31 is a joint gravity torque derivation device, and this joint gravity torque derivation device 31 is connected to a divider 32 and a joint stiffness function unit 33. Further, the joint stiffness function unit 33 is connected to the divider 32.

The operation of the arm hand position calibration device will be described below with reference to FIGS. 1 and 2. The calibration device 1 inputs the arm target position / orientation S1 (xref, yref, zref), which is the target of the arm hand position, to the arm simulator 2. The arm simulator 2 uses the arm target position / posture S1 (xref, yref, zr
ef) is substituted into a predetermined arm model formula having the inside, the arm model is the target S1 (xref,
yref, zref).

Then, the arm simulator 2 obtains each joint angle (θ1, θ2, θ3, θ4, θ5, θ6, θ7) of the arm 12 having seven axes from the posture of the arm model, and the joint angle command S2 is obtained. (Θ1ref, θ2ref, θ3ref,
Output as θ4ref, θ5ref, θ6ref, θ7ref). The joint angle command S2 (θ1ref, θ2ref, ..., θ7ref) is input to the adder 5 and the joint gravity torque derivation device 31 of the gravity torque deflection angle derivation device 3.

The joint gravity torque derivation device 31 substitutes the input joint angle command S2 (θ1ref, θ2ref, ..., θ7ref) into a gravity torque derivation formula which is internally provided, and a gravity torque S3 (Tg1, Tg2, Tg3, Tg4, Tg5, Tg6, T
g7) is output to the divider 32 and the joint stiffness function unit 33. The joint stiffness function unit 33 receives each input gravity torque S3 (Tg
1, Tg2, ..., Tg7) according to the stiffness constant S4 of each joint
(K1, K2, K3, K4, K5, K6, K7) is derived and output to the divider 32.

The rigidity of the joint has a value according to the load, and the gravitational torque applied to each joint changes depending on the posture of the arm 12. Therefore, in order to reduce the deviation of the arm hand position / posture, it is necessary to obtain the rigidity of the joint as accurate as possible. Therefore, the stiffness constant S4 is obtained using the joint stiffness function.

The divider 32 has a gravitational torque S3 (Tg1, Tg).
g2, ..., Tg7) is set to a stiffness constant S4 (K1, K2, ..., K7).
), And the deflection angle S5 (dθ1, dθ2, dθ3, which is the deflection angle due to the gravity torque of each joint).
Deflection angle compensator 4 for dθ4, dθ5, dθ6, dθ7)
Output to The deflection angle corrector 4 drives the arm 12 in advance, and the deviation amount (dx, dy, d
z) from each joint deflection angle (dθ1 ′, dθ2 ′, dθ3
′, Dθ4 ′, dθ5 ′, dθ6 ′, dθ7 ′). The deflection angle corrector 4 internally has the deflection angles (dθ1 ′, dθ2 ′, ..., dθ7 ′) of the joints and the gravitational torque deflection angle S5 (dθ1, dθ2, ..., dθ7).
Correction factors (α1, α2, α3, α4, α
5, α6, α7).

As described above, the gravitational torque deflection angle S5
Is calculated from the joint stiffness constant S4 and the gravitational torque S3, and there is some error from the actual deflection angle of each joint, which affects the deviation of the arm hand position / posture. Therefore, it is necessary to make a correction so that the deflection angle of gravity torque is matched with the actual deflection angle of each joint.

Then, the corrected deflection angles S6 (α1 · dθ1, α2 · dθ2, α3 ·
dθ3, α4 · dθ4, α5 · dθ5, α6 · dθ6,
Outputs α7 · dθ7) (· represents a product) to the adder 5. The adder 5 uses the joint angle command S2 (θ1ref, θ2ref,
, .Theta.7ref) and corrected deflection angles S6 (.alpha.1 .d.theta.1, .alpha.2 .d.theta.2, ..., .alpha.7) corresponding to these variables.
.D.theta.7) and each variable, and the corrected joint angle command S7 (.theta.'1ref, .theta.'2ref, .theta.'3ref,
θ′4ref, θ′5ref, θ′6ref, θ′7ref) are output to the arm hand position / posture deriving device 6.

The arm hand position / posture deriving device 6 inputs the corrected joint angle command S7 (θ'1ref, θ'2ref, ..., θ'7ref) to the internal arm model, and the arm hand position / posture command S8 ( x'ref, y'ref, z'ref) are calculated.
This arm hand position / posture command S8 (x'ref, y're
f, z′ref) is output from the arm hand position / posture derivation device 6 to the outside of the calibration device 1 and input to the subtractor 7.

Then, as shown below, the subtracter 7, the controller 8, the arm hand position / orientation deriving unit 9, the joint angular velocity deriving unit 10, the Jacobian matrix deriving unit 13, the inverse Jacobian matrix deriving unit 14, the joint angle S9, the joint. Using the angular velocity command S10, the joint drive amplifier 11, etc., each joint angle of the arm 12 is driven, and the arm hand position is adjusted to the arm target position / posture S1.

The arm hand position / posture deriving device 9 inputs the joint angle S9 of each axis of the arm 12, and obtains the arm hand position / posture by using the joint angle S9 and the following equation (1). T = A1 * A2 * A3 * A4 * A5 * A6 * A7 (1) The matrix Ai is

[0024]

[Equation 1] And shows the position / orientation of each link. In addition,
In the matrix Ai, ni = (nxi, nyi, nzi): each link normal vector oi = (oxi, oyi, ozi): each link direction vector ai = (axi, ayi, ozi): each link approach vector pi = ( pxi, pyi, pzi): each link position vector.

The reason for deriving the arm hand position / posture in this way is to perform feedback control so that the arm hand position / posture command S8 described above and the actual arm hand position / posture match. .

Next, the difference between the arm hand position / posture command S8 and the arm hand position / posture obtained from the joint angle S9 in the arm hand position / posture deriving device 9 as described above is obtained by the subtracter 7, and the controller 8 The deviation is multiplied by a gain and the corrected arm position / attitude command is output. This is shown in equation (2). dref = k · de (2) The matrices k and de are

[0027]

[Equation 2]

Dref: corrected arm position / posture command k: gain de: deviation of target arm tip position / posture and feedback hand tip position / posture, which is multiplied by the controller 8 is the deviation of arm tip position / posture This is to reduce the size and to improve the response.

Subsequently, the joint angular velocity command derivation unit 10 calculates the inverse Jacobian matrix input from the inverse Jacobian matrix derivation unit 14 described later based on the deviation between the target arm hand position / posture and the fed back hand position / posture. The joint angular velocity command S10 of the arm is obtained by using this. This is shown in equations (3) and (4).

Dθ = J ⁻¹ · dref (3) dθ ′ = dθ / dt (4) J ⁻¹ : Inverse Jacobian matrix dθ ′: Function angular velocity command dθ: Modified joint angle dt: Sampling time The arm 12 The motor cannot be driven without deriving a command (joint angular velocity) to the power source for driving the motor of each joint.

On the other hand, in the Jacobian matrix derivation unit 13, the Jacobian matrix is a matrix consisting of elements of the minute rotation and translation vector in the position and orientation of T when all seven joints are minutely changed, as shown in the equation (5). expressed.

[0032]

(Equation 3)

J: 6 × 7 matrix In this way, each joint angle is substituted into the Jacobian matrix to obtain the Jacobian matrix, and then the inverse Jacobian matrix deriving unit 14 obtains the inverse Jacobian matrix. The matrix for obtaining the correction function angle from the correction arm hand position / posture can be an inverse matrix of the Jacobian matrix as shown in equation (6). Therefore, the inverse Jacobian matrix is derived.

[0034]

(Equation 4)

The joint drive amplifier 11 inputs the function angular velocity command S10 for each joint of the arm 12 input from the joint angular velocity command derivation device 10 to the joint drive current 11 for driving the motor of the arm 12 to the arm 12. Output.
As a result, each joint of the arm 12 is driven.

The present invention is not limited to the above-mentioned embodiment, and can be carried out by appropriately modifying it without departing from the scope of the invention. (Summary of Embodiment) The configuration, operation and effect shown in the embodiment are summarized as follows. The arm tip position calibration device shown in the embodiment includes an arm target position posture S that is a target of the hand position of the arm 12.
The gravity torque S3 is obtained by using the arm simulator 2 which inputs 1 and outputs the joint angle command S2 using the arm model, and the arm simulator 2 which inputs the joint angle command S2 and outputs the gravity torque. According to the joint stiffness function 33 which outputs the stiffness constant of the joint, the stiffness constant S4 of the joint is determined, and the deflection angle S5 is derived from the determined gravity torque S3 and the stiffness constant S4 of the joint. And a deflection angle compensator 4 for reducing a model error included in the deflection angle due to the gravity torque S3 and outputting a corrected deflection angle S6, and adding the joint angle command S2 and the corrected deflection angle S6 to correct the deflection angle. Joint angle command S
7 and an arm hand position / posture deriving device 6 which inputs the corrected joint angle command S7 and outputs the hand target position (S8) of the arm 12.

As described above, in the arm hand position calibration device, the stiffness constant S4 of the joint is obtained according to the gravity torque S3 obtained from the joint angle command S2 related to the arm target position / posture S1, and the gravity torque S3 and the gravity torque S3 are obtained. Deflection angle S5 is derived from joint stiffness constant S4, corrected deflection angle S6 in which the model error included in deflection angle S5 is reduced, and joint angle command S2 are added, and the target end position (S8) of arm 12 is output. Therefore, the deflection angle corrector is driven so that the deflection angle S5 obtained from the gravity torque S3 and the joint stiffness constant S4 matches the deflection angle obtained from the error between the known point position and the arm hand position. Set the correction coefficient for each joint in 4 and set the deflection angle S5
The model error can be reduced by correcting

[0038]

According to the arm hand position calibration device of the present invention, it is possible to reduce the error in the model parameter used to obtain the gravitational torque and the error in the stiffness constant of the joint. And since the model error can be reduced,
By adding the corrected deflection angle to the target joint angle, the deviation between the arm hand position and the target position can be reduced, and the arm hand position can be accurately calibrated.

[Brief description of the drawings]

FIG. 1 is a diagram showing a configuration of an arm hand position calibration device according to an embodiment of the present invention.

FIG. 2 is a diagram showing a configuration of a gravity torque deflection angle deriving device according to an embodiment of the present invention.

FIG. 3 is a diagram showing a configuration of a conventional arm hand position calibration device.

FIG. 4 is a diagram showing the configuration of a gravity torque deflection angle deriving device in a conventional arm hand position calibration device.

[Explanation of Codes] 1 ... Calibration device 2 ... Arm simulator 3 ... Gravity torque deflection angle deriving device 4 ... Deflection angle correcting device 5 ... Adder 6 ... Arm hand position / posture deriving device 7 ... Subtractor 8 ... Controller 9 ... Arm hand position / posture derivation device 10 ... Joint angular velocity command derivation device 11 ... Joint drive amplifier 12 ... Arm 13 ... Jacobian matrix derivation device 14 ... Inverse Jacobian matrix derivation device 31 ... Joint gravity torque derivation device 32 ... Divider 33 ... Joint stiffness Function part 20 ... Calibration device 30 ... Gravity torque deflection angle deriving device

Claims (1)

[Claims] 1. An arm simulator for inputting an arm target position / posture which is a target of an arm end position and outputting a joint angle command using an arm model, and an arm model for inputting the joint angle command and outputting a gravity torque. The gravitational torque is obtained by using, and the stiffness constant of the joint is obtained by the joint stiffness function that outputs the stiffness constant of the joint according to this gravitational torque, and the deflection angle is derived from the obtained gravitational torque and the stiffness constant of the joint. A gravitational torque deflection angle deriving device, a deflection angle corrector that reduces the model error included in the deflection angle due to the gravitational torque, and outputs a corrected deflection angle, and adds the joint angle command and the corrected deflection angle to correct the deflection angle. An adder that outputs a joint angle command; and an arm hand position / posture deriving device that inputs the corrected joint angle command and outputs a hand target position of the arm. Arm tip position calibration apparatus, characterized in that the.