JP3129040B2 - Master-slave manipulator - Google Patents

Description

DETAILED DESCRIPTION OF THE INVENTION

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a master / slave manipulator, and more particularly to a position control device for controlling the position of a slave arm according to the position of a master arm.

[0002]

2. Description of the Related Art A master / slave manipulator has a mechanism of a master arm serving as an operator and a slave arm serving as an operation target. A position signal when the operator operates the master arm and a current signal of the slave arm are provided. A position control device is provided which extracts a deviation from the position signal and drives the slave arm with an actuator such as a motor so that the deviation becomes zero.

A master arm and a slave arm generally have a plurality of joints. The joint angle is detected by a potentiometer or the like for each joint, and the position of the slave arm is controlled by angle control for each joint.

FIG. 4 shows one joint of a conventional position control device. The angle detection signals of the angle sensor 1 of the master arm and the angle sensor 2 of the slave arm are compared by a comparator 3, and the deviation is amplified by an amplifier 4 having a gain K to form a speed command. , The change in the joint angle of the slave arm by the actuator 6 is detected by the angle sensor 2.

[0005]

SUMMARY OF THE INVENTION In the conventional configuration,
The gain K of the amplifier 4 is called a servo gain, and is a proportional constant for determining a control amount proportional to the deviation of the joint angle between the master arm and the slave arm.

The position control performance is greatly affected by the magnitude of the servo gain K. When the servo gain K is set to a small value, stable position control is achieved, but the responsiveness deteriorates.
Conversely, when the servo gain K is increased, the response is improved but becomes unstable.

[0007] With such controllability, poor stability makes it difficult to delicately operate the operation target, and poor responsiveness delays large operation of the operation target.

For this reason, conventionally, the servo gain K is set to a value so as to achieve an operation that is stable to an acceptable degree, so that the responsiveness can be obtained to some extent. It was difficult to control.

It is an object of the present invention to provide a master-slave manipulator with improved responsiveness and stability.

[0010]

SUMMARY OF THE INVENTION In order to solve the above-mentioned problems, the present invention detects a joint angle of a master arm and a joint angle of a slave arm, and amplifies a deviation between both angles by an amplifier having a servo gain K. This output is used as a speed command of the speed amplifier to control the actuator for driving the slave arm.
In the manipulator, an angular velocity detecting means for detecting a rotational angular velocity of the master arm, an angular velocity area detecting means for detecting that the angular velocity detected by the angular velocity detecting means exceeds a set angular velocity, and a detection output is obtained by the angular velocity area detecting means. Switching means for switching the speed command of the speed amplifier from the output of the amplifier to the output of the speed detecting means during the operation.

[0011]

[Function] In addition to detecting the angle of the joint of the master arm, the rotational angular velocity is detected, and when the operation speed of the master arm is lower than the set speed, position control with enhanced stability is performed.
Responsiveness is improved by causing the slave arm to follow the master arm speed at high speed for a high operation speed exceeding the set speed range.

[0012]

FIG. 1 is a block diagram showing an embodiment of the present invention. Blocks equivalent to those in FIG. 4 are denoted by the same reference numerals. The angular velocity sensor 11 has an axis operated by the same joint as the angle sensor 1 of the master arm, and detects the rotational angular velocity of the master arm from the angle change rate of the joint.

[0013] The comparator 12 compares the magnitude of the angular velocity detected V _S of the angular velocity sensor 11 and the set angular speed V _R, obtained whether or not the detected angular velocity exceeds the set angular velocity as an on-off output.

Normally, the changeover switch 13 is set to a switching state in which a speed command to the speed amplifier 5 is set to an output of the amplifier 4 in a normal state. When the comparator 12 detects V _S > V _R , the ON output thereof is sent to the speed amplifier 5. Is switched to the angular velocity detected by the angular velocity sensor 11.

[0015] In this embodiment, in addition to the joint angle detection of the master arm, the rotational angular velocity detected by the sensor 11, the amplifier when the operating speed V _S of the master arm is in a lower low-speed range than the set angular speed V _R Position control is performed using the output of the speed amplifier 4 as a speed command of the speed amplifier 5. At this time, the gain K of the amplifier 4 is set to a value that can obtain sufficient stability for the position control, thereby enhancing the stability of the position control.

Next, when the respective operating speeds V _S of the master arm enter a high speed region exceeding the set angular speed V _R , the angular speed sensor 1
The switching of the changeover switch 13 which uses the output of 1 as the speed command of the speed amplifier 5 is performed so that the drive of the slave arm follows the high operation angular velocity of the master arm, and the slave arm follows the high response of the speed amplifier 5.

FIG. 2 shows another embodiment of the present invention. 1 differs from FIG. 1 in that a differentiating circuit 14 is provided in place of the angular velocity sensor 11 to obtain a signal proportional to the angular velocity of the master arm by differentiating the detection signal of the angle sensor 1.

The operation and effect of this embodiment are the same as those of FIG.

As another embodiment, as shown in FIG. 3, instead of the angle sensor 1 and the angular velocity sensor 11, an encoder 1 is used.
5 to detect the position and angular velocity of the master arm simultaneously.

As described above, in each embodiment, when the master arm is operated at a low speed, a stable slave arm operation is obtained by the position control with the gain of the amplifier 4 reduced, and when the master arm is operated at a high speed, the high gain of the speed amplifier 5 is obtained. The operation of the slave arm with high responsiveness is obtained by the speed control by.

That is, it is possible to realize a master-slave manipulator with stable operation and good followability.

[0022]

As described above, according to the present invention, in the master-slave manipulator for performing position control by detecting the joint angle of the master arm and the joint angle of the slave arm, the rotational angular velocity of the master arm is detected. When the angular velocity is lower than the set angular velocity, position control with a low gain is performed, and speed control with a high gain is performed for a high operation speed that exceeds the set angular velocity range, so that the master arm's low-speed operation that performs fine operation , A position control with improved stability can be obtained, and a position control with improved responsiveness can be obtained for a high-speed operation for largely operating an operation target.

[Brief description of the drawings]

FIG. 1 is a block diagram showing one embodiment of the present invention.

FIG. 2 is a block diagram showing another embodiment of the present invention.

FIG. 3 is a block diagram showing another embodiment of the present invention.

FIG. 4 is a conventional block diagram.

[Explanation of symbols]

 DESCRIPTION OF SYMBOLS 1 ... Angle sensor of master arm 2 ... Angle sensor of slave arm 3 ... Comparator 4 ... Amplifier 5 ... Speed amplifier 6 ... Actuator 11 ... Angular velocity sensor 12 ... Comparator 13 ... Changeover switch 14 ... Differentiating circuit 15 ... Encoder

──────────────────────────────────────────────────続 き Continued on the front page (58) Field surveyed (Int. Cl. ⁷ , DB name) G05D 3/00-3/20 G05B 1/00-7/02 G05B 11/00-13/04 G05B 17 / 00-17/02

Claims (1)

(57) [Claims] 1. A method for detecting a joint angle of a master arm and a joint angle of a slave arm, amplifying a deviation between the two angles by an amplifier having a servo gain K, and using the output as a speed command of a speed amplifier for driving the slave arm. Master / slave with position control device to control the actuator
In the manipulator, an angular velocity detecting means for detecting a rotational angular velocity of the master arm, an angular velocity area detecting means for detecting that the angular velocity detected by the angular velocity detecting means exceeds a set angular velocity, and a detection output is obtained by the angular velocity area detecting means. Switching means for switching the speed command of the speed amplifier from the output of the amplifier to the output of the speed detecting means during the operation of the master / slave manipulator.