• Author / Uploaded
• Chego Pinto

Citation preview

UNIVERSIDAD CATOLICA DE SANTA MARIA ESCUELA PROFESIONAL DE INGENIERIA MECANICA, MECANICA ELECTRICA Y MECATRONICA

CURSO: ROBOTICA I INGENIERO: JUAN CARLOS CUADROS MACHUCA GRUPO DE PRACTICAS: 01 TEMA: MODELO DINAMICO – ALGORITMOS COMPUTACIONALES ALUMNOS:   

CUPI MANGO, MARJORIE GEORGETHE LUQUE PUMA, MARCO ANTONIO PINTO MUÑOZ, DIEGO FRANCO

AREQUIPA PERU 2019

INDICE 1.

INTRODUCCION............................................................................................................... 3 1.1

2.

OBJETIVOS ................................................................................................................ 3

MARCO TEORICO ........................................................................................................... 3 2.1

MODELO DINAMICO .............................................................................................. 3

2.2

ALGORITMO DE LAGRANGE .............................................................................. 3

2.3

ALGORITMO DE NEWTON ................................................................................... 4

3.

MATERIAL Y EQUIPO .................................................................................................... 4

4.

PROCEDIMIENTO ............................................................................................................ 5 4.1

4.1.1

RESOLUCIÓN ANALITICA ..............................Error! Bookmark not defined.

4.1.2

RESOLUCION - MATLAB ............................................................................... 6

4.2

PROBLEMA 2 ........................................................................................................... 10

4.2.1

RESOLUCIÓN ANALITICA .......................................................................... 10

4.2.2

RESOLUCION - MATLAB ............................................................................. 10

4.3 5.

PROBLEMA 1 ............................................................................................................. 5

PROBLEMA 3 ........................................................................................................... 10

CONCLUSIONES Y OBSERVACIONES ..................................................................... 18

1. INTRODUCCION 1.1 OBJETIVOS  Obtener el modelo dinámico de un robot aplicando las formulaciones de NewtonEuler y Lagrange.  Obtener el modelo dinámico de un robot aplicando los algoritmos computacionales de Lagrange y Newton.  Resolver problemas de dinámica del robot aplicando los conceptos de modelo dinámico y los algoritmos computacionales de Lagrange y Newton, empleando en la solución herramientas matemáticas y de software. 2. MARCO TEORICO 2.1 MODELO DINAMICO La dinámica se ocupa de la relación entre las fuerzas que actúan sobre un cuerpo y el movimiento que en él se origina. Por tanto, 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. Esta relación se obtiene mediante el denominado modelo dinámico, que establece la relación matemática entre: 1. La localización del robot definida por sus variables articulares o por las coordenadas de localización de su extremo, y sus derivadas: velocidad y aceleración. 2. Las fuerzas y pares aplicados en las articulaciones (o en el extremo del robot). 3. Los parámetros dimensionales del robot, como longitud, masas e inercias de sus elementos El problema de la obtención del modelo dinámico de un robot es, por tanto, uno de los aspectos más complejos de la robótica, lo que ha llevado a ser obviado en numerosas ocasiones. Sin embargo, el modelo dinámico es imprescindible para conseguir los siguientes fines: 1. Simulación del movimiento del robot. 2. Diseño y evaluación de la estructura mecánica del robot. 3. Dimensionamiento de los actuadores. 4. Diseño y evaluación del control dinámico del robot. 2.1.1 FORMULACIONES DE NEWTON EULER Y LAGRANGE FORMULACION NEWTON EULER Uicker en 1965 [UICKER-65] [UICKER-64] utilizó la representación de DenavitHartenberg basada en las matrices de transformación homogénea, para formular el modelo dinámico de un robot mediante la ecuación de Lagrange. Este planteamiento utiliza, por tanto, las matrices i–1Ai que relacionan el sistema de coordenadas de referencia del elemento i con el del elemento i_1. Se realizan en este caso operaciones de producto y suma innecesarias (recuérdese la información redundante contenida en las matrices i–1Ai debido a la ortonormalidad de la submatriz de rotación), tratándose por ello de un procedimiento ineficiente desde el punto de vista computacional. Puede comprobarse que el algoritmo es de un orden de complejidad computacional O(n4), es decir, el número de operaciones a realizar crece con la potencia 4 del número de grados de libertad. Sin embargo, conduce a unas ecuaciones finales bien estructuradas donde aparecen de manera clara los diversos pares y fuerzas que intervienen en el movimiento (inercia, Coriolis, gravedad). 𝐿 = 𝐸𝑐 − 𝐸𝑝

