A Collision-Avoidance Scheme

for Redundant Manipulators:

Theory and Experiments
• • • • • • • • • • • • • • • • • • • • • • • • • • • • • • •

R. V. Patel
Department of Electrical & Computer
Engineering
University of Western Ontario
London, Ontario, Canada N6A 5B9

F. Shadpey
Bombardier Aerospace
New Commercial Aircraft
2341 Boul Alfred Nobel
Montreal, Quebec, Canada H4S 2A9

F. Ranjbaran
Canadian Space Agency
St. Hubert, Quebec, Canada J3Y 8Y9

J. Angeles
Department of Mechanical Engineering
& Centre for Intelligent Machines
McGill University
Montreal, Quebec, Canada H3A 2A7

Received 2 March 2004; accepted 7 February 2005

The development of a collision- and self-collision-avoidance scheme for redundant ma-

nipulators is discussed in this paper. The method is based on modeling the arm and its
environment by simple geometric primitives 共cylinders and spheres兲. A compact method
of detecting collisions between two cylinders is introduced. By resorting to the notions
of dual angles and dual vectors for representing the axes of cylinders in space, a char-

Contract grant sponsor: Strategic Technologies in Automation and Robotics 共STEAR兲 Program of the Canadian Space Agency.
Contract grant numbers: 9F006-2-0107/01-SW and 9F006-5-0219/01-SW.
Contract grant sponsor: Natural Sciences and Engineering Research Council 共NSERC兲 of Canada.
Contract grant number: RGPIN-1345.

Journal of Robotic Systems 22(12), 737–757 (2005) © 2005 Wiley Periodicals, Inc.
Published online in Wiley InterScience (www.interscience.wiley.com). • DOI: 10.1002/rob.20096
738 • Journal of Robotic Systems—2005
acterization of different types of collisions is introduced. The performance of the pro-
posed scheme is demonstrated for a seven degrees-of-freedom redundant manipulator
via simulations and experiments. © 2005 Wiley Periodicals, Inc.
1. INTRODUCTION
Collision detection and obstacle avoidance are two
features that play an important role in fully or partly
autonomous operations of robotic manipulators in
cluttered environments. In general, the approaches
developed to date for collision avoidance can be di-
vided into two categories: off-line and online meth-
ods. Off-line path-planning methods such as Refs. 1–5
are computationally expensive and applicable at a
higher level of motion planning. They are, in general,
restricted to structured environments and static ob-
stacles. Online methods, on the other hand, are com-
putationally more efficient, can handle unstructured
environments as well as moving obstacles, and can be
used for sensor-based low-level motion planning.
In this paper, we present an online collision-
avoidance strategy for kinematically redundant ma-
nipulators based on an a collision-detection scheme
using the notion of dual vectors and angles. Kine-
matic redundancy has been recognized as a major
characteristic for operation of a manipulator in a clut-
tered environment.6 A model-based obstacle-
detection scheme for the NASA Ranger Telerobot is
described in Ref. 7. This is used in a virtual force-
based obstacle-avoidance strategy in Ref. 8. For re-
dundant as well as nonredundant manipulators, a
real-time collision-avoidance approach is presented
in Ref. 9 where the problem is formulated as a force-
control problem so that the task of collision avoidance
is solved primarily by augmenting the manipulator
control strategy. An optimization-based scheme is
given in Ref. 10 for the kinematic control of a redun-
dant manipulator operating through a window. The
goal of the optimization is to maximize the shortest
distances between the manipulator’s links and the
edges of the window. In Ref. 11, the collision-
avoidance problem for a manipulator in an environ-
ment containing multiple obstacles is formulated as a
convex quadratic optimization-based path planning
problem. Redundancy resolution uses an approach
that exploits the null space of the manipulator’s Jaco-
bian. A collision-avoidance scheme based on the
pseudo-inverse approach for redundancy resolution
at the velocity level is given in Ref. 12. The results are
applied to a 4 degrees-of-freedom 共DOF兲 planar ex-
perimental manipulator. An approach that takes into
account a redundant manipulator’s dynamic perfor-
mance while achieving obstacle avoidance is pre-
sented in Ref. 13. Measures related to a manipulator’s
shape, posture, degree of redundancy, etc., which
quantify the ability of a redundant manipulator to
perform collision avoidance have been investigated
in Ref. 14.
To implement a real-time collision-avoidance
scheme for redundant manipulators, three major ar-
eas need to be investigated: redundancy resolution,
manipulator and environment modeling, and dis-
tance calculation. Obviously, the accuracy with which
a manipulator arm and its environment are modeled
is directly related to the real-time control require-
ments. Greater detail in modeling results in higher
complexity when computing the critical distances be-
tween an obstacle and the manipulator. Much of this
computation can be avoided if the distance measure-
ments are obtained by a proximity sensing system
such as the Sensor Skin described in Ref. 15. For situ-
ations where proximity sensors are not available, a
possible solution is to use simple geometric primi-
tives to represent the arm and its environment. Col-
baugh et al.16 addressed this problem for a planar ma-
nipulator. The obstacles were represented by circles
surrounded by a surface of influence 共SOI兲, and the
links were modeled by straight lines. A redundancy-
resolution scheme was proposed to achieve obstacle
avoidance. This approach was extended to the 3-D
workspace of a 7 DOF manipulator in Refs. 17–19. In
Refs. 17 and 18, the manipulator links are represented
by spheres and cylinders and the objects by spheres.
Although this method is convenient for spherical or
bulky objects, it results in major reduction of the
workspace when dealing with slender objects. More-
over, the method is not capable of dealing with tasks
involving passing through an opening. Glass et al.19
proposed a scheme that considered an application to
remote surface inspection. This application requires
the robot to pass through circular or rectangular
openings for inspection of a space structure, such as
the International Space Station. However, they made
the restrictive assumption of having an infinite sur-
face with one opening which reduces the workspace
of the robot. For instance, this scheme does not permit
an “elbow” to back into another opening. Moreover,
the arm used in the experiment, the Robotics Re-
search Corporation 7 DOF arm 共RRC兲, is modeled as
a series of four straight lines connecting joints 1, 3,
and 5. The thickness of the links is considered via a
buffer region in the openings. This simplified model

