Manipulador Robotico

3 Grados de libertad
RRR
Examen Parcial
Curso : Robotica Movl
Alumno : Castro Martinez, Alexander

CINEMATICA DIRECTA
Matriz de transformacion de coordenadas de "j" Para rotaciones respecto al eje Y
respecto a "i" donde:
cos( j) 0 sin j ( ) Lxi
j = Es la rotacion del sistema "j" con
Ti_j( j , Lxi , Lz i ) :=
respecto al sistema "i". 0 1 0 0
(Lx,Ly) = Es la traslac ion del sistema "j" c on sin( j) 0 cos( j ) Lzi
respecto al sistema "i".
0 0 0 1
Matrices de rotacion

cos( 1 ) ( )
0 sin 1 0 cos( 2 ) 0 sin 2( ) L1

T0_1 := Ti_j( 1 , 0 , 0 )
0
T1_2 := Ti_j( 2 , L1 , 0 )
0 1 0 0 1 0 0
sin( 1 ) 0 cos( 1 ) 0 sin( 2 ) 0 cos 2( ) 0

0 0 0 1 0 0 0 1

cos( 3 ) ( )
0 sin 3 L2 1

0 0 L3


T3_4 := Ti_j( 0 , L3 , 0 )
0 1 0 0
T2_3 := Ti_j( 3 , L2 , 0 )
0 1 0 0
0 0 1 0
sin( 3 ) ( )
0 cos 3 0

0 0 0 1
0 0 0 1

