JPS59146770A - Limiter for range of operation of 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 Application of the Invention] The present invention relates to a device for restricting the operating range of a manibu beater, and is particularly useful for performing operations in nuclear power plants and the like where it is difficult for people to be close to each other by rapid operation. The present invention relates to a motion range limiting device that can limit the motion of a mask/slave manipulator to an appropriate range without impairing the motion noise or operability.

[Prior art]

As is well known, the arms of the manipulator are moved by joints, but their range of motion r, t: must be limited so that the arms do not collide with the manipulator, other mechanical parts, or other objects. As a conventional example of a movement range limiting mechanism of a manipulator joint for this purpose, for example, Japanese Patent Laid-Open No. 53-17
There are 880. 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 determines the joint angle of the arm. If the target signal exceeds the current value,
The position is controlled so that the joint angle of the arm stops at the current position. However, this method does not have a mechanism for returning the joint angle of the arm to its range of motion and allowing it to function normally, so when the joint angle moves out of the range of motion, it is difficult to return it. There is.

In addition, in the case of a mask-slave type manipulator, in which the slave manipulator is configured to be remotely controlled in response to the movement of the master manipulator manually operated by the operator, the joint state of the slave manipulator may fall outside the operating range. At the time, it was known to instruct the operator with a lamp or the like on the entire control console. but,
This method has the problem that since the operator operates the master manipulator by holding the tip of the master manipulator, it is difficult to know in which direction the joint should be moved to return the joint to its range of motion. In particular, when 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 has to temporarily stop working by remote control and move his/her line of sight from the work object to a lamp or the like on the control console, resulting in poor operability.

[Purpose of the invention]

An object of the present invention is to provide a mask-slave type manipulator that can easily and quickly return the joint angle of the arm of the manipulator to within the range of motion when it falls outside the range of motion set in advance. All devices with limited operating range are provided.

[Summary of the invention]

The present invention is capable of stopping a slave manipulator at that position the moment the arm of any manipulator exceeds a preset operating range, and at the same time, a master manipulator operated by a human. By adding a function that generates a torque that allows the operator to move the joint in a direction within the range of motion, the operator feels this torque as an impact force. It is characterized by making it easy to sense, thereby making it easy to return each joint to its range of motion.

[Embodiments of the invention]

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

FIG. 1 shows the overall configuration of a mask/slave manipulator to which the present invention is applied, and is composed of a master manipulator 1 and a slave manipulator 2, which have similar shapes to each other, control surfaces ', 3, and a monitor/revision valve 4. Ru. The manipulators 1, 2 have a structure similar to a human arm, each having a base part 11.21 and a shoulder joint 12.21 at one end of the base part. ．． a forearm 15.25 connected to the upper arm 13, 23 at an elbow joint 14.24 at one end of the upper arm 13, 23; Wrist joint 16.26
and has. The base portion 11 of the master manipulator 1 has its upper end connected to the frame 10, and the base portion 21 of the slave manipulator 2 has its lower end connected to the traveling vehicle 20.