Figure 1. The REDIESTRO manipulator.
would not be appropriate for an arm with a more
complex geometry such as the one used in the re-
search described in this paper.
A simplified geometrical model for links of in-
dustrial manipulators relevant to the study of colli-
sions either with each other or with objects in the
workspace is the cylinder. Also, the cylinder is a very
appropriate primitive for modeling many objects in
the workspace such as rods, mesh structures, open-
ings, etc., without much loss of the available work-
space.
In Section 2, we focus on the special cases of
sphere-sphere, sphere-cylinder, and cylinder-
cylinder collision detection and distance calculations.
Considering the importance of cylinder-cylinder col-
lision detection and also its complexity, a novel
method of detecting collisions between two cylinders
using the notion of dual vectors and angles is pre-
sented. To the best of our knowledge, this is the first
time that this concept has been used in a collision-
avoidance scheme for a redundant manipulator.
REDIESTRO 共see Figure 1 for a photograph兲, an
isotropic redundant research arm, was selected for
experimental verification of the collision avoidance
system. Its special architecture, resulting from kine-
matic isotropic design objectives,20 represents a chal-
lenge for any collision-avoidance scheme: It has joint
offsets, bends in the links, and actuators that are large
Patel et al.: A Collision-Avoidance Scheme • 739
in relation to the size of the links. It is felt that a suc-
cessful demonstration of the collision-avoidance
scheme on such an arm provides confidence that the
system can be developed and applied to other more
conventional 共i.e., commercial兲 7 DOF manipulator
designs. Section 3 extends the redundancy-resolution
module to the 3D workspace of REDIESTRO. It also
describes the incorporation of different additional
tasks into the redundancy-resolution module. Simu-
lation results to study the feasibility of the proposed
scheme as well as effects of different parameters are
given. Section 4 presents the experimental evaluation
of the collision-avoidance scheme using REDIESTRO.
2. PRIMITIVES-BASED COLLISION
AVOIDANCE
Collision avoidance for stationary and moving ob-
jects is achieved by introducing an appropriate con-
straint as the additional task in the configuration con-
trol scheme21,22 for redundancy resolution 共see
Appendix A兲. The idea is to model the links of the ma-
nipulator and the objects by primitives such as
spheres and cylinders. The major components of the
proposed scheme are outlined below:
• Collision detection/prediction: For those ob-
jects 共sublinks兲 that can potentially collide,
determine the critical distance hij, i.e., the dis-
tance from a critical point of the arm to that
of the object. The critical points associated
with the manipulator and the obstacles are
denoted by Pci and Pcj with position vectors pci
and pcj , respectively.
• Critical direction detection: For any pair of
critical points Pci and Pcj , determine the critical
direction, denoted by uij, a unit vector di-
rected from Pci to Pcj .
• Redundancy resolution: Formulate an addi-
tional task and use configuration control to
inhibit the motion of the point Pci towards Pcj
along uij.
2.1. Cylinder-Cylinder Collision Detection
In order to determine the relative position of two
cylinders, first the relative layout of their axes needs
to be established. The axes of the cylinders being
directed lines in three-dimensional space, we resort
to the notion of line geometry. Specifically, with the
aid of dual unit vectors 共or line vectors兲, and the
740 • Journal of Robotic Systems—2005
Figure 2. Cylinder representation, basic notation.
dual angles between skew lines, we will categorize
the relative placement of cylinders and thus deter-
mine the possibility and the nature of collisions be-
tween the two cylinders in question.
We consider each cylinder to be composed of
three parts, the cylindrical surface plus the two cir-
cular disks at the top and the bottom of the cylinder.
Four points along the axis Li of each cylinder Ci are
of interest 共see Figure 2兲, namely, Pi, Bi, Ti, and Hi.
The point Pi is any point of reference along the line.
The points Bi and Ti with position vectors bi and ti,
respectively, are the centers of the bottom and top of
the cylinder, and Hi is the foot of the common nor-
mal of the two lines Li and Lj on Li. To avoid ambi-
guity for the choice of the top and bottom of the
cylinder, we can always choose Bi and Ti in such a
way that the vector ជ BiTi points along ei, with ei being
a unit vector defining the direction of the cylinder
axis 共see Figure 2兲. Each of Bi, Ti, and Hi can alterna-
tively be defined through their line coordinates with
respect to the reference point Pi, namely,
b i = p i + b ie i, t i = p i + t ie i, h i = p i + h ie i .
It should be noted that for a given cylinder Ci, the
scalars bi and ti are known and fixed values.
2.1.1. Review of Line Geometry and Dual Vectors
A brief review of dual numbers, vectors, and their
operations, relevant to our problem is provided in
this section. A more detailed discussion can be found
in Ref. 23. A line L can be defined via the use of a
dual unit vector, also called a line vector:
ê = e + ⑀m, 共2兲
where eTe = 1, eTm = 0, and ⑀ ⫽ 0 is the dual unity
which has the property that ⑀2 = 0. Here, e defines the
direction of L, while m is the moment of L with re-
spect to a self-understood point O, namely,
m=p⫻e 共3兲
with p being the vector directed from O to an arbi-
trary point P of L. Moreover, e and m are called the
primal and dual parts of ê.
Now, let Li and Lj be two lines. Their dual angle is
defined as
v̂ij = vij + ⑀hij , 共4兲
where vij is the real angle between ei and ej, and hij is
the distance between Li and Lj. Furthermore,
sin v̂ij = sin vij + ⑀hij cos vij ,
cos v̂ij = cos vij − ⑀hij sin vij . 共5兲
Hence, the dual angle vij uniquely determines the
relative layout of the two lines Li and Lj in space. Fur-
thermore, the following relations that are in exact
analogy with real vectors can be verified:
cos vˆij = êi · êj, sin v̂ij = 共êi ⫻ êj兲 · nˆij , 共6兲
where nij is the dual vector representing the line Nij
that coincides with the common normal of Li and Lj,
and with the same direction as that of the vector from
Hi to Hj, namely n̂ij = nij + ⑀ñij, where
hj − hi
共1兲 nij = 共7兲
储hj − hi储
and ñij = nij ⫻ hi = nij ⫻ hj. Hence, Eqs. 共1兲–共7兲 uniquely
determine the dual angle vij subtended by the two
lines. Three different possibilities for the layout of
two distinct lines Li and Lj exist as explained below:
• „A… Nonparallel and Nonintersecting Lines:
vij is a proper dual number, i.e., vij ⫽ k␲, with
k = 0 , 1 and hij ⫽ 0.

Figure 3. 共A-1兲 Body-body collision 共nonparallel and
nonintersecting axes兲.
• „B… Intersecting Lines: vij is a real number 共its
dual part is zero兲, i.e., vij ⫽ k␲, with k = 0 , 1
and hij = 0.
• „C… Parallel Lines: vij is a pure dual number
共its primal part is zero兲, i.e., vij = k␲, with k
= 0 , 1 and hij ⫽ 0.
Now, for two cylinders Ci and Cj to collide, one
of the three cases discussed below must occur:
• „1… Body-Body Collision: This situation—the
most likely one—is shown in Figure 3, where
two cylindrical bodies of an object intersect.
• „2… Base-Body Collision: The cylindrical
body of one cylinder collides with one of the
two circular disks of the other cylinder.
• „3… Base-Base: One of the circular disks of one
cylinder collides with a circular disk of an-
other cylinder.
共A兲 Cylinders with Nonparallel and Nonintersecting
Axes: In order to characterize the types of possible
collisions for two cylinders whose major axes are
represented by Li and Lj that are nonparallel and
nonintersecting, the following steps are taken:
共i兲 First, we need to determine the location of the
points Hi along Li and Hj along Lj, i.e., the feet of the
common normal on the two lines. This can be done
by determining the scalars hi and hj, as given below:
共pi − pj兲共ej cos vij − ei兲
hi = , 共8兲
sin2 vij
Patel et al.: A Collision-Avoidance Scheme • 741
Figure 4. 共A-2兲 Base-body collision 共nonparallel and non-
intersecting axes兲.
共pj − pi兲共ei cos vij − ej兲
hj = , 共9兲
sin2 vij
with hi = pi + hiei and hj = pj + hjej.
共ii兲 Now, if hij ⬎ 共Ri + Rj兲, then collision is not pos-
sible.
共iii兲 If hij 艋 共Ri + Rj兲, then collision is possible, as
explained below. 共In this notation 关共A-1兲, for ex-
ample兴, the letter indicates the layout of the axes of
the two cylinders and the number indicates the type
of collision.兲
• 共A-1兲 If bi 艋 hi 艋 ti and bj 艋 hj 艋 tj, then we have
a body-body collision, and the critical points
Pci and Pcj on the axes are Hi and Hj, respec-
tively 共Figure 3兲, with the critical direction be-
ing nij.
• 共A-2兲 If only one of the points Hi or Hj lies
outside of its corresponding cylinder, then
we may or may not have a collision. How-
ever, if the two cylinders collide, then this has
to be in the form of a base-body collision only
共Figure 4兲. As an example, in order to deter-
mine the critical points and the critical direc-
tion, we assume that Hi lies inside Ci with Hj
lying outside Cj. The critical point Pcj of Cj will
thus be one of the two points Bj or Tj, which-
ever lies closer to Hj. Moreover, the critical
point Pci of the cylinder Ci is the projection of
Pcj on Li. If pcj is the position vector of Pcj , we
will have
pci = pi + 共p̃cj · ei兲ei , 共10兲
where p̃cj is the vector connecting Pcj to Pi. We
will thus consider that a collision occurs
742 • Journal of Robotic Systems—2005

