JPH0151314B2 - - Google Patents

Description

DETAILED DESCRIPTION OF THE INVENTION [Technical field to which the invention pertains] The present invention relates to a method for controlling a multi-joint arm that allows simple work to be performed remotely in a narrow space where humans cannot enter.

[Prior art and its problems] Large openings are installed, such as towers and tanks in shielded cells of nuclear fuel reprocessing plants or inside the reactor body of fusion reactors, and people can enter inside to inspect the inside. In facilities where it is impossible to carry out simple tasks due to space limitations or safety restrictions such as high radiation areas, the human operator must remain in a safe area and carry out the designated tasks. There is a strong desire to be able to install only the machine that performs the work in the environment described above and perform the work by remote control.

[Purpose of the Invention] The purpose of the present invention is to provide a method for controlling a multi-jointed arm that allows inspections and simple tasks to be performed in adverse environments by remote control without causing mental or physical burden on the driver. .

[Summary of the Invention] The present invention provides a method for controlling a multi-joint arm in which a multi-joint arm in which a plurality of unit arms are connected in series via joints is moved while controlling the joint angles of the joints, comprising: A trajectory along which the joint arm should move is prepared in advance, the length of the trajectory is divided by the length of the unit arm, the coordinate values of the division points are stored, and the joint parts are sequentially corresponded to the coordinate values. In particular, when the multi-joint arm comes into contact with an object, a new trajectory is created at a position where the multi-joint arm moves away from the object, and this new trajectory is also divided in the same way. The coordinate values of the dividing points are stored, and the joints are controlled to correspond to these coordinate values.

[Effects of the Invention] According to the present invention, a driver can perform simple work in a narrow space in a bad environment, which was previously impossible, by remote control while staying in a safe place. The effects are significant at nuclear power-related facilities, where reducing human radiation exposure is once again important.

[Examples of the invention] Examples of the invention will be described below. FIG. 1 shows an example of an object to be inspected, and for ease of understanding, a two-dimensional space will be described as an example. In Figure 1,
1 is a tank, 2 is an opening of the tank, and 3 is a group of pipes installed inside the tank. This kind of shape is
This is often seen in heat exchangers.

If there is a leak in the tank 1 and an attempt is made to visually inspect the Q point from inside, it is impossible to directly look at the Q point from the opening 2 because the pipe group 3 becomes an obstacle.

The method of controlling the multi-joint arm according to the present invention is useful when the space between the wall of the tank 1 and the pipe group 3 is very narrow and there is no room for a person to enter, or when the inside of the tank 1 is contaminated with radioactive materials and a person cannot enter the tank 1. An articulated arm that can maneuver through narrow, twisting spaces in a way that allows for simple work in narrow spaces that cannot be easily accessed by humans. and a multi-joint arm moving machine groove for supporting the base of the multi-joint arm and moving the entire multi-joint arm, a control device for controlling these, a television camera attached to the end of the multi-joint arm, and a television camera. Using a remote control device consisting of a television monitor that displays images,
The multi-jointed arm is inserted into the tank along the curved trajectory C, and the video signal of point Q is displayed on a TV monitor placed in a safe place where the driver is, using a television camera at the tip. This is a method that allows inspection. 2 and 3 are diagrams showing the mechanism and control portion of the present invention. In FIG. 2, the multi-joint arm 6 is composed of unit arms 4 ₁ to 4 _o and joints 5 ₁ to 5 _o that connect them, and a television camera 16 is attached to the tip. Reference numeral 7 denotes a multi-joint arm moving mechanism for supporting the base of the multi-joint arm 6 and moving the entire multi-joint arm.

When such a multi-joint arm is operated using n position switches connected to motors attached to each joint, the unit arm moves based on each arm coordinate, so the driver can understand the relationship between the stationary coordinate and the arm coordinate. The on/off control of the position switch must be controlled while always considering this in one's head, which is not only inefficient but also places a heavy mental burden on the driver.

In order to solve this problem, the control system for the multi-joint arm 6 and the multi-joint arm moving mechanism 7 is constructed as shown in FIG. In FIG. 3, 8 ₁ to 8 _o are motors for moving each joint 5 ₁ to 5 _o of the multi-joint arm 6, 9 ₁ to 9 _o are encoders that measure the rotation angle of each joint, and 10 ₁ to 10 _o are This is a contact sensor attached to the surface of each unit arm 4 ₁ to 4 _o . moreover,
11 is an encoder that measures the amount of movement of the multi-joint arm moving mechanism 7, and 12 is a motor that moves the multi-joint arm moving mechanism 7. 13 is a motor drive circuit, 1
4 is a computer, 15 is a computer input/output device, and 1
3, 14, and 15 form a control device 18. 16
is a television camera, and 17 is a television monitor.

Next, to explain these operations, they operate in the order of the flowchart shown in Fig. 4, the multi-joint arm moves as shown in Fig. 5, and they are composed of operating means as shown in Fig. 6. In this method, the arm operator first draws a curved trajectory C (see Fig. 5) on the drawing of the tank 3 to be inspected, which is considered to be the most likely to cause no unreasonableness or collision when inserting the multi-jointed arm 6. ,
This is divided into N parts and the coordinates of each dividing point P ₀ to P _N are determined.

Next, the coordinates P ₀ to P _N are inputted from the computer input/output device 15 to the computer 14 to start driving the multi-joint arm 6.

