Universidad Politécnica del Valle de México

Ingeniería Mecatrónica

CONTROL AVANZADO
Grupo: 880901

REPORTE 3. CONTROL DE
MOVIMIENTO
Alumnos:
García Molina Victor Manuel
Guzmán Monterrosa Lesley Dennis
López Cardin Pamela Alexa
Pérez Blancarte Alejandro Baruch
Romero Granados Aranza Yeraldyn (subió el archivo)

05/08/2022
NOTA. Se hizo un cambio de motores, puesto que los usados con anterioridad al hacer las pruebas para las
entregas finales tuvieron fallas y creemos que se quemaron.

1. Regulación

 Presentar los parámetros de cada uno de los actuadores a implementar en el prototipo.

SM060R20B30MNBD
𝐽𝑚=0.0000189 𝑘𝑔∙𝑚2 Inercia polar concentrada de motor
𝐵𝑚=0.000212 𝑁𝑚/𝑟𝑝𝑚 Coeficiente de amortiguamiento viscoso del
eje del motor
𝑟=100 Razón de engranaje

SM060R20B30MNBD
𝐽𝑚=0.0000342 𝑘𝑔∙𝑚2 Inercia polar concentrada de motor
𝐵𝑚=0.000423 𝑁𝑚/𝑟𝑝𝑚 Coeficiente de amortiguamiento viscoso del
eje del motor
𝑟=80 Razón de engranaje

SM060R20B30MNBD
𝐽𝑚=0.0000342 𝑘𝑔∙𝑚2 Inercia polar concentrada de motor
𝐵𝑚=0.000423 𝑁𝑚/𝑟𝑝𝑚 Coeficiente de amortiguamiento viscoso del
eje del motor
𝑟=120 Razón de engranaje

 Para cada actuador en su prototipo plantear posiciones articulares de inicio y final que
corresponderán al problema de regulación del órgano terminal para coordenadas
cartesianas que tengan 10 cm de distancia entre ellas. Esto es, 𝑞=𝑓−1(𝑋), resolver la
cinemática inversa para conocer las configuraciones articulares deseadas adecuadas
inicial y final, según sus posiciones cartesianas deseadas dentro del espacio de trabajo.
Se propuso 5 de los 6 valores de las coordenadas cartesianas y el sexto fue calculado,
procurando que sus posiciones cartesianas deseadas estuvieran dentro del espacio de trabajo:

x 2  y 2  ( z  l1 )2  l2  l3

l 2  ( x f  xi )2  ( y f  yi ) 2

Donde:
 l1  45cm
l2  20cm
l3  20cm

Calculando la coordenada cartesiana restante con los siguientes valores recordando están en
una escala de centímetros:

xi  5, yi  23, zi  25
x f  3, y f  21

l  ( x f  xi ) 2  ( x f  yi ) 2  ( z f  zi ) 2

l 2  ( x f  xi ) 2  ( y f  yi ) 2  ( z f  zi ) 2

( z f  zi ) 2  l 2  ( x f  xi ) 2  ( y f  yi ) 2

z f  zi  l 2  ( x f  xi ) 2  ( y f  yi ) 2

z f  l 2  ( x f  xi ) 2  ( y f  yi ) 2  zi

z f  102  (3  5) 2  (21  23) 2  25

Mediante cinemática inversa en un script desarrollado en Matlab se obtuvo las coordenadas

articulares correspondientes.

Por tanto:
  O3 fx   3   q1 f   1.7127 
    

O3i   O3 fy    21   q f   q2 f    1.4706
O3 f z  30.66   q3 f   1.7522 
   

 Presentar los esquemas de lazo cerrado con el regulador deseado a implementar en cada
actuador.

Donde:
 1   0 
  
G(q)  2    g  m2 lc 2 c2  m3 (lc3C23  l2 C2 ) 
3   m3 glc 3C23 

Dichas matrices y vector fueron obtenidas en un curso anterior, donde se analizo la dinámica
del robot en cuestión.

 Presentar la sintonización de las ganancias del regulador a implementar de acuerdo con

los parámetros de los actuadores.

Ilustración 1 Condiciones iniciales del integrador para la obtención de las posiciones articulares

Ilustración 2 Condiciones iniciales del integrador para la obtención de las posiciones articulares.

Se implemento el siguiente código en Matlab:

%%Control PD Multivariable con compensación de gravedad