Figure 6. 共A-3兲 Base-base collision 共nonparallel and non-

intersecting axes兲.
Figure 5. 共A-3兲 Near parallel axes.

whenever the following inequality is satis- Denote by 兵dk其 the set of distances of Bi and Ti to
fied: Bj and Tj, i.e.,

储pci − pcj 储 艋 共Ri + Rj兲. 共11兲 d1 = 储bi − tj储, d2 = 储bi − bj储,

It should be noted that the above inequality

gives a conservative prediction of collision d3 = 储ti − tj储, d4 = 储ti − bj储, 共13兲
between the base and the body of the two cyl-
inders. In this manner, we implicitly assume
and dc ⬅ min兵d1 , d2 , d3 , d4其; then, we have a base-base
that the base of the cylinder is not a simple
collision if dc 艋 共Ri + Rj兲. Once again, the foregoing
circular disk but a fictitious semi-sphere of
prediction is conservative, as it assumes two semi-
the same radius. The critical direction uij for
spherical base bodies attached to the ends of the cyl-
Ci becomes
inders, rather than simple circular disks.
共B兲 Cylinders with Intersecting Axes: In order to
pcj − pci characterize a collision between two cylinders with
uij = . 共12兲
储pci − pcj 储 intersecting axes, we first project the end-points Bi
and Ti of the cylinder Ci onto the line Lj and denote
Case 共A-2兲 above can lead to instability in the the projected points by Bj⬘ and Tj⬘. Conversely, we
redundancy resolution scheme if the two project the points Bj and Tj of the cylinder Cj onto
lines are almost parallel. In this special situ- the line Li and denote the projected points by Bi⬘ and
ation, the location of the critical points on the Ti⬘. The position vectors of the foregoing four points
two lines can go through major changes for will take on the form
small changes in the angle vij made by them
as shown in Figure 5. To remedy this ill- bi⬘ = pi + bi⬘ei, ti⬘ = pi + ti⬘ei ,
conditioning, we inhibit the motion of two
points of the line Li towards their corre-
sponding projections on Lj whenever the two
lines are almost parallel. This is achieved by bj⬘ = pj + bj⬘ej, tj⬘ = pj + tj⬘ej , 共14兲
identifying two critical directions, one for
each end of Ci, for the redundancy-resolution with
scheme.
• 共A-3兲 If both Hi and Hj lie outside their cor-
bi⬘ = − 共pi − bj兲 · ei, ti⬘ = − 共pi − tj兲 · ei ,
responding cylinders, then we may have a
base-base collision, and the critical points
and direction will be determined as ex-
plained below 共Figure 6兲: bj⬘ = − 共pj − bi兲 · ej, tj⬘ = − 共pj − ti兲 · ej . 共15兲
 Patel et al.: A Collision-Avoidance Scheme • 743

Figure 7. 共B-2兲 Base-body collision 共intersecting axes兲.

Figure 8. 共B-3兲 Base-base collision 共intersecting axes兲.

• 共B-1兲 The case of a body-body collision does

not arise for cylinders with intersecting axes. lines to the origin兲, then the dual representation of
• 共B-2兲 If any one of the following four condi- Nij is given as
tions holds, then we will have a base-body
collision, and the critical direction will be a
hj − hi hi ⫻ hj
unit vector pointing along a vector joining nˆij = +⑀ , 共17兲
the corresponding critical points 共Figure 7兲, hij hij

where hi and hj are the position vectors of the points

bi 艋 bi⬘ 艋 ti and 储bi⬘ − bi储 艋 共Ri + Rj兲, Hi and Hj, respectively, and hij = 储hj − hi储 is the dis-
tance between the two lines.
If hij 艌 共Ri + Rj兲, then the two cylinders do not col-
bi 艋 ti⬘ 艋 ti and 储ti⬘ − ti储 艋 共Ri + Rj兲, lide. However, if hij 艋 共Ri + Rj兲, then, depending on
the location of the cylinders along their axes relative
to each other, two special cases of body-body 共C-1兲
and base-base 共C-3兲 collisions can occur:
bj 艋 bj⬘ 艋 tj and 储bj⬘ − bj储 艋 共Ri + Rj兲,
• 共C-1兲 If hij 艋 共Ri + Rj兲, and the projection of ei-
ther Bi or Ti on Lj is between Bj and Tj, then
bj 艋 tj⬘ 艋 tj and 储tj⬘ − tj储 艋 共Ri + Rj兲. we will have a body-body collision. As in the
case of near-parallel axes mentioned in 共A-3兲,
共16兲 to avoid ill-conditioning, we specify two criti-
cal directions, one for each end of Ci 共Figure
• 共B-3兲 If none of the foregoing conditions is 5兲.
satisfied, then we will not have a base-body
collision. However, we may have a base-base
collision. The procedure for base-base colli-
sion detection for a pair of intersecting lines
is similar to that of case 共A-3兲 explained ear-
lier 共Figure 8兲.

共C兲 Cylinders with Parallel Axes: For the special