The calculator 14 calculates the rotation angle of each joint and _the rotation angle [θ] of the motor of the multi-joint arm movement mechanism 7 in order for the tip to advance by △ _{l i (} hereinafter referred to as the feed pitch) from P i to P i+1. calculate.

In this _case , _each joint 5 ₁ , 5 _{2 ,} . As a special case, if the length of each unit arm is all equal to _la and the feed pitch is also equal to _la ( _la = △l), each joint 5 ₁ , 5 ₂ . . . 5 _o is P _i ,
The calculation will be made so that P _i-1 and P _i-o+1 are obtained.
Therefore, as mentioned above, each joint 5 ₁ , 5 ₂ ...5 _o
In order to ride on the trajectory _A that directly _connects P ₀ , P ₁ , . (In this example, there is no relationship between N and n, and if N=n,
If the length of the trajectory C is extremely different from the full length of the multi-jointed arm n×l _a , each joint 5 ₁ , 5 ₂ , ... 5 _o is P ₀ , P ₁ ,
...It will no longer match the trajectory A that directly connects P _N to each other. ) When the calculation of [θ] is completed, the computer 14 sends a signal to the motor drive circuit 13 so that each motor rotates by [θ].

If the trajectory C initially set on the drawing is inappropriate and, for example, the unit arm 4 _i comes into contact with point R in Fig. 5 soon after starting the insertion work, the contact sensor attached to the unit arm 4 _i 10 _i is turned on, and the computer 14 immediately instructs the motor drive circuit 13 to stop all motors. and,
Information about which side of which unit arm came into contact with the obstacle is output to the computer input/output device 15. In accordance with this information, the driver reconsiders the trajectory C on the tank drawing, corrects the trajectory as shown by the dotted line in Figure 5, and sets new coordinate points P' ₂ , P' ₃ , P' ₄ The coordinate values of are inputted again using the computer input/output device 15, and the work is restarted.

Since the multi-joint arm 6 can reach the point P _o along the trajectory C while repeating the above-described operations, visual inspection of the point Q becomes possible using the television image. When returning the multi-joint arm 6 from point Q, it is sufficient to memorize the command value of the forward path and perform the reverse operation of the forward path.

In other words, these operations are shown in FIG. 6. As shown in FIG. [θ] calculation means 20 for calculating the joint angle [θ], and [θ] rotation command means 21 for commanding the joint angle [θ] from the calculated value.
and a motor control device 23 that controls the joint drive motor 22 based on this command, and the angle during operation is detected by the joint rotation degree detection means 24 and sent to the [θ] calculation means 20 and the motor control device 23. I am typing. In addition, the contact detection means 25 attached to each arm is a contact point display means 27 that displays the contact point via the contact point determination means 26, which confirms where the contact point has been with the obstacle, and detects whether the contact point has been contacted with the obstacle or not. Create a trajectory on the drawing that will not occur. The corrected trajectory obtained by the corrected trajectory creating means 28 is divided into N parts as described above and inputted to the coordinate value inputting means of the dividing points, and the signal processing is performed again by the joint control means 29.

In order to make the content easier to understand, the explanation was given using an example of a multi-jointed arm that moves in a two-dimensional plane, but if you use a multi-jointed arm that has two degrees of freedom per joint, you can move within a three-dimensional space. It is possible to construct a remote control device that can move around freely.

In addition, we used an example of an inspection device with a television camera attached to the end of a multi-jointed arm, but if the end is equipped with a torque wrench, magnetic chuck, etc., it will be easy to tighten nuts or collect loose parts. Work becomes possible.

[Brief explanation of drawings]

Fig. 1 is a sectional view explaining the outline of the present invention, Fig. 2
The figure is a top sectional view showing the device used in the present invention.
FIG. 3 is a circuit diagram showing the configuration of the device used in the present invention, FIG. 4 is a control flow diagram of the control method of the present invention,
FIG. 5 is an explanatory diagram to help understand FIG. 4, and FIG. 6 is a configuration diagram showing the control method of the present invention. 1... Tank, 2... Tank opening, 3... Piping group, 4 ₁ to 4 _o ... Unit arm, 5 ₁ to 5 _o ... Joint, 6... Multi-joint arm, 7... Multi-joint arm Movement mechanism, 8 ₁ - 8 _o ... motor, 9 ₁ - 9 _o ... encoder, 11 ... encoder, 12 ... motor,
13...Motor drive circuit, 14...Computer, 15
...Computer input/output device, 16...TV camera,
17...TV monitor, 18...Control device.

Claims (1)

[Scope of Claims] 1. A method for controlling a multi-joint arm in which a multi-joint arm in which a plurality of unit arms are connected in series through joint parts is moved while controlling the joint angles of the joint parts, comprising: A trajectory on which the robot should move is prepared in advance, the length of the trajectory is divided by the length of the unit arm, the coordinate values of the dividing points are stored, and the joints are made to correspond to the coordinate values in sequence. A method for controlling a multi-joint arm, characterized in that the multi-joint arm is driven by controlling the multi-joint arm. 2 Create a new trajectory at a position where the multi-joint arm moves away from the object when the multi-joint arm comes into contact with the object, divide the length of the new trajectory by the length of the arm, and find the dividing point. Control of the multi-joint arm according to claim 1, characterized in that the multi-joint arm is driven again by storing coordinate values of and controlling the joint parts to correspond to the coordinate values. Method.