function u = fcn(qed, qe, kp, kd, G)
Kp1 = kp(1);
Kp2 = kp(2);
Kp3 = kp(3);
Kd1 = kd(1);
Kd2 = kd(2);
Kd3 = kd(3);
Kp = diag([Kp1 Kp2 Kp3]);
Kd = diag([Kd1 Kd2 Kd3]);
u = (Kp*qe)+(Kd*qed)+G;
%%Compensación de gravedad
function G = fcn(q)
% Constantes de masas, longitudes y aceleración de la gravedad
m1 = 3.8; m2 = 1.8; m3 = 1.8;
l1 = 0.45; l2 = 0.20; l3 = 0.20;
lc1 = l1/2;
lc2 = l2/2;
lc3 = l3/2;
g = 9.81;
% Evaluación de funciones trigonométricas
C1 = cos(q(1));
C2 = cos(q(2));
C3 = cos(q(3));
S1 = sin(q(1));
S2 = sin(q(2));
S3 = sin(q(3));
C23 = (C2*C3)-(S2*S3);
S23 = (S2*C3)+(C2*S3);
% Términos Gravitacionales
g1 = 0;
g2 = g*(m2*lc2*C2 + m3*(lc3*C23+l2*C2));
g3 = m3*g*lc3*C23;
G = [g1; g2; g3];
%%Dinámica del robot
function [qp,qpp] = fcn(q,qd,u)
% Constantes de masas, longitudes, momentos principales en z y aceleración de la gravedad
m1 = 3.8; m2 = 1.8; m3 = 1.8; l1 = 0.45; l2 = 0.20; l3 = 0.20;
Ixx1 = (m1/12)*((0.05^2)+(l1^2));
Iyy1 = (m1/12)*((0.05^2)+(l1^2));
Izz1 = (m1/12)*((0.05^2)+(0.05^2));
Ixx2 = (m2/12)*((0.05^2)+(0.05^2));
Iyy2 = (m2/12)*((l2^2)+(0.05^2));
Izz2 = (m2/12)*((l2^2)+(0.05^2));
Ixx3 = (m3/12)*((0.05^2)+(0.05^2));
Iyy3 = (m3/12)*((l3^2)+(0.05^2));
Izz3 = (m3/12)*((l3^2)+(0.05^2));
lc1 = l1/2;
lc2 = l2/2;
lc3 = l3/2;
g = 9.81;
% Evaluación de funciones trigonométricas
C1 = cos(q(1));
C2 = cos(q(2));
C3 = cos(q(3));
S1 = sin(q(1));
S2 = sin(q(2));
S3 = sin(q(3));
C23 = (C2*C3)-(S2*S3);
S23 = (S2*C3)+(C2*S3);
% Matriz de Inercia
d11 = m2*lc2^2*C2^2 + m3*(lc3*C23+l2*C2) + Iyy1 + Iyy2 + Iyy3 + S2^2*(Ixx2-Iyy2) +
S23^2*(Ixx3-Iyy3);
d12 = 0;
d13 = 0;
d21 = d12;
d22 = m2*lc2^2 + m3*(l2^2+2*l2*lc3*C3+lc3^2) + Izz2 + Izz3;
d23 = 0;
d31 = d13;
d32 = d23;
d33 = m3*lc3^2 + Izz3;
D = [d11 d12 d13;
d21 d22 d23;
d31 d32 d33];
iD = inv(D);
% Matriz de Coriolis
c11 = - (m2*lc2^2*C2*S2 + m3*(lc3*C23+l2*C2)*(lc3*S23+l2*S2) - S2*C2*(Ixx2-Iyy2)
-S23*C23*(Ixx3-Iyy3))*qd(2)-(m3*lc3*S23*(lc3*C23+l2*C2)…
S23*C23*(Ixx3Iyy3))*qd(3);
c12 = -(m2*lc2^2*C2*S2 + m3*(lc3*C23+l2*C2)*(lc3*S23+l2*S2) - S2*C2*(Ixx2-Iyy2) –
S23*C23*(Ixx3-Iyy3))*qd(1);
c13 = -S23*(m3*lc3*(lc3*C23+l2*C2) - C23*(Ixx3-Iyy3))*qd(1);
c21 = - c12;
c22 = - m3*l2*lc3*S3*qd(3);
c23 = - m3*l2*lc3*S3*(qd(2)+qd(3));
c31 = - c13;
c32 = m3*l2*lc3*S3*qd(2);
c33 = 0;
C = [c11 c12 c13;
c21 c22 c23;
c31 c32 c33];
% Términos Gravitacionales
g1 = 0;
g2 = g*(m2*lc2*C2 + m3*(lc3*C23+l2*C2));
g3 = m3*g*lc3*C23;
G = [g1; g2; g3];
% Ecuaciones de Estado
qp = qd;
qpp = iD*(u-(C*qd)-G);
% Efecto de los actuadores
Jm1 = 0.0000189;
Jm2 = 0.0000342;
Jm3 = 0.0000342;
Bm1 = 0.000212;
Bm2 = 0.000423;
Bm3 = 0.000423;
r1 = 100;
r2 = 80;
r3 = 120;
J = diag([r1^2*Jm1 r2^2*Jm2 r3^2*Jm3]);
B = [r1^2*Bm1; r2^2*Bm2; r3^2*Bm3];
M = D+J;
iM = inv(M);
qpp = iM*(u-(C*qd)-(B.*qd)-G);
%%Cinemática de referenciales
function [O1, O2, O3] = cinRefe(q)
l1 = 45;
l2 = 20;
l3 = 20;
S1 = sin(q(1));
S2 = sin(q(2));
S3 = sin(q(3));
C1 = cos(q(1));
C2 = cos(q(2));
C3 = cos(q(3));
S23 = S2.*C3 + C2.*S3;
C23 = C2.*C3 - S2.*S3;
O1 = [0;0;l1];
O2 = [l2*C1*C2;
l2*S1*C2;
l2*S2+l1];
O3 = [l3*C1*C23+l2*C1*C2;
l3*S1*C23+l2*S1*C2;
l3*S23+l2*S2+l1];
Para sintonizar las ganancias para controlar a cada articulación se ajustaron todas a un valor
de 1.