case of two parallel lines Li and Lj for which an infi-
nite number of common normals exist, we resort to a
unique definition for one common normal lying
closest to the origin23 共see Figure 9兲. If the line Nij
passes through the points Hi and Hj of Li and Lj
共with Hi and Hj being the closest points of the two Figure 9. 共C-3兲 Base-base collision 共parallel axes兲.
744 • Journal of Robotic Systems—2005
Figure 10. Cylinder-sphere collision detection.
• 共C-3兲 If 共C-1兲 is not satisfied, but hij 艋 共Ri
+ Rj兲, then we will obtain the distance be-
tween the end points of the two cylinders, as
in 共A-3兲 above 共Figure 9兲.
2.2. Cylinder-Sphere Collision Detection
This case is simpler than that of cylinder-cylinder
collision detection. Figure 10 shows the basic layout
used for collision detection of the cylinder Ci and the
sphere Sj. The notation used for the cylinder is the
same as in the previous section. The sphere Sj is
identified by the location of its center Pj and its ra-
dius. The first step is to determine whether there is a
risk of collision. The point Hi on line Li is deter-
mined by projecting the center of the sphere on Li,
hi = 共pij · ei兲ei + pi , 共18兲
where pij is defined as ជ PiPj. The critical distance hij is
given by hij = 储pj − hi储. Now, if hij ⬎ 共Ri + Rj兲, there is no
risk of collision. If hij 艋 共Ri + Rj兲, then the following
cases can occur:
• If hij 艋 共Ri + Rj兲 and Hi lies inside the cylinder
Ci, then the cylinder and the sphere are in col-
Figure 11. Sphere-sphere collision detection.
lision and the critical points and critical direc-
tion are defined by
pj − hi
uij = , 共19兲
储pj − hi储
pci = hi + Riuij , 共20兲
pcj = pj − Rjuij . 共21兲
• If hij 艋 共Ri + Rj兲 and Hi lies outside the cylinder
Ci, then we may or may not have a collision.
The critical point on the line Li is either Bi or
Ti, depending on which is closer to Hi. Let us
assume that Bi is the closer point to Hi. The
critical distance hij is given by hij = 储pj − bi储.
Now, if hij ⬎ 共Ri + Rj兲, there is no risk of colli-
sion; otherwise, there is a collision and the
critical points and direction are calculated by
replacing hi with bi in Eqs. 共19兲–共21兲. It has to
be mentioned that the foregoing inequality
gives a conservative prediction of collision
between the sphere and the cylinder. In this
manner, we are implicitly assuming that the
base of the cylinder is not a simple circular
disk, but a fictitious semi-sphere of the same
radius.
2.3. Sphere-Sphere Collision Detection
This is the simplest case among the three collision-
detection schemes presented. The critical distance hij

is the distance between the centers of the two
spheres. If hij ⬎ 共Ri + Rj兲, then there is no risk of colli-
sion; otherwise, the two spheres are in collision. 共See
Figure 11.兲
3. KINEMATIC SIMULATION FOR A 7 DOF
REDUNDANT MANIPULATOR
In this section, the collision-detection strategy de-
scribed in the previous section is incorporated in a re-
dundancy resolution scheme 共configuration
control21,22,24兲 and applied to the 7 DOF redundant
manipulator, REDIESTRO, working in a 3-D work-
space. We have chosen a singularity-robust and task-
冢 冣
cos ␪i − cos ␣i sin ␪i sin ␣i sin ␪i
i
i+1T =
sin ␪i
0
cos ␣i cos ␪i
sin ␣i
− sin ␣i cos ␪i ai sin ␪i
cos ␣i
0 0
where i = 1 – 7; ␣i, ␪i, bi, and ai are the twist angle,
joint angle, offset, and link length respectively. The
Denavit-Hartenberg parameters of REDIESTRO are
given in Appendix B. The homogenous transforma-
tion relating frame 8 共end-effector frame兲 to the base
frame is given by
1
8T = 12T 23T ¯ 78T. 共23兲
3.2. Main Task Tracking
The main task is described by the pose 共position and
orientation兲 of the end-effector, defined by the posi-
tion vector 共 1P8兲3⫻1 and the rotation matrix 共 18R兲3⫻3 of
the transformation matrix 18T. The pose is thus di-
mensionally nonhomogenous and needs different
treatment for the three-dimensional vector repre-
senting the end-effector position from the 3 ⫻ 3 rota-
tion matrix representing orientation. Therefore, the
main task is divided into two independent subtasks.
3.2.1. Position Tracking
The position is described in the workspace-fixed ref-
erence frame. Both the desired and the actual posi-
Patel et al.: A Collision-Avoidance Scheme • 745
prioritized formulation22 of configuration control us-
ing the weighted least-squares method at the velocity
level 共see Appendix A兲. However, other redundancy
resolution schemes can also be used. The feasibility of
the algorithms is illustrated using a kinematic simu-
lation.
3.1. Kinematics of REDIESTRO
The kinematic description of REDIESTRO 共Figure 1兲
is obtained by assigning a coordinate frame to each
link with its z axis along the axis of rotation. Frame
兵1其 is the workspace fixed frame and frame 兵8其 is the
end-effector frame. Two consecutive frames 兵i其 and
兵i + 1其 are related by the 4 ⫻ 4 homogenous transfor-
mation matrix,
ai cos ␪i
bi
= 冉 i
共 i+1
0T
R兲3⫻3 共 iPi+1兲3⫻1
1
冊 , 共22兲
0 1
tion are described in this frame. The ith column of
the Jacobian corresponding to the position of the
end-effector in frame 兵1其 is defined by
JPe共i兲3⫻1 = 1i Ẑ ⫻ 共 1P8origin − 1Piorigin兲, i=1–7
共24兲
where 1i Ẑ is the unit vector along the z axis of joint i,
1
P8 is the position of the end-effector, and 1Piorigin is
the position of the origin of the ith frame with re-
spect to frame 兵1其. The position and the velocity er-
rors are given by
ep = 1Pd8 − 1P8, ėp = 1Ṗd8 − JPeq̇, 共25兲
where q̇ is the vector of joint velocities, and the su-
perscript d denotes the desired values.
3.2.2. Orientation Tracking
The orientation of the end-effector is represented by
the 3 ⫻ 3 matrix 18R 共the direction cosine matrix兲. The
746 • Journal of Robotic Systems—2005

冢 冣 冢 冣
ith column of the Jacobian matrix which relates the 500 ␳ 0 ␳
angular velocity of the end-effector 共 e1␻兲 to the joint 1 d-final 1
P8 = 500 , Rd-final = 0 1 0 ,
velocity, i.e., e1␻ = JOeq̇, can be calculated from the fol- 8

lowing relation: 1102.3 −␳ 0 ␳

where ␳ = 冑2/2. It should be noted that we have split

JOe共i兲 = 1i Ẑ, i = 1 – 7. 共26兲 the main task into two independent subtasks with
weighting matrices denoted by WPe and WOe corre-
The procedure for finding the orientation error sponding to the position and orientation, respec-
and its derivative is more complicated than that for tively, of the end-effector. The joint velocities are cal-
the case of position. In this case, the desired orienta- culated 共see Appendix A兲 from
tion is described by a 3 ⫻ 3 matrix whose columns
are unit vectors coincident with the desired x, y, and q̇ = A−1b, 共28兲
z axes of the end-effector. The actual orientation of
the end-effector is given by the matrix 18R. The ori- where
entation error is calculated as follows.25 eO = 1K sin ␪,
where 1K and ␪ are the axis and angle of rotation
A = JPT WPeJPe + JO
T
WOeJOe + JcTWcJc + W␯ , 共29兲
which transform the end-effector frame to the de- e e

sired orientation. The calculation of the angular ve-

locity error is straightforward:
b = JPT WPeṖr + JO
T
WOe⍀r + JcTWcŻd . 共30兲
e e

