JPS63150171A - Controller for manipulator - Google Patents

Abstract

(57) [Summary] This bulletin contains application data before electronic filing, so abstract data is not recorded.

Description

[Detailed Description of the Invention] [Field of Industrial Application] The present invention relates to a master-slave type manipulator,
In particular, the present invention relates to a manipulator control device suitable for improving the operation of this manipulator.

[Conventional technology]

In the conventional master-slave type manipulator,
Normally, the operator on the mask side operates the master arm to move the tip of the slave arm to a desired position while visually observing the state of the slave arm displayed on a television camera.

Regarding this type of operating means, for example, Japanese Patent Application Laid-Open No. 61-338
No. 78 and pages 543 to 546 of the 5th International Symposium on Industrial Robotics, which was held in Chicago from September 22 to 24, 1975.

[Problem that the invention seeks to solve]

” In the conventional technique, the direction of movement of the hand of the slave arm shown on the screen of the TV monitor does not match the direction of movement of the hand of the master arm operated by the operator on the mask side. The hand of the master arm must be moved in a direction different from the direction in which the hand of the slave arm on the monitor screen is to be moved, which is difficult to operate and requires skill. In particular, if multiple television cameras are installed to monitor the movement of the slave arm, and each image of the slave arm's tip taken by these television cameras is displayed on a single television monitor, the television camera Due to the location of the installation, the movement direction of the slave arm and master arm displayed on the TV monitor was different for each TV camera, which caused confusion for the operator and poor operability, as mentioned above. .

The present invention has been made based on the above-mentioned problems, and an object of the present invention is to provide a manipulator control device with good operability.

[Failure to solve the problem]

The above object of the present invention is to provide a master arm, a slave arm, a television camera for photographing the slave arm,
In this manipulator equipped with a television camera that displays an image of the slave arm that has been shadowed by the television camera, the operator's line of sight to the remaster arm tip or the line of sight to the slave arm tip on the television monitor and the light of the television camera to the slave arm tip. Determine the relative positional relationship between the master arm and slave arm, whose axes match, with the reference coordinate system. Based on this relative positional relationship, determine the amount of hand movement of the mask arm as the position on the slave arm's reference coordinate system, and calculate this position. This is achieved by providing an arithmetic circuit that outputs the target position of the slave arm.

[Effect]

The arithmetic circuit calculates the relative positional relationship between the master arm and the slave arm with respect to the reference coordinate system so that the movement direction of the slave arm end on the TV monitor and the movement direction of the master arm end match as seen from the operator. If the movement of the master arm's hand is controlled based on the relative positional relationship, the movement directions of the slave arm's hand of the television monitor and the master arm's hand will match. Therefore, the operability can be improved.

〔Example〕

Embodiments of the present invention will be described below with reference to the drawings.

Prior to a detailed explanation of the present invention, the principle of the present invention will be explained using FIG. 7. In this figure, 1 is the master arm,
2 is a slave arm; 3 is a television camera that photographs the hand of the slave arm 2; 4 is a television monitor that displays an image of the hand of the slave arm 2 taken by the television camera 3; 5;
indicates the operator's eyes on the mask side. The master arm reference coordinate system MO of the hand coordinate system MH of the master arm 1 described above
The relative positional relationship with respect to is expressed by coordinate transformation matrices T and I. Also, the line of sight vector LK from the operator's eye 5 to the tip of the master arm 1 is the operator's eye 5.
A coordinate transformation matrix L indicating the relative position of the mask reference coordinate system MO.

and coordinate transformation 71-RiksV, which indicates the position of the hand. On the other hand, similarly for the slave arm 2, the relative positional relationship of the hand coordinate system SH with respect to the slave reference Pit:standard system SO is expressed by the coordinate transformation 7 matrix Ts. In addition, the optical axis vectors I to LX from the television camera 3 to the hand of the slave arm 2 are a coordinate transformation matrix T-s indicating the relative position of the television camera 3 with respect to the slave reference coordinate system SO, and a coordinate transformation matrix indicating the position of the hand. It is determined from I-Riks T5. Now, suppose that master arm 1 is virtually moved to align the hand positions of master arm 1 and slave arm 2 so that line-of-sight vectors ■, K and optical axis vector LX overlap, then master arm 1 after movement ′
The relative position of the reference coordinate system M O' with respect to the slave reference coordinate system SO is expressed by coordinate transformations 71 to RxR.

