JPS5877490A - Method of avoiding and controlling collision of robot - 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 provides a method for avoiding the collision of the robot's arms with respect to each other. How to Kll.

[Conventional technology (In order to improve the workability of robots and robots, robots with multiple arms or multiple arms with two or more arms are provided with a pot.) The control of the rosette is It is extremely complicated.The reason is that the platform arm has relatively many degrees of freedom, and the degrees of freedom are determined by the plurality of arms provided on the platform arm.Therefore, When controlling a robot, the control of each arm as a whole and the control of each of the five arm parts of the platform arm must be controlled in relation to each other, including their own functions. Control has become complicated.From this point of view, the applicant has developed an ultrasonic sensor for distance measurement as a basic element for controlling the platform arm and each arm, and also Kimimaru has been conducting research and development on the effectiveness of sonic senna.

As a result, the applicant has filed various patent applications regarding the combination of an ultrasonic sensor and a multi-arm robot.

What is particularly important in robot control is the abnormal approach of the arms, and the above-mentioned ultrasonic sensor plays a role in detecting this abnormal approach. There are two possible cases of an abnormal approach: an abnormal approach that was originally unexpected, and an abnormal approach where an abnormal approach happens to occur as a result of control performed based on correct thinking in terms of overall control.

In either of these two forms of abnormal approach, an accident such as a collision between the arms may occur, resulting in a bad result in which instructions for work contents cannot be communicated during a temporary stoppage of work.

On the other hand, the ultrasonic senna is a multi-arm robot O-arm Kjl! During installation, ultrasonic sensors are installed at the base of the two IIMjOs. The first one is for transmitting waves, and the second one is for receiving calls. There are nine types of ultrasonic sensors that can be used both for transmitting and receiving waves, and there are also cases where they are installed only for transmitting and receiving waves without having both functions. In either case, an ultrasonic sensor is installed in each arm portion of the platform arm. Further, an ultrasonic sensor provided for each arm portion of the platform arm is used to measure the approach distance between each arm of the platform arm. This approach distance is detected based on the time from when the ultrasonic wave is transmitted until it is received.The applicant has so far detected the approach distance between each arm of the platform arm based on the method described above. We have also proposed and applied for various palm methods. However, these applications concern distance measurement and abnormal approach detection in trenches, but do not discuss how control will be carried out after abnormal approach.

The most common way to move after detecting an abnormal approach is to stop the movement of the nine approach arms at the same time as the abnormality is detected, but this may result in a temporary suspension of work, or This creates the need for other types of control, such as what kind of control should be carried out after that.

[Purpose of the invention]

The present invention is proposed from such a viewpoint,
When the vehicle approaches the vehicle abnormally, an avoidance action is performed at the same time as the vehicle approaches the vehicle abnormally.

[Summary of the invention]

The present invention provides a distance sensor on the opposing arms, measures the approach distance from the propagation time of the signal received by the distance sensor, detects that the distance is abnormally close, and performs collision avoidance operation when the distance is abnormally close. There is also KIII mold.

As a result, work interruption time can be reduced and work continuity can be maintained.Furthermore, in the present invention, when performing an avoidance operation, the most efficient avoidance direction (vector An avoidance method has been proposed that takes k). The present invention will now be described in detail with reference to the drawings.

[Embodiments of the invention]

FIG. 1 shows a conceptual diagram of the robot system selected as the subject of the present invention, which is composed of the configuration of a multi-arm robot and the installation status of an ultrasonic sensor.

B consists of a robot system. The arm has two arm parts 1.2 and fingers 5 and a joint 7° 8.9. Further, the 1A15 section 1 has two transmitters 1's 1b which are constituted by ultrasonic oscillators, and the arm section 2 similarly has two transmitters 2Jle 1. That is, the arm has a role exclusively for transmission. The arm section 3 has two delivery devices 3m and 3b which are constituted by ultrasonic receivers, and the arm section 4 similarly has two delivery devices 4m and 4b. That is, arm B has a role exclusively for reception. Two arms.

The degree of freedom of movement for both B is 7.8.9 and 1G.

11.12. Both the transmitter and the receiver are connected to the ultrasonic sensor. For each transmitter and delivery device, 3
It is configured to cover an entire space of 66".

Specifically, it is configured to have two ultrasonic elements whose directions are 180' different from each other.

An overall diagram of distance measurement in such a configuration is shown in FIG.
The datar 25 inputs the wave transmission command 8 and the transmitter address P, decodes the address P, and sends the corresponding driver 1.
Marry someone who chooses 3.14.15.16. Each driver 13.14°15.16 is for each transmitter 1a, lb, 2
This is a driver compatible with C and idK. Nine, the sense amplifier 17, 18, 19, 20 is the receiver 3a #3b #4
This is an amplifier compatible with C and 4D. Furthermore, the time difference detection circuits 21, 22, 23, 24 each sense amplifier 17.1g.
, corresponds to 19.20), measures the time from wave transmission to wave reception, and as a result, detects the distance t between the transmitter and the receiver. As such, the zero output from each transmitter is configured to be detected by all the passers.

