PROGRAMA DE DOCTORADO: Ingeniera Mecnica y de Materiales

Trabajo de Investigacin
OBTAINING ADJACENT
OBTAINING ADJACENT

CONFIGURATION WITH MINIMUM
CONFIGURATION WITH MINIMUM

T
T
IME CONSIDERING THE ROBOT
IME CONSIDERING THE ROBOT

DYNAMICS.
DYNAMICS.
AUTOR: Fares Jawad Abu Dakka
TUTOR: Francisco Valero
CURSO: 2005/2006
2
1 INTRODUCTION: ........................................................................................... 4
2 PROBLEM FORMULATION: .......................................................................... 6
3 PROBLEM CONSTRAINTS: ....................................................................... 11
3.1. KINEMATICS OF THE ROBOT: ......................................................................................................... 11
3.2. DYNAMICS OF THE ROBOT: .............................................................................................................. 12
3.3. COLLISION AVOIDANCE CONSTRAINTS: .................................................................................... 14
RESULTS ....................................................................................................... 26
4 CONCLUSION: ............................................................................................. 35
REFERENCES................................................................................................36
3
1 INTRODUCTION:
The resolution of efficient trajectory planning problem with prevention of collisions
for robots in complex environments, requires computationally costly algorithms that
prevent their industrial application.
Mainly, these algorithms act as sequential form, so that in the first place the path is
obtained and subsequently the trajectory is adjusted, remaining this seriously conditioned
by the result of the first phase where the criteria of optimality associated to dynamic
parameters cant be utilized.
The present work is oriented toward the attainment of an algorithm that permits
avoiding obtain a prior trajectory. It is based on the utilization of the concept of Adjacent
Configuration with dynamic compatibility for the subsequent application to the obtaining
of trajectories. It is proposed to work in the robot joint coordinates, and it will search to
generate new configurations starting with known one, so that the collisions avoidance can
be guaranteed and the capacity of the actuators to carry out a polynomial trajectory with
minimum time between them.
Thus, the proposed optimization problem, where the variables will correspond to
the degrees of freedom of the robot plus the time, and the restrictions are the proper of the
actuators in addition to the associated collisions avoidance that will be formulated in
Cartesian coordinates to avoid the conversion of the obstacles in joint space, will be
constructed.
So that in the present work, a generic procedure will be proposed in order to obtain
Adjacent Configurations with dynamic compatibility, that subsequently will be applied to
a robot PUMA 560 (available in the laboratory of robotic of Mechanical Engineering), on
which examples of application will be carried out in order to enunciate conclusions, and
to decide if it is adequate for the development of the procedure for the enhancement of
generation process of trajectories.
In this work, a discrete configuration space is generated between two feasible robot
configurations, an initial robot configuration and a final or goal robot configuration that
the robot should reach. The configuration space generation is based on the concept of
adjacent configuration, Valero, et al. [1] which enables to consider the generation of free-
4
collision configurations as an optimization problem. With the purpose of including the
dynamical characteristics of the robot, an optimal trajectory planning problem between
couples of adjacent configurations is established, which attempts to obtain the minimum
time trajectory between them that is compatible with the robot features. So, the motion
between adjacent configurations is characterized by the travel time.
Steffen and Samarago [2] used polynomial functions to represent the path between
two adjacent configurations in the joint space. Continuity conditions to guarantee a
smooth motion for the manipulator are used to spline lower degree polynomial together.
The path between adjacent configurations in this problem has been modelled using three
degree polynomial, where the configurations can be expressed in terms of the joint space
using the kinematic transformation equations.
Although the proposed algorithm to obtain adjacent configurations could be applied
to any robot manipulator, in this project it has been applied on an industrial robot (PUMA
560) model with six degree of freedom.
5
2 PROBLEM FORMULATION:
The robotic system has been modelled in variable of generalized coordinates and
concerned as a wired model consist of rigid links joined together by the corresponding
kinematics joints, also the robot configuration has been modelled or defined as a function
of joint variables ( )
j
i
C q , but the workspace has been modelled using Cartesian
coordinates to facilitate the definition of the whole collision avoidance process.
A set of points are called significant points ( )
j
i i
q have been modelled which are
expressed in joint coordinates. The mainly function of these points that the geometry of
the robot can be determined, so they can used to avoid collide with obstacles. Beside
these points, other interesting points
j
k
p are considered which have the same
functionality, which facilitate to formulate the obstacle avoidance problem. These points
also are obtained as a function of generalized coordinates of robot. The robot
configuration ( )
j
i
C q has been converted to the Cartesian coordinates to be
( ( ), ( ))
j j j
ik i k i
C q p q to facilitate the collision avoidance technique.

4
p
1
p
2
p
3
p
4
Figure (1). Robot wired
model
X
Y
Z
O
6
The obstacles contained in the workspace are modeled using a combination of three
basic elements: spheres, cylinders, and quadrilateral planes because they are
computationally simple. They can be defined in the space as follow:
Sphere E
k
( Qe
k
, re
k
)
Qe
k
centre
re
k
radius
Cylinder CL
k
( Qc
k1
, Qc
k2
, rc
k
)
Qc
k1
End 1
Qc
k2
End 2
rc
k
radius
Quadrilateral
plane
PR
k
( Qp
k1
, Qp
k2
, Qp
k3
, ep
k
)
Qp
k1
vertex 1
Qp
k2
vertex 2
Qp
k3
vertex 3
ep
k
height
According to Lozano-Prez and Wesley [3] a process of growing obstacles
technique has been used in order to obtain the actual dimensions of the robot.
The workspace of the robot has been modelled in Cartesian coordinates. The end
effector of the robot moved small increments in any direction of the directions of the
reference system. And this will help to generate adjacent configurations, where they
defined as: two configurations
p
C

