JP2006102866A - Control device of master slave manipulator - Google Patents

Abstract

<P>PROBLEM TO BE SOLVED: To provide a master slave manipulator on head ends of a master slave manipulator and a slave manipulator of which a tool is installed and which is capable of moving differently by the master manipulator and the slave manipulator. <P>SOLUTION: This master slave manipulator is furnished with an operation handle 3 arranged on the master manipulator 1 through a power sensor S2, a joy stick 6 arranged on the master manipulator to be in parallel with the operation handle, a change-over switch 9b to switch an operating object by the joy stick over to the master manipulator 1 or to the slave manipulator 2 and a control device 8. Consequently, the manipulator designated by the operation handle can be operated by the joy stick in addition to master slave operation by the operation handle. <P>COPYRIGHT: (C)2006,JPO&NCIPI

Description

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

As a manipulator that grasps an object and moves to a predetermined position according to an operator's command, the manipulator moves the driven slave manipulator at a speed corresponding to the position of the input lever by operating the input lever. There are (manipulated) manipulators and master-slave manipulators that cause a slave manipulator to perform the same movement by operating a master manipulator that is an input device.
As a manipulator of this type, Patent Document 1 discloses a three-degree-of-freedom operation of an arm position for positioning an end effector using a master-slave system with respect to a three-degree-of-freedom operation of an end effector such as a gripper holding an object. A manipulator using a joystick method is described.
Of the above two methods, the joystick type manipulator corresponds to the position of the input lever and the speed of the slave manipulator, so that the operator can move the slave manipulator greatly with a small movement, but the accuracy of the slave manipulator There has been a problem that accurate positioning and fine operations cannot be performed.
On the other hand, the master-slave manipulator corresponds to the position of the master manipulator that is the input device and the position of the slave manipulator that is the output device, so that the slave manipulator can be accurately positioned and finely operated. There is a problem that the operator needs to perform a large movement when moving a large amount.
In order to solve such a problem, the slave manipulator can be accurately positioned and finely operated, and the slave manipulator can be moved greatly by a small operation of the master manipulator, and the master slave manipulator operating device with good operability. Has been proposed (see, for example, Patent Document 2). FIG. 6 is a diagram showing a schematic configuration of a conventional master-slave manipulator operating device.
In the figure, reference numeral 104 denotes a master manipulator, and an actuator for driving an arm, an angle detector and an angular velocity detector for detecting a rotation angle and a rotation angular velocity of each joint are provided at a joint portion. 107 is a slave manipulator, and an input lever 106 gripped by the operator and a force torque detector 105 for detecting the force and torque applied by the operator are attached to the tip of the master manipulator 104. Further, an end effector 109 such as a gripper for gripping a work piece and a force torque detector 108 for detecting the force and torque applied to the end effector 109 are attached to the tip of the slave manipulator 107. A control device 101 is connected to a switch 110 and signal lines 111 and 112 to which signals output from the force torque detectors 105 and 108 are transmitted. The output signals are six signals including a force signal in the X, Y, and Z axis directions and a torque signal around each axis. Arrows 114 and 113 are signals output from the angle detector and the angular velocity detector of each joint of the master manipulator 104 and the slave manipulator 107, respectively, and input to the control device 101. Reference numerals 102 and 103 denote servo amplifiers which are output from the control device 101 and amplify drive signals for driving actuators provided at the joints of the master manipulator and the slave manipulator, respectively.
The control device 101 includes switching means for switching between a master slave mode in which the position of the slave manipulator 107 moves corresponding to the position of the master manipulator 104 and a joystick mode in which the slave manipulator 107 moves at a speed corresponding to the position of the master manipulator 104. When the load on the master manipulator 104 becomes zero, the control means for causing the speed of the slave manipulator 107 to correspond to the amount of movement of the master manipulator 104 from the start position of the joystick mode and the master manipulator 104 to the start position of the joystick mode And a returning control means. When the master / slave mode is selected by the switching means, the positions of the master manipulator 104 and the slave manipulator 107 correspond to each other, so that the slave manipulator 107 can be accurately positioned and finely operated. When the joystick mode is selected by the switching means, the movement amount of the master manipulator 104 from the start position of the joystick mode and the speed of the slave manipulator 107 correspond to each other, so that the operator does not need to return the master manipulator 104 to the neutral point. The slave manipulator can be moved greatly by a small operation of the master manipulator 104 with the start position of the joystick mode as a neutral point.