e␻
1 d
ėo = − JOeq̇.
3.2.3. Simulation Results
The performance of redundancy resolution in track-
ing the main task trajectories is studied here by com-
puter simulation. The integration step size in the fol-
lowing simulations is 10 ms, and the main task
consists of tracking the position and orientation tra-
jectories, generated by linear interpolation between
the initial and final poses. It should be noted that
interpolation of rotations is a much more complex
problem than point interpolation in R苹. Sophisti-
cated algorithms have been proposed in the litera-
ture for this purpose,26,27 but these are not intended
for real-time implementation. For this reason, we use
simple linear interpolation for both translation and
rotation, which nevertheless leads to satisfactory re-
sults. The initial and final poses are specified below:
共27兲
The subscript c refers to the additional task
which is not active in the simulation presented in
this section. It should be noted in the following
simulations that redundancy resolution is imple-
mented in closed loop. Hence, the reference veloci-
ties are given by
Ṗr = 1Ṗd8 + Kpp共 1Pd8 − 1P8兲, 共31兲
⍀r = e1⍀d + KpoeO , 共32兲
where KpP and KpO are the position and orientation
proportional gains, respectively. In the first simula-
tion, the subtask corresponding to tracking the de-
sired orientation is inactive. Figures 12共a兲 and 12共b兲
show the position and orientation errors. In the sec-
ond simulation only the orientation subtask is active,
and the results are shown in Figures 12共c兲 and 12共d兲.
In this case, no attempt has been made to follow the
position trajectory. The position and orientation er-

冢 冣
61.8
rors are mainly due to the presence of W␯ in the
1 d-initial
P8 = 231.4 , damped least-squares formulation of the redun-
1127.1 dancy resolution.
In the following simulations, both position and
orientation subtasks are active. Figures 13共a兲–13共c兲

冢 冣
show the results of the simulation with small W␯
0.143 − 0.25 − 0.958
1
共the singularity robustness factor兲. As we can see in
Rd-initial
8 = − 0.93 0.30 − 0.22 , Figure 13共a兲, at some point, the position and orien-
0.339 0.921 − 0.19 tation subtasks are in conflict with each other. This

Figure 12. Simulation results for position and orientation tracking.
causes the whole Jacobian of the main task to ap-
proach a singular configuration where the condition
number of the Jacobian matrix is Condmax = 403.
Therefore, there is considerable error on both sub-
tasks. Figures 13共d兲–13共f兲 show the simulation result
with a larger value of W␯. This time, the whole Jaco-
bian matrix remains relatively far from singularity
共Condmax = 105兲, and the maximum errors are re-
duced significantly. However, in the case that W␯
= 20I7⫻7, there is considerable error at the end of the
trajectory. This shows that W␯ should be selected as
small as possible.
The isotropic design of REDIESTRO reduces the
risk of approaching a singular configuration over a
greater part of the workspace. However, this risk
cannot be eliminated completely, and the singularity
robustness factor W␯ should either be selected large
enough, which introduces errors in the main task, or
be time varying, with diagonal entries proportional
Patel et al.: A Collision-Avoidance Scheme • 747
to the inverse of the minimum singular value of the
“normalized” Jacobian of the main task. The Jaco-
bian matrix is normalized using the concept of char-
acteristic length28 to resolve the dimensional inhomo-
geneity in the matrix due to the presence of
positioning and orienting tasks. Figure 14 shows the
comparison between these two approaches. As one
can conclude, the variable-weight formulation
shows better performance because W␯ has small val-
ues far from a singular configuration. Hence, vari-
able weights do not introduce errors on the main
task, and grow appropriately near a singular con-
figuration. While the computational complexity of
the numerical implementation of the SVD algorithm
for a 7 DOF arm may not be acceptable for real-time
control, bounds for the singular values of J can be
found by means of bounds on the real, non-negative
eigenvalues of JJT. As shown in Ref. 29, these bounds
748 • Journal of Robotic Systems—2005
Figure 13. Simulation results when both main subtasks are active; left column W␯ = 1I3⫻3, right column W␯ = 20I3⫻3.
can be found quite economically by resorting to the
Gerschgorin theorem.30
3.2.4. Additional Tasks
The following collision avoidance tasks were incor-
porated as additional tasks in the redundancy-
resolution module: stationary and moving obstacle
collision avoidance 共SOCA, MOCA兲, and self-
collision avoidance 共SCA兲.
Stationary and Moving Obstacle Collision Avoid-
ance: A photograph of REDIESTRO, with its actual
links and actuators, is shown in Figure 1, while Fig-
ure 15 depicts a graphical rendering of the arm with
each moving element of the arm enclosed in a cylin-
drical primitive. The links and the actuator units are
modeled by 14 cylinders in total, the fourth link hav-
ing the maximum number of four sublinks. The end-
effector and the tool attached to it are enclosed in a
sphere.
The environment is modeled by spherical and
cylindrical objects. Each obstacle is enclosed in a cy-
lindrical or a spherical surface of influence 共SOI兲.
Note that the dimensions of the SOIs are used in
distance calculation, collision detection, and obstacle
avoidance modules rather than the actual dimen-
sions of the obstacles.
Additional Task Formulation: Let us assume that
after performing the distance calculation, the jth
sublink of the ith link of the manipulator—Sij or Cij
depending on the primitive used for modeling—has
the risk of collision with the kth obstacle 共Sk or Ck兲.