to
k
C said to be adjacent if there is no collision and
the three following conditions are satisfied, Valero, et al. [4], Valero, et al. [5]:
1. The end effector position correspond to a point of the workspace and it is one
increment far from the point corresponding to the
k
C configuration, so it is
said that the two configuration are neighbouring and they must have between
them a given increment less than the smallest obstacle size.
2. Verification of absence of obstacles between adjacent configurations
k
C
and
p
C . Since the algorithm is working in a discrete workspace it is necessary
to verify that there are no obstacles between adjacent configurations, and also
verify that the distance between significant points meet the following condition,
( )
j
k
i
p
i
r min 2
(1)
7
where r
j
is the minimum characteristic dimensions of the obstacles used to
model the workspace.
3.
p
C should be such as to minimize the function:
( ) ( )
2
2 2 2 2
1 1 2 2 3 3
( ) ( ) ( )
i i
p k f i p k p k p k
C C A t B q q C + + + + (2)
where A,B, and C are coefficients.
The first term
2
t (3)
is to minimize the needed time to move the end effector between two adjacent
configurations.
The second term
( )
2
i i
f i
q q (4)
where are the joint coordinates and it aim to minimize the difference between
the initial configuration and the goal configuration of the robot, where q is the
joint value.
The last three terms
2 2 2
1 1 2 2 3 3
( ) ( ) ( )
p k p k p k
+ + (5)
where are expressed in Cartesian coordinates and it aim to minimize the
distance between every group of the first three significant points to achieve
the second condition of obtaining adjacent configurations.
So the configuration space is generated by means of adjacent configurations, the
configuration
k
C will be obtained as final configuration by using
p
C as initial one.
The motion of robot between two adjacent configurations (
( )
p
ip
C q
and ( )
k
ik
C q )
has been expressed in polynomial function as following:
3 2
t d t c t b a q
pk
i
pk
i
pk
i
pk
i
pk
i
+ + +
(6)
8
And obtaining the minimum time necessary
pk
t
to go from
p
C

to
k
C , satisfactory
with robot torque constraints
max
i
and
min
i
. The verification of the maximum and
minimum torque in each actuator is done by dividing the interval
pk
t
and solving the
corresponding inverse dynamic problem using Newton-Euler formulation to solve Luh-
Walker-Paul algorithm.
For each guess of
pk
t
the coefficients of the polynomial function are obtained from
the following 4 equations.
For t = 0
( )
( ) 0 0
0

pk
i
ip
pk
i
q
q q

(7)
For t = pk
t

( )
( ) 0

pk
pk
i
ik pk
pk
i
t q
q t q

(8)
Require velocity zero at the ends do not fit to the motion conditions between the
configurations
p
C and
k
C as if they were part of the pursued trajectory, but it facilitates
the comparison between the configurations that constitutes the discrete space since
common initial and goal velocity requirements are imposed.
So the coefficients of the polynomial could be determined as following:
( )
( )
( )
( )
2
3
0
3
*
2
*
i i
i
i f i
i f i
a q
b
c q q
t
d q q
t

(9)
The solution of the optimization problems is obtained by using Sequential
Quadratic Programming techniques (SQP), where both the objective function and the
constraints are nonlinear. In this problem the objective function has been modified many
times to facilitate the search technique and improve the final result. The main of this
problem is to minimize the time between two adjacent configurations, so it started with
9
objective function
} {t Min
after that this objective modified to be the summation of the
squares of time and the difference between the final and initial configuration.
( ) { }
2 2
i f
q q t Min + (10)
Finally it modified to be like this:
Objective function
( ) { }
2
3 3
2
2 2
2
1 1
2 2
) ( ) ( ) (
k
i
p
f
k
i
p
f
k
i
p
f i f
q q t Min f + + + + (11)
Where the last three parts are the distances between the significant points of two
adjacent configurations which have to be less than the minimum size of smaller obstacle.
The derivative of the objective function has been determined analytically with
respect of the generalized coordinates (
, 1,...,6 i i
q

).
10
3 PROBLEM CONSTRAINTS:
In this problem there are mainly two types of constraints; dynamical and collision
avoidance constraints, so to explain how these constraints have been built, firstly an
overview should be made concerning the kinematics and the dynamics of the robot.
3.1. KINEMATICS OF THE ROBOT:
The robot kinematics has been modelled in recursive way basing on Denavit-
Hartenberg notation, where the local reference frame { } i
is attached to link i and has its
origin laying on joint axis i. So the transform which defines frame { } i
relative to the
frame { } 1 i
has been determined, Craig [6].
Figure (1): Denavit-Hartenberg Notation
Basing on Denavit-Hartenberg the rotation matrix can be found from
( ) ( )
( ) ( ) ( ) ( ) ( )
( ) ( ) ( ) ( ) ( )
1
1 1 1
1 1 1
cos sin 0
sin cos cos cos sin
sin sin cos sin cos
i i
i
i i i i i i
i i i i i
q q
R q q
q q

1
1

1
1
]
(12)
After that the vector
1
1
,
i i
i
O O r

