Dynamics of PKM

Prof. Rosario Sinatra

Dipartimento di Ingegneria Industriale e MeccanicaUniversità degli Studi di Catania

March 27, 2007

EURON07 WINTER SCHOOL PARALLEL ROBOTS: Theory and Applications 2nd International UMH Robotics Winter School Flamingo Oasis Hotel, Benidorm, SpainMarch 26 - 30, 2007

Physical System

S

P

U

BP

MP

Mechanical and Iconic Models

W Ci i i i i i i

W Ci i i im

I I n n

c f f

Physical Laws

n

d L L

dt q q

I CMathematical Model

Analysis

Multibody Dynamic Systems

Software for Multibody System Simulation ADAMS by MSC Software, United States alaska , by Technical University of Chemnitz, Germany AUTOLEV , by OnLine Dynamics Inc., United States AutoSim by Mechanical Simulation Corp., United States COMPAMM by CEIT, Spain Dynawiz by Concurrent Dynamics International DynaFlexPro by MotionPro Inc, Canada Hyperview and Motionview by Altair Engineering, United States LMS Virtual.Lab Motion by LMS, Belgium MECANO by Samtech, Belgium MBDyn by Politecnico di Milano, Italy MBSoft by Universite Catholique de Louvain, Belgium NEWEUL by University of Stuttgart, Germany RecurDyn by Function Bay Inc., Korea Robotran by Universite Catholique de Louvain, Belgium SAM by Artas Engineering Software, The Netherlands SD/FAST by PTC, United States SimCreator by Realtime Technologies Inc., United States SimMechanics by The Mathworks, United StatesSIMPACK by INTEC GmbH, Germany SPACAR by University of Twente, The NetherlandsUniversal Mechanism by Bryansk State Technical University, Russia Working Model by Knowledge Revolution, United States

http://real.uwaterloo.ca/~mbody/

ii

i

ωt

c

ii

i

nw

f

E Ci i i w w w

THE NATURAL ORTHOGONAL COMPLEMENT METHODFirst introduced by Jorge Angeles and Sangkoo Lee [1, 2]

Preliminary Definitions

twist of i-th rigid body:(1)

wrench of i-th rigid body:

(2)

(3)

Figure 1: notations

ii

ω 0W

0 0

= 1, 2,..., E Ci i i i i i i i r M t WM t w w

3i

im

I 0M

0 1

the 6 × 6 extend angular velocity matrix Wi and matrix extend mass Mi

Newton-Euler equation for i-th body:

angular velocity matrix of i-th rigid body:

( )ii i

ω x

ω 1 ωx

(4)

(5)

(6)

1 2 rM diag(M ,M ,.....,M )

T T T T1 2 rt [t ,t , ...., t ]

1 2

T T TE E E E Trw [w ,w ,....,w ]

1 2

T T TC C C C Trw [w ,w ,....,w ]

T T T T1 2 rt [t ,t , ...., t ]

UNCONSTRAINED DYNAMICAL EQUATIONS

where:

1 2 rW diag(W ,W ,.....,W )

N-E equations:

(7)

(8)

(9)

(10)

(11)

(12)

E C .

M t WMt w w

Figure 2: notations

t Tq

T C t w 0

T T C T C q T w 0 T w 0

T C T w 0

E G J D w w w w

C Tw K λ

0 = Kt b = Kt 0

p nKT O

KINEMATIC CONSTRAINTS

where K is p×6r constrains matrix:

where T is 6r×n twist shaping matrix:

the power developed by the constraint wrench wC is zero:

the external wrench is:

the matrix T is an orthogonal complement of K

(13)

(14)

(15)

(16)

(17)

(18)

(19)

(20)

.T T T E T M t T WMt T w

t T T

T T T T E T MT T M T T WMT T w

: Tn n genralized inertia matrix I T MT

1 are ( , ) ( )Tn vector of quadratic terms of inertia force C T M T WMT

1 : T Jn vector of torques T w

1 : T Gn vector of dissipative forces T w

( ) ( , ) I C

CONSTRAINED DYNAMICAL EQUATIONS

where

Euler-Lagrange equations of the system:

(21)

(22)

(23)

(24)( ) ( , ) I C

( )i i 1 i i i i i 1 ω ω e E ω ω 0

1 1 1

1

( )

i i i i i i i

