Modelling, Simulation and Visualisation of Multibody Dynamics

(1)

Visualisation of Multibody-Satellite

Dynamics

UNIVERSITY OF PISA

Department Of Aerospace Engineering

Master Degree in Space Engineering

Francesco Infante

Supervisor:

Prof. Ing. Giovanni Mengali

Conducted at:

Astrium GmbH - Germany Supervisor:

Dipl.-Ing. Patrick Bergner

(2)

(3)

La migliore compagnia che la vita

potesse donarmi.

(4)

(5)

Acknowledgements

It is a pleasure for me to thank all the people who supported me during my last six months.

Among all, a person I want to thank is Dipl.-Ing. Patrick Bergner. Thank you for giving me the opportunity to work on a very interesting project that covered a big area of my study. Thank you for the support and the advice, and of course for your patience, given during these six months in Friedrichshafen.

I would like to thank also the colleagues of Astrium with whom I spent a great time. It was a pleasure for me to meet you all. I hope to work once more with you in the future. In particular, thanks to Alexandre Hiri-coiu, Jennifer Nan, Susi Alpen, Deborah Paglialunga and Seth Wilkenfeld for their amazing and needed help in drafting this document.

I am grateful to thank Prof. Giovanni Mengali for giving me the oppor-tunity to put my studies into practice in a company like Astrium.

Finally a special thanks goes to all the guys of the “Crew” in the Bo-densee. I never imagined it would have been an experience like that. Thanks for the support, and all the moments that you have given me. Whereever in the world you are now - THANK YOU.

(6)

(7)

Chapter 1 Introduction

1.1 Motivation of Thesis

The study of multi-body dynamics is a wide field within mechanics. De-scribing the motion of a multi-body systems has been discussed for several centuries by various physicists. Nevertheless, it is difficult to find a generic modelling approach which can be applied to arbitrary topologies of multi-body systems.The thesis at hand presents such an approach for the rigid multi-body dynamics commonly used to model multi-body satellite configurations. Due to the complexity of state-of-the-art satellites (solar array pointing mecha-nisms, robotic arms, antenna pointing mechamecha-nisms, reaction wheels) a big improvement would be a software tool to support the modelling and simula-tion of such complex systems.

The tool should allow to formulate the problem in an abstract way in order to be able to treat generic kinds of multi-body systems.

A multi-body system is used to model the dynamic behavior of intercon-nected rigid or flexible bodies, each of which may undergo large translational and rotational displacements. There is no good and completely general de-scription for modelling such system, in particular, using the Newton-Euler Method. This is the reason for the thesis. We would establish a method to derive the nonlinear equations of motion of a complete satellite from

(12)

sic information (geometrical data, center of gravity, MoI, Frames, type of joints) about all of its parts. In this way, we can define and figure out the entire dynamics of a S/C in order to have a proper description of the satellite dynamics. The dynamics description is essential when addressing the S/C dynamics during the device’s design phase.

The work is based on the study of the single bodies and how they are con-nected. The only information needed to run a simulation is related to their geometric structure, forces and torques.

The goal of this document is to develop and describe algorithms in suffi-cient detail that the reader may, given properties of a multi-body system, assemble by inspection exact, nonlinear dynamical equations of motion suit-able for numerical integration by computer. Mathematica R_{was chosen as the}

software to implement the tool in. The reason for the choice to use Mathemat-ica R_{instead of other software like MATLAB} R_{is because we are interested in}

an analytical approach. In fact, Mathematica R_{for our work is more intuitive}

and easy to use with respect to (used in MATLAB R_{). Furthermore, the}

vi-sualisation capabilities of Mathematica R_{are better for our purpose than the}

(13)

1.2 Examples of Multi-Body Systems

A typical multibody spacecraft dynamics problem is the deployment of solar panels. Fig 1.1 shows a satellite in two configurations. In the first the solar arrays are folded around the central body, while the second is the operational configuration with solar panels deployed.

Spacecraft and mechanisms form multibody systems whose external environ-ment is predictable and relatively simple. The same is largely true for the interactions between the bodies. Good dynamic models can be constructed, without too much difficulty, which give satisfactory results in the light of physical experience. The main problems appear when modeling the interac-tions. Interactions also are simple to model in a typical early investigation of

Figure 1.1: Spacecfraft with solar panels folded (left) and deployed (right).

a closed circuit mechanism, as an automobile hood connected to the frame of the car.

Another typical example, useful for our task, could be the manipulator in Fig 1.2. Attaching sensors to its hand this machine can perform complicated manipulative tasks without external mechanical assistance. The principal problem in this case is the design and analysis of closed loop controls for the forces and torques in the individual joints of the robot arms. This problem is

(14)

especially difficult when there are redundant degrees of mechanical freedom in the manipulator or robot. Others appear in connection with such

prob-Figure 1.2: A typical manipulator.

lems as the dynamics of aircraft landing gears, Fig. 1.3, in fact it is not very far from the typical mechanism of the antenna in the satellite. Still another example taken from the latter source is the parachute model shown in Fig. 1.4, which has been used to study the swinging motion of the attached load.

(15)

Figure 1.3: Aircraft landing system.

(16)

1.3 Classification of Multi-Body System

In order to determine how a real system would behave( e.g. when it is built)it is necessary to derive a mathematical model of the system.

Mathematical modeling of a dynamic system means the formulation of the mathematical equations which describe all the system features considered to be essential. Essential features are those, which have a significant effect on the behavior of the physical system being modelled. Ideally, one would like to solve the mathematical equation (EoM) analytically, even if only ap-proximately. If this can be done, the dependency of the system behavior on various design parameters can be seen explicity. However, explicite so-lutions are almost always impossible for multi-body systems. One of the functions of a computer simulation is to permit an investigator to explore the behavior of a system before it is built, to discover the effects of changes in system configuration and parameter values on its behavior. The dynami-cal behavior usually remains important throughout the system’s life, so the value of computer simulation does not suddenly disappear after the system is built. Parameter values can change, the external environment can change, failures can occur, operations can be deliberately modified and data anal-ysis can create needs for the prediction of motion. Multi-body dynamical systems usually are described mathematically by a set of coupled no linear ordinary differential equations, possibly subjected to algebric constraints of some of the dependent variables. The system’s motion, or behavior, is ob-tained by solving an initial value problem for these differential equations. The differential equations might be of first or second order, depending on the dependent variable that a modeler chooses. That choice is influenced by his selection of a dynamical formalism. There are several kinds of formalisms ( Lagrangian, Hamiltonian, Newton-Euler method etc) that are explained roughly below. Another classification of multi-body formalisms can be the variables used to represent the system motions of the system( the choice of variables is left to the user or depends on the formalism). The motion might be described by absolute variables (representing a motion with respect to

(17)

an inertial frame) or in terms of relative motions between interacting bodies of the system. All possibilities have their advantages. If the choice is left to the user, the formalism is very flexible, but some of the problems that arise during the formulation of the equations of motion also must be handled by the user instead of being left to the computer. A multibody system as considered here is a system of N rigid bodies enumerated i=1,2,...N. A refer-ence frame representing the overall motion of the system is labeled with i=0. The bodies interact with each other and with points in the reference frame across Nj joints labeled s=1,2,...Nj . The term “joint” is considered here

to represent the entity of interaction between two bodies or one body and any point in the reference frame. With these definitions of bodies and joints and with the chosen labeling conventions, it is simple to distinguish the two basic topological configurations of multibody systems. If N = Nj the system

has a tree configuration. Systems with closed circuits are characterized by Nj > N . The treatment of the latter class of systems is considerably more

complicated, as the degrees of freedom of the system cannot be determined as easily as in the tree case. For a tree system, one just adds the numbers of degrees of freedom of the whole system. In the case of closed circuit systems, one must also consider the kinematic consistency conditions along with the constraints on relative motion across the joints.combining these, one gets a set of constraint equations. To determine the number of degrees of freedom, the rank of a matrix must be calculated. To represent the motion of the system, variables must be chosen from which the motion of any point of the system can be calculated.