cos( 1 + 2 + 3 ) (
0 sin 1 + 2 + 3 ) (
L3 cos 1 +

T0_4 := T0_1 T1_2 T2_3 T3_4 simplify
0 1 0
sin( 1 + 2 + 3 ) (
0 cos 1 + 2 + 3 ) (
L3 sin 1 +

0 0 0

L3 cos( 1 + 2 + 3 ) + L2 cos( 1 + 2 ) + L1 cos( 1 )

P0_4 := T0_4 P4_4 simplify 0
L3 sin( 1 + 2 + 3 ) L2 sin( 1 + 2 ) L1 sin( 1 ) P0_3

1
CINEMATICA INVERSA
Partimos de la informacion brindada de la matriz de transformacion.

(
x0 := P0_3 L2 cos 1 + 2 + L1 cos 1
0
) ( )
y0 := P0_3 0
1
(
z0 := P0_3 L2 sin 1 + 2 L1 sin 1
2
) ( )
Posicion y Orientacion

( )
x = L2 cos 1 + 2 + L1 cos 1 ( )
z = L2 sin( 1 + 2 ) L1 sin( 1 )
No consideraremos la longitud del efector final para la
cos( ) = cos( 1 + 2 + 3 ) cinematica inversa

sin( ) = sin( 1 + 2 + 3 )

Solucionando las ecuaciones se tendra:

( 1) (L2) ( 1) (L2)
2 2 2 2 2 2 2 2
x +z L x +z L
( 2)
cos
2 L L
D :=
2 L L
1 2 1 2

( 2) :=
2
sin 1D Dependiendo del signo elegido tenemos
( )
cos 2 = D dos valores. Los cuales representas
nuestras 2 soluciones.
( 2) :=
2
sin 1D
Elbow up (codo arriba).
Elbow down (codo abajo).
L sin( )
1 y 1
x tan L + L cos
2 2
:= tan
1
1 2 ( 2)

1 D2 1 D2
Elbow UP := arctan Elbow DOWN := arctan
2
D 2
D

Luego

1 + 2 + 3 = atan ( sin( ) , cos( ) )

3 = atan ( sin( ) , cos( ) ) 1 2

 CINEMATICA DIFERENCIAL

Jacobiano del punto P0_4

L3 cos( 1 + 2 + 3 ) + L2 cos( 1 + 2 ) + L1 cos( 1 )

P0_4 0
L3 sin( 1 + 2 + 3) L2 sin( 1 + 2 ) L1 sin( 1)

1
x0_4 := P0_4
0
( ) ( )
x0_4 L3 cos 1 + 2 + 3 + L2 cos 1 + 2 + L1 cos 1 ( )
y0_4 := P0_4 y0_4 0
1
z0_4 := P0_4
2
( ) ( )
z0_4 L3 sin 1 + 2 + 3 L2 sin 1 + 2 L1 sin 1 ( )
d x d
x0_4
d
x0_4
d 0_4 d2 d3
1 L3 sin( 1 + 2 + 3 ) L2 sin( 1 + 2 ) L1 sin( 1 ) L3 sin(
d d d
d y0_4 d2
y0_4
d3
y0_4 0
1 L cos( + + ) L cos( + ) L cos( ) L cos(
d d d 3 1 2 3 2 1 2 1 1 3
d z0_4 d2
z0_4
d3
z0_4
1

d x d
x0_4
d
x0_4
d 0_4 d2 d3
1
d d d
JP0_4 := y0_4 y0_4 y0_4
d1 d2 d3

d d d
d z0_4 d2
z0_4
d3
z0_4
1

L3 sin( 1 + 2 + 3) L2 sin( 1 + 2 ) L1 sin( 1) L3 sin( 1 + ) (

2 + 3 L2 sin 1

JP0_4 0 0
L cos( + + ) L cos( + ) L cos( ) L cos( + ) (
2 + 3 L2 cos 1
3 1 2 3 2 1 2 1 1 3 1
 ( ) ( ( )
R i_j j := submatrix Ti_j j , 0 , 0 , 0 , 2 , 0 , 2
Matriz de transformacion de coordenadas de "j"
respecto a "i" donde:
cos( j) 0 sin( j)
j = Es la rotacion del sistema "j" con respecto al
R i_j( j ) 0 1 0
sistema "i".
pq_p = Punto en el sistema "p" visto desde el sin( ) 0 cos( )
j j
sistema "q".
Lx q_p = Coordenada "x p " visto desde el sistema "q".
Lyq_p = Coordenada "yp " visto desde el sistema "q". Lxq_p

p q_p := Lyq_p
Lz
q_p
1
( )
R 0_1 := R i_j 1 R 1_0 := R 0_1

cos( 1 ) 0 sin( 1 )
cos( 1) 0 sin( 1)
R 1_0 simplify 0
R 0_1 0 1 0
1 0
sin( ) 0 cos( ) sin( ) 0 cos( )
1 1 1 1

1
( )
R 1_2 := R i_j 2 R 2_1 := R 1_2

cos( 2) 0 sin( 2) cos( 2 ) 0 sin( 2 )

R 2_1 simplify 0 1 0
R 1_2 0 1 0
sin( ) 0 cos( )
sin( ) 0 cos( ) 2 2
2 2

1
( )
R 2_3 := R i_j 3 R 3_2 := R 2_3

cos( 3) 0 sin( 3) cos( 3 ) 0 sin( 3 )

R 3_2 simplify 0 1 0
R 2_3 0 1 0
sin( ) 0 cos( ) sin( ) 0 cos( )
3 3 3 3

1
R 3_4 := R i_j( 0 ) R 4_3 := R 3_4

1 0 0 1 0 0
R 3_4 0 1 0 R 4_3 simplify 0 1 0

0 0 1 0 0 1
 0 0 0
w0_0 := 0 w1_1 := R 1_0 w0_0 + '1 1 w1_1 '1

0 0 0

0 0
v0_0 := 0 P0_1 := 0

0 0
0
(
v1_1 := R1_0 v0_0 + w0_0 P0_1 ) v1_1 0

0
0 0
w2_2 := R 2_1 w1_1 + '2 1 w2_2 '1 + '2
0
0
L1 L1 '1 sin( 2)

P1_2 := 0 v2_2 := R2_1 ( v1_1 + w1_1 P1_2 ) v2_2 simplify 0
0 L ' cos( )
1 1 2
0
0
w3_3 := R 3_2 w2_2 + '3 1
w3_3 '1 + '2 + '3
0
0
L2 L1 '1 sin( 2 + 3 )
P2_3 := 0
v3_3 := R3_2 ( v2_2 + w2_2 P2_3 ) v3_3 simplify
0
L ' cos( + )
1 1 2 3
0
0
w4_4 := R 4_3 w3_3 + 0 1
w3_3 '1 + '2 + '3
0
0
L3 L1 '1
P3_4 := 0
v4_4 := R4_3 ( v3_3 + w3_3 P3_4 ) v4_4 simplify
0
L ' L ' L
3 1 3 2 3
 Ejemplo, cuando el manipulador se encuentra en el eje X:

simplify 0

v4_4 substitute , 1 = 0 , 2 = 0 , 3 = 0 0
( 1 2 3 ) 1 ( 2 3 ) 2 3 3
collect , '1 , '2
L L L ' + L L ' L '

v0_4 := R0_1 R1_2 R2_3 R 3_4 v4_4

( L3 sin( 1 + 2 + 3 ) L2 sin( 1 + 2 ) L1 sin( 1 ) ) '1 + ( L3 sin( 1

simplify
v0_4 0
collect , '1 , '2 , '3
( L cos( + + ) L cos( + ) L cos( ) ) ' + ( L cos(
3 1 2 3 2 1 2 1 1 1 3

L3 sin( 1 + 2 + 3 ) L2 sin( 1 + 2) L1 sin

simplify
v10_4 := v0_4 0
substitute , '1 = 1 , '2 = 0 , '3 = 0
L cos( + + ) L cos( + ) L cos
3 1 2 3 2 1 2 1

L3 sin( 1 + 2 + ) (
3 L2 sin 1 + 2 )
simplify
v20_4 := v0_4 0
substitute , '1 = 0 , '2 = 1 , '3 = 0
L cos( + + 3 ) L2 cos( 1 + 2 )
3 1 2

L3 sin( 1 + 2 + 3 )
simplify
v30_4 := v0_4 0
substitute , '1 = 0 , '2 = 0 , '3 = 1
L cos( + + )
3 1 2 3

(
JV0_4 := augment v10_4 , v20_4 , v30_4 )

L3 sin( 1 + 2 + 3 ) L2 sin( 1 + 2 ) L1 sin( 1 ) L3 sin( 1 + 2 + 3 L )

JV0_4 simplify 0 0
L cos( + + ) L cos( + ) L cos( ) L cos( + 2 + 3 L )
3 1 2 3 2 1 2 1 1 3 1

0

w0_4 := R 0_1 R1_2 R2_3 R3_4 w4_4 w0_4 '1 + '2 + '3

0
 MODELO DINAMICO

Segun Euler-Lagrande: M ( q ) q'' + V ( q , q') =

1 1
( ) ( ) ( )
T T
Ki_i vi_i , wi_i , Pi_Ci , ICi_i , mi := mi vi_i + wi_i Pi_Ci vi_i + wi_i Pi_Ci + wi_i ICi_i wi_i
2 2

Ixxi 0 0
1
( )
Pi_Ci Pk_i := Pk_i
2
k := i + 1 ICi_i ( Ixxi , Iyyi, Izzi) := 0 Iyyi 0
0 0 Izzi


Links y posicion del centro de masa.

L1 Ixx1 0 0

P1_C1 := Pi_Ci ( P1_2 )
2
IC1_1 := ICi_i ( Ixx1 , Iyy1 , Izz1) 0 Iyy1 0
0 0
0 Izz1

0

L2 Ixx2 0 0


P2_C2 := Pi_Ci ( P2_3 )
2
IC2_2 := ICi_i ( Ixx2 , Iyy2 , Izz2) 0 Iyy2 0
0
0 0 Izz2

0

L3 Ixx3 0 0

P3_C3 := Pi_Ci ( P3_4 )
2
IC3_3 := ICi_i ( Ixx3 , Iyy3 , Izz3) 0 Iyy3 0
0 0
0 Izz3

0

Energia Cinetica (
Ki_i vi_i , wi_i , Pi_Ci , ICi_i , mi )
L1

0 0
P1_C1
2
v1_1 0 w1_1 '1 0

0 0
0

'1 m1 L1 + 4 Iyy1
2 2

( )
T1 := Ki_i v1_1 , w1_1 , P1_C1 , IC1_1 , m1 simplify
8
 L1 '1 sin( 2) L2
0
P2_C2
2
v2_2 simplify 0 w2_2 '1 + '2
L ' cos( ) 0 0
1 1 2
0

( )
2 2 2
m2 L1 '1 m2 cos 2 L1 L2 '1 m2 cos
( )
T2 := Ki_i v2_2 , w2_2 , P2_C2 , IC2_2 , m2 simplify
2
+
2
+

L1 '1 sin( 2 + 3 ) + L2 '1 sin( 3 ) + L2 '2 sin( 3 ) 0

w3_3 '1 + '2 +
v3_3 simplify 0

L ' cos( + ) L ' cos( ) L ' cos( ) 0
1 1 2 3 2 1 3 2 2 3

2 2
m3 L1 '1
( )
T3 := Ki_i v3_3 , w3_3 , P3_C3 , IC3_3 , m3 simplify
2
( ) 2
+ m3 cos 2 L1 L2 '1 + m3 cos (
Efectos de la gravedad

L1 cos( 1 )
L1
2
2 0 L1 sin 1 ( )
P1_C1 := 0 P0_C1 := T0_1 P1_C1 z0_C1 := P0_C1
0 L1 sin( 1) 2 2
2
1
1

L2 cos( 1 + 2 )
L2 + L1 cos( 1 )
2
2 0
P2_C2 := 0 P0_C2 := T0_1 T1_2 P2_C2 simplify
0 L2 sin( 1 + 2 ) L sin
2 1 ( 1)
1
1

L3 cos( 1 + 2 + 3 )
L3 + L2 cos( 1
2
2 0
P3_C3 := 0 P0_C3 := T0_1 T1_2 T2_3 P3_C3 simplify
0 L3 sin( 1 + 2 + 3) L sin
2 2 ( 1
1
1
 L1 g m1 sin 1( )
P1 := m1 g z0_C1
2

L2 sin( 1 + 2 )
P2 := m2 g z0_C2 g m2 + L1 sin( 1 )
2

L3 sin( 1 + 2 + 3 )
P3 := m3 g z0_C3 g m3 + L2 sin( 1 + 2 ) + L1 sin( 1 )
2

1 '1 ''1
q := q' := q'' :=
2 '2 ''2

L := T1 + T2 + T3 P1 P2 P3

'1 m1 L1 + 4 Iyy1
2 2 2 2 2 2 2 2
Iyy2 '1 Iyy2 '2 Iyy3 '1 Iyy3 '2 Iyy3 '3 L1 m2
L + + + + +
+
2 2 2 2 2 8 2

Para 1

simplify 2
L1 m1
2
L2 m2
d 2 2 2
I + Iyy2 + Iyy3 + + L1 m2 + L1 m3 + + L2 m3 +
collect , '1 , '2 , '3 yy1
L
d'1 4 4

d d
M 1 := L
dt d'1

2
L1 m1
2
L2 m2
2
L3 m3
2 2 2
M 1 := Iyy1 + Iyy2 + Iyy3 + + L1 m2 + L1 m3 + + L2 m3 + + L1 L3 m3
4 4 4

simplify 2
L1 m1
d 2 2
1 := M 1 I + Iyy2 + Iyy3 + + L1 m2 + L1 m
collect , ''1 , ''2 , ''3 , '1 , '2 , '3 yy1
L
d1 4

Para 2

simplify 2
L2 m2
2
L3 m3 L1 L3 m3 cos( 2 + 3 )
d 2
L Iyy2 + Iyy3 + + L2 m3 + + +
d'2 collect , '1 , '2 , '3 4 4 2
 d d
M 2 := L
dt d'2

2
L2 m2
2
L3 m3 L1 L3 m3 cos( 2 + 3 ) L1 L2 m2 cos( 2 )
2
M 2 := Iyy2 + Iyy3 + + L2 m3 + + + +L
4 4 2 2

simplify 2
L2 m2
2
L3 m3 L1 L3 m3 cos
d 2
2 := M 2 L Iyy2 + Iyy3 + + L2 m3 + +
d2 collect , ''1 , ''2 , ''3 4 4 2

Para 3

simplify 2
L3 m3 L1 L3 m3 cos( 2 + 3 ) L2 L3 m3 cos( 3 )
d
L Iyy3 + + + '1 +
d'3 collect , '1 , '2 , '3 4 2 2

d d
M 3 := L
dt d'3

2
L3 m3 L1 L3 m3 cos( 2 + 3 ) L2 L3 m3 cos( 3 ) L1 L3 m3 sin( 2 + 3
M 3 := Iyy3 + + + ''1 +
4 2 2 2

simplify 2
L3 m3 L1 L3 m3 cos( 2 + 3 ) L2 L3 m3 cos(
d
3 := M 3 L Iyy3 + + +
d3 collect , ''1 , ''2 , ''3 4 2 2

L L m ' 2 sin( + ) L3 g m3 cos( 1 + 2 + 3 ) L2 g m

1 3 3 1 2 3
L2 g m3 cos( 1 + 2 )
2 2
1
2
L2 m2 L3
2
2
:= Iyy2 + Iyy3 +
2
+ L2 m3 +
4 4
3

Iyy3

 1

) ( )
2 + 3 + L2 cos 1 + 2 + L1 cos 1 ( ) 0 0

P4_4 := P3_3 :=
0 0
0
0 0
2 + 3 ) L2 sin( 1 + 2 ) L1 sin( 1 )
1 1

1

L2 cos( 1 + 2 ) + L1 cos( 1 )

0
0_3 := T0_1 T1_2 T2_3 P3_3 simplify L sin + L sin
2 ( 1 2) 1 ( 1)

1
 ) (
1 + 2 + 3 L2 sin 1 + 2 ) (
L3 sin 1 + 2 + 3 )
0 0
1 + 2 + 3 ) L2 cos( 1 + 2 ) L3 cos( 1 + 2 + 3 )

+ 2 ) (
L3 sin 1 + 2 + 3 )
0
+ 2 ) L3 cos( 1 + 2 + 3 )

 ( ) ( )
+ L2 '1 sin 3 + L2 '2 sin 3

0
L2 '1 cos( 3 ) L2 '2 cos( 3 )

( ) ( )
sin 2 + 3 + L2 '1 sin 3 + L2 '2 sin 3 ( )

0

3 '3 L1 '1 cos( 2 + 3 ) L2 '1 cos( 3 ) L2 '2 cos( 3 )
 ) ( )) ( (
1 + 2 + 3 L2 sin 1 + 2 '2 + L3 sin 1 + 2 + 3 '3 ))


1 + 2 + 3 ) L2 cos( 1 + 2 ) ) '2 + ( L3 cos( 1 + 2 + 3 ) ) '3

( )
sin 1

cos( 1 )

(
L2 sin 1 + 2 ) (
L3 sin 1 + 2 + 3 )
0
L2 cos( 1 + 2 ) L3 cos( 1 + 2 + 3 )

 ( )
2 2 2 2 2 2
cos 2 L1 L2 '1 '2 m2 L2 '1 m2 L2 '1 '2 m2 L2 '2 Iyy2 '1 Iyy2 '2
+ + + + + Iyy2 '1 '2 +
2 8 4 8 2 2

L3

P3_C3
2
'3
0


0

( ) ( ) ( )
2
m3 cos 2 + 3 L1 L3 '1 m3 cos 2 + 3 L1 L3 '1 '2 m3 cos 2 + 3 L1
)
2 L1 L2 '1 '2 +
2
+
2
+
2

(
L2 sin 1 + 2 )
z0_C2 := P0_C2
2 2
( )
L1 sin 1

)
+ 2 + L1 cos 1 ( )

