JP2635776B2 - Master / slave type manipulator - Google Patents

Description

DETAILED DESCRIPTION OF THE INVENTION [Object of the Invention] (Industrial application field) The present invention relates to a master-slave manipulator capable of operating a slave arm in a desired direction.

(Prior Art) A master-slave type manipulator is a minipulator system composed of a manipulator (slave arm) that acts on an object and a manipulator (master arm) that instructs its movement, and includes a master arm and a slave. The arm usually has a plurality of axes, but the structure and mechanism are the same even if the dimensions are different. That is, the slave arm is driven (synchronously) while taking the same posture as the master arm moved by the operator.

FIG. 3 is a block diagram showing an automatic control system for each axis of a master arm and a slave arm of a force feedback type bilateral servo manipulator of the master / slave type manipulator.

This master-slave type manipulator has the following 10
Blocks and two addition points. That is, servo motors 3, 4 for driving the master arm axis 1 and the slave arm axis 2 corresponding to the master arm axis 1, respectively.
An amplifier 5, which amplifies a signal related to driving of the servo motors 3, 4
6. A master arm torque detector (hereinafter referred to as "M torque detector") 7 for detecting torque acting on each of the master arm shaft 1 and the slave arm shaft 2, and a slave arm torque detector (hereinafter referred to as "S torque detector"). 8) Position of master arm axis 1 and slave arm axis 2 (arm angle)
Arm position detector (hereinafter referred to as “M position detector”) 9 and slave arm position detector (hereinafter “S
A position detector 11), a position adjuster 11 for adjusting the arm angle of the slave arm axis 2 so that the arm angles of the master arm axis 1 and the slave arm axis 2 are the same, the master arm axis 1 and the slave arm axis 2 A torque adjuster 12 for adjusting the torque of the master arm shaft 1 so that the torque of the master arm 1 becomes the same, a position adder 13 and a torque adder 14.

The M position detector 9 and the S position detector 10 are connected to the master arm shaft 1 and the slave arm shaft 2 via the M torque detector 7 and the S torque detector 8, respectively.
Connect with The position adjuster 11 and the torque adjuster 12 include a position adder 13 and an amplifier 6 and a torque adder 14 respectively.
And the amplifier 5. Servo motors 3 and 4 are connected to master arm axis 1 and amplifier 5, and slave arm axis 2 and amplifier 6, respectively.

In this automatic control system, signals relating to the torque and the position of the master arm shaft 1 and the slave arm shaft 2 are sent to the M torque detector 7 and the S torque detector 8, and to the M position detector 9 and the S position detector 10, respectively. Can be Upon receiving this signal, the M position detector 9 and the S position detector 10 detect the positions of the master arm axis 1 and the slave arm axis 2 and send the detected values to the position adder 13. The adder 13 calculates a deviation _εΘ from the values detected by the two position detectors 9 and 10, and
Send to 11. If there is a position deviation between the master arm shaft 1 and the slave arm shaft 2, the position adjuster 11 sends a position deviation elimination command to the amplifier 6 indicating that this deviation should be eliminated. Amplifier 6
Upon receiving this position deviation elimination command, it amplifies it and sends it to the servomotor 4. Upon receiving the amplified position deviation elimination command, the servo motor 4 drives the slave arm shaft 2 to have the same arm angle as the master arm shaft 1.

On the other hand, when the M-torque detector 7 and the S-torque detector 8 receive the above-mentioned signal regarding the torque, they detect the torque of the master arm shaft 1 and the slave arm shaft 2, respectively, and send the detected values to the torque adder 14. . The adder 14 calculates a deviation epsilon _T from the value both torque detector 7 and 8 has detected and sent to the torque regulator 12. When there is a torque deviation between the master arm shaft 1 and the slave arm shaft 2, the torque adjuster 12 sends a torque deviation elimination command to the amplifier 5 to eliminate the deviation.
When the amplifier 6 receives this torque deviation elimination command, it amplifies it and sends it to the servomotor 3. Upon receiving the amplified torque deviation elimination command, the servomotor 3 applies the same torque to the master arm shaft 1 as to the slave arm shaft 2. Then, the operator holding the master arm shaft 1 can feel the magnitude of the torque acting on the slave arm shaft 2.

(Problems to be Solved by the Invention) However, such a master-slave type manipulator can finally move the entire slave arm to a desired point for work, but has a limitation of multi-axis and multi-joint. The operator who manually moves the master arm
It is difficult to control the way to the final point of the slave arm. For this reason, for example, in a work of gripping a rod-shaped object with a slave arm and inserting it into a ring or a hole, the slave arm cannot be moved linearly from the ring or hole entrance to the back, and the rod-shaped object The end of the ring or the hole may collide and cause a breakage accident.

The present invention has been made in view of the above circumstances, and an object of the present invention is to provide a master-slave manipulator that can control a path of movement of a slave arm axis.

[Configuration of the invention]

(Means for Solving the Problems) In order to solve the above problems, the present invention provides a master-slave manipulator including a slave arm acting on an object and a master arm for instructing the movement of the slave arm. Means for setting a constrained point on the master arm, means for setting the operation direction of the constrained point, and means for driving the master arm such that the constrained point operates in the set operation direction A master / slave type manipulator is provided.

(Operation) The master / slave type manipulator of the present invention includes a means for setting a constrained point on a master arm, a means for setting an operation direction of the constrained point, and a method for setting the constrained point in the set operation direction. Means for driving the master arm to operate. For this reason, if there is a portion on the slave arm where the path of movement is desired to be controlled, a corresponding portion on the master arm is set as a constrained point. Then, a path to be controlled for the slave arm is set as the operation direction of the restrained point. Thereafter, if the master arm is driven so that the constrained point moves in the set operation direction, the movement path of the slave arm can be controlled.

