JP2520660B2 - Remotely operated manipulator device - Google Patents

Description

TECHNICAL FIELD The present invention relates to a remote-controlled manipulator device used in a nuclear power plant or the like.

[Conventional technology]

FIG. 3 is a conceptual diagram showing a conventional remote-controlled manipulator device disclosed in, for example, Japanese Patent Application No. 61-155130, in which 1 is an X direction or a Y direction for giving a command to the manipulator 3. , Operating rod with 3 degrees of freedom in Z direction, 2
Is a control unit that controls the movement of the manipulator 3 according to the operation amount of the operating rod 1, 4a and 4b are television cameras (hereinafter referred to as TV cameras) for remotely monitoring the movement of the manipulator 3, and 5 are TV cameras 4a and 4b. Is a monitor television (hereinafter referred to as a monitor TV) that selects and projects the image.

In addition, 8 is a partition wall for separating the operator, 6 is piping,
Reference numerals 7a and 7b are valves attached to the pipe 6 and are the operation targets of the manipulator 3.

FIG. 4 is a control rod 1 of the manipulator device shown in FIG.
, Control unit 2, manipulator 3, TV camera 4,
It is a block diagram showing the mutual relation of the monitor TV5, and the same or corresponding parts in FIG. Reference numeral 10 is an operating rod attitude detecting means for detecting the speed command vector from the movement of the operating rod 1, and 14 is a TV camera.
Viewpoint detecting means for detecting the position and attitude of the operating TV camera among 4a and 4b, 11 is a speed command vector input from the operating rod attitude detecting means 10 based on the information from the viewpoint detecting means 14 and the speed of the visual coordinates. Visual coordinate conversion means for converting into a command vector, 12 is a joint coordinate conversion means for converting a velocity command vector of this visual coordinate into an angular velocity vector which is a motion of each joint axis of the manipulator 3, and 13 is a hand of the manipulator 3 according to the angular velocity vector. It is a joint axis control means for moving the vehicle according to the velocity command vector.

 Next, the operation will be described.

FIG. 5 is a perspective view showing the details of the positional relationship between the TV cameras 4a, 4b and the manipulator 3 in FIG. 3, and the same or corresponding parts as in FIG. 3 are designated by the same reference numerals. Here, for simplicity of explanation, it is assumed that the arm of the manipulator 3 at the initial position and the TV camera 4a are parallel to each other with the TV camera 4a as a reference. On the other hand, the TV camera 4b has a TV around the y-axis.
Since the camera 4a is rotated by θy = 180 °, the TV camera 4a
And the optical axes are the same, and the directions are opposite.

When operating the valve 7a while monitoring the work with the TV camera 4a, the relationship between the TV image and the operation rod 1 is as shown in FIG. 6 (a). When the operating rod 1 is moved in the operating direction, the operating rod attitude detecting means 10 causes the speed command vector = (, 0,0)
^t is obtained. The speed command vector is multiplied by the conversion matrix H obtained from the information of the viewpoint detecting means 14 in the visual coordinate converting means 11 to obtain the speed command vector in the visual coordinates. Since the transformation matrix H is limited to rotation around the y axis in FIG. 5, In this case, since it is the reference position, θy = 0 Becomes a unit matrix, and the velocity command vector in visual coordinates
_V also does not change at _{V 2} = H · =.

Thereafter, the joint coordinate conversion means 12 obtains the angular velocity vector, and the joint axis control means 13 controls the operation of the manipulator 3.

On the other hand, the relationship when operating the valve 7b while monitoring with the TV camera 4b is shown in FIG. 6 (b). In this case, if the operating rod 1 is moved in the operating direction, the operating rod attitude detecting means 10 can obtain the speed command vector = (, 0,0) ^t . In the visual coordinate transformation means 11, if the vector is multiplied by the transformation matrix H determined by the rotation angle θy = 180 ° of the viewpoint from the reference position obtained by the viewpoint detection means 14 to obtain the velocity vector _V in the visual coordinates, This indicates that the manipulator 3 is moved based on the operation direction of the operation rod 1 and the velocity command vector − having the opposite sign. By reversing the actual traveling direction of the manipulator 3, FIG. As shown in b), the operating direction of the operating rod 1 and the advancing direction of the manipulator 3 on the screen are matched.

