INSTITUTO TECNOLÓGICO DE LA LAGUNA Departamento de Estudios de Posgrado e Investigación Maestría en Ingeniería Eléctrica Línea de investigación: Mecatrónica y control Semestre: Primero Nombre de la materia: Robótica Nombre del profesor: Dr. José Alfonso Pámanes García Tarea 1: Simulación en Matlab del Modelo Cinemático Directo del robot Motoman ES280D Nombre del alumno: Pedro Zúñiga Flores Número de control: M07131483 Fecha de entrega: 30/mar/2012 ENERO – JUNIO 2012 TORREÓN, COAH.
This document is posted to help you gain knowledge. Please leave a comment to let me know what you think about it! Share it to your friends and learn new things together.
Transcript
INSTITUTO TECNOLÓGICO DE LA LAGUNA
Departamento de Estudios de Posgrado e Investigación
Maestría en Ingeniería Eléctrica
Línea de investigación: Mecatrónica y control
Semestre: Primero
Nombre de la materia: Robótica Nombre del profesor: Dr. José Alfonso Pámanes García
Tarea 1:
Simulación en Matlab del Modelo Cinemático Directo del robot Motoman ES280D
Nombre del alumno: Pedro Zúñiga Flores Número de control: M07131483 Fecha de entrega: 30/mar/2012
ENERO – JUNIO 2012 TORREÓN, COAH.
OBJETIVO
Obtener los siguientes puntos:
1- Esquema cinemático
2- PDH modificados
3- Matrices elementales
4- Ecuaciones del MCD de posición (X06, Y06, Z06, α, β, γ)
5- Modelo del robot para animación
6- Efectuar la simulación para las siguientes trayectorias articulares:
𝜃𝑖 = 𝜃𝑖 𝑖𝑛𝑖+ 𝛿𝜃𝑖 ∙ 𝑓𝜃 𝑡 𝑝𝑎𝑟𝑎 𝑖 = 1, 2, ⋯ , 𝑛
INTRODUCCIÓN
El modelo cinemático directo de posición (MCDP) consiste en el establecimiento de las
funciones que hacen posible determinar las coordenadas operacionales de un robot a partir
de sus coordenadas articulares. En el caso que se presentará en este estudio, se analizará el
MCDP de un robot manipulador de 6 grados de libertad (6 gdl) con el cual se desarrollará
una animación en Matlab para ejecutar varias trayectorias articulares del robot dentro de
sus límites articulares dados por el fabricante. Este robot del cual se habla, se trata del
Motoman ES280D, que es un manipulador serial de 6 gdl que está diseñado para el manejo
de materiales no mayores de 280 kg con un rango máximo de trabajo de 2.446 m. A
continuación en la Figura 1 se presenta un diagrama del espacio de trabajo de este robot
donde se representa el alcance de operación del Motoman ES280D además de sus
dimensiones físicas y algunos de sus límites articulares.
Figura 1. Espacio de trabajo de Motoman ES280D y configuración inicial.
En la siguiente Tabla 1 se especifican los límites articulares representados por letras
mayúsculas en la Figura 1.
Ejes Rango máximo de movimiento (grados)
S ± 180
L +76/-60
U +230/-142.5
R ± 360
B ±125
T ± 360
Tabla 1. Límites articulares
1. Esquema cinemático
El esquema cinemático del robot Motoman ES280D se obtendrá a partir del siguiente
diagrama de alambre de la Figura 2.
Figura 2. Diagrama de alambre del robot Motoman ES280D.
Las distancias del diagrama de alambre son las que se muestran en la Tabla 2, las cuales
son extraídas de la Figura 1.
Longitudes Unidades (cm)
L1 65
L2 28.5
L3 115
L4 25
L5 101.5
L6 25
Tabla 2. Longitudes del robot.
Para obtener el esquema cinemático a partir del diagrama de alambre, se seguirá el método
convencional de asignación de los ejes (x, y, z) para cada eslabón de la cadena cinemática
del robot. Este método convencional se describe a continuación:
El eje zj se define a lo largo del eje de la articulación aj. Este eje es el de rotación en
el caso de una articulación tipo R (rotatoria), o el de deslizamiento de la corredera
en el caso de una articulación tipo P. En este último caso la posición de zj se fija
arbitrariamente.
El eje xj se define a lo largo de la perpendicular común a zj y a zj-1. Si estos dos ejes
son paralelos, xj no se puede definir de manera única; en tal caso se recomienda
seguir un criterio de simetría o de simplicidad para definir la ubicación de este eje.
Por supuesto, el punto de intersección de xj y zj definen al origen oj del marco.
El eje yj se define a partir de los ejes xj y zj, de tal manera que se complete un marco
de mano derecha.
Nótese que mediante esta convención no es posible definir los marcos unidos a los
eslabones C0 y Cn (eslabón fijo de la base y eslabón correspondiente a la ultima
articulación). Tales marcos se pueden especificar de manera simple como sigue: el marco
∑0 se escoge de tal manera que este alineado con el ∑1 cuando la variable articular q1 sea
nula; por su parte, el marco ∑n se elige de modo que este alineado con el ∑n-1 cuando qn=0.
A continuación se presentan los marcos asignados al diagrama de alambre en la Figura 3.
Figura 3. Esquema cinemático del manipulador Motoman ES280D.
Los marcos con subíndices 0, 1, 2, 3, 4, 5 y 6 son los que se obtuvieron siguiendo el
método convencional para la ubicación de los ejes (x, y, z) en cada eslabón de la Figura 3.
Los marcos con subíndices ‘a’ y ’h’ se colocaron arbitrariamente en una posición (x, y, z)
de la cadena cinemática del robot y están referenciados al marco predecesor, ya que tienen
la misma orientación que el marco que los antecede solo que una distancia de separación en
alguno de sus ejes con respecto al marco anterior. A estos marcos Oa y Oh les llamaremos
marcos auxiliares ya que se utilizarán más adelante en el modelo de animación del robot en
Matlab.
Una vez que se obtuvo el esquema cinemático del manipulador Motoman ES280D, se
procederá a obtener los parámetros de Denavit-Hartenberg Modificados (PDHM) tomando
como referencia la Figura 3.
2. Parámetros de Denavit-Hartenberg Modificados (PDHM).
Una vez asignado un marco a cada eslabón, se definirán los parámetros geométricos que
especifican la posición y orientación de cada marco respecto al precedente. Para esto se
seguirán las siguientes especificaciones de dichos parámetros:
αj Es el ángulo de zj-1 a zj, medido respecto a xj-1 conforme a la regla de la
mano derecha.
dj Es la distancia entre zj-1 y zj, a lo largo de xj-1.
θj Es el ángulo de xj-1 a xj, medido respecto a zj conforme a la regla de la mano
derecha.
rj Es la distancia entre xj-1 y xj a lo largo de zj.
σj Esta variable vale cero si la articulación j es tipo R (rotacional) y vale uno si
la articulación j es de tipo P (prismática).
Los PDHM obtenidos de la Figura 3 son los que se muestran en la Tabla 3.
j σj αj (grados) dj θj rj
1 0 0 0 θ1 0
2 0 90 d2 θ2 0
3 0 0 d3 θ3 0
4 0 90 d4 θ4 r4
5 0 -90 0 θ5 0
6 0 90 0 θ6 0
Tabla 3. Parámetros de Denavit Hartenberg Modificados (PDHM).
Donde las distancias d2, d3 y d4 corresponden a las longitudes L2, L3 y L4 respectivamente,
y la distancia r4 corresponde a la longitud L5, según las Figuras 2 y 3.
Una vez que se obtuvieron los PDHM del esquema cinemático del manipulador Motoman
ES280D, se procederá a obtener las matrices elementales del robot.
3. Matrices elementales.
Para obtener las matrices elementales del robot Motoman ES280D, haremos uso de la
matriz de transformación homogénea general que defina, en función de los PDHM, la
posición y orientación del marco ∑j respecto al ∑j-1. Esta matriz es la siguiente:
𝑇𝑗𝑗−1
=
𝑐𝜃𝑗𝑐𝛼𝑗 𝑠𝜃𝑗𝑠𝛼𝑗 𝑠𝜃𝑗
−𝑠𝜃𝑗𝑐𝛼𝑗 𝑐𝜃𝑗𝑠𝛼𝑗 𝑐𝜃𝑗
0 0
0−𝑠𝛼𝑗
𝑐𝛼𝑗
𝑑𝑗
−𝑟𝑗 𝑠𝛼𝑗
𝑟𝑗 𝑐𝛼𝑗
0 1
Nota: Por simplicidad se denota a la función seno con la letra ‘s’ y a la función coseno con
la letra ‘c’ en la matriz anterior.
Sustituyendo en la matriz anterior los PDHM de la Tabla 3, pertenecientes a cada eslabón
del robot, se obtienen las siguientes seis matrices de transformación homogéneas, las cuales
son llamadas matrices elementales del robot.
𝑇10 =
𝑐1
𝑠1
0
−𝑠1
𝑐1
00 0
001
000
0 1
𝑇21 =
𝑐2
0𝑠2
−𝑠2
0𝑐2
0 0
0−10
𝑑2
00
0 1
𝑇32 =
𝑐3
𝑠3
0
−𝑠3
𝑐3
00 0
001
𝑑3
00
0 1
𝑇43 =
𝑐4
0𝑠4
−𝑠4
0𝑐4
0 0
0−10
𝑑4
−𝑟4
00 1
𝑇54 =
𝑐5
0−𝑠5
−𝑠5
0−𝑐5
0 0
010
000
0 1
𝑇65 =
𝑐6
0𝑠6
−𝑠6
0𝑐6
0 0
0−10
000
0 1
Nota: Las matrices elementales están escritas de manera simplificada, tomando en cuenta la
siguiente notación de la Tabla 4.
s1=sin(θ1)
c1=cos(θ1)
s2=sin(θ2)
c2=cos(θ2)
s3=sin(θ3)
c3=cos(θ3)
s4=sin(θ4)
c4=cos(θ4)
s5=sin(θ5)
c5=cos(θ5)
s6=sin(θ6)
c6=cos(θ6)
Tabla 4. Notación de las funciones seno y coseno.
Una vez obtenidas las matrices elementales del robot Motoman ES280D, se procederá a
obtener una matriz de transformación homogénea que defina la posición y orientación del
marco ∑6 con respecto al marco de la base del robot ∑0. Esto para obtener las ecuaciones
del Modelo Cinemático Directo de posición.
4. Ecuaciones del MCD de posición (X06, Y06, Z06, α, β, γ)
La matriz que define las ecuaciones del MCD de posición es la resultante de multiplicar
matricialmente las matrices elementales del robot como se muestra a continuación:
𝑇 = 𝑇10 ∙ 𝑇2
1 ∙ 𝑇32 ∙ 𝑇4
3 ∙ 𝑇54 ∙ 𝑇6
560
Por la complejidad de las operaciones se decide hacer las multiplicaciones matriciales en
MATLAB, y el resultado se dividió en las 12 ecuaciones que definen la posición y
orientación del marco ∑6 con respecto al marco ∑0, y que forman parte de los elementos de
A partir de la igualación anterior se pueden despejar los ángulos de Euler de las ecuaciones
de ambas matrices como se muestra a continuación:
𝛼 = 𝑎𝑡𝑎𝑛2 𝑇21 ,𝑇11
𝛽 = 𝑎𝑡𝑎𝑛2 −𝑇31 ,𝑇11
𝑐𝛼
𝛾 = 𝑎𝑡𝑎𝑛2(𝑇32 ,𝑇33)
Estos tres ángulos de Euler anteriores, son los que me definen las rotaciones sucesivas (en
el orden z, y, x) del marco∑6 con respecto al marco ∑0.
Y como ya se dijo anteriormente las ecuaciones T14, T24 y T34 son las que me definen el
vector de posición del último marco ∑6 con respecto al marco ∑0.
𝑥06 𝑦06 𝑧06 ′ = 𝑇14 𝑇24 𝑇34 ′
Las últimas 4 ecuaciones arriba descritas conforman el MCDP del robot Motoman
ES280D. El siguiente punto a tratar es sobre el modelo del robot para animación en Matlab.
5. Modelo del robot para animación.
Para la animación del robot Motoman ES280D en Matlab se considerarán los marcos
auxiliares ∑a y ∑h que se colocaron arbitrariamente en la Figura 3 del esquema cinemático
del robot. Las matrices de transformación homogéneas para estos marcos con respecto a su
marco predecesor son las siguientes:
𝑇 =𝑎3
100
010
0 0
001
𝑑4
00
0 1
𝑇 =ℎ6
100
010
0 0
001
00𝑟6
0 1
Nótese que las 2 matrices de transformación homogéneas anteriores poseen una submatriz
identidad correspondiente a la matriz de rotación de cada una, debido a que los marcos
auxiliares ∑a y ∑h tienen la misma orientación que los marcos ∑3 y ∑6 respectivamente,
solo que los marcos auxiliares están desplazados una distancia en un eje con respecto a su
marco predecesor como se ve en sus matrices de transformación. Las distancias d4 y r6
corresponden a las longitudes L4 y L6 respectivamente de la Figura 2.
Para la graficación de la ruta del centro de la herramienta de la Figura 3, se considerará un
marco ∑ch en el centro de la herramienta, el cual posee la siguiente matriz de
transformación con respecto a su marco predecesor (∑h).
𝑇 =𝑐ℎℎ
100
010
0 0
001
004
0 1
En la siguiente página se presenta el código de simulación escrito en un archivo Script de
Matlab, en el cual se describe detalladamente las instrucciones para realizar el modelo de
animación del robot.
% Programa que efectúa la animación del robot Motoman ES280D % y muestra la ruta que describe el órgano terminal % Programa realizado por J. Alfonso Pámanes García % Última actualización: Mar 27/2012
% Parámetros geométricos del robot: d2=28.5;%Igual a L2 d3=115;%Igual a L3 d4=25;%Igual a L4 r4=101.5;%Igual a L5 r6=25;%Igual a L6 rch=4;%distancia del inicio de la herramienta al centro de la herramienta
% Emplazamiento del robot: POSICION Y ORIENTACION DE LOS MARCOS CON % RESPECTO LA ESTACION DE TRABAJO (EL MARCO DEL MUNDO DEL MATLAB) aa=0; bb=0; cc=0; %Matriz de emplazamiento Te0=[0 -1 0 aa 1 0 0 bb 0 0 1 cc 0 0 0 1 ];
% Parámetros de graficación: np=50;
% Parámetros del movimiento del robot:
T=5;
% Configuracion inicial del robot segun el fabricante: th1i=0;%ANGULOS INICIALES EN CADA ARTICULACION th2i=pi/2; th3i=0; th4i=0; th5i=0; th6i=0;
delth1g=180;%ANGULOS EN GRADOS A INCREMENTAR EN CADA ARTICULACION delth2g=0; delth3g=0; delth4g=0; delth5g=0; delth6g=0;
delth1=delth1g*pi/180;%Conversion de grados a radianes delth2=delth2g*pi/180; delth3=delth3g*pi/180; delth4=delth4g*pi/180; delth5=delth5g*pi/180; delth6=delth6g*pi/180;
for i=0:np t=T*i/np;%Ecuacion de funcion del tiempo funt = (t/T)-(1/(2*pi))*(sin(2*pi*t/T)); th1 = th1i+delth1*funt; th2 = th2i+delth2*funt; th3 = th3i+delth3*funt; th4 = th4i+delth4*funt; th5 = th5i+delth5*funt; th6 = th6i+delth6*funt; th12 = th1+th2; th123 = th12+th3; th1234= th123+th4; th12345= th1234+th5; th123456= th12345+th6;
rpost=Te0*[xch ych zch 1]';%para obtener las cordenadas del vector
de posicion % del centro de la herramienta respecto al
marco de estacion de trabajo xpn=rpost(1); ypn=rpost(2); zpn=rpost(3);
pn1(i+1)=xpn;%SE UTILIZAN PARA DIBUJAR LA TRAZA DE LA RUTA DE LA
PINZA pn2(i+1)=ypn; pn3(i+1)=zpn;
% figure (1) %Para ubicar el vector de posicion de cada marco con respecto %a la base del robot x2=d2*c1;%Coordenadas de marco 2 en marco cero y2=d2*s1;%T02=T01*T12 z2=0;% x3=d2*c1+d3*c1*c2;%Coordenadas de marco 3 en marco cero y3=d2*s1+d3*s1*c2;%T03=T02*T23 z3=d3*s2;% xa=c1*(d2 + d4*c23 + d3*c2);%Coordenadas de marco a en marco cero ya=s1*(d2 + d4*c23 + d3*c2);%T0a=T03*T3a za=d4*s23 + d3*s2;% x4=c1*(d2 + d4*c23 + r4*s23 + d3*c2);%Coordenadas de marco 6 en marco
rposp2=Te0*[x2 y2 z2 1]'; rposp3=Te0*[x3 y3 z3 1]'; rpospa=Te0*[xa ya za 1]'; rposp4=Te0*[x4 y4 z4 1]'; rposp4a=Te0*[x4 y4 z4 1]';%Para dibujar un circulo en marco 4, 5 y
sp1=[0 0 0 1]';%Longitud L1 sp2=[0 0 -65 1]';% b1=[-20 20 -65 1]';%4 puntos de la base del robot b2=[ 20 20 -65 1]'; b3=[ 20 -20 -65 1]'; b4=[-20 -20 -65 1]';
s1n=Te0*sp1;%Se multiplican por la matriz de emplazamiento para en caso
de s2n=Te0*sp2;%querer mover la base del robot de lugar con referencia a la b1n=Te0*b1;%estacion de trabajo que es el marco o mundo de Matlab b2n=Te0*b2; b3n=Te0*b3; b4n=Te0*b4;
%COMPONENTES X,Y,Z DEL PUNTO INICIAL Y PUNTO FINAL DE LA LINEA DE SOPORTE s1x=[s1n(1) s2n(1)];%Longitud L1 s1y=[s1n(2) s2n(2)]; s1z=[s1n(3) s2n(3)]; %4 LINEAS DE LA BASE b1x=[b1n(1) b2n(1)];%Lineas de la base b1y=[b1n(2) b2n(2)]; b1z=[b1n(3) b2n(3)];
% Marco e: MARCO DEL MUNDO DE MATLAB O ESTACION DE TRABAJO e1x=[0 5]; e1y=[0 0]; e1z=[0 0];
e2x=[0 0]; e2y=[0 5];
e2z=[0 0];
e3x=[0 0]; e3y=[0 0]; e3z=[0 5];
%Matriz de transformacion del marco cero a la herramienta %T0h=T01*T12*T23*T34*T45*T56*T6h T0h = [ s6*(c4*s1 + s4*(c1*s2*s3 - c1*c2*c3)) + c6*(c5*(s1*s4 -
figure(1) clf % Esta instrucción limpia el espacio de graficación %hold on % Graficación de la ruta del OT: for j=2:i %j=2 para que existan 2 iteraciones realizadas plot3([pn1(j-1),pn1(j)],[pn2(j-1),pn2(j)],[pn3(j-1),pn3(j)]); hold on% Esta instrucción permite retener la traza de la ruta % En cada iteración, la ruta se dibuja desde el primer % punto hasta el actual end
% Graficación del robot en la configuración actual (iteración i):
grid on plot3(e1x,e1y,e1z,'m',e2x,e2y,e2z,'g',e3x,e3y,e3z,'c') plot3(b1x,b1y,b1z,'r',b2x,b2y,b2z,'r',b3x,b3y,b3z,'r',b4x,b4y,b4z,'r') plot3(s1x,s1y,s1z,'r','MarkerSize',5) plot3(r2x,r2y,r2z,'k',r3x,r3y,r3z,'k-