i i i i i-1 i 1

ω

ω ω

c c ω a ρ ρ

c c R D 0

1: CPM of ( )i i i R a ρ : CPM of i iD ρ

DERIVATION OF CONSTRAINT EQUATIONS AND TWIST-SHAPE RELATIONSOF THE SYSTEM WITH SIMPLE KINEMATIC-CHAIN STRUCTURE

Let be Ei the cross product matrix of vector; for the i-th revolute joint

and

;

Figure 3: a revolute jointOO

O

O

O

i

i-1

i+1Fi

Fi+1

C

C i

a i-1

ai

i-1

i-1

i

e

e

i

i+1

(i-1)thlink

ithlink

i+1

i

(25)

(26)

1

, 2

1 1

1 1

11 1

i i i 1 i i i

ω

ω

i ,...,r

E 0

c R 0

K t 0

K t K t 0

, , 111 6×6 3×3 3×3

1

E OK O 1 O

R 1

ii,i-1

i-1

E OK

D 1i

iii

E OK

R 1

if the first link is inertial:

where

,

r r

r r r r

,

K O O O O

K K O O O

K

O O O K O

O O O K K

Matrix K is:

(27)

(28)

(29)

(30)

Next, the link twists are expressed as linear combinations of the joint-rate vector θ

Figure 4: kinematic subchain; links J,J+1,…i

,

j

j ij

ij

if j i

otherwise

e

e rt

0

0

whose -th column is given asijj t

.... ......

....... ....

........

j j 1 i 1 i

iij

if j i

if j i

otherwise

a a a ρ

r ρ

0

when

(31)

(32)

then the twist-shape matrix T is:

2r r rr r r

t 0 0

t t 0T

t t t

(34)

1 1 2 2 , 1,...,i i i i ii i r t t t t

twist ti of i-th link as linear combination of the ith joint:

(33)

Figure 5: a prismatic joint OO

c

ci-1

i-1

ai-1

Fi

i

Oi

Fi+1

ai

b ii

d i-1

i+1e

ei

(i-1) linkth

i linkth

O i+1

Oi-1

i+1

for the i-th revolute joint

i i 1ω ω

1 i 1i i i 1 i i i i i( b ) e c c δ ρ e bω

i i-1 i R D R

: CPM of ( )i i i ibR ρ e 1: CPM of i iD δ

If i-th is a prismatic joint :

We introduce a definition below:

(35)

(36)

(37)

1i i i i i i' e c c R ω b 0

1i i i i( ω ) iE c c R' 0

eq. (40) can be rewritten:

Multiplication by Ei

a 6-dimensional linear homogeneous equation in the ti:

1i,i-1 i ii it t K K' 0

(38)

(39)

(40)

1i ii

,

1 OK

0 E ,i ii i i

1 OK

E R E

011 1t K

11

1 0K

0 E

.... , 1,.......,i 1 i1 k ik i ii i r t θ t b t θ t

0ik

k

te

where

if the first joint is prismatic:

where

if the kth joint is prismatic, with 1<k <i:

(41)

(42)

(43)

(44)

(45)

T

;( )

otherwise

j j

j j ij j ijij

if j i

ω e

ω e r e rt

0

0

DERIVATION OF MATRIX

for the i-th revolute joint

ij j j j 1 j 1 i i...... r ω a ω a ω ρ

0ik

k k

t

ω e

where

for the k-th pair is prismatic and 1<k <i

(46)

(47)

(48)

ww is the external wrench applied in C of m M

DYNAMICS of Parallel Manipulators

Figure 6: J-leg of a simple platform parallel manipulator

Euler’s formula for Graphs (Harary, 1972)

1 36 32 1 5i j l

we label the legs with Roman numerals J = I, II, ….,VI

(49)

Figure 7: the free-body diagram of M

The twist of M

ωt

cM

MM

(50)

N-E equations of M:

VIw c

jj

M t W M t w wM M M M M (51)

q for =I, ... ,VI variable of the actuated joint of the th legJ J J

q vector of coordinate of six actuated ( n dof) joints

[ ]T

I II VIq q .... qq

(52)

, J J J = I, II,....,VIJ θ tM

is the Jacobian matrix of the th legJJ 6 6 J

The dynamics model is:

( , ) T CJ J J J J J J J J I θ C θ θ θ τ J w