The kinds of examples used for our computation are open circuit, we will not consider the tree structure case. Because we are dealing with rigid bodies, the motion of the system is known when we know for each body the vari-ables which represent the location of a specified point of the body and the orientation of a body-fixed reference frame, together with the velocity of the origin of the body frame and its angular velocity. We speak of “position” as a generic term embracing location and orientation of the body frame, and

(18)

of “velocity” as a generic term embracing the translational velocity and the angular velocity of the body frame.

A natural choice is to define position and velocity with respect to an iner-tial frame are the called “absolute variables”. The most basic mechanical elements of the multi-body systems considered here are bodies and joints. Their properties have to be defined by the user, and are:

• Frames: Mechanical, Body-fixed, Joints

• location of the center of mass, CoM in body frame; • mass;

• inertia matrix with respect to the CoM ;

• location of attachment points in body-fixed frame, used as base points for the calculation of interaction forces and torques applied by contigu-ous bodies;

• applied external forces and torques to each body in Inertial frame. About the joints, it is useful to emphasize the functions associated with the joints are represented in terms of state variables describing relative motion across the joint. It is possible to have different kind of joint. Taking in to ac-count the number of degrees of freedom, we can differentiate between 1-DoF, 2-DoF or 3-DoF; in addition it is possible to have rotational joints, transla-tional joints or both case. Thus they can be derived without any knowledge about the multibody aspect of the system. The joint’s description is not affected by one’s selection of state variables for the system as a whole. In the end, we can summerize following this classification, which kind of multi-body systems we consider. We will use an open tree structure of bod-ies, connected with 1-DoF joints. In particular, we will analyze different configurations of the system.

(19)

1.4 Terminology and Notation

• Lowercase bold letter indicate vectors. Vectors may be expressed in an n-dimensional Euclidean space:

r s d a b c

p q v w y z

α ω θ δ ψ

• Uppercase bold letter indicate matrices:

R A I L

• Uppercase bold F and T indicate force and torque vector

• Letter with a hat indicate unit vectors. Unit vectors are not bold: ˆi ˆj ˆk eˆ uˆ nˆ

ˆ

I Jˆ Kˆ eˆθ eˆψ eˆφ

• The left subscript on a vector denotes the frame in which the vector is expressed, i.e. the vector is expressed using the unit vectors of that frame:

Ir = position vector expressed in frame FI

• Lowercase letter with tilde indicate 3x3 skew symmetric matrices asso-ciated to vectors. For example ˜xy = x × y, where x is expressed with respect to the frame system Ti:

˜ x =     0 −x3 x2 x3 0 −x1 −x2 x1 0     ; Tix =     x1 x2 x3     (1.1)

(20)

• Lowercase letter with a overbar denotes a symmetric matrix such that ¯

xy = x×(x × y), where x is expressed with respect to the frame system Ti: ¯ x =     −x2 2− x23 x1x2 x1x3 x2x1 −x23− x21 x2x3 x3x1 x3x2 −x21− x22     Tix =     x1 x2 x3     (1.2)

• The two subscripts oa a bold letter “r” indicate the start and end points of a position vector:

rAB = a position vector from point A to point B

• A quaternion ←→q is defined as a vector plus a scalar quantity, where q_s is the scalar and q is the vector. A quaternion ←→q can also be written as a four-element vector, where ←→q is expressed with respect to the frame system Ti: Ti ←→_{q =} " q qs # =       qx qy qz qs       (1.3)

• Ω(←→q ) indicates a 4x4 matrix associated to a quaternion:

Ω (←→q ) =       qs −qx −qy −qz qx qs −qz qy qy qz qs −qx qz −qy qx qs       (1.4)

• The Euclidian-norm of a vector is indicated by a non bold lowercase letter:

r=||r||, a=||a||, b=||b||, s=||s|| ||x|| = P

i=1|xi| 212

(21)

• The capital letter F is used to denote the coordinate frames. The following frames are used, and they will be explained in the chapter Topology and Parameterization of Multi-Body Systems:

FI: Inertial frame

FM i: Mechanical frame of body i, where i is a number starting from 1

FBi: Body Frame parallel to FM i, located in the center of mass (CoMi),

where i is a number starting from 1 FJ k: Joint frame, where k is a letter

FP: Frame of principal axes of inertia

• Capital bold letter R indicate rotation matrices:

Rz,α =     cos(α) sin(α) 0 − sin(α) cos(α) 0 0 0 1     (1.5)

• “RA,B” indicates a rotation matrix between two frames. The left

sub-script denotes the destination frame, while the right letter denotes the departure frame:

RFI,FB = Rotation matrix from frame FB to frame FI

• The right subscript on an angular velocity vector indicates the frame with respect to which the angular vector is measured:

ωB,I = angular velocity of the body coordinate frame FB with respect

to the global coordinate frame FI

• Left superscript on an angular velocity vector denotes the frame in which the angular velocity is expressed:

(22)

B2ωI,B1= angular velocity vector of the body coordinate frame FB1with

respect to inertial coordinate frame FI, expressed in body coordinate

frame FB2

• Capital bold letter I indicates an identity matrix, which, depending on the dimension of the matrix equation, could be a 3 × 3 or a 4 × 4 identity matrix. I3 or I4 are also being used to clarify the dimension of

I. For example: I = I3 =     1 0 0 0 1 0 0 0 1     (1.6)

• The overdot operator denotes differentiation with respect to time in a body fixed frame; time derivatives of vector r in a frame FB are related

to derivatives in an inertial frame FI by :

