Formation constrained multi-agent control

Abstract

We propose a model independent coordination strategy for multi-agent formation control. The main theorem states that under a bounded tracking error assumption our method stabilizes the formation error. We illustrate the usefulness of the method by applying it to rigid body constrained motions, as well as to mobile manipulation.

Full text

IEEE TRANSACTIONS ON ROBOTICS AND AUTOMATION, VOL. 17, NO. 6, DECEMBER 2001 947
[6] R. Lindemann, L. Reid, and C. Voorhees, “Mobility Sub-System for the
Exploration Technology Rover,” in Proc. 33rd Aerospace Mechanisms
Symp., 1999, pp. 115–130.
[7] E. T. Baumgartner, “In-Situ Exploration of Mars Using Rover Systems,”
in Proc. AIAA Space 2000 Conf., 2000, AIAA Paper # 2000–5062.
[8] E. T. Baumgartner, H. Aghazarian, A. Trebi-Ollennu, T. L. Huntsberger,
and M. S. Garrett, “State Estimation and Vehicle Localization for the
FIDO Rover,” in Proc. SPIE Conf. Sensor Fusion and Decentralized
Control in Autonomous Robotic Systems III, vol. 4196, Nov. 2000, pp.
329–336.
[9] R. E. Arvidson, S. Squyres, E. T. Baumgartner, L. Dorsky, and P.
Schenker, “Rover Trials for Mars Sample Return Mission Prove
Successful,” EOS Trans. Amer. Geophys. Union, vol. 81, no. 7, pp.
65–72, 2000.
[10] P. G. Backes and J. S. Norris, “Automated Rover Sequence Report Gen-
eration,” in Proc. IEEE Aerospace Conf., Big Sky, MT, March 2001.
[11] D. B. Gennery, “Camera Calibration Including Lens Distortion,” JPL,
Pasadena, CA, JPL Tech Report D-8580, 1991.
[12] R. M. Haralick, S. R. Sternberg, and X. Zhuang, “Image analysis
using mathematical morphology,” IEEE Trans. PAMI, vol. 9, no. 4, pp.
532–550, 1987.
[13] M. D. Levine, Vision in Man and Machine. New York: McGraw-Hill,
1985.
[14] D. H. Titterton and J. L. Weston, “Strapdown inertial navigation tech-
nology,” in IEE Radar, Sonar, Navigation andAvionics Series 5, E. Brad-
sell, Ed. London, U.K.: Peter Pergrinus Ltd, 1997, pp. 455–455.
[15] A. Brandt and J. F. Gardner, “Constrained Navigation Algorithms for
Strapdown Inertial Navigation Systems with Reduced Set of Sensors,”
in Proc. American Control Conf., vol. 3, 1998, pp. 1848–1852.
[16] E. Nebot and H. Durrant-Whyte, “Initial calibration and alignment of an
inertial navigation,” in Proc. Fourth Annual Conf. on Mechatronics and
Machine Vision in Practice, 1997, pp. 175–180.
[17] S. L. Moshier, “Self-contained ephemeris calculator,”, As-
tronomy and Numerical Software web site (http://people.ne.me-
diaone.net/moshier/index.html)..
[18] F. Cozman and E. Krotkov, “Robot Localization Using a Computer
Vision Sextant,” Proc. IEEE Int. Conf. on Robotics and Automation
(ICRA’95), pp. 106–111, 1995.
[19] E. T. Baumgartner, H. Aghazarian, and A. Trebi-Ollennu, “Rover Local-
ization Results for the FIDO Rover,” in Sensor Fusion and Decentral-
ized Control in Autonomous Robotic Systems IV, vol. 4571, SPIE Proc,
Newton, MA, 2001.
Formation Constrained Multi-Agent Control
Magnus Egerstedt and Xiaoming Hu
Abstract—We propose a model independent coordination strategy for
multi-agent formation control. The main theorem states that under a
bounded tracking error assumption our method stabilizes the formation
error. We illustrate the usefulness of the method by applying it to rigid
body constrained motions.
Index Terms—Coordinated control, mobile robots, stability.
I. INTRODUCTION
In the maturing field of mobile robot control, a natural extension to
the traditional trajectory tracking problem [4], [7], [9], [15] is that of
coordinated tracking. In its most general formulation,the problem is to
find a coordinated control scheme for multiple robots that make them
maintain some given, possibly time-varying, formation atthe same time
as the robots, viewed as a group, execute a given task. The possible
tasks could range from exploration of unknown environments where an
increase in numbers could potentially reduce the exploration time, nav-
igation in hostile environments where multiple robots make the system
redundant and thus robust [2], to coordinated path following [5]. The
latter of these tasks is applicable in manufacturing or construction sit-
uations where multiple robots are asked to carry or push objects in a
coordinated fashion [11], [13].
In this paper, we focus on a particular type of path following, and the
idea is to specify a reference path for a given, nonphysical point. Then
a multiple agent formation, defined with respect to the real robots as
well as to the nonphysical virtual leader, should be maintained at the
same time as the virtual leader tracks its reference trajectory.
The formation problem for multiple robots has been extensively
studied in the literature, and, for instance, in [2] a behavior-based,
decentralized control architecture is exploited, where each individual
platform makes sure that it is placed appropriately with respect to its
neighbors. In [5] and [6], the situation is slightly different and the
solution is based on letting one robot take on the role of the leader,
meaning that all of the other robots position themselves relative to
that robot. Furthermore, in [10], [16], and [18], an extensive line of
work has been conducted with the dynamic model taken into account
explicitly, while a very specific type of “string stability” is achieved
for multiple autonomous vehicles.
In contrast to this, the approach suggested in this paper is plat-
form-independent, proven successful, and general enough to support a
number of different actual controllers. The idea that we capitalize on
is that the tracking controllers can be designed independently of the
coordination scheme, which provides us with additional control power.
Manuscript received April 20, 2000; revised August 3, 2001. This paper
was recommended for publication by Associate Editor L. Kavraki and Editor
V. Lumelsky upon evaluation of the reviewers’ comments. The work of M.
Egerstedt was supported in part by the Army Research Office under Grant
DAAG 5597-1-0114, and in part by the Sweden–America Foundation 2000
Research Grant. The work of X. Hu was supported in part by the Swedish
Foundation for Strategic Research through its Centre for Autonomous Systems
at KTH, in part by TFR. This work was presented in part at ICRA, Seoul,
Korea, May 2001.
M. Egerstedt is with the School of Electrical and Computer Engi-
neering, Georgia Institute of Technology, Atlanta, GA 30332 USA (e-mail:
magnus@ece.gatech.edu).
X. Hu is with the Division of Optimization and Systems Theory, Royal Insti-
tute of Technology, Stockholm, Sweden (e-mail: hu@math.kth.se).
Publisher Item Identifier S 1042-296X(01)10916-X.
1042–296X/01$10.00 © 2001 IEEE

