JPH0683972B2 - Master-slave system robot - Google Patents

Description

Description: TECHNICAL FIELD OF THE INVENTION The present invention relates to a master-slave system robot that moves a slave arm following the movement of a master arm and causes the slave arm to make a desired movement.

[Technical Background of the Invention and Problems Thereof] Generally, in a master-slave system robot, an operator mounts a master arm on an arm for operation, and follows the movement of the master arm to operate the slave arm. However, the movement of each joint of the master arm may not always accurately grasp the movement of the operator's arm, and the posture of the master arm and the posture of the slave arm do not exactly match, making the operation difficult. There is a problem that.

An object of the present invention is to solve such a problem, and to detect the detection output of the encoder on the master arm side when the master arm and the slave arm have the same basic posture in the teaching mode. Provide a master-slave system robot in which the movement of the slave arm is made closer to the movement of the operator's arm by finding the difference in the amount of movement of the drive unit of the slave arm and correcting the output of the encoder on the master arm side. The purpose is to

[Structure of the Invention] In order to achieve the above object, the present invention provides a master-slave system robot that moves a slave arm following the movement of a master arm, and the slave arm follows the movement of the master arm. Of the joint portions when the slave arm is moved from the first basic posture to the second basic posture when the teaching mode selection switch provided on the grip at the tip of the master arm is operated. An output of a first measuring means for measuring an amount and a number of pulses of an encoder of each joint when the master arm moves from a first basic posture to a second basic posture which are almost the same as those of the slave arm. The comparison means for calculating the ratio of the amount of movement to the number of pulses by comparing the output of the second measurement means and the comparison result of this comparison means are stored as a correction value. However, when the slave arm is made to follow the actual movement of the master arm, a correction means for determining the movement amount of the slave arm by correcting the output pulse number of the encoder of each joint part of the master arm is provided. The correction value storage unit is individually provided in each axis control unit that controls the drive unit of each joint of the slave arm.

Embodiments of the Invention Embodiments of the present invention will be described in detail with reference to the drawings. FIG. 2 shows a state in which the master arm 1 of one embodiment of the present invention is mounted on the operator 3, and FIG. 3 shows the entire slave robot 5, with left and right slave arms 7, 9 respectively.
Is equipped with. Although FIG. 2 shows a partial view in which the left arm of the operator 3 is equipped with the master arm 1 for the left arm, the right arm of the operator 3 is similarly equipped with the master arm for the right arm.

The master arm 1 is mounted so as to follow the movement of each joint of the arm 11 of the operator 3, and the shoulder joint 13 has a horizontal rotation (R ₁ ) detection encoder 15 and a torsion (R ₂ ) detection encoder. 17, it is equipped with an encoder 19 for detecting forward and backward rotation (R ₃ ). The elbow joint portion 21 of the master arm 1 is provided with a vertical rotation (V) detection encoder 23, and the wrist joint portion 25 is also provided with a vertical rotation (W) detection encoder 27. Further, the wrist joint portion 25 is provided with a grip 29, and as shown in FIG. 4, the grip 29 is provided with a master effective switch SW ₁ , a teaching mode selection switch SW ₂ , and a vertical drive switch SW _3. ing.

The left and right slave arms 7 and 9 of the slave robot 5 shown in FIG. 3 follow the same movements of the left and right master arms 1, respectively, and have shoulder joint parts 13 ₁ and elbow joint parts 21 _{1 respectively} . A horizontal rotation (R ₁ ) drive unit 15 ₁ , a twist (R ₂ ) drive unit 17 ₁ , a longitudinal rotation (R ₃ ) drive unit 19 ₁ , and a vertical rotation (V) drive unit, each of which is composed of a servo motor in the wrist joint unit 25 _1. 23 ₁ , and a vertical rotation (W) drive unit 27 ₁ .

The slave robot 5 has left and right slave arms 7,
It is provided with a vertical drive mechanism 31 for moving the 9 up and down at the same time, and a carriage 33 for moving forward and backward and turning.

The operation of the master-slave robot having the above configuration will be described. When the operator 3 moves the left and right arms 11 to a desired posture, the master arm 1 is moved according to the movement of the arms 11. By this movement of the master arm 1, encoders 15, 19, provided on the joints 13, 21, 25,
23 and 27 output a pulse according to the movement of each joint of the arm. The pulses of each encoder are counted, and the drive units 15 ₁ , 17 ₁ , 19 ₁ , 23 ₁ , 27 ₁ of the left and right slave arms 7, 9 of the slave robot 5 are driven by the same amount, and the posture taken by the operator 3 is taken. Move the left and right slave arms 7 and 9 so that they have the same shape as.