𝜏𝑖 =

𝑑 𝜕𝐿 𝜕𝐿 − 𝑑𝑡 𝜕𝑞𝑖̇ 𝜕𝑞𝑖

Con: 𝐿: Función Lagrangiana. 𝐸𝑐 : Energía cinética. 𝐸𝑝 : Energía potencial. 𝑞𝑖 : Coordenadas generalizadas (en este caso las articulares). 𝜏𝑖 : Fuerza o pares aplicado sobre el grado de libertad qi. FORMULACION DE LAGRANGE La obtención del modelo dinámico de un mecanismo, y en particular de un robot, se basa fundamentalmente en el planteamiento del equilibrio de fuerzas establecido en la segunda ley de Newton, o su equivalente para movimientos de rotación, la denominada ley de Euler: 𝑑 ∑ 𝐹 = (𝑚𝑣) 𝑑𝑡 𝑑 ∑ 𝑇 = (𝑰𝝎) = 𝑰𝝎̇ + 𝝎 × (𝑰𝝎) 𝑑𝑡 𝝉 = 𝑫𝒒̈ + 𝑯 + 𝑪 2.1.2 ALGORITMO DE LAGRANGE L 1. Asignar a cada eslabón un sistema de referencia de acuerdo a las normas de DH. L 2. Obtener las matrices de transformación 00𝐴1 para cada elemento i. L 3. Obtener las matrices Uij definidas por: L 4. Obtener las matrices Uijk definidas por: L 5. Obtener las matrices de pseudoinercias Ji para cada elemento, que vienen definidas por: L 6. Obtener la matriz de inercias D L 7. Obtener los términos Hikm l. 8. Obtener la matriz columna de fuerza Coriolis y centrípeta H L 9. Obtener la matriz columna de fuerzas de gravedad C L 10. Ecuación dinámica del sistema 2.1.3

ALGORITMO DE NEWTON

N-E-1. Asignar a cada eslabón un sistema de referencia de acuerdo con las normas de D-H. N-E 2. Establecer las condiciones iniciales. N-E-3. Obtener las matrices de rotación y sus inversas N-E-4. Obtener la velocidad angular del sistema N-E-5. Obtener la aceleración angular del sistema N-E-6. Obtener la aceleración lineal del sistema N-E-7. Obtener la aceleración lineal del centro de gravedad del eslabón i N-E-8. Obtener la fuerza ejercida sobre el eslabón i N-E-9. Obtener el par ejercido sobre el eslabón i N-E-10. Obtener la fuerza o par aplicado a la articulación i 3. MATERIAL Y EQUIPO Una PC con SO Windows y MATLAB

4. PROCEDIMIENTO 4.1 PROBLEMA 1 Obtener las ecuaciones del movimiento del robot de la figura, considerando que la masa de cada uno de los enlaces se concentra en su centro de masas. Emplear las formulaciones de Newton-Euler y Lagrange. También emplear los algoritmos computacionales de Lagrange y Newton en la solución. Aplicar MATLAB.

4.1.1

ALGORITMO DE NEWTON EULER

0 0𝑣0

0 0𝑤0

= [0,0,0]𝑇

0 0𝑣0

= [0,0,0]𝑇

𝑇

= −[𝑔𝑥𝑜 , 𝑔𝑦𝑜 , 𝑔𝑧𝑜 ] = −[0,0, −𝑔]𝑇 = [0,0, 𝑔]𝑇 𝑧0 = [0,0,1]𝑇

1 0𝑝1 2 0𝑝2

1 0𝐼1

= [0,0,0]𝑇

= [0,0, 𝑑2 ]𝑇

1 0𝑠1

= [0 0 𝐿1 ]

2 0𝑠2

= [0 0 0]

0 0 = [0 0 0 0

0 0 0 0] , 20𝐼2 = [0 0 0 0 0

0 0] 0

Matrices de Rotación 0 0𝑅1

𝐶1 = [ 𝑆1 0

0 −𝑆1 1 0 𝐶1 ] , 10𝑅2 = [0 −1 0 0

0 0 𝐶1 0 −𝑆1 1 0], 00𝑅2 = [ 𝑆1 0 𝐶1 ] 0 1 0 −1 0

1 0𝑣̇ 1

1 0𝑎1

1 0𝑤1

0 0 = [−𝜃1̇ ] , 20𝑤2 = [−𝜃1̇ ] 0 0

1 0𝑤̇1

0 0 = [−𝜃1̈ ] , 20𝑤̇2 = [−𝜃1̈ ] 0 0

