JP2656584B2 - Control device for master-slave manipulator - Google Patents

Description

The present invention relates to a master-slave manipulator control device including a master manipulator and a slave manipulator.

[Conventional technology]

The manipulator which grips an object and moves it to a predetermined position to position it has a joystick type (steering type) in which an input lever is operated to move a slave slave manipulator on the driven side at a speed corresponding to the position of the input lever. There are a manipulator and a master-slave type manipulator that causes a slave manipulator to perform the same movement by operating a master manipulator as an input device.

Japanese Patent Application Laid-Open No. 61-95886 discloses this type of manipulator.
As described in Japanese Patent Application Laid-Open Publication No. H10-260, regarding the three-degree-of-freedom operation of an end effector such as a gripper for gripping an object, a master-slave method is used, and the three-degree-of-freedom operation of an arm position for positioning the end effector is described. 2. Description of the Related Art A manipulator using a Joystick method is known.

[Problems to be solved by the invention]

Of the above two methods, the joystick type manipulator is such that the position of the input lever corresponds to the speed of the slave manipulator, so that the operator can largely move the slave manipulator with a small operation.
There has been a problem that accurate positioning and precise operation of the slave manipulator cannot be performed.

On the other hand, in the master-slave type manipulator, since the position of the master manipulator as an input device corresponds to the position of the slave manipulator as an output device, accurate positioning and precise operation of the slave manipulator must be performed. However, when the slave manipulator is largely moved, there is a problem that the operator needs to perform a large operation.

Further, in the conventional example described in Japanese Patent Application Laid-Open No. 61-95886, accurate angle determination and fine operation are possible for the operation of the end effector, but three degrees of freedom of the position by moving the arm are possible. Regarding the operation, accurate positioning and precise operation cannot be performed because of the joystick method. For this reason, it is effective when the work object is gripped by a gripper or the like and the posture of the work object is slightly changed in the air or moved greatly, but when this work object is attached to another object. However, there is a problem that the operability is poor because accurate positioning cannot be performed.

The present invention has been made in view of the above circumstances, and enables accurate positioning and precise operation of a slave manipulator, and enables large movement of a slave manipulator with a small operation of a master manipulator. ,
An object of the present invention is to provide a master-slave manipulator control device with good operability.

[Means for solving the problem]

The present invention provides a master-slave manipulator including a master manipulator, a slave manipulator, and a control device that controls the slave manipulator based on an operation of the master manipulator. Switching means for switching between a master slave mode in which the position of the slave manipulator moves and a joystick mode in which the slave manipulator moves at a speed corresponding to the position of the master manipulator,
Control means for making the speed of the slave manipulator correspond to the movement amount of the master manipulator from the start position of the joystick mode, and when the load on the master manipulator becomes zero, the master manipulator is moved to the start position of the joystick mode. And return control means.

[Action]

According to the above configuration, when the master-slave mode is selected by the switching means, the positions of the master manipulator and the slave manipulator correspond to each other, so that accurate positioning and precise operation of the slave manipulator can be performed. . Further, when the joystick mode is selected by the switching means, the operator returns the master manipulator to the neutral point, because the movement speed of the master manipulator from the start position of the joystick mode corresponds to the speed of the slave manipulator. No operation is required, and the slave manipulator can be largely moved by a small operation of the master manipulator with the start position of the joystick mode as a neutral point. Further, when the operator releases his / her hand from the master manipulator, the master manipulator returns to the position at the time when the joystick mode is started, that is, a newly set neutral point, so that the slave manipulator stops. .

〔Example〕

Hereinafter, an embodiment of a control device for a master / slave manipulator according to the present invention will be described with reference to the drawings.

