LAGRANGIAN MODELING FOR A CLASS OF MOBILE MULTIBODY SYSTEMS

LAGRANGIAN MODELING FOR A CLASS OF MOBILE MULTIBODY SYSTEMS Eric Conrado de Souza ∗,1 Newton Maruyama ∗ ∗ Av. Prof. Mello de Moraes, 2231, Dept. de Engenharia Mecatrˆ onica e de Sistemas Mecˆ anicos, POLI-Universidade de S˜ ao Paulo, S˜ ao Paulo, Brasil, 05508-030

Abstract: In what follows, we employ a geometrical approach to the modeling of a proposed class of multibody systems through the Lagrangian setting. This approach yields an interesting decomposition of the system equations of motion in the shape, or relative coordinate, space and the inertial coordinate space. Copyright c IFAC 2007 ° Keywords: Geometric mechanics, Systems with symmetry, Lagrangian modeling

1. INTRODUCTION The contemporary-age paradigm in robotics consists in pushing forward robotic systems capability for interaction with the environment, enhance dynamical performance, and experience controlled complex behavior by mimicking nature and, in particular, taking the various mechanisms in biology as inspirational templates for systems design. Under this motivational setting we propose to provide the modeling for a planar multibody or interconnected robotic system. Figure (1) gives a system description as one formed by N articulated rigid bodies. Notice the presence of external forces Fi and torques Ti on the system, for i = 1, . . . , N . These enable the system to being fully steerable on the inertial space in addition to possessing relative controllability, as proposed in (Souza, 2007). The system has the broad scope in aerospace (Dubowsky and Papadopoulos, 1993) and underwater (Yuh and West, 2001) robotic applications. The technical treatment taken below stems from the geometrical methods for classical mechanics. Some expressions presented here can be quite 1

Partially supported by the CAPES/Brasil.

overwhelming and demand extra time for a clear understanding, perhaps even from the experienced reader on the subject. Despite the apparent intricate mathematics behind the formulations, the geometrical concepts of the mechanics considered here are simple and the authors believe that this approach may lead to a more thorough and detailed yet-to-be-finalized analysis for the system at hand. It is a known fact that the planar system is characterized by having the Lie group SE(2) symmetry. System dynamical symmetry translates to an invariance of the associated Lagrangian function. This symmetry is exploited as a means of providing an elegant and systematical framework for dynamical modeling under the Lagrange d’Alembert formalism. 2. GEOMETRIC MECHANICS 2.1 Configuration space The configuration space Q of a planar rigid body is the special Euclidian group in the plane denoted by SE(2). 2 Because the groups SE(n) and 2

The group SE(n) is the semi-direct product SO(n)sRn .

F2

body N TN

body 1 T1

F1

T2 body 2

The equations of motion for system dynamics are given on the system state-space. In the Lagrangian setting the state-space is taken to be the tangent bundle T Q. Thus, for the configuration q = (r, θ), the state space T Q is given by (v, ω) ∈ Tq Q = Tq (R2 × S1 × . . . × S1 )

FN

Center of Mass

Fig. 1. Mobile multibody system.

2.2 System Lagrangian

SO(n) × Rn are isomorphic to each other, for any n ∈ N, the following mapping is naturally defined · ¸ R r SE(n) 3 7→ (R, r) ∈ SO(n) × Rn 01×n 1

The total kinetic energy KE for the planar system, composed of N bodies, is the sum of the individual kinetic energies KEk , (Bullo and Lewis, 2005). Thus, the system equations of motion are dictated by inertial dynamics only. For a rigid body B to the plane, any point of the body, in body coordinates, may be specified by X. If the body frame distance from the inertial frame is r, then the inertial position of any any body point is x = RX + r. The system Lagrangian L is solely determined by the total kinetic energy KE of the system, given the absence of potential and dissipative forces: Z N N X X 1 L= KEk = ρ(Xk )kx˙ k k2 d2 Xk , 2 Bk