A handle 17 with metal fingers is connected to the 1° 1st joint 16 of the mask's wrist. A finger portion 27 is connected to the slave wrist joint 26 . Manipulators 1 and 2 are
11 to 17 as shown in the arrows in Figure 1 and Table 1.
And 7 own mountains and rivers of SI~s7 have 7. That is, to 41 (St), the base part 11 (21) is connected to the inside of it, I
1.20), and the upper arm 13 (2
31° Forearm 15 (25) is between the eyebrows N1” + 12 (22
), the motion of rotating the elbow joint J4 (24) in the vertical direction 1i+ is shown in the figure. Also, M4, M5. M6
(s4, Ss.

Sal'J, J2, finger 1'7 (27) is wrist joint 16
(26) represents two rotating motes that can take any posture. Nh (s7) G゛l,
Represents the opening and closing movements of the fingers to grasp objects.

An outline of the control force method and operation of such a manipulator is as follows. Operator 5 (r, ll) controls the slave manipulator 2 by operating the master manipulator 1 while presenting an image of the slave manipulator 2 on the monitor television 4. In other words, the operator By grasping the hand 47 and moving the tip ☆114 of the master manipulator, each joint of the master manipulator moves, and with this movement, each joint of the slave manipulator 2 Add %J(to each joint of manipulator 1. As a result,
Master manipulator 1 and sv-7-manivision l, /-
Since the postures of the joints in step 2 are maintained in similar shapes, the operator 5 can intuitively know the posture of the slave manipulator from the posture of the master manipulator. 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 all the joints of the master manipulator l, so a reaction force corresponding to the external force is generated at each joint. do. As a result, the operator 5 can operate the slave marbulator 2 while feeling the force being applied to it.

The control mechanism of the joint that realizes all the 7i-movements, as shown in Figure 2, is based on the mask and the slave's -
One joint system 7'iTK corresponding to tj is independently controlled by 1iIJ. That is, the master base rotation motor 31a1 position detector 41a and force detector 51a on the master manipulator 1 side, the slave base rotation motor 311) on the slave manipulator 2 side, and the position fjt detector 41
b, force detector 511) and the servo calculator 61 in the control device 3 constitute a joint system control mechanism for base rotation. Similarly, the master shoulder joint motor 32 on the master side
a2 position detector 42a.

Force detector 52. a, the slave upper arm swing motor 32b4 position detector 421) on the slave side, and the force detector 52b
A swinging joint system is constructed from the servo calculator 62 in the control device 3 to -1, and the mask (++11 mask finger opening/closing motor 37a, second position Irj: detector 47a, force detector 57a, and slave side Slave finger opening/closing motor 37
1) The configuration is the same up to the finger opening/closing joint system consisting of the position detector 47b, the force detector 57b, and the servo generator 67 in the control device 3. These joint control systems are the first
As shown in the figure, the appearance is 7 o'clock, but the two-boot machine r, 4, which is the basis of its control, is the same.

The motion range control device of the present invention is incorporated into the above-mentioned single-body computing units 61 to 67, but since all joint systems are similar, here, the base rotation joint shown in FIG. This will be explained by an example of the system. In FIG. 3, the mask side machine (TV part) is connected to a motor 31a connected to the joint shaft by a transmission mechanism including a reduction gear (not shown) and a torque around ll11 applied to the joint shaft. A force detector 51a that detects the rotation angle of the joint axis and a position detector 4 that detects the rotation angle of the joint axis.
1a. Furthermore, the mechanism section on the slave side has the same configuration as the mask side, and includes a motor 31b, a force detector 51b1, and a position detector 41b. The circuits shown in FIG. 3 other than these underground structures belong to the servo calculation section 61, and are the force detector 51a.

51b, a subtracter 114 that subtracts the slave position target signal 209 and current position signal 201, and PIT) control 1.
A circuit 115 and an amplifier l'l6a that drives the motors of the master 9i1 and the slave (I!!l).

116b, and an operating range limiting circuit 127 constituting the present invention.
(within the dashed line). Operating range limiting circuit 1
Reference numeral 27 denotes a switch 120 that switches between the operating range detection circuit 117, the reaction force generation circuit 119 of the gate circuit 118, the output signal 210 of the reaction force generation circuit 119, and the force deviation signal 205.
a, a switch 120b that switches between the mask current position signal 200 and the slave target signal 209bi.
and a latch circuit 1 that latches the slave target signal 209.
21.

As shown in FIG. 4, the operating range detection circuit 117 includes a memory 122a that defines the upper limit of the operating range, a memory 122b that defines the lower limit thereof, and an upper limit signal 211 and a lower limit signal 212 output from these respective memories. A comparator 123 that compares the current mask position No. 1B 200 and the mask current position No. 1B.
a, 123b. As shown in FIG. 5, the reaction force generation circuit 119 turns on and off the outputs of the memories 124a and 124b, which define the magnitude of the reaction force acting in the direction within the operating range, and the respective output 1 words 213 and 214. It is composed of switches 125a and 125b and an adder 126 that adds the output signals of the switches.

The operation of the joint system shown in FIGS. 3 to 5 will be explained below, starting with the operation of the force feedback type bilateral servo system. The position of the joint angle on the mask side and the position of the joint angle on the slave side are detected by respective position detectors 41a.

41b detects the current position signals 200, 201, and converts them into a position deviation signal 202 via the subtracter 114 (at this time, 120b selects signal 200). The position error signal 202 is transmitted to the PID control circuit 115 and the amplifier 116.
b, and is input to the motor 31b. Then, the motor 31b rotates, the position signal 201 changes, and eventually the position deviation signal 202 is controlled to become O. Therefore,
When the operator operates the mask side joint, the position of the slave side joint is controlled so that it moves following the mask side joint.