1 to 4 show a first embodiment of the present invention. FIG. 1 is a configuration diagram showing a master / slave manipulator provided with a control device according to the present embodiment. In the figure, reference numeral 1 denotes a master manipulator, which is connected to a plurality of arms 111 to 116 via a plurality of joints 101 to 106, and an actuator for driving the arm,
An angle detector, an angular velocity detector, and the like for detecting a rotation angle and a rotation angular velocity of each joint are provided. Reference numeral 2 denotes a slave manipulator, which includes joints 201 to 206 and arms 211 to 215, like the master manipulator. Arm at the tip of the master manipulator 1
The input lever 3 that is gripped by the operator and the force torque detector 4 that detects the force and torque applied by the operator are attached to 116. An arm 215 at the end of the slave manipulator 2 is provided with an end effector 5 such as a gripper for gripping the work and a force torque detector 6 for detecting the force and torque applied to the end effector 5. Have been. Reference numeral 7 denotes a control device which is a feature of the present embodiment. A switch 8 is connected to signal lines 9 and 10 to which signals output from the torque detectors 4 and 6 are transmitted. The output signals are six signals of a force signal in the X, Y, and Z axis directions and a torque signal around each axis. Also arrow 1
Reference numerals 1 and 12 are signals output from the angle detector and the angular velocity detector of each joint of the master manipulator 1 and the slave manipulator 2 and input to the control device 7, respectively. Reference numerals 13 and 14 denote servo amplifiers for amplifying drive signals output from the control device 7 and driving actuators provided at respective joints of the master manipulator and the slave manipulator.

Next, the configuration of the control device 7 will be described with reference to FIG. The torque detector 4 provided in the master Manipi Yoo regulator 1 is connected to the coordinate converter 15 via the signal line 9, the force torque signal outputted from the torque detector 4 F _M
Is converted from the force torque detector coordinate system to the common coordinate system.
Similarly, a coordinate converter 16 is connected to the force torque detector 6 provided in the slave manipulator 2 via a signal line 10, and a force torque signal F _S output from the force torque detector 6 is output.
Is transformed from the force torque coordinate system to the common coordinate system. Symbol 17
Is a subtractor for calculating the difference between the master-side and slave-side force torque signals converted into the common coordinate system, and the difference signal 171 is guided to the switch 18. The switch 18 is switched to a master-side contact 18A or a slave-side contact 18B according to a signal from the switch 8. A coefficient unit 19, an integrator 20, an adder 21, and an inverse converter 22 are connected to the master side contact A in sequence. Similarly, the coefficient unit 2 is connected to the slave side contact 18B.
3, an integrator 24, an adder 25, and an inverse converter 26 are connected. The inverters 22 and 26 convert the position and orientation command values based on the difference signal 171 represented by the common coordinate system into angle command values of the respective joints on the master and slave sides, and perform the above-described servo amplifier.
13, 14 are input.

Next, the operation of the first embodiment configured as described above will be described with reference to FIGS. The third
FIGS. 4 and 5 show the operation of the master / slave manipulator according to the present embodiment. FIG. 3 shows the case where the switch 8 is switched and the output signal 171 of the subtractor 17 is input to the contact 18A, ie, the master. FIG. 4 shows a case in which the output signal 171 is input to the contact 18B side, that is, a case in the joystick mode. In these figures, (a) shows the output signal 171 of the subtractor 17, (b) shows the position and attitude command signal of the master manipulator 1 in the common coordinate system, that is, the output signal of the adder 21, (c) Is the position of the slave manipulator 2 in the common coordinate system,
3 shows an attitude command signal, that is, an output signal of the adder 25. Each of the signals (a), (b), and (c) consists of the six signals described above, but only one signal is shown for simplicity.

First, a case will be described in which the master-slave mode is selected by the switch 8 and a difference signal 171 between the master-side force torque signal and the slave-side force torque signal is input to the contact 18A as shown in FIG. . However, it is assumed that no force acts on the end effector 5, and the output signal 171 from the subtractor 17 is the same as the master side force torque signal.

