JPH0682286B2 - Robot controller - Google Patents

Description

TECHNICAL FIELD The present invention relates to a robot controller, and more particularly, to a robot controller.
The present invention relates to a robot control device that controls interference corrected for a robot having the above degrees of freedom and interference between the degrees of freedom.

“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 a motor M ₄ , a reducer G ₄ and a drive pipe P ₄ ,
The first axis L ₁ is the freedom to change the axial direction.

The bending degree of freedom θ ₅ is motor M ₅ , reduction gear G ₅ , drive pipe
P _5, the bevel gear B _1, B ₂ and the frame F, a degree of freedom of the second axis L ₂ is pivoted to the first about an axis L _1.

In addition, the second degree of freedom θ ₆ of the twist is the motor M ₆ and the reduction gear.
This is the degree of freedom in which the second shaft L ₂ is rotated by G ₆ , the drive pipe P ₆ , and the bevel gears B ₃ , B ₄ , and B ₅ .

Each axis of the motors M ₄ , M ₅ , M ₆ has speed detectors R ₄ , R ₅ , R ₆ for detecting speed and angle detectors E ₄ , E ₅ , E for detecting angular position. ₆ 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 first degree of freedom θ ₄ of the first twist changes according to the rotation of the motor M ₄ . That is, the first axis L ₁ changes the axial direction. Then, the bevel gear B ₂ moves accordingly. However, the bevel gear B ₁ meshing with the bevel gear B ₂ is stopped because the motor M ₅ is stopped, and therefore the bevel gear B ₂ is rotated. Then, the rotation of the bevel gear B ₂ causes the frame F to rotate about the first axis L ₁ . This is nothing but a change in the bending degree of freedom θ ₅ .

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

Similarly, when the motor M ₅ is driven to change the bending degree of freedom θ ₅ , the bevel gear B ₅ rotates and the second shaft L ₂ rotates even if the motor M ₆ is stopped. 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, when one degree of freedom is changed, another degree of freedom is interfered with and changed.

"Object of the Invention" The object of the present invention is to make a robot that has two or more degrees of freedom and has interference between the degrees of freedom, by performing correction to cancel the interference so that there is no interference. It is to provide a robot control device capable of controlling a robot.

[Constitution of the Invention] The robot controller of the present invention introduces the target control amount s _i for each degree of freedom from the 1st to the Nth (N is an integer of 2 or more) based on the teaching data. When a servo mechanism having a degree of freedom other than the m-th degree and the above-mentioned degree of freedom is driven in obtaining the control amount d _m of the m-th degree of freedom, the other degree of freedom and the m-th degree are calculated. The degree of freedom that influences the m-th degree of freedom is selected in advance by the transmission mechanism that is interposed between the second degree of freedom and the other degree of freedom, which is obtained by the target controlled variable introducing unit. Target control amount
A control amount calculation unit obtained by correcting the target control amount s _m for the m-th degree of freedom using s _i and calculating the actual control amount d _m for the m-th degree of freedom, and each degree of freedom Is provided with an output unit for outputting the controlled variable d _i for the servo mechanism of each degree of freedom.

[Examples] Hereinafter, the present invention will be described in more detail based on the examples shown in the drawings. Here, FIG. 1 is an external view of a robot system including an embodiment of the robot controller of the present invention, and FIG.
FIG. 3 is a schematic mechanical diagram of a drive mechanism for the wrist of the articulated robot, FIG. 3 is a block diagram of the control system of the robot system shown in FIG. 1, and FIG. 4 is interference in the control system shown in FIG. FIG. 5 is a block diagram of a configuration example of the correction calculation unit, and FIG. 5 is a block diagram of a configuration example of the 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 θ ₄ is not affected by the interference from the other degrees of freedom θ ₅ and θ ₆ , the bending degree of freedom θ ₅ is interfered by the first degree of freedom θ ₄ and the second degree of freedom θ ₄ is Twisting degree of freedom θ ₆
Receives interference from both the first twist degree of freedom θ ₄ and the bending degree of freedom θ ₅ . The relationship between the degrees of freedom of mutual interference is known, and the degree of freedom that causes interference for a predetermined degree of freedom is specified in advance from this relationship.
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 the interference correction according to the present invention to control the robot 1 and arithmetic processing to determine the joint angle of each degree of freedom from the detection amount of the angle detector. It differs from the conventional one in that it has the function to obtain.
The configuration other than those 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.