JP-A-61-95886 Japanese Patent No. 2656584

The conventional master-slave manipulator control device is intended to improve the operability when the master manipulator is simply used as an operation input means for the operator, and the master manipulator side has no restraining force other than the operator's hand. It is assumed that it does not work.
However, a manipulator that can be operated by attaching a tool to the tip of the master manipulator in the same way as the slave arm, and working with both the master manipulator and slave manipulator on the work object (for example, work on the master manipulator side) When the position and orientation of the work object are required on the slave manipulator side while holding the object), the work object itself held by the master manipulator moves or moves from the work object part in the conventional control device. There is a problem that it becomes difficult to obtain a desired position and orientation of the slave arm due to the reaction force.
The present invention has been made in view of such problems, and a master-slave manipulator control device that improves the operability and safety of a master-slave manipulator in which tools are attached to the tips of both a master manipulator and a slave manipulator. The purpose is to provide.

In order to solve the above problem, the present invention is configured as follows.
The invention according to claim 1 is a master slave manipulator control device that controls a slave manipulator based on an operation of a master manipulator by an operator, an operation handle disposed on the master manipulator via a force sensor, and the master A manipulator arranged in parallel with the operation handle, a joystick for operating the master manipulator or the slave manipulator, switching means for switching a manipulator to be operated by the joystick, and operation input to the joystick are enabled or disabled The master manipulator in accordance with the switch to be set to, the operation force of the operator acting on the operation handle detected by the force sensor, the output of the switching means, and the output of the switch. Characterized by comprising a cooperative control unit for cooperative control data and the slave manipulator.
According to a second aspect of the present invention, the cooperative control unit is configured to perform the operation according to an operation force acting on the operation handle detected by the force sensor based on a predetermined function when the input valid switch is valid. The operation input to the joystick is adjusted.
The invention according to claim 3 is characterized in that the cooperative control means includes a correction means for correcting an operation input to the joystick with a time point when the switch becomes effective as a zero point.
The invention described in claim 4 is characterized in that the joystick is installed in the master manipulator so as to be rotatable about the operation handle as a rotation center.

  According to the first aspect of the present invention, the slave manipulator can also be operated while directly operating the master manipulator, and the slave manipulator cooperates with the master manipulator, so that the safety and operability are improved. According to the second aspect of the present invention, since the operation amount of the slave manipulator is adjusted according to the operation amount of the master manipulator, it is possible to provide an operation sensation in accordance with the operator's intention and improve operability. According to the third aspect of the present invention, since a constant amount of operation can be provided regardless of the position where the operator holds the joystick, the operability is improved. According to the fourth aspect of the present invention, since the joystick can be easily adjusted to the operation posture of the operator, the operability is improved.

  Hereinafter, embodiments of the present invention will be described with reference to the drawings.

FIG. 1 is a block diagram showing an embodiment of a master-slave manipulator according to claims 1 to 4 of the present invention. In the figure, 1 is a master manipulator. Each joint is provided with an actuator for driving the arm, and an angle detector and an angular velocity detector for detecting the rotation angle and rotation angular velocity of each joint (not shown). ).
Reference numeral 2 denotes a slave manipulator, which is composed of a plurality of joints as in the master manipulator 1, each of which includes an actuator for driving an arm, an angle detector for detecting the rotation angle and rotation angular velocity of each joint, and an angular velocity. A detector is provided (not shown).
A force sensor S <b> 1 that detects force and torque applied to the first tool 5, a fixed portion 4 of the operation handle 3 held by the operator 19, in order from the base side of the master manipulator 1, is provided at the tip of the master manipulator 1. A tool 5 is installed.
Specifically, the first tool 5 is a gripper or the like that grips the work 17 on the work table 18, and detects the force and torque applied by the operator 19 between the fixed portion 4 and the operation handle 3. A force sensor S2 is attached. A joystick 6 is further attached from the fixed portion 4 so as to be parallel to the operation handle 3.