In the above description, the case of θy = 180 ° in the rotation around the y axis has been described, but any change in the viewpoint can be obtained by a combination of rotations around the x, y, and z axes, and the manipulator 3 on the screen is always displayed. The advancing direction of and the operating direction of the operating rod 1 are kept the same.

In addition, as a command from the operating rod 1 to the manipulator 3,
Although the example of inputting the speed command is shown, the position command may be input.

Further, FIG. 7 is a conceptual diagram showing another conventional remote-controlled manipulator device shown in 1310 of the 4th Annual Conference of the Robotics Society of Japan, which is the same as or equivalent to FIG. It is shown with reference numerals. Further, 40 is a position as an operation amount of the operating rod detected by the operating rod attitude detecting means 10, and 32 is a manipulator position command value obtained by coordinate-converting the position of the operating rod 1 by the coordinate converting means 11.
A position control unit 23 controls the joint of the manipulator 3 according to the position command value. FIG. 8 is a schematic diagram showing the positional relationship between the operating rod 1 and the operator's eyes 43, and the positional relationship between the manipulator 3 and the TV camera 4. In the figure, 44 is an operation coordinate system fixed to the base of the operation rod 1, 45 is a visual coordinate system fixed to the operator's eyes 43, 46 is an operation end coordinate system fixed to the tip of the operation rod 1, 47 Is an arm coordinate system fixed to the base of the manipulator 3, 48 is a hand coordinate system fixed to the tip of the manipulator 3, and 49 is a camera coordinate system fixed to the TV camera 4. Operating coordinate system 44 to visual coordinate system 45
To L ^m , the homogeneous transformation matrix to the control end coordinate system 46 to T ^m , and the homogeneous transformation matrix from the arm coordinate system 47 to the camera coordinate system 49 to L ^s , the hand coordinate system 48. The homogeneous transformation matrix of T ^s
Then, T ^m represents the position and orientation of the tip of the operating rod 1, that is, the operation amount 40 of the operating rod 1, and T ^s represents the position and orientation of the manipulator 3. A matrix ▲ T ^s _c ▼ representing the position command value of the manipulator 3 is a homogeneous conversion matrix ▲ R ^-10 ₀ U ₀ into a matrix T ^m representing the operation amount of the operation rod 1 in the coordinate conversion means 11.
By multiplying by, it is calculated that ▲ T ^m _c ▼ = ▲ R ^-1 ₀ ▼ U ₀ T ^m . Here, R ₀ and U ₀ are also homogeneous transformation matrices and R ₀ = L ^m D
₀ (L ^s ) ^-1 , U ₀ = R ₀ ▲ T ^s ₀ ▼ (▲ T ^m ₀ ▼) ^-1 ,, D ₀ = (L ^m )
^-1 ▲ T ^m ₀ ▼ (▲ T ^s ₀ ▼) ^-1 L ^s . Also ▲ T ^m ₀ ▼, ▲
T ^s ₀ ▼ represents the values of T ^m and T ^s in the initial state, and −1 on the right shoulder of the symbol represents an inverse matrix.

[Problems to be solved by the invention]

Since the conventional remote-controlled manipulator is configured as described above, when a speed command is given from the operating rod 1, if there is a slow-response joint in the joint axis control of the manipulator 3, the joint will have a joint coordinate conversion means. Since it cannot follow the angular velocity vector output from the manipulator 3, the manipulator 3 does not move according to the velocity command vector, and the movement direction of the manipulator on the screen deviates from the operation direction of the operating rod, which makes it difficult to operate the manipulator 3 as desired. , When a position command is given from the operating rod 1, the operating rod 1
If you change the attitude of TV cameras 4a and 4b without operating
Since the manipulator 3 is controlled so as to maintain a constant position and orientation on the screen of the monitor TV5, the manipulator 3 moves according to the movement of the TV cameras 4a and 4b. For this reason,
When moving the TV cameras 4a and 4b, it is necessary to pay attention to the movement of the manipulator 3, which causes a problem that the operator's burden is increased.