r
between any consecutive reference frames on the
same link can be determined, as well as the vector
,
i j
i
O O r
r
between the origins of any
11
reference frame i to another reference frame j expressed in i. So the significant points and
the interesting points can be found from the following equation
0 0
0 0 0
, , ,
i i
i
O P O O i O P
r r R r +
r r r
(13)
where
0
0
, O P
r
r
is the vector from the origin of the reference system attached to the base of
robot to the significant or interesting point located in the link i.
and P is one of the points
1
P
,
2
P
,
3
P
,
4
P
,
A
P
,
B
P
,
C
P
,
D
P
,
E
P
,
F
P
,
G
P
,
H
P
, and
FF
P
.
Figure (2): All the points needed to determine the geometry of the robot
So the analytical derivatives of these points could be obtained from
0
0
,
0 0
,
i
O P
i O P
i i
r
dP
Z r
dq q

r
r
r
(14)
where
0
i
Z
r
is a unit vector in the z-axis direction of the reference frame i expressed in the
main reference system 0, Yoshikawa [7].
So with the definitions of these points and their derivatives, it will be easy to obtain
and derive the constraints related with the collision avoidance.
3.2. DYNAMICS OF THE ROBOT:
The dynamic model of the manipulator is obtained by solving the recursive
Newton-Euler formulation to solve Luh-Walker-Paul algorithm.
X
Y
Z
O

4
P
1
P
2
P
H
P
4

3
P
3
P
G
P
FF
P
F
P
D
P
E
P
C
P
B
P
A
P
2


12
The recursive Newton-Euler formulation computes the inverse manipulator
dynamics, that is, the joint torques required for a given set of joint angles, velocities, and
accelerations. The forward recursion or outward iterations propagates kinematic
information such as angular velocities, linear and angular accelerations from the
base reference frame (inertial frame) to the end-effector. The backward recursion or
inward iterations propagates the forces and moments exerted on each link from the end-
effector of the manipulator to the base reference frame. So the torque or the force exerted
by the actuator at joint i could be obtained from, Craig [6]:

i T i
i i i
n Z (15)
where

i
i
Z is a unit vector in Z direction in frame i,
and
i
n is the torque exerted on link i by link i 1.
P
2


If >
If < 0
13
3.3. COLLISION AVOIDANCE CONSTRAINTS:
Figure (3): Design of the collision avoidance algorithm.
3.3.1. SPHERICAL OBSTACLES:
The robot links have been considered as cylispheres. Cylisphere is a cylinder with
hemispheres on each end. A cylisphere is symmetrical about its long axis. So to avoid
spherical obstacles an algorithm has been built to find the minimum distance between
sphere and cylisphere.
Firstly it should be necessary to define a set of vectors and lengths:
1 i
PC
uuuur
is the vector connected the points
1
P
and
i
C
D i
P C
uuuur
is the vector connected the points
D
P
and
i
C
1 D
PP
uuuur
is the vector connected the points
1
P
and
D
P

4
P
1
P
2


P
H
P
4
X
Y
Z
O

3
P
3
P
G
r
i
r
i
+r
j
P
FF
P
F
P
D
P
E
P
C
P
B
C
i
1
i
CL
2
i
CL
P
A
If >
If < 0
14
1
Norm
is the length of the projection of the vector
1 i
PC
uuuur
on
1 D
PP
uuuur
( ) ( )
1
1
2
2
1 1
i
D
PC
i PP
Norm PC proj (16)
1 D
PP
is the length of the vector
1 D
PP
uuuur
1 i
PC
is the length of the vector
1 i
PC
uuuur
D i
P C
is the length of the vector
D i
P C
uuuur
1
1
1 1
1
1 1
i
D
PC i D
PP D
D D
PC PP
proj PP
PP PP

uuuuuuur
uuuur
o
o
is the projection of vector
1 i
PC
uuuur
on
1 D
PP
uuuur
1
1
i
D
PC
PP
proj
is scalar projection of vector
1 i
PC
uuuur
on
1 D
PP
uuuur
Now if the
1
1
i
D
PC
PP
proj
more than
1 D
PP
then
D i
P C
should be compared with
i j
r r +
,
otherwise
1
1
i
D
PC
PP
proj
should be compared again if it has a value less than zero, in this case
1 i
PC
should be compared with
i j
r r +
, otherwise
1
Norm
should be compared with
i j
r r +
. So
the collision prevention with spherical obstacles can be achieved.
Figure (4): The flow-chart of the algorithm to find the distance between cylisphere and sphere.
The derivatives of those distances are obtained as follow:
If >
yes
If < 0
no
yes
The critical
distance is
The critical
distance is
The critical
distance is
no
15
( )
( ) ( ) ( )
1
y
x z
x x y y z z
D
D D D i
D i D i D i
D i
j j j j
dP
dP dP d P C
P C P C P C
P C
dq dq dq dq
_
_
+ +


,
,

