JPH0428507B2 - - Google Patents

Description

[Detailed description of the invention]

[Field of Application of the Invention] The present invention relates to a device for restricting the operating range of a manipulator, and particularly to a device for controlling the operation range of a master/slave manipulator for performing work in nuclear power plants or the like where it is difficult for humans to approach. , relates to a motion range limiting device that can limit the range to an appropriate range without impairing operability. [Prior Art] As is well known, the arm of a manipulator is driven by a joint, but its range of motion needs to be controlled so that the arm does not collide with other mechanical parts of the manipulator or other objects. Conventional examples of mechanisms for limiting the range of motion of manipulator joints for this purpose include:
For example, there is JP-A-53-17880. This mechanism compares the position signal indicating the boundary value of the movement range with the target signal of the arm joint angle in the position control system that controls the joint angle of the arm, and if the target signal exceeds the boundary value. In this case, the target signal is held at the current value and the joint angle of the arm is controlled to stop at the current position. However, this method does not have a mechanism for returning the arm joints to the range of motion and allowing them to function normally, so it is difficult to restore the joint angles when they move out of the range of motion. There is a problem. In addition, in the case of a master-slave type manipulator, in which the slave manipulator is configured to be remotely controlled according to the movement of the master manipulator manually operated by the operator, the joint state of the slave manipulator is A known method is to use a lamp or the like on a control console to notify the operator when the operating range has been exceeded. However, with this method, the operator is operating the master manipulator by the tip of the master manipulator.
The problem was that it was difficult to tell whether the joint would return to its range of motion. In particular, if two or more joints fall outside the range of motion, it becomes very difficult to return them to the range of motion, making this problem even more serious. At the same time, the operator temporarily suspends work by remote control,
This had the disadvantage of poor operability, as the user had to move his/her line of sight from the work object to the lamps, etc. on the control console. [Object of the Invention] An object of the present invention is to provide a master controller that can easily and quickly return the joint angle of the arm of the manipulator to within the operating range when it falls outside the preset operating range. An object of the present invention is to provide a device for limiting the operating range of a slave-type manipulator. [Summary of the Invention] The present invention allows the slave manipulator to stop at that position at the moment the arm of any manipulator exceeds a preset range of motion, and at the same time stops the master manipulator operated by a human being. A function is added to generate a torque that allows movement within the range of motion, and the operator feels this torque as an impact force, allowing the operator to easily sense joints that have exceeded the range of motion and the direction of operation to return them. This feature makes it easy to return each joint to its range of motion. [Embodiments of the Invention] Examples of the present invention will be described below with reference to the drawings. FIG. 1 shows the overall configuration of a master/slave manipulator to which the present invention is applied.
It is composed of a master manipulator 1 and a slave manipulator 2 that are similar in shape to each other, a control device 3, and a monitor television 4. The manipulators 1 and 2 have a structure similar to a human arm, and include base parts 11 and 21, and upper arms 13 and 23 connected to the base parts 11 and 21 at shoulder joints 12 and 22 at one end of the base parts, respectively. , forearms 15, 25 connected to the upper arms 13, 23 at elbow joints 14, 24 at one end of the upper arms 13, 23, and wrist joints 16, 26 at the tips of the forearms. The upper end of the base portion 11 of the master manipulator 1 is connected to the frame 10, and the lower end of the base portion 21 of the slave manipulator 2 is connected to the traveling vehicle 20. A handle 17 including finger parts is connected to the master's wrist link 16. A finger portion 27 is connected to the slave wrist joint 26 . Manipulators 1 and 2 are arranged as shown in the arrows in Fig. 1 and in Table 1.