In the another conventional example, the homogeneous transformation matrix ▲ R
^-1 ₀ ▼ When the U ₀ is substituted for ^{_{U 0 = R 0 ▲ T s}} 0 ▼ (▲ T m 0 ▼) -1, ▲ R -1 0 ▼ U 0 = ▲ T s 0 ▼ (▲ T m 0 ▼ ) As it can be seen from ^-1 , this coordinate transformation is decided only by the initial states of the operating rod 1 and the manipulator 3. Therefore, when the posture of the TV camera 4 changes during the operation of the manipulator 3, such as when the TV camera 4 is installed on a rotatable mount, such a coordinate conversion method cannot deal with the operation. When a position command is given from the rod 1 to control the position of the manipulator 3, there is a problem that the movement direction of the manipulator 3 on the screen of the TV monitor 5 does not correspond to the operation direction of the operating rod 1.

The present invention has been made in order to solve the above-mentioned problems. Even when a speed command is given from the operating stick, a position command is given, or a posture of a TV camera or the like changes during operation. , The operating direction of the operating stick and the moving direction of the manipulator displayed on the monitor TV can be always matched, and the movement amount of the manipulator is determined only by the operating amount of the operating stick, and the posture of the TV camera etc. is changed. Even if the manipulator is not operated even if the operating rod is not operated, it is an object to obtain a remote-controlled manipulator device.

[Means for solving problems]

The remote-controlled manipulator device according to the present invention generates an incremental position command value for each fixed time interval of the manipulator by the incremental command generation means in accordance with the operation amount of the operating rod,
Using the coordinate conversion means, the incremental position command value is subjected to coordinate conversion so that the operating direction of the operating rod and the movement direction of the manipulator on the monitor television match, and the coordinate-converted incremental position is obtained. The position command value of the manipulator is generated by accumulating the command values, and the position of the manipulator is controlled.

[Action]

The position command generating means in the present invention gives a position command value to the manipulator and controls the position of the manipulator. Therefore, when a speed command is given from the operating rod, the same effect as the integral compensation in the orthogonal coordinate system is performed. Is obtained, and the motion of the slow response joint is compensated,
The speed of movement of the manipulator matches the command speed given by the operating rod. When a position command is given from the operating rod, the incremental command generating means generates an incremental command value from the change amount of the operating amount of the operating rod at constant time intervals, and the coordinate converting means converts the coordinate value. Therefore, even if the attitude of the TV camera changes, the position command value generated by the position command generating means does not change unless the operating rod is operated. Further, since the coordinate conversion matrix is changed according to the attitude of the TV camera, even if the attitude of the TV camera is changed during operation, the movement direction of the manipulator on the screen and the operation direction of the operation stick always match. .

Example of Invention

An embodiment of the present invention will be described below with reference to the drawings. In FIG. 1, 1 is an operating rod for instructing a speed command vector which is an instruction to the manipulator 3, and 20 is an operating rod 1.
Incremental command generating means for generating a first incremental command value 30 from the manipulated variable of 4 is a TV camera for monitoring work, 5
Is a monitor TV, 14 is a viewpoint detecting means for detecting the attitude of the TV camera 4, 21 is coordinate conversion of the first increment command value 30 inputted from the increment command generating means 20 based on the information from the viewpoint detecting means 14. Coordinate conversion means for outputting a second increment command value 31,
22 is a position command generating means for accumulating the second incremental position command value 31 to output a position command value 32 of the manipulator, and 23 is position control means for controlling the position of the manipulator 3 in accordance with the position command value 32.

 The operation of this embodiment will be described below with reference to the drawings.

2A is a perspective view showing the positional relationship between the TV camera 4 and the manipulator 1 shown in FIG. 1, and FIG. 2B is a perspective view showing the positional relationship between the monitor TV 5 and the operating rod 1.
The same or corresponding parts as in FIG. 1 are designated by the same reference numerals. Here, four coordinate systems are used to describe the operation. 44 is an operating coordinate system fixed to the base of the operating rod 1, 42 is a monitor coordinate system fixed to the monitor TV5, 47 is an arm coordinate system fixed to the base of the manipulator 3, and 48 is a fixed to the TV camera 4. The camera coordinate system. Regarding the coordinate axes of the monitor coordinate system 42, the depth direction of the screen is the x axis and the upward direction is the z axis. As for the coordinate axis direction of the camera coordinate system 48, the direction of the line of sight of the TV camera 4 is the x axis and the upward direction is the z axis. This TV
The manipulator 3 is moved by operating the operation rod 1 while monitoring the image captured by the camera 4 on the monitor TV 5.