(17)
which is the derivative of the length
D i
P C
with respect to the problem variables
(generalized coordinates of the robot q
j
), where here i index relate to the number of balls
and j relate to joint number (variable number).
( )
( ) ( ) ( )
1
1 1 1
1 1 1
1
1
y
x z
x x y y z z
i
i i i
i
j j j j
dP
dP dP d PC
P C P C P C
PC
dq dq dq dq
_
_
+ +


,
,
(18)
which is the derivative of the length
1 i
PC
with respect to the problem variables.
( ) ( )
( )
1
1
1
1
1 1
1
1
1
i
D
i
D
PC
PP
PC i
i PP
j j j
d proj
d Norm d PC
PC proj
Norm
dq dq dq
_ _
_
_




,
,
, ,
(19)
which is the derivative of the length of the projection of the vector
1 i
PC
uuuur
on
1 D
PP
uuuur
, where
( )
( ) ( )
( )
( )
( )
1
1
1 1
1
1 1 1 1 1
2
1
i
D
i D
D
D i D i D
PC
j j j
PP
j
D
d PC d PP
d PP
PP PC PP PC PP
dq dq dq
d proj
dq
PP
_
_ _ _


+



, , ,
,

uuuur uuuur
uuuur uuuur uuuur uuuur
o o o

(20)
3.3.2. CYLISPHERE OBSTACLES:
The collision avoidance algorithm between two cylispheres has been built
considering the two cylispheres as two segments. Then the minimum distance between
two segments has been determined as following.
16
Figure (5): Minimum distance derivation for two cylispheres.
In Figure (5), segment 1 and segment 2 both represent zero-radius cylispheres,
considering line 1 as the robot link for example the link defined by the points
1
P
r
,
D
P
r
as
shown in figure (3). In this example substitute the values of
1
P
r
,
D
P
r
by
1
P ,
2
P . Points
5
P
and
6
P and Segment 3 must be located. In order to represent the minimum distance,
Segment 3 must be perpendicular to both Segment 1 and Segment 2. Also,
5
P is located
on Segment 1 and Segment 3, and
6
P is located on Segment 2 and Segment 3. By
representing each segment parametrically and utilizing what is known about Segment 3
and points
5
P and
6
P , a system of equations can be solved to determine the coordinates of
points
5
P and
6
P in terms of the coordinates of points
1
P ,
2
P ,
3
P and
4
P .
Any point on Segment 1 can be represented parametrically as
( )
1 1 2 1 1
t P P P P
Line
+ (21)
( )
1 1 1 2 1
2 1 1
Line
j j j j j
dP dP dt dP dP
P P t
dq dq dq dq dq
_
+ +


,
(22)
Any point on Segment 2 can be represented parametrically as
( )
2 3 4 3 2
t P P P P
Line
+ (23)
17
2
0
Line
j
dP
dq

(24)
Although the locations of points
5
P and
6
P are unknown, any point on Segment 3
can be represented in parametric form as
( )
3 5 6 5 3
t P P P P
Line
+ (25)
( )
3 5 3 5
6 5 3
Line
j j j j
dP dP dt dP
P P t
dq dq dq dq
_
+ +


,
(26)
Since Segment 3 must be simultaneously perpendicular to Segment 1 and Segment
2,
( ) ( ) 0
5 6 1 2
P P P P (27)
and
( ) ( ) 0
5 6 3 4
P P P P (28)
Since
5
P is a point on Segment 1, and
6
P is a point on Segment 2,
5
P can be expressed as
( )
1 1 2 1 5
t P P P P + (29)
And its derivative can be determined as equation (22)
and
6
P can be expressed as
( )
2 3 4 3 6
t P P P P + (30)
And its derivative can be determined as equation (24)
In Equations (29) and (30),
1
t and
2
t represent the parameter values for
5
P and
6
P ,
respectively. Taking the difference of Equations (30) and (29) results in
( ) ( )
1 1 2 1 2 3 4 3 5 6
t P P P t P P P P P + (31)
Now, Equation (31) can be substituted into Equations (27) and (28) such that
18
( ) ( ) ( ) ( ) 0
1 1 2 1 2 3 4 3 1 2
+ t P P P t P P P P P (32)
and
( ) ( ) ( ) ( ) 0
1 1 2 1 2 3 4 3 3 4
+ t P P P t P P P P P (33)
Collecting terms and putting Equations (32) and (33) into matrix form leads to
( )
( )
1
]
1

1
]
1

1
]
1

3 1 34
3 1 12
2
1
34 34 34 12
34 12 12 12
P P P
P P P
t
t
P P P P
P P P P




(34)
or
[ ]
1
]
1

B
A
t m
(35)
where
1 2 12
P P P and
3 4 34
P P P . Solving Equation (35) via Cramers rule for t leads
to
1
1
1
1
]
1

1
]
1

B
m
P P
A
m
P P
B
m
P P
A
m
P P
t
t
12 12 34 12
34 12 34 34
2
1


(36)
Because the parametric equations for Segment 1 and Segment 2 represent any point
on segments through the given points while the cylispheres represented are constrained to
the segment segments connecting the given points, new parameters must be defined such
that:

'

>
<

1 0 ,
1 , 1
0 , 0
1 1
1
1
11
t if t
t if
t if
t
(37)
and

'

>
<