First, a wave transmission command S that gives a ting to generate an ultrasonic wave
is added to the predetermined transmitter specified by the transmitter address P through the enable power E of the decoder 25. The wrinkle driver drives a predetermined corresponding transmitter in the transmitter 11=Ib53m, 3bO.

Ultrasonic waves are emitted into the air. When one transmitter is activated, the receiver 3 after 9 hours is proportional to the distance from the transmitter.
m, 3b, 4m, and 4c receive the ultrasonic waves. Each receiver 3m
, 3b, 4m, and 44 are output from each sense amplifier 17, 18.
．． Added on 19.20. The time from the generation of a wave transmission command to the generation of an output in the sense amplifier represents the propagation time of the ultrasonic wave between the corresponding transmitter and receiver. The wave command and the output of the sense amplifier are applied, the time interval between the two is measured, and the distance t is output.

As shown in FIG. 3, the time difference detection circuit includes an 8-S flip-flop 26 with input 1 as a set input and input 2 as a reset input, an oscillator 27, an AND circuit 28 for calculating the logical sum of these outputs, and an input. It consists of a counter 29 which is reset by l and counts the output of the AND circuit 28, and a register 30 which uses this output as a data input and input 2 as a write command.

In this circuit, the R-8 flip 70 knob 26 is turned on only from the time when human power 1 is applied until the time when input 2 is applied.

Therefore, the AND circuit 28 passes the output of the oscillator 27 only from the input 1 to the input 2.The counter 29 is reset when the input 1 is added and then counts the output of the AND circuit 28, so it is not necessary to manually 2 is added and the output of the AND circuit 28 disappears, the number of pulses generated by the oscillator 27 between the input 1 and the input 2 is counted, and this is the time from the input 1 to the input 2. Proportional. Therefore, if input 2 writes the output of counter 29 to register 30, this output will represent the time difference.

By the way, since the speed of ultrasonic waves in the air is approximately constant when the influence of temperature is ignored, the time difference measured in the above manner can be considered as a distance output. Four types of such distances are obtained each time one transmitter is driven. Therefore, by changing the transmitter address, 16 types of outputs can be obtained by sequentially switching and driving the four transmitters.

Table 1 shows the state of the output obtained by applying pressure as described above, and for all combinations K of transmitters and transfer devices, the distance output 'aa # d, b *...... d44 no 1
6 side output is obtained. As a result, the state in which the senna approaches each other can be detected, but the state in which the arm portion and the senna approach each other cannot be directly detected.

No. iIM! Although it is not possible to accurately calculate the distance between the sensor and the arm from the 16 values representing the distance between sensors in Table 3, it is possible to define the numerical value corresponding to this distance lIK as follows. Paying attention to arm part l and transfer device 3b, arm part l
Let Ll be the length of , and define the following quantity.

