JPH0630007B2 - Robot position detector - Google Patents

Description

Description: TECHNICAL FIELD The present invention relates to a robot position detecting device, and more specifically, to each freedom of a robot having two or more degrees of freedom and interference between the degrees of freedom. The present invention relates to a robot position detection device that detects the position of degree.

“Prior Art and Its Problems” FIG. 1 shows a general articulated robot system, which is basically composed of an articulated robot 1, a control device 2, and a remote controller 3.

The wrist portion of the articulated robot 1 has three degrees of freedom, a first twist, a bend, and a second twist, and its power transmission mechanism is, for example, a power transmission mechanism 5 as shown in FIG. Is used.

In this power transmission mechanism 5, the first degree of freedom θ of the twist
₄ is the degree of freedom that the first axis L ₁ changes the axial direction by the motor M ₄ , the speed reducer G _4, and the drive pipe P ₄ .

The bending degree of freedom θ ₅ is such that the second axis L ₂ turns around the first axis L _{1 by} the motor M ₅ , the speed reducer G ₅ , the drive pipe P ₅ , the bevel gears B ₁ and B ₂ and the frame F. Is the degree of freedom

Further, the second degree of freedom θ ₆ of the twist has a motor M ₆ , a speed reducer G ₆ , a drive pipe P ₆ , a bevel gear B ₃ , B ₄ , and B _5.
Is the degree of freedom with which the second axis L ₂ is rotated.

The motors M ₄ , M ₅ , M _{6 have} speed detectors R ₄ , R ₅ , R ₆ for detecting speeds and angle detectors E ₄ , E ₅ , E for detecting angular positions on the respective axes. ₆ is attached.

Now, considering the case where the motors M ₅ and M ₆ are stopped and only the motor M ₄ is rotated, as described above, the motor M ₄ is rotated.
_The degree of freedom θ ₄ of the first twist changes in accordance with the rotation of ₄ .
That is, the first axis L ₁ changes the axial direction. Then, the bevel gear B ₂ moves accordingly. However, the bevel gear B ₁ that meshes with the bevel gear B ₂ is the motor M ₅
Is stopped because it is stopped, which causes the bevel gear B ₂ to rotate. Then, Bevel Gear B
A rotation of ₂ causes the frame F to rotate about the first axis L ₁ . This is nothing but a change in the bending degree of freedom θ ₅ .

Similarly, even though the bevel gear B ₃ is stopped by stopping the motor M ₆ , the bevel gear B ₄ is moved along with the change in the first twist θ ₄ , so that the bevel gear B ₃ is moved.
₄ is rotated, the bevel gear B ₅ meshing with this is rotated, and the second shaft L ₂ is rotated. This is the second
This is nothing but the change in the degree of freedom θ ₆ of the twist.

Similarly, when the motor M ₅ is driven to change the bending degree of freedom θ ₅ , the bevel gear B is stopped even if the motor M ₆ is stopped.
₅ rotates, and the second axis L ₂ rotates. This is nothing but a change in the degree of freedom θ ₆ of the second twist.

As described above, in a robot having two or more degrees of freedom, if one degree of freedom is changed, the other degrees of freedom may be changed due to interference.

By the way, the angle detectors E ₄ , E ₅ , E ₆ are of the incremental type, and the motors M ₄ , M ₅ ,
Since it is generally mounted on each axis of M ₆ , their detected values represent the relative rotational angular positions of the motors M ₄ , M ₅ , and M ₆ . However, because of the interference as described above, the rotational angular positions of the motors M ₄ , M ₅ , and M ₆ do not necessarily correspond to the angular positions of the respective degrees of freedom.

Therefore, when there is interference between the degrees of freedom, there is a problem that the detection amount of the angle detector does not represent the angle of each degree of freedom.

[Object of the Invention] An object of the present invention is to perform a calculation in consideration of the interference in a robot having two or more degrees of freedom and interference between the degrees of freedom to determine the position of each degree of freedom. An object of the present invention is to provide a robot position detecting device which can obtain the actual position of each degree of freedom from the detection amount of the detector.

"Structure of the Invention" The robot position detecting device of the present invention is the first to the Nth.
A position detector provided corresponding to each degree of freedom up to (N is an integer of 2 or more) and outputting a position detection amount D _i representing the relative position of a drive source for driving each degree of freedom. m
A calculation for correcting the position detection amount D _m for the m-th degree of freedom is performed based on the amount of interference received by the m-th degree of freedom due to the movement of the degrees of freedom other than the m-th degree of freedom. A position calculator that obtains an absolute position Θ _m for
Also, the structure is characterized in that it is provided with an output unit for outputting the absolute position Θ _i for each degree of freedom as position data of each degree of freedom.