At the same time, the force being applied from the outside to the mask side and 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 transmitted to switch 120
A and the motor 31 aiC are input through the amplifier 116 a'i. Therefore, the motor 31a receives the torque deviation signal 2
05 is controlled to become 0. As a result, the force applied from the outside to the slave side is transmitted to the mask side, and the operator can feel the force applied to the slave side in the form of a reaction force.

Next, the entire operation of the operating range limiting device, which is the main part of the present invention, will be explained. Upper and lower limits of the motion range of the mask side joint angle (
Boundary values of the respective operating ranges) are written in advance in the respective memories 122a and 122b in FIG. is input to each comparator 123a, 123b. When the mask side current position signal 200 is larger than the upper limit signal 211, the logic signal 206 of the comparator 123a becomes 1.

Otherwise, it remains 0. If the colleague's mask-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 logics 114 No. 2'06
, 207 is manually applied to the reaction force generation circuit shown in FIG.
Switches 125a and 125b that cut off 13' and 214
Used for gate signal. Also, logic signals 206 and 2
07 is connected to the switch 12 via the λ circuit 118 in FIG.
It is used as the gate signal 208 of 0a and 120b.

If the mask side joint angle is within the motion range, the output of the motion range detection circuit 117 is zero, which is the logic signal 206, 2Q.
7 are both in the 0 state υ as described above, and the gate in number 208 which is the output of the OR circuit 118 is also in the 0 state)I.
The switch 120a outputs the torque deviation signal 205 as its output 215, and the switch 120b outputs the mask current position signal 205 as its output, 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 mask side joint exceeds the upper limit of the motion range, the logic signal 2
06 becomes 1, and the p1 gate signal 208 also becomes 1 state. At this time, the switch 1201) switches the mask current position signal 2
00 is disconnected and the output signal 209b of the latch circuit 121 is
i Output. This output signal 209b is a latched value of the slave target position signal 209 immediately before the mask joint goes out of the operating range, and when the mask joint returns to the operating range and the gate signal 208 becomes O, the slave target position signal 209 is latched. Since it is output as the target position signal 209, the position of the slave joint is controlled so that it stops at the current position (position 1 just before the mask joint goes out of the range of motion), and the slave joint never goes out of the range of motion. do not have.

At the same time, since the logic signal 206 is 1, the switch 125a in the reaction force generation circuit 119 is turned on (
5), the reaction force command signal 2''3 is outputted as the output signal 210 of the reaction force generation circuit 119 via the addition circuit 126. Since the gate signal 208 is set to 1, the switch 120a turns off the torque deviation signal 205 and outputs it as the MilL and reaction force command signal 21 (ll-lux command signal 215. As a result, the master side motor 31
a is until the mask side joint comes within the range of motion again.
It is controlled so that a constant torque is generated in a direction within the operating range. 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). Even when the mask side joint exceeds the lower limit of its motion range, 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 mask side joint angle, the slave side joint is automatically prevented from moving outside the required movement range, and the mask side joint This has the effect of making it easier to return the device to the operating range.

Further, as a modification of the present invention, the current position signal 201 of the slave side joint may be used as the input signal of the motion range detection circuit 117 shown in FIG. A similar effect can be obtained even if the configuration is configured as follows.

FIG. 6 shows another embodiment of the motion range detection circuit 117 of FIG. 3, in which the upper and lower limits of the motion range of the slave side joint are added to the circuit of FIG. 4. Specified memory 1.22c, 12
2d, comparator 123 C1123d, OR circuit 12
．． 8a and 128b, and by adding the current position signal 201'tr of the slave side joint as an input signal, if either the mask side or the slave 1111 joint goes out of the operating range, As described above, the slave surveying Mif is stopped and at the same time, a reaction force is generated in the direction within the motion range of the mask side joint. According to the actual practice, for some reason, master 1
The similarity relationship between the rule and the position of slave 1ill is broken (
1, but master ill! i. Regardless of the slave side 1, even if any joint falls outside the range of motion, it can be easily returned to its original position.

The above embodiments are based on each operation mode M! shown in Table 1. ~M
T(81-8t)'fc Although this is based on the assumption that the fc is independently mowed and controlled, it is also possible to regulate this mode by considering a plurality of modes. FIG. 7 shows another embodiment of the present invention, in which the operating range detection circuit 11 shown in FIG.
7 is replaced with an arithmetic processing unit) 140 shown in FIG. 7, and the purpose is to limit the operation mode M2 (82). The arithmetic processing unit 140 includes a coordinate arithmetic unit) 141. Interval differential operation unit 142a.

