control dinámico por torque computado de un robot de dos grados de libertad
Post on 02-Dec-2015
412 Views
Preview:
TRANSCRIPT
2012-B Robótica Control Dinámico Robot 2DOF - RP
1
“Control Dinámico por Torque Computado de un
ROBOT 2DOF – RP”
Cuya Solari, Omar Antonio
oacs_198@hotmail.com
Flores Bustinza, Edwing Irwing
irwing_1988_@hotmail.com
Torres Chavez, Jonathan Emmanuel
jonathan260605@hotmail.com
2012-B Robótica Control Dinámico Robot 2DOF - RP
2
I.- Obtención del Modelo Dinámico del Robot RP mediante la formulación
Lagrange –Euler
La complejidad computacional de este algoritmo depende de la cantidad de
grados de libertad que posea el robot a trabajar O(n4), por lo cual, ya que nuestro
robot solo posee dos grados de libertad es factible realizar este procedimiento
para el modelado dinámico cuyas ecuaciones finales quedarán bien estructuradas
para obtener los pares y fuerzas respectivos para su movimiento.
A continuación detallaremos paso a paso el proceso con su respectivo código en el
programa Matlab. Para trabajar primero definimos nuestros parámetros a tratar,
tanto las fijas como las variables mediante el comando “syms” e ingresamos
nuestra tabla de parámetros D-H. Nuestros puntos serán posición de
coordenadas (q1 y q2), velocidades (dq1 y dq2) y aceleraciones (ddq1 y ddq2).
Luego nuestros fijos son longitud del primer eslabón y desplazamiento (d2) para
el movimiento prismático del segundo eslabón (cabe resaltar que el segundo
eslabón se desplaza desde una distancia inicial 0.5 m y seguido de un
movimiento longitudinal definido por q2).
disp('DINAMICA DE UN ROBOT 2DOF USANDO EL METODO DE LAGRANGE - EULER') disp('--------------------------------------------------------------') syms q1 q2 dq1 dq2 ddq1 ddq2; syms l1 d2; %longitud de los eslabones syms g; %parametro de gravedad [m/seg^2] syms m1 m2 x1 x2 y1 y2 z1 z2 %------------------------------------------------------------ disp('1.- Tabla de Parametros DH') disp('-a---alpha---d---theta---') DH=[ 0 -pi/2 0 q1 0 0 q2 0 ]
1.1.- Paso 1
Se asigna a cada eslabón un sistema de referencia de acuerdo a las normas D-H.
Figura N°1.- Prototipo a trabajar robot RP
2012-B Robótica Control Dinámico Robot 2DOF - RP
3
1.2.- Paso 2
Obtenemos las matrices de transformación para cada elemento i. Para esto
estaremos usando la función “denavit” para obtener nuestras matrices deseadas,
también vamos a usar el comando “simplify” para obtener resultados de manera
simbólica, es decir, en función de nuestras variables.
disp('2.- Calculo de las matrices de transformacion: Aij') disp('-------------------------------------------------------------') %------------------------------------------------------------ %T=rotz(theta)*transl(0,0,d)*transl(a,0,0)*rotx(alpha) disp('Matriz de TH del primer eslabon movil desde la base del robot') disp('-------------------------------------------------------------') A01=simple(simplify(denavit(DH(1,1),DH(1,2),DH(1,3),DH(1,4)))) A12=simple(simplify(denavit(DH(2,1),DH(2,2),DH(2,3),DH(2,4))));
%Matriz de transformacion de la base al efector final disp('--------------------------------------------------------------') disp('Matriz de TH del efector final desde la base del robot') disp('-------------------------------------------------------------') A02=simple(simplify(A01*A12))
A01 = [ cos(q1), 0, -sin(q1), 0] [ sin(q1), 0, cos(q1), 0] [ 0, -1, 0, 0] [ 0, 0, 0, 1] -------------------------------------------------------------- Matriz de TH del efector final desde la base del robot ------------------------------------------------------------- A02 = [ cos(q1), 0, -sin(q1), -q2*sin(q1)] [ sin(q1), 0, cos(q1), q2*cos(q1)] [ 0, -1, 0, 0] [ 0, 0, 0, 1]
1.3.- Paso 3
Obtenemos las matrices definidas por:
2012-B Robótica Control Dinámico Robot 2DOF - RP
4
Es decir obtenemos las derivadas parciales de nuestras matrices de
transformación homogénea respecto a las base solidaria
con referencia a
nuestras variables de posición q1 y q2, ahora usaremos el comando “diff” el cual
nos permite derivar elementos respecto a un punto de referencia; seguidamente
de comando simplify para trabajar en modo simbólico.
disp('3.- Calculo de las matrices : Uij') disp('-------------------------------------------------------------') %------------------------------------------------------------ U11=simple(simplify(diff(A01,q1))) U12=simple(simplify(diff(A01,q2))) U21=simple(simplify(diff(A02,q1))) U22=simple(simplify(diff(A02,q2))) %------------------------------------------------------------
U11 = [ -sin(q1), 0, -cos(q1), 0] [ cos(q1), 0, -sin(q1), 0] [ 0, 0, 0, 0] [ 0, 0, 0, 0] U12 = [ 0, 0, 0, 0] [ 0, 0, 0, 0] [ 0, 0, 0, 0] [ 0, 0, 0, 0] U21 = [ -sin(q1), 0, -cos(q1), -q2*cos(q1)] [ cos(q1), 0, -sin(q1), -q2*sin(q1)] [ 0, 0, 0, 0] [ 0, 0, 0, 0] U22 = [ 0, 0, 0, -sin(q1)] [ 0, 0, 0, cos(q1)] [ 0, 0, 0, 0] [ 0, 0, 0, 0]
1.4.- Paso 4
Obtenemos las matrices definidas por:
2012-B Robótica Control Dinámico Robot 2DOF - RP
5
Mediante este proceso obtenemos la derivada parcial de respecto a los
parámetros de posición q1 y q2, usamos el comando diff para las derivadas,
simplify para obtener ecuaciones en modo simbólico y simple para reducir a
forma concreta.
disp('4.- Calculo de las matrices : Uijk') disp('-------------------------------------------------------------') %------------------------------------------------------------ U111=simple(simplify(diff(U11,q1))) U112=simple(simplify(diff(U11,q2))) U121=simple(simplify(diff(U12,q1))) U122=simple(simplify(diff(U12,q2)))
U211=simple(simplify(diff(U21,q1))) U212=simple(simplify(diff(U21,q2))) U221=simple(simplify(diff(U22,q1))) U222=simple(simplify(diff(U22,q2))) %------------------------------------------------------------
U111 = [ -cos(q1), 0, sin(q1), 0] [ -sin(q1), 0, -cos(q1), 0] [ 0, 0, 0, 0] [ 0, 0, 0, 0] U112 = [ 0, 0, 0, 0] [ 0, 0, 0, 0] [ 0, 0, 0, 0] [ 0, 0, 0, 0] U121 = [ 0, 0, 0, 0] [ 0, 0, 0, 0] [ 0, 0, 0, 0] [ 0, 0, 0, 0] U122 = [ 0, 0, 0, 0] [ 0, 0, 0, 0] [ 0, 0, 0, 0] [ 0, 0, 0, 0] U211 = [ -cos(q1), 0, sin(q1), q2*sin(q1)] [ -sin(q1), 0, -cos(q1), -q2*cos(q1)] [ 0, 0, 0, 0] [ 0, 0, 0, 0]
2012-B Robótica Control Dinámico Robot 2DOF - RP
6
U212 = [ 0, 0, 0, -cos(q1)] [ 0, 0, 0, -sin(q1)] [ 0, 0, 0, 0] [ 0, 0, 0, 0] U221 = [ 0, 0, 0, -cos(q1)] [ 0, 0, 0, -sin(q1)] [ 0, 0, 0, 0] [ 0, 0, 0, 0] U222 = [ 0, 0, 0, 0] [ 0, 0, 0, 0] [ 0, 0, 0, 0] [ 0, 0, 0, 0]
1.5.- Paso 5
Obtenemos las matrices de pseudoinercia para cada elemento, que viene
definidas por:
{
∫
∫
∫ ∫
∫
∫
∫
∫
∫ ∫
∫ ∫
∫
∫
∫
∫ }
Donde las integrales están extendidas al elemento i considerado, y ( ) son las
coordenadas del diferencial de masa dm respecto al sistema de coordenadas del
elemento.
Figura 2
2012-B Robótica Control Dinámico Robot 2DOF - RP
7
Para este paso vamos a utilizar los valores que nos da nuestro diagrama respecto
a los frames, por lo cual para el primer eslabón y respetando su centro de masa y
gravedad hacia el primer frame tendremos:
x1=0 y1=0 z1=L1=0.5 m1=1.4 Kg Análogamente para el segundo eslabón: x2=0 y2=0;
z2=0; m2=1 Kg Ahora estos valores los remplazamos en la matriz de pseudoinercias definido de manera correcta en el matlab. disp('5.- Matrices de Pseudoinercia : Ji') disp('-------------------------------------------------------------') %------------------------------------------------------------ %Para el primer eslabon: J1[Kg.m^2] %m1=1.4Kg %Coordenadas del centro de gravedad 1 respecto al frame 1 %x1=0;y1=0;z1=l1=0.5m; disp('Para el Primer Eslabon') disp('-------------------------------------------------------------') J1=[ 0 0 0 0 0 0 0 0 0 0 (m1*l1^2) (m1*l1) 0 0 (m1*l1) m1 ] %------------------------------------------------------------ %Para el primer eslabon: J1[Kg.m^2] %m2=1Kg %Coordenadas del centro de gravedad 2 respecto al frame 2 %x2=0;y2=0;z2=0; disp('-------------------------------------------------------------') disp('Para el segundo eslabon') disp('-------------------------------------------------------------') J2=[ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 m2] %------------------------------------------------------------------------
J1 = [ 0, 0, 0, 0] [ 0, 0, 0, 0] [ 0, 0, l1^2*m1, l1*m1] [ 0, 0, l1*m1, m1] -------------------------------------------------------------
2012-B Robótica Control Dinámico Robot 2DOF - RP
8
Para el segundo eslabon ------------------------------------------------------------- J2 = [ 0, 0, 0, 0] [ 0, 0, 0, 0] [ 0, 0, 0, 0] [ 0, 0, 0, m2]
1.6.- Paso 6
Obtenemos la matriz de inercias D=[ ] cuyos elementos vienen definidos por:
∑
Con: i,j= 1,2 n=2 (dos grados de libertad)
Para esta etapa haremos uso del comando “trace” el cual realizara la traza de la
matriz correspondiente, lo manipularemos en modo simbolico con “simplify” y
haremos la respectiva suma de las matrices que cumplen con cada posición de D.
disp('6.- Calculo de las matrices de inercia : D = [dij]') disp('-------------------------------------------------------------') %------------------------------------------------------------------------
-- %MATRIZ D11 d11=simple(simplify(trace(U11*J1*U11.')))+simple(simplify(trace(U21*J2*U2
1.'))) %MATRIZ D12 d12=simple(simplify(trace(U22*J2*U21.'))) %MATRIZ D21 d21=simple(simplify(trace(U21*J2*U22.'))) %MATRIZ D22 d22=simple(simplify(trace(U22*J2*U22.'))) %Matriz general D = [d11 d12;d21 d22]
d11 = m1*l1^2 + m2*q2^2 d12 =0 d21 =0 d22 =m2 D = [ m1*l1^2 + m2*q2^2, 0] [ 0, m2]
2012-B Robótica Control Dinámico Robot 2DOF - RP
9
1.7.- Paso 7
Obtener los términos definidos por:
∑
Con i,k,m=1,2
De esta etapa obtendremos ocho valores pues plantearemos a la matriz previa
definida puntos respecto a una nueva referencia k m. Usando los mismos
comandos trace, simplify la adición.
disp('7.- Calculo del vector hikm') disp('-------------------------------------------------------------') %------------------------------------------------------------------------
-- h111=simplify(trace(U111*J1*U11.'))+simplify(trace(U211*J2*U21.')) h112=simplify(trace(U212*J2*U21.')) h121=simplify(trace(U221*J2*U21.')) h122=simplify(trace(U222*J2*U21.'))
h211=simplify(trace(U211*J2*U22.')) h212=simplify(trace(U212*J2*U22.')) h221=simplify(trace(U221*J2*U22.')) h222=simplify(trace(U222*J2*U22.')) %------------------------------------------------------------------------
h111 =0 h112 =m2*q2 h121 =m2*q2 h122 =0 h211 =-m2*q2 h212 =0 h221 =0 h222 =0
1.8.- Paso 8
Obtener la matriz columna de fuerza de Coriolis y centrípeta H=[ ] cuyos
elementos vienen definidos por:
∑ ∑
2012-B Robótica Control Dinámico Robot 2DOF - RP
10
Para esta fase definimos por cálculo las operaciones que nos conlleva a obtener
nuestra matriz de coriolis, utilizando la ultima matriz obtenida con nuestros
valores de posición derivadas.
disp('8.-Calculo del vector columna de fuerza centrifuga y de
coriolis:H=[hi]') disp('-------------------------------------------------------------------
----') %------------------------------------------------------------------------
-- h1=h111*dq1^2+h112*dq1*dq2+h121*dq2*dq1+h122*dq2^2; h2=h211*dq1^2+h212*dq1*dq2+h221*dq2*dq1+h222*dq2^2; %------------------------------------------------------------------------
-- %Matriz de Coriolis H=[h1;h2] %--------------------------------------------------------------------
H = 2*dq1*dq2*m2*q2 -dq1^2*m2*q2
1.9.- Paso 9
Obtener la matriz columna de fuerzas de gravedad C=[ ] cuyos elementos están
definidos por:
∑
Con: i=1,2 g = gravedad
La estructura horizontal de nuestro robot permiten que las fuerzas de gravedad
se proyecten sobre los apoyos, por lo cual la matriz de gravedad C se ve afectada
debido a que el vector gravedad g expresado en el sistema de referencia de la base
{S0} es g = {g 0 0 0}
2012-B Robótica Control Dinámico Robot 2DOF - RP
11
Figura 3
disp('9.-Calculo de la matriz columna de fuerzas de gravedad: C=[ci]') disp('--------------------------------------------------------------') %------------------------------------------------------------------------
-- g1=[g 0 0 0]; % Segun el frame 0 , gravedad actuando en el eje X %------------------------------------------------------------------------
-- % Vector de compuesto por coordenadas de posicion del centro de masas % para cada eslabon r11=[0;0;l1;1]; %CG para el eslabon 1 r22=[0;0;0;1]; %CG para el eslabon 2
c1=-m1*g1*U11*r11 - m2*g1*U21*r22; c2=-m1*g1*U12*r11 - m2*g1*U22*r22; %------------------------------------------------------------------------
-- % Matriz de gravedad C=[c1;c2] %---------------------------------------------------------------------
C = g*l1*m1*cos(q1) + g*m2*q2*cos(q1) g*m2*sin(q1)
1.10.- Paso 10
Finalmente tenemos la ecuación dinámica del sistema
Donde es el vector de fuerzas y pares motores efectivos aplicados sobre cada
coordenada qi.
2012-B Robótica Control Dinámico Robot 2DOF - RP
12
Donde el vector gravedad sería:
[
]
disp('10.- Ecuacion dinamica del robot') disp('-------------------------------------------------------------') %------------------------------------------------------------------------
-- tau=simplify(D*[ddq1;ddq2]+H+C); disp('El torque o par para la primera articulacion revoluta es: ') disp('-------------------------------------------------------------') T1 = tau(1) % Juntura Revoluta disp('-------------------------------------------------------------') disp('La fuerza para al articulacion prismatica es:') disp('-------------------------------------------------------------') F2 = tau(2) % Juntura Prismatica disp('-------------------------------------------------------------') disp('La notacion general de la ecuacion dinamica es:') disp('-------------------------------------------------------------') tau=[tau(1); tau(2)]
T1 = ddq1*(m1*l1^2 + m2*q2^2) + 2*dq1*dq2*m2*q2 + g*l1*m1*cos(q1) + g*m2*q2*cos(q1) ------------------------------------------------------------- La fuerza para al articulación prismática es: ------------------------------------------------------------- F2 = m2*(- q2*dq1^2 + ddq2 + g*sin(q1)) -------------------------------------------------------------
La notación general de la ecuación dinámica es: ------------------------------------------------------------- tau = [ddq1*(m1*l1^2 + m2*q2^2) + 2*dq1*dq2*m2*q2 + g*l1*m1*cos(q1) + g*m2*q2*cos(q1) m2*(- q2*dq1^2 + ddq2 + g*sin(q1))]
2012-B Robótica Control Dinámico Robot 2DOF - RP
13
II.- Prueba de generación de trayectoria
Para elaborar la trayectoria del robot RP necesitamos de las variables de posición
tanto inicial como final (q0 y qf), además de ello vamos a trabajar con dos
funciones adjuntas al código principal que son “cinversa” y “plot RP”, además
utilizaremos el comando jtraj para que nos revuelva la velocidad y aceleración de
la trayectoria con dq y ddq respectivamente.
2.1.- Función cinversa
Toma tus coordenadas de posición q y tu espacio de trayectoria ok, se inicializan
estos parámetros para nuestro robot y se defines las constantes de medida L1 y
d2.
Como nuestro robot es un RP puede crear diversas dimensiones de radio en su
espacio de desplazamiento y rotación, es así que le involucramos parámetros de
radio R para los ejes x e y. Ahora como tenemos al eslabón 2 que toma un inicio
de 0.5m creamos una condicional que si este eslabón es menor a esa medida
retorne y corrija la falla. Nuestros valores de posición q van a actualizarse a cada
paso de nuestra trayectoria usando el comando “atan2” entre x e y para la
rotación y radio total con la dimensión del eslabón 1, cuando la trayectoria es
correcta ok=1 queda definido como proceso correcto y te arrojan los puntos q
siguientes hasta fin del trayecto.
function [q ok]=cinversa(x,y) ok=0; q=[0 0]'; L1=0.5;d2=0.5; R=sqrt(x^2+y^2); if R<0.5 return end q(1)=atan2(y,x); q(2)=R - L1; ok=1; q=[q(1) q(2)]; end
2.2.- Función plotRP
Trabaja con los valores de posición q y los valores fijos de dimensión de los
eslabones, en esta función también se realizan actualización de q, fijando puntos
iniciales a cero, ahora en el ploteo cada eslabón quedará definido con una
función específica en función a la rotación q1, L1 y el enlongamiento q2.
function plotRP(q) L1=0.5; d2=0.5; q1=q(1); q2=q(2); x(1)=0;
2012-B Robótica Control Dinámico Robot 2DOF - RP
14
y(1)=0; % Eslabon 1 x(2)=L1*cos(q1); y(2)=L1*sin(q1); % Eslabon 2 x(3)=(L1+q2)*cos(q1); y(3)=(L1+q2)*sin(q1); hold on; plot(x,y,'m','linewidth',1); plot(x,y,'k.','MarkerSize',20); grid on; axis([-1 4 -1 4]); hold off; end
2.3.- Algoritmo Principal
Aquí ingresando puntos iniciales y finales nos arroja una trayectoria guiada por
un número de puntos N, utilizando por defecto un polinomio de grado 7 con
condiciones limite cero para velocidad y aceleración.
Tomamos puntos iniciales y finales, aplicamos función cinversa a tales puntos y
creamos una condicional de espacio de trayectoria para no salir del rango fijado,
si las condiciones son favorables el trayecto se da sin mayor problema y comienza
a plotear cada uno de los N pares de puntos establecidos.
clear all; close all; clc % Coordenadas cartesiana inicial x0=1; y0=0; % Coordenadas cartesiana final xf=0; yf=3; [q0 ok0]=cinversa(x0,y0); % Calculo de Coordenadas articulares iniciales [qf okf]=cinversa(xf,yf); % Calculo de Coordenada articulares finales if ok0*okf==1 clf; hold on; plot([x0 xf],[y0 yf],'b','LineWidth',2); %Plotea la recta que debe
seguir la trayectoria a interpolar con el jtraj plot([x0 xf],[y0 yf],'r.','MarkerSize',20); title('\bf Prueba de Trayectoria 1 para Manipulador RP') xlabel('\bf x') ylabel('\bf y') grid on; axis([-1 4 -1 4]) hold off;
Con N=50 número de puntos a seguir en la trayectoria invocamos el comando
“jtraj” cuyo papel es devolvernos las evoluciones de velocidad y aceleración para
este proceso en el tiempo así como también la trayectoria en q para cada
2012-B Robótica Control Dinámico Robot 2DOF - RP
15
articulación del robot, creamos un bucle de iteraciones de 1 a N en el cual se va
plotear nuestra actualización de puntos.
N=50; % Numero de puntos [q dq ddq]=jtraj(q0,qf,N);% nos devuelve los valores de la posición,
velocidad y aceleración respectivamente. for k=1:N plotRP(q(k,:)); pause(0.1); end figure, subplot(311), plot(q),title('\bf Posicion Angular (rad)') legend('q_1(R)','q_2(P)'),grid subplot(312), plot(dq),title('\bf Velocidad Angular (rad/seg)') legend('dq_1(R)','dq_2(P)'),grid subplot(313), plot(ddq),title('\bf Aceleracion Angular (rad/seg^2)') legend('ddq_1(R)','ddq_2(P)'),grid else display('Fuera del campo de acción'); end
A continuación se mostrará los diagramas obtenidos mediante el uso de Matlab
para nuestro robot RP:
Figura N°2.- Patrón de trayectoria estimado por el jtraj de HEMERO
Del diagrama de trayectoria, la revoluta es constante para q1 que barre un
ángulo de 90 grados y el desplazamiento de d2 es variable en el espacio de
trabajo hasta llegar al punto final.
2012-B Robótica Control Dinámico Robot 2DOF - RP
16
Figura N°3.- Diagrama donde podemos ver las actualizaciones de posición (q),
velocidad (dq) y aceleración (ddq) para la trayectoria definida en el algoritmo
principal.
Tenemos también otro algoritmo para obtener la trayectoria del robot, pero en
este caso usamos el linspace para el ploteo de inicio a final.
El comando jtraj arrojara trayectoria para los "q, q' y q''" cada dos puntos
consecutivos, ya que se realizara interpolación con N1 puntos entre dos puntos
contenidos consecutivamente en la recta general. Ya no con el inicio y fin de ésta
como en la prueba anterior
close all; clear all; clc; % Coordenadas cartesiana inicial x0=1; y0=-0.5; % Coordenadas cartesiana final xf=0; yf=3; [q0 ok0]=cinversa(x0,y0); % Calculo de Coordenadas articulares iniciales [qf okf]=cinversa(xf,yf); % Calculo de Coordenada articulares finales if ok0*okf==1 clf; hold on; plot([x0 xf],[y0 yf],'b','LineWidth',2); plot([x0 xf],[y0 yf],'r.','MarkerSize',20); title('\bf Prueba de Trayectoria 2 para Manipulador RP') xlabel('\bf x') ylabel('\bf y') axis square axis([-1 4 -1 4]); hold off; N=10; % Cantidad de parejas de puntos a intepolar x=linspace(x0,xf,N); y=linspace(y0,yf,N); % Para interpolacion con N1 puntos entre dos puntos N1=3;
2012-B Robótica Control Dinámico Robot 2DOF - RP
17
%Creando almacenadores de datos Q=zeros(N1,N); dQ=zeros(N1,N); ddQ=zeros(N1,N); for k=2:N [q dq ddq]=jtraj(cinversa(x(k-1),y(k-
1)),cinversa(x(k),y(k)),N1); Q(:,k-1:k)=[q(:,1),q(:,2)]; dQ(:,k-1:k)=[dq(:,1),dq(:,2)]; ddQ(:,k-1:k)=[ddq(:,1),ddq(:,2)]; for j=1:length(q) plotRP(q(j,:)); % Dibuja la evolucion de los eslabones en
XY pause(0.1); end
end end
Figura N°4.- Diagrama de trayectoria para dos puntos consecutivos definidos en la
línea recta de inicio a final.
2012-B Robótica Control Dinámico Robot 2DOF - RP
18
III.- Modelo Dinámica Directa ROBOT RP
El modelo dinámico de un robot tiene por objetivo conocer la relación entre el
movimiento del robot y las fuerzas implicadas en el mismo. El modelo dinámico
directo o dinámica directa expresa la evolución temporal de las coordenadas
articulares del robot en función de las fuerzas y pares que intervienen.
La parte inicial del informe muestra el modelamiento de nuestro Robot RP de 2
grados de libertad en función de Lagrange – Euler. Una vez obtenido este
modelamiento vamos a realizar una simulación de la dinámica. Para ello
emplearemos el software Matlab junto a las herramientas de Simulink que nos
ofrece.
Es de nuestro conocimiento que el robot al ser del tipo Revoluta – Prisma
expresará la evolución de sus coordenadas articulares en función de Torque y
Fuerza. Por tal motivo simularemos el comportamiento de ambos mediantes
señales sinusoidales. Estas señales ingresarán a la dinámica directa del robot y
permitirán observar como ya se dijo la evolución temporal de las coordenadas
articulares del Robot. La idea fundamental de esta simulación es observar esta
evolución.
El esquema general a implementar es el siguiente:
Figura N° 5.- Esquema General de Dinámica Directa del Robot RP
El torque y la fuerza tendrán comportamiento de señales sinusoidales, las
configuraciones respectivas son las siguientes:
2012-B Robótica Control Dinámico Robot 2DOF - RP
19
Figura N° 6.- Configuración de señales sinusoidales para torque y fuerza
El torque se comportará como una señal sinusoidal de amplitud 8 y de frecuencia
2: . La fuerza se comportará como una señal sinusoidal de amplitud 5 y
de frecuencia 1:
Figura N° 7.- Comportamiento del torque y fuerza del robot RP
2012-B Robótica Control Dinámico Robot 2DOF - RP
20
3.1.- La dinámica directa de nuestro robot será el reflejo de las ecuaciones de
fuerza y torque obtenidas en la etapa de modelamiento:
………………………………………………..(1)
Donde la matriz de Masas es:
[
] ………………………………….(2)
La matriz de Coriolis es:
[
]……………………………………………..(3)
Y la matriz de Gravedad es:
[
]…………………………………..(4)
Resultando finalmente:
[(
)
]
(
) …………(5)
…………………………………………………..(6)
3.2.- Recordando el concepto de la dinámica directa donde obtendremos la
evolución de las coordenadas articulares a partir de la fuerza o torque,
despejamos la ecuación general y tenemos:
……………………………………………………………….(7)
Obtenemos entonces la aceleración del motor respectivo. Si empleamos
integradores podemos obtener el comportamiento de la velocidad y posición de
cada articulación.
2012-B Robótica Control Dinámico Robot 2DOF - RP
21
De acuerdo a esto elaboraremos el esquema general de la dinámica directa:
Figura N° 8.- Dinámica directa del Robot RP
Según lo anterior:
“u” es la señal de torque o fuerza.
“invD” es una función matemática que describe la matriz inversa de la
matriz de masas del robot.
“Coriolis” y “gravedad” son las matrices de respectivas del robot.
“invD*R” es función de multiplicación para obtener
Se emplean integradores para obtener no solo la aceleración del sistema,
sino también de la velocidad y posición.
3.3.- Describiremos cada función matemática empleada:
a) Coriolis: la matriz de coriolis se debe al movimiento relativo existente entre los
diversos elementos, así como las fuerzas centrípetas que dependen de la
configuración instantánea del manipulador.
De acuerdo al análisis realizado mediante Lagrange-Euler:
[
]
2012-B Robótica Control Dinámico Robot 2DOF - RP
22
A partir de ello creamos una función matemática en Matlab para emplearla en
nuestro Simulink.
function Cdq=coriolis(v) m2=1; d2=0.5; dq=[v(1);v(2)]; q=[v(3);v(4)]; c11=2*m2*q(2)*dq(2); c12=0; c21=-q(2)*m2*dq(1); c22=0; C=[c11 c12;c21 c22]; Cdq=C*dq;
b) Gravedad: esta matriz es la que se origina por la fuerza de gravedad.
Crearemos también una función para emplearla en el Simulink
[
]
function g=gravedad(q) m1=1.4; m2=1; L1=0.5; g=9.81; g1=m1*g*L1*cos(q(1)) + m2*g*q(2)*cos(q(1)); g2=m2*g*sin(q(1)); g=[g1;g2];
c) invD: esta es la matriz inversa de la matriz de Masas del sistema. Creamos
también esta función de masas en Matlab y luego hallamos su matriz inversa:
[
]
function iD=invD(q) m1=1.4; m2=1; L1=0.5; D11=(L1^2)*m1 + m2*(q(2))^2; D12=0; D21=0; D22=m2; D=[D11 D12;D21 D22]; iD=inv(D);
d) invD*R: este bloque representa la multiplicación de matrices para obtener la
aceleración de las articulaciones:
2012-B Robótica Control Dinámico Robot 2DOF - RP
23
Una vez implementadas las funciones y el esquema en Simulink obtenemos los
siguientes resultados:
Figura N° 9.- Evolución temporal de las coordenadas articulares de cada articulación
empleando simulink
La imagen anterior representa la evolución temporal de las posiciones,
velocidades y aceleraciones de cada articulación. Las gráficas de color rojo
representan las coordenadas articulares de la primera articulación, es decir
provenientes del torque de la revoluta. Las gráficas de color azul representan las
provenientes de la fuerza del prisma.
Si observamos el Simulink en la parte inferior existe un bloque llamado salidas.
Este bloque permite almacenar los valores obtenidos y poder emplearlos en todo
Matlab. Para realizar una contrastación de los resultados obtenidos realizamos
una pequeña programación en el Editor.
clear all; close all; clc
tf=10; ang1=30; q1=pi*ang1/180; q2=0.5; t=sim('Din_robot_2dof',tf); % Tiempo de simulación al código Simulink Q=zeros(length(t),6); % Arreglo de 10x6, almacena los datos q, dq y ddq
for i=1:6 Q(:,i)=salidas(:,i);%almacenamos q1 y d2 en fila 1 y 2, dq1 y dd2
en fila 3 y 4 ... end
2012-B Robótica Control Dinámico Robot 2DOF - RP
24
%Ploteos
subplot(311),plot(t,Q(:,1),t,Q(:,2)),xlabel('tiempo'),ylabel('q_1 &
d_2'),legend('q_1','d_2'),grid subplot(312),plot(t,Q(:,3),t,Q(:,4)),xlabel('tiempo'),ylabel('dq_1 & dd
d_2'),legend('dq_1','d d_2'),grid subplot(313),plot(t,Q(:,5),t,Q(:,6)),xlabel('tiempo'),ylabel('ddq_1 & dd
d_2'),legend('ddq_1','dd d_2'),grid
Los parámetros iniciales son q1 = 30°, es decir la revoluta con un ángulo inicial, y
q2= 0.5, que es la posición inicial del prisma.
El arreglo “Q” de 6 columnas y tantas filas como muestras se tomen en los 10
segundos de simulación. Las filas representan: q1, q2, dq1, dq2, ddq1, ddq2.
Los resultados obtenidos son los siguientes:
Figura N° 10.- Evolución temporal de las coordenadas articulares de cada
articulación empleando el Editor de Matlab
0 1 2 3 4 5 6 7 8 9 10-1000
0
1000
tiempo
q1 &
d2
q1
d2
0 1 2 3 4 5 6 7 8 9 10-200
0
200
tiempo
dq
1 &
dd d
2
dq1
d d2
0 1 2 3 4 5 6 7 8 9 10-50
0
50
tiempo
ddq
1 &
dd d
2
ddq1
dd d2
2012-B Robótica Control Dinámico Robot 2DOF - RP
25
3.4.- Alternativa en código puro Matlab para Dinámica Directa
Así como empleamos Simulink para realizar la dinámica directa, podemos realizar
este proceso implementando un código en el editor de Matlab.
3.4.a.- En la primera parte del código mostramos las condiciones iniciales del
sistema:
clear all; close all; clc % Condiciones iniciales T=0.01; % Tiempo de muestreo q=[pi/6 0]'; % ángulo inicial y posición inicial dq=[0 0]'; % velocidades iniciales ddq=[0 0]'; % aceleraciones iniciales plotRP(q); % dibujo del robot en su posición inicial
3.4.b.- Podemos evaluar varias situaciones de nuestro robot. Las cuales se
muestran a continuación:
% Casos del torque Torq=[0 0]'; %Sin torque de entrada y solo interactuando con la gravedad %Torq=[8 0]'; %Torque < gravedad y sin fuerza al motor 2 %Torq=[0 5]'; %Sin torque al primer motor , solo fuerza al prisma %Torq=[10 5]';%Insertamos torque mayor que al fuerza de gravedad
N=100; % número de muestras Q=zeros(2,N); % arreglo para almacenar posiciones dQ=zeros(2,N); % arreglo para almacenar velocidades ddQ=zeros(2,N); % arreglo para almacenar aceleraciones
3.4.c.- Realizamos la implementación de la dinámica directa recordando el mismo
criterio que las fórmulas empleadas en el simulink:
Debemos indicar antes que para realizar las integraciones respectivas
recordaremos lo siguiente:
2012-B Robótica Control Dinámico Robot 2DOF - RP
26
for k=1:N ddq=Minv(q)*(Torq - G(q(1),q(2))- C(dq(1),dq(2),q(2))); dq=dq+ddq*T; q=q+dq*T; Q(:,k)=[q(1);q(2)]; dQ(:,k)=[dq(1);dq(2)]; ddQ(:,k)=[ddq(1);ddq(2)]; plotRP(q); axis([-5 2 -1 2]) pause(0.01); end
3.4.d.- Finalmente realizaremos los ploteos de la evolución de las coordenadas
articulares en el tiempo:
figure subplot(311),plot(Q(1,:),'r'),hold,plot(Q(2,:),'b'),grid legend('\bf q_1(R)','\bf q_2(P)'),xlabel('\bf t'),ylabel('\bf q(rad)') title('\bf Evolucion Temporal de las Articulaciones') subplot(312),plot(dQ(1,:),'r'),hold,plot(dQ(2,:),'b'),grid legend('\bf dq_1','\bf dq_2'),xlabel('\bf t'),ylabel('\bf dq(rad/seg)') subplot(313),plot(ddQ(1,:),'r'),hold,plot(ddQ(2,:),'b'),grid legend('\bf ddq_1','\bf ddq_2'),xlabel('\bf t'),ylabel('\bf
ddq(rad/seg^2)')
Una vez implementada la programación. Obtenemos los siguientes resultados:
Figura N° 11.- Comportamiento del Robot con Torque: T= [0 0]
-5 -4 -3 -2 -1 0 1 2-1
-0.5
0
0.5
1
1.5
2
0 10 20 30 40 50 60 70 80 90 100-5
0
5
t
q(r
ad
)
Evolucion Temporal de las Articulaciones
q1(R)
q2(P)
0 10 20 30 40 50 60 70 80 90 100-5
0
5
t
dq
(rad
/seg
)
dq1
dq2
0 10 20 30 40 50 60 70 80 90 100-20
0
20
t
dd
q(r
ad
/seg
2)
ddq1
ddq2
2012-B Robótica Control Dinámico Robot 2DOF - RP
27
Figura N° 12.- Comportamiento del Robot con Torque: T= [8 0]
Figura N°13.- Comportamiento del Robot con Torque: T= [0 5]
-5 -4 -3 -2 -1 0 1 2-1
-0.5
0
0.5
1
1.5
2
0 10 20 30 40 50 60 70 80 90 100-10
0
10
t
q(r
ad
)
Evolucion Temporal de las Articulaciones
q1(R)
q2(P)
0 10 20 30 40 50 60 70 80 90 100-20
0
20
t
dq
(rad
/seg
)
dq1
dq2
0 10 20 30 40 50 60 70 80 90 100-20
0
20
t
dd
q(r
ad
/seg
2)
ddq1
ddq2
-5 -4 -3 -2 -1 0 1 2-1
-0.5
0
0.5
1
1.5
2
0 10 20 30 40 50 60 70 80 90 100-10
0
10
t
q(r
ad
)
Evolucion Temporal de las Articulaciones
q1(R)
q2(P)
0 10 20 30 40 50 60 70 80 90 100-20
0
20
t
dq
(rad
/seg
)
dq1
dq2
0 10 20 30 40 50 60 70 80 90 100-50
0
50
t
dd
q(r
ad
/seg
2)
ddq1
ddq2
2012-B Robótica Control Dinámico Robot 2DOF - RP
28
Figura N° 14.- Comportamiento del Robot con Torque: T= [10 5]
Debemos recordar que simplemente se trata del comportamiento de nuestro robot
frente a determinados torques o fuerzas a las que se le está sometiendo. No está
siguiendo ninguna trayectoria en particular.
IV.- Modelo Dinámica Inversa del robot RP
La dinámica inversa expresa las fuerzas y pares que intervienen en función de la
evolución de las coordenadas articulares y sus derivadas. Tal y como lo dice su
nombre realiza el proceso inverso de la dinámica directa, es decir a partir de las
coordenadas articulares halla las fuerzas o pares de nuestras articulaciones. La
aplicación que tiene es que permite seleccionar actuadores, lo que implica la
elección de los motores más adecuados de acuerdo a la exigencia de nuestro
proyecto. Por tal motivo aplicando la dinámica inversa podremos conocer que
motores debemos elegir para implementar nuestro robot.
En nuestro Robot RP, hallaremos la dinámica inversa de empleando nuevamente
simulink. Para esto describiremos el comportamiento de las posiciones,
velocidades y aceleraciones de acuerdo a las siguientes fórmulas:
q1: 2 Sen(u)
q2: 0.5 Cos(u)
dq1: 2 Cos(u)
dq2: -0.5 Sen(u)
ddq1: -2 Sen(u)
ddq2: -0.5 Cos(u)
-5 -4 -3 -2 -1 0 1 2-1
-0.5
0
0.5
1
1.5
2
0 10 20 30 40 50 60 70 80 90 100-5
0
5
t
q(r
ad
)
Evolucion Temporal de las Articulaciones
q1(R)
q2(P)
0 10 20 30 40 50 60 70 80 90 100-20
0
20
t
dq
(rad
/seg
)
dq1
dq2
0 10 20 30 40 50 60 70 80 90 100-100
0
100
t
dd
q(r
ad
/seg
2)
ddq1
ddq2
2012-B Robótica Control Dinámico Robot 2DOF - RP
29
Estos comportamientos ingresarán a nuestra dinámica inversa que arrojaran las
curvas de torque y fuerza deseado para el robot, teniendo en cuenta las perdidas
por fricciones por coriolis y gravedad. La estructura general es la siguiente:
Figura N° 15.- Esquema general de la dinámica inversa
El proceso de dinámica inversa involucra conocer las matrices de Masa, Coriolis y
gravedad. Para ello nuevamente creamos funciones en Matlab para luego
emplearlas en simulink:
a) Masas:
function M=Masas(q) m1=1.4; m2=1; L1=0.5; M11=(L1^2)*m1 + m2*(q(2))^2; M12=0; M21=0; M22=m2; M=[M11 M12;M21 M22];
b) Coriolis:
function Cdq=coriolis(v) m2=1; d2=0.5; dq=[v(1);v(2)]; q=[v(3);v(4)]; c11=2*m2*q(2)*dq(2); c12=0; c21=-q(2)*m2*dq(1); c22=0; C=[c11 c12;c21 c22];
2012-B Robótica Control Dinámico Robot 2DOF - RP
30
Cdq=C*dq;
c) Gravedad:
function g=gravedad(q) m1=1.4; m2=1; L1=0.5; g=9.81; g1=m1*g*L1*cos(q(1)) + m2*g*q(2)*cos(q(1)); g2=m2*g*sin(q(1)); g=[g1;g2];
Estas funciones emplean las mismas matrices obtenidas en el modelamiento
dinámico realizado mediante el método de Lagrange-Euler. En este caso el torque
y fuerza se basan en la siguiente fórmula:
……………………...(8)
Entonces la dinámica inversa sigue el siguiente esquema:
Figura N°16.- Dinámica inversa del robot
Tal y como se mencionó la evolución de nuestras articulaciones ingresan a
nuestra dinámica inversa y de acuerdo a la fórmula antes mencionada, ingresan
a nuestras funciones de Masas, Coriolis y Gravedad para finalmente obtener el
comportamiento del torque y fuerza de nuestros motores.
A su vez en nuestro esquema general observamos que existen dos ganancias, las
cuales provienen de los reductores de ambos motores.
2012-B Robótica Control Dinámico Robot 2DOF - RP
31
Finalmente observamos el comportamiento del torque y fuerza requeridos por
nuestros motores a elegir para nuestro robot:
Figura N°17.- Comportamiento en el tiempo del torque de la revoluta del robot
Figura N°18.- Comportamiento en el tiempo de la fuerza aplicada al prisma del robot
Debemos recordar nuevamente que estos comportamientos se deben a la
evolución que hemos dado de las posiciones, velocidades y aceleraciones de
nuestro robot de acuerdo a funciones sinusoidales. Además debemos aclarar
nuevamente que aún no estamos dando una trayectoria específica a nuestro
robot.
2012-B Robótica Control Dinámico Robot 2DOF - RP
32
V.- Implementación del Control Dinámico por Torque Computado en
LABVIEW CODIGO PURO.
Teniendo en cuenta las pautas trabajadas en los códigos Matlab y Simulink para
la dinámica directa, inversa y generación de trayectorias para cada articulación
de este robot, nos disponemos a corroborar y concluir la experiencia en el
software Labview en programación grafica y utilizando SubVI’s por cuestión de
orden y síntesis para el código. Este código nos traerá la ventaja de poder
interactuar con la performance del control propuesto para nuestro robot, así
como verificar el control de trayectoria en “q” deseada en el Plano XY.
Figura N°19.- Panel Frontal del código principal
5.1.- Contenido de cada Sub Instrumento Virtual (SubVi)
Para implementar el diagrama de bloques del sistema de control por torque
computado del robot RP en Labview se tendrán en cuenta los siguientes SubVI’s y
sectores que representan:
Trayectorias Deseadas.
Algoritmo de control Torque Computado.
Dinámica Inversa.
Dinámica Directa.
Condiciones Iniciales.
Ecuaciones recursivas.
2012-B Robótica Control Dinámico Robot 2DOF - RP
33
Ploteo de resultados.
Ploteo RP.
Seguidamente se tiene el panel de Bloques donde se han consignado cada punto
a explicar en el código:
Figura N° 20.- Diagrama de Bloques del código principal
5.1.a.- Trayectorias Deseadas
En este primer sector del código, se ha creado un Subvi llamado generador el
cual tiene la tarea de entregar seis funciones que describan trayectorias
deseadas, es decir, espacio en “q” o evolución temporal de “q”. Esto lo hemos
logrado con el uso de una estructura Case que tiene en cada caso las funciones
(que para esta ocasión hemos colocado senoidales y cosenoidales) para cada
articulación así como para las velocidades y aceleraciones deseadas:
2012-B Robótica Control Dinámico Robot 2DOF - RP
34
Figura N°21.- Panel Frontal y de bloques
Detallando el SubVi “generador”, contiene una lógica para entregar el valor del
tiempo según un paso determinado a la función trigonométrica. Multiplicamos el
valor del paso con la iteración del bucle de control para este propósito, la función
como ya la dijimos es una función trigonométrica que es invocada desde un Case
que contiene los seis casos para trayectorias deseadas:
Figura N° 22.- Panel Frontal SubVi Generador
q1= 2*sin(t)
q2=2*cos(t)
dq1=2*cos(t)
dq2=-2*sin(t)
ddq1=-2*sin(t)
ddq2=-2*cos(t)
2012-B Robótica Control Dinámico Robot 2DOF - RP
35
Por último cada caso se invocara con una variable tipo String o cadena de texto
desde el panel principal del código, aquí el panel de bloques del SubVi:
Figura N° 23.- Código planteado
5.1.b.- Algoritmo de Control Torque Computado
En este sector del código, explicaremos el algoritmo Torque Computado empleado
para el control dinámico de este robot. Este SubVi recibe varias señales, entre
ellas:
Figura N°24.- Panel Frontal
2012-B Robótica Control Dinámico Robot 2DOF - RP
36
1. Primero las trayectorias deseadas de q1d, q2d, dq1d, dq2d, ddq1d y ddq2d,
además de las trayectorias obtenidas por el robot q, q’ y q’’ las cuales las utiliza
para generar las señales de error respectivas según el diagrama de bloques de la
figura.
2. Segundo, recibe el diseño de las matrices de ganancias Kp y Kd, es decir recibe
el valor del coeficiente de amortiguamiento y la frecuencia natural, que para este
caso comprenden criterios importantes según la teoría de control:
El control en Robótica se basa en básicamente un movimiento
críticamente amortiguado, es decir una respuesta deseada que reduzca
el error lo más rápido posible a cero. Por ello el valor del coeficiente de
amortiguamiento es 1.
Luego la frecuencia natural, siendo un valor netamente experimental, es
recomendable que sea baja, a partir de los 10 rad/seg. Por ello se ha
colocado un control desde este valor mínimo.
Estos criterios nos darán como resultados dos matrices diagonales
correspondientes a Kp y Kd, decimos matrices porque estas ganancias se
multiplicaran a cada señal de error para cada articulación haciendo que el
control sea más específico y con un tracking del error más robusto para
cada eslabón:
Por último, los valores de los elementos de cada matriz diagonal no tienen
porque ser iguales necesariamente, esto tiene que ver con que los motores
empleados para cada eslabón sean homogéneos entre sí, es decir iguales
en parámetros y hardware. Para nuestro caso estamos considerando
motores homogéneos.
2012-B Robótica Control Dinámico Robot 2DOF - RP
37
Figura N°25.- Algoritmo de control en código puro Labview
Todos los criterios explicados se han plasmado en este SubVi. Con estas matrices
de ganancias, el algoritmo pasara a multiplicar cada una a su respectiva matriz
de error (e y e’), a su vez el error producido en las trayectorias de aceleración se
suma a la señal compensatoria para generar la ley de control a emplear en este
algoritmo:
Siendo el torque necesario para compensar los torques por perdidas de fricción
de coriolis y gravedad según topología dinámica del robot ( ) que probablemente
le impedían al robot seguir un patrón de trayectoria deseado para sus
articulaciones desacopladas. El resultado de esta ley de control ingresara como
una aceleración mejorada al modelo dinámico inverso del robot.
2012-B Robótica Control Dinámico Robot 2DOF - RP
38
5.1.c.- Modelo Dinámico Inverso
El propósito de este sector del código es procesar el tau_r proveniente del control
e insertarlo como aceleración mejorada al modelo, luego recalcula dos torques: el
primero correspondiente a las friciones (tau_c) que siempre están y se oponen al
movimiento (coriolis y gravedad), el segundo es el torque de control completo
(tau_m). Esta última señal será la que ingrese a nuestro brazo robótico del cual
se obtendrán datos importantes del siguiente sector del código a explicar. Esta
dinámica Inversa, como ya se explico anteriormente, calcula las fuerzas y pares
que necesita el robot para determinadas evoluciones temporales o trayectorias
deseadas. Se rige bajo la siguiente notación:
Figura N°26.- Código para la ecuación de la dinámica inversa
Esta demás decir que para que este código se defina necesita los SubVi’s de las
matrices de inercias, Coriolis y Gravedad. Cada una de ellas contiene los
parámetros iniciales de las coordenadas de los centros de gravedad según el
enunciado del problema (según las ecuaciones 2, 3 y 4):
[
]
[
]
[
]
2012-B Robótica Control Dinámico Robot 2DOF - RP
39
Figura N°27.- Códigos para cada matriz del modelo dinámico
5.1.d.- Modelo Dinámico Directo
En este sector del código se ingresara el torque de control al robot para obtener
las evoluciones temporales de las articulaciones que deben seguir a la trayectoria
deseada puesto que aquí ingresan valores de torque que ya vienen compensados
de los producidos por las pérdidas de coriolis y gravedad por el algoritmo de
control. La ecuación representada en este código es:
2012-B Robótica Control Dinámico Robot 2DOF - RP
40
Figura N°28.- Dinámica Directa
Los resultados de esta dinámica se realimentan al control y a la dinámica Inversa para
ser recalculados según los cambios en la trayectoria deseada.
5.1.e.- Ecuaciones recursivas para q, q’, y q’’
Realizamos la implementación de las ecuaciones recursivas recordando el mismo
criterio que las fórmulas empleadas en el código Matlab de la dinámica directa:
Figura N°29.- Ecuaciones recursivas implementadas en Labview
2012-B Robótica Control Dinámico Robot 2DOF - RP
41
Para ello nos basamos en la definición matemática de la derivada:
Esta definición nos hace deducir que emplearemos la herramienta de Labview
llamada Shift Register que muestra el comportamiento actual y pasado de los
datos. Teniendo en cuenta ello, implementaremos las siguientes ecuaciones:
5.1.f.- Condiciones iniciales
Las condiciones iniciales consideradas para este programa son:
Para la articulación revoluta q1: 30°
Para la articulación prismática q2: 0.5
Para las velocidades: dq1= 0 , dq2=0
Estos datos ingresaran como condición inicial a los registros de desplazamiento
de cada derivada y q respectivamente, produciendo una evaluación instantánea
en la primera iteración del bucle.
5.1.g.- Ploteo de resultados
En esta parte mostramos los resultados de la perfomance del control:
visualizaremos el seguimiento de las trayectorias deseadas versus las trayectorias
obtenidas del control y la dinámica del robot para las articulaciones en
posiciones, velocidad y aceleración, las cuales llegan a ser iguales. Además se
visualizará el tracking o seguimiento del error cuya convergencia a cero es rápida,
y finalmente el ploteo RP donde nuestro robot traducirá los valores para cada
articulación a puntos o coordenadas en el plano XY.
Figura N°30.- Agrupando y ploteando los datos finales
2012-B Robótica Control Dinámico Robot 2DOF - RP
42
5.1.h.- Plot RP
Este SubVi trabaja con las posiciones de nuestro robot obtenidas para “q” y las
plotea en el plano XY. Para esto insertamos los parámetros iniciales fijos L1 = d2
= 0.5m que representan las distancias de los centros de gravedad de los
eslabones respecto de su frame actual y anterior respectivamente. Con esto
veremos las posiciones en nuestro robot en el plano XY ante el control y
trayectoria deseada.
Se inicializa con parámetros de posición “0” para x(1) y y(1) que representa el
origen de coordenadas que en 3D seria la base del robot que se extiende en el eje
“Z”, la cual, coincide con el frame solidario y el frame del primer eslabón.
Seguidamente para el primer eslabón que va realizar movimiento de rotación
trabajamos en función a L1 (distancia de origen del frame base hacia el
centro de masa del primer eslabón) y el ángulo girado q1 que definirán el
punto x(2) y y(2). Para el segundo eslabón tomaremos en cuenta L1, q2
(distancias y juntura del segundo eslabón) y el ángulo barrido por q1 que
definirán el punto x(3) y y(3). Todo esto se ha realizado basándonos en el frame
solidario.
Figura N°31.- Diagrama de bloques “plot RP”
2012-B Robótica Control Dinámico Robot 2DOF - RP
43
Podemos apreciar los procesos para obtener x(2),y(2) y x(3),y(3), comprobados en
Matlab:
q1=q(1); q2=q(2); x(1)=0; y(1)=0; % Eslabon 1 x(2)=L1*cos(q1); y(2)=L1*sin(q1); % Eslabon 2 x(3)=(L1+q2)*cos(q1); y(3)=(L1+q2)*sin(q1);
Ahora unimos cada coordenada mediante un bloque Merge Signal y obtenemos
los puntos de ploteo para x e y; para el labview le anexamos puntos finales xf e yf,
la grafica del eje de coordenadas y finalmente lo visualizamos haciendo uso del
Build XY Graph.
A su vez colocamos un retardo de 5ms en mostrar los cambios, con fines visuales
y unos puntos fijos para insertar un eje de referencia o eje solidario en el origen
de coordenadas. A continuación algunas pruebas previas:
Figura N°32.- Posición A
2012-B Robótica Control Dinámico Robot 2DOF - RP
44
Figura N°33.- Posición B
5.2.- Simulaciones y performance de control
Al simular el sistema implementado para este control dinámico por Torque
computado, verificamos que las trayectorias deseadas son seguidas de forma
adecuada y con la rapidez que demanda el diseño del controlador. A su vez
podemos visualizar el dibujo en el plano XY del Robot que en los primeros
instantes a condiciones iniciales se encuentra en una posición base que luego es
adecuada a la trayectoria insertada. También se puede revisar el tracking o
seguimiento del error, éste resulta ser instantáneo de acuerdo al diseño
establecido.
Figura N°34.- Simulación al inicio
top related