Figure 14. Comparison between fixed and time-varying
singularity robustness.
The critical points on the sublink and the obstacle
共Pcij and are Pck兲 and the critical direction 共uij,k兲 are
determined by the collision detection algorithm de-
scribed in Section 2. Now, the additional task zi for
the redundancy resolution module is defined by
T
zi,k = hij,k, żij,k = − uij,k 共Jcijq̇ − ṗck兲,
where hij,k is the critical distance, Jcij ⬅ ⳵pcij/⳵q is the
Jacobian matrix mapping the joint rates q̇ into the
Figure 15. REDIESTRO with simplified primitives.
Patel et al.: A Collision-Avoidance Scheme • 749
velocity of the critical point Pcij of the manipulator,
while ṗck is the velocity of the obstacle k. The desired
values for the active constraints 共additional tasks兲
are zdi = żdi = 0.
Note that we still need to calculate the Jacobian
of the active constraints. First, the Jacobian of the
critical point is calculated, i.e.,
Jcij = 共J̄3⫻i 03⫻7−i兲. 共34兲
The kth column of the matrix J̄ is given by
J̄共k兲3⫻1 = âk ⫻ 共pcij − pkorigin兲, k=1−i 共35兲
where âk is the unit vector in the direction of rotation
of the kth joint and pkorigin is the position vector of the
origin of the kth local frame. Note that all variables
are defined in frame 兵1其. Further, the Jacobian of the
additional task to be used by the redundancy-
resolution module is calculated as
T c
Jc = − uij,k Jij . 共36兲
Analysis: The performance of the obstacle avoid-
ance scheme has been studied by various simula-
tions for different scenarios. As an example, the
simulation results for MOCA are illustrated in Fig-
ure 16. In these simulations, the main task consists of
keeping the position of the end-effector constant
共33兲 while avoiding collision with a moving object. Fig-
ure 16 shows the results of the simulations for dif-
ferent constant values of the weighting matrix corre-
sponding to the collision avoidance task. It should
be noted that, when Wc is too small, the object col-
lides with the arm. When Wc is large enough, no
collision occurs, but there is a rapid increase in the
joint velocities which results in a large pulse in joint
accelerations 共see Figure 16兲. In a practical imple-
mentation, the maximum acceleration of each joint
would be limited and this commanded joint accel-
eration would result in saturation of the actuators.
The optimal value of Wc depends on factors
such as object velocity, end-effector velocity, and lo-
cation of the critical point. Therefore, from prelimi-
nary simulations, it was observed that finding a
fixed value which performs well in different situa-
tions is very difficult. To overcome this problem, a
time-varying formulation16 has been used to adjust
the weighting factor automatically. In this way, the
weighting factor corresponding to each active task is
adjusted according to the following scheme:
750 • Journal of Robotic Systems—2005
Figure 16. Simulation results for MOCA with fixed weighting factor.
Wc = k 冉 1
−
1
共dc − RO兲2 共SOI − RO兲2
冊
, 共37兲
where dc is the distance between the critical point on
the link and either the center of the object for a
spherical object or the projection of the critical point
on the axis of the cylinder in the case of a cylindrical
object. RO and SOI are the radii and surface of influ-
ence of the objects. Figure 17 shows the results of the
simulation using this formulation, which for the case
of k = 0.01 shows successful operation of MOCA,
with minimum acceleration.
4. EXPERIMENTAL EVALUATION USING A 7
DOF REDUNDANT MANIPULATOR
The main objective of these experiments is to dem-
onstrate the capability of the redundancy resolution
module in performing the main tasks 共position and
orientation tracking兲 while using the extra degrees of
freedom to fulfill additional tasks 共obstacle and joint
limit avoidance兲 for REDIESTRO. The three major
modules involved in the hardware experiment are
• the redundancy resolution module 共RR兲,
• the robot and its associated control hardware
and software, and
• the robot animation software: multi-robot
simulation 共MRS兲 system.31,32
In order to distinguish between the performance
of the robot controller and the redundancy-resolution
scheme, two separate control loops are used, one at
the Cartesian space level 共including the RR兲 and one
for the low-level joint control. In this way, the kine-
matic simulation 共including RR兲 running on an SGI
workstation generates the desired joint trajectory and
this trajectory is then transferred as the joint set
points to the VME-bus based controller to drive the
robots PID joint controller.
An obstacle-avoidance system essentially deals
with a complex environment. There are many limi-
tations in creating 共modeling兲 a robot’s environment
such as space, material, equipment, and financial
limitations. Creating a time-varying environment 共as
in the case of moving obstacles兲 can be even more dif-
ficult. One solution to this problem is online transmis-
sion of a robot’s configuration to a workstation run-
ning a graphics visualization of the arm 共MRS兲. MRS
serves as a virtual environment; the graphics model
of the robot mirrors the motions of the arm, and the
environment can also be modeled in the graphics pro-
gram. This approach has two main advantages:
• Any complex environment 共including a time-
varying environment兲 can be modeled with a
desired precision.
• The risk of damage to the robot is reduced.
4.1. Experimental Results
Three different scenarios were selected to verify the
performance of the obstacle-avoidance based
redundancy-resolution scheme in executing the fol-
lowing tasks: position tracking, orientation tracking,
stationary and moving obstacle collision avoidance,

Figure 17. MOCA simulation results for the time-varying weighting factor.
and self-collision avoidance. In each of these sce-
narios, one or multiple features were active at differ-
ent instants of execution. The sequence of steps in
each case was as follows:
1. Generate the joint trajectory using the redun-
dancy resolution and obstacle-avoidance
simulation.
2. Verify the result using MRS 共e.g., are the ob-
stacles avoided?兲.
3. Adjust parameters and repeat step 2 if nec-
essary.
4. Position the stationary obstacles in the work-
space.
5. Use the command trajectory to run the robot.
6. Record the joint history for further analysis
Remark: For demonstration purposes, the sta-
tionary obstacles were built using styrofoam and ac-
Patel et al.: A Collision-Avoidance Scheme • 751
curately positioned in the workspace. However, the
moving object used in the second scenario was not
constructed; instead, the performance of the collision
avoidance algorithm was observed using the virtual
models of the arm and the object in MRS. In order to
illustrate the experimental results of the collision
avoidance maneuver, the pose data captured from
actual experiments were fed into MRS’s 3-D graph-
ics based kinematic simulation of REDIESTRO.
Snapshots obtained from this graphics animation are
shown in Figures 18, 20, and 22.
4.2. Case 1: Collision Avoidance with Stationary
Spherical Objects
In this scenario, the end-effector was commanded to
move from its initial position to a final desired posi-
tion: There were two stationary objects to be avoided
752 • Journal of Robotic Systems—2005
Figure 18. Collision avoidance for stationary spherical
objects 共visualization of experimental results using MRS兲.
in the workspace. The orientation tracking task was
not activated in this scenario; the orientation of the
end-effector was not controlled. Figure 18 shows
snapshots of the graphics visualization of the arm
motion based on actual experimental data. We can
see that without activating the obstacle avoidance
feature 共left sequence兲, the position trajectory is fol-
lowed perfectly, but there are several collisions with
the obstacles. Figure 18 共right sequence兲 shows the
successful operation of position tracking and ob-
stacle avoidance 共visualization of hardware experi-
ment兲. As an example, the plot of the commanded
and actual joint values and rates for the first joint are
given in Figure 19共a兲–19共c兲. One can see that the ac-
tual rates follow adequately the joint set-point com-
mand, except when the joint motion is dominated by
stiction. The stiction effects also explain the position
error at the end of the trajectory. Note that the PID
controller only uses the rate information obtained by
numerically differentiating the measured joint
angles. The oscillations shown in the PID rates are
primarily due to the effect of noise and numerical
differentiation. This scenario demonstrates the capa-
bility of the redundancy resolution module to per-
form position tracking and to avoid collisions with
the obstacles.
4.3. Case 2: Collision Avoidance with a Moving
Spherical Object
In the second scenario, the end-effector was com-
manded to keep its initial position while the orien-
tation was changed. There was also a moving object
to be avoided. In order to satisfy the main task,
6 degrees of freedom are required, leaving 1 DOF for
additional tasks. Figure 20, left sequence, shows that
without any obstacle avoidance, joint-limit avoid-
ance, and self-collision avoidance provisions, only
the main task consisting of position and orientation
tracking can be successfully executed. However,
there are multiple collisions with objects and self-
collision with the base. The right sequence of Figure
20 shows that by activating different modules, both
the main and additional tasks can be performed si-
multaneously 共visualization of the hardware experi-
ment兲. Figure 21 shows the actual joint angles for
joints 2 and 3. The joints initially start moving to
realize the commanded change of orientation, but
this direction is reversed for joint 2, at 0.9 s, when
the arm starts to take evasive action to prevent a
collision. The joint 2 angle rapidly increases to a
peak value of ⬃35° at 2 s. At 2.4 s, joint 2 quickly
changes its direction to respect the imposed joint
limit 共software limit to prevent self-collision兲 of
+35°. One should note that there are more active ad-
ditional tasks than the available degrees of redun-
dancy. However, task-prioritized formulation of re-
dundancy resolution is capable of handling these
difficult situations and leads only to a graceful per-
formance degradation for the less prioritized tasks
共in this case position and orientation tracking兲.
4.4. Case 3: Passing Through a Triangular Opening
The environment was modeled by three cylindrical
objects forming a triangular opening. The end-
effector trajectory was defined as a straight line pass-
ing through this opening. Each obstacle is enclosed
in a cylindrical surface of influence 共SOI兲. The left
column in Figures 22共a兲–22共g兲 shows the motion of
the arm when the obstacle-avoidance module is not
activated. As we can see, the end-effector follows the
desired trajectory; however, there are multiple colli-
sions between the links or the actuators and the ob-

