UNIVERSIDAD CATÓLICA DE SANTA MARÍA

ESCUELA PROFESIONAL DE INGENIERÍA MECÁNICA,

MECÁNICA – ELÉCTRICA Y MECATRÓNICA

CURSO:
CONTROL MECATRÓNICO III

INFORME N°06: Control Óptimo Cuadrático (LQR) -

Aplicación

GRUPO : 02

DOCENTE : DR. QUISPE CCACHUCO, MARCELO

ALUMNO : QUISPE GUTIERREZ, ERICK MARTINS

AREQUIPA-PERÚ
2020
Ejercicio N° 01:

Se tiene un robot tipo carro con tracción delantera. Se desea que estacione de forma
horizontal con un control LQR y se muestre una simulación de la evolución de la posición,
así como un diagrama de la señal de control u (timón del carro)

Consideraciones:
- el robot va hacia atrás.
- la coordenada será ubicada en la parte delantera.

Armando el sistema en espacio-estado

Si se consideran ángulos muy pequeños, entonces

Estados Entrada Salida
 𝑥1̇ = 𝑣(𝑥2 +𝑢)

a) Probar el siguiente programa con los siguientes valores:

Peso q1: 0.2
Peso q2: 0.2
Introducir coordenada inicial x [10]: 20
Introducir coordenada inicial y [-30 30]: 5
Introducir ángulo inicial phi (grados) [-180 180]: 90
Introducir yast (posición de estacionar): 0
clear; Borra todas las variables cerradas
clc; close anteriormente
all; Limpia la pantalla
Cierra las ventanas como graficas
v = 2; L = 3; dt Velocidad inicial
= 0.001; ti = 0; Longitud del camion
tf = 20; Espaciado del tiempo
Tiempo inicial y final
A = [ 0 v Matrices de espacio estado
0 0 ];
B = [ v
-v/L ];
q1 = input('Peso q1: '); Pesos iniciales para la solución de riccati
q2 = input('Peso q2: '); Q
= diag([ q1 q2 ]); Se crea una matriz diagonal con esos pesos
P = are(A,B*B',Q);
K = B'*P; k1 = Solución de la ecuación de riccati
K(1,1); k2 = K es la ley de control u=.-kx
K(1,2); Se separa k1 y k2
x = input('Introducir Ingresar valores iniciales a traes del
coordenada inicial x [10]: '); workspace
y = input('Introducir
coordenada inicial y [-30
30]: '); fi =
input('Introducir angulo
inicial phi (grados) [-180
180]: ');
fi = fi*pi/180; Conversión de grados a radianes
yast = input('Introducir yast: Ingreso de ydeseado
'); fiast Angulo inicial
= 0; k = Cotador inicia en 1
1;
tanpi4 = tan(pi/4); Cambio de variable
while((x>0) & (x<50)) Bucle infinito
posx(k,1) = x; posy(k,1) De la solución del modelo matemáticos,
= y; posphi(k,1) = fi; tenesmo las siguientes ecuaciones y para
t(k,1) = (k-1)*dt; graficar sevan generando vectores tiempo,
tandel = -k1*(y-yast) - vector y, vector
k2*(fi-fiast); if(tandel Ley de control
> tanpi4) tandel =
tanpi4; elseif(tandel < Se hace la saturación al valor máximo y al
-tanpi4) tandel = - valor mínimo entre –pi/4 y pi/4
tanpi4; end del =
atan(tandel);
delta(k,1) = 180/pi*del;
xp = v*cos(fi); yp =
v*sin(fi); fip = -
v/L*tan(del); x = x +
xp*dt; y = y + yp*dt; Se aplican las ecuaciones que encontramos
fi = fi + fip*dt; k = k matemáticamente y se van generando
+ 1; end vectores

figure(1); Se grafica en un plano xy La

plot(posx,posy); grid; variación del ángulo.
title('Trajectory'); Se imita la gráfica entre 0 y 50 en el eje x
axis([0 50 -40 40]); Y entre -40 y 40 el eje y
figure(2); plot(t,delta);
grid; title('Steering Se le pone título a la grafica
Angle')

Tenemos:

Trayectoria realizada por el carro para llegar a las condiciones deseadas

 Movimiento realizado por el timón para llegar a la trayectoria deseada

b) Haciendo variar el ángulo inicial phi desde -180° hasta 180°, y la posición inicial
“y” de -30 a 30, con los valores invariantes de q1=0.1 y q2=0.1 xini=10 y*=0,
llenar la siguiente tabla:

yini
-30 -25 -15 -10 -5 0 5 10 15 25 30
-180
-150
fi_ini
-120
-90
-60
0
60
90
120
150
180

Ejercicio N° 02:

Se tiene un robot tipo carro con tracción delantera. Se desea que estacione de forma
horizontal con un control LQR y se muestre una simulación de la evolución de la posición,
así como un diagrama de la señal de control u (timón del carro).
 Simplificación del Simplificación del modelo
La dirección y tracción es
modelo
delantera

Armando el sistema en espacio-estado

Si se consideran ángulos muy pequeño: % se descarta, porque no depende de

nada

Estados Entrada Salida

Haciendo el cambio de variable

𝑥1̇ = 𝑣.𝑥2
Modificar el código anterior y probar el siguiente programa con los siguientes valores:
Ejercicio N° 03:

Se tiene un robot manipulador de 2 GDL. Se desea controlar los torques para tener una
trayectoria lineal con un control LQR y se muestre una simulación de la evolución de la
posición, así como un diagrama de la señal de control u.

Luego de hacer el modelamiento:

Diagrama de cuerpo libre para el modelamiento del sistema

Analizamos la barra 1 aplicando la ley de Newton

 Analizamos la barra 2 aplicando la ley de Newton

Obtenemos las ecuaciones dinámicas, y haciendo un arreglo tenemos:

𝐼1 + 𝑚1 ∗ 𝑙12 + 𝑚2 ∗ 𝐿21 + 𝑚2 ∗ 𝐿1 ∗ 𝑙2 ∗ 𝑐𝑜𝑠(𝜃2 ) 𝑚2 ∗ 𝐿1 ∗ 𝑙2 ∗ cos⁡(𝜃2 ) 𝜃1̈
[ ][ ]
𝐼2 + 𝑚2 ∗ 𝑙22 + 𝑚2 ∗ 𝐿1 ∗ 𝑙2 ∗ cos⁡(𝜃2 ) 𝐼2 + 𝑚2 ∗ 𝑙22 𝜃2̈
2
𝑚2 ∗ 𝐿1 ∗ 𝑙2 ∗ (𝜃1̇ + 𝜃2̇ ) ∗ 𝑠𝑒𝑛(𝜃2 )
=[ 2
]
−𝑚2 ∗ 𝐿1 ∗ 𝑙2 ∗ 𝜃1̇ ∗ 𝑠𝑒𝑛(𝜃2 )
−(𝑚1 ∗ 𝑙1 + 𝑚2 ∗ 𝐿1 ) ∗ cos⁡(𝜃1 ) 1 −1 𝜏1
+[ ]𝑔 + [ ][ ]
−𝑚2 ∗ 𝑙2 ∗ cos⁡(𝜃1 + 𝜃2 ) 0 1 𝜏2
Se hace una aproximación alrededor de 𝜃1 = 0, 𝜃2 = 0
𝑠𝑒𝑛(𝜃1 ) = 𝑠𝑒𝑛(𝜃2 ) = 0
cos(𝜃1 ) = cos(𝜃2 ) = 1
Tenemos:

𝑀11 𝑀12 𝜃1̈ 0 𝐺1 𝜏1

[ ] [ ] = [ ] + [ ] 𝑔 + [𝑆] [𝜏 ]
𝑀21 𝑀22 𝜃2̈ 0 𝐺2 2

𝑀𝜃̈ = 𝑆𝑇
𝜃1̇ 0 0 1 0 𝜃1
𝜃2̇ 0 0 0 1 𝜃2 𝑍𝑒𝑟𝑜𝑠(2,2) 𝜏1
=[ ] ̇ +[ ] [𝜏 ]
𝜃1̈ 0 0 0 0 𝜃1 𝑀−1 𝑆 2

[𝜃2̈ ] 0 0 0 0 [𝜃2̇ ]

Donde:
𝑀11 = 𝐼1 + 𝑚1 ∗ 𝑙1 ∗ 𝑙1 + 𝑚2 ∗ 𝐿1 ∗ 𝐿1 + 𝑚2 ∗ 𝐿1 ∗ 𝑙2
𝑀12 = 𝑚2 ∗ 𝐿1 ∗ 𝑙2
𝑀21 = 𝐼2 + 𝑚2 ∗ 𝑙2 ∗ 𝑙2 + 𝑚2 ∗ 𝐿1 ∗ 𝑙2
 𝑀22 = 𝐼2 + 𝑚2 ∗ 𝑙2 ∗ 𝑙2
1 −1
𝑆=[ ]
0 1
Ejecutar el siguiente código y analizar cada una de sus partes:
% Control de brazo robot de dos
grados de libertad.
% Linealización por aproximación
alrededor del punto
% de operación fi1=0, fi2=0.
clear; Limpieza de las variables
close all;
clc; Cerramos todas las ventanas abiertas
Limpiamos ventana de comandos
m1 = 2.5;
L1 = 0.35;
l1 = 0.16; % Longitud
I1 = 1.1; % Inercia
m2 = 1.8; Valores de las variables del brazo
L2 = 0.30;
l2 = 0.12; % Longitud
I2 = 0.8; % Inercia
M11 = I1 + m1*l1*l1 + m2*L1*L1 + Para su simplificación en el código se
m2*L1*l2;
M12 = m2*L1*l2;
trabajan individualmente y se agregan a
M21 = I2 + m2*l2*l2 + m2*L1*l2; la matriz después.
M22 = I2 + m2*l2*l2;
M = [ M11 M12
M21 M22 ];
S = [ 1 -1 Valor de S
0 1 ]; Tanto ceros como iden vienen a
ceros = zeros(2,2); simplificar la construcción de la matriz
iden = eye(2);
A = [ ceros iden siguiente
ceros ceros ];
B = [ ceros Definimos la matriz A y B
inv(M)*S ];
q1 = input('Peso q1 [100 - 5e5]: Definimos los pesos de la matriz Q y
');
q2 = input('Peso q2 [100 - 5e5]:
usamos el comando diag para obtener
'); matrices diagonales rápidamente.
q3 = input('Peso q3 [5]: ');
q4 = input('Peso q4 [5]: ');
% El peso q3 y q4 reduce la mala
respuesta inicial.
Q = diag([ q1 q2 q3 q4 ]);
R = [1 0;0 1]; Aplicamos control óptimo cuadrático lqr
[K,P] = lqr(A,B,Q,R);

%ti = 0; tf = 4; dt = 0.001;
%t = ti:dt:tf;
%t = t';
Condiciones iniciales comentadas
%r1 = 80*pi/180; % Angulo deseado
r1
%r2 = 80*pi/180; % Angulo deseado
r2
% Trayectoria triangular. Inicio
(0.65,0)
% Triangulo vertices: Trayectoria deseada comentada
(0.40,0.00)(0.525,0.10)(0.65,0)
v = 2*0.01; % Velocidad de
desplazamiento en X
ti = 0; dt = 0.001;
t1 = 0.125/v;
t2 = 0.250/v;
tt1 = ti:dt:t1; tt1 = tt1';
tt2 = (t1+dt):dt:t2; tt2 = tt2';
tf = t2; Ecuaciones para el eje x, limitando los
x1 = -0.125/t1*tt1 + 0.65;
x2 = 0.125/(t1-t2)*(tt2-t2) + puntos a trabajar
0.40;
xr = [ x1; x2 ];
y1 = 0.10/t1*tt1;
y2 = 0.10/(t1-t2)*(tt2-t2);
yr = [ y1; y2]; Ecuaciones para el eje y, triángulo
x2y2 = xr.*xr + yr.*yr; tomamos 2 rectas
AA = xr./sqrt(x2y2);
BB = (x2y2+L1*L1-
L2*L2)./(2*L1*sqrt(x2y2));
r1A = acos(AA);
r1B = acos(BB);
r1 = r1A - r1B;
ZZ = (x2y2-
(L1*L1+L2*L2))./(2*L1*L2);
r2 = acos(ZZ);
% r1 = r1A + r1B; % Calculo de r1
- Posicion simetrica
% r2 = -r2; % Calculo de r2 -
Posicion simetrica Trabajamos con la parte real del ángulo
r1 = real(r1);
r2 = real(r2);
nr12 = length(r1);
% r1 = pi/4*ones(nr12,1);
% r2 = -pi/6*ones(nr12,1); Obtenemos nuestras funciones listas para
xx = L1*cos(r1) + L2*cos(r1+r2);
plotear la trayectoria deseada
yy = L1*sin(r1) + L2*sin(r1+r2);
fi1 = 0; fi2 = 0;
fi1p = 0; fi2p = 0; Condiciones iniciales del brazo, posición
ang = [ fi1 fi2 ]'; angular y velocidad igual a cero.
vel = [ fi1p fi2p ]';
k = 1;
for tt = ti:dt:tf
ang1(k,1) = fi1;
Ciclo for para la obtención de vectores de
ang2(k,1) = fi2; ángulo de posición y torque.
t(k,1) = tt;
x = [fi1-r1(k,1); fi2-r2(k,1);
fi1p; fi2p ];
T = -K*x; Calcular el torque de entrada
T1(k,1) = T(1,1); Torque servomotor 1
T2(k,1) = T(2,1); Torque servomotor 2
M11a = I1 + m1*l1*l1 + m2*L1*L1
+ m2*L1*l2 * cos(fi2);
M12a = m2*L1*l2*cos(fi2);
M21a = I2 + m2*l2*l2 +
m2*L1*l2*cos(fi2);
Definimos matriz M considerando
M22a = I2 + m2*l2*l2;
M = [ M11a M12a ángulos
M21a M22a ];
 C1 =
m2*L1*l2*(fi1p+fi2p)^2*sin(fi2);
C2 = -
Nuestra matriz G
m2*L1*l2*fi1p*fi1p*sin(fi2);
C = [ C1
C2 ]; Ecuaciones diferenciales
accel = inv(M)*(C + S*T);
ang = ang + vel*dt;
vel = vel + accel*dt; Obtenemos valores
fi1 = ang(1,1);
fi2 = ang(2,1);
fi1p = vel(1,1);
fi2p = vel(2,1);
k = k + 1;
end
%ang1 = ang1*180/pi;
%ang2 = ang2*180/pi;
%figure(1);
%plot(t,ang1);
%figure(2);
%plot(t,ang2);
xra = L1*cos(ang1) +
L2*cos(ang1+ang2); Obtenemos nuestras funciones listas para
yra = L1*sin(ang1) + plotear la trayectoria realizada a partir del
L2*sin(ang1+ang2); control
figure(1);
plot(t,ang1*180/pi,t,r1*180/pi); Graficamos la función de ángulo teta 1
title('Angle Fi1 [deg]');
figure(2);
plot(t,ang2*180/pi,t,r2*180/pi);
title('Angle Fi2 [deg]');
Graficamos la función de ángulo teta 2
figure(3);
plot(xra,yra,xx,yy); Graficamos la trayectoria dibujada
title('Trayectoria X - Y');
figure(4);
plot(t,T1,'-r',t,T2,'b'); Graficamos el torque de los servomotores
title('Torques T1 (rojo) y T2
(azul)');
disp('Pause');
Le avisa al usuario que hay una pausa
pause; Hace la pausa
figure(5);
grid;
Se grafica una última ventana
axis([ 0 0.8 -0.4 0.4]); Definimos sus ejes x y y
hold on;
nk = length(ang1);
dk = 15; % Saltos para la
animación
xra = [ xra
xra(nr12,1)*ones(dk,1) ];
yra = [ yra
yra(nr12,1)*ones(dk,1) ];
% xra(nr12+1:nr12+dk-1,1) =
xra(nr12,1)*ones(dk,1);
% yra(nr12+1:nr12+dk-1,1) =
yra(nr12,1)*ones(dk,1);
for k = 1:dk:nk Ciclo for para obtener vectores para
x1A = 0; graficar en la animación
y1A = 0;
x1B = x1A + L1*cos(ang1(k,1));
y1B = y1A + L1*sin(ang1(k,1)); Obtenemos el vector de coordenadas
x2A = x1B; para la animación del brazo en la figura
 y2A = y1B;
ang12 = ang1(k,1) + ang2(k,1);
x2B = x2A + L2*cos(ang12);
y2B = y2A + L2*sin(ang12);
xxx = [ x1A x2A x2B ]; Valores que toma el eje x
yyy = [ y1A y2A y2B ]; Valores que toma el eje y
plot(xxx,yyy,'-
b','Linewidth',2); Graficamos la función de color azul con la
trayectoria que dibuja
pause(0.001); Tiempo para el siguiente plot
plot(xxx,yyy,'-
w','Linewidth',2); Pintamos de blanco todo lo que no es
necesario mostrando solo un brazo para
la animación finalmente

xxx1 = xra(k,1); Calculamos las coordenadas de la

xxx2 = xra(k+dk-1,1); trayectoria dibujada por el brazo
yyy1 = yra(k,1);
yyy2 = yra(k+dk-1,1);
xxra = [ xxx1 xxx2 ];
yyra = [ yyy1 yyy2];
plot(xxra,yyra,'- Graficamos dicha trayectoria
r','Linewidth',2);
end
figure(5); plot(xx,yy,'-b'); Graficamos la trayectoria deseada
grid; Grilla

Obtenemos:

Comportamiento del ángulo tetha 1 para realizar el movimiento

 Comportamiento del ángulo tetha 2 para realizar el movimiento

Trayectoria realizada en un espacio aleatorio desde el descanso del brazo robótico

 Torques de entrada necesarios para realizar la trayectoria

Parte de la animación del brazo trabajando en un espacio determinado

Modificando el código anterior, para que trace una trayectoria circular:

Comportamiento del ángulo tetha 1 para realizar el movimiento

Comportamiento del ángulo tetha 2 para realizar el movimiento

Trayectoria realizada en un espacio aleatorio desde el descanso del brazo robótico

Torques de entrada necesarios para realizar la trayectoria

 Parte de la animación del brazo trabajando en un espacio determinado

Final de la animación comparando la trayectoria realizada (rojo) con la trayectoria

realizada (azul)