Wei Zhang
University of Bridgeport
Bridgeport, CT 06601
This paper presents an obstacle avoidance approach for manipulators based on obstacle avoidance path planning concept. Although the obstacle dealt with in the paper is of cubic type, it can be transformed to the spherical type as showed in the paper, which has a simpler analytical description than the former one. Obstacle avoidance problem has been formulated in terms of collision avoidance of links rather than points. Link collision avoidance is achieved by continuously controlling the link's closest point to the obstacle. For the robot SIR-1 which has an articulated chain, a link can be represented as the line segment defined by the Cartesian position of its two neighboring joints. By doing so we can implement a single and simple obstacle avoidance path planning. A program which simulates this approach is also included.
1. General consideration
For the most part, we will consider motions of a manipulator
as motions of tool frame, {T}, relative to the station
frame, {S}. This is the same manner in which an eventual
user of the system would think, and designing a path description
and generation system in these terms will result in a few important
advantages.
When we specify paths as motion of the tool frame relative to
the station frame, we decouple the motion description from any
particular robot, end-effector, or workpieces. This results in
a certain modularity, and would allow the same path description
to be used with a different manipulator, or with the same manipulator
with a different tool size. Further, we can specify and plan motions
relative to a moving work station ( perhaps a conveyor belt) by
planning motions relative to the station frame as always, and
at run time causing the definition of {S} to be changing
with time.
The basic problem is to move the manipulator from an initial position
to some desired final position. That is, we wish to move the tool
frame from its current value, {Tinit}, to a
desired final value, {Tfinal}. Note that this
motion in general involves a change in orientation as well as
a change in position of the tool relative to the station.
For the purpose of obstacle avoidance, the path of the end-effector
can be further constrained by the addition of via points intermediate
to the initial and final configurations. It is necessary to specify
the motion in much more detail than simply stating the desired
final configuration. One way to include more detail in a path
description is to give a sequence of via points or intermediate
points between the initial and final positions. Thus, in completing
the motion, the tool frame must pass through a set of intermediate
positions and orientations as described by the via points. Each
of these via points is actually a frame which specifies both the
position and orientation of the tool relative to the station.
The name path points includes all the via points plus the initial
and final points. Remember that "points" mentioned here
are actually frames which give both position and orientation.
Along with these spatial constraints on the motion, the user may
also wish to specify temporal attributes of the motion. For example,
the time elapsed between via points might be specified in the
description of the path.
Usually, it is desired for the motion of the manipulator to be
smooth. For our purposes, we will define a smooth function as
one which is continuous and has a continuous first derivative.
Sometimes, a continuous second derivative is also desirable. Rough,
jerky motions tend to cause increased wear on the mechanism, and
cause vibrations by exciting resonance in the manipulator. In
order to guarantee smooth paths, we must put some sort of constraints
on the spatial and temporal qualities of the path between the
via points. At this point there are many choices that may be made,
and consequently a great variety in the ways that paths might
be specified and planned. Any smooth functions of time which pass
through the via points could be used to specify the exact path
shape.
2. Path description and generation
For path planning, we are concerned only with position of the
end-effector in spatial space, since the orientation of the end-effector
will not change the path planned.
Path generation usually includes the human interface problem of
how we wish to specify a path through space. In order to make
the description of manipulator motion easy for human user of a
robot system, the user should not be required to write down complicated
functions of space and time to specify the task. Rather, we must
allow the capability of specifying paths with simple descriptions
of the desired motion, and let the system figure out the details.
For example, the user may just specify the desired goal position
and orientation of the end-effector, and leave it to the system
to decide on the exact shape of the path to get there, the duration,
the velocity profile, and other details.
We also are concerned with how paths are represented in the computer
after they have been planned. Finally, there is a problem of actually
computing the path from the internal representation, or generating
the path. Generation occurs at run time and, in the most general
case, position, velocity, and acceleration are computed. Since
these paths are computed on digital computers, the path points
are computed at certain rate, called the path update rate. In
typical manipulator system this rate lies between 20 and 200 Hz.
3. Joint space schemes
Here we consider methods of path generation in which the path
shapes (in space and in time) are described in terms of functions
of joint angles.
Each path point is usually specified in terms of a desired position
and orientation of the tool frame, {T}, relative to the
station frame, {S}. Each of these via points is "converted"
into a set of desired joint angles by application of the inverse
kinematics. Then a smooth function is found for each of the n
joints which pass through the via points and end at the goal point.
the time required for each segment is the same for each joint
so that all joints will reach the via point. Other than specifying
the same duration for each joint, the determination of the desired
joint angle function for a particular joint does not depend on
the functions for the other joints.
Hence, joint space schemes achieve the desired position and orientation
at the via points. In between via points the shape of the path,
while rather simple in joint space, is complex if described in
Cartesian space. Joint space schemes are usually the easiest to
compute, and, because we make no continuous correspondence between
joint space and Cartesian space, there is essentially no problem
with singularities of the mechanism. 4. Inverse kinematics for SIR-1 robot manipulator
For SIR-1 robot manipulator, given the position as P={x,
y, z}thus the required first three joint variables are evaluated
as follows:
The motion of the first three joints is calculated by computing
the joint variables corresponding to
P. So we can use above formula to convert any P
in Cartesian space to its corresponding Joint space.
5. Cubic polynomials function for path smoothing
Consider the problem of moving the tool frame from its initial
position to a goal position in a certain amount of time. Using
the inverse kinematics the set of joint angles that corresponding
to the goal position and orientation can be calculated. The initial
position of the manipulator is also known in the form of a set
of joint angles. What is required is a function for each joint
whose value at is the initial position
of the joint, and whose value at is the
desired goal position of that joint. There are many smooth functions,
, which might be used to interpolate the
joint value.
In making a single smooth motion, at least four constraints onare
evident. Two constraints on the function's value come from the
selection of initial and final value:
An additional two constraints are that the function is continuous in velocity, which in this case means that the initial and final velocity are zero:
These four constraints can be satisfied by a polynomial of at least third degree. Since a cubic polynomial has four coefficients, it can be made to satisfy the four constraints given by (1) and (2). These constraints uniquely specify a particular cubic. A cubic has the form
and so the joint velocity and acceleration along this path are clearly
Combining (3) and (4) with the four desired constraints yields four equations in four unknowns:
Solving these equations for thewe obtain:
Using (6) we can calculate the cubic polynomial that connects
any initial joint angle position with any desired final position.
This solution is for the case when the joint starts and finishes
at zero velocity.
6. Cubic polynomials for a path with via points
So far we have considered motions described by a desired duration
and a final goal point. In general, we wish to allow paths to
be specified which include intermediate via points. If the manipulator
is to come to rest at each via point, then we can use the cubic
solution as before. Usually, we wish to be able to pass through
a via point without stopping, and so we need to generalize the
way in which we fit cubics to the path constraints.
As in the case of a single goal point, each via point is usually
specified in terms of a desired position and orientation of the
tool frame relative to the station frame. Each of these via points
is converted into a set of desired joint angles by application
of the inverse kinematics. We then consider the problem of computing
cubic which connect the via point values for each joint together
in a smooth way. If desired velocities of the joints at the via points are known, then we can determine cubic polynomials as before, but now the velocity constraints at each end are not zero, but rather, some known velocity. The constraints become:
The four equations describing this general cubic are
Solving these equations for thewe obtain
Using (9) we can calculate the cubic polynomial that connects
any initial and final positions with any initial and final velocities.
Another way is let the system automatically chooses reasonable
intermediate velocities using some kind of heuristic. This choice
is the result of applying a conceptually and computationally simple
heuristic. Imagine the via points connected with straight line
segments: if the slope of these lines changes sign at the via
point, choose zero velocity, if the slope of these lines does
not change sign, choose the average of the two slopes as the via
velocity. In this way, from specification of the desired via points
alone, the system can choose the velocity at each point.
7. Collision-free path planning for SIR-1 robot manipulator
Now, let us consider the obstacle avoidance, or collision-free,
path planning for SIR-1 robot manipulator.
It would be extremely convenient if we could simply tell the robot
system what the desired goal point of the manipulator motion is,
and let the system determine where and how many via points are
required so that the goal is reached without the manipulator hitting
any obstacles. In order to do this, the system must have models
of the manipulator, the work area, and all potential obstacle
in the area. A second manipulator may even be working in the same
area and hence each arm must be considered as a moving obstacle
for the other.
The obstacle we will deal with is of the cubic form. For simplifying
the problem we assume that there is only one cubic in work area,
so the problem becomes how to find a set of via points so that
the SIR-1 robot manipulator can go from initial position to final
position without hitting the cubic.
Considering the presentation of cubic in Cartesian space we noticed
that for any cubic in work area we can always find a sphere to
cover it completely, but its description in Cartesian space is
much simpler than cubics'. So we found a way to find via points:
in order to avoid hitting obstacle each via point must resides
on or above the surface of the sphere not allowing the end-effector
as well as the links of the robot to cut through the surface.
Following we will find mathematics formula by applying this rule
on them.
8. Robot obstacle avoidance: A geometric approach
The manipulator obstacle avoidance problem has been formulated
in terms of collision avoidance of links rather than points. Link
collision avoidance is achieved by continuously controlling the
link's closest point to the obstacle. Additional links can be
artificially introduced or the length of the last link can be
extended to account for the manipulator tool or load. For SIR-1
robot, a link can be represented as the line segment defined by
the Cartesian position of its two neighboring joints.
The axes of the frame of reference R are chosen such that
its z-axis is the base z-axis of manipulator and
its origin is at its bottom. The manipulator and obstacle's parameters
are designated by respectively.
First let us take a look down along the z-axis and we can get the obstacle range for, that is:
; .
To guarantee link not to hit the obstacle, D must
be greater than R, otherwise the obstacle is too large
to be avoided. The range for of the obstacle
thus can be expressed in.
Then we calculate the distances of link 2 and link 3 to the surface of the sphere. We project sphere onto the plane which is formed by line L (see figure). In this way we reduce the 3-D problem into a 2-D problem. From the figure we obtain:
These are distances of link 2 and link 3 to the surface of the sphere. Now let L2 and L3 be r, then we get the minimum as follow:
or let r be L2 or L3 we can get minimum:
So now in order to avoid link collision with an obstacle, must satisfy the above conditions as well as point avoidance conditions discussed bellow.
9. Summary and Discussion
Carrying out the above operations, we have an algorithm for a
spherical-obstacle avoidance path planning. Given initial, final
and obstacle configurations, first convert them into joint space
representations, then calculate the path with cubic polynomial
functions without obstacle, then take the obstacle into consideration:
when enters the range, that means the
manipulator already hit the object, then apply the following algorithm
to calculate the in-between path; when
goes out of the range, operation resumes. For the point avoidance it occurs when do not satisfy the conditions above, that is , if this happens following conditions must be met:
Algorithm:
10. Results and Comparisons
In order to have a general picture of the above algorithm, let's
take a look at two paths planned:
The parameters for the manipulator are: d1 = 1, a2
= 3, a3 = 2; initial and final points are:
Following are the via points generated with the algorithm discussed
above:
We can see the differences between points and joint angles with
or without obstacle. And because the algorithm's simplicity and
efficiency it might be used in real-time robot path generating
with an obstacle avoidance.
Acknowledgments
I'd like to thank Dr. Sobh for his guide and support for this
paper.
References Craig, J. "Introduction To Robotics", Addison-Wesley, 1989 Spong Vidyasagar. "Robot Dynamics and Control", Wiley 1989 Khatib, "Real time obstacle avoidance for manipulators and mobile robots", The International Journal of Robotics Research. 1984
Tomas Lozano-Perez, "A simple Motion-Planning Algorithm for
General Robot Manipulators", IEEE journal of Robotics and
Automation, 1987 |