EOM with Newton-Euler Equations - Multibody … dynamics...Skew matrix notation: 0 , if , , 0 ªº...

EOM with Newton-Euler Equations

Concurrent Dynamics International

December 2017

copyright © Concurrent Dynamics International 12/7/2017

Content

• Introduction

• Notations

•

• O(N) Recursive Algorithms

• Acceleration Equations

• Dynamics Equations

• Forward & Inverse Dynamics

• Constraint Forces

• Computing System Mass Matrix

• O(N) Solution of EOM

• Examples

• Summary

• References

12/7/2017

Influence Matrices and T

copyright © Concurrent Dynamics International

Introduction

•

•

12/7/2017 copyright © Concurrent Dynamics International

Equations motion of a rigid-body system using the Newton-Euler equations is

considered. The influence matrix and its companion operators are used to

derive them. The acceleration solution by both

A

N

3order( ) and order( ) methods

are presented. Notably, Lagrange multipliers are not needed for the derivations given

here.

N N

3Much has been published on the order( ) method in the 60's and 70's

6 to 10 . The dynamics equations derivation shown here is post 1990's

similar to the work by Radriguez-Jain-Kreuze 1 , except for a di

N

fference

in the definition of the influence matrix operator .

•

•

•

12/7/2017

1: 1:

Generalized coordinates and system rates for these are

,

where inboard joint angle of

i ii N i N

i i

MBS

q q

b

Ensuing discussions cover 1) dynamics equations of an , 2) solving the

dynamics equations for the system accelerations, .

MBS

q


Tree-structured rigid multibody systems that have only single axis rotational

motion between joint connected bodies are considered to expedite the discussion.

The root body is connected to the grou

MBS

nd in all cases considered. These systems

encompass robots, mechanical arms, multi-fingered hands, cranes, gimbaled

antennas, transmission and suspension systems.

Fig. 1 Notations

12/7/2017

Inertial frame

bj

bi

x

z

y

dj sj

si

joint1

jointj

ir

i

cmi

yi

external force

external torque

i

i

f

if

i body

inertial position of joint

inertial position of cm

impact position from joint

joint position from joint

cm position from joint

angular rate of

i

i i

i i

i i i

i i ip

i i i

i i

b i

r

y f

d

s

b


jointi

i

• The root body b1 is the reference body whose position and attitude serve as the starting value to compute the same for other bodies in the system in a hierarchical manner. In the following, b1 is connected to the ground

• Body indexing rule used here is the Parent-First order meaning that the index of a body is always a lower integer number than the indices of its children.

• The chain of bodies between b1 and bj shall be denoted as The less-than-or-equal relation over body indices is a topological order and not a

numerical order.

• The set of bodies branching from bj shall be denoted as The greater-than-or-equal relation over body indices is a topological order and not a

numerical order. • All vectors in the following discussion are given in the format xi

j . The subsript i denotes the body that x belongs to and the superscript j denotes the coordinate frame that the vector is in.

• Vectors with no superscript are given in inertial coordinates unless defined otherwise

12/7/2017

{ | } or just .i i j i j

{ | } or just .i i j i j


•

12/7/2017

1 2 3

A stacked vector y is a column vector whose elements are also vectors.

The latter can be of different sizes. We define this stacked vector as

[ , , , , ], is a vector

where size( )

N iy y y y y y

y

1

1 2 3

1

1 2 3 2

3

( ) 1, ( )= ( )

For example:

Let , ,

Then [ , , ]

N

j

j

x

x

x y

y

z

x

x

y

x

y

z

len y len y len y

ws

y v y y ws

w

v

sy

sy y y y y

wy

w

w


•

•

•

•

•

12/7/2017

0

Skew matrix notation: 0 , if , ,

0

z y

z x x y z

y x

a a

a a a a a a a

a a

The relations { , , , } shall mean topological order in the following

expressions when used to group bodies, unless stated otherwise.

{ | ( ) } , indices of children of jj parent j b

identity matrix, depends on context e k k k


( ) , index of parent of iip parent i b

•

•

•

12/7/2017

,

6 6

Influence matrices , and , are defined by the parent child relations of the

considered mechanism and some block diagonal matrix :

if parent of [ ]