Here, since the master arm 1 is firmly fixed to the arm 11 of the operator 3, when the operator 3 unintentionally moves the arm 11 suddenly or when the operator 3 falls down,
The desired operation of the slave arms 7 and 9 will make a sudden movement that is far from the desired operation, and if such a situation occurs during the operation of the slave arms 7 and 9, danger will occur. If the slave arms 7 and 9 always follow the movement of the master arm 1, the operator 3 needs to always be aware of the position of his or her own arm 11, which causes a great deal of mental fatigue. There is also a problem that depending on the posture of 9, the operator is forced to have an unnatural posture for a long time.

Therefore, the grip 29 is equipped with a master effective switch SW ₁ ,
Only while the switch SW ₁ is being operated by the operator 3, the slave arms 7 and 9 operate so as to follow the incremental amount (increase amount) of the master arm 1. That is, as shown in FIGS. 5 and 6, the operator 3
Is moving the arm 11 and one encoder of the master arm 1 is outputting a pulse as shown in FIG. 6 (b), the slave arm of the slave robot 5 is still active when the effective switch SW ₁ is not in the ON state. 7 and 9 do not follow the movement of master arm 1. And enable switch SW ₁ is O
Only in the N state, the follow-up operation is performed according to the pulse output of the encoder of the master arm 1, as shown in FIG.

Moreover, in this case, the effective switch SW ₁ of the master arm 1 is turned off.
The slave arms 7 and 9 move in accordance with the incremental amount (increase amount) while it is N. In other words, as shown in FIG. 5, even when the slave arms 7 and 9 are lifted to a high position, the arm 11 of the operator 3 is moved in a low and comfortable position, and the arm 11 is slightly moved forward and backward to move the arm forward. Operate so that the valid switch SW ₁ is turned on only when sending. By this operation, the slave arms 7 and 9 can be raised to a high position, and the operator 3 can take a desired posture on the slave arms 7 and 9 while taking a comfortable posture.

In order to prevent danger, the slave arms 7 and 9 are controlled so as not to move at a speed equal to or higher than a predetermined set value even if the operator 3 makes a sudden arm movement.
If the robot moves at an abnormally high speed, it is possible to prevent the danger during operation by determining that it is in an emergency and stopping the slave arms 7 and 9 in an emergency.

That is, as shown in FIG. 7, even if the movement of the master arm 1 is rapid, if the movement speed is within the range that does not exceed the emergency stop speed Vst, the speeds of the slave arms 7 and 9 are the set maximum speeds. Control to move at Vmax. However, when the moving speed of the master arm 1 exceeds the emergency stop speed Vst as shown in FIG. 8, the slave arms 7 and 9 are brought to an emergency stop at that time T to avoid danger.

Furthermore, it is difficult to completely grasp the movement of the arm 11 of the operator 3 by the five encoders of the master arm 1 as shown in FIG. Therefore, the operator 3 moves the arm 11 to operate the master arm 1, and outputs pulses from the encoders 15, 17, 19, 23, and 27 of the master arm 1 to the drive units 15 ₁ of the corresponding slave arms 7 and 9, respectively. , 17 ₁ , 1
When the drive amount command value of 9 ₁ , 23 ₁ , 27 ₁ is used, there are the following problems. In other words, even if the master arm 1 is operated to move the arm 11 straight ahead, the shoulder joint portion 13 of the arm 11 actually moves in the form of a universal joint, while the axes of the encoders are slightly shifted. Because of this, it cannot move like a universal joint. Therefore, the movement that completely follows the movement of the arm 11 cannot be obtained.

Therefore, as shown in FIG. 9, the movement deviation between the master arm 1 and the slave arms 7 and 9 is corrected by teaching. For that purpose, first, the slave arms 7 and 9 are respectively placed in a first basic posture, for example, as shown in FIG.
₁ keeps the upper arm hanging vertically, and keeps the elbow joint 21 ₁ bent at a right angle.
Keep the first basic posture almost the same. Then, in this state, the teaching mode selection switch SW ₂ of the grip 29 is operated to assume that the encoders 15, 17, 19, 23, 27 of the master arm 1 are at the virtual origin.

Then move the slave arm 7,9 second reference position, for example, forward horizontally from shoulder joints 13 ₁ in a state in which the entire arm is extended. Then, the movement amount of each drive unit 15 ₁ , 17 ₁ , 19 ₁ , 23 ₁ , 27 ₁ of the slave arms 7 and 9 at that time is calculated by each axis control unit 35, 3
Give to 7,39,41,43.