1 0 ,
1 , 1
0 , 0
2 2
2
2
22
t if t
t if
t if
t
(38)
19
Now potential coordinates for
5
P and
6
P can be calculated such that:
( )
11 1 2 1 5
t P P P P + (39)
and
( )
22 3 4 3 6
t P P P P + (40)
A potential minimum distance, d , between the two cylispheres is
6 5
P P d (41)
and the magnitude of this potential minimum distance,
d
, is
( ) ( )
6 5 6 5 6 5
P P P P P P d (42)
Figure (6): Minimum distance when
11 1
t t or
22 2
t t .
If
1
t =
11
t and
2
t =
22
t , the values calculated using Equations (41) and (42) are
correct. However, if
11 1
t t or
22 2
t t , further checks need to be done. Figure (6) shows a
sample case of when the calculated minimum distance is incorrect. In order to find the
coordinates for the desired
5
P , the algorithm for calculating the minimum distance
between a cylisphere and a sphere was used with Segment 1 representing the cylisphere
20
and point
4
P representing the sphere. Overall, if
11 1
t t or
22 2
t t , the algorithm for
calculating the minimum distance between a cylisphere and a sphere must be used with
each of the cylisphere endpoints,
1
P ,
2
P ,
3
P , and
4
P representing the sphere. Then, the
smallest of the five minimum distance magnitudes calculated is chosen along with its
respective
5
P and
6
P coordinates. Finally, the radii of the two cylispheres are subtracted
from the chosen minimum distance magnitude to get the true minimum distance
magnitude value, Harden [8].
3.3.3. QUADRILATERAL PLANE OBSTACLES:
A quadrilateral plane is a basic building block for a wide variety of shapes. It is
defined by three points
1
P ,
2
P , and
3
P , and a half thickness, t . The fourth point of the
quadrilateral plane is calculated as
4 2 3 1
P P P P + (43)
The minimum distance between quadrilateral plane and cylinder has been
calculated as the minimum distance between segment and plane as following:
Figure (7) shows the picture used to derive the minimum distance between a
cylisphere and a quadrilateral plane. This derivation process can also be used for an
infinite plane. In the figure, Segment 1 represents a zero-radius cylisphere, which is the
robot link. Points
3
P ,
4
P ,
5
P , and
6
P represent a zero-thickness quadrilateral planar
surface. Points
1
P ,
2
P ,
7
P , and
8
P are potential witness points, and Segment 2 is the
desired minimum distance line. The symbols r , s , and n represent a mutually
orthogonal set of unit vectors. Here, r and s are both located in the plane, and n is
normal to the plane. For the derivation, it is assumed that Segment 1 cannot intersect the
Planar Surface because such an intersection indicates a collision, which we are trying to
avoid.
21
Figure (7): Minimum distance derivation for a cylisphere and a quadrilateral plane.
The minimum distance between a cylisphere and an infinite plane is always the
difference between one of the cylisphere endpoints and the corresponding projection of
the same endpoint onto the surface of the plane. If the plane is a quadrilateral, then checks
must be performed to be sure the cylisphere endpoint projection is inside the quadrilateral
and that both cylisphere endpoints are on the same side of the plane.
For the example shown in Figure (7), the first step in determining the minimum
distance is to calculate the unit vectors r , s , and n . The unit vector r is calculated as
3 4
3 4

P P
P P
r

(44)
Then, n can be calculated as
3 5
3 5

P P
P P
r n

(45)
Finally, s can be calculated as
r n s (46)
22
Once these unit vectors are known, the projections of points
1
P and
2
P onto the
plane can be calculated as
( ) ( )
3 3 1 3 1 7
P s P P r P P P + + (47)
7 1 1

j j j
dP dP dP
r r s s
dq dq dq
_ _
+


, ,
o o
(48)
and
( ) ( )
3 3 2 3 2 8
P s P P r P P P + + (49)
8 2 2

j j j
dP dP dP
r r s s
dq dq dq
_ _
+


, ,
o o
(50)
A potential minimum distance magnitude,
d
, can then be calculated as
{ }
8 2 7 1
, min P P P P d
(51)
( )
( )
( )
1 7
1 7
1 7 1 7
1 7
1 7
1 7
1 7

1
x x
x x
y y
y y
x z
z z
j j
j j j
j j
dP dP
P P
dq dq
dP dP d P P
P P
dq dq dq P P
dP dP
P P
dq dq
_ _



,

_ _

, ,

_

+


,
,
(52)
( )
( )
( )
2 8
2 8
2 8 2 8
2 8
2 8
2 8
2 8

1
x x
x x
y y
y y
x z
z z
j j
j j j
j j
dP dP
P P
dq dq
dP dP d P P
P P
dq dq dq P P
dP dP
P P
dq dq
_ _



,

_ _

, ,

_

+


,
,
(53)
If the obstacle is an infinite plane, then the result in Equation (51) is correct and no
further calculation is needed. For the example shown in Figure (7), Equation (51) gives
,,,,
23
the result that
8 2
P P d
. Therefore, a check must be made to insure that the potential
obstacle witness point,
8
P , is inside the quadrilateral,
5 6 4 3
P P P P . The obstacle witness
point,
8
P , is inside the quadrilateral if all of the following equations are true,
( ) ( ) ( )
( ) ( ) ( )
( ) ( ) ( )
( ) ( ) ( ) . 0
, 0
, 0
, 0
5 8 5 3
6 8 6 5
4 8 4 6
3 8 3 4
>
>
>
>
n P P P P
n P P P P
n P P P P
n P P P P