In this state, as shown in Figure 3 by the operator grips the input lever 3 (a), and gradually increase this force from time t _0, between time t ₁ of t ₂ is constant, the time performing gradually reduced and at time t ₃ as returned to zero operation from t _2, multiplied by a coefficient Kv by the coefficient multiplier 19 to the torque signal 171,
Further, by dividing the value integrated by the integrator 20 into two, 201 and 202, one integrated value 201 is output to the master-side inverse converter 22 via the adder 21. And the other integral value 20
2 is output to the slave-side inverse converter 26 via the adder 25. These inverters 22 and 26 convert the integral values 201 and 202, which are the position and orientation command values represented in the common coordinate system, into angle command values for each joint. Then, a difference between the angle command value and the actual angle is detected by a detector (not shown), and a value multiplied by a certain gain is output to the servo amplifiers 13 and 14.
By performing such control, the master manipulator 1 and the slave manipulator 2 move as shown by (b) and (c) in FIG. 3, and stop at predetermined positions M ₁ and S ₁ respectively. I do.

Next, from this state, the switch 8 is switched to the joystick mode, the difference signal 171 is input to the contact 18B side, and the operator grips the input lever 3 as shown in FIG. Is added, the master manipulator 1 obtains a position / posture command value 231 proportional to the force and the torque and an integral value 201 which is a position / posture command value in the master / slave mode by a command value 211 obtained by adding the adder 21. To work,
The time t ₃ ′ at which the master manipulator ₁ moves once from M ₁ and returns the force to zero again returns to M ₁ . However, since the slave manipulator 2 operates according to the position / posture command value 241 based on the difference signal 171 integrated by the integrator 24, the fourth operation is performed.
And Kuraido from the position of the S ₁ shown in FIG. (C), and stops at the position at time t ₃ 'in S ₂ back force to zero.

In the above description, the case where no force acts on the end effector 5 has been described in order to facilitate understanding. However, even when the force acts on the end effector 5, only the magnitude of the difference signal 171 changes. It is clear that the above operation is established.

According to the present embodiment, the position or speed of the slave manipulator 2 can be switched so as to correspond to the position of the master manipulator 1, so that accurate positioning of the slave manipulator 2 performs a fine operation. In addition, the slave manipulator can be largely moved by a small operation of the master manipulator, and the operability is improved. In particular, since the force acting on the end effector 5 is fed back to the operator even in the joystick mode, the end effector 5 can be largely moved while following the surface of the object,
Operability is further improved.

In the above embodiment, the case where the master manipulator 1 is operated with the position and posture command values proportional to the force and the torque in the joystick mode has been described. However, the position command value and the torque proportional to the force are integrated. The master manipulator 1 may be operated with the posture command value.

In addition, the master uses a position command value that is proportional to the force of one or two axes in the three directions of X, Y, and Z axes, a position command value that integrates the forces of the other axes, and a posture command value that integrates the torque. The manipulator 1 may be operated. By controlling in this manner, for example, even when a wide plane is operated while applying a force in one direction with the end effector 5, the operation of the master manipulator 1 may be small and operability can be improved.

In the first embodiment shown in FIG. 2, the angle command values of the master manipulator 1 and the slave manipulator 2 are respectively converted by the inverse transformers 22 and 26 into the position and posture command values expressed in the common coordinate system. Although the description has been given of the case where the conversion has been made, the inverse conversion formula cannot be derived depending on the joint arrangement of the manipulator. The configuration of the control device that copes with this case is the second embodiment shown in FIG. In the figure, the same or equivalent parts as those of the first embodiment shown in FIG. The coefficient unit 19 includes a master inverse Jacobian matrix 27 and a slave inverse Jacobian matrix.
28 are connected in parallel, and these inverse Jacobian matrices 27,2
Numeral 8 has an operation of converting a signal obtained by converting a force and torque signal into a translation speed and a posture speed in a common coordinate system by a coefficient unit 19, into master and slave joint angular velocity command values. Integrators 29 and 30 are connected to these inverse Jacobian matrices 27 and 28, respectively. The integrators 29 and 30 convert the joint angular velocity command value into the joint angle command value.