(53)

(54)

JI

JC

Jθ

Jτ

: 6×6 inertial matrix of the manipulator;

:6×6 matrix coefficient of the inertia terms;

:6-dimensional vector of joint variables;

:6-dimensional vector of joint torques;

for J-leg:J1

J

J

J

.

.

.

θ

θ

θ

θ

0

0

J Jk

.

.

.τ

.

.

.

τ (55)

J = I, II,....,VI

where -k denoting the only actuated joint k of legJ J - th

J J JKfτ e (56)

forceJf

torqueJf

if k-joint is prismatic:

if k-joint is revolute:

mapping J Jq L

J Jθ L q (57)

from eq.(75):

C TJ J J J J J J

w J τ I θ C θ (58)

N-E equations of the moving platform free of constraints

VIw -T

J J J J Jj I

M t W M t w J τ I θ C θM M M M M

(59)

NOC method : t qM

t TqM(60)

which upon differentiation with respect to time:

t Tq Tq M(61)

then:

J( )

VI VI-T TJ J J J J J J

j I I

w

M Tq Tq W M Tq J I θ C θ w J τM M M

(62)

J J J θ L q L q

wVI VI

-T Tj J J J J J J J J

j I j I

M Tq M Tq W M Tq J I L q I L q C L q w J τM M M M

which upon differentiation with respect to time

Model in terms of actuated joint:

where

(63)

(64)

( ); ( , )J J J J I I q C C q q(65)

1J J

L J T

w( , )VI

J Jj I

M q q N q q q τ L τ

(66)

Final step is to formulate the model in terms only of actuated joint variables

(67)

where,

( )VI

j I

T TJ J J

M q T M T L I LM

( , ) ( )VI

T T

jj I

J J J J

N q q T M T W M T L I L C LM M M

M

W T Wτ T w

(68)

VIIII

IIII ,......,,diag

diag , ,......,I II VI

C C C C

.....I II VI

L L L .L

......I Ik II IIk VI VIk

Λ L e L e L e

......I II VI

TΦ f f f

If :

(36×36 matrix)

(36×36 matrix)

(6×36 matrix)

(6×36 matrix)

(6-dimensional vector)

T T M q T M T L ILM

( , ) ( , )T T N q q L IL L C q q LVI

IJ J

JL τ ΛΦ

w( , ) M q q N q q q τ ΛΦ

and hence

The mathematical model takes a form:

(69)

(70)

-1 w( , ) Φ Λ M q q N q q q τ

q r

-1 w, r M N q r r τ Λf

For purposes of the Inverse dynamics:

For purposes of the direct dynamics:

(71)

(72)

X

Z

Y

B

S C

I C c

NO

Y

x

ZS

BP

MP

U

U

UXi

Zi

Yi

O

IS

I1

il

i

i

I Ui

i

bi

it

t

t

i

P

hRPi

Figure 8: Hexapod with fixed-length legs

HEXAPOD with fixed-length legs

Natural Orthogonal Complement Method

Dynamic Modeling

N-E equation for each body

1,...,6,ii i i i i i p .

M t WM t w

ii

e

e

(73)

(74)

(75)

ii

OW

O O iim

I OM

O 1

assembled system dynamics equations are given as:

W N .

M t WMt w w

(76)1 2 pM diag(M ,M ,.....,M )

where and , (6p 6p) are:M W

1 2 pW diag(W ,W ,.....,W )(77)

(78)

and the 6 -dimensional generalized vectorsp

W1

W

Wp

w

w

w

1

p

t

t

t

N1

N

Np

w

w

w

(Angeles and Lee 1988) kinematic constraints

(79)60 pKt

(80)

t Ts

t Ts Ts

where is the natural orthogonal complement of .T K

E-L equations:

T a g d Is Cs T w w w

where

, ( )T T I T MT C T MT WMT

(81)

(82)

(83),

,

a T a g T g

d T d I

T w T w

T w Is Cs (84)

the inverse dynamics of the system can be given as:

a I g d

where is the vector representing the applied actuator forcesaτ

(85)

Inverse Dynamics generalized twist:

1

6

pc

t

t

t

t

(86)

for the moving platform :pct

p c h Rρ p c v ω Rρ

pc p pt H t

(87)

(88)

where

p

1 EH