[Examples] Hereinafter, the present invention will be described in more detail based on the examples shown in the drawings. FIG. 1 is an external view of a robot system including an embodiment of the robot position detecting device of the present invention, and FIG. 2 is a schematic mechanical diagram of a drive mechanism of a wrist portion of the articulated robot shown in FIG. FIG. 3 is a block diagram of a control system of the robot system shown in FIG. 1, and FIG. 4 is a block diagram of a configuration example of an interference correction calculator in the control system shown in FIG.
FIG. 7 is a block diagram of a configuration example of a joint angle calculation unit of the control system shown in FIG. The present invention is not limited to the embodiments shown in the drawings.

The robot system shown in FIG. 1 is an articulated robot 1
The control device 2 and the remote controller 3 are basically included.

The articulated robot 1 has six degrees of freedom of the theta ₁ through? _6, of which theta ₁ through? ₃ are degrees of freedom of the arm, theta ₄ through?
₆ is the degree of freedom of the wrist.

The drive mechanism of the wrist is as shown in FIG.
The first degree of freedom θ ₄ does not interfere with the other degrees of freedom θ ₅ and θ ₆ , the bending degree of freedom θ ₅ interferes with the first degree of freedom θ ₄ , and the second degree of freedom θ ₄ The degree of freedom θ _{6 of} is subject to interference from both the first twist degree of freedom θ ₄ and the bending degree of freedom θ ₅ . The description of this drive mechanism is the same as that described in detail in the section of "Prior art and its problems", and thus will not be repeated.

The control device 2 has a built-in computer, and has a function of performing interference correction to control the robot 1 and performing arithmetic processing according to the present invention to calculate the joint angle of each degree of freedom from the detection amount of the angle detector. It is different from the conventional one in that it has the function to obtain.
The configuration other than these points is the same as the conventional one.

The remote controller 3 has the same configuration as the conventional one.

FIG. 3 is a control system diagram of the robot system shown in FIG. 1. The left side portion of the chain double-dashed line is provided in the control device 2, and the right side portion of the chain double-dashed line is provided in the robot 1.

When the operator teaches the position (x, y, z) and the posture (l, m, n) of the hand portion via the control panel _2a or the remote controller 3, these teaching data are obtained.
By the forward conversion calculation unit 21, the degrees of freedom θ ₁ , θ ₂ , θ ₃ ,
Target angular position φ that θ ₄ , θ ₅ , and θ ₆ should achieve respectively
It is converted into ₁ , φ ₂ , φ ₃ , φ ₄ , φ ₅ , and φ ₆ .

Also, at the time of reproduction, the stored data (x,
y, z, l, m, n) are similarly transformed.

The forward conversion calculation unit 21 has the same configuration as a conventionally known configuration.

Next, target angular positions φ ₁ , φ ₂ , φ ₃ , φ ₄ , φ ₅ , φ
₆ is target control amounts s ₁ , s ₂ , s ₃ , s ₄ , s ₅ , to be given to the servo mechanism by the angle / control amount conversion unit 22.
respectively converted to s ₆ .

The angle / control amount conversion unit 22 does not consider interference, and, for example, the conversion coefficient for each degree of freedom is C
_{When 1} , C ₂ , C ₃ , C ₄ , C ₅ , and C ₆ , s _i = φ _i / C _i + s _io (i = 1,2,3,4,5,6) (s _io is Constant) is performed.

Next, with respect to these target control amounts s _{1 to} s ₆ , the interference correction calculation unit 23 performs a correction for canceling the interference, and the control amounts d _{1 to} d actually given to the servo mechanism of each degree of freedom. Calculate ₆ .

For example, the three degrees of freedom θ ₄ , θ ₅ , and θ ₆ of the wrist will be described. First, the first degree of freedom θ ₄ of the twist does not receive interference from other degrees of freedom, and therefore the correction amount is 0. , As shown in FIG. ₄ , s ₄ = d ₄ .

Next, the bending degree of freedom θ ₅ is the first twisting degree of freedom θ ₄
Therefore, if the interference amount is θ ₄₅ , the correction amount h ₄₅ that cancels the θ ₄₅ is subtracted from s ₅ to obtain the actual control amount d ₅ .

