KR100218106B1 - A collision prevention method of a dual-arm robot - Google Patents

Abstract

The present invention relates to a dual-arm robot system, in particular, having a function of checking the collision of the two arms to perform different tasks to determine whether the collision of the arm occurs, and when the collision is possible, detailed modeling of the arm The present invention relates to a collision avoidance method of a dual-arm robot which reduces the time required for collision calculation of a robot arm by performing a step of detecting a collision.

Description

Anti-collision control method of dual-arm robot

The present invention relates to a dual-arm robot system, and in particular, to control a dual-arm robot system in which two robots are fixed on one base, the two robots can work without colliding with each other. The present invention relates to an anti-collision control method of a dual-arm robot that implements a method of quickly detecting a collision of a robot arm.

1 is a diagram illustrating a general dual-arm robot system, which includes a dual-arm robot 10 and a system controller 20.

On the other hand, the dual-arm robot 10, the support member 11 for supporting two robots that each perform independent work, and the specific work is installed in the support member 11, respectively, according to a predetermined control signal It consists of first and second arms 13, 15 (17, 19). In addition, the first and second arms 13, 15, 17, 19 are connected to the support member 11, respectively, to operate the first link 13 and 17 along a predetermined trajectory, and the first It is composed of second links 15 and 19 which are connected to the links 13 and 17 and which are operated and perform specific tasks according to a predetermined program.

In addition, the system controller 20 is configured to set a predetermined program and independently control the operation of the first and second arms 13, 15, 17, 19 of the dual-arm robot according to the set program. The first and second arms control the link of the first and second arms so that they do not collide with each other through the shared memory storing the movement trajectories of the first and second arms.

The system control unit 20 of the conventional dual-arm robot configured as described above is to independently control the first and second arms (13, 15) (17, 19) installed in the support member (11). Each unit 13 generates a trajectory for moving the arms 13 and 15 and 17 and 19.

After modeling each arm for the trajectories generated in this way, if the modeling information does not overlap between the rectangular modeling of other arms, save the modeling information for the new trajectory in shared memory and then load each arm of the robot. To drive.

On the other hand, when the overlap occurs in the modeling, the robot is decelerated and stopped through the step of ignoring the newly generated trajectory.

Subsequently, a new trajectory is generated by repeating the step of generating a new trajectory, and the trajectory is repeated to check whether the trajectory collides with another arm, and when the arm of the robot is released from the collision state, the trajectory is moved along the new trajectory. .

If the new trajectory is the target position, the trajectory generation ends.

Therefore, as described above, in the modeling step of the arm for detecting a collision of the robot arm, the modeling of each arm has a rectangular shape, and in the collision detection step, the collision and modeling calculations are performed by comparing all the rectangular modeling results. There was a problem that it takes a long time to calculate a collision.

In order to solve the problems of the prior art as described above, an object of the present invention is to determine whether a collision of arms occurs by checking a collision between two arms that perform different tasks, and if the collision is possible, The present invention provides a collision avoidance method of a dual-arm robot that reduces collision calculation time of a link on a trajectory of each arm by performing detailed modeling.

The anti-collision method of the dual-arm robot for achieving the above object comprises the steps of generating and calculating the trajectory of each arm to be moved after a unit time, and obtaining the positions of the two robot links on the Cartesian coordinate system; A first judging step of judging whether the trajectory data of each arm obtained on the coordinates is position data where link overlap may occur; A second judging step of judging whether or not each link is overlapping data in the judging step, whether the position data collides with a link of another arm and the end of the predetermined link; A third judging step of judging whether the links of a predetermined arm overlap with each other if the links are not colliding data in the judging step; If the link of each arm collides or crosses data in the second and third determination steps, ignoring the new trajectory and decelerating and stopping the robot; Driving the link of each arm according to a predetermined trajectory program if the link of each arm is not collided or crossed in the second and third determination steps; And after performing the above steps, determining whether the link of each arm has reached the target position.

1 is a view showing a general dual-arm robot system.