0 otherwise

T

i j i

i j

b b

,

6 6

Def. 1

if parent of [ ] Def. 2

0 otherwise

where

T

T j i j

i j

b b

1: 1 diag{ } , a block diagonal matrix, with R , 0

integer > 1

k k

j j N i

k

Forward influence matrix, is strictly lower triangular.

Influence Matrices and T


Backward influence matrix, is strictly upper triangular.T

•

•

•

12/7/2017

1: 1Let { } , , =0, and [ ], [ ], with

, , 1.

is a column vector representation of the forward element-

by-element (parent-to-child) calculations

Prop1.

k k

i i N i i i

k

i i

diag R x col x z col z

x z R k

z x

for 1: (P1)

is a column vector representation of the backward element-

by-element (child-to-p

Prop2.

i i ip

T

z x i N

z x

arent) calculations

for 1: (P2)T

i j j

j i

z x i N

1Each body in a tree-configured mechanism has one parent body except for .

Thus, each row of has only one non-zero submatrix entry and the first

row is all zeros.

b

.

1

Given that is square and strictly triangular, it is nilpotent of degree , i.e.

0

where, length of the longest link in the system from , and .

m

m

m b m N


•

•

•

12/7/2017

1

2 1

If is square and strictly lower triangular, then ( ) exists

and can be expressed as

(P3)

where ni

m

e

e

m

lpotency of A


1

,2 , 1

It follows that is square and strictly upper triangular, and ( ) exists

and can be expressed as

T T T

T T T T m

e

e

(P4)

1 1

6

The power series expansion of and serve to show the existence of

( ) and ( ) . More efficient way of computing and

for any R is shown shortly.

T

T T

N

e e x x

x

Fig. 2 Example Body Sets

12/7/2017

2 1

4 3

6

5

8 7

10 9

branch1={j ≥ 1}={1:10} branch2={j ≥ 2}={2} branch3={j ≥ 3}={3:8} branch4={j ≥ 4}={4,7,8}

1 {2,3,9}

2 6 8 10 { }

3 {4,5}

4 {7}

empty

{j ≤ 2}={2,1} {j < 5}={3,1} {j ≤ 6}={6,5,3,1} {j ≤ 10}={10,9,1}

10 body example


For Fig. 2 Example

12/7/2017

A

2

3

4

5

6

7

8

9

10

0

0

0 0 0

0 0 0

0 0 0 0

0 0 0 0 0

0 0 0 0 0 0

0 0 0 0 0 0 0

0 0 0 0 0 0 0 0

0 0 0 0 0 0 0 0 0


For Fig. 2 Example

12/7/2017

2

3

4 3 4

1 5 3 5

6 5 3 6 5 6

7 4 3 7 4 7

8 7 4 3 8 7 4 8 7 8

9

10 9 10

If is a square submatrix for this example, then by (P3)

0 0

0

0 0

0 0

0 0 0

0 0 0

0 0 0 0 0 0 0

0 0 0 0 0 0 0

i i

e

e

e

e

ee

e

e

e

e

e

nilpotency( )=5


Fig. 3 Nilpotency 2 System

12/7/2017

Inertial frame

x

z

y

hinge1 1

b1

b2

b3

b4

bN d2

d3

d4

dN


•

•

12/7/2017

2

3

By Defs. 2 and 3, if is a square matrix, then for nilpotency 2 systems per (P3) are

with

0 0 0 0

0 0 0

0 0 0

0 0 0N

e


2:

1

2

3

A nilpotency 2 tree configured system has all bodies as immediate

children of . See Fig. 3. For these systems the forward recursive operator per (P3) is

0 0 0

0 0

0 0

0 0

N

A

N

b

b

e

A e

A e

A e

O(N) Recursive Algorithms

•

•

•

12/7/2017

Examples presented earlier showed the lower-triangular nature of given the

parent-child relation of bodies in a tree-configured system.

Computing expressions involving or by direct matrix-vector

multiplication (i.e. using (P3) and (P4) for and ) is inefficient.

T

T

x x


The next two algorithms are order( ) computations of those expressions.N

•

•

12/7/2017

A stacked vector given by is the result of solving for z,

where is square and strictly lower triangular per Def 1. Thus, given ,