(54)
( ) ( ) ( )
( ) ( ) ( )
( ) ( ) ( )
( ) ( ) ( )
4 3 7 3
6 4 7 4
5 6 7 6
3 5 7 5
0,
0,
0,
0.
P P P P n
P P P P n
P P P P n
P P P P n
>
>
>
>
o
o
o
o
(55)
If all of the equations (55) are satisfied the obstacle witness point,
7
P , is inside the
quadrilateral. If all of the Equations (54) are satisfied, a final check must be made to
insure that the cylisphere endpoints,
1
P and
2
P , are located on the same side of the plane.
This is the case if
( ) ( )
8 2
8 2
7 1
7 1
P P
P P
P P
P P

(56)
If any of the Equations (54) are not satisfied, then the minimum distance magnitude
calculated using Equation (51) is incorrect, and the true minimum distance must be
determined using the process for calculating the minimum distance between two
cylispheres. If Equation (56) is not satisfied, then the minimum distance magnitude
calculated may be correct, but must be compared with the four potential minimum
distances that can be calculated using the distance calculation algorithm for two
cylispheres. An illustration of this situation is shown in Figure (8). In the figure, dotted
lines represent potential minimum distances that are considered. Even though Segment 3
represents an apparently correct minimum distance, Segment 2 is the actual minimum
distance. This occurs because points
1
P and
2
P are on opposite sides of the plane.
Calculation proceeds by treating each edge of the quadrilateral plane as a cylisphere and
,,,,
24
computing four new potential minimum distances. Of all the valid potential minimum
distances, the one with the smallest magnitude is chosen along with its respective witness
points. Finally, the radius of the cylisphere and the thickness of the plane are subtracted
from the chosen minimum distance magnitude to get the true minimum distance
magnitude value, Harden [8].
Figure (8): Minimum distance when cylisphere endpoints outside quadrilateral.
Figure (9): The algorithm to find the distance between quadrilateral and cylisphere. )
,,,,
If all the
equations (54)
and (55) satisfied
{ }
8 2 7 1
, m i n P P P P d
The algorithm to find the
distance between two
cylispheres will be used
Yes No
25
RESULTS
In order to evaluate the influence of the objective function in obtaining the
geometrical solution for adjacent configurations, some examples have been solved using a
Puma 560 robot (as Application Example) in different workspaces. In order to obtain
many adjacent configurations, the workspace is modelled as a rectangular prism with its
axis parallel to the Cartesian reference system. The end points of the diagonal represent
the initial and goal position of the robot end effector, which correspond to the initial
i
C
and final
f
C robot configuration of the pursued trajectory (points
i
4
and
f
4
in figure
Error: Reference source not found).
The obtained prism is discretized according to increments (
x
D
,
y
D
,
z
D ) in the
directions of the Cartesian reference system, with the following increases (see figure
Error: Reference source not found):
Figure (11). Workspace generation

4
f

4
i

4
j

4
j+1

4
j+2

4
j+3

4
j+4

4
j+6 X
Y
Z
O

4
j+5
26
( ) ( )
x
i
N
f
N
x
D
x x
1 1 3 1 1 3 + +


( ) ( )
y
i
N
f
N
y
D
x x
2 1 3 2 1 3 + +


( ) ( )
z
i
N
f
N
z
D
x x
3 1 3 3 1 3 + +


So with workspace like this it will be easy to obtain many adjacent configurations
to be able to test the ability of the optimization process.
Many examples were applied in many cases. The first example will demonstrate the
problem without obstacles.
******************************************************************
THE FINAL CONIFIGURATION OF THE ROBOT

f
q
= 0.5000 -1.8199 3.0180 0.0000 0.0000 0.0000

f
q
= 28.6479 -104.2726 172.9187 0.0000 0.0000 0.0000
The Final Position of the End Effecter

f
4
= 0.3998 0.3883 1.3638
**********************************************************************
THE INITIAL CONFIGURATION OF THE ROBOT

i
q
= 0.0000 -1.9199 2.6180 0.0000 0.0000 0.0000

i
q
= 0.0000 -110.0022 150.0004 0.0000 0.0000 0.0000
THE INITIAL POINT OF ALFA_4

i
4
= 0.2859 0.1491 1.6261
**********************************************************************
Which are
i
4
and
f
4
the end points of the prism diagonal between the initial and final
configurations.
The success configurations are 11 out of 12 with average time t = 5.7668, in this
example.
One of these adjacent configurations can be selected and studied. For example the
first adjacent configuration
From
i
4
= 0.2859 0.1491 1.6261
To
f
4
= 0.3998 0.1491 1.6261
0.00 < 0 < 6.6; -0.49 < < 0.00
0.00 < 0 < 6.5; -1.9 < < -1.7
0.00 < 0 < 6.5; 2.6 < < < 2.6
0.00 < 0 < 6.5; -0.59 < < 0.00
0.00 < 0 < 6.5; -1.2 < < 0.00
0.00 < 0 < 6.5; -2.54e-7 < < 0.00

