Ecuaciones de posicin

x = la Cos[[t]] - lp Sin[[t]] Sin[[t]]

y = la Sin[[t]] + lp Cos[[t]] Sin[[t]]
z = lp Cos[[t]]
la Cos[[t]] - lp Sin[[t]] Sin[[t]]
lp Cos[[t]] Sin[[t]] + la Sin[[t]]
lp Cos[[t]]
Vx = D[x, t]
- lp Cos[[t]] Sin[[t]] [t] - lp Cos[[t]] Sin[[t]] [t] - la Sin[[t]] [t]
Vy = D[y, t]
Vz = D[z, t]
Velocidad
V = SimplifyVx2 + Vy2 + Vz2
Energia cinetica y potencial del sistema
km =
ka =

ut =

2
1
2
1

kp =
kt =

2
1

2
1
2
1
2

J (t)2
Ia (t)2
Ip (t)2 +

Ia (t)2 +

1
2
1
2

mp (t)2 la2 + lp2 sin2 ((t)) + 2 la lp (t) cos((t)) (t) + lp2 (t)2
Ip (t)2 +

1
2

J (t)2 +

mp (t)2 la2 + lp2 sin2 ((t)) + 2 la lp (t) cos((t)) (t) + lp2 (t)2
g lp mp cos((t))

Lagrangiano
L=

2
1
2
1
2