[Table] It has 7 degrees of freedom, M ₁ to M ₇ and S ₁ to S ₇ . That is, M ₁ (S ₁ ) is the base portion 11, 21
represents the movement of rotating around its central axis relative to the frame 10 (traveling vehicle 20), and M ₂ , M ₃
(S ₂ , S ₃ ) are upper arm 13, 23, forearm 15, 25
represents the movement of rotation in the vertical plane around the shoulder joints 12, 22 and the elbow joints 14, 24, respectively. Furthermore, M ₄ , M ₅ , M ₆ (S ₄ , S ₅ , S ₆ ) are the finger portions 17,
27 represents a rotation mode in which an arbitrary posture can be taken with respect to the wrist joints 16, 26.
M ₇ (S ₇ ) represents the opening and closing motion of fingers to grasp an object. An outline of the operation method and operation of such a manipulator is as follows. The operator 5 controls the position of the slave manipulator 2 by operating the master manipulator 1 while viewing the image of the slave manipulator 2 displayed on the monitor television 4 . That is, by grasping the handle 17 and moving the tip of the master manipulator 1, the operator 5 moves each joint of the master manipulator 1, and with this movement, the slave manipulator 2 moves.
Each joint of the master manipulator 1 is connected to each joint of the master manipulator 1. As a result, the joint postures of the master manipulator 1 and slave manipulator 2 are maintained in similar shapes, so the operator 5 can intuitively determine the posture of the slave manipulator from the posture of the master manipulator. can be known in detail. At the same time, the force applied to the slave manipulator 2 from the outside is detected by a force sensor, etc., and fed back to the motor that drives each joint of the master manipulator 1. A corresponding reaction force is generated. As a result, the pilot 5
The slave manipulator 2 can be operated while feeling the force being applied to it. As shown in FIG. 2, the joint control mechanism for realizing the above-mentioned operations is independently controlled for each corresponding joint system of the master and slave. That is, the master base rotation motor 31a and the position detector 41 on the master manipulator 1 side
a, a joint system for base rotation from the force detector 51a, the slave base rotation motor 31b on the slave manipulator 2 side, the position detector 41b, the force detector 51b, and the servo calculator 61 in the control device 3; A control mechanism is constructed. Similarly, the master shoulder joint motor 32a and position detector 42 on the master side are explained below.
a, a force detector 52a, a slave upper arm swing motor 32b on the slave side, a position detector 42b, a force detector 52b, and a servo calculator 62 in the control device 3;
A joint system for swinging the upper arm is constituted by a motor 37a for opening and closing the master finger on the master side, and a position detector 47.
a, the finger opening/closing joint system consisting of the force detector 57a, the slave finger opening/closing motor 37b, the position detector 47b, the force detector 57b, and the servo calculator 67 in the control device 3 is the same. It is a composition. Although these joint control systems differ in appearance as shown in FIG. 1, the servo mechanism that is the basis of their control is the same. The motion range control device of the present invention is incorporated into the above-mentioned servo computing units 61 to 67, but since all joint systems are the same, the embodiment of the one-joint system for base rotation shown in FIG. 3 will be described here. Let me explain. In FIG. 3, the master side mechanism unit transmits torque around the axis applied to the joint shaft with a motor 31a connected to the joint shaft by a transmission mechanism including a reducer (not shown). a force detector 51a for detecting;
It is composed of a position detector 41a that detects the rotation angle of the joint axis. Further, the mechanism section on the slave side has the same configuration as the master side, and includes a motor 31b, a force detector 51b, and a position detector 41b. The circuits shown in FIG. 3 other than these mechanical sections belong to the servo calculation section 61, and include the force detectors 51a and 5.
A subtracter 113 that calculates the difference between the output signals 203 and 204 of 1b, and a subtracter 114 that subtracts the slave position target signal 209 and current position signal 201.
, a PID control circuit 115, and amplifiers 116a and 116 that drive the motors on the master side and slave side.
b, and the operating range limiting circuit 127 constituting the present invention.
(within the dashed line). The operation range limiting circuit 127 includes a reaction force generation circuit 119 of the operation range detection circuit 117 and the gate circuit 118, a switch 120a that switches between the output signal 210 of the reaction force generation circuit 119 and the force deviation signal 205, and a master current position signal 200. and a switch 120b that switches the signal 209b that latches the slave target signal, and a latch circuit 12 that latches the slave target signal 209.
1. The operating range detection circuit 117 includes a memory 122a that defines the upper limit of the operating range, as shown in FIG.
, a memory 122b that defines the lower limit thereof, and an upper limit signal 211 output from each memory.
and comparators 123a and 123b that respectively compare the lower limit signal 212 and the master current position signal 200. The reaction force generation circuit 119 includes memories 124a and 124b that define the magnitude of the reaction force acting in the direction within the operating range, as shown in FIG.
It is composed of switches 125a and 125b that turn on and off the outputs of the respective output signals 213 and 214, and an adder 126 that adds the output signals of these switches. The operation of the joint system shown in FIGS. 3 to 5 will be described below, starting with the operation of the force feedback bilateral servo system. The position of the joint angle on the master side and the position of the joint angle on the slave side are determined by the respective position detectors 41a,
41b as the current position signals 200, 201, and is converted to the position deviation signal 202 via the subtracter 114 (at this time, 120b is detected as the current position signal 200, 201).
(select 0). The position error signal 202 is sent to the motor 31b via the PID control circuit 115 and the amplifier 116b.
is input. Then, the motor 31b rotates, the position signal 201 changes, and eventually the position deviation signal 2
02 is controlled to become 0. Therefore, when the operator operates the master joint, the position of the slave joint is controlled so that it follows the master joint. At the same time, the force being applied from the outside to the master side and the slave side is detected by the respective force detectors 41a,
41b as current torque signals 203 and 204, and is converted into a torque deviation signal 205 via a subtracter 113. Torque deviation signal 205
is input to the motor 31a via the switch 120a and the amplifier 116a. Therefore, the motor 31
a is controlled so that the torque deviation signal 205 becomes zero. As a result, the force applied from the outside to the slave side is transmitted to the master side, and the operator can feel the force applied to the slave side in the form of a reaction force. Next, the operation of the motion range limiting device, which is the main part of the present invention, will be explained. The upper and lower limits of the movement range of the master side joint angle (boundary values of each movement range) are written in advance in the respective memories 122a and 122b in FIG. 4, and the output signals 211 and 212 are input to the comparators 123a and 123b. and transmits the current position signal 200 of the master side joint to each comparator 12.
3a, 123b. If the master side current position signal 200 is larger than the upper limit signal 211,
Logic signal 206 of comparator 123a becomes 1. Otherwise, it remains 0. Similarly, when the master side current position signal 200 is smaller than the lower limit signal 212, the logic signal 207 of the comparator 123b becomes 1, and otherwise remains 0. These logic signals 206 and 207 are input to the reaction force generation circuit shown in FIG. 5, and when the master side joint is within the operating range, they are input to the gate signals of switches 125a and 125b that cut off the reaction force signals 213 and 214. used. Further, the logic signals 206 and 207 are sent to the switch 120 via the OR circuit 118 in FIG.
It is used as a gate signal 208 for signals a and 120b. If the master side joint angle is within the motion range, the logic signals 206 and 207 that are the outputs of the motion range detection circuit 117 are both in the 0 state as described above, and the gate signal 2 that is the output of the OR circuit 118 is in the 0 state.
08 is also in the 0 state, the switch 120a takes the torque deviation signal 205 as its output signal 215, and the switch 120b takes the master current position signal 200 as its output signal 215.
is its output, which is the slave target position signal 209. As a result, the entire servo system is configured as the aforementioned force feedback type bilateral servo system, and the manipulator is placed in a normal operating state. However, if the master side joint exceeds the upper limit of its range of motion, the logic signal 206 becomes 1 and the gate signal 206
8 also becomes 1 state. At this time, switch 120b
disconnects the master current position signal 200 and outputs the output signal 209b of the latch circuit 121. This output signal 209b is a value obtained by latching the value of the slave target position signal 209 immediately before the master joint goes out of the operating range, and the slave target position signal 209 is latched until the master joint returns to the operating range and the gate signal 208 becomes 0. Since it is output as a position signal 209,
The position of the slave joint is controlled so that it stops at the current position (the position immediately before the master joint goes out of the range of motion), and the slave joint does not go out of the range of motion. At the same time, since the logic signal 206 has become 1, the switch 125a in the reaction force generation circuit 119
turns on (Fig. 5), and the reaction force command signal 21
3 passes through the addition circuit 126 to the reaction force generation circuit 11
9 is output as an output signal 210. Since the gate signal 208 is set to 1, the switch 120a disconnects the torque deviation signal 205 and converts the reaction force command signal 210 into the torque command signal 21.
Output as 5. As a result, the master side motor 3
1a is controlled so that a constant torque is generated in the direction within the motion range until the master side joint enters the motion range again. Therefore, by sensing this torque, the operator can easily perform an operation in the return direction (for example, loosen the force and allow the torque to move it in the direction of natural movement). The master side joint is
Even when the lower limit of the operating range is exceeded, the same process is performed, so the explanation will be omitted. According to this embodiment, when the master side joint moves outside the movement range defined by the master side joint angle, the slave side joint is automatically prevented from moving outside the required movement range, and the master side joint This has the effect of making it easier to return the joint to its range of motion.
Furthermore, as an embodiment of the present invention, the current position signal 201 of the slave side joint is used as the input signal of the motion range detection circuit 117 shown in FIG. 4 instead of the current position signal 200 of the master side joint. The same effect can be obtained. FIG. 6 shows another embodiment of the motion range detection circuit 117 of FIG. 3, in which the circuit of FIG. 4 includes memories 122c and 122d that define the upper and lower limits of the motion range of the slave joint. Comparator 123c, 1
23d and OR circuits 128a and 128b, and by adding the current position signal 201 of the slave side joint as an input signal, it is possible to make it possible for either the master side or slave side joint to go outside the operating range. For example, as described above, the slave side joint is stopped, and at the same time, a reaction force is generated at the master side joint in a direction within the range of motion. According to this embodiment, even if the similarity between the positions of the master side and the slave side is broken for some reason, no matter which joint goes out of the range of motion, regardless of whether it is on the master side or the slave side, it is possible to restore it. This has the effect of being easy to perform. The above embodiments are based on each operation mode shown in Table 1.
Although this is based on the assumption that M ₁ to M ₇ (S ₁ to _{S 7} ) are regulated independently, regulation can also be performed taking into consideration a plurality of these modes. FIG. 7 shows another embodiment of the present invention, in which the operating range detection circuit 117 shown in FIG. 3 is replaced with an arithmetic processing unit 140 shown in FIG.
The purpose is to limit M ₂ (S ₂ ). The arithmetic processing unit 140 includes a coordinate arithmetic unit 141,
Spatial differential operation units 142a, 142b, logic operation units 143a, 143b, and OR circuit 14
4a and 144b. The input signal of the arithmetic processing unit 140 is not only the output signal 20 of the shoulder joint position detector 42a or 42b as in the case of FIG. 3, but also the elbow joint position signal 221, and the output signal is , logic signal 24 indicating that the upper limit of the operating range has been exceeded, similar to FIG.
2 (corresponding to signal 206) and a logic signal 244 (corresponding to signal 207) indicating that the lower limit has been exceeded. The operation of each part will be explained below. First, the rotation centers of the shoulder joints 12, 22, elbow joints 14, 24, and wrist joints 16, 26 in FIG. Assuming the center 314, these three points are always within the same vertical plane 300 (this vertical plane 300 is based on the base rotation M ₁ or
(rotated by S ₁ ). Therefore, on this vertical plane 300, the shoulder joint center 310 is set as the origin (also referred to as 310), and a straight line from this origin 310 to the elbow joint center 312 when the current position signal 220 of the shoulder joint is zero is defined as a positive Consider an orthogonal coordinate system in which the axial direction is the positive X-axis direction, and the positive Y-axis direction is the direction rotated 90 degrees counterclockwise about the origin 310.
Then, in general, the shoulder joint position signal 220 is
A straight line 31 connecting the elbow joint center 312 and the origin 310
It is given by the angle θ ₁ between 3 and the X axis. Furthermore, if the initial conditions are determined so that the wrist joint center 314 is on the straight line 313 when the elbow joint position signal 221 is zero, the position signal 221 generally indicates that the wrist joint center 314 and the elbow joint center 312 are Connecting straight line 315
It is given by the angle θ ₂ formed by the straight line 313 and the straight line 313. Regarding the orthogonal coordinate system (X _, Y) in FIG _. 8, the _coordinate values (X _R , Y _R ) of the _wrist joint center ₃₁₄ are as follows; ₂ ) Y _R = l ₁ sin θ ₁ + l ₂ sin (θ ₁ + θ ₂ ) ...(1) Here, l ₁ and l ₂ are points 310, 312, and 3
12 and point 314, which are fixed lengths corresponding to the lengths of upper arms 13, 23 and forearms 15, 25 in FIG. Therefore, in the coordinate calculation unit 141 of FIG. 7, the input signals 220, 22
The calculation based on the above equation is performed using θ ₁ and θ ₂ given by 1, and a signal 240a corresponding to X _R and a signal 240b corresponding to Y _R are output. In the spatial differential calculation units 142a and 142b, the differential value ∂X R /∂θ 1 of the coordinate values X _R and Y _R with respect to the shoulder joint θ ₁ is calculated as ∂X _R /∂θ ₁ =−{l ₁ sin θ ₁ +l ₂ sin (θ ₁ +θ ₂ )}...
(2) ∂Y _R / ∂θ ₁ = l ₁ cos θ ₁ + l ₂ cos (θ ₁ + θ ₂ ) ...(3) The input signals 220 (θ ₁ ), 240a (X _R ), 24
0b(Y _R ), and the signal 250a,2
50b. The logic operation unit 143a performs logic operations by inputting 240a and 250a corresponding to X _R and ∂X _R /∂θ ₁ , and generates a logic signal 246 as shown in Table 2
a, 248a are output. However, the X of the wrist joint
Let the upper limit of the operating range of the coordinate X _R be X _U and the lower limit as _XL .

