JPH0768480A - Method for controlling articular angle of manipulator - Google Patents

Abstract

PURPOSE:To improve the absolute precision of the manipulator fingers by effectively correcting the articular angle which affects much to the error of the manipulator fingers. CONSTITUTION:The difference between the positional attitude of a manipulator fingers 9 (or the position control point) to be measured by a position measuring instrument 2 and the positional attitude of the manipulator fingers 9 to be calculated by an articular angle detector 5 provided on each joint of the manipulator and the link length between the respective joints, i.e., the quantity to be corrected is made to return to the corrective quantity of the articular angle by using the Jacobian matrix to show the relationship between the positional attitude of the manipulator fingers 9 and the articular angle. In addition, the articular link length gain which is preset by the link length from each joint to the manipulator fingers 9 is multiplied to execute the weighting to the corrective quantity, the corrective quantity of each articular angle of the manipulator is obtained to realize the correction, and the offset quantity of the manipulator joint is fixed by repeating this process.

Description

Detailed Description of the Invention

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a joint angle control method for manipulators.

[0002]

2. Description of the Related Art The most general calibration method applied to the joint angle control of a manipulator (robot) is performed by using an encoder having a origin finding function as a joint angle sensor and using origin matching which is this function. ing. However, this method cannot solve any error in mounting the encoder or other errors of the mechanism (due to processing or assembly).

Further, generally-known calibration methods often perform error correction between the robot's actual environment and its image data (TV image), and do not solve the error due to the mechanism of the manipulator. The content shown in the present invention is a method for improving the positioning accuracy of the robot, and the positioning accuracy of the robot depends on the relative positioning accuracy because there is no such method in the past or it was difficult to commercialize. It is the current situation.

[0004]

When the manipulator performs work in a certain actual environment, the positioning accuracy of the manipulator hand in the work environment becomes a problem. That is,
If the three-dimensional positioning accuracy (absolute accuracy) from the reference point set in the work environment is high, for example, the position and orientation of the handling object to be worked can be described by the three-dimensional coordinates from the previous reference point. Work with a manipulator becomes possible.

However, since there are errors in the mechanism of the manipulator (link bending, assembly error, etc.), the position of the manipulator hand calculated from the joint angle sensor value installed at each joint of the manipulator and the link length between the joints. The posture value always contains an error with respect to the actual position and posture in the actual environment.

Therefore, the manipulator work does not depend on the above-mentioned absolute positioning accuracy, but the manipulator position and orientation using the joint angle sensor value and each joint link length calculated in a form including an error are mainly used, that is, The work is performed depending on the relative positioning accuracy.

Therefore, when considering the teaching of a handling object in the work environment, the manipulator relative positioning accuracy must be relied upon. Therefore, if a human actually holds the manipulator hand and grips the object, for example, it is actually necessary. It is necessary to teach by teaching teaching that the manipulator is used for gripping. Therefore, there is a problem that environmental teaching is very troublesome.

In recent years, this manipulator is automatically operated (autonomously).
The issue of lessening the teaching to become more important is becoming important.
Therefore, it is necessary to realize teaching by off-line teaching using CAD information or the like that can reduce teaching. However, the improvement of the absolute positioning accuracy of the manipulator is important for this, and it is a big problem.

The present invention has been made in view of the above situation, and provides a joint angle control method for a manipulator capable of effectively correcting the angle of a joint having a large influence on the manipulator hand error and improving the absolute accuracy of the manipulator hand. The purpose is to provide.

[0010]

A joint angle control method for a manipulator according to the present invention comprises a position and orientation of a manipulator hand or a position control point measured by an arbitrary measuring instrument, an angle sensor installed at each joint of the manipulator, and an angle sensor installed at each joint. The difference between the position and orientation of the manipulator fingertip calculated by the link length between joints is reduced to the joint angle correction amount using the Jacobian matrix that shows the relationship between the position and orientation of the manipulator handtip and the joint angle, and the correction amount is weighted. In order to perform, perform the correction by deriving the correction amount for each joint angle of the manipulator by multiplying the joint link length gain preset by the link length from each joint to the tip of the manipulator, and repeating this to fix the offset amount of the manipulator joint. It is characterized by

[0011]