−𝜃1̈ 𝑑2 − 2𝜃1̈ 𝑑̇2 0 2 −𝑔 = [−𝑔] , 0𝑣̇ 2 = [ ] 2 0 ̈ ̇ 𝑑2 − 𝜃1 𝑑2

−𝜃1̈ 𝐿1 −𝜃1̈ 𝑑2 − 2𝜃̇1 𝑑2 2 −𝑔 = [ −𝑔 ] , 0𝑎2 = [ ] 2 2 𝜃̇1 𝐿1 𝑑̈2 − 𝜃̇1 𝑑2 2 0𝑓2

1 0𝑓1

−𝜃1̈ 𝑑2 𝑚2 − 2𝜃̇1 𝑑̇2 𝑚2 − 𝜃1̈ 𝑙1 𝑚1 −𝑔(𝑚2 + 𝑚1 ) =[ ] 2 2 𝑑̈2 𝑚2 − 𝜃̇1 𝑑2 𝑚2 − 𝜃̇1 𝑙1 𝑚1 2 0𝑛1

1 0𝑛1

−𝜃1̈ 𝑑2 𝑚2 − 2𝜃̇1 𝑑̇2 𝑚2 −𝑔𝑚2 =[ ] 2 ̈ ̇ 𝑑2 𝑚2 − 𝜃1 𝑑2 𝑚2

𝑑2 𝑚2 𝑔 = [−(𝜃1̈ 𝑑2 − 2𝜃̇1 𝑑̇2 𝑑2 )𝑚2 ] 0 2

(𝑑2 𝑚2 + 𝑙1 𝑚1 )𝑔 = [−(𝜃1̈ 𝑑2 2 − 2𝜃̇1 𝑑̇2 𝑑2 )𝑚2 − 𝜃1̈ 𝑙1 2 𝑚1 ] 0

2

𝐹2 = 𝑑̈2 𝑚2 − 𝜃̇1 𝑑2 𝑚2

𝑇1 = (𝜃1̈ 𝑑2 2 − 2𝜃̇1 𝑑̇2 𝑑2 )𝑚2 + 𝜃1̈ 𝑙1 2 𝑚1 4.1.2

RESOLUCION - MATLAB

%MODELO DINAMICO - ROBOT 2GDL - ALGORITMO NEWTON - EULER fprintf('Algoritmo de Newton Euler para el robot de 2 GDL'); %Parámetros Denavit Hartenberg syms q1 q2 l1 l2 d1 d2 dd2 g m1 m2 ; ang=sym(pi); %Parametros Denavit Hartenberg link1=[q1 0 0 -pi/2]; link2=[0 dd2 0 0]; %Velocidad angular - Derivada de q1 y d2

syms qd1 qd2; %Aceleración angular - Derivada de qd1 y qd2 syms qdd1 qdd2; %Definicion de la trayectoria %PASO 1: ASIGNAR SISTEMAS DE REFERENCIA A CADA ESLABON SEGUN D-H %——————————————————————————————————————————————————————— %PASO 2:ESTABLECER CONDICIONES INICIALES %Para el sistema de la base {S0} % Velocidades y aceleraciones de la base del robot w0=[0 0 0]';%Velocidad angular 0 w 0 dw0=[0 0 0]';%Aceleración angular 0 dw 0 v0=[0 0 0]';%Velocidad lineal 0 v 0 dv0=-[0 0 -g]';%Aceleración lineal 0 dv 0 % Definicion de Fuerzas en el extremo %Como no se ejercen fuerzas externas en el extremo del robot esto es cero: sf3=[0 0 0]'; %Fuerzas externas sn3=[0 0 0]'; %Pares externos %-----------------------------z0=[0 0 1]'; %Siempre tiene esta forma segun Barrientos %Vectores que unen el origen {Si-1} con {Si}->[ai,di*sin(alphai),di*cos(alphai)] p1=[link1(3) link1(2)*sin(link1(4)) link1(2)*cos(link1(4))]'; %vector de {S0} a {S1}en el %sistema {S1} p2=[link2(3) link2(2)*sin(link2(4)) link2(2)*cos(link2(4))]'; %vector de {S1} a {S2}en el %sistema {S2} %Coordenadas del centro de masas del eslabon i respecto del sistema {Si} s1=[0 0 l1]'; %Coordenadas del cdg de la barra 1 respecto {S1} s2=[0 0 0]'; %Coordenadas del cdg de la barra 2 respecto {S2} %Matrices de inercia del eslabón i expresado en un sistema paralelo a {Si} con el origen I1=zeros(3); %Inercias de barra 1 respecto su cdg en la base{S1}; I2=zeros(3); %Inercias de barra 2 respecto su cdg en la base{S2}; %Son ceros ya que toda la masa está concentrada en el centro de gravedad %——————————————————————————————————————————————————————— % PASO 3: Obtención de las matrices de rotación R01 y R12 R01=[cos(link1(1)) -cos(link1(4))*sin(link1(1)) sin(link1(4))*sin(link1(1)) sin(link1(1)) cos(link1(4))*cos(link1(1)) sin(link1(4))*cos(link1(1)) 0 sin(link1(4)) cos(link1(4))]; R12=[cos(link2(1)) -cos(link2(4))*sin(link2(1)) sin(link2(4))*sin(link2(1)) sin(link2(1)) cos(link2(4))*cos(link2(1)) sin(link2(4))*cos(link2(1)) 0 sin(link2(4)) cos(link2(4))]; %—————————————————————————————————————————————————————— %PASO 4: Obtención de la velocidad angular del sistema {Si} w1=R01'*(w0+z0*qd1); %Rotación w2=R12'*(w1+z0*qd2); %————————————————————————————————————————————————————————— % PASO 5: Obtención de la aceleracion angular de {Si} dw1=R01'*(dw0 +z0*qdd1)+cross(w0,z0*qd1); %Rotación dw2=R12'*dw1; %Traslacion %————————————————————————————————————————————————————————— % PASO 6: Obtención la aceleración lineal del sistema i dv1=cross(dw1,p1)+cross(w1,cross(w1,p1))+R01'*dv0; %Rotación