948 IEEE TRANSACTIONS ON ROBOTICS AND AUTOMATION, VOL. 17, NO. 6, DECEMBER 2001
The outline of this paper is as follows. In Section II we discuss the co-
ordinated tracking problem from a theoretical point of view, including
our main stability theorem. In Section III, we show how our platform
independent coordination scheme can be applied to the class of uni-
cycle robots. We then conclude in Section IV with an illustration of
how the proposed method can be used for generating rigid body mo-
tions.
II. FORMATION CONTROL
The multi-agent system that we consider in this paper is given by
m
mobile robots, each of which is governed by its own set of system
equations
_
z
i
=
f
i
(
z
i
)+
g
i
(
z
i
)
u
i
x
i
=
h
(
z
i
)
where
z
i
2
p
is the state of the system,
u
i
2
k
is the control,
and
x
i
2
n
are geometric variables used for defining the formation
in
n
. (Throughout this paper, we use boldface to distinguish vectors
from scalars.) These geometric variables can either be just the state of
the system or output projections onto some space on which one wants
the formation to evolve. The
m
robots should keep a certain relative
position and orientation, while moving along one given path, specified
for the virtual leader, e.g., the geometric center of the formation.
Definition II.1 (Formation Constraint Function): Given a continu-
ously differentiable, positive definite (
F
=0
only at one point) map
F
:
n
21112
n
!
+
.If
F
(
x
1
;
...
;
x
m
)
is strictly convex, then
we say that
F
(
x
1
;
...
;
x
m
)
is a formation constraint function. The
shape and orientation of the robot formation is uniquely determined by
(
x
1
;
...
;
x
m
)=
F
0
1
(0)
.
The formation is thus given by the kernel of a formation constraint
function, which is a mathematically appealing way of capturing the
desired formation. It is obvious that, for a given formation, the corre-
sponding formation constraint function is not unique. For example, for
a given polygon in
2
, one can choose either
F
=
m
i
=2
(
k
x
i
0
1
0
x
i
k
2
0
d
i
)
2
+(
k
x
i
k
2
0
r
i
)
2
+
k
x
1
0
a
1
k
2
or
F
=
m
i
=1
k
x
i
0
a
i
k
2
:
From an implementation point of view, the former is preferable since
the relative distance is coordinate-free and easier to measure than the
absolute position.
We, of course, want to allow for the possibility of having a moving
formation since we want the virtual leader to follow a given path. If
the desired path that we want the virtual leader to follow is given by
p
0
(
1
)
, we choose to parameterize the trajectory for the virtual leader,
x
0
2
n
,as
x
0
(
t
)=
p
0
(
s
0
(
t
))
where
s
0
2
is a function of
t
(time), and where we assume that the
trajectory is smooth, i.e.,
k
(
@
p
0
(
s
0
)
=@s
0
)
k6
=0
for all
s
0
.
The reason for calling
x
0
, together with its dynamics, a virtual leader
is because it takes on the role of the leader for the formation. Using this
terminology, our additional task is to design
m
new virtual robots for
the individual robots to follow. We are thus free to design the evolution
of these additional virtual vehicles, and we ignore the question con-
cerning how to actually track these new virtual vehicles for the time
being.
In light of the previous paragraph, it is more convenient to consider a
moving frame with coordinates centered at
x
0
. In the new coordinates
we have
~
x
=
x
0
x
0
. Let the desired trajectories (subscript
d
), or virtual
vehicles, be defined in the moving frame by
~
x
id
=~
p
i
(
s
i
)
;i
=1
;
...
;m
_
~
x
id
=
@
~
p
i
(
s
i
)
@s
i
_
s
i
where we have not yet specified what the desired trajectories should
look like. In fact,
@
~
p
i
(
s
i
)
=@s
i
and
_
s
i
2
can be designed by us, and
they should be chosen in a systematic fashion so that the formation
constraint is respected.
The solution we propose is to let the desired trajectories be given by
the steepest descent direction to the desired formation, i.e., we set
@
~
p
(
s
)
@
s
=
0r
F
(~
x
d
)
where we have grouped together the contributions from the different
robots as
r
F
(~
x
d
)
T
=
@F
(~
x
d
)
@
~
x
1
d
T
;
...
;@F
(~
x
d
)
@
~
x
md
T
~
p
(
s
)
T
=~
p
T
1
(
s
1
)
;
...
;
~
p
T
m
(
s
m
)
s
T
=(
s
1
;
...
;s
m
)
~
x
T
d
=(
x
T
1
d
0
x
T
0
;
...
;
x
T
md
0
x
T
0
)
:
The idea now is to let the evolution of the different virtual vehicles be
governed by differential equations containing error feedback in order to
make the control scheme robust. This can be viewed as a combination
of the conventional trajectory tracking, where the reference trajectory
is parameterized in time, and a dynamic path-following approach [15],
where the criterion is to stay close to the geometric path, but not nec-
essarily close to an a priori specified point at a given time.
In order to accomplish this, we define the evolution of the reference
points as
_
s
i
=
ce
0
 