0 1 is the CPM of and the twist-shaping-matrix isp iE Rp T (89)

1

6

p p

T

T

H T

T

(90)

DYNAMIC ISOTROPY AND PERFORMANCES

w( , ) M q q N q q q τ ΛΦ

( ) , n n R M q I

isotropy dynamic:

In the literature there are many performance indices:

- ASADA: generalized inertia ellipsoid (GIE);

-YOSHIKAWA: dynamic manipulability ellipsoid ;

-WIENS et al.: indices for measure of non linear inertia forces;

-KHATIB and BURDICK: Isotropy acceleration;

-MA and ANGELES: isotropy dynamic and dynamic conditioning index.

1W

2T TDCI R e e

dynamic conditioning index :

(91)

(92)

(93)

2 DOF POINTING SYSTEM

Figure 9

Figure 10

Point 1 2, rad 2

1 2( ) kgm

1 (0.0,0.683763) 0.042100

2 (0.0, 1.066059) 0.045507

Workspace 1 2, 0K

1 2, 0G

1

2

1

2p= 1

bL

= 0.433b

1

2

1 1E-L equations : Iθ Cθ

Spherical Parallel Manipulator

Figure 11: spherical parallel manipulator

(94)

ihih

ih

ωt

c

MM

M

ωt

c

ih ih 1t T θ

twist(i = I, II, III h = 1, 2)

(i = I, II, III h = 1, 2)

Twist-shaping matrix

i1i1

i1

AT

B

(i=I,II,III h=1,2)

i2i2

i2

AT

BM

MM

AT

B

with

M = platform matrix

TT T T T T T T

I1 I2 II1 II2 III1 III2 M T T T T T T T T

(95)

(96)

(97)

(98)

I1 I2 II1 II2 III1 III2 MdiagM M M M M M M M

0

0ih

ihihm

IM

1(i = I, II, III h=1,2)

I1 I2 II1 II2 III1 III2 MdiagW W W W W W W W

ihih

Ω 0W

0 0(i=I,II,III h=1,2)

generalized mass matrix M and generalized angular velocity matrix W:

(99)

(100)

(101)

(102)

References

J. Angeles, 2002, Fundamental of Robotic Mechanical Systems, Springer.

J. Angeles, and S. Lee, 1988, “The Formulation of Dynamical Equations of Holonomic Mechanical Systems Using a Natural Orthogonal Complement”, ASME Journal of Applied Mechanics, Vol. 55, pp. 243-244.

J. Angeles, and S. Lee, 1989, “The modelling of Holonomic Mechanical Systems Using a Natural orthogonal Complement”, Trans. Canadian Society of Mechanical Engineers, vol. 13, pp. 81-89.

K. E. Zanganesh, R. Sinatra and J. Angeles, 1997, Kinematics and Dynamics of a Six-Degree-of-Freedom Parallel Manipulator with Revolute Legs, ROBOTICA International Journal, Vol. 15, pp. 385-394.

O. Ma and J. Angeles, “The concept of dynamic isotropy and its applications to inverse kinematics and trajectory planning”, Proc. Of ICRA, Cincinnati, USA, 1990, pp. 481-486.

F. Xi, R. Sinatra, and W. Han, 2001, Effect of Leg Inertia on Dynamics of Sliding-Leg Hexapods. ASME Journal of Dynamics, Measurement and Control, Vol. 123, pp. 265-271.

F. Xi and R. Sinatra, 2002, Inverse Dynamics of Hexapods using the Natural Orthogonal Complement Method, Journal of Manufacturing Systems, Vol. 21, No 2, pp. 73-82.

F. Xi, O. Angelico and R. Sinatra, 2005. Tripod Dynamics and Its Inertia Effect. ASME Journal of Mechanical Design, Vol. 127/1 , pp. 144-149.

A. Cammarata and R. Sinatra, 2005, Dynamics of a two-dof parallel pointing mechanism, Fifth ASME International Conference on Multibody Systems, Nonlinear Dynamics and Control , Sept. 24-28, 2005, Long Beach, California, USA.

R. Di Gregorio, A. Cammarata and R. Sinatra, 2005, On The Dynamic Isotropy Of 2-Dof Mechanisms, Fifth ASME International Conference on Multibody Systems, Nonlinear Dynamics and Control , Sept. 24-28, 2005, Long Beach, California, USA.