JPS6315111B2 - - Google Patents

Description

DETAILED DESCRIPTION OF THE INVENTION The present invention relates to a control method for a multi-degree-of-freedom master-slave type manipulator, and in particular, to control a slave mechanism at a constant speed that does not follow the movement speed of a master mechanism, and improves operability and workability. Regarding things that aim to improve etc.

Work in non-residential areas, such as in adverse environments such as areas with high radiation levels, the ocean floor, and areas with a lot of dust, must be carried out remotely from a safe location to protect workers. In the master-slave type manipulator used for this purpose, the movement of the manipulator on the driving side and the movement of the slave on the driven side are linked in a similar manner, and the movement given by the operator's hand on the master side is It is now copied directly on the slave side. To explain an example of such a manipulator, as shown in FIG. 1, the arm 2 on the master 1 side has three degrees of freedom (X _M , Y _M ,
Z _M direction), the wrist part 3 has three degrees of freedom (α _M , β _M , γ _M directions) in the polar coordinate system, and the arm part 5 of the slave 4 has three degrees of freedom ( Xs, Ys, Zs
direction), and the wrist part 6 has three freedoms (αs, βs, γs directions).
If an operator directly operates the master 1, the slave will follow the master and move in the same direction as the master's movement.

The manipulator shown in FIG. 2 is used for deburring work on castings, and a grinder 9 is attached to the wrist 8 on the slave 7 side, and the manipulator performs casting finishing work. The generated cast burr A is removed with a grinder, and the surface 11
I'm finishing up. The operator operates the master 12 so that the slave 7 moves forward in the Y direction, and also pays attention to the movement in the Z direction so that the grinder 9 does not dig into the casting surface too much. In the case of such casting deburring work, if the grinder is sent at a constant speed, there will be less tool wear and tear.
This improves the finish of the casting, but with conventional master-slave type manipulators, the movement of the slave is controlled to follow the movement of the master.
In order to feed the slave at a constant speed, the master must be moved at a constant speed. However, such constant speed work is not easy for the worker and is tiring work. Furthermore, in the case of a multi-degree-of-freedom type manipulator, constantly operating all six axes, for example, causes fatigue for the operator.

The present invention aims to eliminate the drawbacks of the conventional master-slave type manipulator as described above.
In a multi-degree-of-freedom system master-slave type manipulator in which a slave mechanism on the driven side operates in response to the movement of a master mechanism on the driving side, some of the slave mechanisms can be operated at a predetermined speed and direction independently of the master mechanism. The present invention provides a method for controlling a master-slave type manipulator that is driven.

Next, embodiments embodying the present invention will be described in detail with reference to the accompanying drawings starting from FIG.
FIG. 3 is a block diagram of a control system used in a control method according to an embodiment of the present invention, and FIG. 4 is a flowchart showing a control procedure of the control method.

In Fig. 3, position detectors Hx, Hy, ... from the X axis to the γ axis corresponding to each degree of freedom on the master side are shown.
Hγ are connected to the input side of the microcomputer 14 via the interface circuit 13, and the X-axis, Y-axis, . . . , γ on the slave side
Position detectors Kx, Ky, which detect the position in the axial direction
..., Kγ are each connected to the microcomputer 14 via an interface circuit 15. The servo amplifiers Sx, Sy, ..., Sγ that receive output signals from the microcomputer 14 are slave-side load mechanisms with each degree of freedom.
Servo motor Mx that drives Wx, Wy, ..., Wγ,
Connected to My,…, Mγ. TG is a tacho generator that measures the rotation speed of each servo motor, and outputs the measured value to each servo amplifier Sx, Sy, ..., Sγ, and uses a damping effect to stabilize each servo motor. Constant _- speed feed _control boxes _B constant speed feed switches SVx, SVy, SVz that notify the computer 14 whether or not the
DSy, DSz, direction switch HSx for instructing the direction of constant speed feed (forward or backward),
HSy and HSz are provided.

Next, the operating procedure of the method of the present invention will be explained with reference to FIGS. 3 and 4. Now, a case will be described in which automatic constant speed feeding is performed in the X-axis direction. In this case, the operator must operate the digital switch DSx and direction switch HSx of control box Bx.
After determining the feed speed ν _p and feed direction by operating the , set the constant speed feed switch SVx to the "on" position. Then, as shown in Fig. 4, in step (a), the current position of the master on the X-axis is read into the computer memory with reference to the output value of the position detector Hx, and the current position of the slave on the X-axis is read into the computer memory. The current position of is also read into the memory in step (b) with reference to the output value of the position detector Kx.