;i
=1
;
...
;m
where
c; 
i
>
0
and

i
=
k
x
i
0
x
id
k
=
k
~
x
i
0
~
x
id
k
. We further-
more want the motion of
s
0
to capture how well the formation is being
respected. For this, we define

a
=
m
i
=1

i
and set
_
s
0
=
c
0
@
p
0
(
s
0
)
@s
0
e
0
 
where
c
0
;
0
>
0
.
With these designs, we have the following stability theorem.
Theorem II.1 (Coordinated Tracking and Formation Con-
trol): Under the assumption that the real robots track their respective
reference trajectories perfectly, it holds that
lim
t
!1
F
(~
x
d
)=0
:
Remark II.1: This theorem shows that we have quite some freedom
in initializing the virtual vehicles and that the algorithm is robust to
measurement noises.

IEEE TRANSACTIONS ON ROBOTICS AND AUTOMATION, VOL. 17, NO. 6, DECEMBER 2001 949
Proof:
d
dt F
(~
x
d
)=
r
F
(~
x
d
)
T
_
~
x
d
=
0
m
i
=1
@F
(~
x
d
)
@
~
x
id
2
ce
0
 
:
Now assume that we have perfect tracking, i.e.,

i
=0
,
i
=1
;
...
;m
.
This assumption, combined with the assumption that
F
is positive def-
inite and convex, implies that
(
d=dt
)
F
(~
x
d
)
is negative definite since
otherwise
F
would have a local minimum. This concludes the proof.
Corollary II.1: If all the tracking errors are bounded, i.e., it holds
that