Obteniendo así las siguientes gráficas:

Podemos observar que la respuesta es muy lenta, el valor de la constante de acción
proporcional de la primera articulación se aumentó hasta 20, siendo un valor relativamente
alto.
Obtenemos las siguientes gráficas:
Ahora podemos observar cómo gracias al ajuste la respuesta paso a ser más rápida, pero con
un sobre impulso considerables. Ahora reduciremos el valor de Kp1.

Obtenemos las siguientes gráficas:

Con las constantes Kp encontradas no se presentaron sobre impulsos.

Dándonos las siguientes gráficas:

2. Seguimiento

 De las posiciones articulares deseadas establecidas en el punto 1; interpolar el

movimiento de tal forma que represente un movimiento de línea recta del órgano terminal
y se realice en un tiempo de 5s.
El siguiente código se implemento para que nos entregara la planeación de la trayectoria
rectilínea interpolando con polinomios de tercer y quinto orden.

n = 6; %% Número de muestras
t0 = 0; %% Tiempo inicil
tf = 5; %% Tiempo final
Qt = (tf-t0)/n; %%Indice de cuantificación
t = [t0:Qt:tf]; %Intervalo de tiempo de duración de la trayectoria
mt = (t-t0)/(tf-t0); %Indice de crecimiento lineal del tiempo

%Cálculo de las trayectorias

% Coordenadas iniciales y finales de trayectoria lineal
x0 = 5;
y0 = 23;
z0 = 25;
xf = -3;
yf = 21;
zf = 30.66;
mxy = (yf-y0)/(xf-x0); % Pendiente de la trayectoria lineal
myz = (zf-z0)/(yf-y0);

% Coordenadas cartesianas
x = (xf-x0)*mt+x0;
y = (x-x0)*mxy+y0;
z = (y-y0)*myz+z0;

% Velocidades cartesianas
xp = ((xf-x0)/(tf-t0))*ones(1,length(t));
yp = ((yf-y0)/(tf-t0))*ones(1,length(t));
zp = ((zf-z0)/(tf-t0))*ones(1,length(t));

% Gráfica de trayectoria
figure(1)
plot3(x,y,z)
xlabel('x')
ylabel('y')
zlabel('z')
axis([-45 45 -45 45 0 90])
grid on

% Cálculo de la cinematica inversa

[q1, q2, q3] = cinematicaInv(t,x,y,z);
Q = [q1; q2; q3]; % Aprupamieto de las posiciones articulares

% Cálculo de la velocidad inversa

qp = velocidadInv(xp,yp,zp,Q);
q1p = qp(1,:);
q2p = qp(2,:);
q3p = qp(3,:);

% Cálculo de la aceleración articular

q1pp = diff(q1p);
q2pp = diff(q2p);
q3pp = diff(q3p);
q1s = [];
q2s = [];
q3s = [];
v1s = [];
v2s = [];
v3s = [];
a1s = [];
a2s = [];
a3s = [];
ts = [];

% Interpolación por polinomios