dv2=R12'*(z0*qdd2+dv1)+cross(dw2,p2)+2*cross(w2,R12'*z0*qd2)+cross( w2,cross(w2,p2)); %Traslación %Traslación %—————————————————————————————————————————————————————— % PASO 7: Obtención de la aceleración lineal de los cdg del eslabón i a1=cross(dw1,s1)+cross(w1,cross(w1,s1))+dv1; a2=cross(dw2,s2)+cross(w2,cross(w2,s2))+dv2; %————————————————————————————————————————————————————— % PASO 8: Obtención de la fuerza ejercida sobre el eslabón i R23=eye(3); f2=R23*sf3+m2*a2; f1=R12*f2+m1*a1; %——————————————————————————————————————————————————— % PASO 9: Obtención del par ejercido sobre el eslabón i n2=R23*(sn3+cross((R23'*p2),sf3))+cross((p2+s2),m2*a2)+I2*dw2+cross (w2,(I2*w2)); n1=R12*(n2+cross((R12'*p1),f2))+cross((p1+s1),m1*a1)+I1*dw1+cross(w 1,(I1*w1)); %———————————————————————————————— % PASO 10: Obtención de la fuerza o par aplicado a la articulación i fprintf('\nTorque en la articulación 1: \n'); T1=simplify(n1'*R01'*z0); disp(T1); %Rotación fprintf('\nFuerza en la articulación 2: \n'); F2=simplify(f2'*R12'*z0); disp(F2);%Rotación % FIN DEL ALGORITMO