Figure 19. Experimental results: case 1 共a兲 joint 1 共degrees兲, 共b兲 derivative of the joint set-point command, and 共c兲
derivative of joint trajectory.
stacles. By activating the obstacle-avoidance module,
both the end-effector trajectory following and ob-
stacle avoidance were achieved, as can be seen in the
right column of Figures 22共h兲–22共k兲. Figure 23 shows
a sequence of photographs of REDIESTRO accom-
plishing the motion through the triangular opening
in the hardware experiment.
5. CONCLUDING REMARKS
In this paper, the problem of obstacle avoidance with
redundancy resolution was addressed. The obstacle-
avoidance algorithm was developed for 3-D objects.
A novel primitives-based collision-avoidance scheme
was introduced. This scheme is general and provides
realism, efficiency of computation, and economy in
use of the amount of free space around a redundant
manipulator. Different possible cases of collisions
were considered. In particular, the case of cylinder-
cylinder collision avoidance which represents a com-
plex case for a collision detection scheme was formal-
ized using the notion of dual vectors and dual angles.
Before performing hardware experiments using
an experimental redundant manipulator, REDI-
ESTRO, to evaluate the performance of the
Patel et al.: A Collision-Avoidance Scheme • 753
redundancy-resolution and obstacle-avoidance mod-
ule, extensive simulations were performed using the
kinematic model of the manipulator. These simula-
tions were aimed at addressing the following issues:
• Position and orientation tracking: Consider-
ing the complexity of the singular regions ex-
isting in the 3D workspace of a 7 DOF ma-
nipulator, the singularity robustness
formulation of redundancy resolution was
shown to be necessary in practical applica-
tions. It was shown that by a proper selection
共or a time-varying formulation兲 of W␯, the
weighting matrix of the singularity robust-
ness task, the effect of this term on the track-
ing performance can be minimized.
• Performing additional task„s…: Obstacle-
avoidance tasks were implemented for REDI-
ESTRO. It was shown that the formulation of
additional tasks as inequality constraints
may result in rapid changes in joint velocities
causing large pulses in joint accelerations. In
a practical implementation, since the maxi-
mum acceleration of each joint would be lim-
ited, such a commanded joint acceleration
would result in saturation of the actuators. A
754 • Journal of Robotic Systems—2005

Figure 20. Collision avoidance for moving spherical ob-

Figure 22. Passing through a triangular opening 共visual-
ject 共visualization of experimental results using MRS兲.
ization of experimental results using MRS兲.

Figure 21. Experimental results: Case 2 共a兲 joint 2 共degrees兲 and 共b兲 joint 3 共degrees兲.

Figure 23. Passing through a triangular opening
共experiment兲.
time-varying formulation of the weighting
matrix was proposed, which successfully
overcame this problem.
• Fine-tuning of control gains and weighting
matrices.
Three scenarios encompassing most of the
redundancy-resolution and obstacle-avoidance fea-
tures described in this paper were successfully dem-
onstrated on real hardware, i.e., the REDIESTRO ma-
nipulator. Despite the geometrical complexity of
REDIESTRO, the arm is entirely modeled by decom-
position of the links and attached actuators into sub-
links modeled by simple volume primitives. More-
over, due to the complex and unusual shape of
REDIESTRO, it is believed that adapting the algo-
rithms to manipulators of more conventional archi-
tecture can only be simpler.
Patel et al.: A Collision-Avoidance Scheme • 755
The current redundancy-resolution and obstacle-
avoidance scheme provides an intelligently assisted
tele-operation mode to the human operator in that
one only needs to specify the desired location and ori-
entation of the end-effector, and the system automati-
cally takes care of the details of motion control, con-
figuration selection, and generalized collision
avoidance, including self- collision avoidance, in ad-
dition to collision with objects in the workspace.
6. APPENDIX A: CONFIGURATION
CONTROL
The implementation of the collision-avoidance strat-
egy described in this paper is based on a singularity-
robust and task prioritized formulation of configura-
tion control.22
For a robot with r extra degrees of freedom, un-
der configuration control, the pose vector X is aug-
mented by the k ⫻ 1 additional task vector Z, and the
augmented 共m + k兲 ⫻ 1 task vector is defined by YT
= 关XT , ZT兴. The differential augmented kinematic re-
lations are given by
TY = 冉 冊
TX
TZ
= JAq̇, 共A1兲
where TX, TY, and TZ are the twists33 corresponding
to the vectors X, Y, and Z, respectively. The twists are
related to the time derivatives of X, Y, and Z as fol-
lows:
TX = HXẊ, TY = HYẎ, TZ = HZŻ, 共A2兲
where HX, HY, and HZ are matrices of appropriate di-
mensions 共see Ref. 33 for details兲. The augmented
Jacobian matrix JA is given by
JA = 冉冊
Je
Jc
, 共A3兲
where Je and Jc are the m ⫻ n and k ⫻ n Jacobian ma-
trices of the main and additional tasks, respectively.
Seraji and Colbaugh22 proposed a singularity-
robust and task-prioritized formulation, denoted by
configuration control, using the weighted damped
least-squares method at the velocity level. The solu-
tion is given by
756 • Journal of Robotic Systems—2005

collision avoidance for redundant manipulators, IEEE