% Selección del tipo de interpolación
selec = input('Indicar el orden del polínomio a utilizar para la interpolación ---> ')
for i = 1:length(t)-1
% Cubico
if selec == 3
d1 = [q1(i) q1p(i) q1(i+1) q1p(i+1) t(i) t(i+1)];
d2 = [q2(i) q2p(i) q2(i+1) q2p(i+1) t(i) t(i+1)];
d3 = [q3(i) q3p(i) q3(i+1) q3p(i+1) t(i) t(i+1)];
[q1d,v1d,a1d,tp] = interPoliCubicos(d1);
[q2d,v2d,a2d,tp] = interPoliCubicos(d2);
[q3d,v3d,a3d,tp] = interPoliCubicos(d3);
end
% Quinto orden
if selec == 5
d1 = [q1(i) q1p(i) 0 q1(i+1) q1p(i+1) 0 t(i) t(i+1)];
d2 = [q2(i) q2p(i) 0 q2(i+1) q2p(i+1) 0 t(i) t(i+1)];
d3 = [q3(i) q3p(i) 0 q3(i+1) q3p(i+1) 0 t(i) t(i+1)];
[q1d,v1d,a1d,tp] = interPoliQuinto(d1);
[q2d,v2d,a2d,tp] = interPoliQuinto(d2);
[q3d,v3d,a3d,tp] = interPoliQuinto(d3);
end

% Unión de los segmentos de la trayectoria