(
L3 sin 1 + 2 + 3 )
z0_C3 := P0_C3
2 2
( )
L2 sin 1 + 2 L1 sin 1 ( )
+ 2 ) L1 sin( 1 )




 2 2 2 2 2 2 2 2 2 2 2 2 2 2
'1 L1 m3 '1 L2 m2 '1 L2 m2 '2 L2 m3 '1 L2 m3 '2 L3 m3 '1 L3
+ + + + + + +
2 8 8 2 2 8

2
L3 m3
+ + L1 L3 m3 cos( 2 + 3 ) + L1 L2 m2 cos( 2 ) + 2 L1 L2 m3 cos( 2 ) + L2 L3 m3 cos( 3 ) '1
4

( ) ( ) ( ) ( )
3 cos 2 + 3 + L1 L2 m2 cos 2 + 2 L1 L2 m3 cos 2 + L2 L3 m3 cos 3 ''1 + L1 L3 m3 sin 2 (

2 2
L2 m2 L3 m3
( ) ( ) ( )
2
m3 + + L2 m3 + + L1 L3 m3 cos 2 + 3 + L1 L2 m2 cos 2 + 2 L1 L2 m3 cos 2
4 4

L1 L2 m2 cos 2 ( ) 2
L2 m2
+ L1 L2 m3 cos( 2 ) + L2 L3 m3 cos( 3 ) '1 +
2
Iyy2 + Iyy3 + 4 + L2 m3 +
2

L1 L3 m3 sin( 2 + 3 ) ( '2 + '3 ) L1 L2 m2 sin( 2 )
L1 L2 m3 cos( 2 ) + L2 L3 m3 cos( 3 ) ''1 +
2 2

( ) ( )
2
cos 2 + 3 L1 L2 m2 cos 2 L2 m2
+ + L1 L2 m3 cos( 2 ) + L2 L3 m3 cos( 3 ) ''1 + Iyy2 + Iyy3 + 4
2

m3 L3
2
( )
L2 m3 cos 3 L3 m L 2
3 3
+ + Iyy3 '2 + 4 + Iyy3 '3
4 2

)( ) ( ) m L 2 L m cos( ) L L2 m3 sin(
3 '2 + '3 L2 L3 m3 sin 3 '3 3 3 2 3 3 3
'1 + + + Iyy3 ''2 +
2 4 2 2

) m L 2 L m cos( ) L m L 2 L1 L3 m3 '1 sin( 2 +

