- Email: [email protected]
Real time, rule based collision avoidance for two robots
Preprints of the Fourth IFAC Symposium on Robot Control September 19-21, 1994, Capri, Italy
Real time, rule based collision avoidance for two robots U. Borgolte • °FernUn'"er•• t
Abstract. An algorithm is presented which serves fo r collision avoidance in ~D space between two robots of the Stanford-type. This includes the detection of danger of collisions as well as the avoidance of suc h coll isions. The method is based on the state space description of the robots involved and takes care of their dynamic behaviour. Deviations from the programmed paths, e .g . caused by sensor information or due to faults , are immediately regarded . The required computation time perm it. an onl ine application. As the structure of the algorithm allow. it. execution on parallel hardware, further improvement of efficiency can be achieved .
Key Word•• Robots; obstacle avoidance; multirobot systems ; colli.ion detection ; collision avoidance
isn't possible with the restraints of a projection on the xy-plane, but given in the 3-dimensional case - can be recognized and handled properly.
1. INTRODUCTION With increased automation and application of CAD/CAM methods in connection with automatically generated robot programs (e.g. by implicit programming) an overall consideration of multi-robot systems becomes more important. This seems to be the only possibility to reduce the programming effort and the fault rate and to provide flexibility in manufacturing. In spite of the importance for future developments very few results are known in this filed. In this context, collision avoidance strategies, which have to be superimposed on all coordinated operations, become a basic requirement. An obvious approach to avoid collisions is the generation of secure paths before the start of the movements. The basis for this can be methods like the Configuration Space or Potential Field Approach as introduced by Lumelsky (1987) and Khatib (1986). These are computed offline and are able to generate optimized trajectories. But they are insufficient if the environment is changing unpredictibly or it is not completely known. In this cases a method for online collision avoidance has to be implemented , too.
If
Robot k
Fig. 1. System with two robots
The algorithm presented is based on the StanfordArm with the degrees of freedom rotation-translationtranslation for the first three axes. The hand including gripper and load is considered by safety factors . The goal is the coordination of a working cell with r robots. Every robot j in the cell is brought in relation to every other robot k therein (fig. 1). 'I'''i denotes the angle between the x-axis of the local coordinates system j and the connecting line a"i .
The algorithm presented is based on Borgolte (1991b) . It detects the danger of collisions in realtime and generates collision free trajectories while the agents are moving. Their dynamic behavior and constraints are considered, too. Due to the very limited computation time, the only criterion for optimization is the avoidance of collisions. To further improvement it has now been enlarged to the third dimension.
The position of the gripper/tool of robot j in relation to a"i is given by 4>~i(tn): (1) where N(a) is the mapping of a on [-~ , +~] . The corresponding coefficients for robot k are 'Pi" (t n ) and
2. THE VIRTUAL HINDRANCE ROBOT To detect and estimate the danger of collision between the robots, a so-called Virtual Hindrance Robot (VHR) is constructed . The difference between this virtual robot and the robot j, together with the rela.tive velocity between these systems, gives a valuation for the time until a collision may occur. The geometrical structure presented in this paper has the advantage to take the whole working space of robot j into account. Furthermore, a crossing of the arms - which
4>j,,(tn). 2.1 . The characteri$tic triangle With the basic denominations a characteristic triangle for the robots j and k is given. The baseline is a"i , the sides result from the prolongation of the robot arms (fig. 2) . This triangle exists then and only then ,
783
Fig. 3. Significant values in the characteristic triangle Robot
Robot k
J
2.2. Condruction of the Virtual Hindrance Robot Fig. 2. The characteristic triangle if the second axes are not parallel and none of them is aligned with the baseline. These cases are treated seperately. The region bounded by the third axis of robot j and the straight line through the origin on robot k parallel to it is the common working space of the robots j and k with respect to robot j. That part of the third axis of robot k which projects into this region is nearest to the third axis of robot j. It is therefore part of the charcteristic triangle (fig. 2). Assume the configuration in fig. 2. Due to the shortage of pla.ce the construction of the VHR in this paper is done with this example. The deta.iled description of the geometric considerations is in Hoyer (1993). With this, the angle between the baseline akj and that part of the third axis of robot j which is part of the characteristic triangle is given by
q,k)(t n ) = J.I [q,~j(tn) -11"]
(2)
The translational coefficient of the third axis of robot j within the characteristic triangle is described by
1 .( ) _ {rj(tn) if q,~j(tn) = q,kj(t n ) I . rj - rj(t n ) otherwlse
rk) tn -
To compute the translation rkj(t n ) and the rotation tPkj(t n ) of the VHR, the shortest distance between the third axes of the robots j and k has to be determinated. Furthermore, the distance dkj(t n ) between the third axes of the robots, the distances ekj(t n ) resp. ejk(t n ) between the TCP of one arm and the ba.ckpoint of the other, as well as the perpendicular lines ljk(t n ) resp. lkj(t n ) from the end of one arm to the other one (fig. 3) must be taken into account. The end of an arm means the final point on the third axis which is part of the characteristic triangle. Backpoint means the opposite final point. So, with the exemplary configuration shown in fig. 2, dkj(t n ) is given by dkj(t n)
= COS
[r~j(tn),e~j(tn), q,~j(tn) - Xkj(t n)]
(7) (3)
where r} ist the total length of the third axis. The lateral length rkj(t n ) of the triangle is given by
• Isin[q,ik(t n )lI rkj(t n ) = akj Sin . [ ( t n )) CXk)
The VHR has the same geometry as the robots j and k. The first axis (not the angle) is identical to the first axis of the robot j. The position of its (virtual) TCP depends on the shortest distance between the third axes projected on the xy-plane, the distance between the second axes of the robots, and the shortest distance from the origin of robot j to the third axis of robot k.
(4)
with
CXkj(tn) = 11" -1q,jk(tn)I-Iq,:j(tn)1 (5) The angle CXkj(tn) designates that angle of the characteristic triangle which is opposite of akj. The difference between the lateral length rkj{tn) and the translational value r~j{tn) is denominated by rZi(t n ) as follows:
(6)
The function COS( a, b, 'P) gives the third side of a triangle with the sides a and b and the angle 'P between them. The shortest distance can also be the perpendicular line from the end of one arm to the third axis of the other robot. If the chara.cteristic triangle can be constructed and the lateral length rik(t n ) is greater than the translational coefficient rjk(t n ), i.e. ri;(t n ) > 0, then lkj(t n ) is given by
lkj{tn)
= e~j(tn) Isin[q,kj(t
n) -
Xkj(tn))1
(8)
ljk(t n ) is set up like lkj(t n ) with the indices reversed. The perpendicular lines are considered only if they are dropped on the arms and not just on the straight lines defined by them (like ljk(t n ) in fig. 3).
This coefficient is negative if the translational value is greater than the corresponding lateral length.
2.3. The First Axis - Rotation
The parameters q,ik(tn), rjk(tn), rik(tn) and ri;(tn) are set up like q,kj(t n ), r~j(tn), rkj(t n ) and rki(t n ) from eqs. (2) - (6) with the indices reversed. If both rki (t n ) and ri; (tn) are less than zero, the third axes are crossing ea.ch other. If the third axes are parallel or one is aligned with akj, the characteristic triangle does not exist. This special case is treated separately.
The rotation tPkj(t n ) of the Virtual Hindrance Robot (i.e. the first axis) is oriented to that point on the third axis of robot k, which has the shortest distance to robot j. This can be the end of the arm of robot k resp. its ba.ckpoint or the intersection of ljk(t n ) with the third axis of robot k. The last case is considered only if Ijk(t n ) is dropped on the arm and not just on
784
the straight line defined by it. The second axes are not taken into account. If the third axes are intersecting, the crossing point is of interest. If the third axes of the robots are para.llel, akj is choosen for the rotation of the VHR.
VHR need to get into the same position. With this, the dynamic behaviour of the systems is included, too. Like it was done by Borgolte (1991 b) for the first and third axis of robot j, the danger of collision for the second axis is given by TZkj(tn): tn $ z) Il!.Zz,"'.·.("t")ll·f lAUZkj ()I I otherwise I
The angle Xkj(t n ) from eq. (10), is used in two cases. First, if the third axis of robot j is aligned with akj and ljk(t n ) is not dropped on the third axis of robot k. Second , if one of dkj(t n ) , ekj(t n ) or lkj(t n ) denotes the shortest distance between the arms of robot j and k. This means for the configuration of fig. 2:
l!.Z.i ( t") l!.z.i(t,,)
with
with Xkj(t n ) as the angle between akj and the line from the origin of robot j to the end of the arm of robot k , which is denoted as e~)(tn) :
= -IIjk(t n ),x Hj(tn) , akj, rjk(tn)j
(10)
The function ,x( a , b, c ) gives the angle opposite to side c in the triangle a , b, c. IIjk(t n ) is the sign of
Tkj(tn)
(16)
Zj(t n )
(17)
=[
T
1
(18)
where T
2.4. The Second Axi., - Translation The coefficient of the second axis of the VHR is that of the robot k :
4. COLLISION AVOIDANCE The goal of the algorithm is the generation of desired values for a.ll axes which let the arm move without collisions. This is done with regard to the position and velocity as well as the dynamic behaviour of each axis. If there is no danger of collision, the original trajectory given by the pathplanning module has to be ·driven. Therefore the deviations are as sma.ll as possible.
(12) 2.5. The Third Axis - Translation The translational coefficient rkj(t n ) denotes the position of the third axis of the VHR. It is defined as the minimal distance between the third axis of robot k and the origin of robot j (which is identical to the origin of the VHR): rk(t n ) ,
Zj(tn)
So the vector for the danger of collision is given as
(11 )
= COS[ akj ,
(15)
This estimation is independent of the actual states of the other axes. The constant z'jaz is given by the motor and gearing of the axis. The danger of collision decreases as the distance increases and the relative velocity slows down. The usage of z'jGZ resp. ~Zkj(tn) limits z'jaz to an upper bound .
The parameter e~j(tn) is given by
rkj(t n )
= Zkj(t n ) ~Zkj(tn) = Zkj(t n ) -
LlZkj(tn)
(9)
Xkj(t n )
· mGZ
fir
The strategy has to consider the actual values for the danger of collision of a.ll axes. This is due to the fact that even if one axis is critical the whole arm may not be in danger (e.g. if the arms are crossing in the (x,y)-plane but they are far enough in the zdirection) . After leaving a dangerous situation the original trajectory will be reentered as fast as possible.
(13)
This computation of the VHR belongs to the configuration in fig. 2. The general case is described by Hoyer (1993).
The first choice for avoiding collisions is the second axis (z) . An alteration of the movement of this axis is able to let the
2. 6. Th e state-spa ce vector of th e VHR The position values for the first three axes of the VHR, together with their velocities, are describing the state of the VH R. With this the state-space vector is given as
(14)
The algorithm presented deals only with wire-models of the robot arms. The real volumes of the arms are covered by variable safety factors for each axis. This has the advantage to minimize the computational effort and a.llows to adjust the safety distance dynamica.lly as needed . The values 6€'jaz , with € E {fP , z,r} are including the volume of the arm as well as the tool
3. THE PARAMETERS FOR THE DANGER OF COLLISION The actual danger of collision is measured by a vector T, which describes for every degree of freedom the time that the corresponding axes of robot j and the
785
resp. gripper tance for one for the other distance may
and the VHR. It is given by
with workpiece. The actual saiety disaxis depends on the danger of collision axes. If this danger is low, the saiety be reduced. With
(19)
else, if
IAZkj(tn)1 ::; 5z;naz ] [ /\ IA'Pkj(tn)l::; 5'Pja%
5° 5rja% Ckj (t n ) else, if [/\
(20)
5° 5rja% Akj(t n ) a feasible transition between the minimal and the maximal value of 5Zkj(tn) can look like
else, if [/\
5°5rjaz Akj(tn)Ckj(t n ) otherwise (24) To compensate the effect of the control error, the safety distances can be enlarged by a factor 5° and the saiety regions by a factor 5'. This is due to the behaviour of the axis control. If the control error is compensated, 5° and 5' should be 1. All the safety distances 5ekj(tn), with e E {'P,z,r} are taking care of the real volume of the robot plus an additional space to move without collision.
else, if
5°5z;naZCkj(tn) else, if
Arkj(tn)::; 0
5°5z;naz Bkj(tn)Ckj(t n ) else, if
IA'Pkj(tn)1 > 5'Pjaz
4.1. Strategy for the axu
colli~ion
avoidance of the
~econd
To avoid a collision by moving the axis z, a new desired value for it has to be computed. This is done with regard to the vector for the danger of collision and to the dynmic behaviour of the robot. First, three intermediate values are generated.
(21) The safety distance 5Zkj(tn) reaches its maximum if r~j (t n ) is greater than the corresponding value r kj (t n ) and at the same time there is a danger in the rotational axis. It is at its minimum if either the angular distance is more than 5' times the secure distance 5'Pjaz, or the translational distance is more than 5' the secure distance 5rja%. Otherwise it is interpolated with a cosine function. The safety distance 5'Pkj(t n ) for the first axis is defined analogous to 5Zkj(tn) :
The first of them prevents the conformity of the axis Z of the real robot and the corresponding axis of the Virtual Hindrance Robot. The second one keeps the calculated variable distance between the real robot and the VHR. The third value ensures the maximal safety distance. Four auxiliary values have to be determinated first: 1. T~j(tn) is that Tekj(tn) with e E {'P,r}, which
indicates the higher danger of collision. else, if
0]
Arkj(tn) < [ /\ IAZkj(tn)1 < 5z;naz
T~j(tn)
5°5'Pja% Akj(t n ) else, if
Arkj(tn)
IAZkj(tn)1
T'Pkj(tn)
if T'Pkj(t n )::; -Trkj(tn)
0
else, if - Trkj(tn) < 0
-Trkj(tn) otherwise
(25) 2. (kj(t n ) holds information about the relative position of axis Z with respect to the corresponding axis of the VHR. If the third axis of robot j is above the third one of the VHR, it is less, otherwise greater than zero:
<0
5°S'Pjaz Akj(tn)Bkj(t n ) else, if
={
> 5zja%
5° 5'Pjaz Bkj(t n ) otherwise (22)
(26)
with
3. 'Ikj(t n ) describes whether the robot j and the VHR are getting closer in the z-dimension (-1) or they are departing (+1):
if sign[~Zkj(tn)] = (kj(t n ) I 'Ikj(t n ) := { -1 otherwise
The safety distance 5rkj(tn) depends on the differences in the first and the second axes of the real robot
(27)
786
4. tkj(t n ) is the time needed for the second axis
.Z.t
of robot j to get into a safe distance from the second axis of the YHR:
ztj(tn)
Alztj(tn) - Zkj(tn)1 > I~Zkj(tn}1
Zkj(t n )
Zkj(t n) + Zkj(tn)T~j(tn)
+
else, if
(kj(tn) [Zj(tn} - Zkj(t n }] [ V I~Zkj(tn)1 < C5zkj(tn) [ A .:izkj(t n ) = 0
[r,kj(tn)Z~j(tn) - z}(tn)] (kj(tn)
2znorn
if
ATrkj(tn) ~ -2t kj (t n ) ATIPkj(t n ) $ 2t kj (t n )
The first intermediate value Zkj(t n ) is the normal one. It holds the second axis of the robot in a secure distance to the YHR. This is independent of the movements in the (IP,r)-plane.
=
follows:
Trkj(tn) > Trkj(tn-d ] [ VTIPkj(tn) < TIPkj(tn-d
(28)
Zkj(t n)
aB
(29)
J
-~t
.:iZkj(tn}
[Zkj(tn) - Zj(tn)] (kj(tn)
V
-6Zkj (tn)(kj (t n ) The first term in (29) describes the expected zposition of the YHR at that time when the faster one of the IP~ resp. r-axis is coincident with the corresponding axis. The second term is the distance to be driven at most until the robot j reaches the velocity of the YHR. The third term is the way driven by the second axis during the time interval ~t . With the last term a safety distance is added. From this the normal intermediate value Zkj(t n ) describes that z-position of robot j which is characterized by: • it can be reached in the time T~j(tn), even if only the nominal acceleration can be achieved; • the second axis of robot j holds the same velocity as the corresponding axis of the YHR; • a secure distance is kept. In some cases, this can include a brake of the movement and an acceleration in the opposite direction of the second axis. The second intermediate value Zkj(t n ) keeps the zaxis of robot j in the calculated variable distance from the YHR. The corresponding second axes are moving with the same velocity and the safety distance
6Zkj(t n ): Zkj(t n ) = Zkj(t n ) + Zkj(tn)~t - (kj(tn)6Zkj(tn) (30) In (30) , Zkj(t n ) + Zkj(tn)~tis the position of the second axis of the YHR in the following time step. According to the relative position the safety distance is added resp. subtracted. The last intermediate value ztj(tn) ensures the maximal safety distance between the second axis of the robot j and the second one of the YHR:
ztj(t n ) = Zkj(t n ) - (kj(t n )6°6zjG%
(31)
Now it must be decided which of the above derived intermediate values is used for collision avoidance. Criteria are the actual danger of collision and the strategy used at the previous time tn-l . 4.2. Deci$ion of the new de$ired vector If a danger of collision exists and robot j has to avoid a collision with respect to robot k, the desired vector wJ(t n ) given by the path interpolation is overwritten by a modified vector Wkj(tn) . In the following, the conditions for modifying wJ(t n ) and for chasing one of the intermediate values are described.
[
0]
1
1
< 6Zkj(t n)
else, if
Zkj(t n )
[
f:. 0
A'1 > 0 AI~Zkj(tn)1
~
A~~to(tn} f:. 0 AI~Zkj(tn)1
1
< 6Zkj(t n)
A(kj(t n ) [Zj(t n ) - Zkj(t n )] > 0
zj(t n )
otherwise
(32) The third intermediate value ztj(t n } is used if the danger for the r- or lP-axis is growing, indicating the last chance for the z-axis to avoid, and this position is better than the current one. The second intermediate value Zkj(t n ) is used, if the distance between the second axes of robot j and the YHR is constant, but within the secure distance, or it is less than Zkj(t n }. It is used also, if the distance is growing, but still not secure. The normal intermediate value Zkj(t n ) is used if the distance between the second axes is shrinking, not secure and less than Zkj(t n ). In any other case Zkj (t n ) is set to the original desired value zj(tn). As the z-axis is the first step for collision avoidance, an examination has to be done for the other axes if Zkj(t n ) f:. zJ(t n ) . A collision avoidance for the other axis is possibly necessary, if Z is critical and the danger for Z is growing, or if all axes are critical, or if the YHR is very close to the column of robot k . The algorithms for the tp- resp. r-axis are described by Borgolte (1991a) . The axes of a real robot are limited in their movement, so if the value from (32) is out of these bounds, it must
be restricted by
zmu
.Z.t
{
~rin
if
Zkj(t n } > zjU
if
Zkj(t n } < zjin
(33)
Zkj(t n } otherwise
With the new desired value Zkj(t n } for the second axis and the values r"j(t n } and rp"j(t n ) from Borgolte (1991a), the modified desired vector Wkj(t n ) is given by
(34)
787
2.5
2.-----.---r-.-----.---r----,.----~__,
.------r---..---~--.--,
1.5
0.5 .!Il
~
>.
0
-0.5 -1
-1.5 -2
o '----'---.........- ........----'--..... o 0.5 1 1.5 2
'--.....0---1._-'--........---''--.........---'----'
-2 -1 .5 -1 -0.5 0 0.5 x-axis
1 1.5 2
Fig. 4. Programmed trajectory in the (x,y)-plane
Time [see)
Fig. 5. Distance of the
This new desired vector Wkj{t n ) makes available a collision free trajectory. The original path is altered to this new one then and only then, if, with regard to the dynamic behaviour, it is necessary to prevent a collision. The algorithm has been developed in MODULA 2 on a SPARCstation ELC. Depending on the configuration of the robots, the time needed for the calculations takes about 1 msec, which allows an online application.
2
5. SIMULATION EXAMPLE Fig. 4 shows an example of a trajectory in the projection on the {'P-r )-plane. The start positions of the arms are marked by solid lines, the end positions by dashed lines, the paths of the TCP by dotted lines. The start point of the z-axis of robot 1 is at 1.0m, that of robot 2 at 0.5m. The destination point for the z-axis of robot 1 is also at 1.0m, that of robot 2 at 1.5m. The necessary crossing of the arms would cause a collision of the arms near the akj-line (in this example the x-coordinate). The resulting trajectory with collision avoidance in the projection on the 'P-rplane looks like fig. 4, no collision avoidance is done be the first and third axis. Fig. 5 shows the distances of the arms. The safety distance of 0.2m is always kept . Fig. 6 shows the movement of the z-axis of robot 2. Robot 1 does no collision avoidance, therefore the movements are not recorded.
arIllII
o '--_-'--_........._ ........_---'-_...J o 0.5 1 1.5 2 Time [sec)
Fig. 6. Robot 2, position z(t)
Their dynamic behaviour and constraints are considered, too. In this paper, the algorithms are given for two robots in a multi-robot system, where the computational time needed, permits a real time application. The performance of this new approach is demonstrated by a simulation example. A further advantage of the approach presented is, that for more than two robots the computational effort for online collision avoidance can be kept generally constant by using parallel processing hardware. Its pratical application to robot systems is currently in action in our laboratories.
When approaching the critical area around akj , robot 2 delays its movement in z until its third axis has crossed the third one of robot 1. After that the axis is also moving to its programmed end position.
7. REFERENCES Borgolte,U. (1991a). Flexible, realzeitfahige Kolin Mehrroboter-Sy"temen. li"ion"vermeidung Springer, Berlin. Borgolte, U. (1991b). Flexible, online collision avoidance in multi-robot systems. Preprinb 3rd IFAC Sympo"ium on Robot Control, 759-764. Hoyer, H., U. Borgolte and F. Wrosch (1993). New approach for online collision avoidance in 3D-space. Proceeding" 2nd International Conference on Advanced Mechatronic", 300-305 . Khatib, O . (1986). Real-time obstacle avoidance for manipulators and mobile robots. Int. J . of Robotics Ruearch, 5, 90-98. Lumelsky, V.J. (1987). Effect of kinematics on motion planning for planar robot arms moving amidst unknown obstacles. IEEE Journal of Robotic" and Automation, Vol. RA-3, 207-223.
6. CONCLUSION Even if global planning and programming is done in multi-robot workcells, in real-world applications it is important to supervise the actual movements and to influence them if necessary. This is due to sensor driven changes of the trajectories, incomplete information of the environment and possible failures in mechanical devices. Therefore, precautions have to be taken for online collision detection and avoidance. Based on a previous published algorithm for online collision avoidance in the (x,y )-plane the method presented in this paper can do the online supervision in 3D. It detects the danger of collisions and generates collision free trajectories while the agents are moving.
788
Real time, rule based collision avoidance for two robots U. Borgolte • °FernUn'"er•• t
Abstract. An algorithm is presented which serves fo r collision avoidance in ~D space between two robots of the Stanford-type. This includes the detection of danger of collisions as well as the avoidance of suc h coll isions. The method is based on the state space description of the robots involved and takes care of their dynamic behaviour. Deviations from the programmed paths, e .g . caused by sensor information or due to faults , are immediately regarded . The required computation time perm it. an onl ine application. As the structure of the algorithm allow. it. execution on parallel hardware, further improvement of efficiency can be achieved .
Key Word•• Robots; obstacle avoidance; multirobot systems ; colli.ion detection ; collision avoidance
isn't possible with the restraints of a projection on the xy-plane, but given in the 3-dimensional case - can be recognized and handled properly.
1. INTRODUCTION With increased automation and application of CAD/CAM methods in connection with automatically generated robot programs (e.g. by implicit programming) an overall consideration of multi-robot systems becomes more important. This seems to be the only possibility to reduce the programming effort and the fault rate and to provide flexibility in manufacturing. In spite of the importance for future developments very few results are known in this filed. In this context, collision avoidance strategies, which have to be superimposed on all coordinated operations, become a basic requirement. An obvious approach to avoid collisions is the generation of secure paths before the start of the movements. The basis for this can be methods like the Configuration Space or Potential Field Approach as introduced by Lumelsky (1987) and Khatib (1986). These are computed offline and are able to generate optimized trajectories. But they are insufficient if the environment is changing unpredictibly or it is not completely known. In this cases a method for online collision avoidance has to be implemented , too.
If
Robot k
Fig. 1. System with two robots
The algorithm presented is based on the StanfordArm with the degrees of freedom rotation-translationtranslation for the first three axes. The hand including gripper and load is considered by safety factors . The goal is the coordination of a working cell with r robots. Every robot j in the cell is brought in relation to every other robot k therein (fig. 1). 'I'''i denotes the angle between the x-axis of the local coordinates system j and the connecting line a"i .
The algorithm presented is based on Borgolte (1991b) . It detects the danger of collisions in realtime and generates collision free trajectories while the agents are moving. Their dynamic behavior and constraints are considered, too. Due to the very limited computation time, the only criterion for optimization is the avoidance of collisions. To further improvement it has now been enlarged to the third dimension.
The position of the gripper/tool of robot j in relation to a"i is given by 4>~i(tn): (1) where N(a) is the mapping of a on [-~ , +~] . The corresponding coefficients for robot k are 'Pi" (t n ) and
2. THE VIRTUAL HINDRANCE ROBOT To detect and estimate the danger of collision between the robots, a so-called Virtual Hindrance Robot (VHR) is constructed . The difference between this virtual robot and the robot j, together with the rela.tive velocity between these systems, gives a valuation for the time until a collision may occur. The geometrical structure presented in this paper has the advantage to take the whole working space of robot j into account. Furthermore, a crossing of the arms - which
4>j,,(tn). 2.1 . The characteri$tic triangle With the basic denominations a characteristic triangle for the robots j and k is given. The baseline is a"i , the sides result from the prolongation of the robot arms (fig. 2) . This triangle exists then and only then ,
783
Fig. 3. Significant values in the characteristic triangle Robot
Robot k
J
2.2. Condruction of the Virtual Hindrance Robot Fig. 2. The characteristic triangle if the second axes are not parallel and none of them is aligned with the baseline. These cases are treated seperately. The region bounded by the third axis of robot j and the straight line through the origin on robot k parallel to it is the common working space of the robots j and k with respect to robot j. That part of the third axis of robot k which projects into this region is nearest to the third axis of robot j. It is therefore part of the charcteristic triangle (fig. 2). Assume the configuration in fig. 2. Due to the shortage of pla.ce the construction of the VHR in this paper is done with this example. The deta.iled description of the geometric considerations is in Hoyer (1993). With this, the angle between the baseline akj and that part of the third axis of robot j which is part of the characteristic triangle is given by
q,k)(t n ) = J.I [q,~j(tn) -11"]
(2)
The translational coefficient of the third axis of robot j within the characteristic triangle is described by
1 .( ) _ {rj(tn) if q,~j(tn) = q,kj(t n ) I . rj - rj(t n ) otherwlse
rk) tn -
To compute the translation rkj(t n ) and the rotation tPkj(t n ) of the VHR, the shortest distance between the third axes of the robots j and k has to be determinated. Furthermore, the distance dkj(t n ) between the third axes of the robots, the distances ekj(t n ) resp. ejk(t n ) between the TCP of one arm and the ba.ckpoint of the other, as well as the perpendicular lines ljk(t n ) resp. lkj(t n ) from the end of one arm to the other one (fig. 3) must be taken into account. The end of an arm means the final point on the third axis which is part of the characteristic triangle. Backpoint means the opposite final point. So, with the exemplary configuration shown in fig. 2, dkj(t n ) is given by dkj(t n)
= COS
[r~j(tn),e~j(tn), q,~j(tn) - Xkj(t n)]
(7) (3)
where r} ist the total length of the third axis. The lateral length rkj(t n ) of the triangle is given by
• Isin[q,ik(t n )lI rkj(t n ) = akj Sin . [ ( t n )) CXk)
The VHR has the same geometry as the robots j and k. The first axis (not the angle) is identical to the first axis of the robot j. The position of its (virtual) TCP depends on the shortest distance between the third axes projected on the xy-plane, the distance between the second axes of the robots, and the shortest distance from the origin of robot j to the third axis of robot k.
(4)
with
CXkj(tn) = 11" -1q,jk(tn)I-Iq,:j(tn)1 (5) The angle CXkj(tn) designates that angle of the characteristic triangle which is opposite of akj. The difference between the lateral length rkj{tn) and the translational value r~j{tn) is denominated by rZi(t n ) as follows:
(6)
The function COS( a, b, 'P) gives the third side of a triangle with the sides a and b and the angle 'P between them. The shortest distance can also be the perpendicular line from the end of one arm to the third axis of the other robot. If the chara.cteristic triangle can be constructed and the lateral length rik(t n ) is greater than the translational coefficient rjk(t n ), i.e. ri;(t n ) > 0, then lkj(t n ) is given by
lkj{tn)
= e~j(tn) Isin[q,kj(t
n) -
Xkj(tn))1
(8)
ljk(t n ) is set up like lkj(t n ) with the indices reversed. The perpendicular lines are considered only if they are dropped on the arms and not just on the straight lines defined by them (like ljk(t n ) in fig. 3).
This coefficient is negative if the translational value is greater than the corresponding lateral length.
2.3. The First Axis - Rotation
The parameters q,ik(tn), rjk(tn), rik(tn) and ri;(tn) are set up like q,kj(t n ), r~j(tn), rkj(t n ) and rki(t n ) from eqs. (2) - (6) with the indices reversed. If both rki (t n ) and ri; (tn) are less than zero, the third axes are crossing ea.ch other. If the third axes are parallel or one is aligned with akj, the characteristic triangle does not exist. This special case is treated separately.
The rotation tPkj(t n ) of the Virtual Hindrance Robot (i.e. the first axis) is oriented to that point on the third axis of robot k, which has the shortest distance to robot j. This can be the end of the arm of robot k resp. its ba.ckpoint or the intersection of ljk(t n ) with the third axis of robot k. The last case is considered only if Ijk(t n ) is dropped on the arm and not just on
784
the straight line defined by it. The second axes are not taken into account. If the third axes are intersecting, the crossing point is of interest. If the third axes of the robots are para.llel, akj is choosen for the rotation of the VHR.
VHR need to get into the same position. With this, the dynamic behaviour of the systems is included, too. Like it was done by Borgolte (1991 b) for the first and third axis of robot j, the danger of collision for the second axis is given by TZkj(tn): tn $ z) Il!.Zz,"'.·.("t")ll·f lAUZkj ()I I otherwise I
The angle Xkj(t n ) from eq. (10), is used in two cases. First, if the third axis of robot j is aligned with akj and ljk(t n ) is not dropped on the third axis of robot k. Second , if one of dkj(t n ) , ekj(t n ) or lkj(t n ) denotes the shortest distance between the arms of robot j and k. This means for the configuration of fig. 2:
l!.Z.i ( t") l!.z.i(t,,)
with
with Xkj(t n ) as the angle between akj and the line from the origin of robot j to the end of the arm of robot k , which is denoted as e~)(tn) :
= -IIjk(t n ),x Hj(tn) , akj, rjk(tn)j
(10)
The function ,x( a , b, c ) gives the angle opposite to side c in the triangle a , b, c. IIjk(t n ) is the sign of
Tkj(tn)
(16)
Zj(t n )
(17)
=[
T
1
(18)
where T
2.4. The Second Axi., - Translation The coefficient of the second axis of the VHR is that of the robot k :
4. COLLISION AVOIDANCE The goal of the algorithm is the generation of desired values for a.ll axes which let the arm move without collisions. This is done with regard to the position and velocity as well as the dynamic behaviour of each axis. If there is no danger of collision, the original trajectory given by the pathplanning module has to be ·driven. Therefore the deviations are as sma.ll as possible.
(12) 2.5. The Third Axis - Translation The translational coefficient rkj(t n ) denotes the position of the third axis of the VHR. It is defined as the minimal distance between the third axis of robot k and the origin of robot j (which is identical to the origin of the VHR): rk(t n ) ,
Zj(tn)
So the vector for the danger of collision is given as
(11 )
= COS[ akj ,
(15)
This estimation is independent of the actual states of the other axes. The constant z'jaz is given by the motor and gearing of the axis. The danger of collision decreases as the distance increases and the relative velocity slows down. The usage of z'jGZ resp. ~Zkj(tn) limits z'jaz to an upper bound .
The parameter e~j(tn) is given by
rkj(t n )
= Zkj(t n ) ~Zkj(tn) = Zkj(t n ) -
LlZkj(tn)
(9)
Xkj(t n )
· mGZ
fir
The strategy has to consider the actual values for the danger of collision of a.ll axes. This is due to the fact that even if one axis is critical the whole arm may not be in danger (e.g. if the arms are crossing in the (x,y)-plane but they are far enough in the zdirection) . After leaving a dangerous situation the original trajectory will be reentered as fast as possible.
(13)
This computation of the VHR belongs to the configuration in fig. 2. The general case is described by Hoyer (1993).
The first choice for avoiding collisions is the second axis (z) . An alteration of the movement of this axis is able to let the
2. 6. Th e state-spa ce vector of th e VHR The position values for the first three axes of the VHR, together with their velocities, are describing the state of the VH R. With this the state-space vector is given as
(14)
The algorithm presented deals only with wire-models of the robot arms. The real volumes of the arms are covered by variable safety factors for each axis. This has the advantage to minimize the computational effort and a.llows to adjust the safety distance dynamica.lly as needed . The values 6€'jaz , with € E {fP , z,r} are including the volume of the arm as well as the tool
3. THE PARAMETERS FOR THE DANGER OF COLLISION The actual danger of collision is measured by a vector T, which describes for every degree of freedom the time that the corresponding axes of robot j and the
785
resp. gripper tance for one for the other distance may
and the VHR. It is given by
with workpiece. The actual saiety disaxis depends on the danger of collision axes. If this danger is low, the saiety be reduced. With
(19)
else, if
IAZkj(tn)1 ::; 5z;naz ] [ /\ IA'Pkj(tn)l::; 5'Pja%
5° 5rja% Ckj (t n ) else, if [/\
(20)
5° 5rja% Akj(t n ) a feasible transition between the minimal and the maximal value of 5Zkj(tn) can look like
else, if [/\
5°5rjaz Akj(tn)Ckj(t n ) otherwise (24) To compensate the effect of the control error, the safety distances can be enlarged by a factor 5° and the saiety regions by a factor 5'. This is due to the behaviour of the axis control. If the control error is compensated, 5° and 5' should be 1. All the safety distances 5ekj(tn), with e E {'P,z,r} are taking care of the real volume of the robot plus an additional space to move without collision.
else, if
5°5z;naZCkj(tn) else, if
Arkj(tn)::; 0
5°5z;naz Bkj(tn)Ckj(t n ) else, if
IA'Pkj(tn)1 > 5'Pjaz
4.1. Strategy for the axu
colli~ion
avoidance of the
~econd
To avoid a collision by moving the axis z, a new desired value for it has to be computed. This is done with regard to the vector for the danger of collision and to the dynmic behaviour of the robot. First, three intermediate values are generated.
(21) The safety distance 5Zkj(tn) reaches its maximum if r~j (t n ) is greater than the corresponding value r kj (t n ) and at the same time there is a danger in the rotational axis. It is at its minimum if either the angular distance is more than 5' times the secure distance 5'Pjaz, or the translational distance is more than 5' the secure distance 5rja%. Otherwise it is interpolated with a cosine function. The safety distance 5'Pkj(t n ) for the first axis is defined analogous to 5Zkj(tn) :
The first of them prevents the conformity of the axis Z of the real robot and the corresponding axis of the Virtual Hindrance Robot. The second one keeps the calculated variable distance between the real robot and the VHR. The third value ensures the maximal safety distance. Four auxiliary values have to be determinated first: 1. T~j(tn) is that Tekj(tn) with e E {'P,r}, which
indicates the higher danger of collision. else, if
0]
Arkj(tn) < [ /\ IAZkj(tn)1 < 5z;naz
T~j(tn)
5°5'Pja% Akj(t n ) else, if
Arkj(tn)
IAZkj(tn)1
T'Pkj(tn)
if T'Pkj(t n )::; -Trkj(tn)
0
else, if - Trkj(tn) < 0
-Trkj(tn) otherwise
(25) 2. (kj(t n ) holds information about the relative position of axis Z with respect to the corresponding axis of the VHR. If the third axis of robot j is above the third one of the VHR, it is less, otherwise greater than zero:
<0
5°S'Pjaz Akj(tn)Bkj(t n ) else, if
={
> 5zja%
5° 5'Pjaz Bkj(t n ) otherwise (22)
(26)
with
3. 'Ikj(t n ) describes whether the robot j and the VHR are getting closer in the z-dimension (-1) or they are departing (+1):
if sign[~Zkj(tn)] = (kj(t n ) I 'Ikj(t n ) := { -1 otherwise
The safety distance 5rkj(tn) depends on the differences in the first and the second axes of the real robot
(27)
786
4. tkj(t n ) is the time needed for the second axis
.Z.t
of robot j to get into a safe distance from the second axis of the YHR:
ztj(tn)
Alztj(tn) - Zkj(tn)1 > I~Zkj(tn}1
Zkj(t n )
Zkj(t n) + Zkj(tn)T~j(tn)
+
else, if
(kj(tn) [Zj(tn} - Zkj(t n }] [ V I~Zkj(tn)1 < C5zkj(tn) [ A .:izkj(t n ) = 0
[r,kj(tn)Z~j(tn) - z}(tn)] (kj(tn)
2znorn
if
ATrkj(tn) ~ -2t kj (t n ) ATIPkj(t n ) $ 2t kj (t n )
The first intermediate value Zkj(t n ) is the normal one. It holds the second axis of the robot in a secure distance to the YHR. This is independent of the movements in the (IP,r)-plane.
=
follows:
Trkj(tn) > Trkj(tn-d ] [ VTIPkj(tn) < TIPkj(tn-d
(28)
Zkj(t n)
aB
(29)
J
-~t
.:iZkj(tn}
[Zkj(tn) - Zj(tn)] (kj(tn)
V
-6Zkj (tn)(kj (t n ) The first term in (29) describes the expected zposition of the YHR at that time when the faster one of the IP~ resp. r-axis is coincident with the corresponding axis. The second term is the distance to be driven at most until the robot j reaches the velocity of the YHR. The third term is the way driven by the second axis during the time interval ~t . With the last term a safety distance is added. From this the normal intermediate value Zkj(t n ) describes that z-position of robot j which is characterized by: • it can be reached in the time T~j(tn), even if only the nominal acceleration can be achieved; • the second axis of robot j holds the same velocity as the corresponding axis of the YHR; • a secure distance is kept. In some cases, this can include a brake of the movement and an acceleration in the opposite direction of the second axis. The second intermediate value Zkj(t n ) keeps the zaxis of robot j in the calculated variable distance from the YHR. The corresponding second axes are moving with the same velocity and the safety distance
6Zkj(t n ): Zkj(t n ) = Zkj(t n ) + Zkj(tn)~t - (kj(tn)6Zkj(tn) (30) In (30) , Zkj(t n ) + Zkj(tn)~tis the position of the second axis of the YHR in the following time step. According to the relative position the safety distance is added resp. subtracted. The last intermediate value ztj(tn) ensures the maximal safety distance between the second axis of the robot j and the second one of the YHR:
ztj(t n ) = Zkj(t n ) - (kj(t n )6°6zjG%
(31)
Now it must be decided which of the above derived intermediate values is used for collision avoidance. Criteria are the actual danger of collision and the strategy used at the previous time tn-l . 4.2. Deci$ion of the new de$ired vector If a danger of collision exists and robot j has to avoid a collision with respect to robot k, the desired vector wJ(t n ) given by the path interpolation is overwritten by a modified vector Wkj(tn) . In the following, the conditions for modifying wJ(t n ) and for chasing one of the intermediate values are described.
[
0]
1
1
< 6Zkj(t n)
else, if
Zkj(t n )
[
f:. 0
A'1 > 0 AI~Zkj(tn)1
~
A~~to(tn} f:. 0 AI~Zkj(tn)1
1
< 6Zkj(t n)
A(kj(t n ) [Zj(t n ) - Zkj(t n )] > 0
zj(t n )
otherwise
(32) The third intermediate value ztj(t n } is used if the danger for the r- or lP-axis is growing, indicating the last chance for the z-axis to avoid, and this position is better than the current one. The second intermediate value Zkj(t n ) is used, if the distance between the second axes of robot j and the YHR is constant, but within the secure distance, or it is less than Zkj(t n }. It is used also, if the distance is growing, but still not secure. The normal intermediate value Zkj(t n ) is used if the distance between the second axes is shrinking, not secure and less than Zkj(t n ). In any other case Zkj (t n ) is set to the original desired value zj(tn). As the z-axis is the first step for collision avoidance, an examination has to be done for the other axes if Zkj(t n ) f:. zJ(t n ) . A collision avoidance for the other axis is possibly necessary, if Z is critical and the danger for Z is growing, or if all axes are critical, or if the YHR is very close to the column of robot k . The algorithms for the tp- resp. r-axis are described by Borgolte (1991a) . The axes of a real robot are limited in their movement, so if the value from (32) is out of these bounds, it must
be restricted by
zmu
.Z.t
{
~rin
if
Zkj(t n } > zjU
if
Zkj(t n } < zjin
(33)
Zkj(t n } otherwise
With the new desired value Zkj(t n } for the second axis and the values r"j(t n } and rp"j(t n ) from Borgolte (1991a), the modified desired vector Wkj(t n ) is given by
(34)
787
2.5
2.-----.---r-.-----.---r----,.----~__,
.------r---..---~--.--,
1.5
0.5 .!Il
~
>.
0
-0.5 -1
-1.5 -2
o '----'---.........- ........----'--..... o 0.5 1 1.5 2
'--.....0---1._-'--........---''--.........---'----'
-2 -1 .5 -1 -0.5 0 0.5 x-axis
1 1.5 2
Fig. 4. Programmed trajectory in the (x,y)-plane
Time [see)
Fig. 5. Distance of the
This new desired vector Wkj{t n ) makes available a collision free trajectory. The original path is altered to this new one then and only then, if, with regard to the dynamic behaviour, it is necessary to prevent a collision. The algorithm has been developed in MODULA 2 on a SPARCstation ELC. Depending on the configuration of the robots, the time needed for the calculations takes about 1 msec, which allows an online application.
2
5. SIMULATION EXAMPLE Fig. 4 shows an example of a trajectory in the projection on the {'P-r )-plane. The start positions of the arms are marked by solid lines, the end positions by dashed lines, the paths of the TCP by dotted lines. The start point of the z-axis of robot 1 is at 1.0m, that of robot 2 at 0.5m. The destination point for the z-axis of robot 1 is also at 1.0m, that of robot 2 at 1.5m. The necessary crossing of the arms would cause a collision of the arms near the akj-line (in this example the x-coordinate). The resulting trajectory with collision avoidance in the projection on the 'P-rplane looks like fig. 4, no collision avoidance is done be the first and third axis. Fig. 5 shows the distances of the arms. The safety distance of 0.2m is always kept . Fig. 6 shows the movement of the z-axis of robot 2. Robot 1 does no collision avoidance, therefore the movements are not recorded.
arIllII
o '--_-'--_........._ ........_---'-_...J o 0.5 1 1.5 2 Time [sec)
Fig. 6. Robot 2, position z(t)
Their dynamic behaviour and constraints are considered, too. In this paper, the algorithms are given for two robots in a multi-robot system, where the computational time needed, permits a real time application. The performance of this new approach is demonstrated by a simulation example. A further advantage of the approach presented is, that for more than two robots the computational effort for online collision avoidance can be kept generally constant by using parallel processing hardware. Its pratical application to robot systems is currently in action in our laboratories.
When approaching the critical area around akj , robot 2 delays its movement in z until its third axis has crossed the third one of robot 1. After that the axis is also moving to its programmed end position.
7. REFERENCES Borgolte,U. (1991a). Flexible, realzeitfahige Kolin Mehrroboter-Sy"temen. li"ion"vermeidung Springer, Berlin. Borgolte, U. (1991b). Flexible, online collision avoidance in multi-robot systems. Preprinb 3rd IFAC Sympo"ium on Robot Control, 759-764. Hoyer, H., U. Borgolte and F. Wrosch (1993). New approach for online collision avoidance in 3D-space. Proceeding" 2nd International Conference on Advanced Mechatronic", 300-305 . Khatib, O . (1986). Real-time obstacle avoidance for manipulators and mobile robots. Int. J . of Robotics Ruearch, 5, 90-98. Lumelsky, V.J. (1987). Effect of kinematics on motion planning for planar robot arms moving amidst unknown obstacles. IEEE Journal of Robotic" and Automation, Vol. RA-3, 207-223.
6. CONCLUSION Even if global planning and programming is done in multi-robot workcells, in real-world applications it is important to supervise the actual movements and to influence them if necessary. This is due to sensor driven changes of the trajectories, incomplete information of the environment and possible failures in mechanical devices. Therefore, precautions have to be taken for online collision detection and avoidance. Based on a previous published algorithm for online collision avoidance in the (x,y )-plane the method presented in this paper can do the online supervision in 3D. It detects the danger of collisions and generates collision free trajectories while the agents are moving.
788