the elements of are obtained by the algorithm bel

z x z z x

x

z

1 1

ow :

1. set since the first row of is zero

2. for 2 :

;

end

i i ip i

z x

i N

z z x

The above computation of given is order( ). Thus is an order( ) operator.z x N N


Algorithm for z x

•

•

12/7/2017

A stacked vector given by is the result of solving for ,

where is square and strictly upper triangular. Thus, given , the elements

of are obtained by the algorithm below :

T T

T

z x z z x z

x

z

1. set since the first column of is zero

2. for : 2

: ;

end

T

T

ip ip i i

z x

i N

z z z


Algorithm for Tz x

The above computation of given is order( ). Thus is an order( ) operator.T

z x N N

Acceleration Equations

•

•

12/7/2017

Relation between the two accelerations is

(2)

0where

( )

i i i i

i

i i i

v S v A

As

Relation between and is (See Fig. 4 )

(1)

where [ , ], [ , ] and to

j j

i i i

i i i i i i i

v v

v S v

v r v

1 1

tal angular rate of

velocity of , velocity of joint

0 since joint is attached to ground in the considered system

0 , displacement f

i

i i i i

i i

i

b

r cm

eS s

s e

rom joint to i i

cm


•

12/7/2017

1:

Total accelerations of (2) in stacked column form is

(3)

where [ ] , [ ]i i N i

v Sv A

v col v S diag S

1: 1: 1:

, [ ] , [ ]i N i i N i i N

v col v A col A


•

•

12/7/2017

1 1

Relation between and the relative rate is

(4)

0where , 0, 0

i i

i i ip i i

i p

i

v q

v D v G q

eD D v

d e

3 1 [ , 0 ] for 1:

i iG g i N


Given (4), the relation between the total acceleration and the relative

acceleration is

i

i

i i ip i i i

v

q

v D v G q A

1

(5)

where , 0 (5a).( )

ip i i

i i ip i i

ip ip i

gA D v G q a

d

•

•

•


1:

Given Def. 1 and Prop. 1, Eq. (5) can be put in column vector form as

(6)

where, [ ]

D

i i

v v Gq A

v col v

1: 1: 1:

1:

, [ ] , [ ] , [ ]

[ ]

N i i N i i N i i N

i i N

D diag D G diag G q col q

A col A

1

Total acceleration in (6) can be solved to be

( ) (7)

where ( )

D

D D

v Gq A

e

Given (7), Equation (3) becomes

( ) (8) D

v S Gq A A

Fig. 4 Forces and Torque on bi

12/7/2017

1

Inertial frame

cmi

x

z

y

dα

si

hingeα

τα

fα

fi

τi

ωi

yi

ir

i

bi

hinge1

if

i

body

hinge position from hinge

y impact positionfrom hinge

f interbody force from to

external force on

interbody torque from to

external force on

i

i

i i i

i ip i

i i

i ip i

i i

i

b i

d

f

b b

f b

b b

b

angular rate of ib


hingei

Dynamics Equations

•

•

12/7/2017

Newton-Euler equations for each body of an per Fig. 4 are

(9)

( ) ( ( ) )

r

i

i i i i i i i i i i i i

j

i

i i i i

i

b MBS

s f I I y s f

d s f

f m f f

(10)

Eqs. 9 and 10 can be made into a 6 x 1 equation to become

(11)

0where , ,

00

T T T

i i i i i i i

i

i iT

i i

i i

F D F S M v B Y F

Ie dF D M

f me

, ,0

, (see Fig. 4 for def. of variables.)0

i i i i

i i

i

iiT

i i

i

Iv B

e r

e yY F

fe


12/7/2017

, joint torque-force pair

, parent-to-child influence matrix0

0, cm centered mass matrix

0

, acceleration pair

, nonlinear force 0

i

i

i

T

i

i

i

i

i

i

i i i

i

Ff

e dD

e

IM

m e

vr

IB

pair

, backward moment-force influence matrix0

, external torque-force pair

iT

j

i

j

i

e yY

e

Ff


•

•

•

•

12/7/2017

1: 1:

Given Def . 2 and Prop. 2, 11 can be put in column vector form as

(12)

where [ ] , [ ] ,