1, s b= d,, + dbb −L s
・・・・・・・・・・・・・・・・・・・・・(1) This amount is the difference between the sum of two sides of the triangle formed by the senna and the arm and the other side, The closer the senna and the arm get, the smaller it becomes, and it becomes zero when they come into contact. Therefore, this amount can be used as a guideline for the distance between the senna and the arm. If the same amount is determined for all combinations of arm portion and senna, 16 types of values will be obtained as shown in Table 2.

Next, a summary avoidance method will be explained using the 16 types of values indicating the distance between the senna and the 16 types of values corresponding to the distance between the senna and the arm, which were obtained as described above. First, the entire system will be explained, and then, since the sensor on the finger side and other sensors are different, each avoidance method will be explained based on each embodiment.

First, before explaining the whole thing, let me explain what kind of structure the platform arm actually consists of.
In this figure, which is a configuration diagram of the 1000-bit, each transmitter is omitted from the drawing. In Figure 0, which is for the purpose of simplifying the explanation, the same symbols as in Figure 1 indicate the same contents, and for driving,
Motors 31, 32, 33, 34, 35, 36, 37 are provided. The combination of the driving directions of these motors determines the degree of freedom of the direction of movement of arm A and arm portion 1.2. rl, r2. rl, 14°r 5e'
6m'7 respectively indicate the direction of movement, rl, r2.
rl, r4 and rs are motors 31°32.33.34.
The two degrees of freedom at the tip of the arm portion 2 are driven by the sum and difference of the rotational movements of the motors 35° and 36. That is, the left and right movement of the finger 5 is the sum (rs
+ra) The forward and backward movement is driven by the difference (rs-rs).

Further, the degree of freedom of the tip of the finger 5 is set by the direction of rotational movement r7 by the motor 37.

The above configuration of arm A is the same for other arms B as well.