i

<
1
,
i
=1
;
...
;m
, then
lim
t
!1
F
(~
x
d
)=0
:
The proof of this corollary is just a straightforward extension of the
proof of the previous theorem. This corollary is furthermore very useful
since one typically does not want

=0
due to the potential chattering
that such a control strategy might give rise to [7] and [8]. Instead it is
desirable to let
>
0
be the look-ahead distance at which the robots
should track their respective reference trajectories.
III. CONTROL OF MOBILE ROBOTS
In this section we shift our focus to the actual tracking of the virtual
reference points in the workspace of
2
(i.e.,
n
=2
). Our solution
to this problem is based on position and orientation error feedback.
The solution is largely model-independent because it provides only the
rotational and translational velocity controls. In other words, they are
higher-level controls. Naturally, for platforms that do not support direct
control over these velocities, one needs to be somewhat more careful
when designing the actuator controllers.
Under the assumption that we can control the rotational and transla-
tional velocities, we model the robots as unicycles of the form
_
x
=
v
cos

_
y
=
v
sin

_

=
!
where
(
x; y
)
is the center of gravity of the robot in the inertially fixed
coordinate system, and

is its orientation. The two controlled inputs
(
v; !
)
correspond to the longitudinal and angular velocities, respec-
tively.
It should be noted that we, throughout this section, choose to drop
the subscript
i
2f
1
;
...
;m
g
since we assume that all robots have
the same dynamics. The evolution of the reference points are moreover
still given by the coordination algorithm from the previous section.
Let
1
x
=
x
d
0
x
,
1
y
=
y
d
0
y
, and
1

=

d
0

, where
x
d
=
p
x
(
s
0
;s
)
,
y
d
=
p
y
(
s
0
;s
)
, and

d
= atan2(1
y;
1
x
)
. Here
p
(
s
0
;s
)=(
p
x
(
s
0
;s
)
;p
y
(
s
0
;s
))
T
is the desired trajectory, and
p
x
(
s
0
;s
)=
p
0
x
(
s
0
)+~
p
x
(
s
)
and
p
y
(
s
0
;s
)=
p
0
y
(
s
0
)+~
p
y
(
s
)
for
each of the
m
agents, where
s
0
and
s
are as defined before. We propose
the following simple, intuitive control algorithm for the actual robots.
Algorithm III.1:
v
=

cos 1

!
=
k
1

+_

d
where

= 1
x
2
+1
y
2
and
k;  >
0
.
We should point out that
1

is not defined at

=0
since

d
is not
defined. In implementation, one can replace

d
by the equation shown
at the bottom of the page where

is a small positive number. It is easy to
see that
^

d
is well defined at

=0
since
lim

!
0

d
(
0
2

3
+3

2
)=
0
.The error dynamics then becomes
_
1
x
=
0
@p
0
x
(
s
0
)
@s
0
_
s
0
+
@
~
p
x
(
s
)
@s
_
s
0

cos 1

cos

_
1
y
=
@p
0
y
(
s
0
)
@s
0
_
s
0
+
@
~
p
y
(
s
)
@s
_
s
0

cos 1

sin

_
1

=
0
k
1
:
Assumption III.1: Along trajectories, the formation satisfies
k
F
(~
x
d
)
k
<M<
1
for some
M
2
+
.
We can now formulate the following stability theorem.
Theorem III.1 (Stability): Under the control action given in Algo-
rithm III.1, it holds that
lim sup
t
!1

(
t
)

d
lim sup
t
!1
k
1

k

for some
d;  >
0
that can be made arbitrarily small with an appro-
priate choice of the control parameters
k
and

.
Proof: Since
_
1

=
0
k
1

, the second of the two control objec-
tives clearly holds. Furthermore, differentiating

gives
_

=
c
0
e
0
 
cos(

d
0

r
)+
ce
0

@
~
p
(
s
)
@s
1
cos(

d
0

r
)
0

cos
2
1

where

r
= atan2
@
~
p
y
(
s
)
@s ;@
~
p
x
(
s
)
@s
and

