Industrial Robot II Report: Newton-Euler Framework and simulation of RRR Planar Robot

1 FORWARD KINEMATIC

1.1 DH-PARAMETER

1.2 DENAVIT HATENBERG CONVENTION

i ia id i i

1 1l 0 1 0

2 2l 0 2 0

3 3l 0 2 0

1

cos cos sin sin sin cos

sin cos cos sin cos sin

0 sin cos

0 0 0 1

i i i i i i i

i i i i i i iii

i i i

a

aA

d

1.2.1 Transformation matrix

1 1 1 1

1 1 1 101

cos sin 0 cos

sin cos 0 sin

0 0 1 0

0 0 0 1

l

lA

2 2 2 2

2 2 2 212

cos sin 0 cos

sin cos 0 sin

0 0 1 0

0 0 0 1

l

lA

3 3 3 3

3 3 3 323

cos sin 0 cos

sin cos 0 sin

0 0 1 0

0 0 0 1

l

lA

12 12 1 1 2 12

12 12 1 1 2 1202

s 0

c 0 s

0 0 1 0

0 0 0 1

c l c l c

s l l sA

1 1c cos , 1 1s sin , 2 2c cos , 2 2s sin ,

3 3s sin , 123... 1 2 3c cos ......n n ,

123... 1 2 3sin ......n ns

123 123 1 1 2 12 3 123

123 123 1 1 2 12 3 12303

s 0

c 0 s

0 0 1 0

0 0 0 1

c l c l c l c

s l l s l sA

23 23 2 2 3 123

23 23 2 2 3 12313

s 0

c 0

0 0 1 0

0 0 0 1

c l c l c

s l s l sA

1.2.2 Rotation matrix

1.3 DEFINE POSITION VECTOR

1.3.1 The position vector of {i} with respect to {i-1} and describe in {0}

1.3.2 The position vector of ith link center of mass with respect to {i} and describe in {0}

1 101 1 1

cos sin 0

sin cos 0

0 0 1

R2 2

12 2 2

cos sin 0

sin cos 0

0 0 1

R3 3

23 3 3

cos sin 0

sin cos 0

0 0 1

R

12 1202 12 12

s 0

s c 0

0 0 1

c

R123 123

03 123 123

s 0

s c 0

0 0 1

c

R23 23

13 23 23

s 0

s c 0

0 0 1

c

R

1 1

*

1 1 1

0

l c

p l s

2 12

*

2 2 12

0

l c

p l s

3 123

*

3 3 123

0

l c

p l s

11

11 1

2

2

0

lc

ls s

212

22 12

2

2

0

lc

ls s

3123

33 123

2

2

0

lc

ls s

2 KINEMATIC PARAMETER BY FORWARD METHOD

Forward Equation for 1,2,3i

i

i : Absolute angular velocity of link i with reference to frame i

i

i : Absolute angular acceleration of link i with reference to frame i

i

ia : Absolute linear acceleration of link i at the origin of frame i with reference to frame i

i

i

ca : Absolute linear acceleration of link i at the center of mass of link i with reference to frame i

2.1 LINK 1

2.1.1 Angular velocity

2.1.2 Angular acceleration

Initial condition 00 0 ,00 0 , 0

0

0

v g

1 1 0 1 0 1

1 0 1 0 0 0 1 1 1

0

= = ( ) 0

1

R R R k q

1

1 1

0

0

1

2.1.3 Linear acceleration

2.1.4 center of mass Linear acceleration

2.2 LINK 2

2.2.1 Angular velocity

2.2.2 Angular acceleration

2.2.3 Linear acceleration

1 1 1

1

1 1 1 1

s

0

g l

a gc l

1

11 1

1 11 1

s2

2

0

c

lg

la gc

22 1 20

= 0

1

22 1 20

0

1

22

1 1 1 2 1 1 1 2 2 1 2

22 2

2 1 1 1 2 1 1 1 2 2 1 2

( ) s

( ) s

0

gc l s l g c l

a gc l c l g s l

2.2.4 center of mass Linear acceleration

2.3 LINK 3

2.3.1 Angular velocity

2.3.2 Angular acceleration

2.3.3 Linear acceleration

2

22 2

1 1 1 2 1 1 1 2 1 2

22 2 2

1 1 1 2 1 1 1 2 1 2

( ) s2

( ) s2

0

C

lgc l s l g c

la gc l c l g s

33 1 2 30

= 0

1

22 1 2 30

0

1

2 2 2 2

