ALGORITMO COMPUTACIONAL DE LAGRANGE
Autor:
Leidy Yulieth Velasco Becerra
William Eduardo Telles Ramirez
Kristhian José Montoya Montoya
FACULTAD DE INGENIERÍAS y ARQUITECTURA
ROBOTICA II
ING. Luis Evelio Prado Perez
SEPTIEMBRE, 2023
“Formando líderes para la construcción de un nuevo país en paz”
Universidad de Pamplona
Pamplona - Norte de Santander - Colombia 1
Tels: (7) 5685303 - 5685304 - 5685305 - Fax: 5682750
[Link]
SC-CER96940
ALGORITMO COMPUTACIONAL DE LAGRANGE
Autor:
Leidy Yulieth Velasco Becerra
William Eduardo Telles Ramirez
Kristhian José Montoya Montoya
FACULTAD DE INGENIERÍAS y ARQUITECTURA
ING. Luis Evelio Prado Perez
UNIVERSIDAD DE PAMPLONA
SEDE VILLA DEL ROSARIO
INGENIERIA MECATRONICA
Cúcuta-Norte de Santander
Septiembre, 2023
“Formando líderes para la construcción de un nuevo país en paz”
Universidad de Pamplona
Pamplona - Norte de Santander - Colombia 2
Tels: (7) 5685303 - 5685304 - 5685305 - Fax: 5682750
[Link]
SC-CER96940
Se propondrá el algoritmo computacional de Lagrange para manipuladores de hasta 6 GDL. Se
tendrá en cuenta el ejemplo proporcionado en los pdf de la clase, para comparar resultados
Primeramente, se le pedirá al usuario lo siguiente
1. La cantidad de grados de libertad del manipulador
2. Saber qué tipo es cada articulación, colocar r si es rotacional y p si es prismático
3. Se debe ingresar de manera simbólica las variables articulares, en este caso se
recomienda colocar q1,q2,q3, etc
4. Se debe ingresar de manera simbólica las variables de velocidad, en este caso se
recomienda colocar dq1,dq2,dq2, etc
5. Se debe ingresar de manera simbólica las variables de aceleración, en este case se
recomienda colocar ddq1, ddq2, ddq3, etc
6. Ingresar de manera simbólica las masas, m1,m2,m3, etc
7. Ingresar las coordenadas de centro de masa de cada articulación
8. Conocer el vector de gravedad el cual se va indicar con g o -g el sentido del mismo
9. Conocer los parámetros DH del manipulador, teniendo en cuenta que cada Angulo que
se ingrese debe estar en radianes ya que el calculo del algoritmo lo realiza de forma
simbólica
“Formando líderes para la construcción de un nuevo país en paz”
Universidad de Pamplona
Pamplona - Norte de Santander - Colombia 3
Tels: (7) 5685303 - 5685304 - 5685305 - Fax: 5682750
[Link]
SC-CER96940
Ejemplo
Se tiene el siguiente manipulador de 2 GDL
1. Cuantos Grados de libertad tiene el manipulador
n=input('Cuantos GDL tiene el manipulador: ');
R/ 2
2. Según el tipo de articulación ingrese “r” si es rotacional y p si es Prismática
fprintf('Segun el tipo de articulacion ingrese r si es Rotacional y p si
es Prismatica\n').
for i=1:n
fprintf('articulacion %i: ',i)
art(i)=input(' ');
end
se creo un for que empezara de 1 hasta n, recordemos que n son los grados de libertad,
por lo que el Ford recorrerá la cantidad de grados de libertad que tiene el manipulador, y
guardara en un vector “art” cada articulación del manipulador.
“Formando líderes para la construcción de un nuevo país en paz”
Universidad de Pamplona
Pamplona - Norte de Santander - Colombia 4
Tels: (7) 5685303 - 5685304 - 5685305 - Fax: 5682750
[Link]
SC-CER96940
3. Ingrese simbólicamente las variables articulares
for i=1:n
var=input('Variable:','s');
q(i)=str2sym(var);
end
se recorre un for que empieza de 1 hasta n que es el numero de grados de libertad del
manipulador, y se guardara en un vector q, las variables articulares.
4. Ingrese simbólicamente las velocidades de las articulaciones
for i=1:n
vel=input('Velocidad:','s');
dq(i)=str2sym(vel);
end
se recorre un for que empieza de 1 hasta n que es el numero de grados de libertad del
manipulador, y se guardara en un vector dq, las velocidades.
5. Ingresar simbólicamente las aceleración de las articulaciones
for i=1:n
acel=input('Aceleracion:','s');
ddq(i,1)=str2sym(acel);
end
se recorre un for que empieza de 1 hasta n que es el numero de grados de libertad del
manipulador, y se guardara en un vector ddq, las aceleraciones
6. Ingrese las masas simbólicas
for i=1:n
masa=input('Masa:','s');
m(i)=str2sym(masa);
end
se recorre un for que empieza de 1 hasta n que es el número de grados de libertad del
manipulador, y se guardara en un vector m las masas
“Formando líderes para la construcción de un nuevo país en paz”
Universidad de Pamplona
Pamplona - Norte de Santander - Colombia 5
Tels: (7) 5685303 - 5685304 - 5685305 - Fax: 5682750
[Link]
SC-CER96940
7. Ingreso de las coordenadas de los centros de masa
for i=1:n
masax=input('x:','s');
cmasa(1,i)=str2sym(masax);
masay=input('y:','s');
cmasa(2,i)=str2sym(masay);
masaz=input('z:','s');
cmasa(3,i)=str2sym(masaz);
cmasa(4,i)=1;
end
se recorre un for que empieza de 1 hasta n que es el número de grados de libertad del
manipulador, se pedirá cada coordenada de los centros de masa de las articulaciones, y
se guardara en una lista en esta lista se guardará las coordenadas de cada articulación,
cmasa(1,i)=str2sym(masax), esta línea significa que se guardara en la posición 1,i la
coordenada de la masa x, si i vale 1 significa que se guardara en la posición de la fila 1
columna 1, lo mismo seria con las demás líneas pero cambiando las filas, por lo tanto las
coordenadas de los centros de masa de la articulación 1 se guardara en la columna 1, y
las coordenadas de la articulación 2 se guardara en la columna 2
8. Ingreso del vector Gravedad
fprintf('Ingreso del vector Gravedad, usar g 0 -g para indicar en que eje y sentido apunta
la gravedad\n')
grav=input('gx: ','s');
grx=str2sym(grav);
grav=input('gy: ','s');
gry=str2sym(grav);
grav=input('gz: ','s');
grz=str2sym(grav);
gravedad=[grx,gry,grz,0];
se pide las coordenadas de la gravedad, se guarda en una variable grav y esta variable
después se guardara ya sea en grx, gry o grz dependiendo de la coordenada, la variable
grav, es una cadena de caracteres que se transforma en sym para poder operar.
Una vez ingresado cada coordenada de la gravedad, se guardara en un vector
gravedad.
“Formando líderes para la construcción de un nuevo país en paz”
Universidad de Pamplona
Pamplona - Norte de Santander - Colombia 6
Tels: (7) 5685303 - 5685304 - 5685305 - Fax: 5682750
[Link]
SC-CER96940
9. Ingreso de los parámetros de Denavit Hartenberg
fprintf('Parametros DH\n')
fprintf('Los angulos deben ser ingresados en radianes ya que el calculo es simbolico')
for i=1:n
thetai=input('theta: ','s');
matrizdh(i,1)=str2sym(thetai);
di=input('d: ','s');
matrizdh(i,2)=str2sym(di);
ai=input('a: ','s');
matrizdh(i,3)=str2sym(ai);
alfai=input('alfa: ','s');
matrizdh(i,4)=str2sym(alfai);
end
se tiene un Ford que recorrerá de 1 a n, n son los números de grados de libertad del
manipulador
en estas líneas de código se crea la tabla de la matriz D-H
Primeramente se guardara los parametros de la primera articulación cuando i vale 1, se
observa que en matrizdh(i,1), matrizdh(i,2), etc, lo que cambia es la posición de la
columna por lo que la matriz que se crea, simula la tabla anterior, cuando i vale 2, se
simulara la fila dos de la tabla anterior
%Calculo de la MTH
for i=1:n
if i==1
MTH=denavit(matrizdh(i,1),matrizdh(i,2),matrizdh(i,3),matrizdh(i,4));
MMTH(:,:,i)=MTH
else
MTH=denavit(matrizdh(i,1),matrizdh(i,2),matrizdh(i,3),matrizdh(i,4));
MMTH(:,:,i)=MMTH(:,:,i-1)*MTH
end
end
se envia a una función los parámetros de la articulación correspondiente, para después
realizar el calculo de la Matriz Denavit Hartenbertg.
“Formando líderes para la construcción de un nuevo país en paz”
Universidad de Pamplona
Pamplona - Norte de Santander - Colombia 7
Tels: (7) 5685303 - 5685304 - 5685305 - Fax: 5682750
[Link]
SC-CER96940
-
if i==1
MTH=denavit(matrizdh(i,1),matrizdh(i,2),matrizdh(i,3),matrizdh(i,4));
MMTH(:,:,i)=MTH
En esta parte del código entra cuando i es igual a 1, se hace el cálculo de la matriz de
0 A , después esta matriz se guardará en una lista en este caso se guardar en la posición
1
1 de la lista,
else
MTH=denavit(matrizdh(i,1),matrizdh(i,2),matrizdh(i,3),matrizdh(i,4));
MMTH(:,:,i)=MMTH(:,:,i-1)*MTH
end
cuando i es mayor que 1 entra en el else se hace el cálculo de la matriz 1 A , después se
2
multiplica la matriz que esta en la lista en este caso es la matriz 0 A y se multiplica por la
1
matriz 1 A , así obtenemos la matriz 0 A , esta matriz se guardara en la siguiente posición
2 2
de la lista en este caso como i ahora es dos se ubicara en la posición 2 de la lista
function Axi = denavit(theta,d,a,alfa)
nx=simplify(cos(theta));
ny=simplify(sin(theta));
nz=0;
ox=simplify(-(cos(alfa)*sin(theta)));
oy=simplify(cos(alfa)*cos(theta));
oz=simplify(sin(alfa));
ax=simplify(sin(alfa)*sin(theta));
ay=simplify(-(sin(alfa)*cos(theta)));
az=simplify(cos(alfa));
px=simplify(a*cos(theta));
py=simplify(a*sin(theta));
pz=simplify(d);
Axi=[nx,ox,ax,px;ny,oy,ay,py;nz,oz,az,pz;0,0,0,1];
end
“Formando líderes para la construcción de un nuevo país en paz”
Universidad de Pamplona
Pamplona - Norte de Santander - Colombia 8
Tels: (7) 5685303 - 5685304 - 5685305 - Fax: 5682750
[Link]
SC-CER96940
Regla 3
for i=1:n
for j=1:n
U=MMTH(:,:,i);
dU=diff(U,q(j));
Uij(:,:,i,j)=dU;
end
end
en estas líneas se crean las matrizes U ij , que tiene dos for, un for mas externo
que es la variable i y un for interno que representa la variable j, ambos for
empiezan en 1 y terminan hasta n, necesitamos resolver la siguiente formula
0
∂ Ai
U ij = , esta fórmula consiste en obtener la derivada parcial de la matriz 0 A
∂q
j
i
respecto q j , por lo que se derivara cada matriz denavih calculado anteriormente
con respecto a cada articulación, cada matriz derivada se guardara en una lista
de matrices
Regla 4
for i=1:n
for j=1:n
for k=1:n
dU=Uij(:,:,i,j);
ddU=diff(dU,q(k));
Uijk(:,:,i,j,k)=ddU;
end
end
end
este paso es parecido al anterior, pero en este caso se tiene 3 for anidados, arranca en
∂U ij
1 y llegan a n, respondiendo a la siguiente formula U ijk = , se usara los resultados
∂ qk
de la regla anterior, que se guardaron en la lista Uij, estas matrices se derivaran con
“Formando líderes para la construcción de un nuevo país en paz”
Universidad de Pamplona
Pamplona - Norte de Santander - Colombia 9
Tels: (7) 5685303 - 5685304 - 5685305 - Fax: 5682750
[Link]
SC-CER96940
respecto a cada articulación y se guardara en otra lista, todo eso gracias a los for
anidados
Regla 5
for i=1:n
J(1,1)=((cmasa(1,i))^2)*(m(i));
J(1,2)=(cmasa(1,i))*(cmasa(2,i))*(m(i));
J(1,3)=(cmasa(1,i))*(cmasa(3,i))*(m(i));
J(1,4)=(cmasa(1,i))*(m(i));
J(2,1)=(cmasa(1,i))*(cmasa(2,i))*(m(i));
J(2,2)=((cmasa(2,i))^2)*(m(i));
J(2,3)=(cmasa(2,i))*(cmasa(3,i))*(m(i));
J(2,4)=(cmasa(2,i))*(m(i));
J(3,1)=(cmasa(3,i))*(cmasa(1,i))*(m(i));
J(3,2)=(cmasa(3,i))*(cmasa(2,i))*(m(i));
J(3,3)=((cmasa(3,i))^2)*(m(i));
J(3,4)=(cmasa(3,i))*(m(i));
J(4,1)=(cmasa(1,i))*(m(i));
J(4,2)=(cmasa(2,i))*(m(i));
J(4,3)=(cmasa(3,i))*(m(i));
J(4,4)=m(i);
Ji(:,:,i)=J;
end
En esta regla se busca las matrices de pseudoincercias
“Formando líderes para la construcción de un nuevo país en paz”
Universidad de Pamplona
Pamplona - Norte de Santander - Colombia 10
Tels: (7) 5685303 - 5685304 - 5685305 - Fax: 5682750
[Link]
SC-CER96940
Como se integra con respecto a m, se quitará las integrales y simplemente se
multiplicara cada coordenada del centro de masas por m
Se tendrá J i para cada barra i
Lo que se realiza en las líneas anteriores es la creación de la matriz que observamos en
la matriz pseudoinercia y se guardara en una lista
Regla 6
Obtenemos la matriz de inercia D cuyos elementos están definidos por
for i=1:n
for j=1:n
if i==j
l=i;
elseif i>j
l=i;
else
l=j;
end
sumatoria=0;
for k=1:n
sumatoria=(trace((Uij(:,:,k,j))*(Ji(:,:,k))*(Uij(:,:,k,i).')))+(sumatoria);
end
dij(:,:,i,j)=simplify(sumatoria);
end
end
for i=1:n
for j=1:n
D(i,j)=dij(:,:,i,j);
“Formando líderes para la construcción de un nuevo país en paz”
Universidad de Pamplona
Pamplona - Norte de Santander - Colombia 11
Tels: (7) 5685303 - 5685304 - 5685305 - Fax: 5682750
[Link]
SC-CER96940
end
end
i y j empiezan en 1, y k será el valor máximo existente entre estos dos, por eso en las
líneas anteriores existen condicionales esto se hace para saber que valor toma k
comparando i y j, y el valor que sea mayor será el valor de k, una vez sabido el valor de
k simplemente se resuelve la sumatoria hasta que k valga dos, después cambiara el
valor de j se volverá a compara y asi sucesivamente, estas matrices se guardan en una
lista, para posteriormente guardarla en una matriz D
Regla 7
Se obtiene los terminos defidos por
for i=1:n
for k=1:n
for l=1:n
if i==k
if i==l
h=i;
elseif i>l
h=i;
else
h=l;
end
elseif i>k
if i==l
h=i;
elseif i>l
h=i;
else
h=l;
“Formando líderes para la construcción de un nuevo país en paz”
Universidad de Pamplona
Pamplona - Norte de Santander - Colombia 12
Tels: (7) 5685303 - 5685304 - 5685305 - Fax: 5682750
[Link]
SC-CER96940
end
else
if k==l
h=k;
elseif k>l
h=k;
else
h=l;
end
end
sum=0;
for j=h:n
sum=(trace((Uijk(:,:,j,k,l))*(Ji(:,:,j))*(Uij(:,:,j,i).')))+(sum);
end
hikl(:,:,i,k,l)=simplify(sum);
end
end
end
en este caso es muy similar al anterior la diferencia es que se utilizara las matrices Uijk
halladas anteriormente, al contar con tres variables necesitaremos 3 for mas 1 extra que
nos pide la formula para poder hacer la sumatoria, i, k, l, comenzaran en 1, depues se
harán comparaciones para obtener el valor de k, recordemos que el valor de k es el
mayor de los 3 (i, k, l), se realiza la formula planteada y los resultados se guardan en
una lista .
Regla 8
Obtener el vector columna H de fuerzas de Coriolis y centrigufas cuyos elementos son
“Formando líderes para la construcción de un nuevo país en paz”
Universidad de Pamplona
Pamplona - Norte de Santander - Colombia 13
Tels: (7) 5685303 - 5685304 - 5685305 - Fax: 5682750
[Link]
SC-CER96940
for i=1:n
suma=0;
for k=1:n
for l=1:n
suma=((hikl(:,:,i,k,l))*(dq(k))*(dq(l)))+(suma);
end
end
hi(i)=simplify(suma);
end
for i=1:n
H(i,1)=hi(i);
end
como necesitamos hikl , hallados anteriormente, necesitaremos de 3 for anidados, se
resuelve la formula y se guarda en un vector y este vector se usara para formar la matriz
H
Regla 9
Obtener el vector columna C de fuerzas de gravedad, cuyos elementos son
for i=1:n
sumat=0;
for j=1:n
sumat=(-(m(j))*(gravedad)*(Uij(:,:,j,i))*(cmasa(1:4,j)))+(sumat);
end
c(i)=simplify(sumat);
end
for i=1:n
C(i,1)=c(i);
end
“Formando líderes para la construcción de un nuevo país en paz”
Universidad de Pamplona
Pamplona - Norte de Santander - Colombia 14
Tels: (7) 5685303 - 5685304 - 5685305 - Fax: 5682750
[Link]
SC-CER96940
Tenemos que la formula tiene dos variables i y j, por lo que se necesitara 2 for además
la sumatoria depende de j
Se realiza la operación con las matrices que se necesitan y se guardan en un vector
para posteriormente usarlo para formar la matriz C
Resultado buscado
Resultado obtenido
“Formando líderes para la construcción de un nuevo país en paz”
Universidad de Pamplona
Pamplona - Norte de Santander - Colombia 15
Tels: (7) 5685303 - 5685304 - 5685305 - Fax: 5682750
[Link]
SC-CER96940