142b, logic operation units 143a and 143b to-or circuits 1.44a and 144b.

The input signal of the arithmetic processing unit 140 is not only the output signal 20 of the position detector 42a or 42b of the H joint as in the case of FIG. 3, but also the position signal 22'1 of the elbow joint. The output signals are a logic signal 242 (6 for signal 206) indicating that the upper limit of the operating range has been exceeded and a logic signal 244 indicating that the lower limit has been exceeded, as in FIG.
(corresponding to signal 207).

The operation of each part will be explained below. First, the rotation centers of the shoulder joint 12 (22), the wrist joint 14 (241), and the wrist joint 16 (26) in FIG. 1, the shoulder joint center 31o, the elbow joint center 312, and Assuming that the wrist joint center 314 is the center of the wrist joint, these three points are within the same vertical plane 300 (this vertical plane 300 is rotated according to the base rotation M1 or Sl shown in Table 1). Above, the shoulder joint center 310 is the origin (also 3J
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 the positive X-axis direction, and this positive X-axis direction is the origin 310.
Consider an orthogonal coordinate system in which the direction rotated 90° counterclockwise around , is the positive Y-axis direction. Then, in general, the shoulder joint position signal 220 is generated between the elbow joint center 312 and the origin 310.
It is given by the angle θ1 between the straight line 313 connecting these lines 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 is generally equal to the wrist joint center 314.
A straight line 315 connecting 4 and the elbow joint center 312, and a straight line 31
It is given by the angle θ2 with 3. Regarding the orthogonal coordinate system (X, Y) in FIG. 8, the coordinate values (Xn, YR) of the wrist joint center 314 are as follows; Here, tI, 12 are the points 310, 312, and 312. The length between points 314 is a fixed length corresponding to the length of upper arm 13 (233) and forearm 15 (25) in FIG. 1. Therefore, in the coordinate calculation unit 141 in FIG. θ given by input signal 220°221
The calculation based on the above formula is performed using 1, 02, and a signal 240a corresponding to XR.

A signal 240b corresponding to Yn is output.

In the spatial differential calculation units 142a and 142b, the differential values of the coordinate values Xn and Yn with respect to the shoulder joint θ1 are calculated based on the input signal 2.
20 (θH), 240 a (XR).

240b(Yi)Th and signal 250a.

250b.

A logical operation is performed by inputting 240a and 250ai corresponding to
．． a is output. However, the upper limit of the movement range of the X seat @ X R of the hand-raised joint is 'kXυ, and the lower limit is XL.

The meanings of the logic outputs 246a and 248a in Table 2 are as follows. Among the input conditions, aXR/θθl>O is θ1
Indicates that when the total is increased, XR will also increase [)
, and θXn/aθ+ (O is the opposite, 1, and for example (r, 1: 1 in Table 2), XR is the upper limit X+
1. Since θI is small, the signal 2483 to increase θI is set to 1 to increase Xlt. This unit: In the circuit 117 of FIG. 4 or 6, the signal 207 is "1.
In the case of θ1 ke' stomach dog (θ1 is a turning motion that exceeds its full limit/Kotori. Similarly, in the case of 2 in Table 2), decrease 01 ke to increase XRr. signal 24
6a7. Let it be rl. In the case where the value is 1 (corresponding to 3 when the value θ exceeds the upper limit of 7L by 4), no limiting operation is performed. 4
．． ), 5) +1 combination is also the same as 1), 2).