% Posición
q1s = [q1s q1d];
q2s = [q2s q2d];
q3s = [q3s q3d];
% Velocidad
v1s = [v1s v1d];
v2s = [v2s v2d];
v3s = [v3s v3d];
% Aceleración
a1s = [a1s a1d];
a2s = [a2s a2d];
a3s = [a3s a3d];
% Tiempo
ts = [ts tp];
end
% Gráficas de la trayectoria completa
figure(2)
subplot(3,3,1)
plot(ts,q1s,'r')
title('q1')
grid on
subplot(3,3,2)
plot(ts,q2s,'r')
title('q2')
grid on
subplot(3,3,3)
plot(ts,q3s,'r')
title('q3')
grid on
subplot(3,3,4)
plot(ts,v1s,'g')
title('q1p')
grid on
subplot(3,3,5)
plot(ts,v2s,'g')
title('q2p')
grid on
subplot(3,3,6)
plot(ts,v3s,'g')
title('q3p')
grid on
subplot(3,3,7)
plot(ts,a1s,'b')
title('q1pp')
grid on
subplot(3,3,8)
plot(ts,a2s,'b')
title('q2pp')
grid on
subplot(3,3,9)
plot(ts,a3s,'b')
title('q3pp')
grid on
%Creación de arreglo para las funciones de las posiciones articulares
q.time = ts;
q.signals.values = [q1s',q2s',q3s'];
q.signals.dimensions = 3;
% %Creación de arreglo para las funciones de las velocidades articulares
qd.time = ts;
qd.signals.values = [v1s',v2s',v3s'];
qd.signals.dimensions = 3;
%Creación de arreglo para las funciones de las aceleraciones articulares
qdd.time = ts;
qdd.signals.values = [a1s',a2s',a3s'];
qdd.signals.dimensions = 3;
 Presentar las gráficas de las trayectorias articulares obtenidas.

 Presentar los esquemas de lazo cerrado del sistema de seguimiento.

Se implemento un control de dinámica inversa en espacio articular (control de par calculado)
 Especificar los parámetros del bloque feedforward.

 Presentar la simulación de seguimiento de cada actuador.

Ilustración 3 Condiciones iniciales del integrador para la obtención de las posiciones articulares.

Ilustración 4 Condiciones iniciales del integrador para la obtención de las posiciones articulares.

Control lazo interno

function u = fcn(q,qd,aq)
%% Constantes de masas, longitudes, momentos principales en z y
aceleración
%% de la gravedad
m1 = 3.8; m2 = 1.8; m3 = 1.8; l1 = 0.45; l2 = 0.20; l3 = 0.20;
Ixx1 = (m1/12)*((0.05^2)+(l1^2));
Iyy1 = (m1/12)*((0.05^2)+(l1^2));
Izz1 = (m1/12)*((0.05^2)+(0.05^2));
Ixx2 = (m2/12)*((0.05^2)+(0.05^2));
Iyy2 = (m2/12)*((l2^2)+(0.05^2));
Izz2 = (m2/12)*((l2^2)+(0.05^2));
Ixx3 = (m3/12)*((0.05^2)+(0.05^2));
Iyy3 = (m3/12)*((l3^2)+(0.05^2));
Izz3 = (m3/12)*((l3^2)+(0.05^2));
lc1 = l1/2;
lc2 = l2/2;
lc3 = l3/2;
g = 9.81;
%% Evaluación de funciones trigonometricas
C1 = cos(q(1));
C2 = cos(q(2));
C3 = cos(q(3));
S1 = sin(q(1));
S2 = sin(q(2));
S3 = sin(q(3));
C23 = (C2*C3)-(S2*S3);
S23 = (S2*C3)+(C2*S3);
%% Matriz de Inercia
d11 = m2*lc2^2*C2^2 + m3*(lc3*C23+l2*C2) + Iyy1 + Iyy2 + Iyy3
...
+ S2^2*(Ixx2-Iyy2) + S23^2*(Ixx3-Iyy3);
d12 = 0;
d13 = 0;
d21 = d12;
d22 = m2*lc2^2 + m3*(l2^2+2*l2*lc3*C3+lc3^2) + Izz2 + Izz3;
d23 = 0;
d31 = d13;
d32 = d23;
d33 = m3*lc3^2 + Izz3;
D = [d11 d12 d13;
d21 d22 d23;
d31 d32 d33];
iD = inv(D);
%% Matriz de Coriolis
c11 = - (m2*lc2^2*C2*S2 + m3*(lc3*C23+l2*C2)*(lc3*S23+l2*S2) ...
- S2*C2*(Ixx2-Iyy2) - S23*C23*(Ixx3-Iyy3))*qd(2) ...
- (m3*lc3*S23*(lc3*C23+l2*C2) - S23*C23*(Ixx3-Iyy3))*qd(3);
c12 = -(m2*lc2^2*C2*S2 + m3*(lc3*C23+l2*C2)*(lc3*S23+l2*S2) ...
- S2*C2*(Ixx2-Iyy2) - S23*C23*(Ixx3-Iyy3))*qd(1);
c13 = -S23*(m3*lc3*(lc3*C23+l2*C2) - C23*(Ixx3-Iyy3))*qd(1);
c21 = - c12;
c22 = - m3*l2*lc3*S3*qd(3);
c23 = - m3*l2*lc3*S3*(qd(2)+qd(3));
c31 = - c13;
c32 = m3*l2*lc3*S3*qd(2);
c33 = 0;
C = [c11 c12 c13;
c21 c22 c23;
c31 c32 c33];
%% Términos Gravitacionales
g1 = 0;
g2 = g*(m2*lc2*C2 + m3*(lc3*C23+l2*C2));
g3 = m3*g*lc3*C23;
G = [g1; g2; g3];
%% Ecuaciones de Estado
qp = qd;
% qpp = iD*(u-(C*qd)-G);
%% Efecto de los actuadores
Jm1 = 0.0000189;
Jm2 = 0.0000342;
Jm3 = 0.0000342;
Bm1 = 0.000212;
Bm2 = 0.000423;
Bm3 = 0.000423;
r1 = 100;
r2 = 80;
r3 = 120;
J = diag([r1^2*Jm1 r2^2*Jm2 r3^2*Jm3]);
B = [r1^2*Bm1; r2^2*Bm2; r3^2*Bm3];
M = D+J;
u = M*aq + C*qd + B.*qd +G;

Control lazo externo

function aq = fcn(qtilde, qtildep, qdpp, wn, zeta)
K0 = (wn^2)*eye(3);
K1 = (2*zeta*wn)*eye(3);
aq = qdpp -K0*qtilde - K1*qtildep;

Los primeros valores para utilizar fueron unos bajos para la frecuencia natural y del
coeficiente de amortiguamiento.

Obteniendo las siguientes gráficas:

Al analizar las respuestas, optamos por aumentar la frecuencia natural general hasta que las
articulaciones sigan adecuándose.

Dándonos las siguientes gráficas:

Al llegar a estos resultados, no dimos cuenta que la respuesta de las velocidades articulares,
por tener condiciones iguales a 0, presentaban un cambio repentino con un permisible sobre
impulso.

Entonces, para mejorar esto se introdujeron las condiciones iniciales que arrojó la planeación
de la trayectoria, con esto se pudo mejorar la respuesta de las posiciones y las velocidades
articulares, esto podemos observarlo en la siguiente gráfica:
 Presentar en grafica X-Y el movimiento del órgano terminal para apreciar que tan
aproximado es el movimiento a la línea recta.
Observando así una trayectoria recta en los tres planos presentados.