Next, in step (c), set the constant speed feed switch SVx.
It is determined whether or not a constant speed feed command is issued by referring to the output from. If YES, input ν _p into the memory with reference to the set value of the digital switch DSx (step (d)). Next, in step (e), the feed direction is determined by referring to the direction switch HSx, and if the feed direction is forward, the process proceeds to step (f), and if a backward signal is issued, the previous one is memorized in step (g). After reversing the sign of the set speed ν _p to negative, it is stored again and the process proceeds to step (f). In step (f), the following calculation is performed: ν _o =ν _o-1 + (ν _{p −lo} −lo ₋₁ /Δt) to calculate the target speed ν _o . Here, ν _o : Target speed ν _o-1 : Target speed of previous stage l _o : Current position of slave (X-axis direction) l _o-1 : Position of slave at previous stage (X-axis direction) △t: Sampling time Interval In this operation, ν _o passes through a certain transient state,
gradually approaches ν _p . Next, ν _o ×△t = deviation is calculated (step (h)), and the deviation thus calculated is output to the servo amplifier Sx (step (i)), and the servo motor Mx is set at a constant speed in the forward or backward direction. The slave mechanism moves at a constant speed ν _p in the X-axis direction.

If the constant speed feed switch SVx is turned off, the process proceeds in the NO direction at step (c), and the slave target position is calculated from the previously input master current position (step (j)). ), then in step (k) the deviation between the slave target position and the slave current position is calculated, and in step (l) this deviation is output to the servo amplifier Sx. Therefore, in this case, the operation of the slave will follow the master.

Although the above-mentioned operations have been explained only for the X-axis, they are performed similarly for the Y-axis and the Z-axis. However, it is not necessary to perform constant speed control for all degrees of freedom; for example, in the case of a manipulator for deburring castings, the α, β, and γ axes that determine the operation of the neck are made to follow the master.

The calculation in step (j) above is for calculating the target position of the slave from the master position when the size ratio of the master and slave is not 1:1, and is not necessary when the size ratio is 1:1. . In addition, the calculation in step (f) cannot be performed simply by outputting the set value ν _p to the servo amplifier.
The speed of the slave may change accordingly, and this is done to prevent this. In other words, since ν _o-1 = 0 and l _o = l _o-1 at the start of constant speed feed,
The initial target speed ν _o is equal to ν _p . Thereafter, the speed is checked every △t time, and if the current speed is larger than ν _p , ν _o is calculated so that it is smaller than the previous one, and vice versa, so that it becomes larger, and the feed speed is set to the set value ν _p It is controlled so that

As described above, in a multi-degree-of-freedom system master-slave type manipulator in which a slave mechanism on the driven side operates in response to the movement of a master mechanism on the driving side, some of the slave mechanisms are moved relative to the master mechanism. In a multi-degree-of-freedom master-slave type manipulator, a specific slave can be operated at any constant speed, so the operator can Because of the constant speed control, the operator does not feel fatigue, and during constant speed work, it is only necessary to operate the axes other than the axis being controlled at constant speed, making the work easier and reducing the occurrence of work errors.
This type of constant speed control is particularly effective when it is necessary to feed tools at a constant speed in a constant direction, such as in manipulators for casting finishing work.
This eliminates operator fatigue, reduces tool wear, and significantly improves the finishing accuracy of castings and the smoothness of finished surfaces.

[Brief explanation of the drawing]

FIG. 1 is a perspective view of a general master-slave type manipulator, FIG. 2 is a perspective view of a master-slave type manipulator for deburring, FIG. 3 is a block diagram of a control device for the method of the present invention, and FIG. The figure is a flowchart showing the steps of the method. (Explanation of symbols), 1...Master mechanism, 4...Slave mechanism, 14...Microcomputer, 1
3,15...Interface circuit, Bx, By,
Bz...Control box, SVx...SVz...Constant speed feed switch, DSx...DSz...Digital switch,
HSx…HSz…Direction switch, Hx…Hγ…
...Master side position detector, Kx...Kγ...Slave side position detector, Sx...Sγ...Servo amplifier, Mx...
Mγ...Servo motor, Wx...Wγ...Load mechanism,
TG...Takoji generator.

Claims (1)

[Claims] 1 In a multi-degree-of-freedom master-slave type manipulator in which a slave mechanism on the driven side operates in response to the movement of a master mechanism on the driving side, the operating speed and direction of operation of the slave mechanism are determined in accordance with one degree of freedom. 1. A method for controlling a master-slave type manipulator, characterized in that a constant-speed feed control box is provided to drive the slave mechanism at a predetermined speed and direction independently of the master mechanism.