Now, let us say that the operation amount vector of the operation rod 1 viewed from the operation coordinate system 44 is X ₀ . This X ₀ is a three-dimensional vector. In the increment command generation means 20, this manipulated variable vector X ₀ is input,
First, the velocity command vector V ₀ = proportional to the manipulated variable vector X ₀
Make kX ₀ . Here, k is a constant. Then, the first increment command value 30 at which the manipulator 3 should move within the constant time interval ΔT seconds is generated as ΔX ₁ = ΔTV ₀ = ΔTkX ₀ by multiplying the speed command vector V ₀ by ΔT.

Next, the first incremental position command value 30 is converted into the second incremental position command value 31 by the coordinate conversion means 21. The calculation formula in the incremental command conversion means 20 is as follows. That is, the x-axis, y-axis, z-axis of the operation coordinate system 44 viewed from the monitor coordinate system 42.
Let X _m , Y _m , and Z _m be the axis direction vectors, and let H ₁ = [X _m , Y _m , Z _m ] be a 3 × 3 matrix having these as column vectors. Also, x of the arm coordinate system 47 as seen from the camera coordinate system 48
Axis, y-axis, the three-dimensional direction vector of the z-axis and X _a, Y _a, Z _a, these a matrix of 3 × 3 to a column vector H ₂ = [X _a, Y _a, Z _a]
And At this time, the second increment command value 31 is ΔX ₂ = ▲ H ^-1 ₂ by multiplying ΔX ₁ which is the first increment command value 30 by a 3 × 3 matrix ▲ H ^-1 ₂ ▼ H _1. It can be calculated as ▼ ・ H ₁・ ΔX ₁ . Assuming that the manipulator 3 moves by ΔX ₂ in the arm coordinate system 47 during the constant time interval ΔT seconds, the moving amount of the manipulator 3 seen from the camera coordinate system 48 is ΔX _a = H ₂ · ΔX ₂ = H ₁ · ΔX ₁ . , The moving speed of the manipulator 3 viewed from the camera coordinate system 48 is V _a = ΔX _a / ΔT
= H ₁ · ΔX ₁ / ΔT = H ₁ · V ₀ = kH ₁ · X ₀ . Monitor tv5
Since the image captured by the TV camera 4 is displayed as is, the moving speed of the manipulator in the monitor coordinate system 42 is V _m = V _a = kH ₁ · X ₀ . This is the operation coordinate system 44
When the coordinates are converted into, the speed V of the manipulator viewed from the operation coordinate system 44 becomes V = H ₁ ^-1 V _m = V ₀ = kX ₀ , which coincides with the speed command value given from the operation rod 1. That is, monitor TV5
The movement direction of the manipulator 3 seen through the
Match the operation direction of. In this way, the coordinate conversion means 21
The second increment command value 31 generated in step 2 is the position command generating means 22.
Are integrated and output as the position command value 32 of the manipulator 3 to the position control means 23. The position control means 23 controls the manipulator 3 according to the given position command value 32.

It should be noted that in the above embodiment, the operation rod 1 to the manipulator 3 are used.
Although the speed command is given, the position command value may be given from the operating rod 1. In this case, the increment command creating unit inputs the operating amount of the operating rod 1 at constant time intervals ΔT seconds. Then, the difference between the current operation amount and the operation amount input one time before may be multiplied by a coefficient to obtain the first incremental position command value.

Further, although the operating stick 1 is of the joystick type in the above-mentioned embodiment, it may be of another type such as a master manipulator, and the same effect as that of the above-mentioned embodiment is obtained.

