Motion generation for formations of robots: a ... - Semantic Scholar
Motion generation for formations of robots: a geometric approach Calin Belta
Vijay Kumar
GRASP Laboratory University of Pennsylvania Philadelphia, PA 19104
Abstract This paper develops a method for generating smooth trajectories for mobile robots in formation. The problem of trajectory generation is cast in terms of designing optimal curves on the Euclidean group, SE (3), equipped with a left-invariant Riemannian metric. Speci cally, the method generates the trajectory that minimizes the total energy associated with the translations and rotations of the robots, while maintaining a rigid formation. When the mobile robots are nonholonomic, trajectories that allow rigid formations to be maintained must satisfy appropriate boundary conditions. These conditions are derived, and invariance properties of the resulting trajectories are proved. An ecient algorithm to obtain the trajectories is described. Finally, the approach is illustrated with examples involving formations of aircrafts described by rst order dynamics with nonholonomic constraints.
1 Introduction Multi-robotic systems are versatile and ecient in exploration missions, military surveillance, and cooperative manipulation tasks. Recent research on such systems include work on cooperative manipulation, multi-robot motion planning [13], mapping and exploration [8], software architectures for multi-robotic systems, and formation control. Our work is inspired by the framework of [5], in which the team of robots in formation is modeled as a triple (g; r; H), where g 2 SE (3) represents the gross position and orientation of the team (for example, the pose of the leader), r is a set of shape variables that describes the relative positions of the robots in the team, and H is a control graph which describes the control strategy used by each robot. Our main focus in this paper, however, is on motion planning in which the control strategy and the overall formation does not change. In other words, the control graph H remains the same. Thus we are primarily interested in the gross motion of the team, g(t), and the possible shape
changes r(t) that might accompany it. Attempts to model and analyze changes in control graph are addressed in [5, 7]. The basic idea behind this work is to consider the formation of robots as a rigid body, and investigate the motion of such rigid formations. Virtual structures have been proposed in [12] and used for motion planning [9], coordination and control of space-crafts [2], etc. Our de nition of a rigid formation requires the distances between robots (or between reference points on robots) to remain xed. Such a rigid formation is geometrically de ned as a polyhedron (a virtual rigid body) formed by the reference points (e.g., centers of mass) for each robot. The relative orientations of each robot are not restricted in such a rigid formation. In this paper we build on the results from [14, 3, 4] to generate trajectories for robots so that they move in a certain spatial formation and the overall energy of motion is minimized. The geometry of the formation induces a metric on the SE (3) of the virtual rigid body. In [4] we proved that this metric is naturally inherited from the ambient manifold GA(3). Our method involves three steps: (1) the generation of optimal trajectories for the virtual body in GA(3); (2) the projection of the trajectories from GA(3) to SE (3), and (3) the translation of the motion to position trajectory for each individual robot. The overall procedure is invariant with respect to both the local coordinates on the manifolds and the choice of the inertial frame. It should be noted that there are several papers that address the controllability of holonomic and nonholonomic mobile robots. For our purposes, we will cite the work of Tabuada and d'Andrea-Novel [11, 1] who derive controllers for three-dimensional nonholonomic mobile robots for following trajectories in R3 . The paper is organized as follows. In Section 2 we brie y introduce the di erential geometry tools [6] and the projection method [4] that we use throughout this work. The spatial formation problem is formulated in Section 2.2. We treat the holonomic case in Section 4.1 and the under-actuated case in Section 4.2. Examples for a formation of 5 robots are given in Section 5.
The paper ends with conclusions and future work in Section 6.
2 Background and formulation of the problem
2.1 The Lie groups SO(3) and SE (3)
Let GL(n) denote the general linear group of dimension n. As a manifold, GL(n) can be regarded as an 2 open subset of n . Moreover, matrix multiplication and inversion are both smooth operations, which make GL(n) a Lie group. The special orthogonal group is a subgroup of the general linear group, de ned as IR
SO(n) = R j R 2 GL(n; ); RRT = I; detR = 1 IR
SO(n) is referred to nas the rotation group on n . GA(n) = GL (n) is the ane group. SE (n) = SO(n) n is the special Euclidean group, and is the set of all rigid displacements in n . Special consideration will be given to SO(3) and SE (3). IR
IR
IR
IR
Consider a rigid body moving in free space. Assume any inertial reference frame fF g xed in space and a frame fM g xed to the body at point O0 as shown in Figure 1 (a). At each instance, the con guration (position and orientation) of the rigid body can be described by a homogeneous transformation matrix, A, corresponding to the displacement from frame fF g to frame fM g. SE (3) is the set of all rigid body transformations in three-dimensions:
SE (3) = A j A = R0 d1 ; R 2 SO(3); d 2
IR
3
:
SE (3) is a closed subset of GA(3), and, therefore, a
Lie group. The Lie algebras of SO(3) and SE (3), denoted by so(3) and se(3) respectively, are given by:
so(3) = !^ 2 se(3) =
!^ v
0 0
IR
; !^ T = ?!^ ;
3 3
j !^ 2 so(3); v 2
IR
3
:
A 3 3 skew-symmetric matrix !^ can be uniquely identi ed with a vector ! 2 3 so that for an arbitrary vector x 2 3 , !^ x = ! x, where is the vector cross product operation in 3 . Each element 2 se(3) can be thus identi ed with a vector pair f!; vg. Given a curve A(t) : [?a; a] ! SE (3); A(t) = R0(t) d(1t) IR
IR
IR
an element (t) of the Lie algebra se(3) can be associated to the tangent vector A_ (t) at an arbitrary point t by: T_ (t) = A?1 (t)A_ (t) = !^ 0(t) R0 d : (1) where !^ (t) = RT R_ is the corresponding element from so(3). A curve on SE (3) physically represents a motion of the rigid body. If f!(t); v(t)g is the vector pair corresponding to (t), then ! physically corresponds to the angular velocity of the rigid body while v is the linear velocity of the origin O0 of the frame fM g, both expressed in the frame fM g. In kinematics, elements of this form are called twists and se(3) thus corresponds to the space of twists. The twist (t) computed from Equation (1) does not depend on the choice of the inertial frame fF g. If P is an arbitrary point on the rigid body with position vector r in frame fM g (Figure 1 (a)), then the velocity of P in frame fM g is given by (2) vP = ?(r); ?(r) = [ ?r^ I3 ] where is the twist of the rigid body.
2.2 Problem formulation
Consider N robots moving (rotating and translating) in space. We choose a reference point on each robot. It is convenient (but not necessary) to x it at the center of mass Oi of each robot. We say N robots move in rigid formation if the motion preserves the distances between any two reference points. Alternatively, a rigid formation implies that the speci ed reference points form a rigid polyhedron. A mobile frame fMig is attached to each robot at its center of mass Oi . Robot i has mass mi and matrix of inertia Hi with respect to frame fMig. Given the positions di of the centers of mass with respect to a reference frame fF g, the center of mass O0 of the P system of robots has coPN ordinates d = i=1 (mi di )= Ni=1 mi with respect to the same inertial frame. The formation is interpreted as a virtual rigid body with centroid O0 , to which we attach a formation mobile frame fM g (Figure 1 (b)). Let R; Ri 2 SO(3) denote the rotations of fM g and fMig in fF g. If ri is the position vector of Oi in fM g, then moving in rigid formation preserves0 the0 ri 's. Given two poses of the system of robots d ; R at t = 0 and d1 ; R1 at t = 1 such that they have the same rigid formation, (i.e., have the same ri 's) and given the individual orientations Ri0 at t = 0 and Ri1 at t = 1, we want to generate interpolating motion for each robot so that:
the rigid formation is preserved;
(a)
(b)
z {M} x
O’
y z
z
{M 1}
{M 2}
y
O1
z
{M}
y r2
O2
y
r1
z
z
P r
O’
x
y
{M}
x
x d2
d1
O’
r3
z {M 3}
z
d x
x
O3
d3
d {F}
y
{F}
O
y
y
O
x
x
Figure 1: (a) The inertial ( xed) frame and the moving frame attached to the rigid body; (b) A formation of N = 3 robots of various shapes.
the total kinetic energy is minimized; and the nonholonomic constraints (if any) are satis-
ed. To mathematically formalize the energy requirement, we need to de ne a meaningful metric on SE (3).
2.3 Riemannian metrics on SO(3) and SE (3)
In [4], we show that there is a simple way of de ning a left or right invariant metric in SO(n) (SE (n)) by introducing an appropriate metric in GL(n) (GA(n)). De ning a metric at the Lie algebra so(n) (or se(n)) and extending it through left (right) translations is equivalent to inheriting the appropriate metric from the ambient manifold. Detailed proofs for all the results in this section can be found in [4]. Let W be a symmetric positive de nite 3 3 matrix. For any M 2 GL(3) and any X; Y 2 TM GL(3), de ne < X; Y >GL= Tr(X T Y W ) (3) Quadratic (3) is symmetric and positive de nite, therefore a proper metric in GL(3). Let R be an arbitrary element from SO(3) and X , Y be two vectors from TR SO(3). The metric on SO(3) at R inherited from GL(3) is of the form where
< X; Y >SO = !xT G!y
(4)
G = Tr(W )I3 ? W; W = 21 Tr(G)I3 ? G
(5)
and !x ; !y 2 3 are the vector forms of the corresponding twists from so(3). Let W 0 ~ (6) W= 0 w be a symmetric positive de nite 4 4 matrix, where W is the matrix of the metric given by (3) and w 2 . If X and Y are two vectors in the tangent space at an arbitrary point B 2 GA(3), then the following form de nes a metric: < X; Y >GA= Tr(X T Y W~ ) (7) If A is an arbitrary element from SE (3) and X; Y are two vectors from TA SE (3), then the restriction of (7) to SE (3) gives: IR
IR
< X; Y >SE =
!xT
vxT
G~ !vyy ;
(8)
G~ = G0 wI0 3 where G is given by (5) and f!x; vx g is the vector representation of the twist corresponding to X . The norm of metric (8) gives the kinetic energy of a moving (rotating and translating) rigid body, if the mass of the body is m = 2w and the matrix of inertia about frame fM g placed at the centroid of the body is H = 2G. If fM g is aligned with the principal axes of the body, then G is diagonal.
2.4 Projection onto SE (3)
We can use the norm induced by metric (7) to de ne the distance between elements in GA(3). Using this
distance, for a given B 2 GA(3), we de ne the projection of B on SE (3) as being the closest A 2 SE (3) with respect to metric (7). The following result is stated and proved in [4]: Proposition 2.1
block partition
Let B 2 GA(3) with the following
B = B01 B12 ; B1 2 GL(3); B2 2
IR
translation and exponential coordinates to
3
and U; ; V the singular value decomposition of B1 W :
B1 W = U V T
(9)
Then the projection of B on SE (3) is given by
T A = UV0 B12 2 SE (3)
(10)
Based on Proposition 2.1, a procedure for generating near optimal curves on SE (3) follows: generate the curves in the ambient space and project them on SE (3). Due to the fact that the metric we de ned in GA(3) is the same at all points, the corresponding Christo el symbols are all zero. Consequently, the optimal curves in the ambient space assume simple analytical forms (i.e geodesics - straight lines, minimum acceleration curves - cubic polynomial curves, minimum jerk curves - fth order polynomial curves, all parameterized by time). The resulting curve in GA(3) is linear in the boundary conditions, and therefore left and right invariant. Moreover, the projection on SE (3) is left invariant [4], and so is the overall procedure. Finally, note that due to the product structure of metric G~ , the rotational motion R(t) can be generated separately from the translational motion d(t) [14].
3 Optimal curves on SE (3) In this section we brie y describe the methods to generate optimal or near optimal interpolating motion between end poses (geodesics) or poses and velocities (minimum acceleration curves). In what follows, the given boundary conditions will be denoted by R0 ; d0 ; R_ 0; d_0 at t = 0 and R1 ; d1 ; R_ 1 ; d_1 at t = 1.
3.1 Minimum geodesics
energy
curves
G!_ (t) + !(t) (G!(t)) = 0 (11) d(t) = 0 (12) The solution to (12) is straightforward: d(t) = d0 + (d1 ? d0 )t; t 2 [0; 1]. In the particular case when G = I3 , equation (11) is easily integrable using left R(t) = R0 e!^ 0 t ; !^0 = log R0 T R1 If G 6= I3 , (11) does not have an analytical solution. A local parameterization of SO(3) should be chosen and three rst order di erential equations augment the system. A solution can be found using iterative procedures such as the shooting method or the relaxation method [10]. Alternatively, the projection method outlined in Section 2.4 can be used. In this case, the rotational part when t 2 [0; 1] is given by: [R0 + (R1 ? R0 )t]W = U (t)(t)V T ; R(t) = U (t)V T ;
3.2 Minimum acceleration curves
The di erential equations to be satis ed by the minimum acceleration curves on SE (3) equipped with metric (8) are derived for the case G = I3 in [14]:
!(3) + ! ! = 0 d(4) = 0
(13) (14) Again, the solution to the translational part (14) is immediate: d(t) = d0 + d_0 t + (?3d0 + 3d1 ? 2d_0 ? d_1 )t2 +(2d0 ? 2d1 + d_0 + d_1 )t3 (15) For the rotational part, a numerical method or the projection method should be used. If we choose the latter, there is no restriction on the form of G and the solution is given by R(t) = U (t)V T (t) where: M (t) = R0 + R_ 0 t + (?3R0 + 3R1 ? 2R_ 0 ? R_ 1 )t2 +(2R0 ? 2R1 + R_ 0 + R_ 1 )t3 (16) and U , V are given by the W - weighted SVD of M (t) as in (9).
-
4 Generation of smooth motion
The di erential equations to be satis ed by the geodesics on SE (3) equipped with metric (8) are derived in [14]:
Holonomic and nonholonomic robots (like airplanes) are considered in the next two sections. Before we start, two observations are in order. First, moving in
rigid formation is equivalent to generating trajectories with respect to fF g for each Oi in the form:
di (t) = d(t) + R(t)ri ; i = 1; : : : ; N
(17)
Second, if vi is the velocity of Oi in fMi g, the moving in rigid formation restriction can be written equivalently in terms of velocities in the form
vi (t) = Ri (t)T R(t)?(ri ) (t)
(18) where ri satis es equation (17) at some instant (say t = 0). 2 se(3) is the twist of the virtual rigid body and ?(ri ) is as in equation (2).
4.1 Optimal Motion Generation for Formations of Fully-actuated Robots
In this section we will assume that each robot can actuate each of its 6 degrees of freedom. Assume the initial (t = 0) and nal (t = 1) poses of each robot are given in frame fF g and the problem is to generate interpolating motion while keeping formation and minimize the total kinetic energy of the system. Obviously, the initial and nal poses should have the same formation, i.e. have the same ri 's. The total instantaneous kinetic energy is given by:
T=
N X i=1
iT G~ i i
N X i=1
!iT Gi !i + wi T ?T (ri )RT Ri RiT R?(ri )
Noting that RT Ri RiT R = I and total kinetic energy becomes
T=
N X i=1
PN
i=1 mi ri = 0, the
PN
i=1 (?mi r^i ) 2
0
PN
i
Z 0
0
i=1 mi I3
1
!iT Gi !i dt; i = 1; : : : ; N;
(ii) min
Z 0
1
~ T Gdt
The solutions to problems (i) are geodesics on SO(3) for metric Gi . The solution to problem (ii) is the geodesic on SE (3) for metric G~ . Both these problems can be solved as described in Section 3.1. We are now in the position to outline a procedure for generating smooth motion for each robot to interpolate between two given positions while preserving the formation and minimizing the kinetic energy. First, we locate the centroids Oi , attach the frames fMig and, from the geometric properties of each robot, calculate the inertia matrices Hi . If fMi g is aligned with the principal axes of robot i, then Hi is diagonal, but this is not necessary. If the initial and nal rotations Ri0 , Ri1 of robot i are given, then the rotation of the robot is the interpolating geodesic on SO(3) equipped with metric Gi = (1=2)Hi . Given d01 ; d02 ; : : : ; d0N at t = 0 and d11 ; d12 ; : : : ; d1N at t = 1, the initial and nal positions of the centroid of the ctitious rigid body are given by: PN
j i=1 mi di ; i=1 mi
PN
j = 0; 1
A frame fM g is xed to the virtual rigid body at its center of mass, which will give the initial and nal orientations of the formation, R0 and R1 . The ri 's are then determined by ri = R0 T (d0i ? d0 ), which will induce a metric G~ on the SE (3) of the formation, according to (19). The geodesic (d(t); R(t)) on the SE (3) of the formation with boundary conditions (d0 ; R0 ), (d1 ; R1 ) can be found using the relaxation method or the projection method. The trajectory of the centroid of each robot is nally determined in form (17).
4.2 Motion Generation for Formations of under-actuated robots
~ !iT Gi !i + T G
where G~ is dependent on the mass and the geometry of the formation by
G~ = 21
(i) min !
dj =
where i = [!iT viT ]T 2 se(3), the twist corresponding to the i-th robot is assumed in the vector form and G~ i is of the form described in equation (8) with Gi = (1=2)Hi and wi = (1=2)mi . Using equation (18), we get:
T=
Therefore, using the formation constraint, the problem reduces from dimension 6N to 3N + 6 and is separable into N + 1 independent subproblems:
(19)
In this section we assume that a subset of M from the N robots are under-actuated. Using the airplane model, suppose that robot i, i = 1; : : : ; M can only translate along the x-axis of fMi g. Besides the formation restriction (18), in this case we have an additional under-actuation constraint of the form C = 0, where
C is a 2M 6 matrix obtained by stacking 2 6 matrices of the form 0 1 0 RT R?(r ); i = 1; : : : ; M (20) i 0 0 1 i Therefore, the system of N robots moves while keeping formation fr1 ; : : : ; rN g and observing the nonholonomic constraints if and only if , the twist of the ctitious rigid body, is in the null space of matrix C given by (20). In other words, the formation of N robots of which M are under-actuated can move (i.e. 6= 0) if and only if the null space of matrix C is nonempty, or, equivalently, if rank(C ) 5. It is also interesting to note that the null space of matrix C is the intersection of the null spaces of each 2 6 block, which are all invariant under arbitrary rotations about the xaxis (the proof is straightforward). That is, replacing Ri with Ri Rot(x; ) in (20) leaves the null space of the matrix unchanged. The signi cant information about the individual orientations of the airplanes contained in the null space of C is, therefore, up to a rotation about the x axis of each airplane. As in Section 4.1, assume the initial (d01 ; R10 ); : : : ; (d0N ; RN0 ) and nal 1 1 (d1 ; R1 ); : : : ; (d1N ; RN1 ) poses of each robot are so that the rigid formation is preserved. If C 0 and C 1 denote the0 initial and nal C 1matrices, we assume that rank(C ) 5 and rank(C ) 5 (i.e, motion of formation is possible both at t = 0 and t = 1). We want to generate interpolating motion while keeping formation and observing the nonholonomic constraints so that the obtained curves are as smooth as possible. The nonholonomic constraints at t = 0 and t = 1 restrict at the boundaries, allowing 12 ? rank(C 0 ) ? rank(C 1 ) degrees of freedom. The projection method outlined in Section 3 will be used to generate interpolating motion for the virtual rigid body between the end poses and velocities. The obtained curves will be parameterized by the allowed degrees of freedom. Minimum energy is the criterion to select among the several choices. Explicitly, let fa1 ; : : : ; an g and fb1; : : : ; bmg (n; m 5) be the coordinates of 0 and 1 in some basis of the null spaces of C 0 and C 1 . If we write 0 = [!0 T v0 T ]T and 1 = [!1 T v1 T ]T , then R_ 0 = R0 !^ 0 , R_ 1 = R1 !^ 1 , d_0 = R0 v0 , d_1 = R1 v1 are all linear in ai 's and bi 's. The metric on the SE (3) of the virtual rigid body is determined by the mass properties of the robots and the geometry of the formation in the form given by (19). Minimum acceleration curves (M (t); d(t)) are generated in GA(3) with the corresponding metric W~ in the form (16), (15). The energy of the curve in GA(3)
E (ai ; bi ) =
Z 0
1
h
i
_ ) + wd_T d_ dt Tr(M_ T MW
is a positive de nite quadratic in ai 's and bi 's. Let ai and bi be the arguments minimizing E . The corresponding curve is then projected onto SE (3), as described in Section 3.2. The obtained (R(t); d(t)) will determine trajectories for each Oi using equation (17). The generated di (t); i = 1; : : : ; N have the property that the corresponding vi (0) and vi (1) are in accordance with the nonholonomic constraints. A position tracking algorithm [11] can be used for the M nonholonomic robots. The resulting tracking error will be zero. For the remaining N ? M holonomic robots, rotational motion can be generated separately as described in Section 4.1.
5 Examples A spatial formation of N = M = 5 identical airplanes is described in Figure 2. With the formation frame fM g xed at the centroid and oriented as shown in the gure, the geometry of the formation is described by: 2
h
4 5
r1 = 4 0 0
3
2
5
; r2 = 4
?ah 5
a2 2
2
3
3
?h 5 ; r = 4 ?a 5 ; a 5
3
2
2
2
? ha ?ah r = 4 ? 5; r = 4 ?a ?a 5
4
2
3
2
5
5
2
2
3 5
2
The inertial frame fF g is assumed to be coincident with fM g at t = 0. The geometry will induce metrics in SE (3) and GA(3) as follows: if m is the mass of each airplane, then the matrix G~ of the kinetic energy metric as de ned in (19) is given by
G~ = G0 wI0 3 ; G = m2 diagf2a2; a2 + 4h3 ; a2 + 4h5 g; w = 32m 2
2
The matrix of the metric W~ in the ambient manifold GA(3) is determined using (5) and (6). For the formation at t = 0, we have R10 = R20 = R30 = R40 = R50 = R0 = I3 ; d0 = [0 0 0]T Some elementary but tedious algebra gives the null space of the matrix C 0 in the form: Null(C 0 ) = span([0 0 0 1 0 0]T ) Therefore, as expected, the formation can move at t = 0 while observing the nonholonomic constraints only
z
x
y
y 1
z
x
1 h x
4h/5 z 2 z
z a
x
z
z
z x
a 5
z
z 2
y
y {M}
y
1
z
y
x
z
x x
x
5
x
y
2
5
y z
z
y x
3
z
y
x
4
{M}
z
y
x
a
y
y z
{M}
x
a
y
3
x y
y
x y
x
4
x y z
t=1 (a)
t=0
4
3 t=1 (b)
Figure 2: A formation of 5 identical airplanes. 30
30
1 2 3 4 5 O
25 20 15
30
1 2 3 4 5 O
25 20 15
20 15
10
10
10
5
5
5
0
0
0
−5 3
−5 3
−5 4
2
10
−1 0
−2 −3
−10
30
1
20
0
i)
2
30
1
30
2
20
0
ii)
1 2 3 4 5 O
25
iii)
0
−2 −3
20
0
10
−1 −10
10 −2
0 −4
−10
Figure 3: The generated position trajectory for airplane i, i = 1; : : : ; 5 and the curve described by the centroid of the virtual rigid body (O) for the case m = 1, a = 5, h = 10, X = 20, Y = 20: i) The airplanes are assumed holonomic; ii) End orientations as in case (a); and iii) End orientations as in case (b). by translating along the x axis of fM g. The formation at t = 1 is described by d1 = [X 0 Y ]T ; R1 = Rot(y; ?90) The cases (a) and (b) di er by the individual orientations of each airplane. In case (a), R11 = R21 = R31 = R41 = R51 = I3 while in case (b) R11 = Rot(y; ?90); R21 = Rot(z; ?135)Rot(y; ?45);
R31 = Rot(z; ?45)Rot(y; ?45); R41 = Rot(z; 45)Rot(y; ?45); R51 = Rot(z; 135)Rot(y; ?45): The corresponding null spaces are again 1dimensional: (a) Null(C 1 ) = span([0 0 0 0 0 1]T );
p
(b) Null(C ) = span([ a2 0 0 1 0 0]T ) 1
The interpolating minimum acceleration curves on the ambient manifold (M (t); d(t)), which depend on a1 and b1 as described in Section 4.2, are given by " 1 + f (t) 0 f (t) # 0 1 0 (a) M (t) = ?f (t) 0 1 + f (t)
a1 t ? Xf (t) + (b1 ? 2a1)t2 + (a1 ? b1 )t3 d(t) = 0 ?Zf (t) "
1 + f (t) g(t) f (t) 0 1 g(t) (b) M (t) = ?f (t) 0 1 + f (t) "
2
d(t) = 4
a1 t + 3Xt2 + (?2X + a1 )t3 0 (3Z ? b1 )t + (?2Z + b1 )t3 2
#
3 5
#
where
p
f (t) = ?3t2 + 2t3; g(t) = a2 b1 t2 (1 ? t) The energy is minimized along the above curves in metric W~ if (a) a1 = D; b1 = ?D; (b) a1 = ? 1948X ; b1 = 4(1 3+Z2m) The corresponding curve is then projected onto SE (3) as described in Section 3. The resulting position trajectories for cases (a) and (b) are given below for comparison together with the optimal curves which we would obtain if the airplanes were holonomic: Note that in cases ii) and iii), the end orientations are consistent with the nonholonomic restrictions. Also, in the holonomic case i), the trajectory of the centroid is a uniformly parameterized line, as expected.
6 Conclusion and future work We consider the trajectory generation problem for a formation of N arbitrarily shaped robots of which M are under-actuated. Our method yields trajectories that (nearly) minimize a measure of total energy while satisfying the nonholonomic constraints and maintaining a rigid formation. The resulting curves are invariant with respect to the choice of the inertial frame and parameterization. Our ongoing research is devoted to including changes in the shape of the formation to accommodate velocity constraints and to generating strategies and controllers for switching among pre-de ned formations.
References [1] B. d'Andrea Novel, G. Campion, and G. Bastin, \Control of nonholonomic wheeled mobile robots by state feedback linearization," International Journal of Robotics Research, vol. 14, no. 6, pp. 543{559, 1995. [2] R. W. Beard and F. Y. Hadaegh, \Constellation templates: an approach to autonomous formation
ying," in World Automation Congress, (Anchorage, Alaska), 1998. [3] C. Belta and V. Kumar, \An ecient geometric approach to rigid body motion interpolation," in ASME 2000 Design Engineering Technical Conferences, (Baltimore, MD), 2000.
[4] C. Belta and V. Kumar, \New metrics for rigid body motion interpolation," in Ball 2000 Symposium Commemorating the Legacy, Works, and Life of Sir Robert Stawell Ball, (University of Cambridge, UK), 2000. [5] J. P. Desai, J. P. Ostrowsky, and V. Kumar, \Controlling formations of multiple mobile robots," in Proc.IEEE Int.Conf.Robot.Autom., (Leuven, Belgium), pp. 2864{2869, 1998. [6] M. P. do Carmo, Riemannian geometry. Boston: Birkhauser, 1992. [7] R. Fierro, A. Das, and V. Kumar, \Hybrid control of formations of robots," in submitted to ICRA, 2001. [8] D. Fox, W. Burgard, H. Kruppa, and S. Thrun, \Collaborative multi-robot exploration," Autonomous Robots, vol. 8, no. 3, 2000. [9] F. Gentili and F. Martinelli, \Robot group formations: a dynamic programming approach for a shortest path computation," in Proc. of the IEEE Int. Conference on Robotics and Automation, (San Francisco, CA), 2000. [10] W. H. Press, S. A. Teukolsky, W. T. Vetterling, and B. P. Flannery, Numerical Recipes in C. Cambridge: Cambridge University Press, 1988. [11] P. Tabuada and P. Lima, \Position tracking for underactuated rigid bodies on SE(3)." preprint, 2000. [12] K. Tan and M. A. Lewis, \Virtual structures for high-precision cooperative mobile robot control," Autonomous robots, vol. 4, no. 4, pp. 387{403, 1997. [13] A. Yamashita, M. Fukuchi, J. Ota, T. Arai, and H. Asama, \Motion planning for cooperative transportation of a large object by multiple mobile robots in a 3d environment," in Proceedings of IEEE International Conference on Robotics and Automation, (San Francisco, CA), pp. 3144{ 3151, 2000. [14] M. Z efran, V. Kumar, and C. Croke, \On the generation of smooth three-dimensional rigid body motions," IEEE Transactions on Robotics and Automation, vol. 14(4), pp. 579{589, 1995.
Vijay Kumar
GRASP Laboratory University of Pennsylvania Philadelphia, PA 19104
Abstract This paper develops a method for generating smooth trajectories for mobile robots in formation. The problem of trajectory generation is cast in terms of designing optimal curves on the Euclidean group, SE (3), equipped with a left-invariant Riemannian metric. Speci cally, the method generates the trajectory that minimizes the total energy associated with the translations and rotations of the robots, while maintaining a rigid formation. When the mobile robots are nonholonomic, trajectories that allow rigid formations to be maintained must satisfy appropriate boundary conditions. These conditions are derived, and invariance properties of the resulting trajectories are proved. An ecient algorithm to obtain the trajectories is described. Finally, the approach is illustrated with examples involving formations of aircrafts described by rst order dynamics with nonholonomic constraints.
1 Introduction Multi-robotic systems are versatile and ecient in exploration missions, military surveillance, and cooperative manipulation tasks. Recent research on such systems include work on cooperative manipulation, multi-robot motion planning [13], mapping and exploration [8], software architectures for multi-robotic systems, and formation control. Our work is inspired by the framework of [5], in which the team of robots in formation is modeled as a triple (g; r; H), where g 2 SE (3) represents the gross position and orientation of the team (for example, the pose of the leader), r is a set of shape variables that describes the relative positions of the robots in the team, and H is a control graph which describes the control strategy used by each robot. Our main focus in this paper, however, is on motion planning in which the control strategy and the overall formation does not change. In other words, the control graph H remains the same. Thus we are primarily interested in the gross motion of the team, g(t), and the possible shape
changes r(t) that might accompany it. Attempts to model and analyze changes in control graph are addressed in [5, 7]. The basic idea behind this work is to consider the formation of robots as a rigid body, and investigate the motion of such rigid formations. Virtual structures have been proposed in [12] and used for motion planning [9], coordination and control of space-crafts [2], etc. Our de nition of a rigid formation requires the distances between robots (or between reference points on robots) to remain xed. Such a rigid formation is geometrically de ned as a polyhedron (a virtual rigid body) formed by the reference points (e.g., centers of mass) for each robot. The relative orientations of each robot are not restricted in such a rigid formation. In this paper we build on the results from [14, 3, 4] to generate trajectories for robots so that they move in a certain spatial formation and the overall energy of motion is minimized. The geometry of the formation induces a metric on the SE (3) of the virtual rigid body. In [4] we proved that this metric is naturally inherited from the ambient manifold GA(3). Our method involves three steps: (1) the generation of optimal trajectories for the virtual body in GA(3); (2) the projection of the trajectories from GA(3) to SE (3), and (3) the translation of the motion to position trajectory for each individual robot. The overall procedure is invariant with respect to both the local coordinates on the manifolds and the choice of the inertial frame. It should be noted that there are several papers that address the controllability of holonomic and nonholonomic mobile robots. For our purposes, we will cite the work of Tabuada and d'Andrea-Novel [11, 1] who derive controllers for three-dimensional nonholonomic mobile robots for following trajectories in R3 . The paper is organized as follows. In Section 2 we brie y introduce the di erential geometry tools [6] and the projection method [4] that we use throughout this work. The spatial formation problem is formulated in Section 2.2. We treat the holonomic case in Section 4.1 and the under-actuated case in Section 4.2. Examples for a formation of 5 robots are given in Section 5.
The paper ends with conclusions and future work in Section 6.
2 Background and formulation of the problem
2.1 The Lie groups SO(3) and SE (3)
Let GL(n) denote the general linear group of dimension n. As a manifold, GL(n) can be regarded as an 2 open subset of n . Moreover, matrix multiplication and inversion are both smooth operations, which make GL(n) a Lie group. The special orthogonal group is a subgroup of the general linear group, de ned as IR
SO(n) = R j R 2 GL(n; ); RRT = I; detR = 1 IR
SO(n) is referred to nas the rotation group on n . GA(n) = GL (n) is the ane group. SE (n) = SO(n) n is the special Euclidean group, and is the set of all rigid displacements in n . Special consideration will be given to SO(3) and SE (3). IR
IR
IR
IR
Consider a rigid body moving in free space. Assume any inertial reference frame fF g xed in space and a frame fM g xed to the body at point O0 as shown in Figure 1 (a). At each instance, the con guration (position and orientation) of the rigid body can be described by a homogeneous transformation matrix, A, corresponding to the displacement from frame fF g to frame fM g. SE (3) is the set of all rigid body transformations in three-dimensions:
SE (3) = A j A = R0 d1 ; R 2 SO(3); d 2
IR
3
:
SE (3) is a closed subset of GA(3), and, therefore, a
Lie group. The Lie algebras of SO(3) and SE (3), denoted by so(3) and se(3) respectively, are given by:
so(3) = !^ 2 se(3) =
!^ v
0 0
IR
; !^ T = ?!^ ;
3 3
j !^ 2 so(3); v 2
IR
3
:
A 3 3 skew-symmetric matrix !^ can be uniquely identi ed with a vector ! 2 3 so that for an arbitrary vector x 2 3 , !^ x = ! x, where is the vector cross product operation in 3 . Each element 2 se(3) can be thus identi ed with a vector pair f!; vg. Given a curve A(t) : [?a; a] ! SE (3); A(t) = R0(t) d(1t) IR
IR
IR
an element (t) of the Lie algebra se(3) can be associated to the tangent vector A_ (t) at an arbitrary point t by: T_ (t) = A?1 (t)A_ (t) = !^ 0(t) R0 d : (1) where !^ (t) = RT R_ is the corresponding element from so(3). A curve on SE (3) physically represents a motion of the rigid body. If f!(t); v(t)g is the vector pair corresponding to (t), then ! physically corresponds to the angular velocity of the rigid body while v is the linear velocity of the origin O0 of the frame fM g, both expressed in the frame fM g. In kinematics, elements of this form are called twists and se(3) thus corresponds to the space of twists. The twist (t) computed from Equation (1) does not depend on the choice of the inertial frame fF g. If P is an arbitrary point on the rigid body with position vector r in frame fM g (Figure 1 (a)), then the velocity of P in frame fM g is given by (2) vP = ?(r); ?(r) = [ ?r^ I3 ] where is the twist of the rigid body.
2.2 Problem formulation
Consider N robots moving (rotating and translating) in space. We choose a reference point on each robot. It is convenient (but not necessary) to x it at the center of mass Oi of each robot. We say N robots move in rigid formation if the motion preserves the distances between any two reference points. Alternatively, a rigid formation implies that the speci ed reference points form a rigid polyhedron. A mobile frame fMig is attached to each robot at its center of mass Oi . Robot i has mass mi and matrix of inertia Hi with respect to frame fMig. Given the positions di of the centers of mass with respect to a reference frame fF g, the center of mass O0 of the P system of robots has coPN ordinates d = i=1 (mi di )= Ni=1 mi with respect to the same inertial frame. The formation is interpreted as a virtual rigid body with centroid O0 , to which we attach a formation mobile frame fM g (Figure 1 (b)). Let R; Ri 2 SO(3) denote the rotations of fM g and fMig in fF g. If ri is the position vector of Oi in fM g, then moving in rigid formation preserves0 the0 ri 's. Given two poses of the system of robots d ; R at t = 0 and d1 ; R1 at t = 1 such that they have the same rigid formation, (i.e., have the same ri 's) and given the individual orientations Ri0 at t = 0 and Ri1 at t = 1, we want to generate interpolating motion for each robot so that:
the rigid formation is preserved;
(a)
(b)
z {M} x
O’
y z
z
{M 1}
{M 2}
y
O1
z
{M}
y r2
O2
y
r1
z
z
P r
O’
x
y
{M}
x
x d2
d1
O’
r3
z {M 3}
z
d x
x
O3
d3
d {F}
y
{F}
O
y
y
O
x
x
Figure 1: (a) The inertial ( xed) frame and the moving frame attached to the rigid body; (b) A formation of N = 3 robots of various shapes.
the total kinetic energy is minimized; and the nonholonomic constraints (if any) are satis-
ed. To mathematically formalize the energy requirement, we need to de ne a meaningful metric on SE (3).
2.3 Riemannian metrics on SO(3) and SE (3)
In [4], we show that there is a simple way of de ning a left or right invariant metric in SO(n) (SE (n)) by introducing an appropriate metric in GL(n) (GA(n)). De ning a metric at the Lie algebra so(n) (or se(n)) and extending it through left (right) translations is equivalent to inheriting the appropriate metric from the ambient manifold. Detailed proofs for all the results in this section can be found in [4]. Let W be a symmetric positive de nite 3 3 matrix. For any M 2 GL(3) and any X; Y 2 TM GL(3), de ne < X; Y >GL= Tr(X T Y W ) (3) Quadratic (3) is symmetric and positive de nite, therefore a proper metric in GL(3). Let R be an arbitrary element from SO(3) and X , Y be two vectors from TR SO(3). The metric on SO(3) at R inherited from GL(3) is of the form where
< X; Y >SO = !xT G!y
(4)
G = Tr(W )I3 ? W; W = 21 Tr(G)I3 ? G
(5)
and !x ; !y 2 3 are the vector forms of the corresponding twists from so(3). Let W 0 ~ (6) W= 0 w be a symmetric positive de nite 4 4 matrix, where W is the matrix of the metric given by (3) and w 2 . If X and Y are two vectors in the tangent space at an arbitrary point B 2 GA(3), then the following form de nes a metric: < X; Y >GA= Tr(X T Y W~ ) (7) If A is an arbitrary element from SE (3) and X; Y are two vectors from TA SE (3), then the restriction of (7) to SE (3) gives: IR
IR
< X; Y >SE =
!xT
vxT
G~ !vyy ;
(8)
G~ = G0 wI0 3 where G is given by (5) and f!x; vx g is the vector representation of the twist corresponding to X . The norm of metric (8) gives the kinetic energy of a moving (rotating and translating) rigid body, if the mass of the body is m = 2w and the matrix of inertia about frame fM g placed at the centroid of the body is H = 2G. If fM g is aligned with the principal axes of the body, then G is diagonal.
2.4 Projection onto SE (3)
We can use the norm induced by metric (7) to de ne the distance between elements in GA(3). Using this
distance, for a given B 2 GA(3), we de ne the projection of B on SE (3) as being the closest A 2 SE (3) with respect to metric (7). The following result is stated and proved in [4]: Proposition 2.1
block partition
Let B 2 GA(3) with the following
B = B01 B12 ; B1 2 GL(3); B2 2
IR
translation and exponential coordinates to
3
and U; ; V the singular value decomposition of B1 W :
B1 W = U V T
(9)
Then the projection of B on SE (3) is given by
T A = UV0 B12 2 SE (3)
(10)
Based on Proposition 2.1, a procedure for generating near optimal curves on SE (3) follows: generate the curves in the ambient space and project them on SE (3). Due to the fact that the metric we de ned in GA(3) is the same at all points, the corresponding Christo el symbols are all zero. Consequently, the optimal curves in the ambient space assume simple analytical forms (i.e geodesics - straight lines, minimum acceleration curves - cubic polynomial curves, minimum jerk curves - fth order polynomial curves, all parameterized by time). The resulting curve in GA(3) is linear in the boundary conditions, and therefore left and right invariant. Moreover, the projection on SE (3) is left invariant [4], and so is the overall procedure. Finally, note that due to the product structure of metric G~ , the rotational motion R(t) can be generated separately from the translational motion d(t) [14].
3 Optimal curves on SE (3) In this section we brie y describe the methods to generate optimal or near optimal interpolating motion between end poses (geodesics) or poses and velocities (minimum acceleration curves). In what follows, the given boundary conditions will be denoted by R0 ; d0 ; R_ 0; d_0 at t = 0 and R1 ; d1 ; R_ 1 ; d_1 at t = 1.
3.1 Minimum geodesics
energy
curves
G!_ (t) + !(t) (G!(t)) = 0 (11) d(t) = 0 (12) The solution to (12) is straightforward: d(t) = d0 + (d1 ? d0 )t; t 2 [0; 1]. In the particular case when G = I3 , equation (11) is easily integrable using left R(t) = R0 e!^ 0 t ; !^0 = log R0 T R1 If G 6= I3 , (11) does not have an analytical solution. A local parameterization of SO(3) should be chosen and three rst order di erential equations augment the system. A solution can be found using iterative procedures such as the shooting method or the relaxation method [10]. Alternatively, the projection method outlined in Section 2.4 can be used. In this case, the rotational part when t 2 [0; 1] is given by: [R0 + (R1 ? R0 )t]W = U (t)(t)V T ; R(t) = U (t)V T ;
3.2 Minimum acceleration curves
The di erential equations to be satis ed by the minimum acceleration curves on SE (3) equipped with metric (8) are derived for the case G = I3 in [14]:
!(3) + ! ! = 0 d(4) = 0
(13) (14) Again, the solution to the translational part (14) is immediate: d(t) = d0 + d_0 t + (?3d0 + 3d1 ? 2d_0 ? d_1 )t2 +(2d0 ? 2d1 + d_0 + d_1 )t3 (15) For the rotational part, a numerical method or the projection method should be used. If we choose the latter, there is no restriction on the form of G and the solution is given by R(t) = U (t)V T (t) where: M (t) = R0 + R_ 0 t + (?3R0 + 3R1 ? 2R_ 0 ? R_ 1 )t2 +(2R0 ? 2R1 + R_ 0 + R_ 1 )t3 (16) and U , V are given by the W - weighted SVD of M (t) as in (9).
-
4 Generation of smooth motion
The di erential equations to be satis ed by the geodesics on SE (3) equipped with metric (8) are derived in [14]:
Holonomic and nonholonomic robots (like airplanes) are considered in the next two sections. Before we start, two observations are in order. First, moving in
rigid formation is equivalent to generating trajectories with respect to fF g for each Oi in the form:
di (t) = d(t) + R(t)ri ; i = 1; : : : ; N
(17)
Second, if vi is the velocity of Oi in fMi g, the moving in rigid formation restriction can be written equivalently in terms of velocities in the form
vi (t) = Ri (t)T R(t)?(ri ) (t)
(18) where ri satis es equation (17) at some instant (say t = 0). 2 se(3) is the twist of the virtual rigid body and ?(ri ) is as in equation (2).
4.1 Optimal Motion Generation for Formations of Fully-actuated Robots
In this section we will assume that each robot can actuate each of its 6 degrees of freedom. Assume the initial (t = 0) and nal (t = 1) poses of each robot are given in frame fF g and the problem is to generate interpolating motion while keeping formation and minimize the total kinetic energy of the system. Obviously, the initial and nal poses should have the same formation, i.e. have the same ri 's. The total instantaneous kinetic energy is given by:
T=
N X i=1
iT G~ i i
N X i=1
!iT Gi !i + wi T ?T (ri )RT Ri RiT R?(ri )
Noting that RT Ri RiT R = I and total kinetic energy becomes
T=
N X i=1
PN
i=1 mi ri = 0, the
PN
i=1 (?mi r^i ) 2
0
PN
i
Z 0
0
i=1 mi I3
1
!iT Gi !i dt; i = 1; : : : ; N;
(ii) min
Z 0
1
~ T Gdt
The solutions to problems (i) are geodesics on SO(3) for metric Gi . The solution to problem (ii) is the geodesic on SE (3) for metric G~ . Both these problems can be solved as described in Section 3.1. We are now in the position to outline a procedure for generating smooth motion for each robot to interpolate between two given positions while preserving the formation and minimizing the kinetic energy. First, we locate the centroids Oi , attach the frames fMig and, from the geometric properties of each robot, calculate the inertia matrices Hi . If fMi g is aligned with the principal axes of robot i, then Hi is diagonal, but this is not necessary. If the initial and nal rotations Ri0 , Ri1 of robot i are given, then the rotation of the robot is the interpolating geodesic on SO(3) equipped with metric Gi = (1=2)Hi . Given d01 ; d02 ; : : : ; d0N at t = 0 and d11 ; d12 ; : : : ; d1N at t = 1, the initial and nal positions of the centroid of the ctitious rigid body are given by: PN
j i=1 mi di ; i=1 mi
PN
j = 0; 1
A frame fM g is xed to the virtual rigid body at its center of mass, which will give the initial and nal orientations of the formation, R0 and R1 . The ri 's are then determined by ri = R0 T (d0i ? d0 ), which will induce a metric G~ on the SE (3) of the formation, according to (19). The geodesic (d(t); R(t)) on the SE (3) of the formation with boundary conditions (d0 ; R0 ), (d1 ; R1 ) can be found using the relaxation method or the projection method. The trajectory of the centroid of each robot is nally determined in form (17).
4.2 Motion Generation for Formations of under-actuated robots
~ !iT Gi !i + T G
where G~ is dependent on the mass and the geometry of the formation by
G~ = 21
(i) min !
dj =
where i = [!iT viT ]T 2 se(3), the twist corresponding to the i-th robot is assumed in the vector form and G~ i is of the form described in equation (8) with Gi = (1=2)Hi and wi = (1=2)mi . Using equation (18), we get:
T=
Therefore, using the formation constraint, the problem reduces from dimension 6N to 3N + 6 and is separable into N + 1 independent subproblems:
(19)
In this section we assume that a subset of M from the N robots are under-actuated. Using the airplane model, suppose that robot i, i = 1; : : : ; M can only translate along the x-axis of fMi g. Besides the formation restriction (18), in this case we have an additional under-actuation constraint of the form C = 0, where
C is a 2M 6 matrix obtained by stacking 2 6 matrices of the form 0 1 0 RT R?(r ); i = 1; : : : ; M (20) i 0 0 1 i Therefore, the system of N robots moves while keeping formation fr1 ; : : : ; rN g and observing the nonholonomic constraints if and only if , the twist of the ctitious rigid body, is in the null space of matrix C given by (20). In other words, the formation of N robots of which M are under-actuated can move (i.e. 6= 0) if and only if the null space of matrix C is nonempty, or, equivalently, if rank(C ) 5. It is also interesting to note that the null space of matrix C is the intersection of the null spaces of each 2 6 block, which are all invariant under arbitrary rotations about the xaxis (the proof is straightforward). That is, replacing Ri with Ri Rot(x; ) in (20) leaves the null space of the matrix unchanged. The signi cant information about the individual orientations of the airplanes contained in the null space of C is, therefore, up to a rotation about the x axis of each airplane. As in Section 4.1, assume the initial (d01 ; R10 ); : : : ; (d0N ; RN0 ) and nal 1 1 (d1 ; R1 ); : : : ; (d1N ; RN1 ) poses of each robot are so that the rigid formation is preserved. If C 0 and C 1 denote the0 initial and nal C 1matrices, we assume that rank(C ) 5 and rank(C ) 5 (i.e, motion of formation is possible both at t = 0 and t = 1). We want to generate interpolating motion while keeping formation and observing the nonholonomic constraints so that the obtained curves are as smooth as possible. The nonholonomic constraints at t = 0 and t = 1 restrict at the boundaries, allowing 12 ? rank(C 0 ) ? rank(C 1 ) degrees of freedom. The projection method outlined in Section 3 will be used to generate interpolating motion for the virtual rigid body between the end poses and velocities. The obtained curves will be parameterized by the allowed degrees of freedom. Minimum energy is the criterion to select among the several choices. Explicitly, let fa1 ; : : : ; an g and fb1; : : : ; bmg (n; m 5) be the coordinates of 0 and 1 in some basis of the null spaces of C 0 and C 1 . If we write 0 = [!0 T v0 T ]T and 1 = [!1 T v1 T ]T , then R_ 0 = R0 !^ 0 , R_ 1 = R1 !^ 1 , d_0 = R0 v0 , d_1 = R1 v1 are all linear in ai 's and bi 's. The metric on the SE (3) of the virtual rigid body is determined by the mass properties of the robots and the geometry of the formation in the form given by (19). Minimum acceleration curves (M (t); d(t)) are generated in GA(3) with the corresponding metric W~ in the form (16), (15). The energy of the curve in GA(3)
E (ai ; bi ) =
Z 0
1
h
i
_ ) + wd_T d_ dt Tr(M_ T MW
is a positive de nite quadratic in ai 's and bi 's. Let ai and bi be the arguments minimizing E . The corresponding curve is then projected onto SE (3), as described in Section 3.2. The obtained (R(t); d(t)) will determine trajectories for each Oi using equation (17). The generated di (t); i = 1; : : : ; N have the property that the corresponding vi (0) and vi (1) are in accordance with the nonholonomic constraints. A position tracking algorithm [11] can be used for the M nonholonomic robots. The resulting tracking error will be zero. For the remaining N ? M holonomic robots, rotational motion can be generated separately as described in Section 4.1.
5 Examples A spatial formation of N = M = 5 identical airplanes is described in Figure 2. With the formation frame fM g xed at the centroid and oriented as shown in the gure, the geometry of the formation is described by: 2
h
4 5
r1 = 4 0 0
3
2
5
; r2 = 4
?ah 5
a2 2
2
3
3
?h 5 ; r = 4 ?a 5 ; a 5
3
2
2
2
? ha ?ah r = 4 ? 5; r = 4 ?a ?a 5
4
2
3
2
5
5
2
2
3 5
2
The inertial frame fF g is assumed to be coincident with fM g at t = 0. The geometry will induce metrics in SE (3) and GA(3) as follows: if m is the mass of each airplane, then the matrix G~ of the kinetic energy metric as de ned in (19) is given by
G~ = G0 wI0 3 ; G = m2 diagf2a2; a2 + 4h3 ; a2 + 4h5 g; w = 32m 2
2
The matrix of the metric W~ in the ambient manifold GA(3) is determined using (5) and (6). For the formation at t = 0, we have R10 = R20 = R30 = R40 = R50 = R0 = I3 ; d0 = [0 0 0]T Some elementary but tedious algebra gives the null space of the matrix C 0 in the form: Null(C 0 ) = span([0 0 0 1 0 0]T ) Therefore, as expected, the formation can move at t = 0 while observing the nonholonomic constraints only
z
x
y
y 1
z
x
1 h x
4h/5 z 2 z
z a
x
z
z
z x
a 5
z
z 2
y
y {M}
y
1
z
y
x
z
x x
x
5
x
y
2
5
y z
z
y x
3
z
y
x
4
{M}
z
y
x
a
y
y z
{M}
x
a
y
3
x y
y
x y
x
4
x y z
t=1 (a)
t=0
4
3 t=1 (b)
Figure 2: A formation of 5 identical airplanes. 30
30
1 2 3 4 5 O
25 20 15
30
1 2 3 4 5 O
25 20 15
20 15
10
10
10
5
5
5
0
0
0
−5 3
−5 3
−5 4
2
10
−1 0
−2 −3
−10
30
1
20
0
i)
2
30
1
30
2
20
0
ii)
1 2 3 4 5 O
25
iii)
0
−2 −3
20
0
10
−1 −10
10 −2
0 −4
−10
Figure 3: The generated position trajectory for airplane i, i = 1; : : : ; 5 and the curve described by the centroid of the virtual rigid body (O) for the case m = 1, a = 5, h = 10, X = 20, Y = 20: i) The airplanes are assumed holonomic; ii) End orientations as in case (a); and iii) End orientations as in case (b). by translating along the x axis of fM g. The formation at t = 1 is described by d1 = [X 0 Y ]T ; R1 = Rot(y; ?90) The cases (a) and (b) di er by the individual orientations of each airplane. In case (a), R11 = R21 = R31 = R41 = R51 = I3 while in case (b) R11 = Rot(y; ?90); R21 = Rot(z; ?135)Rot(y; ?45);
R31 = Rot(z; ?45)Rot(y; ?45); R41 = Rot(z; 45)Rot(y; ?45); R51 = Rot(z; 135)Rot(y; ?45): The corresponding null spaces are again 1dimensional: (a) Null(C 1 ) = span([0 0 0 0 0 1]T );
p
(b) Null(C ) = span([ a2 0 0 1 0 0]T ) 1
The interpolating minimum acceleration curves on the ambient manifold (M (t); d(t)), which depend on a1 and b1 as described in Section 4.2, are given by " 1 + f (t) 0 f (t) # 0 1 0 (a) M (t) = ?f (t) 0 1 + f (t)
a1 t ? Xf (t) + (b1 ? 2a1)t2 + (a1 ? b1 )t3 d(t) = 0 ?Zf (t) "
1 + f (t) g(t) f (t) 0 1 g(t) (b) M (t) = ?f (t) 0 1 + f (t) "
2
d(t) = 4
a1 t + 3Xt2 + (?2X + a1 )t3 0 (3Z ? b1 )t + (?2Z + b1 )t3 2
#
3 5
#
where
p
f (t) = ?3t2 + 2t3; g(t) = a2 b1 t2 (1 ? t) The energy is minimized along the above curves in metric W~ if (a) a1 = D; b1 = ?D; (b) a1 = ? 1948X ; b1 = 4(1 3+Z2m) The corresponding curve is then projected onto SE (3) as described in Section 3. The resulting position trajectories for cases (a) and (b) are given below for comparison together with the optimal curves which we would obtain if the airplanes were holonomic: Note that in cases ii) and iii), the end orientations are consistent with the nonholonomic restrictions. Also, in the holonomic case i), the trajectory of the centroid is a uniformly parameterized line, as expected.
6 Conclusion and future work We consider the trajectory generation problem for a formation of N arbitrarily shaped robots of which M are under-actuated. Our method yields trajectories that (nearly) minimize a measure of total energy while satisfying the nonholonomic constraints and maintaining a rigid formation. The resulting curves are invariant with respect to the choice of the inertial frame and parameterization. Our ongoing research is devoted to including changes in the shape of the formation to accommodate velocity constraints and to generating strategies and controllers for switching among pre-de ned formations.
References [1] B. d'Andrea Novel, G. Campion, and G. Bastin, \Control of nonholonomic wheeled mobile robots by state feedback linearization," International Journal of Robotics Research, vol. 14, no. 6, pp. 543{559, 1995. [2] R. W. Beard and F. Y. Hadaegh, \Constellation templates: an approach to autonomous formation
ying," in World Automation Congress, (Anchorage, Alaska), 1998. [3] C. Belta and V. Kumar, \An ecient geometric approach to rigid body motion interpolation," in ASME 2000 Design Engineering Technical Conferences, (Baltimore, MD), 2000.
[4] C. Belta and V. Kumar, \New metrics for rigid body motion interpolation," in Ball 2000 Symposium Commemorating the Legacy, Works, and Life of Sir Robert Stawell Ball, (University of Cambridge, UK), 2000. [5] J. P. Desai, J. P. Ostrowsky, and V. Kumar, \Controlling formations of multiple mobile robots," in Proc.IEEE Int.Conf.Robot.Autom., (Leuven, Belgium), pp. 2864{2869, 1998. [6] M. P. do Carmo, Riemannian geometry. Boston: Birkhauser, 1992. [7] R. Fierro, A. Das, and V. Kumar, \Hybrid control of formations of robots," in submitted to ICRA, 2001. [8] D. Fox, W. Burgard, H. Kruppa, and S. Thrun, \Collaborative multi-robot exploration," Autonomous Robots, vol. 8, no. 3, 2000. [9] F. Gentili and F. Martinelli, \Robot group formations: a dynamic programming approach for a shortest path computation," in Proc. of the IEEE Int. Conference on Robotics and Automation, (San Francisco, CA), 2000. [10] W. H. Press, S. A. Teukolsky, W. T. Vetterling, and B. P. Flannery, Numerical Recipes in C. Cambridge: Cambridge University Press, 1988. [11] P. Tabuada and P. Lima, \Position tracking for underactuated rigid bodies on SE(3)." preprint, 2000. [12] K. Tan and M. A. Lewis, \Virtual structures for high-precision cooperative mobile robot control," Autonomous robots, vol. 4, no. 4, pp. 387{403, 1997. [13] A. Yamashita, M. Fukuchi, J. Ota, T. Arai, and H. Asama, \Motion planning for cooperative transportation of a large object by multiple mobile robots in a 3d environment," in Proceedings of IEEE International Conference on Robotics and Automation, (San Francisco, CA), pp. 3144{ 3151, 2000. [14] M. Z efran, V. Kumar, and C. Croke, \On the generation of smooth three-dimensional rigid body motions," IEEE Transactions on Robotics and Automation, vol. 14(4), pp. 579{589, 1995.