FIG. 1 shows a principle diagram for explaining the present invention. In FIG. 1, xo −yo −zo: reference coordinate system (reference coordinate) xm −ym −zm: reference coordinate of manipulator (manipulator coordinate) To: position from reference coordinate measured by an external measuring instrument to manipulator coordinate Posture Ts: Position and posture from the reference coordinates measured by an external measuring instrument to the manipulator coordinate hand Tm: Position and posture of the manipulator hand calculated from the joint angle and link length with reference to the manipulator coordinates. However, what is Tα

[0012]

[Equation 1] Is a 4 × 4 matrix represented by.

Here, the manipulator hand position / posture Tm
Includes an error of the joint or link of the manipulator 1a. Therefore, To and Ts measured by an external measuring instrument
Is used to find ^ Tm = TsTo ^-1 (2). Since this value is measured by an external measuring device, it has more reliable accuracy than the position and orientation Tm. Next, the error Δr of the manipulator hand position / posture is obtained from the positions / postures Tm and Tm.

[0014]

[Equation 2]

This error Δr is the amount to be corrected by calibration, and is reduced to the amount of calibration at the joint angle by using the Jacobian matrix that is the conversion of the position and orientation of the manipulator hand and each joint angle.

[0016]

[Equation 3]

Δθ in the equation (4) is estimated to be an offset amount to be corrected at each joint, but the influence of each joint error on the manipulator hand position / posture is not uniform and the root joint has a larger error influence. Become. Paying attention to this point, the joint offset amount Δθ is weighted. Therefore, as the joint link length gain Ke

[0018]

[Equation 4] Then, the final correction amount is derived by Δθ correction = Ke · Δθ (6).

The joint link length gain Ke is obtained according to the total link length from each joint to the manipulator hand as shown in FIGS. 2 and 3, for example, in the case of a 6-axis manipulator, and is generally an N-degree-of-freedom manipulator. In

[0020]

[Equation 5] Set with.

Then, as shown in the following equation, the correction amount Δθ is corrected by adding an offset to the reading value θs of each joint sensor of the manipulator. θs = θs + Δθ correction (10) As a result, the manipulator eliminates the error according to the correction amount Δθ correction.

Similarly, the positions and orientations Tm and Tm are calculated and measured, and the above procedure is repeated. The calibration is completed by repeating this process until the hand error Δr becomes equal to or less than a certain allowable value Δre as shown in the following equation Δr <Δre (11).

As described above, in the method according to the present invention, the manipulator hand error Δr is obtained by using an external measuring instrument or the like, and then the error amount Δθ at each joint of the manipulator for correcting the hand error Δr is obtained.

When correcting the manipulator hand tip error at each joint, pay attention to the fact that the joint located farther from the manipulator hand tip (located at the root) has a greater effect as shown in FIG. Then, the error amount Δθ at each joint is weighted. The weighting gain is determined based on the link length from each joint to the hand. Since the joint having a great influence on the manipulator hand end error is effectively corrected by this weighting, the correction of the manipulator hand end error can also be effectively realized.

[0025]

DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS An embodiment of the present invention will be described below with reference to the drawings. FIG. 5 shows an example of a 6-axis manipulator. As shown in the figure, 6-axis manipulator 1
Is a control amplifier 4, a joint angle detector 5, and a control computer 6
The position is controlled by the block diagram shown in FIG.

Now, the 6-axis manipulator 1 is controlled by the hand position / posture Tm. At this time (the manipulator is in a stationary state), the position and orientation Ts from the reference coordinate 7 to the manipulator hand 9 and the position and orientation To from the reference coordinate to the manipulator coordinate 8 are measured using the position measuring device 2, and the reference coordinate 7 is used as a basis. Then, the manipulator hand position / orientation ^ Tm at the manipulator coordinate 8 is calculated.

Next, the error Δr on the manipulator hand 9 is obtained from Tm and ^ Tm. This error Δr is obtained by the above equation (3). In the manipulator control computer 6, as shown in FIG. 6, the Jacobian matrix 20 is constantly calculated and output to the manipulator 1 via the control amplifier 4.

In this case, the Jacobian matrix 20 is used to reduce the hand error Δr to the joint error Δθ by using the equation (4).
This joint error Δθ is weighted using the joint link length gain Ke obtained in FIGS. 2 and 3, and the joint correction amount Δθ correction is obtained by the equation (6).