where R is the body rotation matrix, and r the distance of its center of mass from the inertial frame origin. We will identify the elements of SE(n) with the elements of SO(n) × Rn . Moreover, we can identify the groups SO(2) and the circle S1 due to the isomorphism given below q SO(2) 3 R 7→ (x1 , x2 ) ∈ S1 ⊂ R2 , x21 + x22 = 1 Because S1 may be parameterized by the diffeomorphism f : [0, 2π] ∪ [−π, π] → S1 : θ 7→ (cos θ, sin θ), we will denote a point in S1 by θ. For a planar system composed of N free bodies, the configuration space is Qf ree = SE(2) × . . . × SE(2)

(N -times)

with configuration q ∈ Qf ree

k=1

Let rcm ∈ R2 denote the system’s center of mass position, given in the inertial reference frame. On a Riemannian context (with hh., .ii as metric), the kinetic energy metric is defined by hh., .iiG as:

q = ((R1 , r1 ), (R2 , r2 ), . . . , (RN , rN )) Alternatively, in the coordinates, q also takes the form, using vector notation   (r1 , θ1 )T   .. q=  ∈ Qf ree . T

(rN , θN )

From this point forward, we specialize the system to the interconnected one, which is made up by bodies physically coupled two-by-two to each other by hinges. This coupling defines the hinge constraint. Hence, the configuration space for the interconnected system Q ⊂ Qf ree simplifies to: Q = R2 × SO(2) × . . . × SO(2)

(N -times)

From the above, we parameterize Ri = R(θi ) ∈ SO(2) by θi ∈ S1 , and i = 1, . . . , N . Thus, Q may be written as: Q = R2 × S1 × . . . × S1

(N -times)

and consequently, for one system point distance from the inertial frame r ∈ R2 , q = (r, θ1 , θ2 , . . . , θN ) = (r, θ) ∈ Q. Let Tq Q be the tangent space over the configuration space Q at every q ∈ Q. The generalized ˙ velocity q˙ ∈ Tq Q at q, has coordinates (˙r, θ).

k=1

L(vq ) =

1 1 1 hhq, ˙ qii ˙ G = hhG q, ˙ qii ˙ = q˙T G q˙ 2 2 2 (1)

where G is the generalized system inertia matrix. Let g stand for the Lie algebra of the corresponding system G-group symmetry. The coordinate transformation between the inertial and body frames is given by the adjoint action Adg : g → g, which switches system velocity algebras between the body or inertial frame, for every group element g ∈ G.

2.3 The Momentum Map Recall that the momentum map J is a conserved quantity of motion for conservative systems and is defined as, refer to (Marsden and Ratiu, 1999) J : T Q → g∗ where g∗ is the dual of the Lie algebra g. The map J is a constant of motion 3 obtained from 3

A known result from Noether’s Theorem, see (Marsden and Ratiu, 1999).

the expressions of tangent lifts of actions, given by hJ, ξi(vq ) = hhξQ (q), vq iiG ,

vq ∈ T Q

where h., .i is the algebra pairing, ξ is an element of g, and ξQ is the infinitesimal generator on Q, i.e., the vector field of a flow in the configuration space Q induced, via the exponential map, by a path in the algebra g.

2.4 The Principal Connection Definition 1. (Principal Bundle). Let Φq be Lie group G-action on the configuration manifold Q. The space Q is isomorphic to the product of the quotient space Q/G with the group space or fiber G, or Q ' Q/G × G. The base space Q/G is also known as the shape space. In this product structure, an element of Q is coordinatized by (r, g), where r ∈ Q/G. The map π : Q → Q/G is a projection onto the first factor. We take the G-action as a left translation on the second factor. The 4-tuple (Q, π, Q/G, G) is a principal bundle or structure, see Figure 2. Definition 2. (Principal Connection). The principal connection A on a principal bundle π is a Lie algebra valued one-form connection, in the sense that, for all q ∈ Q and ξ ∈ g, A(ξQ (q)) = ξ and A(Tq Φg (vq )) = Adg A(vq ), that is, A is Adequivariant, (Bloch, 2003). The principal connection is defined, at q = (r, g), by the one-form A(q) = dgg

−1

+ Aloc (q)dr

where Aloc is the local form of the connection.

chanical connection on principal structures are reviewed 4 . For q = (r, g) ∈ Q, the locked inertial tensor, which is the inertia keeping all system relative coordinates fixed, is the equivariant map I(q) : g → g∗ defined by the pairing hI(q)η, ξi = hhηQ (q), ξQ (q)iiG ,