2 is a flowchart illustrating a collision avoidance control process of a dual-arm robot according to the present invention.

3 to 6 are diagrams for explaining a method of detecting and determining whether or not each arm according to the above 2 collision.

* Explanation of symbols for main parts of the drawings

10: XY robot. 11: support member.

13: The first link of the first arm. 15: second link of the first arm.

17: First link of the second arm. 19: Second link of the second arm.

20: system control unit.

Hereinafter, with reference to the accompanying drawings will be described in more detail the present invention.

FIG. 2 is a flowchart illustrating an anti-collision control process of a dual-arm robot according to the present invention. Referring to FIG. 1, the present invention will be described in more detail as follows.

First, the system controller 20 generates and calculates a trajectory of the robot to be moved every unit time, and obtains a position of each link with respect to the first arms 13 and 15 and the second arms 17 and 19 on the rectangular coordinate system. Perform (S1).

The trajectory data of each of the arms 13, 15, 17, and 19 obtained on the rectangular coordinate system performs a first determination step of determining whether the link data may be position data at which link overlap may occur (S2).

In the first determination step, if the links of the arms 13, 15, 17 and 19 are overlapping data, a second determination step of determining whether the end data of the arm and the position data collides with the link of the other arm is performed. In operation S3, if the link is not collided with each link in the determination step S3, a third determination step of determining whether the link of each arm overlaps with each other is performed (step S4).

If the links of the arms 13, 15, 17, 19 are collided or crossed in the second and third determination steps S3 and S4, the collision of the link is prevented by ignoring the new trajectory and decelerating and stopping the robot. It prevents (S31, S32), and repeats from step S1 which produces | generates the trajectory of each arm.

If the links of the arms 13, 15, 17, 19 are not collided or crossed in the second and third determination steps S3 and S4, the link of each arm is driven according to a predetermined trajectory program. Perform the step (S5).

After the step S5 is performed, a step of determining whether the link of each arm reaches a desired reference point, that is, the correct target position programmed by the user, is performed, and when each link reaches the correct target position, the process ends (S6). ).

On the other hand, if the data in each link does not overlap in the first determination step (S2), storing the trajectory data of each link in the shared memory of the system controller, and driving the link of each arm in accordance with the program (S5) From (S21).

In other words, if the collision of each link 13, 15, 17, 19 of the dual-arm robot is detected and determined in the same manner as above, and if the collision does not occur in the first determination step S2, no further links are detected. Since collision is not detected, the computation time for collision detection can be reduced.

3 to 6 are diagrams for explaining a method of detecting and determining whether or not each arm of FIG. 2 is collided with reference to FIGS. 1 and 2.

First, Figure 3 is a view showing the arm of a dual-arm robot according to the present invention in the XY coordinate system, the first link (13) (17) of the first and second arms (13, 15) (17, 19) And second links 15 and 19, respectively.

When the first and second arms 13, 15, 17, 19 are on the X-Y plane, the indication of the joints at both ends of each link may be expressed as (Xmin, Xmax) (Ymin, Ymax).

FIG. 4 is a view for explaining the method of the first determination step S2 of FIG. 2, wherein two links 15 of the first arms 13 and 15 and two links of the second arms 17 and 19 ( When the area of 19) is displayed on the XY coordinates and each position is X11, X12, X21, Y11, Y12, Y21, and X22, Y22, it is determined that the first-stage collision occurs under the condition of the following Equation 1. Do it.

Equation 1

(X21≥X12) ∩ (X22≥X11) ∩ (Y21≥Y12) ∩ (Y22≥Y11)

(However, Xx: arbitrary coordinate value of X axis, Yy: arbitrary coordinate value of Y axis)

That is, the system controller 20 compares the area formed by the ends of the links 15 and 19 of each arm as shown in Equation 1 to determine whether the collision of each arm occurs.

Next, FIG. 5 is a view for explaining the method of the second determination step S3 of FIG. 2, and it is determined whether a collision between two links occurs when one link 15 approaches the other link 19. It is to.