I[

d

dt (r)] =B [ d

(23)

Chapter 2 Overview of Multi-Body

Modelling Methods

2.1 Newton-Euler Approach

On the basis of relative kinematic connections in combination with the principle of Jourdan there is a transparent and elegant method to derive the equations of motion of an individual body as well as a system of bodies [4]. We consider on the rigid body in Fig.2.1 the infinitesimal mass element in point P and describe its motion with the help of the body fixed reference point O’ and the inertial point O.

For the analisys of Newton’s Equation for the infinitesimal mass element: Z

K

(dFext− ¨rOPdm)T δ ˙rOP = 0 (2.1)

we need the absolute speed ˙rOP and the absolute acceleration ¨rOP. The

rela-tive speed ˙rO0_P and the relative acceleration ¨r_O0_P disappear if K rigid body.

So the representation, which is independent from the coordinate system is:

˙rOP = ˙rOO0+ ˜ωr_O0_P, (2.2)

(24)

Figure 2.1: Reference points in a rigid body.

¨

rOP = ¨rOO0 + ( ˙˜ω + ˜ω ˜ω)r_O0_P (2.3)

where ω is the angular speed with which that the body K spins with respect to the inertial system. In consideration of constraints we find a set of gener-alized coordinates q, ˙q which with the virtual speed δ ˙rOP in Eq. 2.1 can be

presented as:

δ ˙rOP = (

∂ ˙rOP

∂ ˙q )∂ ˙q (2.4)

(considered that ∂q = 0). Together with Eq. 2.2 this is leading to:

δ ˙rOP = ˙rOO0 ∂ ˙q + ˜r T O0_P ∂ω ∂ ˙q ∂ ˙q (2.5)

Using Eq. 2.3, combined with Eq. 2.1 we get the formula:

Z K ¨rOO0+ ( ˙˜ω + ˜ω ˜ω)r_O0_P dm − dFe T ∂ ˙rOO0 ∂ ˙q + ˜r T O0_P ∂ω ∂ ˙q ∂ ˙q = 0 (2.6)

(25)

After interchanging both factors of the scalar product we get: Z K ∂ ˙qT ∂ ˙rOO0 ∂ ˙q + ˜r T O0_P ∂ω ∂ ˙q T ¨rOO0 + ( ˙˜ω + ˜ω ˜ω)r_O0_P dm − dFe = 0 (2.7) When the set of generalized coordinates q is minimal ∂ ˙q is a freely selectable vector. The scalar product in Eq. 2.7 can just disappear, then:

Z K ∂ ˙rOO0 ∂ ˙q T + ∂ω ∂ ˙q T ˜ rO0_P ! ¨rOO0+ ( ˙˜ω + ˜ω ˜ω)r_O0_P dm − dFe = 0 (2.8) That is how we get from the scalar equation Eq. 2.7 to the differential equation Eq. 2.8. Taking all the quantities that do not include the position vector rOP, out of the integral, os obtains the formula:

∂ ˙r_OO0 ∂ ˙q TZ K ¨rOO0 + ( ˙˜ω + ˜ω ˜ω)r_O0_P dm − dFe + ∂ω ∂ ˙q T Z K ˜ rO0_P ¨r_OO0 + ( ˙˜ω + ˜ω ˜ω)r_O0_P dm − dFe = 0 (2.9)

With the abbreviations:

Fe = Z K dFe, (2.10) Me(O0) = Z K ˜ rOPdFe, (2.11) mrO0_P = Z K ˜ rO0_Pdm, (2.12) IO0 = Z K ˜rT_O0_P˜r_O0_Pdm, (2.13)

(26)

JT = ∂ ˙rOO0 ∂ ˙q , (2.14) JR= ∂ω ∂ ˙q (2.15)

we can obtain with Eq. 2.8 the final Newton-Euler Equation :

JT_T m ¨rOO0 + m ( ˙˜ω + ˜ω ˜ω) r_O0_P − Fe + JT_R m ˜rO0_P¨r_OO0 + I(O 0₎ ˙ ω + ˜ωI(O0)ω − Me(O0) = 0 (2.16)

where I(O0) is the Matrix of Inertia with respect to the point O’, m is the overall mass of the body K concentrated in the centre of gravity. JT and

JR are the Jacobian matrices of translation (index T ) and rotation (index

R). The two summands in Eq. 2.16 are called Impulsive term and Angular momentum of the CoM are express in the inertial frame and in the body fixed frame, respectively:

˙ p = m ¨rOO0+ m ( ˙˜ω + ˜ω ˜ω) r_O0_S, ˙ L(O 0₎ = m ˜rO0_P¨r_OO0 + I(O 0₎ ˙ ω + ˜ωI(O0)ω (2.17)

then we obtain the final form of the Eq. 2.16:

JT_T p − F˙ e + JT

R L˙

(O0)

− Me(O0)_{= 0} _(2.18)

Some notes to Eq.2.18: in the row vectors of the Jacobian matrices JT and

JR, there are the gradients of ˙rOO0(q, ˙q, t) and ω(q, ˙q, t) (with respect to ˙q

) that are defined the constraints. The multiplication with the transposed Jacobi matrices JT_T and JT_R comes from the fact that the movement can only take place in the surface defined by the constraints, but not perpendicular to it. In Eq. 2.18 all forces and torques which are “lost” forces for the move-ment sequences are canceled out. That means, we only consider influences,

(27)

which are able to accelerate the rigid body in directions not restricted by con-straints. From this interpretation of Eq. 2.18 follows for a freely moving the rigid body (no constraints) that the linkage between impulse and momentum disappers:

˙

p = Fe, L˙(O

0₎

= Me(O0) (2.19)

A force, whose line of action does not go through the reference body point O’, can be considered in two different ways:

(a) It is reduced to the point O’ (most of the time the CoM), and the excen-tricity of the torque:

M(O_i 0) = rO0_P

i× Fi (2.20)

(b) We determine the Jacobian matrices of the translation JT Fi for the origin

points of the forces:

JT Fi =

∂rOPi

∂q (2.21)

and Eq. 2.18 modifies to: JT_T ˙q +X i JT FiF e i + J T R ˙L (O0) − Me(O0)_{= 0} _(2.22)

Furthemore, pay attention for the evaluation of Eq. 2.18 to the pleasant fact that it is possible to calculate the two terms of the sum in different coordinate systems. To show this we have a look on the exemplary term JT_Tp in two˙ coordinate systems, which can be transformed into each other through the rotation matrix R21. Since R21is not explicitly dependent on the generalized

coordinates q, it holds: 2JT = ∂2˙r ∂ ˙q = ∂(R211˙r) ∂ ˙q = R21 ∂1˙r ∂ ˙q = R21(1JT) (2.23) with: 2p = R˙ 21(1p)˙ (2.24)

(28)

follows: 2JTT (2p) =˙ 1 JTT R T 21R21 | {z } I3×3 (1p) =˙ 1 JTT (1p)˙ (2.25)

Similarly we could show the situation from above also for other terms. Whereas the first term of the sum in Eq. 2.18 is usually calculated in the inertial sys-tem, we prefer to express the second term of the sum in the body fixed frame, because here the matrix of inertia contains a simple representation.

Consider now the case of a system with several rigid single bodies. For a multi-body system we can extend Eq.2.18 through summation over all n bodies to : n X i=1 h JT_{T i}( ˙p_i− Fe i) + J T Ri ˙L (O0₎ i − M e(O0) i i = 0 (2.26)

If we choose the CoM as body reference point (O0 ≡ S):

rOO0 = r_OS (2.27)

then rOS vanishs in Eq. 2.17 and the timewise derivatives for the impulse

and momentum simplify to: ˙ p_i = mi¨rOSi ˙ L(S)_i = I(S)_i ω˙i + ˜ωiI (S) i ωi (2.28)

(29)

2.2 Lagrangian Approach

In this section we will illustrate an alternative approach, placing Newton’s law into a form particularly convenient for multiple degree of freedom systems or systems in complex coordinate systems. This approach results in a set of equations called Lagrange’s equations. They are the beginning of a complex, more mathematical approach to mechanics called analytical dynamics. The two approaches - Newton and Euler’s Law and Lagrange’s Equations - are totally compatible, no new physical laws result for one approach or the other. We begin by considering the conservation equations for a number of N-bodies in a conservative force field using Cartesian coordinates for the position of the CoM xi. For this system, we write the total kinetic energy as:

T = M X i=1 1 2mi˙x 2 i (2.29)

where M is the number of degrees of freedom of the system. For bodies with only one DoF, only one xi is required to define their positions. So the

number of degrees of freedom M=N. For bodies with three translational DoF, each body requires three xi coordinates, so that 3N. The momentum

of a given body along a given direction can be obtained by differentiating this expression with respect to the appropriate xi coordinate. This gives the

momentum p_i for this particular body in this coordinate direction:

δT

δ ˙x2_i = pi (2.30)

The time derivative of the momentum is:

d dt

δT

δ ˙x2_i = m¨xi (2.31)

For a conservative force field, the force on a body is given by the derivative of the potential at the body position in the desired direction:

(30)

Fi = −

δV δ ˙xi

(2.32) From Newton’s law we know:

Fi = −

dp_i

dt (2.33)

Equating like terms from our manipulations on kinetic energy and the po-tential of a conservative force field, we write:

d dt δT δ ˙xi = δV δxi (2.34) Now we make use of the fact that:

       δT δxi = 0 δV δ ˙xi = 0 (2.35)

Using these results, we can rewrite the equation as: d dt δ(T − V ) δ ˙xi −δ(T − V ) δxi = 0 (2.36)

We now define L=T-V, where L is called Lagrangian. This equation takes the final form called Lagrange’s equation in Cartesian coordinates:

d dt δL δ ˙xi − δL δxi = 0 (2.37)

where i is taken over all of the degrees of freedom of the system. A significant advantage of the Lagrangian approach for deriving the equations of motion for complex systems becomes visible when we leave the Cartesian coordinate system and move into a general coordinate system. We introduce a quite general notation for the relationship between the n Cartesian position vari-ables xi and their description in generalized coordinates ( for some systems,

the number of generalized coordinates is could be larger than the number of degrees of freedom and this is accounted for by introducing constraints

(31)

on the system. This is an important part of the discussion of the Lagrange formulation). Let xi be the Cartesian coordinates of the ith particle in the

inertial frame. The coordinates of every individual particle can be expressed by functions of a set of generalized coordinates q1, q2, ..qn and possibly time

t :

xi = xi(q1, q2, ...qn, t) (2.38)

To obtain the velocity ˙xi, we take the time derivative of the equation above

(by applying the chain rule):

˙xi = n X j=1 δxi δqj ˙ qj (2.39)

Taking the partial derivative of this equation with respect to ˙qi we obtain a

relation between these two derivatives: δ ˙xi

δ ˙qi

= δxi δqi

(2.40) In this step, we take advantage of the fact that the generalized coordinates qi are independent, i.e. _δqδq_ji = 0 for i6=j With the generalized momentum:

p_i = δT δ ˙qi , T = M X n=1 1 2mi˙x 2 i (2.41)

we are now ready to express Newton’s law in the generalized coordinates qj

that we have introduced. For the generalized momentum we have:

p_i = δT δ ˙qi = n X j=1 mi˙xj δ ˙xj δ ˙qi = n X j=1 mi˙xj δxj δ ˙qi (2.42) For a conservative force field, the work done during a displacement dxi is

given by: dW = N X i=1 Fidxi = N X i=1 Fi δxi δqj dqj (2.43)

(32)

We identify this as work done by a “displacement” through dqj is:

dQ = −dV = −δV δqj

dqj (2.44)

where V is the potential function expressed in the coordinate system of the generalized coordinates and δV_δq

j is the change in potential due to a change in

the generalized coordinate qj. So that the generalized force is:

Qj = −

δV δqj

(2.45) We now examine the time derivative of the generalized momentum, p_i:

dp_i dt = d dt δT δqi = N X j=1 mjx¨j δxj δqj + mj˙xj d dt δxj δqi (2.46) Since dxj

dqj is a function of the q’s which are functions of time, we get(by the

chain rule): d dt δxj δqi = N X k=1 δ2_x j δqiδqk ˙ qk (2.47)

From Newton’s law, mjx¨j = Fj, so: N X j=1 mjx¨j δxj δqi = Qi (2.48)

Now let us consider the other term in the equation. Consider the expression for δT_{δ ˙}_q i : δT δ ˙qj = mj˙xj δ ˙xj δqi = mj˙xj δ δqi N X k=1 δxi δqk ˙ qk (2.49)

We have now identified a simpler form for the two expressions in the equation for the time derivative of the generalized momentum. We may now write:

dp_i dt = d dt δT δqi = Qi+ δT δqi = −δV δqj +δT δqi (2.50)

(33)

Since for a conservative system, the potential is independent of the veloci-ties,we can bring this equation into its final form by defining the Lagrangian as L=T-V in order to obtain the final form of Lagrange’s equation:

d dt L δ ˙qi − δL δqi = 0. (2.51)

(34)

2.3 D’Alembert’s Principle

D’Alembert’s principle, also known as the Lagrange-d’Alembert principle, is a statement of the fundamental classical laws of motion. The principle states that the sum of the differences between the forces acting on a system of mass particles and the time derivatives of the momenta of the system itself along any virtual displacement consistent with the constraints of the system, is zero. Thus, in symbols d’Alembert’s principle is written as:

X

i

(Fi− miai) · δri = 0 (2.52)

where:

i : is the number of the i-th particle in the system,

Fi : in the total applied force (excluding constraint forces) on the i-th

par-ticle,

mi : is the mass of the i-th particle,

ai : is the acceleration of the i-th particle,

miai : together as product represent the time derivative of the momentum

of the i-th particle,

δri : is the virtual displacement of the i-th particle, consistent with the

constraints. It is the dynamic analogue to the principle of virtual work for applied forces in a static system and in fact is more general the Hamil-ton’s principle, avoiding restriction to holonomic systems. A holonomic constraint depends only on the coordinates and time, and not on the veloc-ities. If the negative terms in accelerations are recognized as inertial forces, the statement of d’Alembert principle becomes the total virtual work of the impressed forces plus the inertial forces vanishes for reversible displacements.

(35)

2.3.1 General Case with Variable Masses

The general statement of d’Alembert’s principle mentions “the time deriva-tives of the momenta of the system”. The momentum of the i-th mass is the product of its mass and velocity:

p_i = mivi (2.53)

and its time derivative is: ˙

p_i = ˙mivi+ mi˙vi. (2.54)

In many applications, the masses are constant and this equation reduces to: ˙

p_i = mi˙vi = miai, (2.55)

which appears in Eq. 2.52. However, some applications involve changing masses (for example, chains being rolled up or being unrolled) and in those cases both terms ˙mivi and mi˙vi have to remain present, giving:

X

i

(Fi− miai− ˙mivi) · δri = 0. (2.56)

2.3.2 Derivation for Special Cases

To date nobody has shown that D’Alembert’s principle is equivalent to Newton’s Second Law. This is true only for some very special cases e.g. rigid body constraints. However, an approximate solution to this problem does exist.

Consider Newton’s law for a system of particles, i :

F(T )_i = miai, (2.57)

where:

(36)

miai : are the inertial forces that result from the total forces. Moving the

inertial forces to the left gives an expression that can be considered to rep-resent quasi-static equilibrium, but which is really just a small algebraic manipulation of Newton’s law:

F(T )_i − miai = 0. (2.58)

Considering the virtual work δW done by the total and inertial forces to-gether through an arbitrary virtual displacement δri of the system leads to

a zero identity, since the forces involved sum to zero for each particle.

δW =X i F(T )_i · δri− X i miai· δri = 0 (2.59)

The original vector equation could be recovered by recognizing that the work expression must hold for arbitrary displacements. Separating the total force into applied forces Fi and constraint forces Ci yields:

δW =X i Fi· δri+ X i Ci· δri− X i miai· δri = 0. (2.60)

If arbitrary virtual displacements are assumed to be in directions that are orthogonal to the constraint forces, the constraint forces do no work. Such displacements are said to be consistent with the constraints. This leads to the formulation of d’Alembert’s principle, which states that the difference of applied forces and inertial forces for a dynamic system does no virtual work:

δW =X

i

(37)

2.4 Comparison and Suitability of Modelling

Approaches for Problem at Hand

One of the first tasks was to understand which approach is most suitable for our kind of problem. In fact, each approach in its application has ad-vantages points and dis-advantage points depending on examining the multi-body. Our choice was between Newton-Euler mechanics and Lagrangian. As it has been explained, these two approaches show a very different solution structure. In particular the Lagrange Mechanics follow an energetic solu-tion, and present some advantage with respect to the others. When we use this approach, the formulation is not tied to a certain coordinate system but convenient variables may be used to describe the system; these variables are called “generalized coordinates” qi and can be any quantitative attributes of

the system (for example, the strength of the magnetic field at a particular lo-cation, the angle of a pulley, the position of a particle in space, or the degree of excitation of a particular eigenmode in a complex system) which are func-tions of the independent variable(s). This trait makes it easy to incorporate constraints into a theory by defining coordinates that only describe states of the system that satisfy the constraints. If the Lagrangian is invariant un-der a symmetry, the resulting equations of motion are also invariant unun-der that symmetry. This characteristic is very helpful in showing that theories are consistent with either special relativity or general relativity [1]. Using this approach, anyway, we will lose completly a clear information about the system of forces and torques in the joints. Also, through the pure energy approach, we do not have a clear and direct view on the forces involved and the influence they have on the motion of bodies and the direct act. In the latter as we have shown in the previous chapter, there is the express cargo system, internal and external, on multi-body. These amounts are within the study, easier and faster than energy quantity definition.

The Lagrangian Mechanics are not very suited for non-ideal or non-holomic systems, such as systems with friction. Being one of the possible

(38)

improve-ments that reduce the degree of approximation, not considering more rota-tional joints, we need to create the tool which that it is already predisposed to this improvement.

In the end, another advantage of Newton-Euler equations of motion in multi-body applications, is( as we will see in the in the following chapters) that we can calculate the joint forces of one link at a time. Therefore, starting from the end-effector link, we can analyze the links one by one and end up at the base link, or vice versa.

For all this reasons, we decide to use the Newton-Euler approaches to solve the equations of motion of the multi-body system, that we will describe.

(39)

Chapter 3 Description of Position and

Rotation of Rigid Bodies

3.1 Degrees of Freedom

The position of a body with respect to a reference frame F1 defined by

origin O1and unit vectors e1x, e1y, e1z is known if the locations of all points of

the body are known. Any frame having a known, possibly variable, position in space can be used as reference. If the body is rigid, the description of its position is simple. A frame F2 defined by origin O2 and unit vectors

e2x, e2y, e2z can be embedded rigidly in the body frame. In this frame all

points of the body have constant and known coordinates for any position of the body. Therefore, the location of all points of a rigid body with respect to a reference frame can be determined when the position of the body frame F2

is known with respect to the reference frame F1. This relative position can be

considered to be composed of the location of O2with respect to O1 and of the

orientation of e2 with respect to e1. Motions of the rigid body are described

by parameters characterizing the relative location and orientation of the two frames as a function of time. Motion during which the relative orientation of the triads e2 and e1 does not change is called translation. When O2 remains

in a fixed location with respect to O1 the motion is called rotation. The

(40)

motion of a rigid body can be always decomposed into linear combinations of these two elementary motions.

Consider a rigid body with the body frame F2 (see Fig 3.1) and the point P

being a point of the body, and F1. F1 used as a reference frame to describe

the position of the body. The location of point P (defined by vector R) at time t is described by:

R(t) = r(t) + z(t) (3.1)

or, resolving R and r in the F1, but z in the F2, 1_{R =}1_{r + R}

122z (3.2)

The components 2_{z of the (body-fixed) vector z are fixed, so its motion can}

Figure 3.1: Location of a point in a body.

be described by the functions:

1_{r =}1_{r (t)} _(3.3)

(41)

Instead of three Cartesian coordinates, any other set( such as the cylindrical or spherical coordinates) can be used.

The first equation suggests that three parameters, such as the three compo-nents of 1_r

α, are needed to describe the location of O2 with respect to O1,

while the second one suggests that nine parameters are used to describe the orientation of e2 with respect to e1. However, these twelve parameters are

not independent. If all coordinates of three separate points of a rigid body are specified with respect to a reference frame, the position of that body with respect to that frame is fixed. Since it is a rigid body those nine coordinates have to meet the constraint that the three distances between the points are constant. Only six of them are independent which we shall call the position variables.

The motion of a body-fixed frame with respect to a reference frame can be visualized as beeing composed of a translation and a rotation, done in ei-ther order or simultaneously. A pure translation is characterized by constant R12, while a pure rotation is characterized by constant 1r. The ability to

decompose any motion into translation and rotation describe by a theorem known as Euler’s theorem on the composition of displacements. In summary, a rigid body has six positional degrees of freedom: three in translation, de-scribed by the three parameters that are the components of 1r; three in rotation, described by three parameters necessary to construct the direction cosine matrix R12. The fact that rotation can be characterized by just three

parameters follows from Euler’s theorem on rotation about a fixed point. It states that any motion of a body with a fixed point is equivalent to a rotation of the body about an axis through that point (two parameters are needed to describe the direction of such an axis) and an angle of rotation about that axis.

(42)

3.2 Representation of Rotation

In geometry, various formalisms exist to express a rotation in three di-mensions as a mathematical transformation. In physics, this concept extends to classical mechanics where rotational (or angular) kinematics is the science of describing with numbers the purely rotational motion of an object. The orientation of an object at a given instant is describe with the same tools, as it is defined as an imaginary rotation from a reference placement in space, rather than an actually observed rotation from a previous placement in space.

3.2.1 Direction Cosine Matrix (DCM)

Let us consider a rigid body in space. We assume that there exists an orthogonal, right-handed triad, of unit vectors ˆu, ˆv, ˆw fixed to the body. The basic problem is to specify the orientation of this triad (and therefore of the rigid body) relative to some reference coordinate frame, as illustrated in Fig. 3.2. Specifying the components of ˆu, ˆv and ˆw along the three axes of the coordinate frame will fix the orientation completely. This requires nine parameters, which can be regarded as the elements of a 3 × 3 matrix, A, called attitude matrix [1].

A ≡        ˆ uT ˆ vT ˆ wT        ≡     u1 u2 u3 v1 v2 v3 w1 w2 w3     (3.5)

where ˆu = (u1, u2, u3)T, ˆv = (v1, v2, v3)T, and ˆw = (w1, w2, w3)T. Each of

these elements is the cosine of the angle between a body unit vector and a reference axis; for example, u1 is the cosine of the angle between ˆu and the

reference 1 axis.



  

cos(û, 1) cos(û, 2) cos(û, 3) cos(ˆv, 1) cos(ˆv, 2) cos(ˆv, 3) cos( ˆw, 1) cos( ˆw, 2) cos( ˆw, 3)



  

(3.6)

(43)

Figure 3.2: Orientation of the body axes ˆu, ˆv, ˆw w.r.t. axes 1,2,3 of reference.

of the direction cosine matrix are not all independent. The fact that ˆu is a unit vector requires, e.g.:

u2₁+ u2₂ + u2₃ = 1 (3.7)

and the orthogonality of ˆu and ˆv means that:

u1v1+ u2v2+ u3v3 = 0 (3.8)

Those relationships can be summarized by the statement that the product of A and its transpose is the identity matrix I:

AAT = I3×3 (3.9)

This means that A is a real orthonomal matrix. Also, the definition of the determinant is equivalent to:

(44)

so the fact that ˆu, ˆv, ˆw form a right-handed triad means that det (A)=1. Thus, A is a real orthogonal matrix.

In general the direction cosine matrix is a coordinate transformation that maps vectors from one frame to another frame. That is, if a is a vector with components a1, a2, a3 along the reference axes, then The components of Aa

are the components of the vector a along the body triad ˆu, ˆv, ˆw:

Aa =     u1 u2 u3 v1 v2 v3 w1 w2 w3         a1 a2 a3     =     ˆ u · a ˆ v · a ˆ w · a     ≡     au av aw     (3.10)

The product of two proper real orthogonal matrices A00 = A0A can be used to represent the result of two successive rotations A and A’, in that order. Because the transpose and inverse of an orthogonal matrix are identical, AT maps vectors from the body frame to the reference frame.

(45)

3.2.2 Euler Angles

Euler angles are three angles used to describe the orientation of a rigid body in 3-dimensional Euclidean space. They represent a sequence of three elemental rotations, i.e. rotations about axes of a coordinate system. For instance, a first rotation about z by an angle α:

RA,B = Rz,α=     cos(α) sin(α) 0 − sin(α) cos(α) 0 0 0 1     (3.11)

a second rotation about y by an angle β:

Ry,β =     cos(β) 0 − sin(β) 0 1 0 sin(β) 0 cos(β)     (3.12)

and a last rotation about x, by an angle γ:

Rx,γ =     1 0 0 0 cos(γ) sin(γ) 0 − sin(γ) cos(γ)     (3.13)

These rotations start from a know standard orientation in Fig. 3.3 [14]. Any orientation can be obtained by using three elemental rotations. Elemental rotations can either occur about the axes of the fixed coordinate system or about the axes of a rotating coordinate system, is initially aligned with the fixed one, and modifies its orientation after each elemental rotation. The rotating coordinate system may be imagined to be rigidly attached to a rigid body. In this case, it is sometimes called a local coordinate system. The 3-2-1 Euler sequence is one of the most widely used parameters for rotations. Let {E1, E2, E3} be a triad for a fixed spatial frame, and let {e1, e2, e3} be a

triad for a body frame. The 3-2-1 Euler angles provide an orthogonal matrix Q which maps {E1, E2, E3} to {e1, e2, e3},

(46)

Figure 3.3: Schematic of sequence 3-2-1 in terms of Euler angles α, β, γ.

{E1, E2, E3} Q

−→ {e1, e2, e3}

by breaking the action of Q up into three steps

{E1, E2, E3} −→ {a1, a2, a3} −→ {b1, b2, b3} −→ {e1, e2, e3}

where {a1, a2, a3} and {b1, b2, b3} are intermediate frames of reference.

Map from Ej to aj :the yaw rotation

In this section the details of the map {E1, E2, E3}

R(α,E3)

−−−−→ {a1, a2, a3}

are constructed using the rotation tensor:

R(α, E3) = (I − E3× E3) cos α + ˜E3sin α + E3× E3, (3.14)

The equation 3.14 is a counterclockwise rotation about the Z-axis as shown schematically in Fig. 3.4. The new basis is:

a1 = R(α, E3)E1 = E1cos α + E3× E1sin α (3.15)

a2 = R(α, E3)E2 = E2cos α + E3× E2sin α (3.16)

a3 = R(α, E3)E3 = E3 (3.17)

(47)

Figure 3.4: Schematic of rotation by angle α around E3 axis (yaw-rotation).

E1× E2 = E3, E3× E1 = E2, E2× E3 = E1

the matrix representation of the map is:     a1 a2 a3     =     cos(α) sin(α) 0 − sin(α) cos(α) 0 0 0 1         E1 E2 E3     (3.18)

Map from aj to bj:the Pitch Rotation

In this section details of the map: {a1, a2, a3}

R(β,E2)

−−−−→ {b1, b2, b3}

are constructed using the rotation tensor represented in Fig.3.5

R(β, E2) = (I − a2× a2) cos(β) + ˜a2sin(β) + a2× a2, (3.19)

where:

a2 = −E1sin(α) + E2cos(α) (3.20)

(48)

Figure 3.5: Schematic of rotation by angle β around a2 axis (pitch-rotation).

b1 = R(β, a2)a1 = a1cos(β) + a2× a1sin(β) (3.21)

b2 = R(β, a2)a2 = a2 (3.22)

b3 = R(β, a2)a3 = a3cos(β) + a2× a3sin(β) (3.23)

with matrix representation:     b1 b2 b3     =     cos(β) 0 − sin(β) 0 1 0 sin(β) 0 cos(β)         a1 a2 a3     (3.24)

Map from bj to ej: the Roll Rotation

In this section the details of the map {b1, b2, b3}

R(γ,b1)

−−−−→ {e1, e2, e3}

are constructed using the rotation tensor represented in Fig. 3.6

R(γ, b1) = (I − b1× b1) cos(γ) + ˜b1sin(γ) + b1× b1, (3.25)

where

(49)

The new basis is:

Figure 3.6: Schematic of rotation by angle γ around b1 axis (roll-rotation).

e1 = R(γ, b1)b1 = b1 (3.27)

e2 = R(γ, b1)b2 = b2cos(γ) + b1× b2sin(γ) (3.28)

e3 = R(γ, b1)b3 = b3cos(γ) + b1× b3sin(γ) (3.29)

with matrix representation:     e1 e2 e3     =     1 0 0 0 cos(γ) sin(γ) 0 − sin(γ) cos(γ)         b1 b2 b3     . (3.30)

The Composite Rotation from Ej to ej

In this section the details of the map: {E1, E2, E3}

R(γ,b1),R(β,a2),R(α,E3)

−−−−−−−−−−−−−−→ {e1, e2, e3}

The three rotation are combained, trough moltiplication of the three rotation matrix in the right sequence, and we obtain the whole rotation matrix that

(50)

maps Ej to ej:     e1 e2 e3     =     1 0 0 0 cos(γ) sin(γ) 0 − sin(γ) cos(γ)         cos(β) 0 − sin(β) 0 1 0 sin(β) 0 cos(β)         cos(α) sin(α) 0 − sin(α) cos(α) 0 0 0 1         E1 E2 E3     =    

cos β cos α cos β sin α − sin β

sin γ sin β cos α − cos γ sin α sin(γ) sin β sin α − cos γ cos α sin γ cos β cos γ sin β cos α + sin γ sin α cos γ sin β sin α − sin γ cos α cos γ cos β

        E1 E2 E3     (3.31)

(51)

3.2.3 Euler Symmetric Parameters (Quaternions)

The parameterization of the rotation in terms of Euler symmetric param-eters qx,qy,qz,qs has proven to be quite useful for space appications. This

parameterization is a consequence of Euler’s rotation theorem in three-dimensional space. It says that in any displacement of a rigid body from one orientation to another such that a point on the rigid body remains fixed, is equivalent to a single rotation about some axis that goes through the fixed point(see Fig.3.7). It also means that the composition of several rotations can be expressed by a single a rotation. Therefore, the set of rotations has a struc-ture known as a rotation group [1]. So any generic rotation can be expressed

Figure 3.7: Three different rotations equivalent to a single rotation.

by a rotation of a certain angle Φ around a rotation axis ˆe = {ex, ey, ez}

as shows in Fig.3.7( in general, the axis of rotation will not be one of the reference axes). Using the unit vector ˆe as rotation axis, and Φ as rotation ange, the generic direction cosine matrix is ( for simplification we will use,

(52)

cΦ instead of cos Φ, and sΦ instead of sin Φ): A =     cΦ + e2 1(1−cΦ) e1e2(1−cΦ) + e3sΦ e1e3(1−cΦ) − e2sΦ e1e2(1−cΦ) − e3sΦ cΦ + e22(1−cΦ) e2e3(1−cΦ) + e1sΦ e1e3(1−cΦ) + e2sΦ e2e3(1−cΦ) − e1sΦ cΦ + e23(1−cΦ)     = (3.32) = cos(Φ)I3×3+ (1 − cos(Φ)) ˆeˆeT − sin(Φ)˜e (3.33)

In this way, the Euler parameters are defined by: q_x ≡ exsinΦ₂ q_y ≡ eysinΦ₂ q_z ≡ ezsinΦ₂ q_s ≡ cosΦ 2 (3.34)

The four Euler symmetric parameters are not independent, but satisfy the constraint equation:

q_x2+ q2_y+ q2_z+ q_s2 = 1 (3.35) These four parameters can be regarded as the components of a quaternion:

←→_{q =}       qx qy qz qs       ≡ " q qs #

The direction cosine matrix can be expressed in terms of the Euler symmetric parameters by: A (←→q ) =     q_x2− q2 yqz2+ qs2 2(qxqy+ qzqs) 2(qxqz− qyqs) 2(qxqy − qzqs) −qx2+ qy2− q2z+ q2s 2(qyqz+ qxqs) 2(qxqz+ qyqs) 2(qyqz− qxqs) −qx2− q2y + qz2+ qs2     = (3.36)

(53)

= q_s2− q2_I

3×3+ 2qqT − 2qsq˜ (3.37)

where ˜q is the skew symmetric matrix:

˜ q ≡     0 −qz qy qz 0 −qx −qy qx 0     (3.38)

These equations can be verified by substituting Eq. 4.13 into them, using some trigonometric identities, and comparing them with Eq. 3.33.

Euler symmetric parameters provide a very convenient parameterization of the attitude. They are more compact than the direction cosine matrix, be-cause only four parameters, rather than nine, are needed. They are more convenient than the Euler axis and angle parameterization (and the Euler angle parameterization to be considered below) because the expression for the direction cosine matrix in terms of Euler symmetric parameters does not involve trogonometric functions, which require time-consuming computer op-erations. Another advantage of Euler symmetric paramteres is the relatively simple form for combining the parameters for two individual rotations to ob-tain the parameters for the product of the two rotations.

Thus, if: R (←→q 00) = R (←→q 0) R (←→q ) (3.39) then: ←→_q 00 =       q_s0 q0_z −q0 y qx0 −q0 z q 0 s q 0 x q 0 y q_y0 −q0 x q 0 s q 0 z −q0 x −q 0 y −q 0 z q 0 s       ←→_q _(3.40)

Kinematic Equation of Motion

In this section we would find an explicity relation between the variation of quaternion and the angular velocity. In this way, we can create a connection with the Euler’s equations of the multi-body.

(54)

from the product relation, Eq. 3.40. Let the quaternion ←→q represent the orientation of a rigid body with respect to the reference system at time t, and ←→q 00 represent the orientation with respect to the reference system at time t + ∆t. We shall denote these by ←→q (t) and ←→q (t + ∆t), respectively. Then ←→q 0 specifies the orientation of the ˆi,ˆj,ˆk triadat time t + ∆t relative to the position that it occupied at time t. Considering Eq. 4.13, and ∆Φ is the rotation in time ∆t, thus:

←→_{q (t + ∆t) =} ( cos∆Φ 2 I4×4+ sin ∆Φ 2       0 ez −ey ex −ez 0 ex ey ey −ex 0 ez −ex ez −ez 0       ) ←→_{q (t)} (3.41) Where I4×4 is the 4 × 4 identity matrix. Eq. 3.41 is particularly useful if the

axis of rotation does not change over the time interval ∆t, and is often used in inertial navigation.

Assuming that ω = {ω1, ω2, ω3} is the spacecraft angular velocity vector, the

kinematic equations can be written in the form: d←→q dt = 1 2Ω← →_q _(3.42) where: Ω =       0 ω3 −ω2 ω1 −ω3 0 ω1 ω2 ω2 −ω1 0 ω3 −ω1 −ω2 −ω3 0       (3.43)

The quaternion representation is generally prefered to the Euler Angle rep-resentation because of its analytical characteristics. The reprep-resentation of a rotation as a quaternion has other several advantages. It is more compact than the representation as an orthogonal matrix (9 numbers). Futhermore, for a given axis and angle, one can easily construct the corresponding quater-nion, and conversely, for a given quaternion one can easily read off the axis

(55)

and the angle. Both of these are much harder with matrices or Euler angles. When composing several rotations on a computer, rounding errors necessar-ily accumulate. A quaternion that’s slightly off still represents a rotation after being normalised: a matrix that is slightly off may not be orthogonal anymore and is harder to convert back to a proper orthogonal matrix. Quaternions also avoid a phenomenon called gimbal lock which can result when, for example in pitch-yaw-roll rotational systems, the pitch is rotated 90deg up or down, so that yaw and roll then correspond to the same motion, and a degree of freedom of rotation is lost. In a gimbal-based aerospace iner-tial navigation system, for instance, this could have disastrous results if the vehicle is in a steep dive or ascent.

(56)

3.2.4 Advantage and Disadvantages of Different

Rep-resentation of Rotation

We explored different kind of representation in the previous section. Each of them have different notation, variable and advantage and disadvantages. In the Table 3.1 are summerized the alternative representation of rotation where it is possible to see the caracteristic of them:

Parameterization Notation Advantages Disadvantages

Direction A = [Aij] -No singularities -Six redundant

cosine -No trigonometric parameters

matrix functions

-Convenient product rule for

successive rotations

Euler qx, qy, qz, qs -No singularities -One redundant

symmetric -No trigonometric parameter

parameters ←→q functions -No obvious physical

(quaternion) -Convenient product interpretation

rule for

successive rotations

Euler α, β, γ -No redundant -Trigonometric

Angles parameters functions

-Physical interpretation -Singularity at some θ

is clear in -No convenient product

some cases rule for

successive rotations Table 3.1: Alternative Representation Rotation[1].

(57)

3.3 Conversion Between Different

Represen-tations of Rotation

3.3.1 Direction Cosine Matrix ↔ Euler Angles

The Euler angles {α, β, γ} can be extracted from the DCM R by inspect-ing the rotation matrix in analytical form. Usinspect-ing the sequence 3 − 2 − 1 Euler angles γ, β and α (around Z,Y and the X -axis) can be obtained as follows:

α = arctan 2 [R1,1, R1,2] (3.44)

β = arcsin (−R1,3) (3.45)

γ = arctan 2 [R3,3, R2,3] (3.46)

The function arctan2 is the arctangent function with two arguments. The purpose of using two arguments instead of one, is to gather information of signs of the inputs in order to return the appropriate quadrant of the computed angle, which is not possible for the single-argument arctangent function. For any real number arguments x and y not both equal to zero, arctan 2 [x, y] is the angle in radiants between the positive x -axis of the plane and the point given by the coordinates (x,y) on it. The angle is positive for counter-clockwise angles (upper half-plane y > 0), and negative for clockwise angle (lower half-plane, y < 0).

The motivation for it, is that, the one-argument arctangent function can not distinguish between diametrically opposite directions. For example, the anticlockwise angle from the x -axis to the vector (1,1), calculated in the usual way as arctan(1/1), is π/4(radiants), or 45 deg. However, the angle between the x -axis and the vector (-1,-1) appears, by the same method, to be arctan(-1/-1), again π/4, even though the answer clearly should be −3π/4 or -135 deg

The arctan 2 function takes into account the signs of both vector components, and places the angle in the correct quadrant.

(58)

The DCM R is generated from the Euler angles by multiplying all three matrices generated by rotations about the axes:

R = R (γ, z) R (β, y) R (α, x) (3.47)

As we have already seen, this yields : R=



  

cos β cos α cos β sin α − sin β

sin γ sin β cos α − cos γ sin α sin γ sin β sin α − cos γ cos α sin γ cos β cos γ sin β cos α + sin γ sin α cos γ sin β sin α − sin γ cos α cos γ cos β



  

3.3.2 Direction Cosine Matrix ↔ Quaternion

When computing a quaternion from a DCM there is a sign ambiguity, since ←→q and −←→q represent the same rotation. The single elements of a quaternion can be calculated from the main diagonal of the corresponding direction cosine matrix. In the case an element is found which is not zero, all other elements can be calculated from this element and the sub-diagonals. As elements close to zero can cause inexact solutions, one can first calculate all four elements from the main diagonal and then take the greatest value as basis for further calculation. As all equations have a square root on the right side, the maximum value is also the maximum absolute value.

qs,0 = q 1 4 + R1,1+ R2,2+ R3,3 qx,0 = q 1 4 + R1,1− R2,2− R3,3 qy,0= q 1 4 − R1,1+ R2,2− R3,3 qz,0 = q 1 4 − R1,1− R2,2+ R3,3 (3.48)

The maximum value of Eq. 3.48 is chosen directly and all others are re-calculated as shown in Table 3.2. The first column lists all values from Eqs. 3.48, while the columns two to five list the re-calculated values for

(59)

all other elements: The rotation matrix corresponding to the quaternion maximum qx qy qz qs qx,0 qx,0 R1,2+R2,1 4qx,0 R1,3+R3,1 4qx,0 R2,3+R3,2 4qx,0 qy,0 R1,2+R2,1 4qy,0 qy,0 R2,3+R3,2 4qy,0 R1,3+R3,1 4qy,0 qz,0 R1,3_4q+R3,1 z,0 R2,3+R3,2 4qz,0 qz,0 R1,2+R2,1 4qz,0 qs,0 R3,2−R2,3 4qs,0 R1,3−R3,1 4qs,0 R2,1−R1,2 4qs,0 qs,0

Table 3.2: Building Quaternion Elements from Subdiagonals.

←→_{q = [q}

x qy qz qs]T can be computed as follows:

R =     1 − 2q_y2− 2q2 z 2(qxqy− qzqs) 2(qxqz+ qyqs) 2(qxqy + qzqs) 1 − 2q2x− 2qz2 2(qyqz− qxqs) 2(qxqz− qyqs) 2(qxqs+ qyqz) 1 − 2qx2− 2q2y     (3.49)

3.3.3 Euler Angles ↔ Quaternion

We will consider the sequence 3 − 2 − 1 Euler angles for the following algorithm. Terms of the algorithm depend on the convention used. We can compute the quaternion ←→q = [qx qy qz qs]

T

from the Euler angles (α, β, γ) as follows:

(60)

qx = cos α−γ₂ sin β₂ qy = sin α−γ₂ sin β₂ qz = sin α+γ₂ cos β₂ qs= cos α+γ₂ cos β₂ (3.50)

Given the rotation quaternion ←→q = [qx qy qz qs]T, the sequence 3 − 2 − 1

Euler angles (α, β, γ) can be computed by: α = arctan 1−2qy2−2q2z 2(qxqy−qzqs) β = arcsin (−2(qxqz + qyqs)) γ = arctan 1−2qx2−2qy2 2(qxqs+qyqz) (3.51)

(61)

Chapter 4 Derivation of Equations of

Motion

In this chapter we will define the development for obtain the equations of motions for the multi-body system. In the first part a description of conven-tions are shown, such as reference frames and unit vectors definiconven-tions. Then, a simpler matrix formulation to implement is derived from Newton-Euler equation, in the case of two bodies connected with a joint. This formulation is then generalised for an N-bodies case in order to define the most general case.

The last passage will be the introduction of two additional assumptions, one regarding the DoF constrained in the joints, second regarding the forces in the joints.

4.1 Topology and Parameterization of

Multi-Body Systems

The study in this research covers the development of a tool useful for a generic multi-body system. In order to use this tool in the future, all one would need is the geometric dimensions and mass related parameters of the generic body. For the following description of the tool we use three

(62)

different generic bodies to synthesize three parts of a particular satellite. The first body is cubic, and has homogenious mass distribution and will be henceforth referred to as “Main Body”. This Main Body represents the entire satellite with all internal components except for the wheel(s) and robotic arm. The second body taken into account is a homogenious disc. This body represents a reaction wheel of the satellite and rotates around one of the principal axes (in real life situations this axis is usually not parallel to one of the principal axes of the Main Body). The third body is homogenious cylinder and represents the link that make up a robotic arm. The length of each cylinder is determined by its location on the robotic arm. In order to determine the distribution of mass of each body, we will consider the moment of inertia with respect to the principal reference frame defined below. In this way, the matrix of the moment of inertia will have values different than zero just in the diagonal of the matrix. We will consider for each body three different kinds of reference frames:

• Inertial Reference Frame FI: It is a frame of reference that describes

time and space homogeneously, isotropically, and in a time-independent manner.

• Mechanical Reference Frame FM: In our assumption, this frame

is parallel to the baricentrical principal reference frame and will be centered with a convenient fixed point in the body. This frame is used to define every position vector of the single body. We use the Mechanical Reference frame as alternative to the CoM so as to provide a constant point that will not change during the motion of the body. This will ensure the position vector of every point of the body with respect to this frame does not change.

• Barycentric Reference Frame FCoM: This frame has its origin in

the center of mass (CoM ). The axes of this frame are parallel to the principal axes of inertia and the moment of inertia matrix is constructed with respect to this frame.

(63)

• Joint Reference Frame FJ: This frame is centered in the joint point

and will be used to define the degree of freedom of the joint. In particu-lar, we will consider for each joint two reference frames which have one axis in common. One is internal to the joint and is connected to the inner body. The other is external to the joint and is connected with the outer body. The rotation angle between the two joint reference frames is the degree of freedom of the rotational joint.

The study of a multibody system begins with the definition of the different body position vectors of interest. These vectors are expressed in the mechan-ical reference frame of the body, and thus will remain constant during any motion. In addition, we need to know the types of joints and their placement in the mechanical reference frame.

Below we define the notation, which has been used for the development of this particular project. The notation choice is completely arbitrary and sub-ject to individual user definition [6].

• The position of the center of mass of a body is described relative to the Mechanical Frame of the body (for example: r1CoM 1,where “1”

indicates the origin of FM 1and “CoM 1” the position of center of mass).

• The position vector of a joint is described relative to the Mechanical Frame of the body (For example: r1JaI, where JaI indicate the “inner”

part of the joint Ja, as we will see later).

• For each joint (here e.g. Ja) we define two reference frames, one “inner”

frame FJaI and one “outer” frame FJaO. Their origins may coincide or

are described by the vector distance between the inner and outer part of the joint rJaIJaO.

(64)

in the Inner Joint Reference Frame:

F_JaIrJaIJaO ≡ rJaIJaO (4.1)

• In our choice the Inner Joint Reference Frame is parallel to the Mechan-ical Frame of the inner body, while the Outer Joint Reference Frame is parallel to the Mechanical Frame of the outer body.

• The rotation between two bodies connected in a joint will be performed between the two frames of the same joint.

• Joint torques are considered positive when they act on the inner body and negative otherwise. The torque is defined in the Inner Joint Ref-erence Frame:

F_JaITJa ≡ TJa (4.2)

• In each joint will be define its rotation, e.g. for joint Jb, where will

define the rotation ub.

The sequel of the development will be constructed using the example figure above, whose Mathematica Notebook can be found in Appendix D. It is a multibody system, in which the body 1 is the “main body”. To it are at-tached through two rotational joints (Ja, Jb) the bodies 2 and 3. Body 3 is

also connected to body 4(Jc), to simulate a robotic arm with two links. Each

joint has a single degree of freedom, which is a single rotation around the Z-axis that is common to all systems. Once we have defined all the parame-ters of bodies in the body frames, we can start with the kinematic study of the system. From the definition of shape, size and mass of each object, we can calculate the moments of inertia matrix in the Barycentric reference system. By definition, it will be diagonal and will be transformed into the Mechanical reference system through the parallel axes theorem, leveraging our hypotheses of parallelism between the mechanical and the Barycentric reference system of the body.

Modelling, Simulation and Visualisation of Multibody Dynamics

Visualisation of Multibody-Satellite

Dynamics

UNIVERSITY OF PISA

Department Of Aerospace Engineering

Master Degree in Space Engineering

Francesco Infante

La migliore compagnia che la vita

potesse donarmi.

Acknowledgements

Contents

Chapter 1

Introduction

1.1

Motivation of Thesis

1.2

Examples of Multi-Body Systems

1.3

Classification of Multi-Body System

1.4

Terminology and Notation

Chapter 2

Overview of Multi-Body

Modelling Methods

2.1

Newton-Euler Approach

2.2

Lagrangian Approach

2.3

D’Alembert’s Principle

2.3.1

General Case with Variable Masses

2.3.2

Derivation for Special Cases

2.4

Comparison and Suitability of Modelling

Approaches for Problem at Hand

Chapter 3

Description of Position and

Rotation of Rigid Bodies

3.1

Degrees of Freedom

3.2

Representation of Rotation

3.2.1

Direction Cosine Matrix (DCM)

3.2.2

Euler Angles

3.2.3

Euler Symmetric Parameters (Quaternions)

3.2.4

Advantage and Disadvantages of Different

Rep-resentation of Rotation

3.3

Conversion Between Different

Represen-tations of Rotation

3.3.1

Direction Cosine Matrix ↔ Euler Angles

3.3.2

Direction Cosine Matrix ↔ Quaternion

3.3.3

Euler Angles ↔ Quaternion

Chapter 4

Derivation of Equations of

Motion

4.1

Topology and Parameterization of

Multi-Body Systems