NMT EE 589 & UNM ME 482/582 ROBOT ENGINEERING

21
NMT EE 589 & UNM ME 482/582 ROBOT ENGINEERING Dr. Stephen Bruder NMT EE 589 & UNM ME 482/582

Transcript of NMT EE 589 & UNM ME 482/582 ROBOT ENGINEERING

Page 1: NMT EE 589 & UNM ME 482/582 ROBOT ENGINEERING

NMT EE 589 & UNM ME 482/582

ROBOT ENGINEERING

Dr. Stephen Bruder NMT EE 589 & UNM ME 482/582

Page 2: NMT EE 589 & UNM ME 482/582 ROBOT ENGINEERING

© 2011, Dr. Stephen Bruder

7. Robot Dynamics 7.5 The Equations of Motion

Tuesday 6th Nov 2012 ME 482/582: Robotics Engineering

Given that we wish to find the path 𝑞 (𝑡 (in joint space) which minimizes the energy of the system

where the Lagrangian is (7.15)

From Calculus of Variations we know that the optimal path satisfies (7.16)

1min ( ) min ( , ) ndq q q dq dq К υ

1( , ) ( ) ( )

2

Tq q q D q q q υ

( ) ( )0

d

q dt q

Slide 2 / 21

Page 3: NMT EE 589 & UNM ME 482/582 ROBOT ENGINEERING

© 2011, Dr. Stephen Bruder

7. Robot Dynamics 7.5 The Equations of Motion

Tuesday 6th Nov 2012 ME 482/582: Robotics Engineering

Let's further examine the terms on the lhs of equation (7.16). First expand equation (7.15) using (7.17) (7.18)

Applying equation (7.16), in scalar form we get, (7.19)

11 12 1 1

1

1 2

1 1( ) [ ]

2 2

n

T

n

n n nn n

D D D q

q D q q q q

D D D q

1 1

1( )

2

n n

ij i ji jD q q q

( ) ( )0, 1, ,

k k

dk n

q dt q

Slide 3 / 21

Page 4: NMT EE 589 & UNM ME 482/582 ROBOT ENGINEERING

© 2011, Dr. Stephen Bruder

7. Robot Dynamics 7.5 The Equations of Motion

Tuesday 6th Nov 2012 ME 482/582: Robotics Engineering

For k = 1,...,n:

Starting with the second term on the lhs of equation (7.19) (7.20)

(since D is symmetric and the last equals 0) (7.21) (7.22) (7.23)

1 1

( ) 1 1 ( )( ) ( )

2 2

n n

kj j ik ij ik

qD q q D q q

q q

υ

1( )

n

kj jjD q q

1

( )( )

n

kj jjk

d dD q q

dt q dt

1 1

( ) ( )n n

kj j kj jj j

dD q q D q q

dt

1 1 1

( )( )

n n n kj

kj j i jj j ii

D qD q q q q

q

Slide 4 / 21

1 21 11 2

( ) ( ) ( )( )

n n kj kj kj

kj j n jj jn

D D DD q q q q q q

q q q

1 1

1( , ) ( ) ( )

2

n n

ij i ji jq q D q q q q

υ

0 ki ikD D

Page 5: NMT EE 589 & UNM ME 482/582 ROBOT ENGINEERING

© 2011, Dr. Stephen Bruder

7. Robot Dynamics 7.5 The Equations of Motion

Tuesday 6th Nov 2012 ME 482/582: Robotics Engineering

Next, the first term on the lhs of equation (7.19) suggests that (7.24)

Finally, substituting these results (equations (7.23) and (7.24)) back into equation (7.19) yields (7.25)

Hence,

1 1

( )( ) 1 ( )

2

n n ij

i jj ik k k

D q qq q

q q q

υ

1 1

2 2

1 2 1 2[ ( ) ( ) ( )] [ ( , ) ( , ) ( , )] ( ) 0k k kn k k kn k

n n

q q

q qD q D q D q C q q C q q C q q G q

q q

1 1 1

( ) ( )1 ( )( ) 0

2

n n n kj ij

kj j i jj j ii k k

D q D q qD q q q q

q q q

υ

Slide 5 / 21

1 1

1( , ) ( ) ( )

2

n n

ij i ji jq q D q q q q

υ

(7.26)

Page 6: NMT EE 589 & UNM ME 482/582 ROBOT ENGINEERING

© 2011, Dr. Stephen Bruder

7. Robot Dynamics 7.5 The Equations of Motion

Tuesday 6th Nov 2012 ME 482/582: Robotics Engineering

Recalling Eqn. 7.25 (7.25)

we could also write the resulting Matrix/Vector eqn as:

This representation provides better clarity regarding the inherent structure of the equations:

○ D(.) is only a fn of the qi’s and is multiplied only by 𝑞 ,

○ C(.) (as shown above) is only a fn of the qi’s and is multiplied by 𝑞𝑖 𝑞𝑗 type terms, and

○ The G(.) vector is only a function of the qi’s

1 1 1

2 1 2

1 2[ ( ) ( ) ( )] ( ) ( ) 0k k kn k

n n n

q q q

q q qD q D q D q C q G q

q q q

1 1 1

( ) ( )1 ( )( ) 0

2

n n n kj ij

kj j i jj j ii k k

D q D q qD q q q q

q q q

υ

Slide 6 / 21

Page 7: NMT EE 589 & UNM ME 482/582 ROBOT ENGINEERING

© 2011, Dr. Stephen Bruder

7. Robot Dynamics 7.5 The Equations of Motion

Tuesday 6th Nov 2012 ME 482/582: Robotics Engineering

Collecting rows corresponding to k = 1,...,n, and rewriting in matrix form gives: (7.26)

In the general case wherein an external torque (𝜏 ) is applied to the system, the rhs of equation (7.26) becomes non-zero. Specifically, (7.27)

In our robotics problems, 𝜏 is the vector of joint torques generated by the actuator at each joint.

Equation (7.27) represents a very generalized form of a dynamic model, which is valid for any robot with rigid links (and rigid joints).

○ We could also add other dissipative terms such as friction

( ) ( , ) ( )D q q C q q q G q

( ) ( , ) ( ) 0D q q C q q q G q

Slide 7 / 21

( ) ( ) ( ) 0i jD q q C q q q G q

OR

Page 8: NMT EE 589 & UNM ME 482/582 ROBOT ENGINEERING

© 2011, Dr. Stephen Bruder

7. Robot Dynamics 7.5 The Equations of Motion

Tuesday 6th Nov 2012 ME 482/582: Robotics Engineering

Summary:

The path 𝑞 (𝑡 which satisfies (7.27) was obtained by imposing (7.28)

where ℒ ≐ К − 𝛖, is the difference between the kinetic energy and potential energy.

( ) ( )d

q dt q

Slide 8 / 21

Page 9: NMT EE 589 & UNM ME 482/582 ROBOT ENGINEERING

© 2011, Dr. Stephen Bruder

7. Robot Dynamics 7.6 A Step by Step Procedure to Construct the

Dynamic Model

Tuesday 6th Nov 2012 ME 482/582: Robotics Engineering

Step 1: Inertia Matrices

First, compute the inertia matrix of each link, preferably in a coordinate system located at the link's center of mass (frame {𝑖}, i = 1,...,n) of each link.

○ For the purpose of this course, the inertia matrices 𝑖, i = 1,...,n will always be given!

Step 2: Forward Kinematics

Construct the D-H table, and thus the forward kinematic relationships 𝑇1

0 , 𝑇20 , … , 𝑇𝑛

0 . Transform these homogeneous matrices to link center of mass coordinates:

○ From these we can get 𝑃0 𝑐𝑖, 𝑧 0𝑖 , and , i = 1,...,n.

0 0

1 2 1 2( , , , ) ( , , , ) , 1, ,i i

i

c i i i cT q q q T q q q T i n

0

iR

Slide 9 / 21

Page 10: NMT EE 589 & UNM ME 482/582 ROBOT ENGINEERING

© 2011, Dr. Stephen Bruder

7. Robot Dynamics 7.6 A Step by Step Procedure to Construct the

Dynamic Model

Tuesday 6th Nov 2012 ME 482/582: Robotics Engineering

Step 3: Compute Velocities

a) Construct the 𝑣 𝑐𝑖0

(i.e., 𝑣 𝑐𝑖 expressed in terms of frame {0}), which can be

obtained by directly differentiating 𝑃0 𝑐𝑖.

b) Construct the 𝜔0 𝑖, i = 1,...,n, which can be obtained by recalling that then transform this vector to frame {𝑖} by .