A second tool 7 such as a gripper for gripping the work piece 17 and a force sensor S3 for detecting the force and torque applied to the second tool 7 are attached to the tip of the slave manipulator 2. Reference numeral 8 denotes a cooperative control device that is a feature of the present embodiment. Signal control lines 10, 11, and 12 for transmitting signals output from the force sensors S1, S2, and S3, and a changeover switch attached to the operation handle 3, respectively. 9b and a signal line to which an output signal of the input valid switch 9a attached to the joystick 6 is transmitted is connected. The output signals of the force sensor are six signals including a force signal in the X, Y, and Z axis directions and a torque signal around each axis.
The operation with respect to the joystick 6 is effective only while the input valid switch 9a is pressed, and the changeover switch 9b is used when setting the object to be operated with the joystick 6 to the master manipulator 1 or the slave manipulator 2.
Reference numerals 13 and 14 in the figure denote signals that are 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 cooperative control device 8, respectively. Reference numerals 15 and 16 denote servo amplifiers that amplify drive signals output from the cooperative control device 8 to drive the actuators of the joints of the master manipulator 1 and the slave manipulator 2, respectively.

Next, the configuration of the cooperative control device 8 will be described with reference to FIG. The output of the force sensor S2 provided in the fixed part 4 of the master manipulator 1 is connected to the coordinate converter 40 via the signal line 11. The force torque signal output from the force sensor S2 is converted from the force sensor coordinate system to the common coordinate system and output as f2.
Similarly, the output of the force sensor S3 provided in the slave manipulator 2 is connected to the coordinate converter 45 via the signal line 12, and the force torque signal output from the force sensor S3 is changed from the force sensor coordinate system to the common coordinate system. And output as f3.
The output of the force sensor S1 provided in the master manipulator 1 is connected to the coordinate converter 42 via the signal line 10, and the force torque signal output from the force sensor S1 is changed from the force sensor coordinate system to the common coordinate system. And output as f1.
Reference numeral 20 denotes a zero point correction unit that corrects and outputs an operation input of the joystick 6 with the time point when the input valid switch 9a is pressed as a zero point, and 21 denotes a force sensor S2 when the input valid switch 9a is pressed. This is a correction function unit that corrects the output of the zero point correction unit 20 based on a predetermined function in accordance with the operation force of the operator 19 acting on the operation handle 3 detected in (1).
The speed command of the joystick 6 output from the correction function unit 21 is converted into a position command by the integrator 22 and guided to the switch 23. The switch 23 switches between the master side contact 23b and the slave side contact 23a depending on the signal from the changeover switch 9b.
The force control unit 41 is connected to the master side contact 23b, and the output of the force control unit 41 is added to the output of the reaction force generation unit 43. Further, the Jacobian converter 44 converts the torque command in the common coordinate system into a torque command for each joint of the master manipulator 1 and outputs it to the servo amplifier 15.
Similarly, a force control unit 46 is connected to the slave side contact 23 a, and a torque command in the common coordinate system is converted into a torque command of each joint of the slave manipulator 2 by the Jacobi converter 47 and output to the servo amplifier 16.

Here, the force control units 41 and 46 and the reaction force generation unit 43 will be described with reference to FIGS. 3 and 4, respectively. Since the force control units 41 and 46 have the same control structure, the internal configuration will be described collectively with reference to FIG.
The control system of the force control units 41 and 46 shown in FIG. 3 is a force torque based compliance control system which is a well-known technique (a system found in the literature “Robot Engineering Handbook p.254, Robotics Society of Japan, 1990, Corona)”. Dv and Kd in the figure represent virtual viscosity and virtual rigidity, respectively, and Kf represents a force feedback gain. By increasing the force feedback gain Kf and reducing the steady-state deviation, the virtual viscosity Dv and the virtual rigidity Kd can be arbitrarily adjusted to desired values.

  4 outputs a value τ3 obtained by adding the output obtained by amplifying the output f1 of the force sensor S1 by the proportional gain HK and the output amplified by the proportional gain HD via the differentiator 411.