This .DELTA..theta. Correction is corrected to the joint angle .theta.s detected by the joint angle detector 5 as an offset amount as shown in equation (10). As a result, the manipulator current position / posture 21 in the block diagram shown in FIG. 6 changes, and a deviation from the conventional target value Tm is generated, and the manipulator hand 9 is newly moved to the position to the target value Tm.

After the operation of the manipulator 1 is settled, the position measuring instrument 2 is used again to detect the error amount as shown in the flowchart (algorithm) of FIG.
That is, the joint angle based on the manipulator coordinates,
The position / orientation Tm of the manipulator hand is derived from the link length (step A1), and then the manipulator hand position / orientation ^ Tm is calculated from the position / orientation To and Ts measured by the external measuring device by the equation (2) (step A2).
). Further, the manipulator hand tip position / orientation error Δr is obtained from the equation (3).

Then, it is judged whether or not the hand position / orientation error Δr is smaller than a certain allowable value Δre (step A4).
), If the error Δr is larger than the allowable value Δre, (4)
The joint correction amount Δθ is calculated by the equation (step A5), and then the Δθ correction is obtained by the equation (6) and weighted by the joint link length gain Ke (step A6). Further, as shown in the equation (10), the correction amount Δθ correction is added to the reading value θs of the joint sensor by offsetting (step A7
), And then returns to step A1.

Thereafter, the same operation is repeated, and step A
When it is determined that the error Δr becomes smaller than the allowable value Δre in 4, the processing ends. As described above, the above calculation is repeated so that the hand error Δr becomes smaller than the allowable error Δre. This eliminates the error between the position and orientation Tm and ^ Tm.

That is, the position and orientation of the manipulator hand (or position control point) measured by an arbitrary measuring device and the joint angle detector 5 installed at each joint of the manipulator.
And the difference between the position and orientation of the manipulator hand 9 calculated by the link length between the joints, that is, the amount to be corrected, using the Jacobian matrix indicating the relationship between the position and orientation of the manipulator hand 9 and the joint angle. To further weight the correction amount, the joint link length gain preset by the link length from each joint to the manipulator hand 9 is multiplied to derive each joint angle correction amount of the manipulator, and the correction is performed. Is performed repeatedly to fix the offset amount of the manipulator joint,
This improves the hand positioning accuracy (absolute accuracy) with respect to the reference coordinates of the manipulator 1.

[0034]

As described in detail above, according to the present invention, the mechanical unavoidable error generated at the manipulator hand is converted into the error of each joint angle of the manipulator, and the error of the joint angle is weighted, whereby the manipulator is weighted. Since the joint having a great influence on the hand error is effectively corrected, the absolute accuracy of the manipulator hand can be improved.

[Brief description of drawings]

FIG. 1 is a basic principle diagram of a joint angle control method for a manipulator according to the present invention.

FIG. 2 is an explanatory diagram of joint gain length determination according to the present invention.

FIG. 3 is an explanatory diagram of joint gain length determination according to the present invention.

FIG. 4 is a diagram showing a relationship between a joint angle error and a hand end error.

FIG. 5 is a configuration diagram of a joint angle control method for a manipulator according to an embodiment of the present invention.

FIG. 6 is a block diagram of manipulator position control in the same embodiment.

FIG. 7 is a flowchart showing a processing operation in the embodiment.

[Explanation of symbols]

 1 6-axis manipulator 2 Position measuring device 3 Visual sensor controller 4 Control amplifier 5 Joint angle detector 6 Control computer 7 Reference coordinate 8 Manipulator coordinate 9 Manipulator hand 20 Jacobian matrix 21 Manipulator current position and posture

Claims (1)

[Claims] 1. A position and orientation of a manipulator hand or a position control point measured by an arbitrary measuring instrument, and a position and orientation of a manipulator hand calculated by an angle sensor installed in each joint of the manipulator and a link length between each joint. In order to reduce the difference in the joint angle correction amount using the Jacobian matrix that shows the relationship between the position and orientation of the manipulator hand and the joint angle, and to further weight the correction amount, it is set in advance by the link length from each joint to the manipulator hand. A joint angle control method for a manipulator, which comprises: multiplying a joint link length gain to derive a joint angle correction amount for each manipulator, performing the correction, and repeating the correction to fix the offset amount of the manipulator joint.