Step 4: Kinetic & Potential Energy

Develop the expressions for the kinetic energy К = К𝑖𝑖=1

𝑛

, where

and potential energy 𝛖 = 𝛖𝑖𝑖=1

𝑛

0 0 00

1 21 2 ii iz z z

0 0( )i i

T

iR

1( )

2 i i i i

T T

i c c i iv v m К

0( )i

T

i c iq g P mυ

Slide 10 / 21

Page 11: NMT EE 589 & UNM ME 482/582 ROBOT ENGINEERING

© 2011, Dr. Stephen Bruder

7. Robot Dynamics 7.6 A Step by Step Procedure to Construct the

Dynamic Model

Tuesday 6th Nov 2012 ME 482/582: Robotics Engineering

Step 5: Euler-Lagrange Eqn

Apply the scalar Euler-Lagrange equations to the Lagrangian ℒ ≐ К − 𝛖, i = 1,...,n to develop (7.30)

Step 6: Generalized Form

Collect the terms and rewrite the expressions generated from Step 5 in the desired matrix form: (7.31)

○ This is a differential equation which governs the motion of the robot, described in terms of the generalized coordinates 𝑞 (𝑡 .

( ) ( ), 1, ,k

k k

dk n

q dt q

( ) ( , ) ( )D q q C q q q G q

Slide 11 / 21

( ) ( ) ( )i jD q q C q q q G q

OR

Page 12: NMT EE 589 & UNM ME 482/582 ROBOT ENGINEERING

© 2011, Dr. Stephen Bruder

7. Robot Dynamics 7.7 A First Dynamic Model Example

Tuesday 6th Nov 2012 ME 482/582: Robotics Engineering

A First Dynamic Model Example:

A two link RR manipulator.

The Denavit-Hartenberg table:

D-H

params. i di

1 0 0 0

2 0 2 0

i-1 i-1

ai

d i

1( )t

2 ( )t

Slide 12 / 21

Handout:

Mathematica File: RR_bot

Page 13: NMT EE 589 & UNM ME 482/582 ROBOT ENGINEERING

© 2011, Dr. Stephen Bruder

7. Robot Dynamics 7.7 A First Dynamic Model Example

Tuesday 6th Nov 2012 ME 482/582: Robotics Engineering

Step 1: Inertia Matrices

Assume the inertia matrices are computed in the link center of mass coordinate frames {𝑐1} and {𝑐2}, respectively, as

1 2

1 1 2 2

1 2

0 0 0 0 0 0

0 0 ; 0 0

0 0 0 0

c cI I

I I

Model the links as slender rods

Slide 13 / 21

Page 14: NMT EE 589 & UNM ME 482/582 ROBOT ENGINEERING

© 2011, Dr. Stephen Bruder

7. Robot Dynamics 7.7 A First Dynamic Model Example

Tuesday 6th Nov 2012 ME 482/582: Robotics Engineering

Step 2: Forward Kinematics

From the D-H table,

Hence,

1 1 2 2

1 1 2 20 1

1 2

C S 0 0 C S 0 2

S C 0 0 S C 0 0

0 0 1 0 0 0 1 0

0 0 0 1 0 0 0 1

T T

12 12 1

12 12 10 0 1

2 1 2

C S 0 2C

S C 0 2S

0 0 1 0

0 0 0 1

T T T

Slide 14 / 21

Page 15: NMT EE 589 & UNM ME 482/582 ROBOT ENGINEERING

© 2011, Dr. Stephen Bruder

7. Robot Dynamics 7.7 A First Dynamic Model Example

Tuesday 6th Nov 2012 ME 482/582: Robotics Engineering

From the diagram

hence

and

1 2

1 2

1 0 0 1 1 0 0 2

0 1 0 0 0 1 0 0

0 0 1 0 0 0 1 0

0 0 0 1 0 0 0 1

c cT T

1 1 1

1 1 1

1 1 10 0 1 0

1

C S 0 C

S C 0 S

0 0 1 0

0 0 0 1

c cT T T T

2 2 2

12 12 1 12

0 0 2 012 12 1 12

2

C S 0 2 C C

S C 0 2 S S

0 0 1 0

0 0 0 1

c cT T T T

Slide 15 / 21

Page 16: NMT EE 589 & UNM ME 482/582 ROBOT ENGINEERING

© 2011, Dr. Stephen Bruder

7. Robot Dynamics 7.7 A First Dynamic Model Example

Tuesday 6th Nov 2012 ME 482/582: Robotics Engineering

Step 3: Compute Velocities

Compute the linear velocities for each center of mass (𝑣 𝑐𝑖, i = 1,2), with

respect to frame {0}:

1 1

1

0 0

1

C

( ) S

0

c c

d dv P

dt dt

2 2

1 12

0 0

1 12

2(C C )

( ) 2(S S )

0

c c

d dv P

dt dt

2

1 1 12 12

1 1 12 12

2 S S

2 C C

0

cv

1

1 1

1 1

S

C

0

cv

Slide 16 / 21

Page 17: NMT EE 589 & UNM ME 482/582 ROBOT ENGINEERING

© 2011, Dr. Stephen Bruder

7. Robot Dynamics 7.7 A First Dynamic Model Example

Tuesday 6th Nov 2012 ME 482/582: Robotics Engineering

Compute the angular velocity for each link 𝜔0 𝑖 (in terms of frame {0}),

Then, compute , the angular velocity of each link coordinatized in the same frame in which 𝑖 is computed (i.e., center of mass)

○ Link#1:

○ Link#2:

0 0( )i i

T

iR

00

11 1 1

0

0

1

z

1 1

1 1

0 0

1 1 1 1

1

C S 0 0 0

( ) S C 0 0 0

0 0 1 1

T

TR

00 0

22 1 2 2

1 1 2

0 0 0

0 0 0

1

z

2 2

0 0

2

1 2

0

( ) 0TR

Slide 17 / 21

Page 18: NMT EE 589 & UNM ME 482/582 ROBOT ENGINEERING

© 2011, Dr. Stephen Bruder

7. Robot Dynamics 7.7 A First Dynamic Model Example

Tuesday 6th Nov 2012 ME 482/582: Robotics Engineering

Step 4: Kinetic & Potential Energy

Noting that gravity acts in the negative 𝑦 0 direction gives:

where 𝑔 ≐ −9.81 m/s2. The potential energy is:

[0 0]Tg g

2

1 12

0

2 2 2 1 12 2 1 12

2 C C

[0 0] 2 S S 2 (S S )

0

T

cm g P m g m g

υ

1

1

0

1 1 1 1 1 1

C

[0 0] S S

0

T

cm g P m g m g

υ

Slide 18 / 21

Page 19: NMT EE 589 & UNM ME 482/582 ROBOT ENGINEERING

© 2011, Dr. Stephen Bruder

7. Robot Dynamics 7.7 A First Dynamic Model Example

Tuesday 6th Nov 2012 ME 482/582: Robotics Engineering

The kinetic energy is:

1 1 1 11 1 1

1 1

1 1 1 1 1 1 1 1 1

1 1

2

1 1 1

1( )

2

S 0 0 0 01

[ S C 0] C [0 0 ] 0 0 02

0 0 0

1( )

2

T T

c cm v v

m I

I

m I

К

1 12 1 12 2

2 1 12 1 12 2 1 12 1 12 2 1 12 1 12 2 1 2 2

2 1 2

(S S ) S 0 0 0 01

4 [ (S S ) S (C C ) C 0] (C C ) C [0 0 ] 0 0 02

0 0 0

m I

I

2 2 2 22 2 1

1( )

2

T T

c cm v v К

2 2 2

2 1 2 2 2 1 2 1 2 2

1[ ( ) 4 (2(1 C ) 2(1 C ) )]

2I m

Slide 19 / 21

Page 20: NMT EE 589 & UNM ME 482/582 ROBOT ENGINEERING

© 2011, Dr. Stephen Bruder

7. Robot Dynamics 7.7 A First Dynamic Model Example

Tuesday 6th Nov 2012 ME 482/582: Robotics Engineering

Step 5: Euler-Lagrange Eqn

Apply the Euler-Lagrange equations.

2 2 2 2

1 1 2 1 12 1 1 1 2 1 2 2 2 1 2 1 2 2

12 S 4 (S S ) ( ) ( ) 4 2(1 C ) 2(1 C )

2m g m g m I I m

1 2 1 2 К К υ υ

1

1 1

2

1 1 1 12 2 2 2 1 2 2 2 2

1 2 2 1 2 1 2 2 2 2

2 8 4

8 1 4 1

d

dt

g c m c c m m s m s

m c m I I c m I

2

2 2

2

12 2 2 2 1 2 2 2 1 2 2 22 4 4 1 4

d

dt

gc m m s c m I m I

Slide 20 / 21

Page 21: NMT EE 589 & UNM ME 482/582 ROBOT ENGINEERING

© 2011, Dr. Stephen Bruder

7. Robot Dynamics 7.7 A First Dynamic Model Example

Tuesday 6th Nov 2012 ME 482/582: Robotics Engineering

Step 6: Generalized Form

Rewriting in the generalized matrix form

gives rise to

Optionally, in the alternative generalized form

( ) ( , ) ( )D q q C q q q G q

1 2 2 1 2 2 2 2 1 1 2 1 12 12 2 2 2 2 2 2 1

2 2 2 2 2 2 12 21 2 2 1 2

8 (1 C ) 4 (1 C ) C 2 (C C )8 S 4 S

4 (1 C ) 4 2 C4 S 0

m m I I m I gm gmm m

m I m I gmm

1 2 2 1 2 2 2 2 2

2 2 2 2 2 1

8 (1 C ) 4 (1 C )

4 (1 C ) 4

m m I I m I

m I m I

1 1

2 2 2 2 1 1 2 1 12 1

1 2

2 2 2 12 2

2 2

0 8 S 4 S C 2 (C C )

4 S 0 0 2 C

m m gm gm

m gm

( ) ( ) ( )i jD q q C q q q G q

Slide 21 / 21