〔Effect of the invention〕

As is clear from the above description, according to the present invention, it is possible to teach the operator who operates the master manipulator about a joint that has gone out of the range of motion in the form of a reaction force.
In addition, each joint of the manipulator can be automatically returned to the operating range by the reaction force, making it easier to return the manipulator to the operating range.
In addition, the operation of the manipulator is no longer interrupted, which greatly improves the operability of the manipulator.At the same time, it also prevents failures due to operation outside the operating range of the manipulator, which greatly improves the reliability of the manipulator. effective.

[Brief explanation of drawings]

Fig. 1 is a diagram showing the overall configuration of a master-slave type manipulator to which the present invention is applied, Fig. 2 is a block diagram of the control system in Fig. 1, and Fig. 3 is a diagram showing an embodiment of the present invention. Figures 4 and 5 are
6 and 7 are diagrams showing other embodiments of the present invention; FIGS. 8 and 9 are explanatory diagrams of the operation of the embodiment in FIG. 7; 10th
The figure shows the operating range when the embodiments of FIGS. 3 and 7 are combined. 1... Master manipulator, 2... Slave manipulator, 3... Control device, 12, 22...
...shoulder joint, 14,24...elbow joint, 16,26...
...Wrist joint, 31a, 31b...Motor, 41
a, 41b...Position detector, 51a, 51b...
Force detector, 117... Operating range detection circuit, 118
...OR gate, 119...Reaction force generation circuit, 12
0a, 120b... Selector switch, 121... Latch circuit, 127... Operating range limiting circuit.