T T T

D

T

i i N i i N

F F S Mv B Y F

F col F D diag D S

1: 1:

1: 1: 1: 1:

[ ] , [ ] ,

[ ] , [ ] , [ ] , [ ]

T

i i N i i N

T T

i i N i i N i i N i i N

diag S M diag M

v col v B col B Y diag Y F col F

1

in (12) is solved to be

( ) (13)

where ( )

T T T

D

T T

D D

F

F S Mv B Y F

e

Use (8) in (13) to get

( ( ( ) ) ) (14)T T T

D DF S M S Gq A A B Y F

f is the actuation force/torque from to along the moving

joint axis. The complement of that force in are the constraint forces/torque

that prevent motion along the constrained axes of

T

i i i ip i

i

G F b b

F

joint( ).i


•

12/7/2017

1:

After some rearrangement, 14 becomes

( ) (15)

where

{ }

T T T

D D D D

T T

i i i i i N

i i i i i

i

F M Gq M A B Y F

M S MS diag S M S diag M

I m s s m sM

1:

(16) , joint centered mass matrix

( ), nonlinear force

( )

i

i i i

T

i i N

i i i i i i

i

i i i i

m s m e

B S MA B col B

I m s sB

m s

(17)


•

12/7/2017

By projecting of 15 onto the free joint axes, the dynamics equations

becomes

f ( , ) (18)

where f , actuatioT

F

q C

G F

n force/torque along free axes (18a)

( , ) ( ) (18b)

, system mass matrix

T T T

D D

T T

D D

C G M A B Y F

G M G

(18c)


•

12/7/2017

1

1

Acceleration state vector in (18) can be solved to be

(f ( , ) ) (19a)

[ ] [ ](f ( , )T T T T

D D

q C

e K G R e G K C

1

1:

1:

1:

) (19b)

(See [1][2] on the factorization of . Note that the operator in [2] is the

operator here. )

where [ ]

[ ]

[ ] , w

i i N

T

i i i i N

i i N

q

diag e G K D

D diag D

E

1 6 6

1 1

1:

1

1:

1:

ith 0

[ ] ,

inertia tensor

[ ] ,

[ ]

T

i i N i i i i

i i

i i N i i i i

i i N

D

R diag R R G J G

J branch

K diag K K J G R

G diag G


•

12/7/2017

1:

1

Backward recursive computation of branch inertia tensors :

a) for 1: ; ; end

b) for : 2

ˆ

ˆ

end

(see Ref

i i N

i i

T

i i i i i i i

T

ip ip i i i

J

i N J M

i N

J J J G R G J

J J D J D

. [1] and [2].)


•

•

•

•


Note that (18a) follows from the definition of f. Each f of f is the net

force/torque from the control and the spring/damper forces transmitted along

the free axis of joint .

i

i

Eqs. 19a and 19b [2] are two acceleration state solution options.

Eq. 19a requires the assembly of using 18c . While that matrix is

fractored is an advantage in the assembly process. However, acceleration

solution of 18 by any linear equation solvers involving is3

an order( )

process.

N

Eq. (19b) requires that (f ( , ) ) be operated from right to left using the

indicated operators. Every one of those serial operations is an O( ) process.

Therefore, the solution process by (19b) is O

C

N

( ). N

Forward & Inverse Dynamics

•

•

12/7/2017

Forward dynamics regarding (18) is the problem of solving for the system

accelerations given the interbody forces along the free joint axes and the

external forces, i.e. {f , }. This is effectivelyi i

q

F (19a, or 19b).

*

Inverse dynamics regarding (18) is the calculation of the required interbody

forces along the joint free axes {f } given the system accelerations and

the external forces, i.e. { , }.

i

i iq F


Constraint Forces

•

•

12/7/2017

6 5

Proposition 5. Given that f is the actuation force/torque along the

ˆfree axis of ' inboard hinge, then f is the constraint forces/torque

at the same hinge, where

ˆ R , c

T

i i i

c c

i i i i

i

G F

b s G F

G

1 5 5 5

ˆ ˆ ˆonstraint matrix; 0 , 1, (20)

ˆMore specifically, if is a particular column of , then f is the

constraint torque (or force) along .

T T T