On the other hand, the coefficient side 23 is connected to a master-side inverse Jacobian matrix 33 and an integrator 34 via an adder 31 and a subtractor 32.
Further, a slave-side inverse Jacobian matrix 35 and an integrator 36 are connected to the coefficient unit 23 in parallel with these, and the inverse Jacobian matrix
Operations similar to those of the integrators 27 and 28 and the integrators 29 and 30 are performed. Master-side positive converters 37 and 38 are connected between the output side of the master-side integrator 29 and the adder 31 and between the output side of the master-side integrator 34 and the subtractor 32, respectively. . And the positive converter 3
7 converts the master joint angle command value 291 output from the integrator 29 into a position and orientation value in the common coordinate system, and outputs it to the adder 31. Similarly, the positive converter 38 converts the master joint angle command value 341 output from the integrator 34 into a position and orientation value in the common coordinate system, and outputs the same to the subtracter 32. Reference numeral 39 denotes a switch for connecting one of the master joint angle command values 291 and 341 output from the integrators 29 and 34 to the master side servo amplifier 13 through the switch 8. Similarly, reference numeral 40 denotes a switch for connecting one of the slave joint angle command values 301 and 361 output from the integrators 30 and 36 to the slave servo amplifier 14 by the switch 8.

Next, the operation of the second embodiment configured as described above will be described with reference to FIG. First, the master-slave mode is selected by the switch 8, and as shown in FIG. 5, a difference signal 171 between the master-side force torque signal and the slave-side force torque signal is input to the contact 18A side.
A case in which A and 39C and the contacts 40A and 40C of the switch 40 are connected will be described. However, End F Ekta 5
Is not acting on the output signal 171 and the output signal 171 from the subtractor 17 is
Is the same as the force torque signal on the master side.

In this state, when the operator grips the input lever 3 and applies a force, the force torque signal is first converted by the coefficient unit 19 into a translation speed and a posture speed in the common coordinate system. These signals are converted into master and slave joint angular velocity command values by inverse Jacobian matrices 27 and 28, and input to servo amplifiers 13 and 14 through integrators 29 and 30, respectively.
Since such control is performed, the master manipulator 1
The input lever 3 and the end effector 5 of the slave manipulator 2 perform exactly the same operation.

Next, from this state, the switch 8 is switched to the joystick mode, and the difference signal 171 is transmitted to the contact 18B side of the switch 18, and at the same time, the contacts 39B and 39C of the switch 39, and the contacts 40B and 40C of the switch 40. Will be described. When the switch 8 is switched as described above, first, the output value 291 of the integrator 29 is set as the initial value of the integrator 34, and
The output value 301 of 0 is set as the initial value of the integrator 36. After setting these initial values, the switches 39 and 40 are switched.
In this way, even if the switches 39 and 40 are switched, the master and slave manipulators 1 and 2 do not move abnormally. When the operator holds the input lever 3 and applies a force from this state, the master-side control system operates as follows.

A position / orientation command value 231 proportional to the difference signal 171 and a positive converter 37 at the time when the mode is switched to the joystick mode.
And position values on the master manipulator 1 side output from the
The sum with 371 is calculated by the adder 31. And this adder 311
Then, the subtractor 32 subtracts a value 381 obtained by positively converting the output value 341 of the integrator 34 by the positive converter 38. The subtraction value 321 is regarded as the reverse speed and the attitude speed in the common coordinate system, and this value is converted into the master-side joint angular velocity command value by the inverse Jacobian matrix 33 and input to the master-side servo amplifier 13 through the integrator 34. Is done. Since such control is performed in the master-side control system, the position and posture are proportional to the force and torque applied to the input lever 3 by the operator with the position switched to the joystick mode as the neutral point.

In the slave control system, the operation is the same as in the master-slave mode. That is, the speed and the posture speed are proportional to the force and the torque applied to the input lever 3 by the operator.

In the Joystick mode, the master manipulator 1 was operated with the position and posture command values proportional to the force and torque, but by replacing the posture values in the output values 371 and 381 of the positive converters 37 and 38 with zero, It is also possible to operate the master manipulator with a position command value proportional to the force and a posture command value obtained by integrating the torque.