(
1
0
^
-
9
)
27
Which represents the next increment from the initial position in x-axis direction. As
mentioned above the robot motion between two adjacent configurations has been
expressed in polynomial functions, equation (6), so the coefficients of that polynomial
had been calculated, producing the following values:
The optimized TIME is t = 6.5838
The coefficients are:
a = 0.000000000 -1.919900000 2.618000000 0.000000000 0.000000000 0.000000000
c = -0.033751010 0.017943786 0.001078791 -0.040865542 -0.081333677 -0.000000019
d = 0.003417556 -0.001816950 -0.000109236 0.004137959 0.008235677 0.000000002
The next figure shows the problem variables; joint coordinates drawn with the time
variable which is the time of motion between the two adjacent configurations.
0.00 < 0 < 6.6; -0.49 < < 0.00
0.00 < 0 < 6.5; -1.9 < < -1.7
0.00 < 0 < 6.5; 2.6 < < < 2.6
0.00 < 0 < 6.5; -0.59 < < 0.00
0.00 < 0 < 6.5; -1.2 < < 0.00
0.00 < 0 < 6.5; -2.54e-7 < < 0.00

(
1
0
^
-
9
)
28
0.00 < 0 < 6.6; -0.49 < < 0.00
1
q
t
-0.20
2.00 4.00 6.00
0.00 < 0 < 6.5; -1.9 < < -1.7
2
q
-1.90
-1.80
-1.70
0.00 2.00 4.00 6.00
0.00 < 0 < 6.5; 2.6 < < < 2.6
3
q
t
2.62
2.63
2.63
0.00 2.00 4.00 6.00
0.00 < 0 < 6.5; -0.59 < < 0.00
4
q
-0.60
-0.40
-0.20
0.00
0.00 2.00 4.00 6.00
0.00 < 0 < 6.5; -1.2 < < 0.00
5
q
t
-1.00
-0.50
0.00
0.00 2.00 4.00 6.00
0.00 < 0 < 6.5; -2.54e-7 < < 0.00

(
1
0
^
-
9
)
-200.00
-100.00
0.00
0.00 2.00 4.00 6.00
0.00 < 0 < 6.5; -0.11 < < 0.00
0.00 < 0 < 6.5; 0.00 < < 5.91e-2
0.00 < 0 < 6.5; 0.00 < < 3.55e-3

(
1
0
^
-
3
)
0.00 < 0 < 6.5; -0.13 < < 0.00
0.00 < 0 < 6.5; -0.27 < < 0.00 0.00 < 0 < 6.5; -6.02e-8 < < 8.26e-9
-

(
1
0
^
-
9
)
29
Figure (10): The problem variables (joints of the robot) versus time of motion.
where the next figure show the joint velocity of robot drawn versus the time variable of
motion.
0.00 < 0 < 6.5; -0.11 < < 0.00
0.00 < 0 < 6.5; 0.00 < < 5.91e-2
0.00 < 0 < 6.5; 0.00 < < 3.55e-3

(
1
0
^
-
3
)
0.00 < 0 < 6.5; -0.13 < < 0.00
0.00 < 0 < 6.5; -0.27 < < 0.00 0.00 < 0 < 6.5; -6.02e-8 < < 8.26e-9
-

(
1
0
^
-
9
)
30
0.00 < 0 < 6.5; -0.11 < < 0.00
1
q

t
-0.10
-0.05
0.00
0.00 2.00 4.00 6.00
0.00 < 0 < 6.5; 0.00 < < 5.91e-2
2
q

0.00
0.02
0.04
0.06
0.00 2.00 4.00 6.00
0.00 < 0 < 6.5; 0.00 < < 3.55e-3

(
1
0
^
-
3
)
t
0.00
1.00
2.00
3.00
0.00 2.00 4.00 6.00
0.00 < 0 < 6.5; -0.13 < < 0.00
4
q

-0.10
-0.05
0.00
0.00 2.00 4.00 6.00
0.00 < 0 < 6.5; -0.27 < < 0.00
5
q

t
-0.20
-0.10
0.00
0.00 2.00 4.00 6.00
0.00 < 0 < 6.5; -6.02e-8 < < 8.26e-9

(
1
0
^
-
9
)
-60.00
-40.00
0.00
0.00 2.00 4.00 6.00
0.00 < 0 < 6.5; -6.75e-2 < < 6.67e-2 0.00 < 0 < 6.5; -3.54e-2 < < 3.59e-2

(
1
0
^
-
3
)
0.00 < 0 < 6.5; -2.13e-3 < < 2.16e-3
0.00 < 0 < 6.5; -8.17e-2 < < 8.07e-2
0.00 < 0 < 6.5; -0.16 < < 0.16 0.00 < 0 < 6.5; -3.80e-8 < < 4.05e-8
-

(
1
0
^
-
9
)
31
Figure(11): The joint velocity versus time of motion.
and finally the joint acceleration of robot drawn versus the time of motion as shown in the
next figure.
0.00 < 0 < 6.5; -6.75e-2 < < 6.67e-2 0.00 < 0 < 6.5; -3.54e-2 < < 3.59e-2

(
1
0
^
-
3
)
0.00 < 0 < 6.5; -2.13e-3 < < 2.16e-3
0.00 < 0 < 6.5; -8.17e-2 < < 8.07e-2
0.00 < 0 < 6.5; -0.16 < < 0.16 0.00 < 0 < 6.5; -3.80e-8 < < 4.05e-8
-