r
= atan2
@p
0
y
(
s
0
)
@s
0
;@p
0
x
(
s
0
)
@s
0
:
Now let
a
(
t
)=
0

cos
2
1

, and let
8(
t; s
)
be the transition matrix
of
a
(
t
)
. Then
k
8(
t; s
)
k
=exp
t
s
a
(

)
d
= exp
0
t
s

(1
0
sin
2
1

)
d

e

(1

(0)
=k
0
(
t
0
s
))
;
8
t

s

0
which gives
j

(
t
)
jj
8(
t;
0)
j

(0) +
t
0
j
8(
t; 
)
j
c
0
+
c@
~
p
(
s
(

))
@s d:
However, since the constraint function satisfies
F
(~
x
d
)
<M
ac-
cording to Assumption III.1, and since it is continuously differentiable,
^

d
=

d
;
if
>

d
(
0
2

3
+3

2
)+

r
(
0
2(

0

)
3
+3

(

0

)
2
)

3
;
if




950 IEEE TRANSACTIONS ON ROBOTICS AND AUTOMATION, VOL. 17, NO. 6, DECEMBER 2001
there exists a positive constant
K<
1
such that
j

(
t
)
jj
8(
t;
0)
j

(0) +
t
0
j
8(
t; 
)
j
(
cK
+
c
0
)
d

e

(1

(0)
=k
0
t
)

(0) +
cK
+
c
0
e

1

(0)
=k
where the first term decays exponentially, and the second term can be
made arbitrarily small with an appropriate choice of
k
and

. The the-
orem thus follows.
IV. RIGID BODY MOTIONS
In this section, we show how our coordination method can be used
for executing translational rigid body motions. With such a motion,
we understand a formation constraint that specifies a desired distance
between the different robots, as well as distances between the robots
and the virtual leader. The term “rigid body” is somewhat misleading
since we have no guarantee that the right distances are maintained for
all times. On the contrary, the introduction of flexibility into the system
is crucial, as we will see further on, when reactive obstacle avoidance
terms are added to the controller. In that case, we both want to maintain
formation and avoid obstacles at the same time, which calls for a certain
amount of flexibility.
Let the formation constraint be given by
F
(~
x
)=
m
i
=1
j
6
=
i

ij
k
~
x
i
0
~
x
j
k
2
0
d
2
ij
2
+
m
i
=1

i
k
~
x
i
k
2
0
d
2
i
2
where

ij
=

ji

0
are the weights that determine how important it
is that a particular distance
d
ij
=
d
ji
is maintained between
~
x
i
and
~
x
j
and similarly

i
and
d
i
determine how close
x
i
should be to the virtual
leader
x
0
. Of course, the
d
ij
s and
d
i
s need to be chosen in such a way
that a physically feasible formation is being defined by
F
0
1
(0)
.
In this case, no orientation of the formation is specified. Thus,
F
does
not meet the condition that
j
F
0
1
(0)
j
=1
in Definition II.1. In fact,
F
has a continuum of global minima, which each corresponds to a given
orientation of the formation. However, since each of these solutions are
acceptable, our method is still applicable.
Example IV.1 (Triangular Formations): We consider a triangular
formation without the orientation fixed
F
(~
x
)=(
k
~
x
1
0
~
x
2
k
2
0
1)
2
+(
k
~
x
2
0
~
x
3
k
2
0
1)
2
+(
k
~
x
3
0
~
x
1
k
2
0
1)
2
+
k
~
x
1
k
2
0
1
3
2
+
k
~
x
2
k
2
0
1
3
2
+
k
~
x
3
k
2
0
1
3
2
which corresponds to maintaining an equilateral triangular shape (side
lengths equal to one) between the different robots. (One of the terms
in the function is actually redundant for defining the shape.) The mid-
point of the triangle is the virtual leader in this case. An example of this
can be seen in Fig. 1.
As pointed out in [11], [13], [14], and [17], such rigid body forma-
tions are useful in a number of applications where groups of robots are
asked to carry or push objects in a coordinated manner.
A. Obstacle Avoidance
If we assume that the robots we are controlling are of the unicycle
type, we can add a standard, reactive obstacle avoidance term (see, for
example, [1]) to the individual control algorithms in Algorithm III.1.
We choose to keep the longitudinal velocity from Section III, i.e.,
v
=

cos1

, but augment the angular velocity with an avoidance term
!
=
w
OA
(
d
)(

OA
0

)+
k
1

