Examen 4 Control optimo

Kevin Lopez Preciado

3 de mayo de 2015

Modelado del Acrobot

a) Calcular la energa Cinetica y Potencial

Sea q = (matriz)q1 q2 Rn la posicion generalizada del pendulo.

Figura 1: Acrobot

Realizamos un analisis geometrico del pendulo para obtener la energa cineti-ca y potencial asociadas al pendulo.Escribimos entonces las ecuacion es respectivas para el angulo q1:

x1 = l1Cos(q1) (1)

y1 = l1Sen(q1) (2)

Ahora escribimos las ecuacion es correspondientes al angulo q2.

x2 = x1 + l2Cos(q1 + q2)

x2 = l1Cos(q1) + l2Cos(q1 + q2) (3)y2 = y1 + l2Sen(q1 + q2)

1

Figura 2: Analisis geometrico

y2 = l1Sen(q1) + l2Sen(q1 + q2) (4)

La energa Cinetica total asociada al pendulo es aquella asociada a q1 y laasociada a q2 y la escribimos como:

K(q, q) =1

2m1l

21 q1

2 +1

2m2(x

22 + y

22) (5)

Para poder encontrar una expresion para la ecuacion 5 procedemos a encon-trar las derivadas de las componentes geometricas del pendulo.

x2 = l1Sen(q1)q1 l2Sen(q1 + q2)(q1 + q2)y2 = l1Cos(q1)q1 + l2Cos(q1 + q2)(q1 + q2)

Ahora procedemos a encontrar el termino (x22 + y22) de la ecuacion 5.

x22 = (l1Sen(q1)q1 l2Sen(q1 + q2)(q1 + q2))2

= (l1Sen(q1)q1)2 + 2(l1Sen(q1)q1)(l2Sen(q1 + q2)(q1 + q2)) +(l2Sen(q1 + q2)(q1 + q2))2

= l21Sen2(q1)q1

2 + 2l1l2Sen(q1)Sen(q1 +q2)q1(q1 + q2) + l22Sen

2(q1 +q2)(q1 + q2)

2

y22 = (l1Cos(q1)q1 + l2Cos(q1 + q2)(q1 + q2))2

= (l1Cos(q1)q1)2 + 2(l1Cos(q1)q1)(l2Cos(q1+q2)(q1+q2)) + (l2Cos(q1+

q2)(q1 + q2))2

2

= l21Cos2(q1)q1

2 + 2l1l2Cos(q1)Cos(q1 +q2)q1(q1 + q2) + l22Cos

2(q1 +q2)(q1 + q2)

2

Debido a que:Sen2(q1) + Cos

2(q1) = 1Sen(q1)Sen(q1 + q2) + Cos(q1)Cos(q1 + q2) = Cos(q2)

Entonces:(x22 + y

22) = l

21Sen

2(q1)q12 +2l1l2Sen(q1)Sen(q1 +q2)q1(q1 + q2)+ l

22Sen

2(q1 +q2)(q1+q2)

2+l21Cos2(q1)q1

2+2l1l2Cos(q1)Cos(q1+q2)q1(q1+q2)+l22Cos

2(q1+q2)(q1 + q2)

2

= l21q12 + 2l1l2Cos(q2)(q

21 + q1q2) + l

22 (q1 + q2)

2

Despues de realizar este procedimiento encontramos la expresion para laenerga cinetica total del sistema:

K(q, q) =1

2m1l

21 q1

2 +1

2m2(x

22 + y

22)

=1

2m1l

21 q1

2 +1

2m2(l

21q1

2 + 2l1l2Cos(q2)(q21 + q1q2) + l

22 (q1 + q2)

2)

=1

2(((m1 + m2)l

21 + 2m2l1l2Cos(q2) + m2l

22 )q1

2 + (2m2l1l2Cos(q2) +

2m2l22 )q1q2 + (m2l

22 )q

22)

Reescribimos la ecuacion 5 de forma matricial de la forma:

K(q, q) =1

2 qT M(q) q

= 12(q1 q2

)T ((m1 +m2)l12 +m2l22 + 2l1l2m2cos(q1) m2l22 + l1l2m2cos(q1)m2l2

2 + l1l2m2cos(q1) m2l22

)(q1q2

)Ahora encontramos la expresion matematica de la energa potencial aso-ciada al pendulo:

P (q) = m1 g h1 + m2 g h2 P (q) = m1 g l1Sen(q1) +m2 g (l1Sen(q1) + l2Sen(q1 + q2))

P (q) = (m1 +m2) g l1Sen(q1) + gm2l2Sen(q1 + q2) (6)

2) Encontramos ahora el modelo Lagrangiano, para ello tenemos que re-solver las ecuacion es de Euler Lagrange.

3

El Lagrangiano del sistema es:

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

Por lo tanto:

L(q, q) = 12(q1 q2

)T ((m1 +m2)l12 +m2l22 + 2l1l2m2cos(q1) m2l22 + l1l2m2cos(q1)m2l2

2 + l1l2m2cos(q1) m2l22

)(q1q2

)+ (m1 +m2) g l1Sen(q1) + gm2l2Sen(q1 + q2)

Procedemos a resolver las ecuacion es de Euler Lagrange:

d

dtLq Lq = F (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)qd

dtLq(q, q) = M(q)q + M(q)q

La ecuacion 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 segundajunta relacionada con el angulo q2.

Reemplazamos el termino Kq(q, q)por una expresion en funcion del angulo q el cual escribiremos como:

Kq(q, q) = F (q, q)q

De esta forma la ecuacion 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:

M(q) =

((m1 +m2)l1

2 +m2l22 + 2l1l2m2cos(q1) m2l2

2 + l1l2m2cos(q1)m2l2

2 + l1l2m2cos(q1) m2l22

)

4

F (q, q) = ( M(q) F (q, q)) = C(q, q) = m2l1l2sin(q2)(q2 q2 + q1q1 0

)Pq(q) =

((m1 +m2)l1gcos(q1) +m2l2gcos(q1 + q2)

m2l2gcos(q1 + q2)

)G =

(01

)Ahora procedemos a realizar la simulacion en lazo abierto con lo que seobtuvo:

Figura 3: Fig.1: Diagrama de simulacion

Con el codigo de Qu2(q)=

1 function Qu2 = matrix(q,q1)2 m1=0.2;3 m2=0.6;4 l_1=0.6;5 l_2=0.3;6 g=9.81;7 a11= ((m1+m2)*l_12 + m2*l_22+2*l_1*l_2*m2*cos(q(2,1)));8 a12= (m2*l_22 + m2*l_1*l_2*cos(q(2,1)));9 a21= a12;

10 a22= (m2*l_22);

5

11 M=[ a11 a12 ; a21 a22];12 Minv=inv(M);13 h=-m2*l_1*l_2*sin(q(2,1));14 C=[ h*q1(2,1) h*(q1(2,1)+ q1(1,1)); -h*q1(1,1) 0];15 a1=(m1+m2)*g*l_1*cos(q(1,1))+ m2*g*l_2*cos(q(1,1)+q(2,1));16 a2=m2*l_2*g*cos(q(1,1)+q(2,1));17 P=[ a1; a2];18 Qu2=Minv*(-C*q1-P);

Este bloque recibe la informacion de los angulos y la derivada de losmismos, despues evalua las expresiones para obtener las matrices M, C, P yregresa el valor de la aceleracion. El codigo para la simulacion es:

1 for i=1:size(q)2 m1=0.2;3 m2=0.6;4 l_1=0.6;5 l_2=0.3;6 x0=0;7 y0=0;8 x1= l_1*cos(q(i,1));9 y1= l_1*sin(q(i,1));

10 x2= l_1*cos(q(i,1))+l_2*cos(q(i,1)+q(i,2));11 y2= l_1*sin(q(i,1))+l_2*sin(q(i,1)+q(i,2));12 x=[x0;x1;x2];13 y=[x0;y1;y2];14 figure(3)15 %hold on16 grid()17 plot(x,y,ok)18 axis([-1 1 -1 1])19 %hold off20 %pause(0.0001)21 end

De los valores que se obtuvieron de los angulos guardados en vectores, serealizan iteraciones for para graficar, basados en las ecuaciones del acrobot,la dinamica del movimiento, esto se grafica con una representacion masvisual con un conjunto de lineas y circulos que permiten observar mejor elmovimiento del acrobot. Lo cual nos genera las siguientes graficas:

6

Figura 4: Angulo q1

Figura 5: Angulo q2

7

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

x =

x1x2x3x4

=q1q2q1q2

proponemos:

x1 = q1 ;x2 = q2 ;x3 = q1 ;x4 = q4

x1 = x3x2 = x4

x3 = ((2 l2 u+ 2 l1 u cos(x2) g l1 l2 m2 cos(x1 + 2 x2)l21 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))/(l21 l2 (2 m1 +m2 m2 cos(2 x2))))

x4 = (2 l21 m1 u+ 2 l21 m2 u+ 2 l22 m2 u 2 l1 l32 m22 x23 sin(x2) 2 l31 l2 m22 x23 sin(x2) 2 l1 l32 m22 x42 sin(x2) + 2 g l1 l22 m22 cos(x1)2 l21 l22 m22 x23 sin(2x2) l21 l22 m22 x24 sin(2x2) + 4 l1 l2 m2 u cos(x2) 2 l31 l2 m1 m2 x23 sin(x2) 4 l1 l32 m22 x3 x4 sin(x2) + 2 g l21 l2 m22 sin(x1) sin(x2) + 2 g l1 l22 m1 m2 cos(x1)2 g l1 l22 m22 cos(x1) cos(x2)22 l21 l22 m22 x3 x4 sin(2 x2) + 2 g l1 l22 m22 cos(x2) sin(x1) sin(x2) + 2 g l21 l2 m1 m2 sin(x1)sin(x2))/(2 l21 l22 m2 (m1 +m2m2 cos(x2)2))

Entonces procedemos a linealizar este sistema para escribirlo de la forma: = A +Bu

Con A =f

x| x = x;u = u y B = f

u| x = x;u = u

tenemos entonces en la condicion inicial x =

pi2000

y u = 0

A =

0 0 1 00 0 0 1g/l1 (g m2)/(l1 m1) 0 0g/l1 (g (l1 m1 + l1 m2 + l2 m2))/(l1 l2 m1) 0 0

B =

00

(l1 + l2)/(l21 l2 m1)(l21 m1 + l21 m2 + l22 m2 + 2 l1 l2 m2)/(l21 l22 m1 m2)

8

Ahora simulamos el sistema linealizado para la condicion inicial =

0

0,100

Figura 6: Simulacion 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 ycomo se puede observar en estas graficas el sistema linealizado se comportade forma lineal en un area cercana al punto x

Disenamos la ley de control u = K que minimice el costo:

J(u) = nf

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

Con la matriz Q =

10 0 0 00 10 0 00 0 1 00 0 0 1

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 siguien-tes resultados de la simulacion del sistema linealizado en lazo cerrado:

10

Figura 9: Simulacion del sistema linealizado en lazo cerrado

El sistema no lineal se realimento con la ganancia obtenida en la seccionanterior y se encontro que para condiciones iniciales mayores a x(0)=0.02 elsistema no se estabilizaba, pero para condiciones iniciales menores se obtie-nen los resultados de las figuras (13)-(14) para q1 y q2 respectivamente, enla figura (12) se presenta el diagrama utilizado para la simulacion. El codigode matlab usado para el bloque U es el siguiente:

1 function u = fcn(X)%#codegen2 K=[-824.8358 -227.1914 -242.1133 -69.1705];3 xB=[pi/2;0;0;0];4 X1=X-xB;5 u=-K*X1;

Se reciben los valores de las variables x1, x2, x3, x4 y les resta la condicioninicial, despues se multiplica por la matriz de ganancias K y este resultadose le asigna a la salida u.

Comentarios finales

Podemos observar de las graficas obtenidas como el sistema no lineal enlazo cerrado funciona de manera considerablemente aceptable para condi-ciones iniciales pequenas, bajo estas condiciones iniciales podemos entoncesestabilizar el acrobot en el punto de equilibrio no lineal resolviendo un pro-blema de Control Optimo que nos brinde la ganancia K optima para estefin.

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

Figura 14: Angulo q2 del sistema no lineal en lazo cerrado

13