q∈Q

where η, ξ ∈ g, ηQ , ξQ are the corresponding infinitesimal generators, and hh., .iiG is the kinetic energy metric on Q. Definition 3. (The Mechanical Connection). The mechanical connection on the principal bundle Q → Q/G is defined to be the map A : T Q → g which assigns to every (q, vq ) ∈ T Q the corresponding angular velocity of the locked system. For vq = (r, ˙ g), ˙ the mechanical connection computation from the momentum map is made through A(q) · vq = I−1 (q)J(q, vq )

Q

q

(3)

One may be wish to obtain the corresponding body representation AB (q) of the system velocity A(q) through the adjoint operator Adg A(r, g) = dgg −1 + Aloc (r, g)dr = Adg (g −1 dg + Aloc (r)dr) = Adg AB (r, g)

(4)

From the above, Adg Ω = Adg AB (q)q, ˙ which renders the computation of the body frame velocity Ω = AB (q) · q˙ as Ω = g −1 g˙ + Aloc (r)r˙ = ξB + Aloc (r)r˙

(5)

Making use of the equivariance property of the locked inertia tensor I(q) map, we define Iloc as the local, G-fiber independent version for the locked inertia tensor on Q/G, given by Iloc (r) = I(r, e) = Ad∗g I(q)Adg

Φg (q)

(2)

(6)

where e is the group G identity element. On the other hand, we know that the momentum map J is a constant of motion and equal to µS . Using this fact and the definition of the mechanical connection in (3) with (6), it follows that A(q) · q˙ = I−1 (q)J = I−1 (q)µS ∗ = Adg I−1 loc (r)Adg µS

π

Q/G

(7)

Using (4) in this last expression results

r

Ad∗g µS = Iloc (r)Adg A(q) · q˙ = Iloc (r)Ω

Fig. 2. Principal bundle and connection.

Thus, noting that Ad∗g µS above is the corresponding momenta in body frame µB we define it, using (5) as µB = Iloc (r)(ξB + Aloc (r)r) ˙

2.5 The Mechanical Connection In this section, a few basic geometrical elements and constructions under the context of the me-

4

Application of the mechanical connection is usually found on the reduction theory of unconstrained systems with symmetry.

or equivalently, isolating ξB , we get the system velocity in body frame ξB = −Aloc (r)r˙ + I−1 loc (r)µB

(8)

In the paragraphs below, we will use this last expression to define a system constraint, and pursue the final equations of motion in regard to the associated constrained Lagrangian. 3. THE REDUCED VARIATIONAL PRINCIPLE In what follows, we give the result of dropping the dynamics on the phase space T Q to the quotient T Q/G by the induced action of G on T Q. For a G-invariant Lagrangian L the variational principle are reduced inducing reduced lagrangian l and corresponding variational principle. For systems with a symmetry group G, its Lagrangian L is invariant with respect to the Gaction on T Q, and it can be shown that the reduced tangent space T Q/G is isomorphic to g × T B which, in turn, induces a reduced Lagrangian l on T Q/G coordinatized by ξ ∈ g and r˙ ∈ T B. Thus, for a left invariant Lagrangian l(r, r, ˙ ξB ) = L(Lg (r, g), T Lg (r, ˙ g)) ˙ = L(r, g −1 g, r, ˙ g −1 g) ˙ = L(r, e, r, ˙ g −1 g) ˙ where ξB is the body Lie algebra element. In terms of the metric hh., .iiG0 , the system reduced Lagrangian l : T Q/G → R equals · ¸· ¸ 1 £ T T ¤ G011 G012 ξ l= ξ r˙ G021 G022 r˙ 2 This reduction yields the splitting of the dynamics into the Lie algebra fiber concatenated with the tangent base space g × T B and given by the Lagrange-d’Alembert derived equations, (Bloch, 2003): µ ¶ d ∂l ∂l − ad∗ξ a = λc ωbc gab + τb gab a dt ∂ξ ∂ξ µ ¶ d ∂l ∂l − i = λc ωic + τi dt ∂ r˙ i ∂r where τ is the forcing function which can be split on the principal bundle structure, with components in fiber and base space directions as τ = τb dg b + τi dri ∈ Tq∗ Q. Refer to (Marsden and Scheurle, 1993) and (Ostrowski, 1995) for details. Unlike previous works on multibody systems, we also consider the presence of forcing in the fiber directions and not only restricted to a single body, (Souza, 2007), which enables steering over the full state space. If we use (8) to define a constraint on the system frame velocity ξB , the function reduced constrained Lagrangian lc is induced on T B × g∗ lc (r, r, ˙ µB ) = l(r, r, ˙ ξB )|ξB

