In this paper we generate optimal smooth trajectories for a set of fully-actuated mobile robots. Given two end configurations, by tuning one parameter, the user can choose an interpolating trajectory from a continuum of curves varying from that corresponding to maintaining a rigid formation to motion of the robots toward each other. The idea behind our method is to change the original constant kinetic energy metric in the configuration space and can be summarized into three steps. First, the energy of the motion as a rigid structure is decoupled from the energy of motion along directions that violate the rigid constraints. Second, the metric is “shaped” by assigning different weights to each term. Third, geodesic flow is constructed for the modified metric. The optimal motions generated on the manifolds of rigid body displacements in 3-D space $SE3$ or in plane $SE2$ and the uniform rectilinear motion of each robot corresponding to a totally uncorrelated approach are particular cases of our general treatment.

1.
Yamashita, A., Fukuchi, M., Ota, J., Arai, T., and Asama, H., 2000, “Motion Planning for Cooperative Transportation of a Large Object by Multiple Mobile Robots in a 3D Environment,” Proc. IEEE Int. Conf. Robot. Automat., San Francisco, CA, pp. 3144–3151.
2.
Kang, W., Xi, N., and Sparks, A., 2000, “Formation control of autonomous agents in 3D workspace,” Proc. IEEE Int. Conf. Robot. Automat., San Francisco, CA, pp. 1755–1760.
3.
Fox
,
D.
,
Burgard
,
W.
,
Kruppa
,
H.
, and
Thrun
,
S.
,
2000
, “
Collaborative Multi-robot Exploration
,”
Autonomous Robots
,
8
(
3
), pp.
325
344
.
4.
Balch
,
T.
, and
Arkin
,
R. C.
,
1998
, “
Behavior-based Formation Control for Multi-Robot Teams
,”
IEEE Trans. Rob. Autom.
,
14
(
6
), pp.
1
15
.
5.
Parker, L. E., 2000, “Current State of the Art in Distributed Autonomous Mobile Robotics,” Proc. International Symposium on Distributed Autonomous Robotic Systems, Knoxville, TN, pp. 3–12.
6.
L. Chaimowicz, Sugar, T., Kumar, V., and Campos, M., 2001, “An Architecture for Tightly Coupled Multi-robot Cooperation,” Proc. IEEE Int. Conf. Robot. Automat., Seoul, Korea, pp. 2992–2997.
7.
Spletzer, J., and Taylor, C. J., 2001, “A Framework for Sensor Planning and Control with Applications to Vision Guided Multi-Robot Systems,” Proc. IEEE Computer Society Conference on Computer Vision and Pattern Recognition, Hawaii, USA.
8.
Chichka, D. F., Speyer, J. L., and Park, C. G., 1999, “Peak-seeking Control with Application to Formation Flight,” Proc. IEEE Conf. Decision and Control, Phoenix, AZ, pp. 2463–2470.
9.
Fierro, R., Song, P., Das, A. K., and Kumar, V., 2002, “Cooperative Control of Robot Formations,” Cooperative Control and Optimization Series, R. Murphey and P. Pardalos, eds., Kluwer Academic Press, pp. 73–93.
10.
Tan
,
K.
, and
Lewis
,
M. A.
,
1997
, “
Virtual Structures for High-precision Cooperative Mobile Robot Control
,”
Autonomous Robots
,
4
(
4
), pp.
387
403
.
11.
Beard, R. W., and Hadaegh, F. Y., 1998, “Constellation Templates: An Approach to Autonomous Formation Flying,” Proc. World Automation Congress, Anchorage, Alaska.
12.
Zefran
,
M.
,
Kumar
,
V.
, and
Croke
,
C.
,
1995
, “
On the Generation of Smooth Three-dimensional Rigid Body Motions
,”
IEEE Trans. Rob. Autom.
,
14
(
4
), pp.
579
589
.
13.
Bloch
,
A. M.
,
Leonard
,
N. E.
, and
Marsden
,
J. E.
,
2000
, “
Controlled Lagrangians and the Stabilization of Mechanical Systems I: The First Matching Theorem
,”
IEEE Trans. Autom. Control
,
45
(
12
), pp.
2253
2270
.
14.
Woolsey, C. A., Bloch, A. M., Leonard, N. E., and Marsden, J. E., 2001, “Physical Dissipation and the Method of Controlled Lagrangians,” Proc. European Control Conference, Porto, Portugal, pp. 2570–2575.
15.
Belta
,
C.
, and
Kumar
,
V.
,
2002
, “
An SVD-based Projection Method for Interpolation on SE3,
IEEE Trans. Rob. Autom.
,
18
(
3
), pp.
334
345
.
16.
Belta, C., and Kumar, V., 2001, “Motion Generation for Formations of Robots: A Geometric Approach,” Proc. IEEE Int. Conf. Robot. Automat., Seoul, Korea.
17.
do Carmo, M. P., 1992, Riemannian Geometry, Birkhauser, Boston.
18.
Park
,
F. C.
, and
Brockett
,
R. W. R. W.
,
1994
, “
Kinematic Dexterity of Robotic Mechanisms
,”
Int. J. Robot. Res.
,
13
(
1
), pp.
1
15
.
19.
Belta
,
C.
, and
Kumar
,
V.
,
2002
, “
Euclidean Metrics for Motion Generation on SE3,
J. Mech. Eng. Sci. Part C
216
(
C1
), pp.
47
61
.
20.
Roth
,
B.
,
1981
, “
Rigid and Flexible Frameworks
,”
Am. Math. Monthly
,
88
, pp.
6
21
.
21.
Press, W. H., Teukolsky, S. A., Vetterling, W. T., and Flannery, B. P., 1998, Numerical Recipes in C, Cambridge University Press.