123 2 3 1 2 3 2 2 3 3 2 3 1 2 3 2 1 23 1 1 23 1 2 3 1 2 2 3 1 3 2 3 2 3

3 2 2 2 2

3 3 1 2 3 123 2 3 1 2 3 2 2 3 3 2 3 1 2 3 2 1 23 1 1 23

2 2 4 2 2

2 2

gs l c l c l c l s l s l c l s l c l c l c

a l gc l s l s l s l c l c l s l c

1 2 3 1 2 2 3 1 3 2 3 2 34 2 2

0

l s l s l s

2.3.4 center of mass Linear acceleration

3 KINETIC PARAMETER BY BACKWARD METHOD

3.1 LINK 1

3.1.1 Force

3

2 2 2 2

123 2 3 1 2 3 2 2 3 3 2 3 1 2 3 2 1 23 1 1 23 1 2 3 1 2 2 3 1 3 2 3 2 3

3 2 2 2 2

3 1 2 3 123 2 3 1 2 3 2 2 3 3 2 3 1 2 3 2 1 23 1 1 23

2 2 4 2 2

2 2C

gs l c l c l c l s l s l c l s l c l c l c

a l gc l s l s l s l c l c l s l c

1 2 3 1 2 2 3 1 3 2 3 2 34 2 2

0

l s l s l s

22 2 1 1 1

1 1 1 1 2 1 1 1 1 2 1 3 1

2 2 2 2 2

3 3 23 1 2 3 3 3 23 1 2 3 2 2 2 1 2

2

( ) ( ) ( )

2 2 2

l mgm s l m l m gm s gm s

l m c l m s l m c

2 2 2 2 2 2 1 22 2 3 2 1 2 3 2 2 2 3 2 3

( )2 2

2

l m sl m c l m c l m c

3 2 3 2 1 2 2 2 2 1 2 2 3 2 1 2 2 3 2 1 3 2 3 2 2 3

3 3 23 1 2 3 3 23 1 3 3 3 23 2 3

( ) 4 2 2l m s l m c l m c l m c l m c

l m c l m c l m c

1 2 3

1

1 4 5 6

0

f

2 2 2

3 3 23 1 2 3 3 3 23 1 2 31 1 14 1 2 1 3 1 1 2 3 1

( ) ( )( ) ( )

2 2 2

l m c l m sl ml m l m m m m gc

2 22 2 22 2 2 1 2 2 2 2 1 2

5 2 3 2 1 2 2 3 2 3 2 3 2 1 2

( ) ( )2 ( ) ( )

2 2

l m s l m cl m s l m s l m c

6 2 2 2 1 2 2 3 2 1 2 2 3 2 1 3 2 3 2 2 3 3 3 23 1 2 3 3 23 1 3 3 3 23 2 34 2 2l m s l m s l m s l m s l m s l m s l m s

3.1.2 Moment

3.2 LINK 2

3.2.1 Force

2 21 3 3 23 2 322 3 3 3 3

5 2 3 3 3 2 1 3 3 23 1 1 3 3 23 12 2

l l m sl l m cl l m c l l m s l l m c

( )1 3 3 23 2 3

6 1 3 3 23 1 2 1 3 3 22

l l m cl l m s

7 1 2 2 2 1 2 1 2 3 2 1 2 1 2 3 2 1 3 1 2 3 2 2 3 2 3 3 3 1 2 2 3 3 3 1 3 2 3 3 3 2 34 2 2 3 2 2l l m s l l m s l l m s l l m s l l m s l l m s l l m s

23 3 1 2 3 1 1 1 3 3 123 2 2 12

2 1 1 2 3 2 3 124 2 2 2

l m gl mc gl m c gl m cgl c m m gl m c

2 222 3 3 3 1 22 2 21 2 2 2 2

3 1 2 3 2 1 1 2 3 2 2 1 2 3 2 3

32

2 2

l l m sl l m sl l m s l l m s l l m s

2 1 2 2 2 24 2 3 3 3 3 1 2 2 2 1 1 2 3 2 1 1 2 3 2 2 2 3 3 3 122

l l m cl l m s l l m c l l m c l l m c l l m c

( )2 2 2

2 21 1 1 2 2 1 2 2 21 1 2 3 1 2 3 2 3 3 2 3 1 1 2 3 1 24 4 4

l m l m l mI I I I I I m m l l m

1

1

1 2 3 4 5 6 7

0

0n

1 2 3

2

2 4 5 6

0

f

2 2

2 2 2 22 2 2 2 2 1