For example, if the interference amount θ ₄₅ is proportional to the driving amount (d ₄ −s ₄₀ ) of the first degree of freedom θ ₄ of the twist, θ ₄₅ = (d ₄ −s ₄₀ ) α with the proportional constant being α _{45. 45,} and the correction amount _{h 45} to drive the motor _{M 5} to counteract this theta ₄₅ is a proportional constant as beta _{45, h 45}
= Θ ₄₅ β ₄₅ , therefore, if the proportional constants are summarized as k ₄₅ , then h ₄₅ = (d ₄ −s ₄₀ ) k ₄₅ .

Therefore, as shown by the calculation unit 23 _a in FIG. 4, d ₅ = s ₅ − (d ₄ −s ₄₀ ) k ₄₅ .

Next, the degree of freedom θ ₆ of the second twist is subject to the interference of the degree of freedom θ ₄ of the first twist and the degree of freedom of bending θ ₅ , so that the interference amounts θ ₄₆ and θ ₅₆ due to them are canceled out. The correction amounts h ₄₆ and h ₅₆ of only _{6 are} subtracted from s ₆ to obtain the actual control amount d ₆ .

As in the case described above, since h ₄₆ = (d ₄ −s ₄₀ ) k ₄₆ h ₅₆ = (d ₅ −s ₅₀ ) k ₅₆ , as shown by the calculation unit 23 _b in FIG. 4, d ₆ = S ₆ − (d ₄ −s ₄₀ ) k ₄₆ − (d ₅ −s ₅₀ ) k ₅₆ .

The actual control amount d _i thus obtained is the feedback control unit 24.
Is output to.

Feedback control unit 24, based on the deviation of the controlled variable _d 1 to d ₆ and the angle detected amount _D 1 to D ₆ of the angle detector _E 1 to E ₆ of each degree of freedom, to the servo amplifier _A 1 to A ₆ Output drive signal.

The servo amplifiers A _{1 to} A ₆ include the speed detectors R _{1 to} R _{6 respectively.}
The servo motors M _{1 to} M ₆ are driven while receiving the feedback of.

Thus, the control in which the interference between the degrees of freedom is corrected is performed, and the degrees of freedom θ _{1 to} θ ₆ can achieve the target angular positions φ _{1 to} φ ₆ , respectively, without being influenced by the interference. Become.

Now, the angle detection amount D _i output by the angle detector E _i represents the amount by which the servomotor M _i is actually driven, and does not represent the angular position of each degree of freedom because of interference. These are detected as the angular position Θ _i only after being converted by the joint angle calculation unit 25.

That is, the joint angle calculation unit 25 transforms the above equation for the degree of freedom θ _q that does not receive interference from other degrees of freedom, and thereby transforms the following equation: θ _q = C _q (D _q −s _q0 ) ... (q is number) (s _q0 of freedom without receiving interference from other performs constant) becomes operational, detecting the absolute angular position theta _q.

For example, the degree of freedom θ ₄ of the first twist of the wrist is not affected by interference from other degrees of freedom, and therefore the calculation unit 25 _{a is} shown in FIG.
As shown in, the angular position Θ ₄ is detected as Θ ₄ = C ₄ (D ₄ −s ₄₀ ).

This angular position Θ ₄ is an angular position achieved only by rotation of the servomotor M ₄ .

On the other hand, with respect to the degree of freedom θ _p that receives interference from other degrees of freedom, the angle position Θ _p is detected by performing an operation of adding an angle change amount due to the interference.

For example, the angular position Θ ₅ achieved with the bending degree of freedom θ ₅ of the wrist is at the angular position Θ ₅₅ produced by its own servomotor M _{5 and} the servomotor M ₄ with the first degree of freedom θ ₄ of the servomotor M ₄ . This is the addition of the angle change Θ ₄₅ caused by the interference due to the rotation.

That is, Θ ₅ = Θ ₅₅ + Θ ₄₅ ... Here, according to the above equation, Θ ₅₅ = C ₅ (D ₅ −s ₅₀ ), and Θ ₄₅ is the actual driving amount of the first degree of freedom θ ₄ of the twist. If it is proportional to (D ₄ −s ₄₀ ), the proportionality constant is K ₄₅ , and θ ₄₅ = K ₄₅ (D ₄ −s ₄₀ ) ... Therefore, as shown by the calculation unit 25 _b in FIG. Then, the angle position Θ ₅ is detected by performing the calculation of Θ ₅ = C ₅ (D ₅ + s ₅₀ ) + K ₄₅ (D ₄ −S ₄₀ ).

Next, the angular position Θ ₆ achieved in the second twist degree of freedom θ ₆ is subject to the interference due to the first twist degree of freedom θ ₄ and the bending degree of freedom θ ₅ and, therefore, due to the interference. The angle changes Θ ₄₆ and Θ ₅₆ are added to the self achievement Θ ₆₆ .