We are now ready to state the reconstruction equations of motion in body form, see (Ostrowski, 1995; Murray, 1997). These are deduced from the reduced nonholonomic variational principle which we reproduce here  ¡ ¢ −1  (9a)  g˙ = g −Aloc (r)r˙ + Iloc (r)µB , ∗ µ˙ B = adξ µB + τ , (9b)   T M (r)¨ r + r˙ C(r)r˙ + N (r) = X (r)τ (9c) The one form X on T ∗ Q gives (X (r)τ )i = τi − τe (Aloc (r))ei The input τ contribution in the momentum equation is τ a = τb gab , where gab is the lifted group action. We emphasize the decoupling of the momentum equation from the group variable g facilitates obtaining the motion on the fiber by integration of the momentum equation given an path in the base-space r(t). From the above set of equations and from the decomposition of the inertia matrix G defined in (1), the reduced inertia matrix is M (r) = G22 (r) − ATloc (r)Iloc (r)Aloc (r)

(10)

The reduced Coriolis and centripetal forces is computed from the tensor µ ¶ 1 ∂Mij ∂Mik ∂Mkj j k Cijk (r)r˙ r˙ = + − r˙ j r˙ k 2 ∂rk ∂rj ∂ri (11) And the system moment dependent N (r) term is ¿ À ∂l 1 ∂I−1 loc µB N (r) = , ad∗ξB Aloc (.) + dAloc (r, (.) ˙ .) + ∂ξB 2 ∂r 4. SYSTEM MODELING As an example of the theory reviewed previously, the modeling of a multi-body system of the class mentioned previously is presented below. We will assume system modeling neglecting the action of potential and dissipative forces, e.g., the influence of gravitational, and drag will not be considered. We also adopt friction-less joint hinges for the interconnection of bodies. In order to simplify the expressions, wherever present, the thruster forces are taken to act at the center of mass of the bodies and are not vectorized. One can show that expanding the system orientation vector θ, to give the joint angles φi = θi+1 −θi (i = 1, . . . , N − 1), by using matrix form gives      1 0 0 ··· 0 φ1 1 1 0 · · · 0   φ2       θ =  .  θ1 +  .  .   ..   .. · · · · · · · · ·  ..  1 1 1 ··· 1 φN = 1N θ1 + Mφ (12) where θ1 is the orientation of the reference frame on body 1 w.r.t. the inertial frame, the vector

φ contains the all system joint angles, 1N is a column vector with N ones and M is a N ×(N −1) matrix defined by   0, if i = 1 Mij = 1, if 1 ≤ j < i (13)  0, otherwise 4.1 2-body System

the momentum map. Let generalized velocity be ˙ T so, from (3): vq = (r˙x , r˙y , θ˙cm , φ) hJ(q, v), ξi = hhξQ (q), vq iiG ⇒ ˙ G ξQ (q) Ji ξ i = [r˙x r˙y θ˙cm φ] where Ji are the components of J and so   mr˙x  mr˙y J(vq ) =  m(rx r˙y − ry r˙x ) + 1TN J1N θ˙cm + J† φ˙