Subsequently, the operator 3 moves the arm 11 to the second basic posture, and moves the master arm 1 to a posture substantially similar to the slave arms 7 and 9. And each encoder at this time
Similarly, output pulses of 15, 17, 19, 23, and 27 are output to each axis control unit 35, 37,
Give to 39,41,43.

In that state, operate the teaching mode selection switch SW ₂ again. As a result, the main control unit 45 controls the respective axis control units 35, 37,
The output pulse of the master arm 1 stored in 39, 41, 43 is compared with the feedback pulse of the movement amount from the slave arm 7, 9 and the ratio of the output pulses corresponding to each axis control unit is used as a correction value. give. Thus the main controller
The correction value of each axis calculated by 45 is stored in each axis control unit, and when the master arm 1 is actually driven, the output pulse from each encoder 15, 17, 19, 23, 27 of the master arm 1 is output. By multiplying the correction values, the movement amount of each corresponding drive unit 15 ₁ , 17 ₁ , 19 ₁ , 23 ₁ , 27 ₁ of the slave arm 7, 9 is determined,
The tracking operation of the slave arms 7 and 9 is controlled by the corrected movement amount.

Furthermore, when the operator 3 operates by moving the arm 11 of the master arm 1, the arm of a person usually quivers in small amounts when in a stationary state. , 27 may detect it.
Therefore, the slave arms 7 and 9 may cause unnecessary small vibrations. Therefore, it is necessary to prevent such a person's arm trembling from being sensed so that the slave arms 7 and 9 are correctly maintained in a stationary state. Circuits for that purpose are shown in FIGS.

An output pulse from each encoder of the master arm 1 attached to the arm 11, for example, the encoder 23 is given to a master arm counter 47, and a gear ratio correction circuit is provided for the count value.
At 49, gear ratio correction is performed. This is because the rotation angle of each axis is given to each encoder, for example, by doubling the rotation angle via the speed reduction mechanism. Subsequently, acceleration / deceleration processing is performed in the main controller 45 of the slave robot 5, the movement amount command value is given to the pulse distributor 51, where it is converted into a pulse output and input to the servo amplifier 53. The servo amplifier 53 feedback-controls the servo motor in the drive unit 23 ₁ according to the input pulse.

The feedback loop including the pulse distributor 51 and the servo amplifier 53 functions as a digital low-pass filter. The encoder of the master arm 1 shown in FIG.
Cut the minute vibration of high cycle of the pulse output from 23, the slave robot 5 side of the drive unit 23 ₁ as shown in FIG. 3 (b) gives the movement command signal as a signal without the minute vibration .

A more detailed structure of this circuit is shown in FIGS. 12 and 13. The pulse distributor 51 includes a subtractor 55, a multiplier 57 and a constant K ₁
It is composed of a multiplier 59 and a constant K ₂ multiplier 61. Also in the drive unit 23 ₁ is provided with a servo motor 63 and the encoder 65. The feedback loop controls the drive by the accumulated amount e, and the subtracter 55 calculates the encoder 65 from the movement command value from the master arm 1.
The value obtained by subtracting the actual movement amount by is given to the multiplier 57 as the accumulated amount e.

In the multiplier 57, from the graph shown in FIG. 13, the coefficient ε is determined according to the size of the accumulated amount e, and the predetermined value A is used as a criterion. When e ≦ A, ε = 1 / A When e> A, ε The value of ε · e is output when = 1. That is, when the master arm vibrates at a low speed, the amount of accumulation e also becomes small, ε · e becomes minute, the speed command given to the servo amplifier 53 becomes small, and the slave arm does not vibrate in small steps. However, the accumulated amount e increases as the speed of the master arm increases, and the coefficient ε = 1, so that normal operation can be obtained.

In this way, the slave arms 7 and 9 perform a stable follow-up operation according to the movement amount of the master arm.

[Effects of the Invention] As will be understood from the above description of the embodiments, the present invention is basically of a master-slave system in which the slave arms (7, 9) are moved in accordance with the movement of the master arm (1). Control means for moving the slave arms (7, 9) following the movement of the master arm (1) as a robot;
Grip (29) at the tip of the master arm (1)
During operation of the teaching mode selection switch (SW ₂ ) provided in, measure the amount of movement of each joint when the slave arm (7, 9) is moved from the first basic posture to the second basic posture. The output of the first measuring means and the master arm (1) are substantially the same as the slave arms (7, 9)
Comparing means for calculating the ratio of the amount of movement to the number of pulses by comparing with the output of the second measuring means for measuring the pulse number of the encoder of each joint when moving from the basic posture to the second basic posture. , The comparison result of this comparison means is stored as a correction value, and when the slave arm (7, 9) is made to follow the actual movement of the master arm (1), the output pulse of the encoder of each joint part of the master arm (1) Correction means for adding a correction to the number to determine the movement amount of the slave arm (7, 9),
And a storage unit for the correction value, each axis control unit (35) for controlling the drive unit of each joint unit of the slave arm (7, 9).
~ 43) are individually prepared.

