Dinámica Robot
Dinámica Robot
Dinámica Robot
Ingeniera en Mecatrónica
Robótica
Reporte 4: dinámica
Alumno:
Eduardo Arriaga Fernández
Profesor:
Dr. Fernando Reyes Cortes
21 de noviembre de 2019
24 de mayo de 2019
Resumen:
Competencias generales
Realizar y comprender los pasos a seguir para desarrollar el control de
posición de un robot de 2 GDL.
Realizar en MATLAB las simulaciones solicitadas, ocupando los códigos
vistos en clase y agregando el control de posición requerido.
Desarrollo de habilidades
Para realizar esta práctica del curso de robótica se necesita ocupar todas las
habilidades que fueron desarrolladas durante el transcurso del periodo ya que este
reporte involucra todos los temas antes vistos:
Hacer uso de los preliminares matemáticos para poder encontrar el punto de
equilibrio del control y demostrar su estabilidad.
Desarrollar y entender los códigos de MATLAB vistos en clase.
Analizar e interpretar los resultados obtenidos en la simulación y desarrollo
matemático.
1) Introducción
Control de robots manipuladores es un tema de control automático vigente y de gran
interés para la comunidad científica de robótica debido a los retos teóricos y
prácticos que involucra él diseño de nuevas estrategias de control con alto
desempeño y exactitud en aplicaciones industriales tales como estibado de cajas,
ensamble, traslado, pintado de objetos, etc. El diseño de nuevos esquemas de
control implica mejorar sustancialmente el desempeño˜no de algoritmos de control
tradicionales.
̃) − 𝑓𝑣 (𝐾𝑣 , 𝒒̇ ) + 𝑔(𝒒)
𝜏 = ∇𝑈𝑎 (𝐾𝑝 , 𝒒
Donde:
∇𝑈𝑎 (𝐾𝑝 , 𝑞̃) > 0 ; ∇𝑈𝑎 𝒯 (𝐾𝑝 , 𝑞̃)𝑞̃ > 0 es la energía potencial artificial.
𝐾𝑝 ∈ ℝ𝑛x𝑛 es la ganancia proporcional, matriz diagonal definida positiva.
𝐾𝑣 ∈ ℝ𝑛x𝑛 es la ganancia derivativa, matriz definida positiva.
𝑓𝑣 (𝐾𝑣 , 𝑞̇ ) es una función disipativa: 𝑞̇ 𝒯 𝑓𝑣 (𝐾𝑣 , 𝑞̇ ) > 0.
1
̃) = 𝒒̇ 𝒯 𝑀(𝒒)𝒒̇ + 𝛻𝑈𝑎 (𝐾𝑝 , 𝑞̃) ⇒ 𝑉̇ (𝒒̇ , 𝒒
𝑉(𝒒̇ , 𝒒 ̃) < 0
2
−𝒒̇
𝑑 𝒒̃ 2 ̃
1 − 𝛼𝑒 −𝛼𝑐𝑜𝑠ℎ (𝒒 ) 0
[ ]= ̇ = [0]
2 ̃ ] 𝑐𝑜𝑠ℎ 𝒒 𝑠𝑒𝑛ℎ 𝒒 − 𝐾𝑣 𝑠𝑒𝑛ℎ 𝒒 − 𝐶(𝒒, 𝒒)𝒒 − 𝐵𝒒]
𝑑𝑡 𝒒̇ 𝑀(𝑞)−1 [𝐾𝑝 [ (̃) (̃) ( ̇) ̇ ̇
2
[ 𝑠𝑒𝑛ℎ (𝒒̃ ) + 𝑒−𝛼𝑐𝑜𝑠ℎ (𝒒) ]
1
𝑉(𝑞̇ , 𝑞̃) = 𝑞̇ 𝑇 𝑀(𝑞)𝑞̇ + 𝒰𝑎 (𝐾𝑝 , 𝑞
̃)
2
𝑀(𝒒)
0
[ 2 ]=𝐴
𝐾𝑝
0
2
1. 𝐴 = 𝐴𝑇
𝑀(𝒒)
2. 𝑎11 = >0
2
𝑀(𝒒)𝐾𝑝
3. 𝑑𝑒𝑡|𝐴| = 4
>0
𝑑 𝑇 𝑑
Recordando 𝑑𝑡
𝒙 𝐴𝒙 = 𝟐𝒙𝑇 𝐴 𝑑𝑡 𝒙 entonces.
𝑇
𝑑 2 𝑑 2
𝒰𝑎 (𝐾𝑝 , 𝑞̃) = 2√ln(𝑠𝑒𝑛ℎ2 (𝒒̃ ) + 𝑒−𝛼𝑐𝑜𝑠ℎ (𝒒̃ ) ) + 1 𝐾𝑝 √ln(𝑠𝑒𝑛ℎ2 (𝒒̃ ) + 𝑒−𝛼𝑐𝑜𝑠ℎ (𝒒̃ ) ) + 1
𝑑𝑡 𝑑𝑡
𝑇
𝑞̃̇ (1 − 𝛼𝑒−𝛼𝑐𝑜𝑠ℎ (𝒒̃) )𝑐𝑜𝑠ℎ(𝒒̃ )𝑠𝑒𝑛ℎ(𝒒̃ )
2
2 2 1
̃ ) + 𝑒 −𝛼𝑐𝑜𝑠ℎ (𝒒̃) ) + 1 𝐾𝑝
= √𝑙𝑛(𝑠𝑒𝑛ℎ2 (𝒒 ∙ 2
2 2 𝑠𝑒𝑛ℎ2 (𝒒̃ ) + 𝑒 −𝛼𝑐𝑜𝑠ℎ (𝒒̃)
̃) + 𝑒 −𝛼𝑐𝑜𝑠ℎ (𝒒̃) ) + 1
√𝑙𝑛(𝑠𝑒𝑛ℎ2 (𝒒
[ ]
2 (𝒒
̃)
Se puede notar que puede eliminarse el valor de √ln(𝑠𝑒𝑛ℎ2 (𝒒
̃) + 𝑒 −𝛼𝑐𝑜𝑠ℎ ) + 1 al
desarrollar la notación de la siguiente forma.
2 (𝒒
= [√ln(𝑠𝑒𝑛ℎ2 (𝒒
̃𝟏 ) + 𝑒 −𝛼𝑐𝑜𝑠ℎ ̃ 𝟏) ̃𝒏 ) + 𝑒 −𝛼𝑐𝑜𝑠ℎ2 (𝒒̃𝒏) ) + 1] ∙
) + 1 ⋯ √ln(𝑠𝑒𝑛ℎ2 (𝒒
2
1 (1 − 𝛼𝑒 −𝛼𝑐𝑜𝑠ℎ (𝒒̃𝟏 ))𝑐𝑜𝑠ℎ(𝒒
̃ 𝟏 )𝑠𝑒𝑛ℎ(𝒒̃𝟏 )
∙ ⋯ 0
𝑠𝑒𝑛ℎ2 (𝒒̃𝟏 ) + 𝑒 −𝛼𝑐𝑜𝑠ℎ2 (𝒒̃𝟏 )
̃ 𝟏 ) + 𝑒 −𝛼𝑐𝑜𝑠ℎ2 (𝒒̃𝟏 )) + 1
√ln(𝑠𝑒𝑛ℎ2 (𝒒
⋮ ⋱ ⋮ ̃̇
𝒒 𝐾𝑝
2
1 (1 − 𝛼𝑒 −𝛼𝑐𝑜𝑠ℎ (𝒒̃𝒏) )𝑐𝑜𝑠ℎ(𝒒
̃𝒏 )𝑠𝑒𝑛ℎ(𝒒
̃𝒏)
0 ⋯ ∙ 2
𝑠𝑒𝑛ℎ2 (𝒒̃𝒏 ) + 𝑒 −𝛼𝑐𝑜𝑠ℎ (𝒒̃𝒏)
̃ 𝒏 ) + 𝑒 −𝛼𝑐𝑜𝑠ℎ2 (𝒒̃𝒏) ) + 1
√ln(𝑠𝑒𝑛ℎ2 (𝒒
[ ]
El valor de la derivada de la energía potencial es:
2 𝑇
𝑑 (1 − 𝛼𝑒−𝛼𝑐𝑜𝑠ℎ (𝒒̃ ) )𝑐𝑜𝑠ℎ(𝒒
̃ )𝑠𝑒𝑛ℎ(𝒒̃ )
̃) = [
𝒰𝑎 (𝐾𝑝 , 𝒒 2 ] 𝒒̃ ̇ 𝐾𝑝
𝑑𝑡 ̃ ) + 𝑒−𝛼𝑐𝑜𝑠ℎ (𝒒̃ )
𝑠𝑒𝑛ℎ2 (𝒒
2 𝑇
1 (1−𝛼𝑒 −𝛼𝑐𝑜𝑠ℎ (𝑞̃) )𝑐𝑜𝑠ℎ(𝑞̃)𝑠𝑒𝑛ℎ(𝑞̃)
Entonces 𝑉̇ (𝑞̇ , 𝑞̃) = 𝑞̇ 𝑇 𝑀(𝑞)𝑞̈ + 2 𝑞̇ 𝑇 𝑀(𝑞)𝑞̇ + [ 2 ] 𝑞̃̇ 𝐾𝑝
𝑠𝑒𝑛ℎ2 (𝑞̃)+𝑒 −𝛼𝑐𝑜𝑠ℎ (𝑞̃)
El elemento [𝒒̇ 𝑇 𝐶(𝒒, 𝒒̇ )𝒒̇ + 12 𝒒̇ 𝑇 𝑀(𝒒)𝒒̇ ] puede apreciarse que por la propiedad
2 ̃)
1−𝛼𝑒 −𝛼𝑐𝑜𝑠ℎ (𝒒
antisimétrica es 0 además el elemento 𝒒̇ 𝑇 𝐾𝑝 [ 2 ̃) ] se elimina con
̃)+𝑒 −𝛼𝑐𝑜𝑠ℎ (𝒒
𝑠𝑒𝑛ℎ2 (𝒒
2 ̃)
(1−𝛼𝑒 −𝛼𝑐𝑜𝑠ℎ (𝒒 ̃)𝑠𝑒𝑛ℎ(𝒒
)𝑐𝑜𝑠ℎ(𝐪 ̃)
[ 2 ̃)
̃)+𝑒 −𝛼𝑐𝑜𝑠ℎ (𝒒
𝑠𝑒𝑛ℎ2 (𝒒
] 𝒒̇ 𝐾𝑝 , recordando la propiedad 𝒙𝑻 𝒚 = 𝒚𝑻 𝒙.
̃ = 𝒒𝒅 − 𝒒̇ si la velocidad es 0 entonces 𝒒
Tomando en cuenta que 𝒒 ̃ = 𝒒𝒅 por lo tanto
se puede escribir.
2 2
(1−𝛼𝑒 −𝛼𝑐𝑜𝑠ℎ (𝒒𝒅 ) )𝑐𝑜𝑠ℎ(𝒒𝒅)𝑠𝑒𝑛ℎ(𝒒𝒅 ) (1−𝛼𝑒 −𝛼𝑐𝑜𝑠ℎ (𝒒𝒅 ) )𝑐𝑜𝑠ℎ(𝒒
̃)𝑠𝑒𝑛ℎ(𝒒𝒅 )
𝜏 𝑚𝑎𝑥 = 𝐾𝑝 2 ; 2 = 𝑓𝑞𝑑
𝑠𝑒𝑛ℎ2 (𝒒𝒅 )+𝑒 −𝛼𝑐𝑜𝑠ℎ (𝒒𝒅 ) 𝑠𝑒𝑛ℎ2 (𝒒𝒅 )+𝑒 −𝛼𝑐𝑜𝑠ℎ (𝒒𝒅 )
𝜏𝑚𝑎𝑥
Despejando: 𝐾𝑝 = 0.8 ; 0.8 se agrega para no ocupar el valor máximo y así
𝑓𝑞𝑑
no saturar los servomotores.
Además de esto es necesario notar que 𝑞𝑑 se compone de 𝑞𝑑1 𝑦 𝑞𝑑2.
Por lo tanto:
𝜏𝑚𝑎𝑥 𝜏𝑚𝑎𝑥
𝑘𝑝1 = 0.8 𝑓1 𝑦 𝑘𝑝2 = 0.8 𝑓2
𝑞𝑑1 𝑞𝑑2
La ganancia 𝑘𝑣 se define como 𝑘𝑣1 = 𝛽𝑘𝑝1 y 𝑘𝑣2 = 𝛽𝑘𝑝2 donde 𝛽 está acotado
por 0 < 𝛽 ≤ 1 tambien se debe mencionar que el valor de 𝛼 cumple con lo mismo
0 < 𝛼 ≤ 1.
Se hizo uso de MATLAB para encontrar los valores de las ganancias, dicho código
se encuentra anexado en el apéndice.
𝛽1 = 0.3 y 𝛽2 = 0.3
𝛼 = 0.1
Una vez hecho esto se realiza la simulación en MATLAB del algoritmo de control
con el modelo dinámico numérico del robot (que se encuentra en el planteamiento
y descripción del problema). prototipo de 2 gdl, para las posiciones deseadas 𝑞𝑑1 =
45° y 𝑞𝑑2 = 90°. Se consideró un tiempo de simulación 𝑑𝑒 𝑡 = 0 𝑎 5 segundos y un
período de muestreo ℎ = 2.5 𝑚𝑠𝑒𝑔. Los códigos ocupados se encuentran en el
apéndice.
Por último se debe trazar una flor con 4 pétalos de radio 0.3 m y con centro en 𝑥 =
0: 3𝑚; 𝑦 = −0: 3𝑚. Tiempo de simulación de 𝑡 = 0 𝑎 30 𝑠𝑒𝑔𝑢𝑛𝑑𝑜𝑠 y un período de
muestreo ℎ = 2.5 𝑚𝑠𝑒𝑔.
Para esto se hizo uso de la cinemática inversa para calcular los valores de 𝒒𝒅 ,
después se ocupa la cinemática directa para obtener la trayectoria deseada. Las
ganancias ocupadas fueron las mismas para las ganancias 𝐾𝑝 y para las ganancias
𝐾𝑣 se necesitó un pequeño ajuste ya que la figura de la flor no se realizaba de una
manera correcta.
Conclusión
Realizar todo el proceso de un algoritmo de control en un robot no es una tarea
sencilla puesto que se debe tener conocimientos de la cinemática y la dinámica del
robot. Es importante antes de simular el control, demostrar la estabilidad del punto
de equilibrio de la ecuación en lazo cerrado, ya que sin esto no se podría demostrar
que el control propuesto servirá de una manera correcta, además, es necesario
sintonizar las ganancias para que los servomotores no se saturen; una vez hecho
esto en la simulación es esencial ajustar la ganancia derivativa para lograr que en
las articulaciones del robot exista el menor error posible y así llegue a la posición
deseada, para lograr que se trace una figura con el extremo final del robot, se debe
hacer uso de la cinemática inversa ya que es la herramienta adecuada para poder
trazar la trayectoria de la figura puesto que calcula punto por punto la trayectoria
deseada, si se conoce la ecuación.
Referencias bibliográficas
1. Reyes Cortés Fernando, MATLAB: Aplicado a robótica y mecatrónica, Alfaomega,
México, 2012.
2. Dr. Fernando Reyes Cortés, “Capítulo 3: Dinamica” 2019
Apéndice:
Sintonía de ganancias:
alpha=0.1;
qd1=45*pi/180; %radianes
qd2=90*pi/180;
Tmax1=150;
Tmax2=15;
c1=exp(-(alpha*(cosh(qd1)).^2));
c2=exp(-(alpha*(cosh(qd2)).^2));
fqd1=((1-alpha*c1)./(((sinh(qd1)).^2)+c1)).*(sinh(qd1).*cosh(qd1));
fqd2=((1-alpha*c2)./(((sinh(qd2)).^2)+c1)).*(sinh(qd2).*cosh(qd2));
kp1=0.8*Tmax1/fqd1
kp2=0.8*Tmax2/fqd2
function xp =robot2gdl(t,x)
q1=x(1);
q2=x(2);
q = [q1;
q2];
%vector de posición articular
qp1=x(3);
qp2=x(4);
qp = [qp1;
qp2];%vector de velocidad articular
M=[2.351+0.1676*cos(q2), 0.102+0.0838*cos(q2);
0.102+0.0838*cos(q2), 0.102];
%matriz de fuerzas centrípetas y de Coriolis
C=[-0.1676*sin(q2)*qp2, -0.0838*sin(q2)*qp2;
0.084*sin(q2)*qp1, 0.0];
%par gravitacional
gq=9.81*[3.9211*sin(q1)+0.1862*sin(q1+q2);
0.1862*sin(q1+q2)];
%fricción viscosa
fr=[2.288*qp1;
0.175*qp2];
[~, tau]=alg_control(t, q,qp);%en esta línea se inserta el controlador
qpp = M^(-1)*(tau-C*qp-gq-fr);
%vector de salida
xp = [qp1; qp2; qpp(1); qpp(2); tau(1); tau(2)];
end
Función del algoritmo de control.
clc;
clear all;
close all;
format short g
ti=0; h=0.0025; tf =5;
t=ti:h:tf;%vector tiempo
ci=[0; 0; 0; 0; 0 ;0];%condiciones iniciales
opciones=odeset('RelTol',1e-6,'InitialStep', h,'MaxStep',h);
%solución numérica del sistema dinámico del robot de 2gdl
[t, x]=ode45('robot2gdl',t,ci,opciones);
q1=x(:,1); q2=x(:,2);
qp1=x(:,3); qp2=x(:,4);
tauf1=x(:,5); tauf2=x(:,6);
tau1=fliplr(tauf1); tau2=fliplr(tauf2);
qt1=45*pi/180-q1;
qt2=90*pi/180-q2;
function xp =robot2FLORgdl(t,x)
q1=x(1);
q2=x(2);
q = [q1;
q2];
%vector de posición articular
qp1=x(3);
qp2=x(4);
qp = [qp1;
qp2];%vector de velocidad articular
M=[2.351+0.1676*cos(q2), 0.102+0.0838*cos(q2);
0.102+0.0838*cos(q2), 0.102];
%matriz de fuerzas centrípetas y de Coriolis
C=[-0.1676*sin(q2)*qp2, -0.0838*sin(q2)*qp2;
0.084*sin(q2)*qp1, 0.0];
%par gravitacional
gq=9.81*[3.9211*sin(q1)+0.1862*sin(q1+q2);
0.1862*sin(q1+q2)];
%fricción viscosa
fr=[2.288*qp1;
0.175*qp2];
[~, tau]=alg_controlflor4p(t, q,qp);%en esta línea se inserta el controlador
MODIFICAR ESTE
qpp = M^(-1)*(tau-C*qp-gq-fr);
%vector de salida
xp = [qp1; qp2; qpp(1); qpp(2)];
end
clc;
clear all;
close all;
format short g
ti=0; h=0.0025; tf =30;
t=ti:h:tf;%vector tiempo
ci=[0; 0; 0; 0];%condiciones iniciales
opciones=odeset('RelTol',1e-6,'InitialStep', h,'MaxStep',h);
%solución numérica del sistema dinámico del robot de 2gdl
[t, x]=ode45('robot2FLORgdl',t,ci,opciones);
q1=x(:,1); q2=x(:,2);
qp1=x(:,3); qp2=x(:,4);
%cinemática directa asociada al cuarto cuadrante
l1=0.45; l2=0.45; beta1=0.15; beta2=0.15;
[n,m]=size(t);
x0=zeros(n,1);
y0=zeros(n,1);
[x0, y0, ~]=cinematica_r2gdl(beta1,l1,q1,beta2,l2,q2);%coordenas cartesianas del
extremo final del robot de 2 gdl
plot(x0,y0)