2
3 3 3 2 3 3 3 3 3

1 4
'' + + + Iyy3 ''2 + 4 + Iyy3 ''3 +
2 2

( )+ ( ) +L ( )
2 2
m2 cos 1 + 2 L1 L2 m2 '1 sin 2 L2 L3 m3 '3 sin 3
( )
2
2 2 1 L2 m3 '1 sin 2 2
L2 L3 m3

m3 (
L1 L3 m3 cos 2 + 3 ) ( )
L1 L2 m2 cos 2
4
+
2
+
2
( )
+ L1 L2 m3 cos 2 + L2 L3 m3 cos 3( )
( ) ( )
2
L3 m3 L1 L3 m3 cos 2 + 3 L2 L3 m3 cos 3
+ + +
4 2 2
 2
2

( )
2 2 2 2 2
1 L3 '1 '3 m3 L2 '1 m3 L2 '2 m3 cos 3 L2 L3 '1
+
2
2
+ m3 L2 '1 '2 +
2
+
2
( )
+ m3 cos 3 L2
 L3 sin( 1 + 2 + 3 )
2 2 2 2
m3 '2 L3 m3 '3
+ + g m3 + L2 sin( 1 + 2 ) + L1 sin( 1 ) + Iyy2 '1 '2 +
8 8 2