(
1
0
^
-
9
)
32
0.00 < 0 < 6.5; -6.75e-2 < < 6.67e-2
1
q

t
-0.05
0.00
0.05
0.00 2.00 4.00 6.00
0.00 < 0 < 6.5; -3.54e-2 < < 3.59e-2
2
q

-0.02
0.00
0.02
0.00 2.00 4.00 6.00

(
1
0
^
-
3
)
t
-2.00
0.00
2.00
0.00 2.00 4.00 6.00
0.00 < 0 < 6.5; -2.13e-3 < < 2.16e-3
0.00 < 0 < 6.5; -8.17e-2 < < 8.07e-2
4
q

-0.05
0.00
0.05
0.00 2.00 4.00 6.00
0.00 < 0 < 6.5; -0.16 < < 0.16
5
q

t
-0.10
0.00
0.10
0.00 2.00 4.00 6.00
0.00 < 0 < 6.5; -3.80e-8 < < 4.05e-8

(
1
0
^
-
9
)
-40.00
-20.00
0.00
20.00
40.00
0.00 2.00 4.00 6.00
33
Figure(12): The joint acceleration versus time of motion.
The rest of the examples are done by considering the required time T (Seconds)
between each adjacent configuration and the percentage of total number of convergence,
taking an average value of time between each couple of adjacent configuration as a
comparative reference for each example. Two examples have been solved which differ
from themselves exclusively in the number and type of obstacles contained in the
workspace. The results are shown in the table (2):
Case 1
Sphere Cylinder Plane
Two
Spheres
Tree
Sphere
s
Sphere
and
cylinder
Avg. Time of
motion (sec)
7.0945 4.3623 1.3608 9.1537 1.5220 6.6180
Avg. Time of
calculation
(sec)
12.97 39.0 16.2 14.9 15.1 7.8
Percentage
of
convergence
79.5% 58.6% 81.6% 77.1% 79.7% 65.3%
Case 2
Avg. Time of
motion (sec)
4.2704 1.9694 7.6882 1.4946 6.9266 1.6208
Avg. Time of
calculation
(sec)
9.1 10.4 12.3 11.7 17.7 17.7
Percentage
of
convergence
83% 86.8% 82.1% 85.9% 83.9% 84.9%
Table (2): The results
34
4 CONCLUSION:
In this work an algorithm had been built to obtain adjacent configurations,
considering the dynamic behaviour of robot in complex environment to facilitate the
obtaining of trajectories in the second work. The robot was defined in joint coordinates
and the obstacles were defined in Cartesian coordinates. This algorithm optimizes the
time needed to move the robot between each adjacent configuration.
So to insure the functionality of the algorithm a huge workspace had been built
between the initial and final configuration of robot. This workspace had been discretized
in prism by starting ramification from the initial position to reach the final or the goal
position. As shown in the examples demonstrated above, the average of the convergence
is reasonable, which means that the algorithm succeed to occupy the majority of
intermediate point of prism. Accordingly, this will facilitate the selection of trajectories
between the initial and final position of the end effector of the robot.
As can be observed by the results shown above in the previous table and despite the
fact that the distance between end effector positions of two adjacent configurations is
relatively small, some of the average time needed for the robot to move the end effector
between these two positions were relatively high. This can be attributed to that the
objective function of the optimization process tries to minimize the distance between the
current and the global final configuration of the robot (joint coordinates), because of this
instead of making a simple movement to achieve the intermediate goal, it makes a large
movement confirming to the objective function.
35
REFERENCES
[1] F. Valero, V. Mata, J. I. Cuadrado, and M. Ceccarelli, "A lFormulation for
Path Planning of Manipulators in Complex Environments by Using
Adjacent Configurations," Advanced Robotics, vol. 11, pp. 33-56, 1997.
[2] V. J. Steffen and S. F. P. Samarago, "Optimization Techniques for Off-
Line Trajectories Planning of Robot Manipulators," presented at ICONE--
Second International Conference on Non-linear Dynamics, Chaos,
Control and their Applications in Engineering Sciences, So Pedro, S.P.
Brazil, 1996.
[3] T. Lozano-Prez and M. A. Wesley, "An Algorithm for Planning Collision-
Free Paths among Polyhedral Obstacles," Communications of ACM, vol.
22, pp. 560-570, 1979.
[4] F. Valero, V. Mata, and A. Besa, "Trajectory Planning in Workspaces
with Obstacles Taking into Account the Dynamic Robot Behaviour,"
Mechanism and Machine Theory, 2005.
[5] F. Valero, V. Mata, and M. Ceccarelli, "Path Planning in Complex
Environments for Industrial Robots with Additional Degrees of Freedom,"
COURSES AND LECTURES- INTERNATIONAL CENTRE FOR MECHANICAL
SCIENCES, pp. 431-438, 2000.
[6] J. J. Craig, Introduction to Robotics Mechanics and Control, Second ed:
Addison-Wesley Publishing Company, 1989.
[7] T. Yoshikawa, Foundations of Robotics Analysis and Control. London: MIT
Press, 1990.
[8] T. A. Harden, "Minimum Distance Influence Coefficients for Obstacle
Avoidance in Manipulator Motion Planning," vol. Doctor of Philosophy.
Austin: The University of Texas, 2002.
36