In the logical operation unit) 143b, the range of motion (Y t, (Y ('Y U )) of the Y seat of the wrist joint is
The same processing as in the 5 logic unit) 143a is performed, and logic signals 246b and 24sb having the same meaning are output.

And 7 and No. 111 246a, 246h and 248, 1° 248b are each OR times t,! 8144 a, 144
b, the logical sum is taken, and the signal 2°42°2
44.

Numerical performance 9 [Final output of part 140 No. 18 242ii, No. 3
It is used as the upper limit logic signal 206 in the figure, and is also used as the signal 2
44 is used as the lower limit logic signal 207, and the operation of the other circuits is the same as that explained in FIG. It started to bubble with the operating range of
The operation can be comprehensively limited to 1.

According to this embodiment, 1t (11
The working range can be regulated within any rectangular area by appropriately setting XL + 'Xu, YL, and Yu.
By combining this embodiment with the embodiment shown in FIG. 3, it is possible to restrict the operation range of the t-merge controller 17 within the region 50 as shown by diagonal lines in FIG. 10, for example. That is, as in the embodiment shown in Fig. 3, the angles θl and θ2 (see Fig. 8) of each joint are individually regulated by upper and lower limit values to create the arc-shaped region boundary metal shown in Fig. 10, and at the same time As in the example in the figure, the first
It is sufficient to create a straight line boundary as shown in Figure 0, and to restrict it using these OR conditions (restrict when either boundary is crossed). Note that the servo arithmetic unit including the operating range detection circuit 117 as described above or the arithmetic processing unit shown in FIG.
This can be easily realized using a microcomputer or the like.

Effects of the Invention] As is clear from the above description, according to the present invention, z(
To be able to give instructions in the form of a reaction force to the operator who operates the joints, master manibulator, and other parts that have gone out of the operating range, and to automatically return each joint of the manibulator to the operating range by the reaction force. This has the effect of making it easier to return the manipulator to its operating range, eliminating the need to interrupt the operation of the manipulator, and greatly improving the overall operability of the manipulator. ,
It is possible to prevent malfunctions due to operation outside the operating range of the manibicolator.It is also possible to improve the reliability of the C5 manipulator.
It has a great effect.

[Brief explanation of drawings]

FIG. 1 is a diagram showing the overall configuration of a mask 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 another embodiment of the unexploded invention. 8 and 9 are explanatory diagrams of the operation of the embodiment of FIG. 7, and FIG. 10 is a diagram showing the operating range when the embodiment of FIG. 3 and FIG. 7 are combined. 1... Master manipulator, 2... Slezo manibulator, 3... iti control device, 12.22...
・11-I joint, 14.24...Elbow joint, 16.26
...Wrist joint, 31a, 311) =-motor, 41a
, 41b-ii'Zfii detector, 51X1.51b・
...Force detector, 117...Operating range detection circuit, 118
...for or games...11...reaction force generation circuit, 12
0a, 120b... uJ exchange switch, 121... latch circuit, 127... c) no operation range tU+ limit circuit. Agent Kairishi Akimoto + E> Su Aburidai 7 Mouth 310 Kaya γ θ X4 8U

Claims (1)

[Claims] 1. Of the position signals indicating the position of each joint of the multi-joint type mask-side manipulator and slave-side manipulator that have joints that are parallel to each other, each of the slave-side joints responds to the position signal of the mask-side joint as a target signal. The slave side servo mechanism is controlled to follow each of the mask side joints, and the deviation of the force signal indicating the external force applied to the corresponding joint is input. 11) The external force applied to each joint is Operation of a manipulator equipped with a mask-side servo mechanism that controls transmission to each of the corresponding mask-side joints: In a device for limiting the operation within a predetermined range, Slave corresponding to fll! IIi'lIi of the l joint (- indicates the input position 1fi: (- is the input position of one or both of the multiple sets of position signals.) (No. 8, and corresponds to the input position signal. an operating range detecting means which is stored therein and turns on a corresponding upper or lower limit switching signal when either the upper limit value or the lower limit value exceeds the input position signal; When both switching signals are off, the target signal is updated and memorized every time sampling is performed, and one of the above upper limit or lower limit switching signals is
a position signal switching means for holding the target signal sound stored at the time of sampling immediately before turning on and outputting it as a target signal of the slave side servo mechanism, and the upper or lower limit of the above-mentioned upper limit or lower limit. When the switching signal is turned on, a reaction force signal is generated in the direction that returns the input position 1i: to the operating range indicated by the signal sound - h.
and a force for inputting and applying the reaction force signal to the master side servo mechanism in place of the deviation of the force signal when either of the switching signals is turned on. An operating range limiting device for a manipulator characterized in that a control system comprising a signal switching means is provided for each corresponding joint to on the mask side and the slave side.