Further, in the above-described embodiment, the case where one TV camera 4 and one monitor TV5 are used is shown, but when a plurality of TV cameras are switched to project on one monitor TV, or when a plurality of TV cameras are used.
The present invention can be applied to the case of monitoring with a plurality of monitor TVs corresponding to TV cameras, and has the same effect as the above embodiment.

Furthermore, even if the position or posture of the base on which the manipulator 3 is installed changes, such as when the manipulator 3 is mounted on a mobile vehicle, etc., if the positional relationship between the base of the manipulator 3 and the TV camera 4 is known, the above-mentioned operation is performed. As in the example, the movement of the manipulator 3 on the screen can be associated with the operation on the operation rod 1.

〔The invention's effect〕

As described above, according to the present invention, an incremental position command value is generated for each constant time interval from the operation amount of the operating rod, and after the incremental command value is subjected to coordinate conversion, a position command value is generated and the manipulator is operated. Since it is configured to control the position of the manipulator, the movement of the manipulator on the monitor TV screen and the operation of the operation stick are in good agreement, and even if the viewpoint of the TV camera changes, the manipulator is inadvertently moved unless the operation stick is operated. It is possible to prevent it, and further, there is an effect that an operation is easy and a highly safe product is obtained.

[Brief description of drawings]

FIG. 1 is a block diagram of a remote-controlled manipulator according to an embodiment of the present invention, FIG. 2 is a perspective view showing the positional relationship between a TV camera and a manipulator, and the positional relationship between a monitor TV and an operating stick. FIG. FIG. 4 is a conceptual diagram of a remote-controlled manipulator, FIG. 4 is a configuration diagram of a conventional remote-controlled manipulator, FIG. 5 is a perspective view showing a relationship between a conventional TV camera and a manipulator, and FIGS. 6 (a) and 6 (b) are FIG. 7 is an explanatory view showing the relationship between a conventional TV screen and an operating stick, FIG. 7 is a block diagram of a conventional remote-controlled manipulator, and FIGS. 8A and 8B are positions of the conventional operating stick and operator's eyes. With relationships and manipulators
It is a schematic diagram which shows the positional relationship of TV cameras. 1 is an operating stick, 2 is a control unit, 3 is a manipulator, 4 is a television camera, 5 is a monitor television, 14 is a viewpoint detecting means, 20 is an increment command generating means, 21 is a coordinate converting means,
22 is a position command generating means, 42 is a monitor coordinate system, and 47 is an arm coordinate system. In the drawings, the same reference numerals indicate the same or corresponding parts.

Claims (6)

(57) [Claims] 1. A remote-controlled manipulator device for operating a manipulator via a control unit by operating a manipulator while monitoring work with a television camera and a monitor television, wherein the control unit controls the manipulator. Incremental command generating means for inputting a manipulated variable and outputting a first incremental command value corresponding to this manipulated variable, and an operating direction of the operating rod and a moving direction of the manipulator on the monitor television are coincident with each other. Coordinate conversion means for coordinate-converting the first increment command value to obtain a second increment command value;
And a position command generating means for generating a position command value of the manipulator by accumulating the incremental command values of the remote manipulator device. 2. The control unit converts the first increment command value into a second increment command value by the coordinate transforming unit based on the output data from the viewpoint detecting unit that detects the posture or the position and posture of the television camera. The remote-controlled manipulator device according to claim 1. 3. The coordinate conversion means outputs the first increment command value to the second increment command based on the output of the arm posture detection means for detecting the posture or the position and posture of the base on which the manipulator is installed. A remote-controlled manipulator device according to claim 1 or 2, wherein the manipulator device is converted into a value. 4. The control unit converts the first increment command value into a second increment command value by the coordinate conversion unit based on the output of the monitor posture detection unit that detects the posture of the monitor television. The remote-controlled manipulator device according to any one of claims 1 to 3. 5. The method according to claim 1, wherein the increment command generating means generates a first increment command value proportional to the operation amount of the operating rod. Remote controlled manipulator device. 6. The method according to claim 1, wherein the increment command generating means generates a first increment command value proportional to the amount of change of the operating amount of the operating rod at constant time intervals. 7. A remote-controlled manipulator device according to any one of 1.