At this time, when half of the thickness of the two links (15) and (19) is referred to as R1 and R2, respectively, the condition of the collision is expressed by the following expression (2).

Equation 2

d = R1 + R2

(Where d is the vertical distance between the thickness center of the first arm and the thickness center of the second arm, R1: half thickness of the link thickness of the first arm, and R2: half thickness of the link thickness of the second arm.)

d = Δxsinθ = Δycosθ

Where Δx is the X axis distance between the link center of the second arm and the link center with the first arm, Δy is the Y axis distance between the link center of the second arm and the link center with the first arm, and θ is the first arm. The angle between the center of thickness and the line Δy.

That is, when each arm can collide in the first determination step, a two-step collision test is performed as shown in Equation 2.

Finally, FIG. 6 is a view for explaining the method of the third determination step S4 of FIG. 2, and after performing collision inspection as in Equation 1 and Equation 2, links 15 and 19 of each arm If it is determined that there is no collision), a method of determining whether or not each link overlaps again is performed.

The third step collision check is to determine whether two links 15 and 19 intersect with each other. If both ends of the two links are expressed as vectors of origin 0 of XY coordinates, as shown in a and b, I can display it. At this time, when the determination condition as shown in Equation 3 is satisfied, the two links cross.

Expression 3

sign (e12, e11 x e12, e22) ≠ sign (e12, e21 x e12, e22)

(Sign (x): 1 if x≥0, -1 if x <0.)

Therefore, in the control of the dual-arm robot system as described above, a process for determining whether two arms collide with each other while working independently of each other through a three-step collision detection algorithm to determine whether each arm has collided or not. There is an effect that can be judged and calculated faster.

Claims (5)

Generating and calculating a trajectory of each arm to be moved after the unit time, and obtaining the positions of the two robot links on the rectangular coordinate system; A first judging step of judging whether the trajectory data of each arm obtained on the coordinates is position data where link overlap may occur; A second judging step of judging whether or not each link is overlapping data in the judging step, whether the position data collides with a link of another arm and the end of the predetermined link; A third judging step of judging whether the links of a predetermined arm overlap with each other if the links are not colliding data in the judging step; If the link of each arm collides or crosses data in the second and third determination steps, ignoring the new trajectory and decelerating and stopping the robot; Driving the link of each arm according to a predetermined trajectory program if the link of each arm is not collided or crossed in the second and third determination steps; And determining whether a link of each arm reaches a target position after performing the above steps. The method of claim 1, wherein in the first determination step, if each link is non-overlapping data, storing the trajectory data of each link in a shared memory of the system controller, and driving the link of each arm according to the program. The anti-collision control method of the dual-arm robot, characterized in that performing. The dual-arm robot according to claim 1, wherein the first determination step determines whether each link collides by using a method of comparing an area formed at the end of each link in the rectangular coordinate system as shown in the following equation. Anti-collision control method. expression (X21≥X12) ∩ (X22≥X11) ∩ (Y21≥Y12) ∩ (Y22≥Y11) (However, Xx: arbitrary coordinate value of X axis, Yy: arbitrary coordinate value of Y axis) The method of claim 1, wherein the second determination step determines whether each link is collided using a method such as the following equation. expression d = R1 + R2 (Where d is the vertical distance between the thickness center of the first arm and the thickness center of the second arm, R1: half thickness of the link thickness of the first arm, and R2: half thickness of the link thickness of the second arm.) d = Δxsinθ = Δycosθ Where Δx is the X axis distance between the link center of the second arm and the link center with the first arm, Δy is the Y axis distance between the link center of the second arm and the link center with the first arm, and θ is the first arm. The angle between the center of thickness and the line Δy. The method of claim 1, wherein the second determination step comprises determining whether or not each link is overlapped using a method such as the following equation. expression sign (e12, e11 x e12, e22) ≠ sign (e12, e21 x e12, e22) (Sign (x): 1 if x≥0, -1 if x <0.)

Images