The movable MN (of the hand of the mask arm controller) is transmitted to the slave arm 2 by changing its direction by coordinate transformation 71-RIXR so that the line-of-sight vector I-LE and the optical axis spectrum LX coincide. Therefore, the movement SM of the hand of the slave arm 2 is different from the movement MM of the hand of the master arm 2 in a different direction. However, the direction of the movement PM of the slave arm hand image P on the television monitor 4 placed on the line of sight connecting the operator's eyes 5 and the master arm's hand coincides with the direction of the movement MM of the master arm's hand. Therefore, the operator can operate the master arm 1 intuitively.

Next, the optical axis of the television camera 3 and the operator's eyes will be explained below. The reference coordinate system of the master arm 1 is MO1, the coordinate transformation matrix 7 from the reference coordinate system MO to the hand is T1, the coordinate transformation 71 from the reference coordinate system MO to the operator's eye 5
- Let the risk be LII.

Furthermore, the reference coordinate system of the slave arm 2 is So, the coordinate transformation matrix from the reference coordinate system SO to the hand is Ts, and the transformation matrix from the reference coordinate system SO to the television camera 3 is L8. In addition, the transformation matrix from the operator's eye 5 to the hand of the master arm 1 is
Assuming that the transformation matrix from to the tip of the slave arm 2 is 8, the transformation equation is as follows.

T,=L,・V, −(1)Ts
= Ls・Vs ・'12) The positions of the hands of master arm 1 and slave arm 2 match,
Assume that the master arm 1 is moved so that the operator's line of sight direction and the optical axis of the television camera 3 overlap. At this time,
If the translational transformation from the operator's eye 5 to the television camera 3 is D, then the reference coordinate system M after moving from the reference coordinate system S○
The conversion R to O' is as follows.

R= L-・D-T-s-'”'
(3) On the other hand, the rotational transformation U of the hand of the master arm 1- for aligning the line of sight with the optical axis of the television camera 3 is as follows.

U=(Ts)−” R−T−−(4)D=vl
l・(vs)-E-(5) If the hand positions of master arm 1 and slave arm 2 at the time of synchronization are Two and Tso, then conversion 71-Rix L
-, Ls are constant, so the value Do of each translational transformation and the value RO of transformation R at that time.

The value Uo of the rotational transformation U is determined as follows.

From equations (1), (2) and (5), Do=V-・(Vs)-1=(L,)-1・T,-(TS)-"-LS-(6)(
3), From formula (6), Ro=Lm-Do・(Ls)-Eng=T,・(TS) 1...(7)
From equations (4) and (7), Uo=(Ts)-”Ro-Tm=(T!+)-'・TII・(Ts)-1・Tm ・・
(8) Using this as a reference point, the target value of the hand coordinate system S H of the slave arm 2 when the master arm 1 is moved can be obtained from FIG. 7 using the following equation.

Ts=Ro-Tm(Uo)-'-(9)
Once the target value of the hand of the slave arm 2 is obtained, each joint angle target value of the slave arm 2 can be obtained by performing inverse coordinate transformation regarding the slave arm 2, as is well known. Therefore, it is sufficient to perform servo control of each axis muscle of the slave arm 2 so that each joint angle of the slave arm 2 becomes equal to the target value.

Next, one embodiment of the apparatus of the present invention will be described using FIG.

In Figure 1, 1 is the master arm, 2 is the master arm 1
Slave arm installed at a remote location, 3A, 3
B is the first and second camera that photographs the hand of slave arm 2.
A television camera 4 is a television camera, and this television monitor 4 displays an image of the hand of the slave arm 2 taken by the first television camera 3A or the second television camera 3B. 5 is the operator's eyes on the mask side;
6 is the second television camera 3A, the second television camera 3
A changeover switch for selectively switching B to TV monitor 4 is shown. 7A to 7C are angle detectors provided at each joint of the master arm 1-, 8A to 8C are actuators provided at each joint of the slave arm 2, 9 is an interface circuit, and 1o
1j- is a servo control circuit for the slave arm 2, 1j- is an arithmetic circuit, 1-2 is an input device such as a keyboard connected to the arithmetic circuit 11, 13 is a data storage unit, and 14 is a changeover switch that operates in conjunction with the changeover switch 6. , 15 is a switch circuit. The input device 12 mentioned above is the television camera 3A.
, 3B to the slave arm 2, a coordinate transformation matrix L indicating the position of the operator's eye 5 in the master arm reference coordinate system MO, and the like are input to the arithmetic circuit 11. Further, the arithmetic circuit 11 described above receives data Lm from the input device 12.
, LF, and each joint angle input information of master arm 1, first calculate the conversion values o, Ro, and Uo based on the above-mentioned equations (6), (7), and (8), and then calculate The value Do + Ro t Do is applied to equation (9), and the target value of the hand of the slave arm 2 is obtained from the input information of each joint angle of the master arm 1. Furthermore, the target angle of each joint of the slave arm 2 is calculated by an inverse transformation calculation. and outputs this to the servo control circuit 10. The data storage unit 13 described above stores conversion values Do calculated for each of the television cameras 3A and 3B.