1 2 12 2 3 1 2 2 3 3 3 12 1 2 2 12

2 2

l m l mgm s l m l m gm s l m c

2 2 23 3 1 2 322 1 3 2 1 2 2 1 2 2 3 1 2 2 3 1 3

4 22

l ml m c l m l m l m

3 3 3 1 2 3

3 2 3 2 3 1 2 2 1 1 3 2 1 3 3 3 1 22

2

l m cl m l m s l m s l m c

4 3 3 3 1 3 3 3 3 2 3l m c l m c

3.2.2 Moment

3.3 LINK 3

3.3.1 Force

2

2 22 2 1 2 3 3 3 1

5 2 3 1 2 2 12 3 12 1 2 2 1 1 3 2 1

2 2

l m l m sl m gm c gm c l m s l m s

2 23 3 3 2 3 3 3 3 1 2 36 1 2 2 1 1 3 2 1

2 2

l m s l m cl m c l m c

7 3 3 3 1 2 3 3 3 1 3 3 3 3 2 3l m s l m s l m s

l m l mI I I I l m

2 22 2 1 2 3 3 1 2 32

1 1 2 3 1 3 2 3 2 3 1 24 4

l l m sgl m c gl m c l l m sgl m c l l m s

2 222 3 3 3 1 223 3 123 2 2 12 1 2 2 2 1

2 2 3 12 1 2 3 2 1

3

2 2 2 2l l m c l l m c

l l m s l l m c l l m c l l m c2 1 2 2 2 1 2 3 3 3 33 2 3 3 3 3 1 2 3 2 1 2 3 3 3 1 2 3 3 3 22 2

l l m s l l m cl l m s l l m s l l m s l l m s

21 3 3 23 1 1 3 3 23 1

4 2 3 3 3 1 2 2 3 3 3 1 31 2 3 3 3 1 3 2 3 3 3 2 33 2 22 2

n22

1 2 3 4

0

0

l mgm s l m c l m s l m c l m c

2

3 3 1 2 3 2 2 21 3 123 1 3 23 1 1 3 23 1 2 3 3 1 2 3 3 22 22

( )l m c l m s l m c l m c l m c l m c22 2 3 3 3 2 3 3 1 2 2 3 3 1 2 2 3 3 1 2 2 3 3 1 3 2 3 3 2 34 4 2 2

3

3

1 2

3 4

0

f

3.3.2 Moment

4 EQUATION OF MOTION

4.1 JOINT 1

( )l mgm c l m s l m c l m s2 23 3 1 2 33 3 123 1 3 23 1 1 3 23 1 2 3 3 122

l m s l m s l m c l m c l m s l m s l m s2 24 2 3 3 2 2 3 3 3 2 3 3 1 2 3 3 2 2 3 3 1 2 2 3 3 1 3 2 3 3 2 32 4 2 2

( ) c( )

l m gl mI l l m s

223 3 1 2 3 3 3 123

1 3 1 2 3 2 3 3 3 24 2l l m s l l m c l l m c l l m s l l m c

l l m s l l m s l l m s2 2

2 3 3 3 3 2 3 3 3 1 2 3 3 3 2 1 3 3 23 1 1 3 3 23 12 2 3 3 3 1 2 2 3 3 3 1 3 2 3 3 3 2 322 2 2 2 2

3

3

1 2

0

0n

l m l mA I I I l l m c l l m c l l m c

2 22 2 3 3

1 1 2 3 1 2 2 2 1 2 3 2 2 3 3 2324 4

A A A A g2 2 21 1 1 2 2 3 3 1 1 2 1 2 3 1 3 4 2 5 2 3 6 3 4

l m l m l l m c l l m cA I I l l m c l l m c

2 22 2 3 3 1 2 2 2 1 3 3 23

2 2 3 1 2 3 2 2 3 3 34 4 2 2

l m l l m c l l m cA I

23 3 2 3 3 3 1 3 3 23

3 3 4 2 2

l l m sl l m s l l m s2 3 3 31 1 2 3 2 1 3 3 23

3

2

l l m s l l m s l l m s l l m s2 2 3 3 3 1 2 3 2 1 2 2 2 1 3 3 233 4

l l m s l l m s l l m s3 2 3 3 3 1 2 3 2 1 3 3 232 2

l l m s l l m s l l m sl l m s2 3 3 3 1 2 2 2 1 3 3 234 1 2 3 2

32

2 2 2

l l m s l l m s l l m s5 2 3 3 3 1 2 3 2 1 3 3 232 2

4.2 JOINT 2