The operator controls the control panel _2a or the remote controller 3
When teaching the position (x, y, z) and the posture (l, m, n) of the hand part via the, the teaching data or the stored data at the time of reproduction can be freely changed by the forward conversion calculation unit 21. The target angular positions φ ₁ , φ ₂ , φ ₃ , φ ₄ , φ ₅ , φ _{6 that the} degrees θ ₁ , θ ₂ , θ ₃ , θ ₄ , θ ₅ , and θ ₆ should achieve, respectively.
Is converted to.

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

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

The angle / control amount conversion unit 22 does not consider interference. For example, when conversion coefficients for each degree of freedom are C ₁ , C ₂ , C ₃ , C ₄ , C ₅ , and C ₆ , s _i = Φ _i / C _i + s _i0 (i = 1,2,3,4,5,6) (s _i0 is a constant).

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. , S ₄ = d ₄ as shown in FIG.

Next, the bending degree of freedom θ ₅ is the first twisting degree of freedom θ ₄
Since it receives the interference due to, if the interference amount is θ ₄₅ ,
The correction amount h ₄₅ enough to offset the theta ₄₅ is the actual control quantity d ₅ is subtracted from the s _5.

For example, if the interference amount θ ₄₅ is proportional to the drive amount (d ₄ −s ₄₀ ) of the first degree of freedom θ ₄ of the twist, θ ₄₅ = (d ₄ −s ₄₀ ) α, where α ₄₅ is the proportional constant. ₄₅ , and the correction amount h ₄₅ that should drive the servomotor M ₅ to cancel this θ ₄₅ is h ₄₅ = θ ₄₅ with β ₄₅ as the proportional constant.
Since β ₄₅ is obtained, h ₄₅ = (d ₄ −s ₄₀ ) k ₄₅ if the proportional constant is put together as k ₄₅ .

Therefore, as shown in the calculating portion 23 _a in Figure _{4, d 5 = s 5 - (} d 4 -s 40) is k ₄₅ ... and.

Next, since the second degree of freedom θ ₆ is subject to the interference of the first degree of freedom θ ₄ and the bending degree of freedom θ ₅ , the interference amounts θ ₄₆ and θ ₅₆ due to them are canceled out. Only the correction amount h ₄₆ and h ₅₆ are subtracted from s ₆ and the actual control amount d
It is considered to be ₆ .

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

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

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

The servo amplifiers A _{1 to} A ₆ drive the servo motors M _{1 to} M ₆ while receiving the feedback from the speed detectors R _{1 to} R ₆ .

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 servo motor M _i is actually driven,
Due to interference, the angular position of each degree of freedom is not shown.
These are first detected as the angular position Θ _i by 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 is not affected by interference from other degrees of freedom, and thus θ _q = C _q (D _q −s _q0 ) ... (q is The number of degrees of freedom that does not interfere with others) (s _q0 is a constant) is calculated, and the angular position Θ _q is detected.