Next, a third embodiment of the present invention will be described. In the second embodiment shown in FIG. 5, the angle command values on the master side and the slave side are calculated based on the force and torque signals, but in this embodiment, the master manipulator 1 is calculated based on the force and torque signals. Is operated to operate the slave manipulator 2 based on each joint angle signal of the master manipulator 1.

FIG. 6 shows a third embodiment of the present invention. In the figure,
The same or equivalent parts as those of the second embodiment shown in FIG. 5 are denoted by the same reference numerals, and description thereof will be omitted.

The control system on the master side is exactly the same as that of the second embodiment shown in FIG. 5, and operates at a speed and attitude speed proportional to the force and torque in the master-slave mode. And the position and posture in proportion to the torque.

Next, the control system on the slave side will be described. The joint angle signal 41 of each joint of the master manipulator 1 is converted into a positive
The forward converter 42 converts each joint angle signal 41 into a position and orientation value in a common coordinate system. The signal converted into the common coordinate system is divided into three signals of 421, 422, and 423, and the signal 421 is transmitted to the contact 45B of the switch 45 via the hold circuit 43 and the adder 44. The signal 422 is subtracted by the subtractor 4.
6, is transmitted to the contact 45B or 45A of the switch 45 via the coefficient unit 47, the integrator 48 and the adder 44 or 49. Also signal 423
Is transmitted to the contact 45A of the switch 45 via the adder 49.
Here, the hold circuit 43 stores the position and the posture value of the master manipulator 1 in the common coordinate system when the mode is switched to the joystick mode by the switch 8. The switch 45 is connected to 45A and 45C in the master-slave mode, and is connected to 45B and 45C in the joystick mode. On the other hand, the joint angle signals 50 of the respective joints of the slave manipulator 2 are input to a positive converter 51, which converts the respective joint angle signals 50 into position and orientation values in a common coordinate system. The signal 511 converted into the common coordinate system is transmitted to the adder 52. The adder 52 is connected to the contact point 45C of the switch 45, and is connected to the slave side servo amplifier 14 via the inverse Jacobian matrix 35 and the integrator 36.

Next, the operation of the control system on the slave side configured as described above will be described. First, the case where the integrator 48 is operated in the master-slave mode from the state where the output value is zero will be described. In this case, the contact of the switch 45 by the switch 8
Since 45A and 45C are connected, the signal 423 of the position and the posture value of the master manipulator 1 in the common coordinate system is directly guided to the contact 45C, and the position and the posture value of the slave manipulator 2 are subtracted by the subtractor 52. Is calculated by multiplying the deviation value by an inverse Jacobian matrix 35 to convert it to a joint angular velocity, and further integrating by an integrator 36 to input to the slave side servo amplifier 14 as a joint angle value. Therefore, the position and posture of the slave manipulator 2 coincide with the position and posture of the master manipulator 1.

Next, the operation in the case of the joystick mode will be described. The position and the attitude of the master manipulator 1 at the time when the mode is switched to the joystick mode by the switch 8 are stored by the hold circuit 43. Then, the amount of displacement of the position and posture of the master manipulator 1 from the time when the mode is switched to the Joystick mode is subtracted by the subtractor 46.
After the displacement is multiplied by a constant coefficient by a coefficient unit 47, the signal is integrated by an integrator 48. The integration output signal and the output signal from the hold circuit 43 are added by the adder 44, and the added signal is input to the subtractor 52 via the contacts 45B and 45C of the switch 45. Therefore, the position and posture of the slave manipulator 2 are values obtained by integrating the displacements of the position and posture of the master manipulator 2 from the time when the mode is switched to the joystick mode.

