JPH0464563B2 - - Google Patents

Abstract

PURPOSE:To detect an error in the link length of a parallel link as an error in angle of rotation and to easily take a measurement with high accuracy by measuring the angle of rotation of the stationary node of the parallel link and the angles of rotation of an input and an output node to a reference surface. CONSTITUTION:The output node LP3 of the parallel link mechanism consisting of the stationary node L2, input node LP1, intermediate node LP2, and output node LP3 is set to a specific angle theta3 of rotation. An inclinometer 23 finds the tilt angle gamma of the output node LP3 and the tilt angle phi of the reference surface and computes an angle psi(=gamma-phi) of rotation of the output node LP3 to the reference surface. Then, the output angle theta3 when there is no error is found from the angle m3 and the angle theta2 of rotation of the stationary node L2. The angle m3 of the input node LP1 to the reference surface when there is no error is computed from the output angle theta3 and the angle theta2 of rotation of the stationary node L2. Then, the angle m3 of rotation and the actual angle m'3 of rotation of the input node LP1 to the reference surface are used to find an error in the angle of rotation of the input node LP1 to the reference angle m'3 to the reference surface when the output node LP3 is at the specific angle theta3 of rotation.

Description

DETAILED DESCRIPTION OF THE INVENTION (Industrial Application Field) The present invention relates to a device for inspecting a parallel link mechanism used in a robot arm, for example, and in particular to an apparatus for inspecting a parallel link mechanism that measures joint angle errors based on link length errors. Concerning improvements to equipment.

(Background Art) As a robot arm using a parallel link mechanism, for example, there is a conventional art proposed by the present applicant in Japanese Utility Model Application No. 124388/1983.

FIG. 5 is a configuration diagram of a conventional vertically articulated robot, where A is a side view and B is a front view. In the figure, 1 is a horizontal rotation motor, 2 is a fixed plate that rotates horizontally by this horizontal rotation motor 1,
Reference numerals 3 and 4 are pulse motors fixed to the fixed plate 2 and arranged so that their rotation axes centers coincide with each other; 5 is a first link driven by one of the pulse motors 3 to rotate; 6 is a first link; An upper arm portion 7 is driven and rotated by the other pulse motor 4; 7 is a forearm portion whose middle portion is engaged with the tip of this upper arm portion 6; 8 is a portion between one end of this forearm portion 7 and the first upper arm portion 7; This is the second link that connects the tip of the link 5. Here, these first and second links, a portion of the upper arm portion, and a portion of the forearm portion constitute a parallelogram link mechanism as shown in the figure. 9
is a hand.

The rotation angles shown are as follows: θ ₁ is the hip rotation angle caused by the horizontal rotation motor 1, θ ₂ is the shoulder rotation angle caused by the pulse motor 4, θ ₃ is the elbow rotation angle caused by the pulse motor 3, and θ ₄
is the wrist twist angle, θ ₅ is the wrist bend angle, and θ ₆ is the hand rotation angle.

In the vertical articulated robot configured in this way, the fixed plate 2 is rotated horizontally by the horizontal rotation motor 1, and with respect to the fixed plate 2,
The upper arm portion 6 is rotated to any angle by the pulse motor 4. The forearm portion 7 is driven via the first link 5 and the second link 8, and rotates around the engagement point A with the upper arm portion 6.

(Prior Art) FIG. 6 is an explanatory diagram of the control of a six-degree-of-freedom vertically articulated robot incorporating a parallel link mechanism. In the figure, L ₀ to L ₆ are links, which are provided between joints that produce rotation angles θ ₁ to _{θ 6} . LP ₁ to _{LP 3} are parallel links, and link L ₂ and parallel link LP ₂ are provided in parallel. G is the position and orientation of the hand 9.

In such a device, when controlling the elbow rotation angle θ ₃ , the angle m ₃ of the parallel link LP ₁ with respect to the reference plane is controlled according to equation (1).

m ₃ = θ ₂ + θ ₃ + π/2 (1) (Problem to be solved by the invention) However, if there is a difference in the length of opposite links of parallel links due to assembly errors or processing errors, (1 )
The relationship in the equation does not hold.