l l m sl l m s l l m s 1 3 3 236 2 3 3 3 1 2 3 2 2

l m c l m c l m cA l m c l m c l m c1 1 1 3 3 123 2 2 124 1 2 1 1 3 1 1 3 122 2 2

l m l m l l m c l l m cB I I l m l l m c l l m c

2 222 2 3 3 1 2 2 2 1 3 3 23

1 2 3 2 3 1 2 3 2 2 3 3 34 4 2 2

l m l mB I I l m l l m c

2 222 2 3 3

2 2 3 2 3 2 3 3 34 4

l l m s l l m s l l m sl l m s1 2 2 2 2 3 3 3 1 3 3 231 1 2 3 2

3

2 2 2

l l m s2 2 3 3 33

l l m s3 2 3 3 32

l l m s2 3 3 34

3

2l l m s5 2 3 3 32

l l m s6 2 3 3 3

l m l l m cB I

23 3 2 3 3 3

3 3 4 2

l m c l m cB l m c3 3 123 2 2 124 2 3 122 2

B B B B g2 2 22 1 1 2 2 3 3 1 1 2 1 2 3 1 3 4 2 5 2 3 6 3 4

4.3 JOINT 3

5 SIMULATION

5.1 STATE SPACE REPRESENTATION

C C C C g2 2 23 1 1 2 2 3 3 1 1 2 1 2 3 1 3 4 2 5 2 3 6 3 4

l m l l m c l l m cC I

23 3 2 3 3 3 1 3 3 23

1 3 4 2 2

l m l l m cC I

23 3 2 3 3 3

2 3 4 2

l mC I

23 3

3 3 4

l l m s

l l m s 1 3 3 231 2 3 3 3 2 l l m s2 2 3 3 32

l l m s3 2 3 3 3

l l m s4 2 3 3 3

l l m s5 2 3 3 3

l l m s2 3 3 3

6 2l m c

C 3 3 1234 2

* C g3 3 4* B g2 2 4

* A g1 1 4

Robot , ,1 2 3 0

1 2 3cos cos cos 1 1 1 2 2 3 3sin ,sin ,sin

A A A A g

B B B B g

C C C C g

1 1 2 3 1 4

2 1 2 3 2 4

3 1 2 3 3 4

m m m kg1 2 3 50 l l l m1 2 3 1

State space representation * input , ,1 2 3 output

, , , , ,x x x x x x1 1 2 2 3 3 1 1 2 2 3 3 x Ax Bu y Cx Du D=0

A g A A A

B g B B B

C g C C C

1 4 1 2 3 1

2 4 1 2 3 2

3 4 1 2 3 3

*

*

*

A A A

B B B

C C C

1

1 1 2 3 1

2 1 2 3 2

3 1 2 3 3

*

*

*

1 11 12 13 1

2 21 22 23 2

3 31 32 33 3

11

22

33

1 11 12 131

2 21 22 232

3 31 32 333

0 0 00 0 0 1 0 0

0 0 00 0 0 0 1 0

0 0 00 0 0 0 0 1

0 0 0 0 0 0

0 0 0 0 0 0

0 0 0 0 0 0

*

*

*

1

2

3

y

1

2

3

1

2

3

1 0 0 0 0 0

0 1 0 0 0 0

0 0 1 0 0 0

5.2 SOLVING EQUATION State equation , ,1 2 3 Simulink ,B ,A C4 4 4

.A4 2207 25 , B4 981 , .C4 245 25 Simulink Block

input 1 N.m 1 2209 , 2 982 , 3 246

( ), ( ), ( )1 2 30 0 0 , ,10 20 15

. . .

. . .

. . .

11

22

33

11

22

33

0 0 0 1 0 0 0 0 0

0 0 0 0 1 0 0 0 0

0 0 0 0 0 1 0 0 0

0 0 0 0 0 0 0 02 0 05 0 02

0 0 0 0 0 0 0 05 0 12 0 1

0 0 0 0 0 0 0 02 0 1 0 17

*

*

*

1

2

3

5.3 RESULT , ,1 2 3 5.3.1,5.3.2,5.3.3

5.3.1 JOINT 1 10

5.3.2 JOINT 2 10

5.3.1

5.3.4,5.3.5, 5.3.6,5.3.7

5.3.3 JOINT 3 10

5.3.1

5.3.4 3

5.3.1

5.3.5 7

5.3.1

5.3.6 9

Recursive Newton-Euler Equation of Motion

Equation of Motion simulation input

simulation

5.3.7 10