Next, the control system for the platform arm is shown in Figure 5. In Figure 1,
The position command device 38 is a device that determines the direction of movement of the arm and determines and commands the amount of each free fK along the direction of movement, and is the most central device in the control system. That is, this device is the center of control for the platform arm. Of course,
The position control system [38 is under control under the control of a global position control system (not shown) which reaches and controls each arm.

Such position command device #38 has seven degrees of freedom r1. r2゜rl, 148
r5. Calculate and output the teno operation amount for rs and r7. This output is input to the coordinate transformation device 39. The l* coordinate conversion device 9 receives the above output and performs coordinate conversion in accordance with the actual positional relationship of the arm.The coordinate conversion device 39 converts the actual degrees of freedom (03 I movement) rxs based on the coordinate conversion. r2
．． rl.

r4. r5. Outputs rs and r7. Each free 1Fi, positioning device [4G, 41.42.43.44°45.46
sent to. Each positioning device! In FIG. 0, only the internal configuration of the positioning device 40 is shown, but the other components have the same configuration. In other words, positioning device #40 includes servo amplifier 4001 motor 31 (
(same as motor 31 in FIG. 4), position detector 401,
It consists of a speed detector 402. This positioning device 40 is
Position detector 4011 speed detector 4 connected to the motor shaft
The motor 31 is operated based on the servo amplifier 400 which receives the 020 output and the output r1 of the coordinate conversion device 39 as inputs. The operation of this positioning device is the same for other positioning devices.

Next, in Figure 6, which shows the overall configuration of the avoidance system of the present invention, the abnormal approach detection device 47 calculates the distance shown in Figure 211 based on the output from the traveling time difference detection circuit shown in Figure 3. This is a device that detects whether there is an abnormal approach. When an abnormal approach is detected, the system indicates the abnormally close part between the relevant arms at that time, such as the corresponding arm part and finger if the arm part and finger are the same, and determines the avoidance direction. to device 48;

The avoidance direction determining device 48 sets an avoidance command 451 and an avoidance direction 48b for the abnormally approached portion,
It is sent to the avoidance command device 49. This avoidance command device 49 uses human power to generate the avoidance command 48a and the avoidance direction 48b, and sets the actual avoidance operation amount. The manipulated variable which is the output of this avoidance command device 49 is shown as an output 49m in FIG. 5 and is sent to a corresponding position command device such as the ninth position lid command device 38. The subsequent operations are performed according to the configuration shown in FIG. 9, the avoidance command device 49 sends the avoidance start command 49b to the circuit 50.
send to This calculation circuit 50 starts its operation based on the avoidance start command 49bK, and the calculation contents are the same as those described below.

In other words, the operation circuit 50 #i detects the operation status of the ninth position stirrup setting device shown in FIG. This *C signal, which is detected and input as the position detection signal 50a, is subjected to continuous Kfi as much as possible.

This input personal power signal SOa is shown in Table 2 and (1
) is converted into data for judgment based on the formula, for example, (1
), the relative distance t1- can be found. This relative distance is continuously detected as the player exits the avoidance motion. In the process of this detection, it is determined whether or not the relative distance is progressing in the avoidance direction. The extent of this determination is determined depending on whether the relative distance is increasing or decreasing as the avoidance movement progresses. If it increases, it is determined that the avoidance operation is being performed normally, and if it decreases, if the avoidance operation continues, the #Todoroki parts will collide with each other, so the avoidance operation is stopped. let At this time, an abnormal approach signal is generated. When it is determined to be normal, the operation continues in step 11. Then, when the relative distance reaches a value that is considered to be far enough away from the abnormal approach distance, that is, when the reference distance and the relative distance are compared and the latter becomes larger, the avoidance operation is considered to have ended. Complete the evasive action.

Next, let's discuss avoidance actions based on one specific example. The avoidance behavior differs slightly depending on which parts are abnormally close to each other. First, let us describe an example of abnormal approach between the senna of the finger joint and the arm section. Fig. 7 shows the wave reception of the arm section 2 of the arm A and the arm section 4 installed close to the finger 6 of the MB. 1) An example of an abnormal approach with 4d is shown. At this time, the direction of abnormal approach, that is, the direction of movement, is the direction of arrow Q. Due to this abnormal approach, arm 2 and receiver 4d
According to Table 2, the distance between can be defined as t and a. Following the abnormal approach, t! 4 is decreasing.

If the abnormal approach judgment value is 1, then lem decreases and t! When -<1, abnormal approach is determined. This determination is made by the abnormal approach detection device 47 shown in FIG. 6. Next, a signal is sent to the avoidance direction determining device 148 to specify the distance between the parts that have approached the abnormally close distance, which is then used to determine the avoidance direction.

When controlling a multi-arm robot, what the control device can reliably grasp and control is the direction of movement of the fingers and the amount of movement of each drive shaft. Therefore, it is effective to perform an avoidance operation based on this amount of information and the amount of operation, and to proceed perpendicularly to the direction Q when it is determined that an abnormal approach is being made. The circle in Fig. 7 is perpendicular to the traveling direction Q and centered on the receiver 4d, and the avoidance direction is chosen on this plane.To determine this direction, the circle performs the following first-order operation. Shujo KXt, X5e
Four points YtsYs are provided so that Xl-Xl and Yl-ys are orthogonal. First, the finger is moved so that the receiver 4d is at the position Xs, and the difference between the finger 12a at this time and before the movement is set as tx. Similarly, the receiver 4d is Ys
, move the finger so that the position of X, Yl is fK, and at this time t! - tvle lem s difference from before movement
Let it be Lvs.

Using these four quantities, the avoidance direction is determined by the following procedure.

(1) lxx @txs e lvs #tx
Choose the point corresponding to the largest one in l, for example, t
If xl is the maximum, then Xl is selected.

(2) Select the point corresponding to the larger one among the axes that the above point is not on. In the above example, tYl and tyl.

Choose the larger one, for example, if lvs is large, Y
It becomes l.

@) Distribute the angle surrounded by the two points selected by pressing as above with the corresponding deviation. In the above example, the angle θ is .

As described above, the direction R (see FIG. 7) in which the avoidance operation is most effectively performed is selected.

Once the avoidance direction R is determined through the above process, the first avoidance command device 49 is activated. Although the avoidance operation was briefly described in FIG. 6, it is actually performed as follows. Let's describe the operation including the operation of the arithmetic circuit 50. The distance of avoidance and advance to be performed at one time is determined, and after - times of avoidance, the advance operation is repeated until Lea≦a. ,
If t and LHa≦a, the avoidance action is resumed.After 1 avoidance, it is checked whether t and a have increased compared to before the avoidance, but even if the avoidance is performed, tea will increase and the If not, it is assumed that the system is out of control, displays an error message, and leaves it to humans to operate. Also, while the advancing motion is repeated, it is checked whether t□ is smaller than a large constant value (1), and if it is smaller, the avoidance motion is continued, but if it exceeds b, It is considered that the evasion is completed and it is 9th, and the evasion movement is aborted and the normal movement returns.

The above has described the avoidance operation when the sensor on the finger side and the arm approach each other, but the case where another sensor and the arm approach each other will be described as another example.

FIG. 8 shows a state in which the arm portion 2 and the receiver 4 are close to each other. In a robot, what should be controlled is the position of the finger, but in the case of Fig. 8, if the method of the above-mentioned embodiment is used to avoid this, there is a possibility that the position of the finger will change greatly, which is undesirable. , Therefore, in this case, the position of the finger is fixed and the avoidance operation is performed. Since the articulated robot shown in FIG. 8 has redundant degrees of freedom, it is possible to move the position of the wave receiver 4C while keeping the fingers fixed.

When performing evasion, the operation axis is the axis at which the arm is attached, and by performing correction operations on other axes according to this movement,
Assume that the attachment point of the arm that keeps the finger in a fixed position has two degrees of freedom, i in the direction toward the other arm and in the direction orthogonal thereto.

Now, when tl becomes smaller than the carefully set value, the control lid of the four bots determines that the robot is approaching abnormally, and starts the evasive action.At the beginning of the 0 evasive action, the direction of the 2 evasive action is determined, but in this case, , the direction in which tl becomes smaller is determined as the avoidance direction by a certain amount in both directions of the θ axis. The user will perform avoidance actions similar to those in the above case.

Note that the above avoidance method can also be performed using a general digital computer. In this case, it is desirable to engage the digital computer after determining the avoidance direction.The reason for this is that, as is clear from the two typical cases mentioned above, Rounding takes the same processing form even when parts are abnormally close to each other. That is, after the avoidance direction is determined, it is sufficient to proceed with the work under the same program. A flowchart of this program is shown in FIG. The operation of this flowchart will be omitted as it can be fully explained in the previous explanation.

〔Effect of the invention〕

According to the present invention described above, collisions of robots operating on complicated trajectories can be automatically avoided.

[Brief explanation of the drawing]

FIG. 1 is a schematic diagram of a multi-arm robot system to which the present invention is applied;
Figure 2 is an example circuit diagram for distance detection, Figure 3 is an example circuit diagram for a time difference detection circuit, Figure 4 is an overview diagram of the robot's arm, Figure 5 is its control system diagram, and Figure 6 is a diagram of the present invention. Embodiment FIGS. 7 and 8 are explanatory diagrams for an embodiment of the present invention, and FIG. 9 is a diagram showing a flowchart of processing. 38...Position command device, 40.41, 42.43°4
4.45.46... Positioning device, 47... Abnormal approach detection device, 48... Avoidance direction determining device, 49...
Avoidance command device, 5°0... calculation circuit. Figure 4 Figure 5) 8 Figure 6 Figure 7 Figure 8 Figure q

Claims (1)

[Claims] 1. In an eye bot with ultrasonic elements attached to a predetermined position on the arm, a human eye is attached to the scissors arm to determine the propagation time of ultrasonic waves between the nine ultrasonic elements. Detecting the approach of the arms to each other, determining whether the arms are approaching each other abnormally using the detected output as 4, and # When it is determined that the arms are approaching each other abnormally, identify the parts of the arms that are abnormally close to each other. determine an avoidance direction based on the identification, and when the wrinkle avoidance direction is determined, perform an avoidance operation along the determined avoidance direction at nine o'clock;
A D&foot collision*iis avoidance control method characterized by performing control to avoid collision between arms that are abnormally close to each other.