The values of Ro and Uo are saved. The switch circuit 15 selects the television cameras 3A and 3 selected by the changeover switch 14.
The converted value corresponding to B is selected and output to the arithmetic circuit 11-.

A specific configuration of the above-mentioned interface circuit 9 is shown in FIG. In this figure, rotary pulse generators are used as the angle detectors 7A to 7C. Angle detector 7A~
From 7C, a set of pulse signals, that is, phase A and phase B, with a phase shift of 90'' is generated according to the rotation angle. This signal is input to the direction determination circuit 9A to determine the direction of the rotation angle. The phase or B phase signal is input to the counter 9B, and the number of pulses is counted.The direction signal 9C output from the direction determination circuit 9A is input to the counter 9B,
Changes the number of pulses to increase or decrease. Therefore, since the value of the counter 9B increases or decreases in accordance with the increase or decrease in the rotation angle, the rotation angle can be detected by reading the output 9D of the counter 9B from the outside.

A specific example of the arithmetic circuit 11 mentioned above is shown in FIG.

This includes a processor IIA that performs data input/output control and addition/subtraction, a memory 11B that stores data such as trigonometric function tables and link parameters of manipulators, a multiplier 11G, and a divider 11D that are connected to the bus circuit 11. Connected by E. Furthermore, the bus circuit 11E includes a serial interface circuit 11F and a parallel interface circuit 1.
1 (3) are connected to these interface circuits gllF, a serial input device 12 such as a keyboard, and a changeover switch 6,
An interface circuit 9 and a servo control circuit 10 of the angle detectors 7A to 7C are connected. Processor IIA
can access all devices connected to the circuit 11E via the bus circuit 11E and process data.

Next, a specific example of the servo control circuit 10 is shown in FIG. The output from the arithmetic circuit 1] passes through a subtracter 10A, is converted into an analog signal via a digital-to-analog conversion circuit 10B, and is input to actuators 8A to 8C. Actuators 8A to 8C drive each joint of slave arm 2 and rotate its angle detectors 8D to 8F. The output signal from the angle detector @29 is input to the interface circuit 10G to generate an angle signal, which is read by the processor 11A via the parallel interface circuit 11G and also input to the subtracter 10A. Subtractor 10
The output of A is the difference between the target value signal output from the calculation circuit 1 and the angle signal input from the angle detectors 8D to 8F via the interface circuit 10C, so the actuator 8A is adjusted so that this difference becomes zero. By driving ~8C, the joint angle can be made to match the target value.

Next, the operation of one embodiment of the above-mentioned apparatus of the present invention will be described.

First, when starting at the initial position, each joint angle of master arm 1 and slave arm 2 is read.

Next, trigonometric functions are determined by referring to the trigonometric function table stored in the memory 1.1B of the arithmetic circuit 11. Furthermore, a positive conversion operation is performed using the values of the obtained trigonometric functions to obtain the hand coordinate T of the master arm 1 and the hand coordinate Ts of the slave arm 2 at the initial position. Furthermore, the above (
6), (7), and (8), the converted values Do and Ro r
Calculate Uo.

After this, the joint angle of the master arm unit is read again, and the target value T of the hand of the slave arm 2 is determined according to the above equation (9).
Find s. Next, target values for each joint of the slave arm 2 are determined by inverse coordinate transformation, and these are output to the servo control circuit 10. Thereby, the movement direction of the hand of the slave arm 2 and the movement of the hand of the master arm 1, which are displayed on the television monitor 4, can be changed. Then, the above calculation is repeatedly executed until the operation of the manipulator is completed. Therefore, if the position of the hand of the master arm co moves, the position of the hand of the slave arm 2 also moves, and the movement of the hand of the master arm 1 is transmitted to the hand of the slave arm 2.