q̇ = 关JeTWeJe + JcTWcJc + W␯兴−1关JeTWeẊd + JcTWcŻd兴, Int Symposium on Assembly and Task Planning, Au-
共A4兲 gust 1997, pp. 13–18.
6. D.G. Hunter, An overview of the Space Station Special
Purpose Dexterous Manipulator, National Research
which minimizes the cost function f given by Council Canada, NRCC No. 28817, Issue A, April 1988.
7. B. Bon and H. Seraji, Real-time model-based obstacle
detection for the NASA Ranger telerobot, IEEE Int
f= ĖeTWeĖe + ĖcTWcĖc T
+ q̇ W␯q̇, 共A5兲 Conf on Robotics and Automation, 1997, pp. 1580–
1587.
where We共m ⫻ m兲, Wc共k ⫻ k兲, and W␯共n ⫻ n兲 are diag- 8. B. Bon and H. Seraji, Online collision avoidance for the
onal, positive-definite weighting matrices that assign Ranger telerobotic flight experiment, IEEE Int Conf on
Robotics and Automation, 1996, pp. 2041–2048.
priority between the main, additional, and
9. H. Seraji and B. Bon, Real-time collision avoidance for
singularity-robustness tasks. Ee = X − Xd and Ec = Z position-controlled manipulators, IEEE Trans. Rob.
− Zd are the n- and k-dimensional vectors represent- Autom. 15:共4兲 共1999兲, 670–677.
ing the residual errors of the main and additional 10. H. Zghal, R.V. Dubey, and J.A. Euler, Collision avoid-
tasks, respectively, and Xd and Zd denote the desired ance of a multiple degree of redundancy manipulator
trajectories for the main and additional tasks. operating through a window, ASME J. Dyn. Syst.,
Meas., Control 114:共4兲 共1992兲, 717–721.
Note that there is no restriction on the dimen- 11. E. Freund, M. Schluse, and J. Rossmann, Dynamic col-
sion共s兲 of the additional task共s兲. Therefore, the joint lision avoidance for redundant multi-robot systems,
velocity 共A4兲 gives a solution that minimizes the joint IEEE/RSJ Int Conf on Intelligent Robots & Systems
velocities when k ⬍ r, i.e., there are not as many active 共IROS兲, 2001, pp. 1201–1206.
tasks as the degree of redundancy, and the best so- 12. L. Zlajpah and B. Nemec, Kinematic control algorithms
for online obstacle avoidance for redundant manipula-
lution in the least-squares sense when k ⬎ r. In all tors, IEEE/RSJ Int Conf on Intelligent Robots and Sys-
cases the presence of W␯ ensures the boundedness of tems 共IROS兲, 2002, pp. 1898–1903.
joint velocities. 13. S. Ma and S. Hirose, A dynamic approach to real-time
obstacle avoidance control of redundant manipulators,
JSME Int J Ser C 共Dyn Control, Robot, Des Manuf兲
39:共2兲 共1996兲, 317–322.
7. APPENDIX B: D-H PARAMETERS OF 14. M. Minami and M. Takahara, Avoidance manipulabil-
ity for redundant manipulators, IEEE/ASME Int Conf
REDIESTRO on Advanced Intelligent Mechatronics 共AIM兲, 2003, pp.
314–319.
i ␣i−1 共deg兲 ai−1 共mm兲 b共i兲 共mm兲 q共i兲
15. H. Seraji, R. Steele, and R. Ivlev, Sensor-based collision
1 0 0 952.29 q共1兲
avoidance: Theory and experiments, J. Rob. Syst.
2 −58.31 0 −22.91 q共2兲 13:共9兲 共1996兲, 571–586.
3 −20.0289 231.13 36.93 q共3兲 16. R. Colbaugh, H. Seraji, and K. Glass, Obstacle avoid-
4 105.26 0 0 q共4兲 ance of redundant robots using configuration control,
5 60.91 398.84 −471.59 q共5兲 Int. J. Robot. Res. 6 共1989兲, 721–744.
6 59.88 0 578.21 q共6兲 17. F. Shadpey, C. Tessier, R.V. Patel, B. Langlois, and A.
7 −75.47 135.59 −145.05 q共7兲 Robins, A trajectory planning and object avoidance
tool 0 234.44 0 0 system for kinematically redundant manipulators: An
experimental evaluation, AAS/AIAA American Astro-
dynamics Conference, August 1995, Halifax, NS,
REFERENCES Canada.
18. F. Shadpey, C. Tessier, R.V. Patel, and A. Robins, A
1. T. Lozano-Perez, Spatial planning: A configuration trajectory planning and obstacle avoidance system for
space approach, IEEE Trans. Comput. 32:共2兲 共1983兲, kinematically redundant manipulators, CASI Confer-
102–120. ence on Astronautics, Ottawa, ON, November 1994.
2. S. Borner and R.B. Kelley, A novel representation for 19. K. Glass, R. Colbaugh, D. Lim, and H. Seraji, Real-time
planning 3-D collision free paths, IEEE Trans. Syst. collision avoidance for redundant manipulators, IEEE
Man Cybern. 20:共6兲 共1990兲, 1337–1351. Trans. Rob. Autom. 11:共10兲 共1995兲, 448–457.
3. T. Hasegawa and H. Terasaki, Collision avoidance: 20. F. Ranjbaran, J. Angeles, M.A. Gonzalez-Palacios, and
Divide-and-conquer approach by space characteriza- R.V. Patel, The mechanical design of a seven-axes ma-
tion and intermediate goals, IEEE Trans. Syst. Man nipulator with kinematic isotropy, J Robot Intell Syst
Cybern. 18:共3兲 共1988兲, 337–347. 13 共1995兲, 1–21.
4. P. Khosla and R.V. Volpe, Superquadric artificial poten- 21. H. Seraji, Task options for redundancy resolution us-
tials for an obstacle avoidance approach, IEEE Int Conf ing configuration control, 30th IEEE Conference on De-
on Robotics and Automation, 1988, pp. 1778–1784. cision and Control, 1991, pp. 2793–2798.
5. Y. Bu and S. Cameron, Active motion planning and 22. H. Seraji and R. Colbaugh, Singularity-robustness and

task prioritization in configuration control of redun-
dant robots, 29th IEEE Conference on Decision and
Control, 1990, pp. 3089–3095.
23. J. Angeles, The application of dual algebra to kine-
matic analysis, in Computational methods in mechani-
cal systems, J. Angeles and E. Zakhariev 共Editors兲,
Springer-Verlag, Heidelberg, 1998, vol. 161, pp. 3–31.
24. H. Seraji, Configuration control of redundant manipu-
lators: theory and implementation, IEEE Trans. Rob.
Autom. 5 共1989兲, 472–490.
25. J.Y.S. Luh, M.W. Walker, and R.P.C. Paul, Resolved-
acceleration of mechanical manipulators, IEEE Trans.
Autom. Control AC-25:共3兲 共1980兲, 468–474.
26. K. Shoemake, Animating rotation with quaternion
curves, ACM Siggraph 19:共3兲 共1985兲, 245–254.
27. Q.J. Ge, An inverse design algorithm for a G2 interpo-
lating spline motion, in Advances in robot kinematics
and computational geometry, J. Lenarcic and B. Ravani
共Editors兲, Kluwer Academic Press, Dordrecht, 1994,
pp. 81–90.
28. M. Tandirci, J. Angeles, and F. Ranjbaran, The charac-
teristic point and the characteristic length of robotic
Patel et al.: A Collision-Avoidance Scheme • 757
manipulators, 22nd ASME Biennial Mechanisms Con-
ference, Scottsdale, AZ, September 13–16, 1992, vol. 45,
pp. 203–208.
29. C.-P. Teng and J. Angeles, A sequential-quadratic pro-
gramming algorithm using orthogonal decomposition
with Gerschgorin stabilization, ASME J. Mech. Des.
123 共2001兲, pp. 501–509.
30. R.S. Varga, Matrix iterative analysis, Springer-Verlag,
New York, 2000.
31. F. Shadpey, M. Noorhosseini, I. Bryson, and R.V. Patel,
An integrated robotic development environment for
task planning and obstacle avoidance, Third European
Conf on Eng System Design and Analysis 共ASME兲,
Montpellier, France, July 1996.
32. I. Bryson and R.V. Patel, A modular software architec-
ture for robotic simulation and visualization, 31st Int
Symp on Robotics 共ISR2000兲, Montreal, Canada, May
2000.
33. J. Angeles, Fundamentals of robotic mechanical sys-
tems: theory, methods, and algorithms, 2nd ed.,
Springer-Verlag, New York, 2002.