ALGORITMO DE LAGRANGE %MODELO DINAMICO - ROBOT 2GDL - ALGORITMO DE LAGRANGE fprintf('Algoritmo de Lagrange para el robot de 2 GDL'); syms q1 q2 L1 L2 d1 d2 dd2 g m1 m2 ; ang=sym(pi); G=[g 0 0 0]; r11=[0 0 L1 1]'; r22=[0 0 0 1]'; %Matrices de Derivación %—————————————————————— Qr=zeros(4);Qr(1,2)=-1;Qr(2,1)=1; Qd=zeros(4);Qd(3,4)=1; %Parametros Denavit Hartenberg link1=[q1 0 0 -pi/2]; link2=[0 dd2 0 0]; %———————————————————————————————————————————————————————————— % Matrices A00 y A11 son la identidad A00=eye(4); A11=eye(4); %Evalua las matrices A01 y A02 A01 =[cos(q1) 0 -sin(q1) 0; sin(q1) 0 cos(q1) 0; 0 -1 0 0; 0 0 0 1]; A12 =[1 0 0 0;

0 1 0 0; 0 0 1 dd2; 0 0 0 1]; %Evalua la matriz A02=A011*A2 A02=A01*A12; % PASO 3 Evaluacion matrices Uij %——————————————————————————————— U11=A00*Qr*A01; U12=zeros(4); U21=A00*Qr*A02; U22=A01*Qd*A12; %PASO 4 Evaluación de las matrices Uijk %——————————————————————————————————————— U111=A00*Qr*A00*Qr*A01; U112=zeros(4); U121=zeros(4); U122=zeros(4); U211=A00*Qr*A00*Qr*A02; U212=A00*Qr*A01*Qd*A12; U221=A00*Qr*A01*Qd*A12; U222=A01*Qd*A11*Qd*A12; % PASO 5 Evaluación matrices de pseudoinercia Ji %——————————————————————————————————————————————— J1=[0 0 0 0 0 0 0 0 0 0 L1^2*m1 L1*m1 0 0 L1*m1 m1]; J2=[0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 m2]; % PASO 6 Evaluación de la matriz de Inercias D %—————————————————————————————————————————————— D11=simplify(trace(U11*J1*U11')+trace(U21*J2*U21')); D12=simplify(trace(U22*J2*U21')); D21=D12; D22=simplify(trace(U22*J2*U22')); D=[D11 D12 D21 D22]; % PASOS 7-8 Evaluación de la matriz de Coriolis H %———————————————————————————————————————————————— h111=trace(U111*J1*U11')+trace(U211*J2*U21'); h112=trace(U212*J2*U21'); h121=trace(U221*J2*U21'); h122=trace(U222*J2*U21'); h211=trace(U211*J2*U22'); h212=trace(U212*J2*U22'); h221=trace(U221*J2*U22'); h222=trace(U222*J2*U22'); H1=h111*qd1*qd1 + h112*qd1*qd2 + h121*qd2*qd1 + h122*qd2*qd2; H2=h211*qd1*qd1 + h212*qd1*qd2 + h221*qd2*qd1 + h222*qd2*qd2; H=[simplify(H1) simplify(H2)]; % PASO 9 Evaluación de la matriz de Gravedad C %—————————————————————————————————————————————— C1=-m1*G*U11*r11-m2*G*U21*r22; C2=-m1*G*U12*r11-m2*G*U22*r22; C=[C1 C2]; % PASO 10 Evaluación de los pares %————————————————————————————————

4.2 PROBLEMA 2 Obtener las ecuaciones del movimiento del robot de la figura, considerando que la masa de cada uno de los enlaces se concentra en su centro de masas. Emplear las formulaciones de Newton-Euler y Lagrange. Emplear los algoritmos computacionales de Lagrange y Newton en la solución. Aplicar MATLAB

4.2.1

RESOLUCIÓN ANALITICA

4.2.2

RESOLUCION - MATLAB

4.3 PROBLEMA 3 Desarrollar un programa que implemente los algoritmos computacionales de Lagrange y Newton para los robots de los ítems anteriores. El programa graficará los pares necesarios para que el extremo del robot describa una determinada trayectoria, que estará definida analíticamente en el mismo programa.

4.3.1

PROBLEMA 1 Tomando como valores dimensionales L1 = 10; m1 = 20; m2 = 10. La trayectoria se define simbólicamente y a partir de ella se obtiene la velocidad y aceleración de manera simbólica, para después ser evaluada en 50 puntos entre 0 y 5 segundos.

% % %

Algoritmo de Lagrange para la solución del modelo dinámico inverso -----------------------------------------------------------------Robot polar de 2 GDL ejercicio

% Definición de la trayectoria %-----------------------------------% Definición simbólica de la trayectoria de t = sym('t','integer','positive','); sq1='sin(t)*t+pi/2'; sq2='sin(5*t)+t+1';

posición

sq1='0'; sq2='sin(2*t)'; % Obtención simbólica de la velocidad y aceleración sqd1=diff(sq1,'t');sqdd1=diff(sqd1,'t'); sqd2=diff(sq2,'t');sqdd2=diff(sqd2,'t'); % Parámetros dimensionales %-----------------------------------L1=10; m1=20; m2=10; g=9.8; G=[g 0 0 0]; r11=[0 0 L1 1]'; r22=[0 0 0 1]'; %Matrices de Derivación %-----------------------------------Qr=zeros(4);Qr(1,2)=-1;Qr(2,1)=1; Qd=zeros(4);Qd(3,4)=1; % Bucle de paso de tiempo %-----------------------------------for tk=1:1:50; t=(tk-1)/10; % Evaluación numérica de posición, velocidad y aceleración %-------------------------------------------------------q1=eval(sq1);Q1(tk)=q1 ;qd1=eval(sqd1) ;qdd1=eval(sqdd1); q2=eval(sq2);Q2(tk)=q2 ;qd2=eval(sqd2) ;qdd2=eval(sqdd2); % PASOS 1 y 2 Obtención de las matrices de transformación Aij %-----------------------------------------------------------% Matrices A00 y A11 son la identidad A00=eye(4);

A11=eye(4); %Evalua las matrices A01 y A02 A01 =[cos(q1) 0 -sin(q1) 0;sin(q1) 0 cos(q1) 0;0 -1 0 0;0 0 0 1]; A12 =[1 0 0 0;0 1 0 0;0 0 1 q2;0 0 0 1]; %Evalua la matriz A02=A011*A2 A02=A01*A12; % PASO 3 Evaluacion matrices Uij %-------------------------------U11=A00*Qr*A01; U12=zeros(4); U21=A00*Qr*A02; U22=A01*Qd*A12; %PASO 4 Evaluación de las matrices Uijk %--------------------------------------U111=A00*Qr*A00*Qr*A01; U112=zeros(4); U121=zeros(4); U122=zeros(4); U211=A00*Qr*A00*Qr*A02; U212=A00*Qr*A01*Qd*A12; U221=A00*Qr*A01*Qd*A12; U222=A01*Qd*A11*Qd*A12; % PASO 5 Evaluación matrices de pseudoinercia Ji %-----------------------------------------------J1=zeros(4);J1(3,3)=L1^2*m1;J1(3,4)=L1*m1;J1(4,3)=L1*m1;J1(4,4)=m1; J2=zeros(4);J2(4,4)=m2; % PASO 6 Evaluación de la matriz de Inercias D %---------------------------------------------D(1,1)=trace(U11*J1*U11')+trace(U21*J2*U21'); D(1,2)=trace(U22*J2*U21'); D(2,1)=D(1,2); D(2,2)=trace(U22*J2*U22'); % PASOS 7-8 Evaluación de la matriz de Coriolis H %------------------------------------------------h111=trace(U111*J1*U11')+trace(U211*J2*U21'); h112=trace(U212*J2*U21'); h121=trace(U221*J2*U21'); h122=trace(U222*J2*U21'); h211=trace(U211*J2*U22'); h212=trace(U212*J2*U22'); h221=trace(U221*J2*U22'); h222=trace(U222*J2*U22'); H(1,1)=h111*qd1*qd1 + h112*qd1*qd2 + h121*qd2*qd1 + h122*qd2*qd2; H(2,1)=h211*qd1*qd1 + h212*qd1*qd2 + h221*qd2*qd1 + h222*qd2*qd2; % PASO 9 Evaluación de la matriz de Gravedad C %---------------------------------------------C(1,1)=-m1*G*U11*r11-m2*G*U21*r22; C(2,1)=-m1*G*U12*r11-m2*G*U22*r22; % PASO 10 Evaluación de los pares

%--------------------------------PARES(:,tk)=D*[qdd1 qdd2]'+H+C; % FIN DEL ALGORITMO %----------------------------------------------%-.-.-.-.--.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.% VERIFICACIÓN DEL RESULTADO: % Este código es únicamente a efectos de verificar % que la programación del algoritmo es correcta % Obtiene la solución analítica obtenida manualmente % (Expresión [5.18]) T1=(m1*L1^2+m2*q2^2)*qdd1+2*q2*m2*qd1*qd2; T1=T1 + g*(m1*L1*cos(q1)+m2*q2*cos(q1)); F2=m2 *qdd2-q2*m2*qd1^2+m2*g*sin(q1); PARES2(1,tk)=T1; PARES2(2,tk)=F2; %-.-.-.-.--.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.end % fin del bucle de paso de tiempo

% Presentación Gráfica de los resultados %---------------------------------------figure(1);clf subplot(2,2,1),plot(Q1),grid,legend(strcat('Q1:',sq1),0) subplot(2,2,2),hold, plot(PARES(1,:)),plot(PARES2(1,:),'rx') grid,title(' PAR Q1 Alg.Lagrange'),hold subplot(2,2,3),plot(Q2),grid,legend(strcat('Q2:',sq2),0) subplot(2,2,4),hold plot(PARES(2,:)),plot(PARES2(2,:),'rx') grid,title(' PAR Q2 Alg.Lagrange'),hold

Trayectoria con solo el primer GDL en movimiento y pares correspondientes.

Trayectoria con solo el primer GDL en movimiento y segundo en cero. Trayectoria deseada:

𝜃1 = sin(2𝑡) + 𝜋/2 𝑑2 = 1

Las ecuaciones de movimiento toman la forma: 𝑇1 = (𝑚1 𝐿21 + 𝑚2 𝑑22 )𝜃̈ + 2𝑑2 𝑚2 𝜃̇2 𝑑̇2 + 𝑚1 𝑔𝐿1 cos 𝜃1 + 𝑚2 𝑔𝑑2 cos 𝜃1 = 𝐾1 𝜃̈1 + 𝐾2 cos 𝜃1 2 𝐹2 = 𝑚2 𝑑̈2 − 𝑑2 𝑚2 𝜃̇1 + 𝑚2 𝑔 sin 𝜃1 = 𝐾3 𝜃̇1 + 𝐾4 sin 𝜃1

4.3.2

PROBLEMA 2

Usando la función rne de la Toolbox de Robotica de Matlab, aplicada al robot

Articulación 𝜃 1 𝑞1 2 𝑞2

d a 0 𝑙 0 𝑙2

𝛼 0 0

% Algoritmo de Newton Euler % para la solución del modelo dinámico inverso de un robot %————————————————————————————————————————————————————————— % % Robot 2 GDL % Número de puntos de la trayectoria n=100; % Definición de la trayectoria %————————————————————————————— % Definición simbólica de la posición sq1='sin(t)+pi/2'; sq2='sin(5*t)+t+1'; % Definicion de Fuerzas en el extremo %———————————————————————————————————— sf3='[0 00 0]'; % Sustituir en cada eje en caso se querer aplicar sn3='[0 0 0]'; % fuerzas o pares externos % Obtención simbólica de la velocidad y aceleración sqd1=diff(sq1,'t') ;sqdd1=diff(sqd1,'t'); sqd2=diff(sq2,'t') ;sqdd2=diff(sqd2,'t'); % Parámetros dimensionales %————————————————————————— l1=10;l2=10; d1=6;d2=6; m1=20;m2=10; g=9.8; % Parámetros iniciales %————————————————————— % Velocidades y aceleraciones de la base del robot w0=[0 0 0]'; dw0=[0 0 0]'; v0=[0 0 0]'; dv0=-[0 -g 0]'; z0=[0 0 1]'; % Matriz de rotación entre {S2} y TCP ({S3}) %———————————————————————————————————————————— R23=eye(3); % Bucle de paso de tiempo %———————————————————————— for tk=1:1:n; t=(tk-1)/10; % Evaluación numérica de posición, velocidad, aceleración % y trayectoria del TCP %————————————————————————————————————————————————————————— q1=eval(sq1) ;qd1=eval(sqd1) ;qdd1=eval(sqdd1); q2=eval(sq2) ;qd2=eval(sqd2) ;qdd2=eval(sqdd2); q(tk,1)=q1;qd(tk,1)=qd1;qdd(tk,1)=qdd1; q(tk,2)=q2;qd(tk,2)=qd2;qdd(tk,2)=qdd2;

XY(tk,1)=l1*cos(q1)+l2*cos(q1+q2); XY(tk,2)=l1*sin(q1)+l2*sin(q1+q2); %Evaluación numérica de las fuerzas y pares en el TCP %————————————————————————————————————————————————————— n3=eval(sn3)' ; f3=eval(sf3)' ; n_3(tk,:)=n3'; f_3(tk,:)=f3'; % PASO 1-2 Evaluacion de los vectores p,s y de la matriz de inercias %——————————————————————————————————————————————————————————————————— p1=[l1 0 0]'; %vector de {S0} a {S1}en el sistema {S1} p2=[l2 0 0]'; %vector de {S1} a {S2}en el sistema {S2} s1=[-(l1-d1) 0 0]'; %Coordenadas del cdg de la barra 1 respecto {S1} s2=[-(l2-d2) 0 0]'; %Coordenadas del cdg de la barra 2 respecto {S2} I1=zeros(3); %Inercias de barra 1 respecto su cdg en la base{S1}; I2=zeros(3); %Inercias de barra 2 respecto su cdg en la base{S2}; % PASO 3 Obtención de las matrices de rotación R01 y R12 %——————————————————————————————————————————————————————— R01=[cos(q1) -sin(q1) 0;sin(q1) cos(q1) 0;0 0 1]; R12=[cos(q2) -sin(q2) 0;sin(q2) cos(q2) 0;0 0 1]; %PASO 4 Evaluación de las velocidades angulares de {Si} %—————————————————————————————————————————————————————— w1=R01'*(w0+z0*qd1); w2=R12'*(w1+z0*qd2); % PASO 5 Evaluación de las aceleraciones angulares de {Si} %————————————————————————————————————————————————————————— dw1=R01'*(dw0 +z0*qdd1)+cross(w0,z0*qd1); dw2=R12'*(dw1 +z0*qdd2)+cross(w1,z0*qd2); % PASO 6 Evaluación de las aceleraciones lineales de {Si} %———————————————————————————————————————————————————————— dv1=cross(dw1,p1)+cross(w1,cross(w1,p1))+R01'*dv0; dv2=cross(dw2,p2)+cross(w2,cross(w2,p2))+R12'*dv1; % PASO 7 Evaluación de la aceleración lineal de los cdg %—————————————————————————————————————————————————————— a1=cross(dw1,s1)+cross(w1,cross(w1,s1))+dv1; a2=cross(dw2,s2)+cross(w2,cross(w2,s2))+dv2; % PASO 8 Evaluación de las fuerzas sobre los eslabones %————————————————————————————————————————————————————— f2=R23*f3+m2*a2; f1=R12*f2+m1*a1; CAPÍTULO 5. DINÁMICA DEL ROBOT 273 % PASO 9 Evaluación de los pares sobre los eslabones %——————————————————————————————————————————————————— n2=R23*(n3+cross((R23'*p2),f3))+cross((p2+s2),m2*a2)+I2*dw2+cross(w2,( I2*w2)); n1=R12*(n2+cross((R12'*p1),f2))+cross((p1+s1),m1*a1)+I1*dw1+cross(w1,( I1*w1)); % PASO 10 Evaluación de los pares %———————————————————————————————— P1=n1'*R01'*z0; P2=n2'*R12'*z0; PARES(tk,1)=P1; PARES(tk,2)=P2; % FIN DEL ALGORITMO %—————————————————— %-.-.-.-.—.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-. %VERIFICACIÓN: % Este código es únicamente a efectos de verificar % que la programación del algoritmo es correcta % Obtiene la solución analítica obtenida manualmente % (Expresión [5.20])

T1=(m1*d1^2+m2*(l1^2+d2^2+2*l1*d2*cos(q2)))*qdd1+... +(m2*d2*(l1*cos(q2)+d2))*qdd2+... -(2*m2*l1*d2*sin(q2))*qd2*qd1+... -(m2*d2*l1*sin(q2))*qd2*qd2+... +g*((m1*d1+m2*l1)*cos(q1)+ m2*d2*cos(q1+q2)); T2= (m2*d2*(l1*cos(q2)+d2))*qdd1+... + (m2*d2^2)*qdd2+... + (m2*d2*l1*sin(q2))*qd1*qd1+... + g*m2*d2*cos(q1+q2); PARES2(tk,1)=T1; PARES2(tk,2)=T2; %-.-.-.-.—.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.end % fin del bucle de paso de tiempo %————————————————————————————————————— %-.-.-.-.—.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-. %VERIFICACIÓN: % Este código es únicamente a efectos de verificar % que la programación del algoritmo es correcta % Obtiene la solución mediante la función rne de la toolbox de robótica de % de la toolbox de robótica de Matlab defScara; % Se define el robot grav=[0,9.8,0]; % Se define la gravedad con respecto al sistema {S0} PARES3=rne(Scara,q,qd,qdd,grav,[f_3,n_3]); %-.-.-.-.—.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.-.% Presentación Gráfica de los resultados figure(1);clf subplot(2,2,1),hold;grid,plot(q(:,1)),title('Q1'),legend(strcat('Q1:', sq1),0),hold; subplot(2,2,2),hold;grid,title(' PAR Q1 Alg.NE (-), Sol. analítica (x),Función rne (o)') plot(PARES(:,1)),plot(PARES2(:,1),'rx'),plot(PARES3(:,1),'bo'),hold; subplot(2,2,3),hold;grid,plot(q(:,2)),title('Q2'),legend(strcat('Q2:', sq2),0),hold; subplot(2,2,4),hold;grid,title(' PAR Q2 Alg.NE (-), Sol. analítica (x),Función rne (o)') plot(PARES(:,2)),plot(PARES2(:,2),'rx'),plot(PARES3(:,2),'bo'),hold;

Trayectoria con solo el primer GDL en movimiento y pares correspondientes.

Trayectoria y pares con los 2 GDL en movimiento. Trayectoria definida por 𝑞1 = sin(𝑡) + 𝑝𝑖/2 𝑞2 = sin(5𝑡) + 𝑡 + 1

5. CONCLUSIONES Y OBSERVACIONES