For a system composed of two interconnected bodies the full 4-dimensional space Q equals R2 × SO(2) × SO(2). A tangent bundle element is (q, q) ˙ = ((r1 , θ1 , θ2 ), (˙r1 , θ˙1 , θ˙2 )) ∈ T Q. Using the decomposition provided by the fiber bundle structure, the coordinates are q = (r, s) = (φ, g), where g = (rcm , θcm ) is the group element, the base space coordinate φ ∈ Q/G is the joint angle between the two bodies, and θcm = (π −φ)/2−θ1 . The coordinates on the reduced tangent space ˙ ξB ) ∈ (T Q)/G, and ξB ∈ (r, r, ˙ g −1 g) ˙ = (φ, φ, g. The tangent space over Q/G has coordinates ˙ ∈ T (Q/G) = S1 × R. (r, r) ˙ = (φ, φ)

To compute the connection (3)

Let the two system bodies be labeled 1 and 2. These are elliptical, homogeneously distributed masses where mi is the i-th body mass, di is the greatest distance from the i-body center of mass to its boundary. System total mass is m = m1 + m2 . System input is made up by two thrusters (F1 , F2 ) and a joint torque T . The symmetric, positive definite inertia matrix J for the 2-body system is, see (Sreenath, 1987): · ¸ I˜1 ²λ J= ²λ I˜2

The connection in body form follows directly from (3), and for Ad(r,R)−1 · ¸ RT r˙ AB (vq ) = Adg−1 A(vq ) = ˙ ˙ T J1N )−1 θcm + J† φ(1 N

where, for the relative angle between the bodies φ = θ2 − θ1 , λ(φ) = d1 d2 cos(φ) and also m1 m2 I˜i = Ii + ²d2i , ² = . m1 + m2 From (1), the system Lagrangian is 1 1 1 ˙ qii ˙ G = ω T Jω + mk˙rcm k2 L(vq ) = hhq, 2 2 2 where J and m are the system inertia matrix and total mass, respectively, and the matrix G gives   mI2 02×1 02×1 G = 01×2 1TN J1N J†  01×2 J† J‡ where J† = (I˜1 − I˜2 )/2 and J‡ = (I˜1 + I˜2 − 2²λ)/4. In order to compute the mechanical connection for the system we need first to obtain its locked inertia tensor. From (2), we know that T hI(q)η, ξi = hhηQ (q), ξQ (q)iiG = ηQ GξQ

and which renders the locked inertia tensor · ¸ mI2 −mMrcm I= −m(Mrcm )T mkrcm k2 + 1TN J1N The matrix M is the symplectic matrix, see (Marsden and Ratiu, 1999). Next we compute

A(vq ) = I−1 J(vq )   (r˙x + ry θ˙cm )1TN J1N − ry J† φ˙ 1 (r˙y − rx θ˙cm )1TN J1N + rx J† φ˙  = T 1N J1N 1TN J1N θ˙cm + J† φ˙ Now, we proceed with computations on the body frame. The coordinate frame transformation is given by the adjoint action Adg on se(2), which in matrix form is the following, see (Marsden and Ratiu, 1999): · ¸ R Mr Ad(r,R) = 01,2 1

Notice the above result has the following structure AB (vq ) = ξB + Aloc (φ)φ˙ The local form of the locked inertia tensor is obtained by (6) and gives ¸ · mI2 02×1 ∗ Iloc (r) = Adg I(q)Adg = 01×2 1TN J1N The body frame velocity, defined by the algebra element ξB in (8), given as a function of the body frame momentum µB = (µB1 , µB2 , µB3 ), is ξB = −Aloc (r)r˙ + I−1 loc (r)µB   · ¸ · ¸−1 µB1 1 02×1 ˙ mI2 02×1 µB2  =− T φ+ 01×2 1TN J1N J† 1N J1N µB3 The kinematic reconstruction equation may be obtained from g˙ = gξB as: · ¸  1 µB1  m R µB2   g˙ =   2µB3 − J† φ˙  21TN J1N The momentum equation may be determined by a (µ˙ B )c = (µB )b cbac ξB

where a, b, c = 1, 2, 3 and the nonzero structure constants for the se(2) algebra are c123 = −c132 = 1 and c231 = −c213 = 1 which gives:   3 µB2 ξB 3  µ˙ B =  −µB1 ξB 2 1 µB1 ξB − µB2 ξB

We are now ready to obtain the inertia matrix for the system dynamics on the base space. For r = φ we have, from (10): M (φ) = G22 (φ) − ATloc (φ)Iloc (φ)Aloc (φ) = MT JM − =

1TN JM (1TN J1N )

I˜2 I˜1 − (²λ(φ))2 (I˜1 + 2²λ(φ) + I˜2 )

The Centripetal and Coriolis term is obtained as a straightforward computation from (11) and from the expression for the inertia M (φ): C(φ) = (²d1 d2 sin(φ))

(²λ(φ) + I˜1 )(²λ(φ) + I˜2 ) (I˜1 + 2²λ(φ) + I˜2 )2

The remaining dynamics term N (φ) to be obtained is N (φ) =

²d1 d2 sin(φ) µ2B ˜ (I1 + 2²λ(φ) + I˜2 )2 3

And, finally, the forcing or input function X (φ)τ X (φ)τ = τφ − [τx , τy , τθ ]Aloc (φ) · ¸ 1 02×1 = τφ − [τx , τy , τθ ] T J† 1N J1N (I˜1 − I˜2 ) = τφ − τθ 2(I˜1 + 2²λ(φ) + I˜2 ) where τφ and (τx , τy , τθ ) are functions of thrusts (F1 , F2 ) and joint torque T . Plugging the last four expressions in (9c), results in the system equation of motion in the base space B M (φ)φ¨ + C(φ)φ˙ 2 + N (φ) = X τ ⇒ I˜2 I˜1 − (²λ(φ))2 ¨ φ+ (I˜1 + 2²λ(φ) + I˜2 ) (²λ(φ) + I˜1 )(²λ(φ) + I˜2 ) ˙ 2 (² sin(φ)d1 d2 ) φ + (I˜1 + 2²λ(φ) + I˜2 )2 ²d1 d2 sin(φ) µ2B = (I˜1 + 2²λ(φ) + I˜2 )2 3 (I˜1 − I˜2 ) τφ − τθ 2(I˜1 + 2²λ(φ) + I˜2 ) Observe, as seen in Section 3, that motion on the base space B is dictated by a second order dynamics. Full system dynamics may be reconstructed by appending to the above base space dynamics the body momentum µB dynamics and the kinematic G-group fiber equations.

5. CONCLUSION & FUTURE RESEARCH In the paragraphs above, a geometric approach in the Lagrangian setting for modeling systems with symmetry was reviewed with relative detail. The equations of motion derived were used to formulate the dynamics of the proposed multibody

system. In particular, the equations of motion were derived for a 2-body system. Current research efforts concentrates on the controllability analysis, trajectory generation and stabilization techniques for the system.

6. ACKNOWLEDGEMENTS The authors wish to thank CAPES for financial support and Prof. Naomi E. Leonard for useful comments on the subject and support during the first author’s one-year research visit to Princeton University.

REFERENCES Bloch, Anthony M. (2003). Nonholonomic Mechanics and Control. Vol. 24 of Interdisciplinary Applied Mathematics. Springer. Bullo, Francesco and Andrew D. Lewis (2005). Geometric Control of Mechanical Systems: Modeling, Analysis, and Design for Simple Mechanical Systems. Vol. 49 of Texts in Applied Mathematics. Springer. Dubowsky, Steven and Evangelos Papadopoulos (1993). The kinematics, dynamics, and control of free-flying and free-floating space robotic systems. IEEE Transactions on Robotics and Automation 9(5), 531–543. Marsden, J. E. and J¨ urgen Scheurle (1993). The reduced euler-lagrange equations. Fields Institute Communications (1), 139–164. Marsden, Jerrold E. and Tudor S. Ratiu (1999). Introduction to Mechanics and Symmetry. Vol. 17 of Texts in Applied Mathematics. 2 ed.. Springer. 3rd printing. Murray, Richard M. (1997). Nonlinear control of mechanical systems: A lagrangian perspective. Annual Reviews in Control 21, 31–45. Ostrowski, James P. (1995). The Mechanics and Control of Undulatory Robotic Locomotion. Ph.d. report. California Institute of Technology. Souza, Eric Conrado (2007). Modeling, Analysis, and Control of a Class of Mobile Multibody Systems. Tese de doutoramento (in preparation). Escola Polit´ecnica, Univesidade de S˜ao Paulo. Sreenath, N. (1987). Modeling and Control of Multibody Systems. Ph.d. report. University of Maryland. Yuh, J. and M. West (2001). Underwater robotics. Advanced Robotics 15(5), 609–639. Review paper.