(Embodiment) An embodiment of the present invention will be described below with reference to FIG. 1 and FIG.

FIG. 1 is a block diagram showing an automatic control system for each axis of a master arm and a slave arm of a master-slave type manipulator of the present invention. In this control system, portions corresponding to those in FIG. 3 are denoted by the same reference numerals, and description thereof is omitted.

The automatic control system of the master / slave type manipulator according to the present invention includes, in addition to the conventional one shown in FIG. 3, a constrained point setting device 20 as means for setting a constrained point on the master arm, The constrained point moves in the motion direction set by the constraining condition setting device 22 together with the position calculator 21, the constraining condition setting device 22 as a means for setting the operation direction of the constrained point, the constraining selection switch 23 and the torque adjuster 12. Position adder as a means to drive the master arm
With 24.

The constrained point position calculator 21 is connected to the constrained point setting unit 20, the constrained position adder 24, and the M position detector 9. The restraint position adder 24 is further connected to a restraint condition setting device 22 and a restraint selection switch 23, and the restraint selection switch 23 is further connected to the torque regulator 2.

If there is a portion of the slave arm whose path of movement is desired to be controlled, a corresponding portion (constrained point) on the master arm is set by the constrained point setting device 20, and the constrained point is calculated. Notify the container 21. The constrained point position calculator 21 calculates the position of the constrained point on the basis of a signal regarding the position transmitted from the M position detector 9 connected to the master arm shaft 1 provided with the constrained point. The value is sent to the adder 24 at the constraint position. The adder 24 at the constraint position
The deviation of the current position of the constrained point from the constraining condition sent from the constraining condition setting unit 22 is calculated and sent to the torque adjuster 12.

By the way, since there is a restraint selection switch 23 between the restraint position adder 24 and the torque adjuster 12, only when it is necessary to control the path of the slave arm, the restraint select switch 23 is turned on to add the restraint position. The deviation calculated by the device 24 is sent to the torque regulator 12. Then, the torque adjuster 12 calculates a torque value that eliminates the torque deviation between the master arm and the slave arm and the deviation from the constraint condition of the position of the constrained point, and sends it to the amplifier 5. Then, the amplifier 5 amplifies this torque value and sends it to the servomotor 3. The servo motor 3 drives the master arm shaft 1 according to the amplified torque value. In this way, for the slave arm that moves synchronously with the master arm, the movement of the portion corresponding to the constrained point can be controlled, and the slave arm can move along a straight line or a plane.

Figure 2 shows seven axes and six joints 25,26,27,28,29,30
Of the master arm 31 having the
32 (the coordinates are (p _x , p _y ,
p _z )) shows the movement. Hereinafter, a procedure from setting of the constrained point P to this movement will be described with reference to FIGS.

In this embodiment, the restrained point P, the point Q (coordinates _{(q x, q y, q} z)) through the plane 33 (the point Q represented by the normal vector Np to a normal vector Np Vertical)
The purpose is to reach point Q.

In this case, the constrained point P is determined by the constrained point setting unit 20 shown in FIG. After the constraint condition (plane 33) is determined by the constraint condition setting unit 22, the non-constraint point P and the plane 33 are determined by the constraint position adder 24.
The distance (deviation) between is calculated. When the deviation is obtained, it is sent to the torque regulator 12 via the restriction selection switch 23.

In the torque regulator 12, first, a force F =
k (P−Q) × Np (k is a proportional constant) is obtained. Then, this force F is further calculated for each joint by the equation T _i = (L _i × F) × e
_i (i is a six joints number _(i = 1~6), T i is the torque of each joint, L _i is the distance to the unconstrained point P from each joint, e _i denotes the direction of rotation of each joint Torque according to unit vector)
It recalculates the T _i, is inputted to the servo motor 3 for driving each axis (joint) via the amplifier 5.

By generating the torque _Ti at each joint in this manner,
The constraint force F is applied to the non-constrained point P, and the tip of the slave arm corresponding to the non-constrained point P is
A point corresponding to the point Q can be reached along a plane corresponding to the plane 33.

〔The invention's effect〕

As described above, in the master-slave type manipulator of the present invention, the means for setting the constrained point on the master arm, the means for setting the operating direction of this constrained point, and the constrained point are set in the above manner. Means for driving the master arm to operate in the operation direction. For this reason, when there is a position on the slave arm axis where the path of movement is desired to be controlled, the corresponding position on the master arm axis is set as a constrained point. Then, a path to be controlled with respect to the slave arm axis is set as the operation direction of the restrained point. Thereafter, if the master arm axis is driven so that the constrained point moves in the set operation direction,
The path of movement of the slave arm axis can be controlled.

[Brief description of the drawings]

FIG. 1 is a block diagram showing an automatic control system of a master-slave type manipulator according to an embodiment of the present invention, FIG. 2 is a schematic diagram showing the movement of a constrained point, and FIG. FIG. 3 is a block diagram showing an automatic control system of the slave type manipulator. 12: Torque adjuster, 20: Constrained point setting device, 22: Constraint condition setting device 22.

Claims (1)

(57) [Claims] 1. A master-slave type manipulator comprising a slave arm acting on an object and a master arm for instructing the movement of the slave arm.
Means for setting a constrained point on the master arm, means for setting the operation direction of the constrained point, and means for driving the master arm such that the constrained point operates in the set operation direction A master / slave type manipulator characterized by the following.