Examen 4 Control optimo

Kevin Lopez Preciado

3 de mayo de 2015

Modelado del Acrobot

a) Calcular la energa Cin

etica y Potencial
Sea q = (matriz)q1 q2 Rn

la posicion generalizada del pendulo.

Figura 1: Acrobot

Realizamos un an
alisis geometrico del pendulo para obtener la energa cinetica y potencial asociadas al pendulo.
Escribimos entonces las ecuacion es respectivas para el angulo q1 :
x1 = l1 Cos(q1 )

(1)

y1 = l1 Sen(q1 )

(2)

Ahora escribimos las ecuaci

on es correspondientes al angulo q2 .
x2 = x1 + l2 Cos(q1 + q2 )

x2 = l1 Cos(q1 ) + l2 Cos(q1 + q2 )
y2 = y1 + l2 Sen(q1 + q2 )

(3)

Figura 2: Analisis geometrico

y2 = l1 Sen(q1 ) + l2 Sen(q1 + q2 )

(4)

La energa Cinetica total asociada al pendulo es aquella asociada a q1 y la

asociada a q2 y la escribimos como:
1
1
K(q, q)
= m1 l12 q1 2 + m2 (x 22 + y 22 )
2
2

(5)

Para poder encontrar una expresion para la ecuacion 5 procedemos a encontrar las derivadas de las componentes geometricas del pendulo.
x 2 = l1 Sen(q1 )q1 l2 Sen(q1 + q2 )(q1 + q2 )
y 2 = l1 Cos(q1 )q1 + l2 Cos(q1 + q2 )(q1 + q2 )
Ahora procedemos a encontrar el termino (x 22 + y 22 ) de la ecuacion 5.
x 22 = (l1 Sen(q1 )q1 l2 Sen(q1 + q2 )(q1 + q2 ))2
= (l1 Sen(q1 )q1 )2 + 2(l1 Sen(q1 )q1 )(l2 Sen(q1 + q2 )(q1 + q2 ))
(l2 Sen(q1 + q2 )(q1 + q2 ))2
= l12 Sen2 (q1 )q1 2
q2 )(q1 + q2 )2

2l1 l2 Sen(q1 )Sen(q1 +q2 )q1 (q1 + q2 )

l22 Sen2 (q1 +

y 22 = (l1 Cos(q1 )q1 + l2 Cos(q1 + q2 )(q1 + q2 ))2

= (l1 Cos(q1 )q1 )2
q2 )(q1 + q2 ))2

2(l1 Cos(q1 )q1 )(l2 Cos(q1 +q2 )(q1 +q2 ))