_{That, Θ 6 = Θ 66 + Θ} 46 + Θ 56 ... _{where, Θ 66 = C 6 (D} 6 -s 60) Θ 46 = K 46 (D 4 -s 40) Θ 56 = K 56 (D 5 -s 50 Therefore, as shown by the calculation unit _25c in FIG. 5, Θ ₆ = C ₆ (D ₆ −s ₆₀ ) + K ₄₆ (D ₄ −s ₄₀ ) + K ₅₆ (D ₅ −s ₅₀ ). The angular position Θ ₆ is detected by performing the following calculation.

Thus, the position detection amount _D 1 to D ₆ of the output of the angle detector _E 1 to E _6, actually achieved angular position theta ₁ through? ₆
Will be detected.

The detected angular positions Θ _{1 to} Θ ₆ are converted into positions (x, y, z) and postures (l,
m, n) and stored in memory.

The inverse conversion calculation unit 26 has the same configuration as a conventionally known configuration.

As can be understood from the above description, the joint angle calculation unit 25 according to the present invention does not depend on the interference from the position detection amounts D _{1 to} D ₆ output by the angle detectors E _{1 to} E ₆ and actually detects them. The achieved angular positions Θ _{1 to} Θ ₆ can be detected.

Further, the interference correction calculator 23 can control the articulated robot 1 without being affected by interference between the degrees of freedom.

As another embodiment, the angle / control amount conversion unit 22 and the interference correction calculation unit 23 are combined into one like the joint angle calculation unit 25, and the angle / control amount conversion unit 22 and the interference correction calculation unit 23 are directly combined from the target angular positions φ _{1 to} φ _6. The control amounts d _{1 to} d ₆ of the above are calculated.

In another embodiment, the joint angle calculation unit 25 is replaced by the joint angle calculation unit 25 such as the angle / control amount conversion unit 22 and the interference correction calculation unit 23.
The angle detection amounts D _{1 to} D ₆ are _first converted into angular positions when there is no interference, and then interference correction is performed to calculate the angular positions Θ _{1 to} Θ ₆ .

[Effect of the Invention] According to the present invention, the relative degrees of the drive sources provided corresponding to the degrees of freedom from the 1st to the Nth (N is an integer of 2 or more) and driving the respective degrees of freedom. Position detection amount D _i representing the position
Based on the amount of interference received by the m-th degree of freedom due to movement of a degree of freedom other than the m-th degree of freedom, the position detector outputs D for the m-th degree of freedom.
_A position calculation unit that performs an operation to correct _m and obtains an absolute position Θ _m for the m-th degree of freedom, and an absolute position Θ _i for each degree of freedom are output as position data of each degree of freedom. Provided is a robot position detecting device characterized by comprising an output part, whereby a robot having a plurality of degrees of freedom and having interference between the degrees of freedom can detect the position of each degree of freedom. It becomes possible to detect it appropriately without being influenced by

[Brief description of drawings]

FIG. 1 is an external view of a robot system including an embodiment of the robot position detecting device of the present invention, and FIG. 2 is a schematic mechanical diagram of a drive mechanism of a wrist portion of the articulated robot shown in FIG. FIG. 4 is a block diagram of a control system of the robot system shown in FIG. 1, FIG. 4 is a block diagram of a configuration example of an interference correction computing section in the control system shown in FIG. 3, and FIG. 5 is control shown in FIG. It is a block diagram of an example of 1 composition of a joint angle calculation part of a system. (Explanation of Codes) 1 ... Articulated robot 2 ... Control device 3 ... Remote controller 5 ... Wrist drive mechanism 23 ... Interference correction calculation unit 23a, 23b ... Calculation unit 25 ... Joint angle calculation unit 25a, 25b, 25c ... Computational section θ ₄ ... degree of freedom of first twist of wrist section θ ₅ ... degree of freedom of bending of wrist section θ ₆ ... degree of freedom of second twist of wrist section M ₄ , M ₅ , M ₆ ... Servomotor E ₄ , E ₅ , E ₆ ... Angle detector.

Claims (1)

[Claims] 1. A relative position of a drive source provided for each degree of freedom from (a) 1st to Nth (N is an integer of 2 or more) and driving each degree of freedom. Position detection amount D _i
(B) The position detector that outputs the
A position calculation unit that performs a calculation to correct the position detection amount D _m for the m-th degree of freedom based on the amount of interference received by the th-degree of freedom, and obtains an absolute position Θ _m for the m-th degree of freedom. And (c) a robot position detecting device comprising an output unit for outputting the absolute position Θ _i for each degree of freedom as position data of each degree of freedom.