As is apparent from the above configuration, in the present invention, the difference in movement between the master arm 1 and the slave arms 7 and 9 during operation of the teaching mode selection switch SW ₂ provided on the grip 29 provided at the tip of the master arm 1. Therefore, the slave arm can be given an accurate movement close to the movement of the operator during actual use.

Particularly, in the present invention, since the teaching mode selection switch SW ₂ is provided on the grip 29 as described above, when the master arm 1 is operated, the slave arms are immediately after the teaching operation for correcting the slave arms 7 and 9 described above. It is easy to operate because you can move to the operation to follow 7 and 9. In addition, even if a re-teaching operation for correction is necessary due to the physical condition of the operator, it is possible to immediately respond.

Since the storage unit for the correction value is individually provided in each axis control unit 35 to 43 for controlling the drive of each joint unit of the slave arms 7 and 9, each axis of the slave arms 7 and 9 is provided. It can be individually corrected and controlled at the same time from the slave arm 7,
The 9 can be controlled smoothly and accurately.

[Brief description of drawings]

1 is a perspective view of a master arm according to an embodiment of the present invention, FIG. 3 is a perspective view of a slave robot according to an embodiment of the present invention, and FIG. FIG. 5 is a perspective view of the grip portion of the master arm, FIG. 5 is a front view showing the operating state of the slave robot of the above-described embodiment, FIG. 6 is a timing chart showing the operating characteristics of the master arm effective switch, and FIGS. A time characteristic diagram of the master arm speed and the slave arm speed showing the operation of the above embodiment,
FIG. 9 is a block diagram of the correction circuit for the teaching mode in the above embodiment, FIG. 10 is a circuit block diagram of the above embodiment, FIG. 11 is a timing chart showing the operation of the block circuit, and FIG. FIG. 13 is a detailed block diagram of the loop, and FIG. 13 is a graph showing the relationship between the accumulation amount and the correction coefficient. 1 ... Master arm, 3 ... Operator, 5 ... Slave robot, 7, 9 ... Slave arm, 13,
13 ₁ ...... Shoulder joint part, 15 ...... Horizontal rotation detection encoder, 15 ₁ ...... Horizontal rotation drive part, 17 …… Twist detection encoder, 17 ₁ …… Twist drive part, 19 …… Forward and backward rotation detection encoder , 19 ₁ …… Back and forth rotation drive, 21,21 ₁ …… Elbow joint, 23 …… Vertical rotation detection encoder, 23 ₁ …… Vertical rotation drive, 25,25 ₁ …… Wrist joint, 27… … Vertical rotation detection encoder, 27 ₁ …… Vertical rotation drive unit, 29 …… Grip, 31 …… Vertical drive mechanism, 33 …… Truck, 35 …… R _1- axis control unit, 37 …… R _2- axis control unit , 39 …… R _3- axis control unit, 41 …… V-axis control unit, 43 …… W-axis control unit, 45 …… Main control unit, 47 …… Master arm counter, 49 …… Gear ratio correction circuit, 51… … Robot side pulse distributor, 53 …… Servo amplifier

Claims (1)

[Claims] 1. A master-slave system robot that moves a slave arm (7, 9) following a movement of a master arm (1), wherein a slave arm (following a movement of the master arm (1) When the teaching mode selection switch (SW ₂ ) provided on the grip (29) at the tip of the master arm (1) is operated, the slave arm (7, 9) is moved to the first position. The output of the first measuring means for measuring the amount of movement of each joint when moving from the basic posture to the second basic posture and the master arm (1) is substantially the same as the slave arm (7, 9). Comparing means for calculating the ratio of the amount of movement to the number of pulses by comparing with the output of the second measuring means for measuring the number of pulses of the encoder of each joint when moving from the first basic posture to the second basic posture And this comparison means comparison The result is stored as a correction value, and when the slave arm (7, 9) is made to follow the actual movement of the master arm (1), the output pulse number of the encoder of each joint part of the master arm (1) is corrected. Correction means for determining the movement amount of the slave arm (7, 9),
And a storage unit for the correction value, each axis control unit (35) for controlling the drive unit of each joint unit of the slave arm (7, 9).
Up to 43) are individually equipped master-slave robots.