Вы находитесь на странице: 1из 5

Universidad Politcnica de Aguascalientes. Carrera: Ingeniera Mecatrnica. Materia: Robtica 2.

Algoritmo de lagrange-euler

Alumnos: Jos Guillermo vila Martnez. Maestro: M.C. Luis Ernesto Anaya Tiscareo. Grupo: 9. C.

5 de octubre de 2012

%--------------Jose Guillermo Avila Martinez, UP090293 %%%%%%%%%%%%%% algoritmo de lagrange-euler %%%%%%%%%%%%%%%%%%%%%%% %este programa se divide en 2 partes, que es la adqusicion de datos y la otra que es entra la funcion en la cual se realiza el calculo de los torques de los motores del robot. clear clc %las siguientes son las valiables del sistema syms teta d a alfa vq aq g M L % es cuando el sistema comienza desde su orige % vq = derivada del desplazaiento q % las variables "Pj", "S" e "I" se calcularan dentro del programa principal n=input('dame el numero de parametros DH que tiene tu robot'); instruccion='para establecer si un eslabon es de rotacion favor de indicarlo con un 1 en la quinta posicion del vector de parametros DH sino marcar con 0 recuerda poner los parametros apartir del ultimo eslabon al primero'; instruccion pause clc %paso1 syms teta d a alfa vq aq g M1 for j=1:n e{j,1}=input('dame el valor de los parametros DH del primer eslabon en forma de vector (teta, d, a, alfa)'); mas{j,1}=input('dame el valor de la masa del esalbon'); s{j,1}=input('dame los valores de las coordenadas del centro de masa en forma de vector[x, y, z, 1]'); clc end; t=lagrange_tork(e, s, mas, n)

%la funcion dinamica_robot( ) esta funcion puede calcular ecuaciones de %torque para un robot de n eslabones. %%%%%% Ejemplo: % e = parametros DH (es un arreglo) % s = coordenadas de centros de masa (es un arreglo) % mas = masa del eslabon (es un arreglo) % n = debera ser el valor de filas de los parametros DH aumentado en 1(es un valor) % dinamica_robot ( e, s, mas, n) %%%%t=[ecuacion1 ; ecuacion 2; ecuacion 3;... ecuacion n] ecuaciones function t = lagrange_tork(e, s, mas, n) aux=0; aux2=0; aux3=0; aux4=0; g=[0 0 -9.8 0]; syms vq aq M1 %este es el paso 2-- obtener las matrices de transformacion 0Ai para cada %elemento i for i=n+1:-1:1 %en esta parte se genera la matriz "A" para guardar %las matrices de trans-homogenia for j=1:n+1 if j==i %si el cursor se encuentra en la diagonal principal se rellenara con un vector unitario A{i,j}=eye(4); else if j-i==1 %en esta parte se compara la posicion de la segunda diagonal para tener los elementos y asi %%%poder multiplicar todas las matrices A{i,j}=Traslado(e{i,1}(1,1), e{i,1}(1,2), e{i,1}(1,3), e{i,1}(1,4)); else if (j-i)>1 %%%%en esta parte se realizan las multiplicaciones %%%correspondientes para completar la matriz A{i,j}=A{i,j-1}*A{j-1,j}; end end end end end

%tipo Qr=[0 1 0 0 Qt=[0 0 0 0

de "Qs" lineal o rotacional -1 0 0; 0 0 0; 0 0 0; 0 0 0]; 0 0 0 0 0 0 0 0 0; 0; 1; 0];

for u=1:n % se guardan las matrices Q dependiendo si el eslabon es de rotacion o traslacion if e{i,1}(1,5)==1 % se pregunta si el eslabon es de rotacion o de traslacion Q{u}=Qr; else Q{u}=Qt; end end %paso 3--se obtienen las matrices Uij % for i=1:n for j=1:n if j<=i Uij{i,j}=A{1,j-1}*Q{j}*A{j,i+1}; else Uij{i,j}=0; end end end %paso 4--se otienen las matrices Uijk % for i=1:n for j=1:n for k=1:n if i>=k && k>=j Uijk{i,j,k}=A{1,j}*Q{j}*A{j,k}*Q{k}*A{k,i+1}; else if i>=j && j>=k Uijk{i,j,k}=A{1,k}*Q{k}*A{k,j}*Q{j}*A{j,i+1}; else if k>i || j>i Uijk{i,j,k}=0; end end end end end end %%%paso 5--se obtienen las matrices de pseudoinercias Ji para cada elemento for i=1:n J{i}=[1/3*s{i,1}(1,1)^2*mas{i,1}, 1/4*s{i,1}(1,1)*s{i,1}(1,2)*mas{i,1}, 1/4*s{i,1}(1,1)*s{i,1}(1,3)*mas{i,1}, 1/2*s{i,1}(1,1)*mas{i,1}; 1/4*s{i,1}(1,1)*s{i,1}(1,2)*mas{i,1}, 1/3*s{i,1}(1,2)^2*mas{i,1}, 1/4*s{i,1}(1,2)*s{i,1}(1,3)*mas{i,1}, 1/2*s{i,1}(1,2)*mas{i,1}; 1/4*s{i,1}(1,3)*s{i,1}(1,1)*mas{i,1}, 1/4*s{i,1}(1,3)*s{i,1}(1,2)*mas{i,1}, 1/3*s{i,1}(1,3)^2*mas{i,1}, 1/2*s{i,1}(1,3)*mas{i,1}; 1/2*s{i,1}(1,1)*mas{i,1}, 1/2*s{i,1}(1,2)*mas{i,1}, 1/2*s{i,1}(1,3)*mas{i,1}, mas{i,1}];

end

%paso 6--se obtiene la matriz de inercias D=[dij] for i=1:n for j=1:n for k=max(i,j):n D{i,j} = trace(Uij{k,j}*J{k}*(Uij{k,i}).'); aux=D{i,j}+aux; %%% axiliar se utiliza para realizar la sumatoria D{i,j}=aux; end end end %paso 7--se obtienen los terminos hikm for i=1:n for k=1:n for m=1:n for j=max(i,max(k,m)):n hikm{i,k,m}=trace(Uijk{j,k,m}*J{j}*(Uij{j,i}).'); aux2=aux2 + hikm{i,k,m}; hikm{i,k,m}=aux2; end end end end %paso 8--se obtiene la matriz columna de fuerzas de coriolis y %centripeta H=[hi]^T for i=1:n for k=1:n for m=1:n hi{i}=hikm{i,k,m}*vq*vq+aux3; aux3=hi{i}; H=(aux3).'; end end end %paso 9--se obtiene la matriz columna de fuerzas de gravedad C=[Ci]^T for i=1:n for j=1:n c{i}=-mas{i}*g*Uij{j,i}*s{i,1}.'; aux4=c{i}+aux4; C=(aux4).'; end end %paso 10--la ecuacion de torque sera t=D*aq+H+C; end

Вам также может понравиться