i i i i i i

c c cT

i i i i i

c

i

G G G G G G e

G G G F

G

Given that the system accelerations are solved by 19a, or 19b , then all the

interbody forces can be determined by 15 .i

F


•

12/7/2017

3 1

Example: Let - be the free axis of joint frame and the request is for the

constraint force along the - of that frame. Given that has been computed

by (15), then

, 0 ,

i

i

x

i i

x axis

z axis F

G e

3 1

3 1

0 ,

Ans: f 0 ,

where unit - of joint frame in inertial coordinates.

c z

i i

Tc z

i i i

z

i i

G e

e F

e z axis


Computing System Mass Matrix

12/7/2017

Procedure to compute per (18c) :

1. Compute 2. Compute

with [ (:,1) (:, 2) (:, )] with [ (:

DJ G Q MJ

J J J J N Q Q

,1) (:, 2) (:, )]

[ (:,1) (:, 2) (:, )] { ( ), 1: }

for 1: for 1:

(:, ) (:, ) ; per AlD

Q Q N

G G G G N M diag M i i N

i N i N

J i G i

g.1 ( ,:) ( ) ( ,:) ; row block matrix mult.

end end

3. , end

Q1: what is the order of the above pr

T

Q i M i J i

J Q

ocedure?

Q2 : can it be made order( )?N


O(N) Solution of EOM

•

12/7/2017

0 1 2

1 1:

1:

For { , , , }, do {

Step 1: Given [ , ] at compute :

a. compute { } given { , }

b. compute { , , , , , , , , , , , , , }

end

k

i i i N

i i i i i i i i i i i i i i i N

t t t t t

q q t q

C q q

I g d s r y D G M J K R

1:

1:

c. compute { , , } given #a, #b and

d. compute { , , f } , application specific input

e. compute { , } per (5a) and (17)

i i i i N

i i i i N

r q

f

A B

3

f. solve (18) for

O( ) opt1: compute (f ( , ) ) per (18a,b,c) and use (19a)

O( ) opt2: compute (f ( , )) per (18a,b) and use (19b)

Step 2: Given [ ,

q

N C

N C

q

1, ] at , integrate [ , ] to }

k kq q t q q t


Examples

• The following examples illustrate how to use the methodology presented earlier to derive the eom of specific cases. These derivations can then be compared with other hand derived eom for the same cases for verification.

• It is recommended that the O(N) solution procedure shown earlier be used to develop a generic program that solves all tree-configured N-body dynamics. That program can easily solve the examples presented and produce identical dynamic responses as those produced by solving the hand derived eom.

12/7/2017 copyright © Concurrent Dynamics

International

Example 1 Simple Pendulum

12/7/2017

θ cm1: I1, m1

s1

Inertial frame

x

y

z(out)

hinge1:ez pendulum angle

gravity accelerationg

yge

1 0


Model Parameters

12/7/2017

1 1

1

1

1

1 1

1 1

0,

(sin( ) cos( ) ) ; cm position from hinge

0 ; external force

f ; hinge torque

, with 0

z

x y

y

T

z

e

s L e e

Fm g

G F

eG G G

6 6

1

6

(M1)

0 (M2)

( )

D

D De e

6 (M3)


•

•

12/7/2017

1

2

1 1

1 1 1

1 1

Let ( , ) ( ) per (18b)

with 0 and for this example.

Thus, given (17) and (M3), we have

0

[0, ]

[ , ]

( ) [

T T T

D D

D

T

T

y y

T T

D

C G M A B Y F

A e

e sY

e

B m s

Y F s m ge m ge

B Y F s m

2

1 1 1

1

, ]

f ( , ) sin( )

y yge m s m ge

C m gL

2

1 1 1

per 18 for this pendulum is

( ) g sin( ) (M4)zz

EOM

I m L m L


Example 2 Pendulum with a Moving Base

12/7/2017

Inertial frame x

θ cm2: I2, m2

b1: I1, m1

s2

x

y

z(out)

(0,0) hinge1

hinge2:ez

xfe

cart force

pendulum angle

gravity acceleration

f

g

yge


Model Parameters

12/7/2017

2 1 1 2

1 2

1 2

1 2

0 , , 0,

