Acta Mech. Sin. (2012) 28(2):482–489 DOI 10.1007/s10409-012-0050-6
RESEARCH PAPER
Coordinating control of multiple rigid bodies based on motion primitives Fan Wu · Zhi-Yong Geng
Received: 28 October 2010 / Revised: 19 March 2011 / Accepted: 25 May 2011 ©The Chinese Society of Theoretical and Applied Mechanics and Springer-Verlag Berlin Heidelberg 2012
Abstract This paper studies the problem of coordinated motion generation for a group of rigid bodies. Two classes of coordinated motion primitives, relative equilibria and maneuvers, are given as building blocks for generating coordinated motions. In a motion-primitive based planning framework, a control method is proposed for the robust execution of a coordinated motion plan in the presence of perturbations. The control method combines the relative equilibria stabilization with maneuver design, and results in a closeloop motion planning framework. The performance of the control method has been illustrated through a numerical simulation. Keywords Coordinating control · Motion plan execution · Motion primitive · Relative equilibrium · Maneuver 1 Introduction Coordinating control for multiple mechanical systems has attracted enormous research efforts during the past decade. Motivations for such research stem from the inherent strength and robustness in a coordinated group when dealing with tasks such as searching, surveying and mapping. For example, a fleet of underwater vehicles is used collectively for adaptive ocean sampling [1]. An important goal is to coordinate the motion of the vehicles so that the resolution of sensing array is optimized to minimize the estimation error in a sampled environment. Another application is the use of a cluster of satellites carrying telescopes for astronomical The project was supported by the National Natural Science Foundation of China (11072002, 10832006). F. Wu State Key Laboratory for Turbulence and Complex Systems, Peking University, 100871 Beijing, China e-mail:
[email protected] Z.-Y. Geng State Key Laboratory for Turbulence and Complex Systems, Peking University, 100871 Beijing, China
interferometry [2, 3]. The goal is to synchronize the attitudes of satellites so that using the telescopes together enhances the resolution. This paper studies the problem of coordinated motion generation for a group of rigid bodies. Coordination here refers to maintaining a prescribed relative configuration (orientation and position) in the group. Two classes of motion primitives, called relative equilibria and maneuvers, are introduced for motion generation. Relative equilibria exist due to the symmetries in the rigid body dynamics, and possess the property of having constant body-fixed velocities. Relative equilibria are natural and feasible trajectories which can be easily manipulated. A maneuver is defined as a controlled trajectory that begins and ends at relative equilibria. The central idea is to use motion primitives as building blocks for generating coordinated motion. The motion-primitive based idea originates from Ref. [4] where motion primitives are defined as equivalent classes of trajectories, induced by symmetries in the system’s dynamics, e.g. invariance with respect to time, translations, and rotations. A maneuver-based motion plan is given by concatenation of a finite number of motion primitives selected from a predefined library and implemented in a maneuver automaton. An application to motion planning for a group of unmanned aerial vehicles is presented in Ref. [5] where the multi-vehicle’s coordinated motion primitives are obtained from the single-vehicle’s motion primitives. However, this method combines motion primitives in an open-loop manner, which restricts its application to nominal scenarios, that is, those without perturbations. A control framework for maneuver-based motion planning which is robust to perturbations has been proposed in a recent extension work [6]. In this framework, we present a control method, which combines the relative equilibria stabilization with maneuver design, to robustly execute a motion-primitive based plan for a group of rigid bodies. Due to the importance of relative equilibria, there has been increasing research on the stabilization issues. See energy shaping as well as symmetry reduction methods in
Coordinating control of multiple rigid bodies based on motion primitives
Refs. [7–9] and references therein. However, despite partial results, no method is currently available to design provably stable maneuvers between two relative equilibria. The main contribution of this paper is the design of a stable maneuver between two relative equilibria. For the rigid-body group, the relative equilibrium is identified with its associated constant velocity. This velocity, expressed as a twist belonging to a Lie algebra, can be controlled to track a parameterized path interpolating two given points in the Lie algebra. State feedback is introduced into the parameterization of this path such that the transition between two points is completed in finite time while the state trajectory is kept bounded during the transition. The paper is organized as follows. Section 2 introduces notation and basic definitions regarding symmetries, relative equilibria and motion primitives. Section 3 introduces the dynamic model of a group of rigid bodies. Section 4 presents a control method implementing a motion-primitive based plan. Section 5 illustrates the control method with a numerical simulation. Section 6 summarizes the paper and outlines research problems for future work. 2 Preliminaries
483
where x ∈ Rn is the state, f (x) and h(x) are smooth functions on Rn , and u ∈ Rm is the control input. In general, we will focus on the system S that has symmetries. Roughly speaking, a symmetry on the system (1) is a group action on the state that leaves the dynamics invariant. Consider a finite dimensional Lie group G with the identity element id. A (left) action of the group G on Rn is a smooth map Ψ : G × Rn → Rn , such that Ψ (id, x) = x for all x ∈ Rn , and Ψ (g, Ψ (h, x)) = Ψ (gh, x) for all g, h ∈ G, x ∈ Rn . We will use Ψg (x) to denote Ψ (g, x). Given x0 ∈ Rn at t = 0, and u(t) ∈ Rm , let the solution of (1) starting from x0 under control u(t) denoted by x (t) = ϕu (t, x0 ). A pair (x(t), u(t)) is called the controlled trajectory. Now, we can introduce the following definition. Definition 1 [10] G is a symmetry group for S if for all x0 ∈ Rn , g ∈ G, and t ∈ R, such that ( ) Ψg (ϕu (t, x0 )) = ϕu t, Ψg (x0 ) . In other words, a symmetry group’s action on the state commutes with the state flow. Figure 1 illustrates the definition of symmetry.
2.1 Notation We set the notation used throughout the rest of the paper. R denotes the real numbers. R+ denotes the nonnegative real numbers, i.e. R+ = [0, ∞). Rn denotes the n-dimensional Euclidean space. The Euclidean norm in Rn is denoted as ∥ · ∥, and the absolute value of a scalar α is denoted as |α|. S O(3) is the 3-dimensional special orthogonal group consisting of orthogonal matrices with determinants equal to +1. The associated Lie algebra so(3) is the set of 3 × 3 skew-symmetric matrices. S E(3) is the Special-Euclidean group consisting of all rigid displacements in R3 . The element of S E(3) has a matrix representation [ ] R b , 0 1 where R ∈ S O(3), and b ∈ R3 . The associated Lie algebra se(3) is the set of twists which has a matrix representation [ ] ω ˆ v , 0 0 where ω ˆ ∈ so(3), v ∈ R3 , and the operator ˆ· is defined as ab ˆ = a × b for all a, b ∈ R3 with × the vector product. A function ρ : R+ → R+ is said to belong to class-K if it is continuous, zero at zero, and strictly increasing. 2.2 Systems with symmetries Consider a nonlinear control system S , described by the following set of controlled differential equations S : x˙ = f (x) + h (x) u,
(1)
Fig. 1 Symmetry illustration for a nonlinear system
2.3 Motion primitives Two motion primitives, called relative equilibrium and maneuver, are employed for motion planning in this paper. For the system S with a symmetry group G, let g be the Lie algebra associated with G, and exp(·) : g → G be the exponential map. For an element ξ ∈ g, the group action induced by ξ is written as Ψexp(ξα) (·), where α ∈ R is a scalar. Now, the notion of relative equilibrium can be defined. Definition 2 [4] If it is possible to find constants u¯ ∈ Rm , and ξre ∈ g such that ϕu¯ (t, x(0)) = Ψexp(ξre t) (x (0)) ,
∀x(0) ∈ Rn .
The resulting class of controlled trajectories (x, u) ¯ is called a relative equilibrium. ξre is called a trim velocity. u ¯ ∈ Rm is called a trim input. x is called a trim trajectory. According to Definition 2, a trim trajectory is a solution to the dynamics equation (1), and meanwhile a group orbit under the group action.
484
F. Wu, Z.-Y. Geng
Example 1 [4] Consider a mechanical system with no potential forces, and with configuration-independent nonconservative forces and control inputs
Under above definitions, there are two sets of motion primitives for the system S with symmetry group G, the relative equilibrium set, and the maneuver set.
q˙ = p,
2.4 Motion plans
p˙ = f (p, u),
where q, p ∈ Rn . The configuration manifold is G = Rn which is an Abelian Lie group, the group action of G on the state is Ψ∆q (q, p) = (∆q + q, p). The Lie algebra g associates with G is also Rn consisting of all momentums p. It is evident that any group action leaves the dynamics unchanged, thus G is a symmetry group. Relative equilibria correspond to constants (p, ¯ u) ¯ such that f (p, ¯ u) ¯ = 0, and result in trim trajectories of the form q(t) = q0 + pt, ¯
p(t) = p. ¯
The exponential map on R is the identity map. A more general kind of symmetry, that is invariance to transition and rotation about an axis in space, is exhibited by a large class of mechanical systems, e.g. human built vehicles. For these systems, the dynamics equation (1) has the form [ ] [ ] [ ] q˙ qξ 0 = + . (2) f(ξ) h(ξ)u ξ˙ n
The state x consists of the configuration q, and the velocity ξ which belongs to the Lie algebra associates with the symmetry group. The symmetry group action is on the configuration alone. f and h are smooth functions defined on the Lie algebra. In such a case, relative equilibria are constants (ξre , u) ¯ for which f(ξre ) + h(ξre )u¯ = 0,
p := {(ξ1 , τ1 ) , π1 , (ξ2 , τ2 ) , · · · , πN , (ξN+1 , τN+1 )} ,
(3)
where πi ∈ S M defines the i-th maneuver for i = 1, 2, · · · , N; ξi ∈ g defines the i-th trim velocity of the i-th relative equilibrium ξi ∈ S T for i = 1, 2, · · · , N + 1; τi defines the time spent executing the i-th relative equilibrium. The final state after the execution of Eq. (3) is given by x f = Ψg p (xξN+1 (0)), where the group displacement g p can be computed as [10] N ∏ g p = exp(ξi τi )gπi exp(ξN+1 τN+1 ) i=1
= gm
N+1 ∏
exp(ηi τi ).
(4)
i=1
In Eq. (4), gm is the group displacement corresponding to the motion plan {(ξ1 , 0) , π1 , (ξ2 , 0) , · · · , πN , (ξN+1 , 0)} , and the vector field ηi is defined implicitly by N ∏ gπ j , ηi , i = 1, 2, · · · , N, ηN+1 = ξN+1 , ξi = Ad j=i
where Ad(g, ζ) := gζg−1 for all g ∈ G, ζ ∈ g. The advantage of this motion-primitive based planning mechanism is that all generated motion plans are implicitly feasible with respect to the dynamics of the system. 3 Rigid body group
i.e. equilibria for the reduced dynamics evolving on the Lie algebra. The rigid body model we studied in Sect. 3 belongs to this case. Another motion primitive, called maneuver, is defined as a finite time transition between two given relative equilibria. Definition 3 [10] A controlled trajectory π : t ∈ [0, T ] → (x(t), u(t)) is a maneuver if there exists two trim trajectories x1 : [0, τ1 ] → Rn , x2 : [0, τ2 ] → Rn that are compatible with x, i.e., there existing two displacements g1 , g2 ∈ G such that x(0) = Ψg1 x1 (τ1 ),
A motion plan is denoted by a sequence [10]
x (T ) = Ψg2 (x2 (0)) .
Because of the symmetry of the dynamics, the displacement on the symmetry group due to the maneuver, g := g−1 1 g2 ∈ G is invariant, and is a constant characterizing the maneuver, satisfying x(T ) = Ψg (x(0)).
In this section, we consider a group of n identical rigid bodies coordinated by a coupling potential and discuss the motion planning for the coordinated group. 3.1 Coordinated dynamics Let (Ri , bi ) ∈ S E(3) be the configuration of the i-th rigid body. The angular velocity of the i-th rigid body in the inertial frame is denoted by Ωi ∈ R3 , and in its body frame is denoted by ωi ∈ R3 . The linear velocity of the i-th rigid body in the inertial frame is denoted by b˙ i ∈ R3 , and in its body frame is denoted by vi ∈ R3 . The kinematics of the i-th rigid body is ˙ i = Ri ω R ˆ i,
b˙ i = Ri vi .
(5)
The state space of the n-rigid-body group is n copies of T (S E(3)) := S E(3) × se(3), denoted by X = T (S E(3)n ). To
Coordinating control of multiple rigid bodies based on motion primitives
study the symmetry of the rigid-body group’s dynamics conveniently, we use the configuration and body fixed velocity to represent the state of the group as x = (R1 , b1 , R2 , b2 , · · · , Rn , bn , ω ˆ 1 , v1 , ω ˆ 2 , v2 , · · · , ω ˆ n , vn ) . Let J ∈ R3×3 be the momentum of inertial matrix and M ∈ R3×3 be the mass matrix of each rigid body in the body frame. We assume that the mass is distributed uniformly and the body frame is chosen so that both J and M are diagonal. We also assume that both J and M are independent of configurations Ri , bi , for i = 1, 2, · · · , n. If each rigid body is not subject to any potential force, the Lagrangian of the total group equals its kinetic energy ) 1 ∑( T ωi J ωi + viT M vi . 2 i=1 n
LK =
Note that the Lagrangian LK is independent of Ri , bi , the rigid-body group has n copies of S E(3) symmetry under the block-diagonal group action. As mentioned in the introduction, coordination in this paper refers to maintaining relative configurations in the group, thus requires to break n − 1 copies of S E(3) symmetry, and leave only S E(3) symmetry. To coordinate, we introduce a coupling potential which has S E(3) symmetry as follows n n ∑ T σ2 ∑
b˜ − b
2 , ˜ i + VC = σ1 trace R1 R i 1 2 i=2 i=2 ˜ i = K1i Ri , where the real constants σ1 < 0, σ2 > 0, and R ˜bi = bi − d1i , for i = 2, 3, · · · , n. The constant vectors d1i ∈ R3 and constant matrices K1i ∈ S O(3) determine the relative configuration between the 1st and the i-th rigid body when at equilibrium. The Lagrangian of the coupled group is L = LK − VC n ) 1 ∑( T = ωi J ωi + viT M vi 2 i=1 n n ∑ T σ2 ∑
b˜ − b
2 . ˜ −σ1 trace R1 Ri − i 1 2 i=2 i=2
(6)
485
The dynamics Eqs. (5) and (7) can be written in the form (2), a special form of (1), as [ ] [ ] [ ] q˙ q⊙ζ 0 Sn : ˙ = + , (8) c (ζ) u Iζ ([
] [ ] [ ]) R1 b1 R2 b2 Rn bn where q = , ,··· , , u 0 1 0 1 0 1 ([ ] [ ] [ ω ˆ 1 v1 ω ˆ v ω ˆ control input, ζ = , 2 2 , ··· , n 0 0 0 0 0 and ⊙ denotes the component-wise product between ζ, i.e.,
is the ]) vn , 0 q and
([
] [ ] [ ] R1 b1 ω ˆ 1 v1 ω ˆ 2 v2 q⊙ζ : = · , ,··· , 0 1 0 0 0 0 [ ] [ ]) Rn bn ω ˆ n vn · . 0 1 0 0 I denotes the inertial tensor of the whole rigid-body group, c(ζ) is the Coriolis term associates with I. 3.2 Coordinated motion primitives To study the behavior of the coordinated rigid-body group conveniently, we use the relative configurations and body fixed velocities to represent the state as ( ) ˜ 1 , b˜ 1 , R ˜ 2 , b˜ 2 , · · · , R ˜ n , b˜ n , ω x˜ = R ˆ 1 , v1 , ω ˆ 2 , v2 , · · · , ω ˆ n , vn . The group action of S E(3) acts on relative configurations alone ( ˜ 1 , R˜ ˜ 2 , Rb˜ 2 + b, · · · , Ψ(R,b) x˜ = RR b1 + b, RR ) ˜ n , Rb˜ n + b, ω RR ˆ 1 , v1 , ω ˆ 2 , v2 , · · · , ω ˆ n , vn , where (R, b) ∈ S E(3). Therefore, Lagrangian (6) admits ( ) L Ψ(R,b) x˜ = L(x). ˜ It can be verified that system S n has S E(3) symmetry. This physically means that the coordinated dynamics are invariant to any translation and/or rotation of the inertial frame. Lemma 1 The coordinated dynamics (8) possess a family of relative equilibria as
The coordinated dynamics is defined as the Lagrangian dynamics of Lagrangian equation (6). The equations of motion can be derived using variation principle, we only state the result as follows
(ω ˆ 1 , v1 ) = (ω ˆ 2 , v2 ) = · · · = ( ω ˆ n , vn ) = ξre ,
(9a)
(R1 (t), b1 (t)) = Ψexp(ξre t) (R10 , b01 ),
(9b)
Jω ˙ i = (J ωi ) × ωi + (M vi ) × vi − gradRi VC , M v˙ i = (M vi ) × ωi − gradbi VC .
Ri (t) = K1iT R1 (t),
(9c)
(7)
The negative gradient of the coupling potential with respect to each rigid body’s attitude and position are interpreted as coordinating controls cp
uτi = −gradRi VC ,
cp
u f i = −gradbi VC .
bi (t) = b1 (t) + d1i ,
0 0 for i = 2, 3, · · · , n, ( with R)1 (0) = R1 , b1 (0) = b1 , K1i = exp(eˆ j λ), ξre = αeˆ j , βe j , j ∈ {1, 2, 3}, where λ ∈ R, α, β ∈ R, d1i ∈ R3 , {e1 , e2 , e3 } is the standard basis for R3 , and t is the time variable.
486
F. Wu, Z.-Y. Geng
Proof Recall that both J and M are diagonal, and the coupling potential VC is at minimum when R1 (t) = K1i Ri (t), b1 (t) = bi (t) − d1i ,
i = 2, 3, · · · , n.
4 Motion plan execution For the coordinated motion plan in Eq. (10), we present a control method for executing it stably in the presence of perturbations.
Direct substitution of Eq. (9) into Eq. (7) shows that 4.1 Relative equilibria stabilization cp cp ˙ˆ i = v˙ i = 0, uτi = u f i = 0, ω
i = 1, 2, · · · , n.
Therefore, ξre ∈ se(3) is the trim velocity, and (9b), (9c) are trim trajectories. Equation (9a) indicates that each rigid body translates while rotates along any of its principle axes, say e j , with a common angular speed α and linear speed β. Equation (9c) indicates that relative orientations are up to constant rotations around e j , and relative positions also maintain constant. Relative equation (9) describe synchronized, steady motions of all rigid bodies in a formation, and ξre is the trim velocity of the formation. With different choices of α, β, λ, and d1i , Eq. (9) represent various coordinated motion primitives for the rigid-body group. Defined as a transition between two relative equilibria, a maneuver for the rigid-body group can be an acceleration or deceleration along a trim trajectory, a switch between two trim velocities, and/or a formation reconfiguration. 3.3 Coordinated motion planning In the remainder of this paper, we restrict our relative equilibria selection to Eq. (9). Consider a trim-maneuver-trim motion plan {(ξ1 , τ1 ), π1 , (ξ2 , τ2 )} ,
(10)
where ξ1 , ξ2 ∈ se(3) are trim velocities. Using the formula (4), the corresponding state trajectory is ( ) Ψexp(ξ1 t) xξ1 (0) , t ∈ [0, τ1 ], ( ) [ ] Ψ xπ1 (t − τ1 ) , t ∈ τ1 , τ1 + T π1 , exp(ξ1 τ1 )g−1 1 ( ) Ψexp(ξ1 τ1 )gπ exp(ξ2 (t−τ1 −Tπ )) xξ2 (0) , t > τ1 + T π1 , 1 1 where g1 is the matching displacement, gπ1 is the maneuver displacement. Assumption 1 On the Lie algebra se(3), there exists a smooth path ξ(s): [0, S ] → se(3), parameterized by a time-varying parameter s(t) : [0, T ] → [0, S ], satisfying ξ(0) = ξ1 , ξ(S ) = ξ2 . Remark 1 Assumption 1 is reasonable since Lie algebra se(3) is a vector space. A smooth path on se(3) can be constructed by a linear interpolation ( s s) ξ(s) = ξ2 + 1 − ξ1 , s ∈ [0, S ], S S ξ(s) = ξ2 , s > S . Intuitively one might set s = t, but we leave the variation of s as a degree of freedom for the stable maneuver design in the next section.
In a nominal case, the trim input is kept constant along the trim trajectory. But in a perturbed case, a local, asymptoticalstabilizing controller for the trim trajectory is required. Let ξ(s): [0, S ] → se(3) be a smooth path satisfying Assumption 1. A point on the path is denoted by ξ s , and the corresponding trim trajectory is denoted by xξs . Assumption 2 There exists a continuously differen+ tiable, positive ( ) definite function V(s, x): [0, S ] × X → R with V s, xξs = 0; a class-K function ρ; a constant C > 0; an upper bound VU on the value of the function V, such that the set {(s, x)|V(s, x) 6 VU } is compact, and there exists a continuous feedback control u(s, x): [0, S ] × X → Rm , such that for each fixed s ∈ [0, S ], − (∂V/∂x)T x˙ > C, ρ (V(s, x))
{∀x ∈ X|V(s, x) 6 VU } .
(11)
Remark 2 Assumption 2 guarantees that for each ξ s on ξ(s), the corresponding trim trajectory xξs is asymptotically stabilizable. Under Assumption 2, the function V(s, x) qualifies as a Lyapunov function and can be constructed based on the total energy (kinetic energy plus coupling potential) of the rigidbody group, such that V has its minimum along the corresponding trim trajectory, and has a positive-definite second variation in a neighborhood of this trim trajectory. Then, the energy based method in Ref. [11] can be used to design stabilizing control u to make V decay to zero asymptotically. 4.2 Maneuver execution Under Assumptions 1 and 2, we identify two relative equilibria (ξ1 , τ1 ) and (ξ2 , τ2 ) with associated trim velocities ξ1 and ξ2 , respectively. The maneuver is executed by controlling the variation of s-parameter along the path ξ(s), such that the Lyapunov function V(s, x) is bounded during the maneuver as well as the convergence of the rigid-body group’s state to the final relative equilibria. As the parameter s(t) varies along ξ(s), the Lyapunov function V(s, x), and the stabilizing control u(s, x) become time dependent during the maneuver. To ensure a stable maneuver, state feedback is introduced into the variation of s. Using the time scaling technique in Ref. [12], we have the following result. Theorem 1 Designate a changing rate v s > 0 for s, let ( ) − (∂V/∂x)T x˙ ρ(VU ) s˙ = min v s , , λ + |∂V/∂s| ρ(V(s, x))
(12)
Coordinating control of multiple rigid bodies based on motion primitives
with addition that s˙ = 0 at the endpoint s = S , and λ > 0 is a small positive constant, ρ is the class-K function in Eq. (11). Then s reaches S in finite time. If V(s(0), x(0)) 6 VU , then V(s(t), x(t)) 6 VU , ∀t > 0. Proof It follows from direct computation that ( )T ∂V ∂V V˙ = x˙ + s˙. ∂x ∂s If ∂V/∂s 6 0, then V˙ 6 (∂V/∂x)T x˙ 6 0 under Assumption 2. If ∂V/∂s > 0, then ( )T ( ) ∂V ∂V − (∂V/∂x)T x˙ ρ (VU ) ˙ V6 x˙ + . ∂x ∂s λ + |∂V/∂s| ρ (V (s, x)) Assume V (s(t1 ), x(t1 )) > VU at time t1 ∈ [0, T ], then ρ (VU ) 6 1. ρ (V (s (t1 ) , x (t1 )))
487
The configuration space is S E(2) with coordinates (θi , bix , biy ),(where )θi is the angle the i-th UV forms with the horizontal, bix , biy is the position of its center of mass. The UV’s velocity, expressed in) the body frame, is ξi ∈ se(2) ( with coordinates ωi , vix , viy . Let two UVs be coordinated by the potential
2 σ1 σ2
˜
b2 − b1
, (θ1 − θ2 )2 + VC = 2 2 where bi = [bix , biy ]T ∈ R2 , i = 1, 2, σ1 , σ2 ∈ R+ , and b˜ 2 = b2 + d, d ∈ R2 . The coordinated UV group has a relative equilibrium ξre = [0, 1, 0]T ,
θ1 = θ2 = 0, b1 = b2 + d.
(14)
Relative equilibrium (14) corresponds to both UVs moving along their x-axes with unit speed, aligning each other, and maintaining a constant relative position. A Lyapunov function for relative equilibrium (14) is 1∑ (ξi − ξre )T I (ξi − ξre ) 2 i=1
2 σ1 σ2
˜
b2 − b1
, + (θ1 − θ2 )2 + (15) 2 2 where I = diag(J, m x , my ). As long as m x > my , a steady motion along UV’s x-axis is Lyapunov stable as well as exponentially stabilizable [7]. Specifically, Eq. (14) can be exponentially stabilized by controls 2
Since ∂V (s, x)|t1 /∂s 6 1, λ + ∂V (s, x)|t1 /∂s and t1 is arbitrarily chosen, this gives ( )T ( )T ∂V ∂V V˙ 6 x˙ − x˙ = 0. ∂x ∂x Therefore, if V (s, x) > VU , then V˙ (s, x) 6 0 along the trajectories. Thus V(s(t), x(t)) 6 VU for all t > 0 if V(s(0), x(0)) 6 VU . Since the set {(s, x)|V(s, x) 6 VU } is compact under Assumption 2, let ( ) ∂V (s, x) . K= max λ + s∈[0,S ],x∈X|V(s,x)6VU ∂s Using Eq. (11), ( ) − (∂V/∂x)T x˙ ρ (VU ) s˙ = min v s , λ + |∂V/∂s| ρ (V (s, x)) ( ) Cρ (VU ) > min v s , = v > 0. K ∫T The completion time T can be defined as 0 d s˙ = S . v is the lower bound on s˙, the upper bound for T is M = S /v. 5 Application to S E 2 (2) case We illustrate the proposed planning method by a numerical simulation. Consider a group of two identical underwater vehicles (UVs) on a plane. The dynamics of the i-th UV are ( ) θ˙i = ωi , Jω ˙ i = m x − my vix viy + uiτ , b˙ ix = vix cos θ − viy sin θ, m x v˙ ix = my viy ωi + uix , (13) b˙ iy = vix sin θ + viy cos θ, my v˙ iy = −m x vix ωi + uiy .
V =
u1τ = −σ1 θ1 − κJ θ˙1 , ] ( ( [ ]) [ ]) u1x 0 0 T (θ ) = −σ2 R 1 b1 · u1y 1 1 [ ] ( [ ]) mx 0 1 T ˙ −κ R (θ1 ) b1 − , 0 my 0 u2 = −σ1 (θ2 − θ1 ) − κJ θ˙2 , [ 2 τ] ( ( )) ux T ˜ 2 = −σ2 R (θ2 ) b2 − b1 uy [ ] ( [ ]) m 0 1 −κ x RT (θ2 ) b˙ 2 − , 0 my 0 [ ] cos θ − sin θ where R(θ) := . sin θ cos θ Numerical values are expressed in SI unit as m x = 4, my = 1, J = 1.5. Control parameters are σ1 = 2, σ2 = 4, κ = 4, d = [0, 5]T . Initial conditions are 0.1 ω1 π/7 θ1 b1x = 0 , v1x = 0.1 , 0.1 v1y t=0 4 b1y t=0
[
π/4 θ2 b2x = 0 , −9 b2y t=0
0.1 ω2 v2x = 0.1 . 0.1 v2y t=0
Figure 2 shows the stabilization of relative equilibrium with the perturbation of initial conditions.
488
F. Wu, Z.-Y. Geng
In this case, we relax the upper bound for the Lyapunov function V and increase the designated changing rate for s, we choose v s = 0.1, λ = 0.01, ρ (z) = z2 , VU = 5. As shown in Fig. 4, the parallel formation is broken during the maneuver but maintained before and after it. The maneuver accelerates (achieves a larger displacement during the same interval compared with Fig. 3, and each UV approximately executes the same maneuver (likewise consumes the similar energy), while in Fig. 3), the UV below must move faster than the above one to maintain a parallel formation during the maneuver.
Fig. 2 Stabilization of the relative equilibrium
{ ′ } Now consider a motion plan ξre , π, ξre ′ ξre = [0, 1.2, 0]T , [ ] ( ) π 0 T π (b1 − b2 ) = θ1 = θ2 = , R . 5 10 10
Let ξ(s): [0, S ] → se(2) be a smooth path with ξ(0) = ξre ,
′ ξ(S) = ξre .
The variation of s-parameter is controlled according to Eq. (12). To maintain a parallel formation during the maneuver, we choose v s = 0.05, λ = 0.01, ρ (z) = z2 , VU = 0.5. Figure 3 shows the execution of motion plan with the same perturbation of initial conditions as in the former example.
Fig. 4 Cross-over maneuver in a motion plan
Compared with motion planning methods based on optimal theory, the proposed planning method results in a closeloop mechanism as opposed to an open-loop mechanism in an optimal based motion planning method. Both the trim trajectory and maneuver are stabilized via feedback, thus the generated motion plan is more robust. Moreover, the computation and store of motion primitives do not expand extremely as the number of rigid bodies increases, since the coordinated motion primitives for the rigid-body group are composed of each rigid body’s motion primitives. While the computation cost of an optimal based motion planning method is not scalable with respect to the number of rigid bodies. The disadvantage of the proposed method is the restriction of all admissible trajectories to sequential combinations of a finite number of motion primitives. The overall optimality can not be proved or computed with respect to cost functions like consumed energy or time.
Fig. 3 Execution of the motion plan
6 Conclusions
Next, consider another relative equilibrium with a large heading angle and a contracted relative distance given by
In this paper, we have considered the problem of how to obtain coordinated motions by concatenating motion primitives for a group of rigid bodies. We introduce a motion-primitive based planning framework for nonlinear systems with symmetries, and apply it to a rigid-body group. Based on available results on the relative equilibria stabilization, a control method is proposed for the robust execution of a coordinated
′′ ξre = [0, 1.2, 0]T ,
θ1 = θ2 =
π , 3
RT
(π) 3
(b1 − b2 ) =
[ ] 0 . 4
Coordinating control of multiple rigid bodies based on motion primitives
motion plan in the presence of perturbations, and results in a close-loop mechanism. The performance of the control method has been illustrated by a numerical simulation. Ongoing research work includes applying the motion primitive based planning framework to a group of under actuated mechanical systems with symmetries. References 1 Fiorelli, E., Leonard, N. E., Bhatta, P., et al.: Multi-auv control and adaptive sampling in monterey bay. IEEE Journal of Oceanic Engineering 31, 935–948 (2006) 2 Bondhus, A. K., Pettersen, K. Y., Gravdahl, J. T.: Leader/ follower synchronization of satellite attitude without angular velocity measurements. In: Proc. IEEE Conference on Decision and Control and European Control Conference, 7270–7277. Seville, Spain (2005) 3 VanDyke, M. C., Hall, C. D.: Decentralized coordinated attitude control of a formation of spacecraft. Journal of Guidance, Control and Dynamics 29, 1101–1109 (2006) 4 Frazzoli, E.: Robust hybrid control for autonoumous vehicle motion planning. [Ph.D. Thesis], Massachusetts Institute of Technology, Cambridge, MA (2001) 5 Frazzoli, E.: Maneuver-based motion planning and coordina-
489 tion for multiple uavs. In: Proc. AIAA/IEEE Digital Avionics Systems Conference, 1–12 (2002) 6 Sanfelice, R. G., Frazzoli, E.: A hybrid control framework for robust maneuver-based motion planning. In: Proc. American Control Conference, 2254–2259. Seattle, WA (2008) 7 Bullo, F.: Stabilization of relative equilibria for underactuated systems on riemannian manifolds. Automatica 36, 1819–1834 (2000) 8 Hanßmann, H., Leonard, N.E.: Symmetry and reduction for coordinated rigid bodies. European Journal of Control 2, 176–194 (2006) 9 Nair, S., Leonard, N. E.: Stable synchronization of mechanical system networks. SIAM Journal on Control and Optimization 47, 661–683 (2008)
10 Frazzoli, E., Dahleh, A., Feron, E.: Maneuver based motion planning for nonlinear systems with symmetries. IEEE Transactions on Robotics 21, 1077–1090 (2005) 11 Nair, S., Leonard, N. E.: Stable synchronization of rigid body networks. Networks and Heterogeneous Media 2, 595–624 (2007) ¨ 12 Ogren, P., Fiorelli, E., Leonard, N. E.: Cooperative control of mobile sensor networks: Adaptive gradient climbing in a distributed environment. IEEE Transactions on Automatic Control 49, 1292–1302 (2004)