Claims (1)

[Scope of Claims] 1. Out of position signals indicating the positions of each joint of a multi-joint master manipulator and slave manipulator having mutually corresponding joints, the position signal of the master joint is used as a target signal to control the slave joint. A slave side servo mechanism for controlling each master side joint to follow each corresponding master side joint, and a deviation of a force signal indicating an external force applied to the corresponding joint as input and applied to each slave side joint. In a device for limiting the operation of a manipulator to within a predetermined range, the device includes a master side servo mechanism that controls external force to be transmitted to each of the corresponding master side joints, the master side joints and the corresponding master side joints. one or both of the position signals of one or more sets of slave side joints to be input position signals, an upper limit value and a lower limit value defining a movement range corresponding to the input position signals are stored therein, and the upper limit value and an operating range detection means that turns on a corresponding upper or lower limit switching signal when the input position signal exceeds one of the upper and lower limit values, and samples the target signal when both the upper and lower limit switching signals are off. When one of the above upper limit or lower limit switching signals turns on, the target signal stored at the sampling immediately before the turning on is held, and the slave side servo mechanism A position signal switching means for outputting as a target signal, and a reaction force for generating a reaction signal in a direction that returns the input position signal to within the operation range when the upper limit or lower limit switching signal is turned on. A control system comprising a generating means and a force signal switching means for supplying the reaction force signal as an input to the master side servo mechanism in place of the deviation of the force signal when any of the switching signals is turned on. A manipulator operating range limiting device characterized in that one set is provided for each corresponding joint on a master side and a slave side. 2 Among the position signals indicating the position of each joint of a multi-joint type master manipulator and slave manipulator having joints that correspond to each other, each of the slave side joints uses the position signal of the master side joint as a target signal to control the corresponding master side. A slave-side servo mechanism for controlling each joint to follow, and a master side to which the external force applied to each slave-side joint corresponds by inputting the deviation of the force signal indicating the external force applied to the corresponding joint. In a device for limiting the operation of a manipulator to a predetermined range, the device includes a master-side servo mechanism that controls transmission to each joint, and when the predetermined range is taught, the slave-side manipulator calculation means for calculating a limit value that limits the motion range of each joint to within the predetermined range based on a position signal indicating the position of each joint; 1. An operating range limiting device for a manipulator, comprising a reaction force generating means for generating a force signal.