2
L2 m2
2
L3 m3 L1 L3 m3 cos( 2 + 3 ) L1 L2 m2 cos( 2 )
2
+
1 yy2
I + Iyy3 + + L 2 m3 + + + + L1 L2
4 4 2 2

)( ) ( ) ( ) ( )
2 + 3 '2 + '3 L1 L2 m2 sin 2 '2 2 L1 L2 m3 sin 2 '2 L2 L3 m3 sin 3 '3 '1 + Iyy2

2
L2 m2
2
L3 m3 L1 L3 m3 cos( 2 + 3 ) L1
+ L2 L3 m3 cos( 3 ) ''1 +
2
+
yy2 yy3
I I + + L 2 m3 + + +
4 4 2

2
L3 m3 m L 2 L m cos( ) L
3 3 2 3 3 3
+ L2 L3 m3 cos( 3 ) '2 + 4 + + Iyy3 '3
4 2
 2
L2 m2
2
L3 m3
'2
L1 L2 m3 sin( 2 ) '2 L2 L3 m3 sin( 3 ) '3 '1 +
2
Iyy2 + Iyy3 + 4 + L2 m3 + +
4

m L 2 L m cos L
( 3) 3
2
L3 m3 3 3 2 3 L1 L3 m
+ L2 L3 m3 cos( 3 ) ''2 +
2
+ L2 m3 + 4 + + Iyy3 ''3 +
4 2

)
3 L3 '3 m L 2
3 3
'2 + + Iyy3 ''3
4

) ( ) ( ) ( )
2 2
+ 3 L3 g m3 cos 1 + 2 + 3 L2 L3 m3 '1 sin 3 L2 L3 m3 '2 sin 3
+ + + L2 L3 m3 '1
2 2 2