In the above-described embodiment, in the joystick mode, the master manipulator is operated with the position and orientation command values proportional to the force and the torque, and the position and the position of the master manipulator 1 are switched from the time when the mode is switched to the joystick mode. The case where the slave manipulator 2 is operated with the command value obtained by integrating the amount of change in the posture has been described.
By replacing the attitude value at the output values of 7, 38 with zero, the master manipulator 1 is operated with the position command value proportional to the force and the attitude command value obtained by integrating the torque, and only the amount of change in the position is calculated by the integrator 48. By outputting the amount of change in the attitude as it is, the command value obtained by integrating the amount of change in the position of the master manipulator 1 from the point in time when the mode is switched to the joystick mode, and the command value of the master manipulator 1 The slave manipulator 2 may be operated with the posture as a command value.

Further, in the above embodiment, the switching from the master-slave mode to the joystick mode can be performed from any position and posture of the master manipulator 1, but the switching from the joystick mode to the master-slave mode is described. It is clear that the switching can be performed from any position and attitude of the master manipulator 1.

FIG. 7 shows a fourth configuration example of the control device 7 of the present invention. In this figure, reference numeral 60 denotes a saturation characteristic circuit, 61 denotes a dead zone characteristic circuit, and the others are the same as those in FIG.

In the case of joystick mode, the force difference signal is
Since K _P multiplied by the signal by 19 is input to the saturation characteristic circuit 60, even if the operator a saturation value or more force to the leading edge of the stencil Manipi Yu rater 1 added with the output value to the master-side angle servo system It does not change. Further, the dead zone characteristic circuit 61 does not change the output value to the slave-side angle servo system for a force equal to or less than a certain dead zone set value.

Therefore, even if the operator slightly applies a force to the tip of the master manipulator 1, the slave manipulator 2 does not move, and if a certain force or more is applied, the slave manipulator 2 is driven at a constant speed. Movement is the same as a normal joystick operation, and operability is good.

The embodiment of FIG. 7 is obtained by adding a saturation characteristic circuit 60 and a dead band characteristic circuit 61 to the embodiment of FIG. 2. However, the saturation characteristic circuit 60 is provided after the coefficient unit 19 of FIG. A similar effect can be obtained even if the dead zone characteristic circuit 61 is added before. Further, after the coefficient unit 19 in FIG.
The same effect can be obtained by adding a dead zone characteristic circuit 61 to the integrator 45 before the integrator 45.

〔The invention's effect〕

As described in detail above, according to the present invention, the position or speed of the slave manipulator can be switched to correspond to the position of the master manipulator, so that accurate positioning of the slave manipulator and The operation of returning the master manipulator to the neutral point is not required, and the slave manipulator can be largely moved by the small operation of the master manipulator with the start position of the joystick mode as the neutral point. And operability is improved.

[Brief description of the drawings]

FIG. 1 is a block diagram showing an example of a master-slave manipulator provided with a control device according to the present invention, and FIGS. 2, 5 to 7 each show control of a master-slave manipulator according to the present invention. FIGS. 3 and 4 are time charts showing the operation in the master-slave mode and the joystick mode of the present invention, respectively, showing the first, second and third embodiments of the apparatus. 1 ... master manipulator, 2 ... slave manipulator, 3 ... input lever, 4, 6 ... force torque detector,
7 ... control device, 8 ... switch (switching means).

 ──────────────────────────────────────────────────続 き Continuing from the front page (72) Inventor Hiroshi Yamamoto 502, Kandate-cho, Tsuchiura-shi, Ibaraki Pref. Machinery Research Laboratory, Hitachi, Ltd. (56) References JP-A-63-150170 (JP, A)

Claims (1)

(57) [Claims] 1. A master-slave manipulator comprising a master manipulator, a slave manipulator, and a control device for controlling the slave manipulator based on an operation of the master manipulator, wherein the position of the slave manipulator corresponds to the position of the master manipulator. And a joystick mode in which the slave manipulator moves at a speed corresponding to the position of the master manipulator, and a switching means for switching the joystick mode. Control means for controlling the speed of the joystick mode when the load on the master manipulator becomes zero. A master-slave manipulator comprising: a control unit for returning to the original position.