Ia ( '[t])2 +

mp
2
2
2
la2 + lp2 Sin[[t]]2 ( '[t])2 + 2 la lp ( '[t]) ( '[t]) Cos[[t]] + lp2 ( '[t])2 +

J ( '[t])2 +

Ip ( '[t])2 -

1
2

mp g lp Cos[[t]]

g lp mp Cos[[t]] +

1
2

Ip [t]2 +

1
2

Ia [t]2 +

1
2

J [t]2 +

mp lp2 [t]2 + 2 la lp Cos[[t]] [t] [t] + la2 + lp2 Sin[[t]]2 [t]2

Printed by Wolfram Mathematica Student Edition

Control.nb

D[L, [t]]
0
phi = D[L, '[t]]
Ia [t] + J [t] +

1
2

mp 2 la lp Cos[[t]] [t] + 2 la2 + lp2 Sin[[t]]2 [t]

D[L, [t]]
1
2

g lp mp Sin[[t]] +
1
2

mp - 2 la lp Sin[[t]] [t] [t] + 2 lp2 Cos[[t]] Sin[[t]] [t]2

theta = D[L, '[t]]

Ip [t] +

1
2

mp 2 lp2 [t] + 2 la lp Cos[[t]] [t]

phi2 = D[phi, t]
1

mp - 2 la lp Sin[[t]] [t]2 + 4 lp2 Cos[[t]] Sin[[t]] [t] [t] +

2
2 la lp Cos[[t]] [t] + 2 la2 + lp2 Sin[[t]]2 [t]

Ia [t] + J [t] +

theta2 = D[theta, t]
Ip [t] +

1
2

mp - 2 la lp Sin[[t]] [t] [t] + 2 lp2 [t] + 2 la lp Cos[[t]] [t]

= phi2
1

mp - 2 la lp Sin[[t]] [t]2 + 4 lp2 Cos[[t]] Sin[[t]] [t] [t] +

2
2 la lp Cos[[t]] [t] + 2 la2 + lp2 Sin[[t]]2 [t]

Ia [t] + J [t] +

cero = theta2 - D[L, [t]]

-

1
2
1
2
1
2

g lp mp Sin[[t]] mp - 2 la lp Sin[[t]] [t] [t] + 2 lp2 Cos[[t]] Sin[[t]] [t]2 + Ip [t] +

mp - 2 la lp Sin[[t]] [t] [t] + 2 lp2 [t] + 2 la lp Cos[[t]] [t]

Simplify[]
- la lp mp Sin[[t]] [t]2 + lp2 mp Sin[2 [t]] [t] [t] +
la lp mp Cos[[t]] [t] + Ia + J + la2 mp + lp2 mp Sin[[t]]2 [t]

Printed by Wolfram Mathematica Student Edition

Control.nb

Expand[]
- la lp mp Sin[[t]] [t]2 + 2 lp2 mp Cos[[t]] Sin[[t]] [t] [t] +
la lp mp Cos[[t]] [t] + Ia [t] + J [t] + la2 mp [t] + lp2 mp Sin[[t]]2 [t]
Simplify[cero]
-

g lp mp Sin[[t]] - lp2 mp Cos[[t]] Sin[[t]] [t]2 +

2
Ip + lp2 mp [t] + la lp mp Cos[[t]] [t]

Expand[cero]
-

g lp mp Sin[[t]] - lp2 mp Cos[[t]] Sin[[t]] [t]2 +

2
Ip [t] + lp2 mp [t] + la lp mp Cos[[t]] [t]

Ecuaciones dinmicas del sistema

1 = - la lp mp Sin[[t]] [t]2 + lp2 mp Sin[2 [t]] [t] [t] +
la lp mp Cos[[t]] [t] + Ia + J + la2 mp + lp2 mp Sin[[t]]2 [t]
1

g lp mp Sin[[t]] - lp2 mp Cos[[t]] Sin[[t]] [t]2 +

2
Ip + lp2 mp [t] + la lp mp Cos[[t]] [t]

cero1 = -

Despeje de [t] y [t]

(t)
Ia + J + la2 mp + lp2 mp sin2 ((t)) la lp mp cos((t))
=
=

(t)
la lp mp cos((t))
Ip + lp2 mp
la lp mp (t)2 sin((t)) + lp2 (-mp) (t) sin(2 (t)) (t) + torq
1
2

a :=

b :=

g lp mp sin((t)) + lp2 mp sin((t)) cos((t)) (t)2

Ia + J + la2 mp + lp2 mp Sin[[t]]2 la lp mp Cos[[t]]

Ip + lp2 mp

la lp mp Cos[[t]]

torq + la lp mp Sin[[t]] [t]2 - lp2 mp Sin[2 [t]] [t] [t]

1
2

g lp mp Sin[[t]] + lp2 mp Cos[[t]] Sin[[t]] [t]2

Ip + lp2 mp

- la lp mp Cos[[t]]

Det[a]

- la lp mp Cos[[t]]

Ia + J + la2 mp + lp2 mp Sin[[t]]2

.b

Inverse[a].b
Ecuaciones de [t] y [t] respectivamente
mp lp2 +Ip -mp sin(2 (t)) (t) (t) lp2 +la mp sin((t)) (t)2 lp+torq
mp lp2 +Ip mp la2 +lp2 mp sin2 ((t))+Ia+J -la2 lp2 mp2 cos2 ((t))

la lp mp cos((t)) lp2 mp cos((t)) sin((t)) (t)2 + g lp mp sin((t))

2

mp lp2 +Ip mp la2 +lp2 mp sin2 ((t))+Ia+J -la2 lp2 mp2 cos2 ((t))

mp la2 +lp2 mp sin2 ((t))+Ia+J lp2 mp cos((t)) sin((t)) (t)2 + g lp mp sin((t))

2

mp lp +Ip mp la +lp mp sin ((t))+Ia+J -la lp

mp2

cos2 ((t))

la lp mp cos((t)) -mp sin(2 (t)) (t) (t) lp2 +la mp sin((t)) (t)2 lp+torq
mp lp2 +Ip mp la2 +lp2 mp sin2 ((t))+Ia+J -la2 lp2 mp2 cos2 ((t))

x1 = '[t]

Printed by Wolfram Mathematica Student Edition

Control.nb

Ip + lp2 mp la lp mp (t)2 sin((t)) + lp2 (-mp) (t) sin(2 (t)) (t) + torq

x2 =
Ip + lp2 mp Ia + J + la2 mp + lp2 mp sin2 ((t)) - la2 lp2 mp2 cos2 ((t))
1

la lp mp cos((t)) 2 g lp mp sin((t)) + lp2 mp sin((t)) cos((t)) (t)2

Ip + lp2 mp Ia + J + la2 mp + lp2 mp sin2 ((t)) - la2 lp2 mp2 cos2 ((t))

x3 = '[t]
1

x4 = Ia + J + la2 mp + lp2 mp sin2 ((t))

g lp mp sin((t)) + lp2 mp sin((t)) cos((t)) (t)2
2
Ip + lp2 mp Ia + J + la2 mp + lp2 mp sin2 ((t)) - la2 lp2 mp2 cos2 ((t)) la lp mp cos((t)) la lp mp (t)2 sin((t)) + lp2 (-mp) (t) sin(2 (t)) (t) + torq
Ip + lp2 mp Ia + J + la2 mp + lp2 mp sin2 ((t)) - la2 lp2 mp2 cos2 ((t))
Representacin en espacios de Estado
x1 =

x2 =
x3 =

x4 =
x1punto = x2
xdospunto = Ip + lp2 mp torq + la lp mp Sin[x3] x42 - lp2 mp Sin[2 x3] x4 x2
- la2 lp2 mp2 Cos[x3]2 + Ip + lp2 mp Ia + J + la2 mp + lp2 mp Sin[x3]2 1

g lp mp Sin[x3] + lp2 mp Cos[x3] Sin[x3] x22

2
- la2 lp2 mp2 Cos[x3]2 + Ip + lp2 mp Ia + J + la2 mp + lp2 mp Sin[x3]2

la lp mp Cos[x3]

x3punto = x4
xcuatropunto = - la lp mp Cos[x3] torq + la lp mp Sin[x3] x22 - lp2 mp Sin[2 x3] x4 x2
- la2 lp2 mp2 Cos[x3]2 + Ip + lp2 mp Ia + J + la2 mp + lp2 mp Sin[x3]2 +
1

Ia + J + la2 mp + lp2 mp Sin[x3]2

g lp mp Sin[x3] + lp2 mp Cos[x3] Sin[x3] x22

2
- la2 lp2 mp2 Cos[x3]2 + Ip + lp2 mp Ia + J + la2 mp + lp2 mp Sin[x3]2

Como lo que en realidad la variable a controlar sera el voltaje del motor (Ea), entonces la relacion
torque - velocidad es la siguiente:
=

ke
ra

Ea
0 1

x1
x1

x2
x2
=

x3
x3

x4
x4

0
2

Ip+lp mp la lp mp x4 sin(x3)+lp (-mp) x2 x4 sin(2 x3)+torq

0 0

Ip+lp2 mp Ia+J +la2 mp+lp2 mp sin2 (x3)-la2 lp2 mp2 cos2 (x3)

0 0

Ip+lp2 mp Ia+J +la2 mp+lp2 mp sin2 (x3)-

0
1

0 0

la lp mp cos(x3) g lp mp sin(x3)+lp2 mp x2

g lp mp sin(x3)+lp mp x2 sin(x3) cos(x3) Ia+J +la mp+lp mp sin (x3)

2

Ip+lp mp Ia+J +la mp+lp mp sin (x3)-la lp mp cos (x3)

la lp mp cos(x3) la lp mp x22 sin(x3)+lp2

Ip+lp2 mp Ia+J +la2 mp+lp2 mp sin2

Debido a que es un sistema no lineal, es necesario linealizar el modelo, especialmente xdospunto y

Printed by Wolfram Mathematica Student Edition

Control.nb

xcuatropunto, se utiliza la linealizacin por Jacobiano, se muestran las derivadas parciales para cada
funcin
D[xdospunto, x1]
D[xdospunto, x2]
D[xdospunto, x3]
D[xdospunto, x4]
D[xdospunto, torq]
xdoslineal[x2_, x3_, x4_] :=
1
g lp mp Sin[x3] + lp2 mp x22 Sin[x3] Cos[x3]
la lp mp Sin[x3]
2
Ip + lp2 mp Ia + J + la2 mp + lp2 mp Sin[x3]2 - la2 lp2 mp2 Cos[x3]2 1

g lp mp Cos[x3] - lp2 mp x22 Sin[x3]2 + lp2 mp x22 Cos[x3]2

2
Ip + lp2 mp Ia + J + la2 mp + lp2 mp Sin[x3]2 - la2 lp2 mp2 Cos[x3]2 +

la lp mp Cos[x3]

la lp mp Cos[x3]

1
2

g lp mp Sin[x3] + lp2 mp x22 Sin[x3] Cos[x3]

2 lp2 mp Sin[x3] Cos[x3] Ip + lp2 mp + 2 la2 lp2 mp2 Sin[x3] Cos[x3]

2

Ip + lp2 mp Ia + J + la2 mp + lp2 mp Sin[x3]2 - la2 lp2 mp2 Cos[x3]2 Ip + lp2 mp 2 lp2 mp Sin[x3] Cos[x3] Ip + lp2 mp + 2 la2 lp2 mp2 Sin[x3] Cos[x3]
la lp mp x42 Sin[x3] + lp2 (- mp) x2 x4 Sin[2 x3] + torq
2

Ip + lp2 mp Ia + J + la2 mp + lp2 mp Sin[x3]2 - la2 lp2 mp2 Cos[x3]2 +

Ip + lp2 mp la lp mp x42 Cos[x3] - 2 lp2 mp x2 x4 Cos[2 x3]
Ip + lp2 mp Ia + J + la2 mp + lp2 mp Sin[x3]2 - la2 lp2 mp2 Cos[x3]2
xdoslineal[0, 0, 0]
-

g la lp2 mp2
2 - la2 lp2 mp2 + Ia + J + la2 mp Ip + lp2 mp
g la lp2 mp2
2 - la2 lp2 mp2 + Ia + J + la2 mp Ip + lp2 mp

torque1[x2_, x3_, x4_] :=

- Ip + lp2 mp - la2 lp2 mp2 Cos[x3]2 + Ip + lp2 mp Ia + J + la2 mp + lp2 mp Sin[x3]2
torque1[0, 0, 0]
-

Ip + lp2 mp
- la2 lp2 mp2 + Ia + J + la2 mp Ip + lp2 mp
Ip + lp2 mp
- la2 lp2 mp2 + Ia + J + la2 mp Ip + lp2 mp

Printed by Wolfram Mathematica Student Edition

Control.nb

D[xcuatropunto, x1]
D[xcuatropunto, x2]
D[xcuatropunto, x3]
D[xcuatropunto, x4]
D[xcuatropunto, torq]
xcuatrolineal[x2_, x3_, x4_] :=
2 la2 lp2 mp2 Cos[x3] Sin[x3] + 2 lp2 mp Ip + lp2 mp Cos[x3] Sin[x3]
1
2

g lp mp Sin[x3] + lp2 mp x22 Cos[x3] Sin[x3] Ia + J + la2 mp + lp2 mp Sin[x3]2

2

- la2 lp2 mp2 Cos[x3]2 + Ip + lp2 mp Ia + J + la2 mp + lp2 mp Sin[x3]2 la lp mp Cos[x3] la lp mp x22 Cos[x3] - 2 lp2 mp x2 x4 Cos[2 x3]
- la2 lp2 mp2 Cos[x3]2 + Ip + lp2 mp Ia + J + la2 mp + lp2 mp Sin[x3]2 +
2 lp2 mp Cos[x3] Sin[x3]

g lp mp Sin[x3] + lp2 mp x22 Cos[x3] Sin[x3]

2
- la2 lp2 mp2 Cos[x3]2 + Ip + lp2 mp Ia + J + la2 mp + lp2 mp Sin[x3]2 +

Ia + J + la2 mp + lp2 mp Sin[x3]2

1

g lp mp Cos[x3] + lp2 mp x22 Cos[x3]2 - lp2 mp x22 Sin[x3]2

2
- la2 lp2 mp2 Cos[x3]2 + Ip + lp2 mp Ia + J + la2 mp + lp2 mp Sin[x3]2 +
la lp mp Cos[x3] 2 la2 lp2 mp2 Cos[x3] Sin[x3] + 2 lp2 mp Ip + lp2 mp Cos[x3] Sin[x3]
torq + la lp mp x22 Sin[x3] - lp2 mp x2 x4 Sin[2 x3]
2

- la2 lp2 mp2 Cos[x3]2 + Ip + lp2 mp Ia + J + la2 mp + lp2 mp Sin[x3]2 +

la lp mp Sin[x3] torq + la lp mp x22 Sin[x3] - lp2 mp x2 x4 Sin[2 x3]
- la2 lp2 mp2 Cos[x3]2 + Ip + lp2 mp Ia + J + la2 mp + lp2 mp Sin[x3]2
xcuatrolineal[0, 0, 0]
g lp mp Ia + J + la2 mp
2 - la2 lp2 mp2 + Ia + J + la2 mp Ip + lp2 mp
g lp mp Ia + J + la2 mp
-

2 - la2 lp2 mp2 + Ia + J + la2 mp Ip + lp2 mp

torque2[x2_, x3_, x4_] := - (la lp mp Cos[x3])

- la2 lp2 mp2 Cos[x3]2 + Ip + lp2 mp Ia + J + la2 mp + lp2 mp Sin[x3]2

Printed by Wolfram Mathematica Student Edition

Control.nb

torque2[0, 0, 0]
-

la lp mp
- la2 lp2 mp2 + Ia + J + la2 mp Ip + lp2 mp
la lp mp
2

- la lp

mp2

+ Ia + J + la2 mp Ip + lp2 mp

Representacion lineal en espacios de estados

0 1

x1

x2

x3

x4

0 0 =

mp2

g la lp
2 -la2 lp2 mp2 +Ia+J+la2 mp Ip+lp2 mp

0 0
0 0

0
2

g lp mp Ia+J+la2 mp
2 -la2 lp2 mp2 +Ia+J+la2 mp Ip+lp2 mp

0
-

-la lp

Ip+lp2 mp
mp Ip+lp2 mp

mp2 +Ia+J+la2

0
-

la lp mp
-la2 lp2 mp2 +Ia+J+la2 mp Ip+lp2 mp

y=(0 0 1 0)

x1
x2
x3
x4

Valores numricos para

mp = 0.0355
lp = 0.195
la = 0.141
ma = 0.34606
Ia = 0.000467
Ip = 0.000004306
J = 0.000005086
g = 9.807

Linealizacin en para la validacin del modelo matemtico

xdoslineal[0, , 0]
torque1[0, , 0]
xcuatrolineal[0, , 0]

Printed by Wolfram Mathematica Student Edition

x1
x2
x3
x4

Control.nb

torque2[0, , 0]

0 1

x1

x2

x3

x4

0 0 =

g la lp
2

2 -la lp

0
2

mp2

mp2 +Ia+J+la2

0 0

mp Ip+lp mp

0 0 -

g lp mp Ia+J+la2 mp
2 -la2 lp2 mp2 +Ia+J+la2 mp Ip+lp2 mp

0
-

Ip+lp2 mp
2
2
2
-la lp mp +Ia+J+la2 mp Ip+lp2 mp

la lp mp
-la2 lp2 mp2 +Ia+J+la2 mp Ip+lp2 mp

0 1
0 0 -

0
2 -la2 lp2 mp2 +Ia+J+la2 mp Ip+lp2 mp

0 0
0 0 0
0
0
0

g la lp2 mp2

0
2 -la2 lp2 mp2 +Ia+J+la2 mp Ip+lp2 mp

1
0
0 -51.581
0
0
0 -62.2446

0
1

g lp mp Ia+J+la2 mp

0
0
1
0
0

Ip+lp2 mp
-la2 lp2 mp2 +Ia+J+la2 mp Ip+lp2 mp

0.245522632

la lp mp

1.7

-la2 lp2 mp2 +Ia+J+la2 mp Ip+lp2 mp

{{0.}, {- 304.482}, {0.}, {219.464}}

Printed by Wolfram Mathematica Student Edition

x1
x2
x3
x4