For example, since the first degree of freedom θ ₄ of the wrist of the wrist is not interfered with from the other degrees of freedom, as shown by the calculation unit 25 _a in FIG. 5, Θ ₄ = C ₄ (D ₄ − The angular position Θ ₄ is detected as 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 wrist is the angular position theta ₅ achieved with freedom theta ₅ bend, the angular position theta ₅₅ caused by the servomotor M ₅ self, the first twist of freedom theta ₄ of the servomotor M ₄ This is the addition of the angle change Θ ₄₅ caused by interference due to 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 to be proportional to (D ₄ -s _40), the proportional constant as _{K 45, Θ 45 = K 45} (D 4 -s 40) ... because made, after all, as shown in the calculating portion 25 _b in FIG. 5 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 is, Θ ₆ = Θ ₆₆ + Θ ₄₆ + Θ ₅₆ ... where Θ ₆₆ = C ₆ (D _{6 −} s ₆₀ ) Θ ₄₆ = K ₄₆ (D _{4 −} s ₄₀ ) Θ ₅₆ = K ₅₆ (D _{5 −} s ₅₀ Therefore, as shown in the calculation unit 25 _c 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 ₁ ~ output from the angle detectors E ₁ ~ E ₆
From D ₆ , the actually achieved angular positions Θ _{1 to} Θ ₆ will be detected.

The detected angular positions Θ _{1 to} Θ ₆ are converted into the position (x, y, z) and the posture (l, m, n) of the hand portion by the inverse conversion calculation unit 26 and stored in the memory.

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

As can be understood from the above description, the interference correction calculator 23 according to the present invention does not depend on the interference between the degrees of freedom,
The articulated robot 1 can be controlled.

Incidentally, the joint angle computing unit 25, the position detection amount D ₁ to D ₆ of the output of the angle detector E ₁ to E _6, irrespective of the interference, the angular position theta ₁ through? ₆ which are actually achieved Can be detected.

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 target angle positions φ _{1 to} φ _{6 are} directly used for actual measurement. Control amount of
An example is _{one in which} d _{1 to} d ₆ are calculated.

In another embodiment, the joint angle calculator 25 is set to
Like the 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 the interference correction is performed to perform angular position correction. Θ _{1 to} Θ
An example is one in which ₆ is calculated.

[Advantageous Effects] According to the present invention, a target control amount introduction unit that obtains the target control amount s _i of each of the first to N-th (N is an integer of 2 or more) degrees of freedom based on teaching data, When obtaining the control amount d _m of the m-th degree of freedom, if a servo mechanism having a degree of freedom other than the m-th degree and the above-mentioned degree of freedom is driven, the other degree of freedom and the m-th degree of freedom are calculated. The degree of freedom that affects the m-th degree of freedom is selected in advance by a transmission mechanism that is interposed between the degree of freedom and the degree of freedom, and the target obtained by the target control amount introduction unit for other preselected degrees of freedom. The target controlled variable s _m for the m-th degree of freedom is corrected using the controlled variable s _i ,
A control amount calculation unit obtained by calculating an actual control amount d _m for the m-th degree of freedom, and a control amount d _i for each degree of freedom
A robot controller is provided, which is characterized by comprising an output section for outputting to a servo mechanism of each degree of freedom, whereby a robot having a plurality of degrees of freedom and interference between the degrees of freedom is provided. Therefore, it is possible to perform suitable control without being affected by interference.

[Brief description of drawings]

FIG. 1 is an external view of a robot system including an embodiment of a robot controller of the present invention, FIG. 2 is a schematic mechanical view of a drive mechanism of a wrist portion of the articulated robot shown in FIG. 1, and FIG. 1 is a block diagram of a control system of the robot system shown in FIG. 1, FIG. 4 is a block diagram of an example of a configuration of an interference correction computing section in the control system shown in FIG. 3, and FIG. 5 is a control system shown in FIG. 3 is a block diagram of a configuration example of a joint angle calculation unit of FIG. (Explanation of reference numerals) 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 , 25 _b , 25 _c … Calculation section θ ₄ … First degree of freedom of twist of the wrist θ ₅ … Degree of freedom of bending of wrist θ ₆ … The degree of freedom of second twist of the wrist M ₄ , M ₅ , M ₆ … Servo motor E ₄ , E ₅ , E ₆ … Angle detector.

Claims (1)

[Claims] 1. A target control amount introducing section for obtaining a target control amount S _i for each degree of freedom from the 1st to the Nth (N is an integer of 2 or more) based on teaching data, (b) When obtaining the control amount d _m of the m-th degree of freedom, if a servo mechanism having a degree of freedom other than the m-th degree and the above-mentioned degree of freedom is driven, the other degree of freedom and the m-th degree of freedom are calculated. A degree of freedom that affects the m-th degree of freedom is selected in advance by a transmission mechanism that is interposed between the degree of freedom and the target degree of control, which is obtained by the target control amount introduction unit with respect to the other degree of freedom selected in advance. A control amount calculation unit obtained by correcting the target control amount s _m for the m-th degree of freedom using the amount s _i and calculating the actual control amount d _m for the m-th degree of freedom; and (c ) this the control amount d _i formed by having an output unit for outputting to each degree of freedom of a servo mechanism for each degree of freedom Robot control apparatus according to claim.