Next, the actual operation will be described with reference to FIG. When the operator 19 pushes the operation handle 3 with the left hand, the operation input f2 to the force control unit 41 is generated, so that the first tool 5 of the master manipulator 1 approaches the work piece 17 on the work table 18. When the operator 19 switches to the slave-side contact 23a with the left hand using the change-over switch 9b of the operation handle 3 with the left hand and then tilts the joystick 6 with the right hand, the operation command ref2 is input to the force control unit 46 and the slave manipulator 2 is Operate.
At this time, the correction function unit 21 adjusts the output of the joystick 6 by calculating a speed command gradient ANG from the operating force f2 by a predetermined function F as shown in FIG. That is, when the operating force acting on the operation handle 3 is reduced, the operating speed of the joystick 6 is automatically adjusted to be lower than the actual speed. Conversely, when the operating force is increased, the operating speed of the joystick 6 is automatically set higher than the actual speed. The master manipulator 1 and the slave manipulator 2 can operate cooperatively by automatically adjusting the speed of the operator 19 and automatically understanding the intention of the operator 19.
Next, when the first tool 5 comes into contact with the work piece 17, the force sensor S 1 detects the contact force, and the reaction force τ 3 is generated by the reaction force generation unit 43, and then added by the adder 24 to be the master manipulator. 1 generates a resistance force and decelerates, so the operator 19 senses contact through the operation handle 3.
When the second tool 7 comes into contact with the work 17, the work control unit 46 holds the work 17 with a constant contact force.

When it is desired to move the master manipulator 1 greatly, if the operator 19 switches to the master side contact 23b using the switch 9b of the operation handle 3 with the left hand and then tilts the joystick 6 with the right hand, the operator manipulator 1 will act on the operation handle 3. An operation command ref <b> 1 corresponding to the operating force being input is input to the force control unit 41, and it is possible to cause the master manipulator 1 to generate a larger operation than when operating alone with the operation handle 3.
In the embodiment described above, when the joystick 6 is rotatably mounted so as to be parallel to the operation handle 3 from the fixed portion 4 as described in claim 4, the operator 19 operates the joystick 6 according to the work posture. It becomes possible to arrange in the position which is easy to do, and operativity improves.

  The present invention can be widely applied to a master-slave manipulator in which tools are attached to the tips of both a master manipulator and a slave manipulator, and the operability and safety are improved.

The figure which shows the whole structure of this invention Control block diagram of the control device of the present invention Control block diagram of force control unit of the present invention Control block diagram of reaction force generation unit of the present invention The figure explaining the correction function part of this invention The figure which shows the control apparatus of the conventional master slave manipulator

Explanation of symbols

DESCRIPTION OF SYMBOLS 1 Master manipulator 2 Slave manipulator 3 Operation handle 4 Fixed part 5 1st tool 6 Joystick 7 2nd tool 8 Control device 9a Input effective switch 9b Changeover switch 10-14 Signal line 15, 16 Servo amplifier 17 Workpiece 18 Worktable 19 Operator 20 Zero point correction unit 21 Correction function unit 22 Integrator 23 Switch 23a Slave side contact 23b Master side contact 24 Adder 40 Coordinate converter 41 Force control unit 42 Coordinate converter 43 Reaction force generation unit 44 Jacobian converter 45 Coordinate converter 46 Force control unit 47 Jacobi converter S1 Force sensor S2 Force sensor S3 Force sensor 101 Controller 102 Servo amplifier 103 Servo amplifier 104 Master manipulator 105 Force torque detector 106 Input lever
107 Slave manipulator 108 Force torque detector 109 End effector 110 Switch 111-114 Signal line 401 Forward converter 402 Differentiator 403 Virtual viscosity 404 Virtual rigidity 405 Force feedback gain 406-409 Adder 410 Proportional gain 411 Differentiator 412 Proportional gain 413 Adder

Claims (4)

In the control device of the master slave manipulator that controls the slave manipulator based on the operation of the master manipulator by the operator,
An operation handle disposed on the master manipulator via a force sensor;
A joystick that is arranged on the master manipulator so as to be in parallel with the operation handle, and operates the master manipulator or the slave manipulator;
Switching means for switching a manipulator to be operated by the joystick;
A switch for enabling or disabling operation input to the joystick;
Coordinate control means for cooperatively controlling the master manipulator and the slave manipulator according to the operation force of the operator acting on the operation handle detected by the force sensor, the output of the switching means, and the output of the switch. A control device for a master-slave manipulator.   The cooperative control means adjusts an operation input to the joystick according to an operation force acting on the operation handle detected by the force sensor based on a predetermined function when the input valid switch is valid. 2. The control device for a master-slave manipulator according to claim 1, wherein the control device is a master-slave manipulator.   The master / slave manipulator control device according to claim 1, wherein the cooperative control unit includes a correction unit that corrects an operation input to the joystick with a time point when the switch is enabled as a zero point. 4. The master-slave manipulator control device according to claim 1, wherein the joystick is installed on the master manipulator so as to be rotatable about the operation handle.

Images