If the actual link length of the arm can be measured, it can be corrected by calculation using a correction formula, but this requires measurement with high precision (for example, if the length of parallel link LP ₁ is 210 mm, the elbow Rotation angle θ ₃
To measure with an accuracy of 2″, the link length should be 2μ
(It is necessary to measure with an accuracy of m) There is a problem.

The present invention solves these problems,
The object of the present invention is to realize a parallel link inspection device that easily and accurately measures joint angle errors due to link length errors of parallel links.

(Means for Solving the Problems) A first invention that achieves such an object includes a first rotation angle measuring means for measuring the rotation angle θ ₂ of a stationary node of a parallel link, and a first rotation angle measuring means for measuring a rotation angle θ 2 of a stationary node of a parallel link; a second rotation angle measuring means for measuring the rotation angle m^ ₃ with respect to the reference plane of the parallel link; a third rotation angle measuring means for measuring the rotation angle Ψ of the output node of the parallel link with respect to the reference plane;
and the output angle θ ₃ from the rotation angle of the third rotation angle measuring means.
a first computing unit that computes the rotation angle of the input node with respect to the reference plane when there is no error from the calculation result of the first computing unit and the rotation angle of the first rotation angle measuring means;
m ₃ , and from the calculation result of this second calculator and the rotation angle of the second rotation angle measuring means, when the output node is a predetermined rotation angle, the reference of the input node is determined. It consists of a third arithmetic unit that calculates the error α that occurs in the rotation angle with respect to the surface.

A second invention to achieve such an object includes a first rotation angle measuring means for measuring a rotation angle θ ₂ of a stationary node of a parallel link, and a rotation angle m ₃ of an input node of a parallel link with respect to a reference plane. a second rotation angle measuring means for measuring the rotation angle Ψ of the output node of the parallel link with respect to the reference plane;
and a first calculator for calculating an output angle θ ₃ without error from the rotation angle of the second rotation angle measuring means, and an output angle including an error from the rotation angle of the first and third rotation angle measuring means. A second computing unit that computes θ^ ₃ and the calculation results of the first and second computing units calculate the error that occurs in the rotation angle of the output node when the input node is at a predetermined rotation angle with respect to the reference plane. It consists of a third arithmetic unit that calculates β.

(Operation) In the first invention, the first rotation angle measuring means measures the rotation angle when a stationary joint of the parallel link, such as a robot arm, rotates. The second and third rotation angle measuring means measure the rotation angles of the input node and output node of the parallel link with respect to the reference plane. The second calculator obtains the output angle from the first calculator and calculates the rotation angle of the input node with respect to the reference plane when there is no error. The third arithmetic unit calculates an error occurring in the rotation angle of the input node of the parallel link from the calculated value and the measured value.

In the second invention, the first arithmetic unit calculates the output angle without error, and the second arithmetic unit calculates the output angle based on the actually measured value. The third arithmetic unit calculates an error occurring in the rotation angle of the output node of the parallel link from the calculated value and the measured value.

Example The present invention will be explained below using the drawings.

FIG. 1 is a block diagram showing the concept of the present invention. In the figure, link L ₂ configuring the parallel link mechanism
corresponds to the stationary clause, the parallel link LP ₁ corresponds to the input clause, LP ₂ corresponds to the intermediate clause, and LP ₃ corresponds to the output clause.
(described on page 7-145).

10 is a motor that rotates the stationary node _L2 , 11 is a motor that rotates the input node _LP1 , and 21 is a stationary node _L2.
22 is a rotation angle measuring device that measures the rotation angle m^ ₃ of the input node LP ₁ with respect to the reference plane, and 23 is a rotation angle measurement device _that measures the rotation angle Ψ of the output node LP ₃ with respect to the reference plane. Inclinometer used to determine, 3
0 is a joint angle control device that commands the rotation angle to the motor 10, and 40 is a rotation angle error that calculates the error that occurs between the command value of the joint angle control device 30 and the actual measurement values of the rotation angle measurement device 20 and the inclinometer 23. A measuring device measures the error that occurs in parallel links.

g is the direction of gravity, g is the angle between the horizontal plane and the reference plane on which the robot is placed, and γ is the angle between the output node LP ₃ and the horizontal plane, which is the output of the inclinometer 23.

FIG. 2 is an explanatory diagram of the error of the parallel link obtained by the present invention, and A is the error α that occurs in the rotation angle m ₃ of the input node LP ₁ with respect to the reference plane when the output node LP ₃ has a predetermined rotation angle θ ₃ . , B indicates the error β that occurs in the rotation angle θ ₃ of the output node LP ₃ when the input node LP ₁ has a predetermined rotation angle m ₃ with respect to the reference plane. In the figure, a broken line indicates an ideal state in which there is no error in the parallel links, and a solid line indicates a parallel link with errors. Furthermore, since the relational expression θ ₃ =f(m ₃ ) (2) determined by the shape of the link holds between the input angle m ₃ and the output angle θ ₃ , the error α and the error β have a one-to-one correspondence. have a relationship.

Here, regarding the error α, m^ ₃ = m ₃ + α (3) holds true. Here, m ₃ is the command value, and m^ ₃ is the actual measured value. Furthermore, since the error α is determined by the shape of the parallel link, it can be expressed as α=α(θ ₃ ) (4).

Next, regarding the error β, θ^ ₃ =θ ₃ +β (5) holds true. Here, _θ3 is the output angle corresponding to the command value _m3 , and θ^ ₃ is the actual output angle. Also, since the error β is determined by the input angle -(π/2+m ₃ -θ ₂ ), it can be expressed as β=β(m ₃ -θ ₂ ).

<Embodiment of the first invention> FIG. 3 is a configuration block diagram showing an embodiment of the first invention. In the figure, 21 is an encoder that measures the rotation angle θ ₂ of the stationary node L ₂ , and 22 is the input node LP ₁
An encoder 31 is an α compensation device that performs necessary compensation for the joint angle control device 30 based on the output of the rotation angle error measurement device ₄₀ .

R1 is a register that temporarily stores the output of the θ ₂ encoder 21, R2 is a register that temporarily stores the tilt angle γ of the output node LP ₃ , R3 is a register that temporarily stores the tilt angle of the reference plane, and registers R2 and R3 are the registers that temporarily store the tilt angle γ of the output node LP 3. Total 2
Store the output of step 3. R4 is the rotation angle θ ₃ of the output node LP ₃
R5 is a register that stores the rotation angle m ₃ with respect to the reference plane when there is no error in the input node LP ₁ , R6 is a register that temporarily stores the output of the m ₃ encoder 22, and R7 is a register that stores the rotation angle m 3 of the input node LP ₁ . This is a register that stores the error α and outputs it to the α compensation device 31.

ALU1 is an arithmetic unit that inputs the values of registers R1, R2, and R3, performs the calculation θ ₃ = γ−+π/2−θ ₂ (7), and outputs it to register R4. The arithmetic unit ALU3 inputs the values of registers R5 and R6, performs the calculation m ₃ = θ ₂ + θ ₃ −π/2 (8) and outputs it to register R5, and inputs the values of registers R5 and R6, α = m^ ₃ -m ₃ (9) This is a subtracter that performs the operation and outputs it to register R7.

The device configured in this manner operates as follows.

First, the output node LP ₃ is set to a predetermined rotation angle θ ₃ . The inclination angle γ of the output node LP ₃ and the inclination angle of the reference plane are determined by the inclinometer 23, and the rotation angle Ψ (=γ−) of the output node LP ₃ with respect to the reference plane can be calculated. The ALU 1 calculates equation (7) from the rotation angle Ψ and the rotation angle θ ₂ of the stationary node L ₂ to calculate the output angle θ ₃ when there is no error.

By ALU2, the output angle θ ₃ calculated by ALU1
and the rotation angle θ ₂ of the stationary node L ₂ by calculating equation (8) to calculate the rotation angle m ₃ of the input node LP ₁ with respect to the reference plane when there is no error. ALU3 has rotation angle m ₃ and actual rotation angle m^ ₃
Input , calculate equation (9), and store the error α in register R7.
send to

<Embodiment of the second invention> FIG. 4 is a configuration block diagram showing an embodiment of the second invention. In FIG. 4, parts having the same functions as those in FIG. 3 are given the same reference numerals and their explanations will be omitted. In the figure, 32 is a rotation angle error measuring device 4.
This is a β compensation device that performs necessary compensation for the joint angle control device 30 using an output of 0.

R8 is a register that temporarily stores the inclination angle γ' of the output node LP ₃ , R9 is a register that temporarily stores the inclination angle ' of the reference plane, R10 is a register that stores the output angle θ^ ₃ obtained by actual measurement, and R11 is the output This is a register that stores the error β of node LP ₃ and outputs it to the β compensator 32.

ALU4 inputs registers R1 and R6, performs the calculation θ ₃ =π/2+m ₃ −θ ₂ (10), and outputs it to register R4. ALU5 inputs the values of registers R1, R8, and R9. Then, θ^=γ'-'+π/2-θ ₂ (11) The arithmetic unit ALU6 inputs the values of registers R4 and R10 and outputs it to register R10, β=θ^ ₃ - This is a subtracter that performs the calculation θ ₃ (12) and outputs it to the register R11.

The device configured in this manner operates as follows.

First, the input node LP ₁ is set at a predetermined rotation angle m ₃ with respect to the reference plane. The ALU 4 calculates the output angle θ ₃ of the error-free designed output node LP ₃ using equation (10). On the other hand, the ALU 5 calculates the rotation angle Ψ'(=γ'-') of the output node LP ₃ with respect to the reference plane from the inclination angle γ' of the output node LP ₃ and the inclination angle of the reference plane, and calculates the rotation angle Ψ' (= γ'-') of the stationary node L _2. Rotation angle θ
₂
(11) using the actual output clause including errors.
Calculate the output angle θ^ ₃ of LP ₃ . The designed output angle θ ₃ and the actual output angle θ^ ₃ are input to the ALU 6, formula (12) is performed, and the error β is sent to the register R11.

<Other Examples> In addition, in FIGS. 3 and 4 above, the rotation angle Ψ of the output node LP ₃ with respect to the reference plane is calculated by using the inclinometer 23, but it is also possible to calculate the rotation angle by installing an encoder. You can ask for it. Also, encoders 21 and 22
It suffices if the angle of rotation can be measured; for example, an angle meter using a gyroscope may be used.

(Effects of the Invention) As explained above, the present invention has the following effects.

(A) Link length errors of parallel links are detected as rotation angle errors, allowing for highly accurate measurements.

(B) In the first invention, the rotation angle error is determined as the error α, and in the second invention, the rotation angle error is determined as the error β, so it may be selected depending on the application. Note that if either the error α or the error β is known, the other can be determined by calculation.

(C) When the inclinometer 23 is used as in the embodiment, measurement can be performed more easily than when an encoder is used.

[Brief explanation of the drawing]

Fig. 1 is a block diagram showing the concept of the invention, Fig. 2 is an explanatory diagram of parallel link error, Fig. 3 is a block diagram showing an embodiment of the first invention, and Fig. 4 is an implementation of the second invention. FIG. 5 is a block diagram showing an example of the configuration of a conventional vertically articulated robot, and FIG. 6 is an explanatory diagram showing control of a robot incorporating a parallel link mechanism. 21...first rotation angle measuring means (θ ₂ ), 22...
...Second rotation angle measuring means ( _m3 ), 23...Third rotation angle measuring means (Ψ), ALU1-6...Arithmetic unit.

Claims (1)

[Claims] 1. A first rotation angle measuring means for measuring a rotation angle θ ₂ of a stationary node of a parallel link; and a rotation angle of an input node of a parallel link with respect to a reference plane.
a second rotation angle measuring means for measuring m^ ₃ ; and a rotation angle Ψ of the output node of the parallel link with respect to the reference plane.
a third rotation angle measuring means for measuring the angle of rotation; a first calculating unit calculating an output angle θ ₃ from the rotation angles of the first and third rotation angle measuring means; and a calculation result of the first calculating unit. and a second arithmetic unit that calculates the rotation angle m ₃ of the input node with respect to the reference plane when there is no error from the rotation angle of the first rotation angle measuring means; and the calculation result of this second arithmetic unit and the second arithmetic unit. A parallel link inspection device comprising a third computing unit that calculates an error α occurring in the rotation angle of the input node with respect to a reference plane when the output node has a predetermined rotation angle from the rotation angle of the rotation angle measuring means. 2. A first rotation angle measuring means for measuring the rotation angle θ ₂ of the stationary node of the parallel link, and the rotation angle of the input node of the parallel link with respect to the reference plane.
a second rotation angle measuring means for measuring m ₃ and a rotation angle Ψ of the output node of the parallel link with respect to the reference plane;
a third rotation angle measuring means for measuring the angle of rotation; a first calculating unit for calculating an output angle θ ₃ when there is no error from the rotation angles of the first and second rotation angle measuring means; a second computing unit that computes an output angle θ^3 including an error from the rotation angle of the rotation angle measuring means of No. 3; and a second computing unit that computes an output angle θ^ ₃ including an error from the rotation angle of the rotation angle measuring means of No. 3; A parallel link inspection device comprising a third arithmetic unit that calculates an error β that occurs in the rotation angle of an output node when the rotation angle is .