(
2 2
L2 m2 L3 m3 L1 L3 m3 cos
( ) ( )
2
'1 '3 sin 3 L2 L3 m3 '2 '3 sin 3 Iyy2 + Iyy3 + + L2 m3 + +
4 4 2
2
L2 m2
Iyy2 + Iyy3 + + L2
4
2
m3 L3 L2
+
4
 ( ) ( ) ( )
2 2
m3 cos 3 L2 L3 '1 '3 m3 cos 3 L2 L3 '2 m3 cos 3 L2 L3 '2 '3 m3 L3
L3 '1 '2 + + + +
2 2 2 8
 L2 sin( 1 + 2)
2
L2 m2 '1 '2
+ Iyy3 '1 '2 + Iyy3 '1 '3 + Iyy3 '2 '3 + g m2 + L1 sin( 1 ) + + L2
2 4

2
L3 m3 L1 L3 m3 cos( 2 + 3 ) L2 L3 m3 cos( 3 )
2 m3 cos( 2 ) + L2 L3 m3 cos( 3 ) '2 + yy3
I + + +
4 2 2

2
L2 m2
2
L3 m3 (
L1 L3 m3 cos 2 + 3 ) ( )
L1 L2 m2 cos 2
(
2
+ Iyy3 + + L2 m3 + + + + L1 L2 m3 cos
4 4 2 2

( )
1 L2 m2 cos 2
2
L3 m3 L1 L3 m3 cos( 2 +
+ L1 L2 m3 cos( 2 ) + L2 L3 m3 cos( 3 ) ''2 + yy3
I + +
2 4 2
 m L 2 L m cos L
3 3 2 3 ( 3) 3
L2 L3 m3 cos( 3 ) ''2 + ( L2 L3 m3 sin( 3 ) '3 ) '2 + 4 + + Iyy3 ''3 +
2

( ) ( ) ( )
2
m3 '1 sin 2 + 3 L3 g m3 cos 1 + 2 + 3 L2 g m2 cos 1 + 2 L1
2
(
L2 g m3 cos 1 + 2 ) 2

2
+

( )
'2 sin 3

)+ ( ) +L
2
2 + 3 L1 L2 m2 cos 2 L3 m3 L1 L3 m3 cos
2
( )
1 L2 m3 cos 2 + L2 L3 m3 cos 3 ( ) Iyy3 +
4
+
2
2 2
L3 m3 m3 L3 L2 m3
( )
2
2 m3 + 4
+ L2 L3 m3 cos 3
4
+

( )
2
2 m3 cos 3 L3 m3 L3
+ Iyy3
2 4
 2 2 2 2 2 2 2 2 2
'1 m3 L3 '1 '2 m3 L3 '1 '3 m3 L3 '2 m3 L3 '2 '3 m3 L3 '3 Iyy3 '1
+ + + + + + +
4 4 8 4 8 2
 2
2
L3 m3 '1 '2
2
L3 m3 '1 '3
2
L3 m3 '2 '3 ( )
L1 g m1 sin 1 L1 L3 m3 '1
2

2 m3 '1 '2 + 4
+
4
+
4
+
2
+
2

)
'3

L1 L3 m3 sin( 2 + 3 ) ( '2 + '3 ) L1 L2 m2 sin( 2) '2
) ( )
2 + L2 L3 m3 cos 3 ''2 +
2

2
L1 L2 m3

3 ) ( )
L2 L3 m3 cos 3
+ ''3 + ( L1 L3 m3 sin( 2 + 3) L1 L2 m2 sin( 2 ) 2 L1 L2 m3 sin( 2 ) )
2
 ( )