Further, as shown in this embodiment, when there are a plurality of television cameras, the values of constant matrices Ro, Uo, and Do are calculated for each television camera 3A and 3B and stored in the data storage unit 13, and A constant matrix corresponding to the selected TV camera is selected in synchronization with the changeover switch 6 of the cameras 3A and 3B in the switch circuit 15, and (9
) to apply to the expression. By doing this, the movement direction of the slave arm 2 displayed on the TV monitor 4 always matches the movement direction of the master arm 1 at the same time as the TV camera is switched, so the operator can
can always be operated intuitively.

FIG. 6 shows another embodiment of the invention.

In the embodiment shown in FIG. 1, a television monitor 4 is placed between the operator's eyes 5 and the tip of the master arm 1. Although this is the most accurate method in principle, in reality it is relatively difficult to place the TV monitor 4 in this position, and generally the TV monitor 4 is placed farther away than the tip of the master arm. There are many things. In this case, the TV monitor 4 should be positioned as far as possible on the extension of the line of sight X connecting the operator's eyes 5 and the tip of the master arm 1; is difficult to see. Due to layout considerations, if the line of sight is placed away from the line of sight Virtual line of sight Z
Assuming that, this virtual line of sight replaces the line of sight X and is superimposed so as to coincide with the optical axis of the television camera 3, and a coordinate transformation calculation is performed. In this way, the motion PM of the hand of the slave arm 2 and the motion MM of the hand of the master arm 1 on the image become parallel, and the sense of discomfort is reduced.

In the above-mentioned embodiment, the master arm 1 and the slave arm 2 are represented in a planar form with three degrees of freedom for ease of understanding, but even if the number of degrees of freedom increases and becomes three-dimensional, there is no difference in the above explanation. , the present invention is realized. Furthermore, the present invention is applicable even if the master arm 1 and slave arm 2 have different structures.

〔Effect of the invention〕

According to the present invention, the movement direction of the slave arm's hand displayed on the television monitor matches the movement direction of the master arm's hand, so the operator can intuitively move the master arm while looking at the slave arm on the television monitor. can be operated, and its operability can be significantly improved.

[Brief explanation of the drawing]

FIG. 1 is a diagram showing the configuration of one embodiment of the device of the present invention, and FIG.
The figure shows an example of an interface circuit constituting the device of the present invention, FIG. 3 shows an example of an arithmetic circuit constituting the device of the present invention, and FIG. 4 shows a servo control circuit constituting the device of the present invention. A diagram showing an example of the circuit, FIG. 5 is a flowchart diagram explaining the operation of the device of the present invention, FIG. 6 is a diagram depicting another embodiment of the present invention, and FIG. 7 is an explanation showing the control principle of the present invention. It is a diagram. DESCRIPTION OF SYMBOLS 1... Master arm, 2... Slave arm, 3° 3A, 3B... TV camera, 4... TV monitor, 6... Selector switch, 9... Interface circuit, 10...・Servo control circuit, 1.1... Arithmetic circuit,
13...Data storage unit, 15...Switch circuit.

Claims (1)

[Claims] 1. In a manipulator that includes a master arm, a slave arm, a television camera that photographs the slave arm, and a television monitor that displays an image of the slave arm photographed by the television camera, the operator's master Find the relative positional relationship between the master arm and the slave arm with respect to the reference coordinate system in which the line of sight to the arm tip or the line of sight to the slave arm tip on the TV camera and the optical axis line of the TV camera to the slave arm tip match, and A control device for a manipulator, comprising an arithmetic circuit that determines the amount of hand movement of a master arm as a position on a reference coordinate system of a slave arm based on a relative positional relationship, and outputs this position as a target position of the slave arm. 2. A storage means for storing coordinate transformation calculation formulas corresponding to a plurality of television cameras, and a switch circuit for reading the corresponding calculation formulas from the storage means and outputting them to the calculation circuit upon selection of switching of the television cameras. A control device for a manipulator according to claim 1, further comprising: a control device for a manipulator according to claim 1;