+_

d
(1)
Fig. 1. The evolution of a triangular formation under a perfect tracking
assumption. The triangular formation and the reference path for the mid-point
of the triangle are shown.
Fig. 2. Obstacle avoidance. In this case, the robots are negotiating the circular
obstacle by moving around it on different sides while the virtual leader passes
through the obstacle.
where
d
= (
x
0
x
ob
)
2
+(
y
0
y
ob
)
2
,
w
OA
(
d
)=1
=d
2
if
d<
d
OA
,
w
OA
(
d
)=0
otherwise, and

OA
=

+ atan2(
y
ob
0
y; x
ob
0
x
)
. Here, the subscript
OA
stands for obstacle avoidance, and
d
OA
is the fixed distance from an obstacle, located at
(
x
ob
;y
ob
)
, where the
behavior becomes active. (If more than one obstacle is present, the con-
tributions from the different obstacles are just summed up in a straight
forward manner.) We thus have a method for controlling the individual
robots so that they drive toward the reference points, at the same time
as they avoid obstacles, as seen in Fig. 2.
V. CONCLUSION
In this paper, we propose a model-independent coordination strategy
for multi-agent formation control. The problem is defined by a for-
mation constraint in combination with a desired reference path for a
nonphysical, so-called virtual leader. We show that if the robots track
their respective reference points perfectly, or if the tracking errors are
bounded, our method stabilizes the formation error. This is a very useful
fact since it allows us to decouple the coordination problem into one