L2 m3 sin 3 L3 '3
'3
2

( ) ( )
2 2
1 L2 m2 '1 sin 2 L2 L3 m3 '3 sin 3
2
2
( )
+ L1 L2 m3 '1 sin 2
2
( )
L2 L3 m3 '1 '3 sin 3 L2

(
cos 2 + 3 ) +
( )
L2 L3 m3 cos 3
( L1 L3 m3 sin( 2 + 3) L1 L2 m2 sin( 2 ) 2 L1 L2 m3
2
''1
( )
cos 3 L3 '' +
2
+ Iyy3 2
''3
2
+ Iyy3

 2 2
Iyy3 '2 Iyy3 '3
+ Iyy3 '1 '2 + Iyy3 '1 '3 + + Iyy3 '2 '3 +
2 2
 (
cos 2 + 3 ) 2
( )
L1 L2 m2 '1 cos 2
2
( )
L2 L3 m3 '1 cos 3 L2 L3 m3
( )
2
+ + L1 L2 m3 '1 cos 2 + +
2 2

2
L3 m3 L1 L3 m3 cos( 2 + 3 ) L2 L3 m3 cos(

3 sin( 2 ) '2 L2 L3 m3 sin( 3 ) '3 '2 + Iyy3 + 4 + +
2 2

L1 L3 m3 sin( 2 + 3 ) L1 L2 m2 sin(
( ( ) ( ))
'2 + L1 L3 m3 sin 2 + 3 L2 L3 m3 sin 3 '3 '1 +
2

2
 ( )
L3 m3 '2 '3 sin 3

L1 L3 m3 sin( 2 + 3 ) L1
( )) ( ( ) ( ))
3 sin 2 '2 + L1 L3 m3 sin 2 + 3 L2 L3 m3 sin 3 '3 '1 +
2

2
(
L1 L3 m3 '1 sin 2 + 3 )
2
(
L2 g m3 cos 1 + 2 )
L
 2
'2 cos 3 ( ) (
L1 L3 m3 '1 '2 cos 2 + 3 ) (
L1 L3 m3 '1 '3 cos 2 + 3 ) (
L1 L2 m2 '1 '2 cos
+ + +
2 2 2 2

3 ) L1 L3 m3 sin( 2 + 3 ) ( '2 + '3) L2 L3 m3 sin( 3 ) '3

''3 + '3
2 2

(2) L1 L3 m3 sin
( ) ( ( ) ( ))
2
L1 L2 m3 sin 2 '2 + L1 L3 m3 sin 2 + 3 L2 L3 m3 sin 3 '3 '2 +
2
 ( )
L2 m2 sin 2 L1
( ) ( ( ) ( ))
2
L1 L2 m3 sin 2 '2 + L1 L3 m3 sin 2 + 3 L2 L3 m3 sin 3 '3 '2 +
2
(
L3 g m3 cos 1 + 2 + 3 ) (
L2 g m2 cos 1 + 2 ) L1 L2 m2 '1 sin 2
2
( )
) (
2
+ + L1 L2 m3 '1 sin 2
2 2 2

( ) ( ) ( ) (
2 2 2
L1 L3 m3 '1 sin 2 + 3 L3 g m3 cos 1 + 2 + 3 L2 L3 m3 '1 sin 3 L2 L3 m3 '2 sin
+ +
2 2 2 2
 (2) ( )
L2 L3 m3 '1 '3 cos 3 L2 L3 m3 '2
( ) ( )
+ L1 L2 m3 '1 '2 cos 2 + L2 L3 m3 '1 '2 cos 3 +
2
+
2

(
sin 2 + 3 ) ( )
L2 L3 m3 sin 3
'3 2 L2 g m2 cos( 1 + 2) + L2 g m3 cos( 1 + 2 ) + L1 g m1 cos
1 1

2 2 2
 (
1 L3 m3 sin 2 + 3 ) ( ) ' 2 1 L
L2 L3 m3 sin 3 1
3 2 2 g m2 cos( 1 + 2) + L2 g m3 cos( 1 + 2 ) + 2
2 2
2
( )
L2 L3 m3 '3 sin 3
)
2 2
( ) ( )
L2 L3 m3 '1 '3 sin 3 L2 L3 m3 '2 '3 sin 3

3 )
( )
+ L2 L3 m3 '1 '2 sin 3
 ( )
2 '3 cos 3
2

)
1
( ) ( ) ( ) (
cos 1 + L1 g m2 cos 1 + L1 g m3 cos 1 + L3 g m3 cos 1 + 2 + 3
2

)
1 1
( ) ( ) ( ) (
L1 g m1 cos 1 + L1 g m2 cos 1 + L1 g m3 cos 1 + L3 g m3 cos 1 + 2 + 3
2 2