(l2 Cos(q1 +

= l12 Cos2 (q1 )q1 2

q2 )(q1 + q2 )2

2l1 l2 Cos(q1 )Cos(q1 +q2 )q1 (q1 + q2 )

l22 Cos2 (q1 +

Debido a que:
Sen2 (q1 ) + Cos2 (q1 ) = 1
Sen(q1 )Sen(q1 + q2 ) + Cos(q1 )Cos(q1 + q2 ) = Cos(q2 )
Entonces:
(x 22 + y 22 ) = l12 Sen2 (q1 )q1 2 +2l1 l2 Sen(q1 )Sen(q1 +q2 )q1 (q1 + q2 )+l22 Sen2 (q1 +
q2 )(q1 +q2 )2 +l12 Cos2 (q1 )q1 2 +2l1 l2 Cos(q1 )Cos(q1 +q2 )q1 (q1 +q2 )+l22 Cos2 (q1 +
q2 )(q1 + q2 )2
= l12 q1 2

2l1 l2 Cos(q2 )(q12 + q1 q2 )

l22 (q1 + q2 )2

Despues de realizar este procedimiento encontramos la expresion para la

energa cinetica total del sistema:
1
1
K(q, q)
= m1 l12 q1 2 + m2 (x 22 + y 22 )
2
2
1
1
= m1 l12 q1 2 + m2 (l12 q1 2 + 2l1 l2 Cos(q2 )(q12 + q1 q2 ) + l22 (q1 + q2 )2 )
2
2
1
= (((m1 + m2 )l12 + 2m2 l1 l2 Cos(q2 ) + m2 l22 )q1 2 + (2m2 l1 l2 Cos(q2 ) +
2
2m2 l22 )q1 q2 + (m2 l22 )q22 )
Reescribimos la ecuaci
on 5 de forma matricial de la forma:
1
qT M (q) q
2


T (m1 + m2 )l1 2 + m2 l2 2 + 2l1 l2 m2 cos(q1 )
q2
m2 l2 2 + l1 l2 m2 cos(q1 )

K(q, q)
=
=

1
2

q1

m2 l2 2 + l1 l2 m2 cos(q1 )
m2 l2 2

Ahora encontramos la expresion matematica de la energa potencial asociada al pendulo:

P (q) = m1 g h1 + m2 g h2
m2 g (l1 Sen(q1 ) + l2 Sen(q1 + q2 ))

P (q) = m1 g l1 Sen(q1 ) +

P (q) = (m1 + m2 ) g l1 Sen(q1 ) + gm2 l2 Sen(q1 + q2 )

(6)

2) Encontramos ahora el modelo Lagrangiano, para ello tenemos que resolver las ecuaci
on es de Euler Lagrange.

 
q1
q2

El Lagrangiano del sistema es:

L(q, q)
= K(q, q)
+ P (q).

(7)

Por lo tanto:
L(q, q)
=

1
2

q1 q2

T


(m1 + m2 )l1 2 + m2 l2 2 + 2l1 l2 m2 cos(q1 )
m2 l2 2 + l1 l2 m2 cos(q1 )


q1
m2 l2 2 + l1 l2 m2 cos(q1 )
2
q2
m2 l2

+ (m1 + m2 ) g l1 Sen(q1 ) + gm2 l2 Sen(q1 + q2 )

Procedemos a resolver las ecuacion es de Euler Lagrange:
d
Lq Lq = F
dt

(8)

Donde F son las fuerzas externas que no se consideran en el modelo.

Lq = Kq (q, q)
+ Pq (q)

Lq (q, q)
= M (q)q
d
Lq (q, q)
= M (q)
q + M (q)q
dt
La ecuaci
on 8 queda escrita como:
M (q)
q + M (q)q Kq (q, q)
Pq (q) = Gu
Donde Gu es el termino que representa el control aplicado a la segunda
junta relacionada con el
angulo q2 .
Reemplazamos el termino Kq (q, q)

por una expresi

on en funcion del angulo q el cual escribiremos como:
Kq (q, q)
= F (q, q)
q
De esta forma la ecuaci
on anterior se reescribe como:
M (q)
q + (M (q) F (q, q))
q Pq (q) = Gu
M (q)
q + C(q, q)
q Pq (q) = Gu
Con las matrices M (q), C(q, q),
Pq (q), G siguientes:

(m1 + m2 )l1 2 + m2 l2 2 + 2l1 l2 m2 cos(q1 )
M (q) =
m2 l2 2 + l1 l2 m2 cos(q1 )

m2 l2 2 + l1 l2 m2 cos(q1 )
m2 l2 2



q2 q2 + q1

F (q, q)
= (M (q) F (q, q))
= C(q, q)
= m2 l1 l2 sin(q2 )
q1
0


(m1 + m2 )l1 gcos(q1 ) + m2 l2 gcos(q1 + q2 )
Pq (q) =
m2 l2 gcos(q1 + q2 )
 
0
G=
1
Ahora procedemos a realizar la simulacion en lazo abierto con lo que se
obtuvo:

Figura 3: Fig.1: Diagrama de simulacion

Con el codigo de Qu2(q)=

1
2
3
4
5
6
7
8
9
10

function Qu2 = matrix(q,q1)

m1=0.2;
m2=0.6;
l_1=0.6;
l_2=0.3;
g=9.81;
a11= ((m1+m2)*l_12 + m2*l_22+2*l_1*l_2*m2*cos(q(2,1)));
a12= (m2*l_22 + m2*l_1*l_2*cos(q(2,1)));
a21= a12;
a22= (m2*l_22);
5

11
12
13
14
15
16
17
18

M=[ a11 a12 ; a21 a22];

Minv=inv(M);
h=-m2*l_1*l_2*sin(q(2,1));
C=[ h*q1(2,1) h*(q1(2,1)+ q1(1,1)); -h*q1(1,1) 0];
a1=(m1+m2)*g*l_1*cos(q(1,1))+ m2*g*l_2*cos(q(1,1)+q(2,1));
a2=m2*l_2*g*cos(q(1,1)+q(2,1));
P=[ a1; a2];
Qu2=Minv*(-C*q1-P);
Este bloque recibe la informacion de los angulos y la derivada de los
mismos, despues evalua las expresiones para obtener las matrices M, C, P y
regresa el valor de la aceleracion. El codigo para la simulacion es:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21

for i=1:size(q)
m1=0.2;
m2=0.6;
l_1=0.6;
l_2=0.3;
x0=0;
y0=0;
x1= l_1*cos(q(i,1));
y1= l_1*sin(q(i,1));
x2= l_1*cos(q(i,1))+l_2*cos(q(i,1)+q(i,2));
y2= l_1*sin(q(i,1))+l_2*sin(q(i,1)+q(i,2));
x=[x0;x1;x2];
y=[x0;y1;y2];
figure(3)
%hold on
grid()
plot(x,y,ok)
axis([-1 1 -1 1])
%hold off
%pause(0.0001)
end
De los valores que se obtuvieron de los angulos guardados en vectores, se
realizan iteraciones for para graficar, basados en las ecuaciones del acrobot,
la din
amica del movimiento, esto se grafica con una representacion mas
visual con un conjunto de lineas y circulos que permiten observar mejor el
movimiento del acrobot. Lo cual nos genera las siguientes graficas:


Figura 4: Angulo
q1

Figura 5: Angulo
q2

Procedemos
encontrar al sistema de la forma x = f (x, u) con el estado
a
x1
q1
x2 q2

x=
x3 = q1
x4
q2
proponemos:
x 1 = q1

; x 2 = q2

; x3 = q1

; x4 = q4

x1 = x 3
x2 = x 4
x3 = ((2 l2 u + 2 l1 u cos(x2 ) g l1 l2 m2 cos(x1 + 2 x2 )
l12 l2 m2 x23 sin(2 x2 ) 2 l1 l22 m2 x23 sin(x2 ) 2 l1 l22 m2
x24 sin(x2 ) + 2 g l1 l2 m1 cos(x1 ) + g l1 l2 m2 cos(x1 ) 4 l1
l22 m2 x3 x4 sin(x2 ))/(l12 l2 (2 m1 + m2 m2 cos(2 x2 ))))
x4 = (2 l12 m1 u + 2 l12 m2 u + 2 l22 m2 u 2 l1 l23 m22 x23
sin(x2 ) 2 l13 l2 m22 x23 sin(x2 ) 2 l1 l23 m22 x42 sin(x2 ) + 2 g
l1 l22 m22 cos(x1 ) 2 l12 l22 m22 x23 sin(2 x2 ) l12 l22 m22 x24 sin(2
x2 ) + 4 l1 l2 m2 u cos(x2 ) 2 l13 l2 m1 m2 x23 sin(x2 ) 4 l1
l23 m22 x3 x4 sin(x2 ) + 2 g l12 l2 m22 sin(x1 ) sin(x2 ) + 2 g l1
l22 m1 m2 cos(x1 ) 2 g l1 l22 m22 cos(x1 ) cos(x2 )2 2 l12 l22 m22
x3 x4 sin(2 x2 ) + 2 g l1 l22 m22 cos(x2 ) sin(x1 ) sin(x2 ) + 2 g
l12 l2 m1 m2 sin(x1 ) sin(x2 ))/(2 l12 l22 m2 (m1 + m2 m2 cos(x2 )2 ))
Entonces procedemos a linealizar este sistema para escribirlo de la forma:
= A + Bu
Con A =

f
|x=x
; u = u

B=

f
|x=x
; u = u

tenemos entonces en la condicion inicial x

=
=0
0 y u
0

0
0
1 0
0
0
0 1

A=
g/l1
(g m2)/(l1 m1)
0 0
g/l1 (g (l1 m1 + l1 m2 + l2 m2 ))/(l1 l2 m1 ) 0 0

B=
2

(l1 + l2 )/(l1 l2 m1 )
2
2
2
2
2
(l1 m1 + l1 m2 + l2 m2 + 2 l1 l2 m2 )/(l1 l2 m1 m2 )
8


0
0,1

Ahora simulamos el sistema linealizado para la condicion inicial =

0
0

Figura 6: Simulacio
n sistema linealizado con u=0

Las graficas resultantes son las siguientes:

Figura 7: Grafica de Angulo

q1 sistema no lineal y sistema linealizado
9


Figura 8: Grafica del Angulo
q2 sistema no lineal y sistema linealizado

La primera grafica nos muestra el comportamiento original del sistema y

como se puede observar en estas graficas el sistema linealizado se comporta
de forma lineal en un area cercana al punto x

Dise
namos la ley de control u = K que minimice el costo:
R nf

(xT (t)Qx(t) + u2(t))dt

10 0 0 0
0 10 0 0

Con la matriz Q =
0 0 1 0
0 0 0 1
J(u) =

Proponemos R = 1 y obtenemos en matlab la matriz que minimiza el costo:

[K, S, E] = lqr(A, B, Q, R)
K =[ -824.8358 -227.1914 -242.1133 -69.1705]
Procedemos a simular el sistema en lazo cerrado. Obtuvimos los siguientes resultados de la simulaci
on del sistema linealizado en lazo cerrado:
10

Figura 9: Simulaci
on del sistema linealizado en lazo cerrado

1
2
3
4
5

El sistema no lineal se realimento con la ganancia obtenida en la seccion

anterior y se encontro que para condiciones iniciales mayores a x(0)=0.02 el
sistema no se estabilizaba, pero para condiciones iniciales menores se obtienen los resultados de las figuras (13)-(14) para q1 y q2 respectivamente, en
la figura (12) se presenta el diagrama utilizado para la simulacion. El codigo
de matlab usado para el bloque U es el siguiente:
function u = fcn(X)%#codegen
K=[-824.8358 -227.1914 -242.1133 -69.1705];
xB=[pi/2;0;0;0];
X1=X-xB;
u=-K*X1;
Se reciben los valores de las variables x1 , x2 , x3 , x4 y les resta la condicion
inicial, despues se multiplica por la matriz de ganancias K y este resultado
se le asigna a la salida u.

Comentarios finales
Podemos observar de las graficas obtenidas como el sistema no lineal en
lazo cerrado funciona de manera considerablemente aceptable para condiciones iniciales peque
nas, bajo estas condiciones iniciales podemos entonces
estabilizar el acrobot en el punto de equilibrio no lineal resolviendo un pro
blema de Control Optimo
que nos brinde la ganancia K optima para este
fin.

11


Figura 10: Angulo
q1 del sistema linealizado en lazo cerrado

Figura 11: Angulo

q2 del sistema linealizado en lazo cerrado

12

Figura 12: Diagrama de simulacion sistema no lineal retroalimentado

Figura 13: Angulo

q1 del sistema no lineal en lazo cerrado

13

Figura 14: Angulo

q2 del sistema no lineal en lazo cerrado