IEEE TRANSACTIONS ON ROBOTICS AND AUTOMATION, VOL. 17, NO. 6, DECEMBER 2001 951
planning problem, with proven features as long as the tracking is good
enough, and one tracking problem.
The tracking problem is solved for a class of nonholonomic robots
of the unicycle type, and we illustrate the soundness of our method by
applying it to rigid body constrained motions.
ACKNOWLEDGMENT
The authors would like to thank the three anonymous reviewers for
their insightful comments.
REFERENCES
[1] R. C. Arkin, Behavior-Based Robotics. Cambridge, MA: MIT Press,
1998.
[2] T. Balch and R. C. Arkin, “Behavior-based formation control for multi-
robot teams,” IEEE Trans. Robot. Automat., vol. 14, pp. 926–939, Dec.
1998.
[3] B. E. Bishop and M. W. Spong, “Control of redundant manipulators
using logic-based switching,” in Proc. 37th IEEE Conf. Decision and
Control, Tampa, FL, Dec. 1998.
[4] C. Canudas de Wit, B. Siciliano, and G. Bastin, Theory of Robot Con-
trol. Berlin: Springer-Verlag, 1996.
[5] J. Desai, J. Ostrowski, and V. Kumar, “Control of formations for mul-
tiple robots,” in Proc. IEEE Int. Conf. Robotics and Automation, Leuven,
Belgium, May 1998.
[6] J. Desai, “Motion planning and control of cooperative robotic systems,”
Ph.D. dissertation, Univ. of Pennsylvania, Philadelphia, 1998.
[7] M. Egerstedt, X. Hu, and A. Stotsky, “Control of a car-like robot using a
virtual vehicle approach,” in Proc. 37th IEEE Conf. Decision and Con-
trol, Tampa, FL, Dec. 1998, pp. 1502–1507.
[8] , “Control of mobile platforms using a virtual vehicle approach,”
IEEE Trans. Automat. Contr., to be published.
[9] R. Frezza, G. Picci, and S. Soatto, “Nonholonomic model-based pre-
dictive output tracking of an unknown three-dimensional trajectory,” in
Proc. 37th IEEE Conf. Decision and Control, Tampa, FL, Dec. 1998.
[10] J. K. Hedrick, D. McMahon, V. Narendran, and D. Swaroop, “Longitu-
dinal vehicle controller design for IVHS systems,” in Proc. American
Control Conf., 1991, pp. 3107–3112.
[11] O. Khatib, K. Yokoi, K. Chang, D. Ruspini, R. Holmberg, A. Casal, and
A. Baader, “Force strategies for cooperative tasks in multiple mobile
manipulation systems,” in Int. Symp. Robotics Research, Munich, Ger-
many, Oct. 1995.
[12] M. Mataric, M. Nilsson, and K. Simsarian, “Cooperative multi-robot
box-pushing,” in Proc. IROS, Pittsburgh, PA, 1995.
[13] P. Ögren, M. Egerstedt, and X. Hu, “A control Lyapunov function ap-
proach to multi-agent coordination,” in IEEE Conf. on Decision and
Control, Orlando, FL, Dec. 2001.
[14] N. Sarkar, X. Yun, and V. Kumar, “Dynamic path following: A newcon-
trol algorithm for mobile robots,” in Proc. 32nd Conf. Decision and Con-
trol, San Antonio, TX, Dec. 1993.
[15] D. Swaroop and J. K. Hedrick, “String stability of interconnected sys-
tems,” IEEE Trans. Automat. Contr., vol. 41, pp. 349–357, Mar. 1996.
[16] J. P. Tabuada, G. J. Pappas, and P. Lima, “Feasible formations of multi-
agent systems,” in Proc. American Control Conf., Arlington, VA, June
2001.
[17] D. Yanakiev and I. Kanellakopoulos, “A simplified framework for string
stability in AHS,” in Proc. 13th IFAC World Congress, vol. Q, 1996, pp.
177–182.
Randomized Path Planning for Linkages With Closed
Kinematic Chains
Jeffery H. Yakey, Steven M. LaValle, and Lydia E. Kavraki
Abstract—We extend randomized path planning algorithms to the case
of articulated robots that have closed kinematic chains. This is an impor-
tant class of problems, which includes applications such as manipulation
planning using multiple open-chain manipulators that cooperatively grasp
an object and planning for reconfigurable robots in which links might be
arranged in a loop to ease manipulation or locomotion. Applications also
exist in areas beyond robotics, including computer graphics, computational
chemistry, and virtual prototyping. Such applications typically involve high
degrees of freedom and a parameterization of the configurations that sat-
isfy closure constraints is usually not available. We show how to implement
key primitive operations of randomized path planners for general closed
kinematics chains. These primitives include the generation of random free
configurations and the generation of local paths. To demonstrate the fea-
sibility of our primitives for general chains, we show their application to
recently developed randomized planners and present computed results for
high-dimensional problems.
Index Terms—Closed linkages, kinematic chains, randomized path plan-
ning.
I. INTRODUCTION
This paper addresses the problem of path planning for general
linkages that have closed kinematic chains with redundant degrees of
freedom (DOF), in an environment that contains obstacles, as shown
in Fig. 1. In general, the constraints imposed by a closed linkage form
an algebraic variety and in principle complete planners such as [6] and
[3] could be used; however, the high computational complexity and
implementation difficulty of all of these algorithms for problems with
high degree of freedom makes them too prohibitive for practical use.
This motivates our approach in this paper, which extends randomized
planning techniques that were developed for open-chain systems [13],
[19] to general closed-chain systems.
Planning for linkages with closed kinematic chains has applications
both in and beyond robotics. Parallel manipulators involve closed kine-
matic constraints [22]. In manipulation planning, when multiple robots
grasp a single object, they form a closed loop containing the object as
a link of the chain [1], [15]. Many of the existing methods for manipu-
lation planning require inverse kinematics solutions for the robots [15]
which can be a limitation. Regrasping is also an important issue as one
or more of the manipulators often attain a singular configuration [23].
The ability to plan for linkages with closed kinematics chains elim-
inates the need of inverse kinematics solutions and could reduce the
number of regrasps needed during manipulation tasks, as the linkage
Manuscript received August 25, 2000; revised September 5, 2001. This paper
was recommended for publication by Associate Editors N. Overmars and N.
Amato and Editor A. De Luca upon evaluation of the reviewers’ comments.
The work of S. LaValle is supported in part by the National Science Founda-
tion through a CAREER award IRI-9875304 and by Honda Research through
a grant. The work of L. Kavraki is supported in part by the National Science
Foundation through a CAREER Award IRI-970228, a Whitaker Award, an ATP
Award, and a Sloan Fellowship.
J. H. Yakey is with the Department of Computer Science, Iowa State Univer-
sity, Ames, IA 50011 USA (e-mail: yakeyj@cs.iastate.edu).
S. M. LaValle is with the Department of Computer Science, University of
Illinois, Urbana, IL 61801 USA (e-mail: lavalle@cs.uiuc.edu).
L. E. Kavraki is with the Department of Computer Science, Rice University,
Houston, TX 77005 USA (e-mail: kavraki@cs.rice.edu).
Publisher Item Identifier S 1042-296X(01)11163-8.
1042–296X/01$10.00 © 2001 IEEE