Post on 24-Oct-2015
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
renglones y columnas de la matriz 𝑇60 .
T11=s6*(c4*s1 + s4*(c1*s2*s3 - c1*c2*c3)) + c6*(c5*(s1*s4 - c4*(c1*s2*s3 - c1*c2*c3)) + s5*(c1*c2*s3 + c1*c3*s2))
T12=c6*(c4*s1 + s4*(c1*s2*s3 - c1*c2*c3)) - s6*(c5*(s1*s4 - c4*(c1*s2*s3 - c1*c2*c3)) + s5*(c1*c2*s3 + c1*c3*s2))
T13=c5*(c1*c2*s3 + c1*c3*s2) - s5*(s1*s4 - c4*(c1*s2*s3 - c1*c2*c3))
T14= c1*(d2 + d4*c23 + r4*s23 + d3*c2)
T21=- s6*(c1*c4 - s4*(s1*s2*s3 - c2*c3*s1)) - c6*(c5*(c1*s4 + c4*(s1*s2*s3 - c2*c3*s1)) - s5*(c2*s1*s3 + c3*s1*s2))
T22=s6*(c5*(c1*s4 + c4*(s1*s2*s3 - c2*c3*s1)) - s5*(c2*s1*s3 + c3*s1*s2)) - c6*(c1*c4 - s4*(s1*s2*s3 - c2*c3*s1))
T23=s5*(c1*s4 + c4*(s1*s2*s3 - c2*c3*s1)) + c5*(c2*s1*s3 + c3*s1*s2)
T24=s1*(d2 + d4*c23 + r4*s23 + d3*c2)
T31=- c6*(s5*(c2*c3 - s2*s3) - c4*c5*(c2*s3 + c3*s2)) - s4*s6*(c2*s3 + c3*s2)
T32=s6*(s5*(c2*c3 - s2*s3) - c4*c5*(c2*s3 + c3*s2)) - c6*s4*(c2*s3 + c3*s2)
T33= - c5*(c2*c3 - s2*s3) - c4*s5*(c2*s3 + c3*s2)
T34= d4*s23 - r4*c23 + d3*s2
En estas 12 ecuaciones anteriores de T11 a T34, se sigue respetando la notación de la Tabla
4, tomando en cuenta además la siguiente notación:
s23=sin(θ2+ θ3)
c23=cos(θ2+ θ3)
Las ecuaciones que definen al MCD de posición son las expresadas en las ecuaciones T14,
T24 y T34 para el vector de posición, mas las ecuaciones que ayudan a definir a los ángulos
α, β y γ (alpha, beta y gamma) para obtener la matriz de orientación siguiendo la
convención de ángulos de Euler.
Los ángulos de Euler corresponden a 3 rotaciones sucesivas de los tres ejes en el orden z, y,
x, llamadas alpha, beta y gamma, que necesitan aplicarse al marco ∑0 (marco fijo de la
base) para hacerlo coincidir con el marco del ultimo eslabón ∑6.
Estos 3 ángulos de Euler los obtendremos igualando la matriz de orientación del MCDP
con la matriz de ángulos de Euler que a continuación se presenta:
𝑐𝛼𝑐𝛽 𝑐𝛼𝑠𝛽𝑠𝛾 − 𝑠𝛼𝑐𝛾 𝑐𝛼𝑠𝛽𝑐𝛾 − 𝑠𝛼𝑠𝛾𝑠𝛼𝑐𝛽 𝑠𝛼𝑠𝛽𝑠𝛾 + 𝑐𝛼𝑐𝛾 𝑠𝛼𝑠𝛽𝑐𝛾 − 𝑐𝛼𝑠𝛾−𝑠𝛽 𝑐𝛽𝑠𝛾 𝑐𝛽𝑐𝛾
Y al igualar la matriz de ángulos de Euler con la matriz de rotación del MCDP, se obtiene:
𝑇11 𝑇12 𝑇13𝑇21 𝑇22 𝑇23𝑇31 𝑇32 𝑇33
=
𝑐𝛼𝑐𝛽 𝑐𝛼𝑠𝛽𝑠𝛾 − 𝑠𝛼𝑐𝛾 𝑐𝛼𝑠𝛽𝑐𝛾 − 𝑠𝛼𝑠𝛾𝑠𝛼𝑐𝛽 𝑠𝛼𝑠𝛽𝑠𝛾 + 𝑐𝛼𝑐𝛾 𝑠𝛼𝑠𝛽𝑐𝛾 − 𝑐𝛼𝑠𝛾−𝑠𝛽 𝑐𝛽𝑠𝛾 𝑐𝛽𝑐𝛾
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;
%%%%%%%%%%%%%%%%%%%%%%%%%%% %Notacion matematica a utilizar s1=sin(th1); c1=cos(th1); s2=sin(th2); c2=cos(th2); s3=sin(th3); c3=cos(th3); s4=sin(th4); c4=cos(th4); s5=sin(th5); c5=cos(th5); s6=sin(th6); c6=cos(th6);
s12=sin(th12); c12=cos(th12); c23=cos(th2+th3); s23=sin(th2+th3); s45=sin(th4+th5); s54=sin(th4-th5); s123=sin(th123); c123=cos(th123); s1234=sin(th1234); c1234=cos(th1234);
%xch,ych,zch, son el vector de posicion del centro de la herramienta %con respecto al marco cero de la base. Este vector se obtiene a
partir %de: T0ch=T01*T12*T23*T34*T45*T56*T6h*Thch xch=r6*(s5*(s1*s4 - c4*(c1*s2*s3 - c1*c2*c3)) + c5*(c1*c2*s3 +
c1*c3*s2)) + rch*(s5*(s1*s4 - c4*(c1*s2*s3 - c1*c2*c3)) + c5*(c1*c2*s3 +
c1*c3*s2)) + c1*(d2 + d4*c23 + r4*s23 + d3*c2);
ych=s1*(d2 + d4*c23 + r4*s23 + d3*c2) - rch*(s5*(c1*s4 + c4*(s1*s2*s3
- c2*c3*s1)) - c5*(c2*s1*s3 + c3*s1*s2)) - r6*(s5*(c1*s4 + c4*(s1*s2*s3 -
c2*c3*s1)) - c5*(c2*s1*s3 + c3*s1*s2));
zch=d4*s23 - r4*c23 + d3*s2 - rch*(c5*(c2*c3 - s2*s3) - c4*s5*(c2*s3
+ c3*s2)) + (r6*s23*s5)/2 - r6*c23*c5 - (r6*s54*s23)/2;
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
cero y4=s1*(d2 + d4*c23 + r4*s23 + d3*c2);%T04=T05=T06=T03*T34*T45*T56 z4=d4*s23 - r4*c23 + d3*s2;% xh=c1*(d2 + d4*c23 + r4*s23 + d3*c2) + r6*(s5*(s1*s4 - c4*(c1*s2*s3 -
c1*c2*c3)) + c5*(c1*c2*s3 + c1*c3*s2));%Coordenadas de marco h en marco
cero yh=-r6*(s5*(c1*s4 + c4*(s1*s2*s3 - c2*c3*s1)) - c5*(c2*s1*s3 +
c3*s1*s2)) + s1*(d2 + d4*c23 + r4*s23 + d3*c2);%T0h=T06*T6h zh=d4*s23 - r4*c23 + d3*s2 + (r6*s23*s45)/2 - r6*c23*c5 -
(r6*s54*s23)/2;%
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
6 rposph=Te0*[xh yh zh 1]';
%DIBUJA LINEAS PARA LOS ESLABONES r2x=[aa rposp2(1)];%Longitud L2 r2y=[bb rposp2(2)]; r2z=[cc rposp2(3)];% r3x=[rposp2(1) rposp3(1)];%Longitud L3 r3y=[rposp2(2) rposp3(2)]; r3z=[rposp2(3) rposp3(3)];% rax=[rposp3(1) rpospa(1)];%Longitud L4 ray=[rposp3(2) rpospa(2)]; raz=[rposp3(3) rpospa(3)];% r4x=[rpospa(1) rposp4(1)];%Longitud L5
r4y=[rpospa(2) rposp4(2)]; r4z=[rpospa(3) rposp4(3)];% r4ax=[rposp4(1) rposp4a(1)];%Para dibujar un circulo en marco 4, 5 y
6 r4ay=[rposp4(2) rposp4a(2)]; r4az=[rposp4(3) rposp4a(3)];% rhx=[rposp4a(1) rposph(1)];%Longitud L6 rhy=[rposp4a(2) rposph(2)]; rhz=[rposp4a(3) rposph(3)];%
% Base del robot:
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)];
b2x=[b2n(1) b3n(1)]; b2y=[b2n(2) b3n(2)]; b2z=[b2n(3) b3n(3)];
b3x=[b3n(1) b4n(1)]; b3y=[b3n(2) b4n(2)]; b3z=[b3n(3) b4n(3)];
b4x=[b4n(1) b1n(1)]; b4y=[b4n(2) b1n(2)]; b4z=[b4n(3) b1n(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 -
c4*(c1*s2*s3 - c1*c2*c3)) - s5*(c1*c2*s3 + c1*c3*s2)), c6*(c4*s1 +
s4*(c1*s2*s3 - c1*c2*c3)) - s6*(c5*(s1*s4 - c4*(c1*s2*s3 - c1*c2*c3)) -
s5*(c1*c2*s3 + c1*c3*s2)), s5*(s1*s4 - c4*(c1*s2*s3 - c1*c2*c3)) +
c5*(c1*c2*s3 + c1*c3*s2), r6*(s5*(s1*s4 - c4*(c1*s2*s3 - c1*c2*c3)) +
c5*(c1*c2*s3 + c1*c3*s2)) + c1*(d2 + d4*c23 + r4*s23 + d3*c2); - s6*(c1*c4 - s4*(s1*s2*s3 - c2*c3*s1)) - c6*(c5*(c1*s4 + c4*(s1*s2*s3 -
c2*c3*s1)) + s5*(c2*s1*s3 + c3*s1*s2)), s6*(c5*(c1*s4 + c4*(s1*s2*s3 -
c2*c3*s1)) + s5*(c2*s1*s3 + c3*s1*s2)) - c6*(c1*c4 - s4*(s1*s2*s3 -
c2*c3*s1)), c5*(c2*s1*s3 + c3*s1*s2) - s5*(c1*s4 + c4*(s1*s2*s3 -
c2*c3*s1)), s1*(d2 + d4*c23 + r4*s23 + d3*c2) - r6*(s5*(c1*s4 +
c4*(s1*s2*s3 - c2*c3*s1)) - c5*(c2*s1*s3 + c3*s1*s2)); c6*(s5*(c2*c3 - s2*s3) + c4*c5*(c2*s3 + c3*s2)) - s4*s6*(c2*s3 + c3*s2),
- s6*(s5*(c2*c3 - s2*s3) + c4*c5*(c2*s3 + c3*s2)) - c6*s4*(c2*s3 +
c3*s2), c4*s5*(c2*s3 + c3*s2) - c5*(c2*c3 - s2*s3), d4*s23 - r4*c23 +
d3*s2 + (r6*s23*s45)/2 - r6*c23*c5 - (r6*s54*s23)/2; 0, 0, 0, 1];
p1 = [4 8 0 1]';%Se describen 8 puntos de la pinza z1 = Te0*T0h*p1;
p2 = [4 8 8 1]'; z2 = Te0*T0h*p2;
p3 = [-4 8 0 1]'; z3 = Te0*T0h*p3;
p4 = [-4 8 8 1]'; z4 = Te0*T0h*p4;
p5 = [4 -8 0 1]'; z5 = Te0*T0h*p5;
p6 = [4 -8 8 1]'; z6 = Te0*T0h*p6;
p7 = [-4 -8 0 1]'; z7 = Te0*T0h*p7;
p8 = [-4 -8 8 1]'; z8 = Te0*T0h*p8;
%PARA DIBUJAR 10 LINEAS DE LA PINZA d12x=[z1(1) z2(1)]; d12y=[z1(2) z2(2)]; d12z=[z1(3) z2(3)]; d24x=[z2(1) z4(1)];
d24y=[z2(2) z4(2)]; d24z=[z2(3) z4(3)]; d13x=[z1(1) z3(1)]; d13y=[z1(2) z3(2)]; d13z=[z1(3) z3(3)]; d34x=[z3(1) z4(1)]; d34y=[z3(2) z4(2)]; d34z=[z3(3) z4(3)]; d15x=[z1(1) z5(1)]; d15y=[z1(2) z5(2)]; d15z=[z1(3) z5(3)]; d37x=[z3(1) z7(1)]; d37y=[z3(2) z7(2)]; d37z=[z3(3) z7(3)]; d57x=[z5(1) z7(1)]; d57y=[z5(2) z7(2)]; d57z=[z5(3) z7(3)]; d78x=[z7(1) z8(1)]; d78y=[z7(2) z8(2)]; d78z=[z7(3) z8(3)]; d86x=[z8(1) z6(1)]; d86y=[z8(2) z6(2)]; d86z=[z8(3) z6(3)]; d65x=[z6(1) z5(1)]; d65y=[z6(2) z5(2)]; d65z=[z6(3) z5(3)];
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-
o',rax,ray,raz,'k',r4x,r4y,r4z,'k',r4ax,r4ay,r4az,'k-o',rhx,rhy,rhz,'k')
plot3(d12x,d12y,d12z,d24x,d24y,d24z,d13x,d13y,d13z,d34x,d34y,d34z,d15x,d1
5y,d15z,d37x,d37y,d37z,d57x,d57y,d57z,d78x,d78y,d78z,d86x,d86y,d86z,d65x,
d65y,d65z)
axis([-176,160,-170,200,-12,200]) view([1,1,1])
end
6- Efectuar la simulación para las siguientes trayectorias articulares:
𝜃𝑖 = 𝜃𝑖 𝑖𝑛𝑖+ 𝛿𝜃𝑖 ∙ 𝑓𝜃 𝑡 𝑝𝑎𝑟𝑎 𝑖 = 1, 2, ⋯ , 𝑛
Donde n = 6 (articulaciones) para el robot Motoman ES280D y donde además:
𝑓𝜃 𝑡 =𝑡
𝑇−
1
2 ∙ 𝜋∙ sin
2 ∙ π ∙ t
T
𝜃𝑖 y 𝛿𝑖 se asignan dependiendo de los limites articulares del robot.
A continuación se presenta en la Figura 4 la configuración inicial que especifica el
fabricante para que a partir de la misma se midan los limites articulares especificados en la
Tabla 1.
Figura 4. Configuracion inicial de referencia para límites articulares.
En la Figura 4 anterior el ángulo de θ2 inicial es igual a 90, debido a que no se tomo la
configuración inicial del fabricante para la ubicación de los marcos de referencia de cada
eslabón y posteriormente para la obtención de los PDHM.
-100
0
100
-50
0
50
100
150
200
0
50
100
150
200
Trayectoria 1: 𝜃1 = 0, 𝜃2 = 90 y 𝛿𝜃1 = 180
Trayectoria 2: 𝜃1 = 0, 𝜃2 = 90 y 𝛿𝜃1 = −180
-100
0
100
200
-100
0
100
200
0
50
100
150
200
250
-100
0
100
-100
0
100
200
0
50
100
150
200
Trayectoria 3: 𝜃2 = 0, 𝜃2 = 90 y 𝛿𝜃2 = 76
Trayectoria 4: 𝜃2 = 0, 𝜃2 = 90 y 𝛿𝜃2 = −60
-100
0
100
-100
0
100
200
0
50
100
150
200
-100
0
100
-100
0
100
200
-50
0
50
100
150
200
En las trayectorias anteriores (3 y 4) se reflejó que el sentido del eje z del marco del eslabón
2 asignado en la Figura 3 no corresponde físicamente al del robot Motoman ES280D,
debido a que cuando se le da un ángulo positivo en la articulación 2 el robot en la
simulación gira en sentido contrario. Esto se arregla reasignando los marcos de referencia
de cada eslabón del robot para que los ejes z de cada articulación correspondan en sentido
de giro a los ángulos que el fabricante especifica en la Figura 1.
Trayectoria 5: 𝜃3 = 0, 𝜃2 = 90 y 𝛿𝜃3 = 230
Trayectoria 6: 𝜃3 = 0, 𝜃2 = 90 y 𝛿𝜃3 = −142.5
-100
0
100
-100
0
100
200
-50
0
50
100
150
200
-100
0
100
-100
0
100
200
-50
0
50
100
150
200
Trayectoria 7: 𝜃4 = 0, 𝜃2 = 90 y 𝛿𝜃4 = 360
Trayectoria 8: 𝜃5 = 0, 𝜃2 = 90 y 𝛿𝜃5 = 125
-100
0
100
-100
0
100
200
-50
0
50
100
150
200
-100
-50
0
50
100
-50
0
50
100
150
200
-50
0
50
100
150
Trayectoria 9: 𝜃5 = 0, 𝜃2 = 90 y 𝛿𝜃5 = −125
Trayectoria 9: 𝜃6 = 0, 𝜃2 = 90 y 𝛿𝜃6 = 360
-100
-50
0
50
100
-50
0
50
100
150
200
-50
0
50
100
150
-100
-50
0
50
100
-50
0
50
100
150
200
-50
0
50
100
150
REFERENCIAS BIBLIOGRÁFICAS
Robótica: Introducción al modelado de manipuladores.
Autor: J. Alfonso Pámanes García.