0, (sin( ) cos( ) ) ; cm position from hinge

0 0, ; external forces

Let f [0,0] ; joint transmitted

x z

x y

x y y

T

d xe e

s s L e e

F Ffe m ge m ge

G F

1 2 1 2

6 6

2

1

force/torque

0{ , }, with , (N1)

0

0 0 0 0, where 0 0 nilpotency( ) 2 (N2)

0 0

( )

z

x

D D

D D

eG diag G G G G

e

D e

e e

6 6

6 6 6 6

0 (N3)

D

e

e e


•

12/7/2017

1

2

2 2

2

1 6 6 2

1 2 2

Let ( , ) per (18b), and we have 0

and 0 for this example. Given (17) and (N3), we have

[[0,0],[0, ]]

0 ,0

[[0, ],[

T T T

D D

T T

T

x y

C G M A B Y F A

B m s

e sY Y

e

Y F fe m ge s m g

2

2

2 2 1 2 2 2

2

2 2 2 2 2

2

2 2

, ] ]

[[ , ],

[ , ] ]

( , ) [ ( ) , g ( )]

Given (18a) and f [0,0] for this cas

y y

T T

D y x y y

y y

e m ge

B Y F s m ge fe m ge m s m ge

s m ge m ge m s

C f m Ls m Ls

2

2 2

e,

f ( , ) [ ( ) , g ( )]C f m Ls m Ls


•

12/7/2017

21 2 2 2

2

2 2 2 2

EOM for this pendulum on a moving base is

cos( ) sin( ) = (N4)

cos( ) g sin( )

The system mass matrix in (N4) was deriv

zz

m m m L x f m L

m L I m L m L

ed using Eq. (18c)


Summary

12/7/2017

• EOM for a tree configured multibody system using generalized coordinates and Newton-Euler equations has been presented.

• EOM formulation here requires no joint constraints to enforce. One can extract the interbody constraint force or torque at any joint from Eq. (15), once the system accelerations have been determined.

• O(N3) and O(N) solution to the EOM, Eq. (18), have been presented. (See Eq. (19a) and Eq. (19b).)


References

1. Rodriguez, G., Jain, A., Kreutz-Delgado, K., ‘Spatial Operator Algebra for Manipulator Modeling and Control’, The International Journal of Robotics Research, Vol. 10, No. 4, August, 1991.

2. Tong, M. ,’Inverse Mass Matrix Factorization Using Momentum Equations of a Rigid Multibody System’, AIAA Guidance, Navigation and Control Conference, 13-16 August, 2012, Minneapolis, Minnesota, Paper No. 1359569

3. Greenwood, D., ‘Classical Dynamics’, Prentice-Hall, Inc., Englewood Cliffs, New Jersey, 1977.

4. Goldstein, H., ‘Classical Mechanics’, Reading, Mass: Addison-welsley Press, Inc. 1950.

5. Pars, L.A., ‘A Treatise on Analytical Dynamics’, London: Heinemann, 1965.

6. Hooker, W. W., and Margulies, G. 1965. The Dynamical Attitude Equations for an n-Body Satellite. J. Astronautical Sciences, vol. 12, no. 4, pp. 123-128.

7. Likins, P.W., 1975. Point-Connected Rigid Bodies in a Topological Tree. Celestial Mechanics, Vol. 11, No.3, 301-317..

8. Wittenburg, J., 1977. Dynamics of Systems of Rigid Bodies. Stuttgart: B. G. Teubner.

9. Ho, J.Y.L, 1977. Direct Path Method for Flexible Multibody Spacecraft Dynamics. Journal of Spacecraft and Rockets, Vol. 14, pp. 102-110.

10. Orlandea, N., Chace, M. A., and Calahan, D. A. 1977. A Sparsity-Oriented Approach to the Dynamic Analysis and Design of Mechanical Systems|Part1. Trans. ASME J. Engineering for Industry, vol. 99, no. 3, pp. 773-779.


EOM with Newton-Euler Equations - Multibody … dynamics...Skew matrix notation: 0 , if , , 0 ªº...

Documents

Transcript of EOM with Newton-Euler Equations - Multibody … dynamics...Skew matrix notation: 0 , if , , 0 ªº...