Documentos de Académico
Documentos de Profesional
Documentos de Cultura
MODELADO CINEMTICO DE UN
SISTEMA ROBOTIZADO
1. OBJETIVO DE LA PRCTICA
Con ayuda de la herramienta CAE MATLAB se pretende desarrollar los programas necesarios
para el modelado de un robot real, en este caso un manipulador industrial PRR con mueca en
lnea.
Adems se pretende modelar un entorno industrial. En nuestro caso, el robot estar dedicado a
tareas de pick & place. Supondremos que las piezas llegan por una cinta transportadora pasando
por un control de calidad, por ejemplo por cmara. Si pasan el control de calidad, el robot las
deber llevar a una mesa, dnde seguir la siguiente fase de montaje. En el caso contrario las
debe arrojar en un cubo de basura.
El objetivo de la prctica es conocer, a diferentes niveles, todos los subsistemas necesarios para
el modelado de un sistema robotizado, profundizando en el modelo cinemtico de un brazo
robot.
Con la prctica se pretende conseguir:
3- Familiarizarse con la herramienta CAE Matlab de gran utilidad en Robtica y en otras reas
de Ingeniera de Sistemas y Automtica.
2. MATLAB
La herramienta CAE Matlab es un poderoso entorno para lgebra lineal y representacin
grfica, disponible para una amplia gama de plataformas. La funcionalidad bsica puede
ampliarse con libreras (Toolboxes) especficas de cada campo de aplicacin (sistemas de
control, procesado de seales, visin, redes neuronales, identificacin, robtica, etc.).
En esta prctica no utilizaremos ninguna de las libreras disponibles en el rea de robtica, sino
que desarrollaremos nuestras propias herramientas de modelado a partir de funciones comunes
de Matlab.
Una de las ventajas de Matlab es que permite definir matrices y operar con ellas con facilidad:
>>A=[1234;5678;9101112]
A=
1234
5678
9101112
>>B=A;
//Elpuntoycomaevitaquehayaeco
>>C=A+B
C=
2468
10121416
18202224
>>D=A*B' //AporBtraspuesta
D=
3070110
70174278
110278446
El acceso a los elementos individuales, como a las filas o columnas es igualmente sencillo:
>>C(1,1)
ans=
2
>>C(1,:)
C(1,:)
ans=
2468
C(:,1)
ans=
2
10
18
Matlab tiene bastantes recursos para visualizacin. Es posible dibujar directamente en una
ventana tanto en 2D como en 3D. Por ejemplo, la siguiente secuencia:
fort=0:10
q=[q;t*t];
end
plot(q)
100
90
80
70
60
50
40
30
20
10
0
0
10
12
3. DESARROLLO DE LA PRCTICA
Para iniciar la sesin prctica deber arrancarse el programa Matlab mediante el icono del
escritorio. A continuacin debe arrancarse el editor de texto que incorpora MATLAB, pinchando
en el icono de new M-FILE. Una vez se haya escrito y almacenado el programa, puede
ejecutarse desde la lnea de comandos de Matlab sin ms que escribir su nombre (sin extensin),
o desde el mismo editor.
Los programas que se implementen debern almacenarse en formato texto, con extensin .m,
dentro del subdirectorio C:\MATLAB\ ALUMNOS \*.m.
Una vez terminada la prctica deberis borrar los programas generados en el directorio
No olvidis apuntar vuestro nombre en la lista que se pasar durante la sesin, indicando los
grupos que habis trabajado en el mismo PC.
La elaboracin de la prctica llevar dos sesiones en las que se vern diferentes aspectos del
Modelado de un Sistema Robotizado:
3.1-Transformaciones Homogneas
3.2-Modelado del Entorno
3.3-Modelo Cinemtico Directo del Brazo Robot
3.4-Generacin de Trayectorias en el Espacio de Articulaciones
3.5-Cinemtica inversa
3.6-Generacin de trayectoria en el espacio Cartesiano
functionA=rotax(alpha)
calpha=cos(alpha);
salpha=sin(alpha);
A=[1
0
0
calpha
0
salpha
0
0
0
0
0
salpha 0
calpha 0
1];
Esta funcin devuelve la transformacin homognea resultante de rotar un ngulo alpha (en
radianes) sobre el eje X.
Igualmente, est definida la funcin rotay(alpha) que devuelve la matriz de rotacin alrededor
del eje Y.
Un punto en robtica se define con un sistema de coordenadas asociado, o sea posicin y
orientacin. Aunque parezca a primera vista que no tiene sentido asignarle una orientacin a un
punto, en robtica es justificado, puesto que esta orientacin representa la que debe tener el
efector final cuando se enva a este punto.
Para rotar un punto alrededor de un eje basta con premultiplicar la matriz asociada al punto por
la matriz de rotacin correspondiente.
Ejemplo:
100
P 100
El punto
100
P=[eye(3),[100;100;100];0 0 0 1]
P=
1
0
0
0
0
1
0
0
0 100
0 100
1 100
0 1 1
P1=rotax(pi/4)*P
P1 =
1.0000
0
0 100.0000
0 0.7071 -0.7071 0.0000
0 0.7071 0.7071 141.4214
0
0
0 1.0000
Ahora, para rotar el nuevo punto obtenido de pi alrededor del eje Y, se premultiplica por la
matriz correspondiente:
P1=rotay(pi/4)*P1
P1 =
0.7071 0.5000 0.5000 170.7107
0 0.7071 -0.7071 0.0000
-0.7071 0.5000 0.5000 29.2893
0
0
0 1.0000
Hacer las mismas dos rotaciones en orden inverso. Notar que el resultado es
diferente.
Definid las funciones rotaz(alpha), Despl(dx,dy,dz) comprobando su correccin.
3.1.1 Rotacin de una lnea
Primero es necesario inicializar el espacio grfico 3D que se va a utilizar. Para ello ha sido
implementado el fichero de Matlab inigraf.m.
Ejecutar el fichero. Aparecer en la ventana grfica un sistema de coordenadas 3D con rejilla.
Para crear una lnea en Matlab se utiliza el comando line introduciendo el color
coordenadas X, Y, Z del punto inicial y final de la siguiente forma:
y la
linuno=line('xdata',[0;1000],'ydata',[0;1000],'zdata',[0;1000],'color','red')
7
linuno =
78.0001
Es el handle del objeto lnea creado. Para
importancia.
Hemos visto que el color se puede definir con una cadena de caracteres. Esto tiene el
inconveniente de que el nmero de cadenas de caracteres previsto es limitado, por lo que lo
sera tambin el nmero de colores disponibles.
Existe otra forma de introducir colores mediante un vector RGB (Red- Green- Blue). Los
valores del vector corresponden a la intensidad de cada uno de los tres colores primarios en la
mezcla y deben estar entre 0 y 1.
Ejemplo
Introducir:
lindos=line('xdata',[0;-2000],'ydata',[0;-2000],'zdata',[0;4000],'color',[0.8 0.1
0.8])
Se obtendr una lnea morada puesto que el rojo y el azul tienen ms peso que el verde.
Para crear un lnea entre los puntos P1 y P2 se ha implementado la funcin crealin(P1,P2,color).
Como datos de entrada utiliza las matrices asociadas a los puntos y el vector RGB de color.
La secuencia en Matlab es la siguiente:
P1=punto([0;0;0])
P1 =
1
0
0
0
0
1
0
0
0
0
1
0
0
0
0
1
P2=despl(1000,0,0)*P1
P2 =
1
0
0
0
0
1
0
0
0
0
1
0
1000
0
0
1
col=[0.1 0.9 0]
col =
0.1000
0.9000
lintres=crealin(P1,P2,col)
lintres =
62.0001
Una lnea ya existente se puede modificar mediante el comando set.
Ejemplo
Para modificar la coordenada X del punto inicial de la lnea dos a 1000 y del punto final
a -2000, teclear:
set(lintres,'xdata',[1000;-2000])
Observar en la grfica la modificacin.
Con el comando set se pueden modificar varias variables a la vez. Teclear:
set(lintres,'ydata',[-1000;-2000],'color','yellow')
Ver el resultado.
Notar que solo se puede modificar una lnea existente. Cuando se borra la ventana grfica, por
ejemplo con las ordenes, clf o inigraf, la lnea deja de existir para el interprete de Matlab, y por
lo tanto no es modificable.
Si se quiere borrar nicamente una lnea, se utiliza el comando delete.
Ejemplo
Teclear:
delete(linuno)
Para extraer datos de una lnea se utiliza el comando get.
Ejemplo
Teclear:
X=get(lindos,'xdata')
Este comando asigna a la variable X las coordenadas x del punto inicial y final de la
lnea lindos.
[P1 P2]=extrae(lintres)
P3=rotaz(pi/2)*P1
P4=rotaz(pi/2)*P2
modlin(lintres,P3,P4)
1. Implementar una funcin dibulin(lnea, matriz) que pueda aplicar cualquier
matriz de transformacin a cualquier lnea.
2. Leer el fichero dibuobj que contiene una funcin de rotacin/desplazamiento
pueda ser aplicable a un vector de lneas. Verificar su funcionamiento en un
vector que contenga las tres lneas existentes.
3.2 MODELADO DEL ENTORNO DE TRABAJO DEL ROBOT
De todo el entorno modelaremos nicamente la mesa, el cubo de basura, la cinta
transportadora y la pieza a trasladar. En posteriores apartados se modelar el
robot.
Antes de empezar, borraremos las lneas anteriormente creadas mediante la orden inigraf.
10
[P1 P2]=extrae(tabla1)
P1=rotaz(pi/2)*P1
P2=rotaz(pi/2)*P2
tabla2=crealin(P1,P2,[0.1 0.1 1])
Verificar en la grfica si la lnea est en el sitio correspondiente.
Ahora vamos a introducir la cinta transportadora. Para ello es suficiente teclear creacinta
puesto que el fichero ya est implementado .
11
12
13
Como los sistemas de coordenadas 1 y 0 son paralelos, los parmetros de Denavitt- Hartenberg
alpha y theta del primer elemento sern cero (no se necesitan rotaciones para alinear los
sistemas de coordenadas). Igualmente, el parmetro a ser cero porque no hay distancia a
lo largo de x1 entre los orgenes. El parmetro d tendr el valor de la coordenada q de la
primera articulacin.
El segundo elemento es una barra de 1000 mm. El eje z2 se pone en su eje de rotacin (paralelo
a z1). El origen o2 debe estar en la normal comn entre z1 y z2. Como son paralelos, hay
infinitas normales comunes. Elegimos la que pasa por el origen o1, puesto que de esta
forma hay menos clculo. El eje x2 ser la normal comn, es decir tendr la direccin del
segundo elemento.
El prametro alpha de esta articulacin ser cero puesto que los ejes z1 y z2 son paralelos. El
parmetro theta es la variable de la articulacin. El parmetro d es cero porque no hay
despalazamiento entre los orgenes a lo largo de z1. El parmetro a equivale a la longitud del
elemento(1000 mm), pero con signo negativo, puesto que o1 se debe desplazar en el sentido
contrario a x2 para coincidir con o2.
Las coordenadas del punto inicial y final del segundo elemento en el sistema de coordenadas
asociado a l son [1000, 0, 0] y [0, 0, 0] respectivamente.
El tercer elemento es una barra de 900 mm de largo.
La cuarta articulacin, la primera de la mano, tiene su eje de giro, z3, en la direccin del tercer
elemento (mueca en lnea).
El origen o3 se pone en la interseccin entre z2 y z3. O sea, coincide con o2.
El eje x3 tiene que ser perpendicular tanto a z2 como a z3.
14
En la imagen estn omitidos los ejes y para no sobrecagarla. De todos modos, estos no se
utilizan segn la convencin de Denavitt- Hartenberg.
El parmetro theta equivaldr a la variable de la articulacin. Los parmetros a y d sern cero ya
que los orgenes coinciden. El parmetro alpha corresponde al ngulo que se debe girar z2
alrededor de x3 para alinearse con z3, en este caso 90 grados.
Las coordenadas del punto inicial y final del tercer elemento en el sistema asociado sern [0, 0,
0] y [0, 0, 900], respectivamente.
El cuarto elemento, es una prolongacin del tercero y gira alrededor de z3. Es una barra de
longitud 100mm. En la imagen el tercer elemento est representado por una lnea punto a punto.
y el cuarto por una lnea ms gruesa.
15
El quinto elemento es una barra de 100mm de longitud que gira alrededor de z4.
Como los ejes z4 y z5 intersectan, el origen o5 coincidir con o4. Por esta razn los parmetros
a y d de esta articulacin sern cero.
El parmetro alpha ser 90 grados.
Las coordenadas del punto inicial y final del quinto elemento en el sistema asociado sern [0, 0,
0] y [0, 0, 100], respectivamente.
El sexto y ltimo elemento gira alrededor de z5 y consiste de una barra de 100mm de largo.
El ltimo sistema de coordenadas representa el del efector final del robot. Por esta razn su
origen tiene que estar en la punta del ltimo elemento. Puesto que no contiene ninguna
articulacin, se tiene plena libertad para elegir los ejes. Sin embargo, para minimizar los
clculos, se suele elegir paralelo al anterior.
Los parmetros aplha y theta sern cero puesto que los sistemas de coordenadas son paralelos.
Notar que en este caso theta no es la variable de la articulacin puesto que no la hay.
nicamente el parmetro d ser diferente de cero y tendr el valor 200.
Para el clculo cinemtico y la representacin grfica del brazo se han implementado las
siguientes funciones:
dh(robot,eje,coordenada): devuelve la matriz homognea de Denavit- Hartenberg
para el elemento requerido del robot requerido en la posicin requerida.
16
Ejemplo:
Para encontrar la matriz de transformacin del segundo elemento de nuestro robot prr al
que corresponde la matriz r2 y estando la articulacin 2 girada de 45 grados:
dh(r1,2,pi/4)
ans =
0.7071 -0.7071
0 -707.1068
0.7071 0.7071
0 -707.1068
0
0 1.0000
0
0
0
0 1.0000
Ejemplo:
Para dibujar el robot prr (r1) asignando a la variable robot su vector de lneas
hay que teclear:
robot=crearob(r1,[1000,pi,-pi/2,0,pi/2,0]')
robot =
67.0007
68.0006
69.0005
70.0004
71.0002
72.0002
73.0001
Ejemplo:
Para dibujar el robot anteriormente creado en otra configuracin, teclear por ejemplo:
modrob(r1,[1400,pi/3,-pi/2,pi/3,2*pi/3,0]',robot)
La funcin dircin que resuelve la cinemtica desde el la base del robot hasta la
palma de la mano tiene algunas limitaciones. El robot no puede estar en
cualquier lugar ni orientado de cualquier forma. El sistema de coordenadas de la
base tiene que coincidir con el de Matlab. Por otra parte, la funcin dircin no
incluye la posibilidad de que el robot lleve una herramienta.
q
(t ) 6at 2b
Para calcular los coeficientes del polinomio partimos de la posicin inicial q i y la posicin
final q f . Sabemos que el polinomio en t=0 (principio de la trayectoria) deber darnos la
posicin inicial y que el polinomio en t=1 (final de la trayectoria) deber dar la posicin final,
con lo que definimos 2 ecuaciones:
I) q (0) d qi
II) q(1) a b c d q f
Tambin sabemos que la velocidad al principio y al final de la trayectoria deber ser cero, con lo
que podemos definir 2 ecuaciones ms:
III) q (0) c 0
IV) q (1) 3a 2b c 0
De estas cuatro ecuaciones podemos obtener los cuatro coeficientes del polinomio para cada
articulacin.
q =[
para t=0
para t=1
para t=2
para t=tf
19
sx
sy
sz
0
ax
ay
az
0
px
p y R p
p z 0 1
R3
4-Calcular 3 R5 0 R3T R
5-Resolver la cinemtica inversa para las tres ltimas articulaciones (q4, q5, q6).
Esto lo resuelve la funcin ciPRR3_6 mediante la matriz de transformacin
inversa. En este caso tambin existen dos soluciones.
La cinemtica inversa completa del robot PRR est resuelta por la funcin invcPRR.
Ella devuelve las cuatro configuraciones posibles de las articulaciones que cumplen el
requisito de que el efector final tenga la posicin y orientacin definidas por la matriz
TH.
Nota: en estas funciones no estn implementadas las singularidades.
20
Siempre que se desee que el efector final siga una trayectoria especificada
geomtricamente en el espacio cartesiano, es necesario planificar la misma en
este espacio.
En esta prctica se realizar una funcin que genere una trayectoria lineal en el espacio
Cartesiano. Dado un punto inicial de la trayectoria lineal p i y un punto final
p f , la trayectoria general puede definirse como:
p (t ) f (t ) p f (1 f (t )) pi
En el caso ms sencillo (es el que vamos a tomar), f(t)=t, con lo que la ecuacin queda:
t 0;1
p (t ) tp f (1 t ) p i
Para esta prctica vamos a mantener la misma orientacin del efector final para el
principio y el final de la trayectoria lineal. Un algoritmo para desplazar el efector final a
lo largo de una trayectoria cartesiana puede ser el siguiente:
Para t=0 hasta 1 incremento 0.05
Calcular p(t)
R
p (t )
q1
q1
q1
...
q1
q2 q3 q4 q5
q2 q3 q4 q5
q2 q3 q4 q5
para t=0
para t=1
para t=2
q2 q3 q4 q5 ]
para t=tf
cinemtica
de
velocidades
q q 1 q 2 ... q n
X v x v y v z x
con
relaciona
las
z .
las
velocidades
21
velocidades
cartesianas
de
las
del
articulaciones
efector
final
vx
v
y
vz
x
y
z
q 1
q
v
2
J (q ) J (q )q
...
q n
J
J v
J
Jv relaciona las velocidades lineares del efector final con las velocidades de las articulaciones y
es de dimensin 3Xn.
J relaciona las velocidades angulares del efector final con las velocidades de las articulaciones
y tambin es de dimensin 3Xn.
Jv se puede representar de la siguiente forma:
J v j v1
jv 2
...
j vi
...
j vn
i .
la herramienta v y la velocidad de la articulacin i, q
Se puede demostrar que:
j vi z i 1 X ( p n pi 1 ) si la articulacin i es de revolucin.
j vi z i 1 si es prismtica.
z i 1 representa el vector unitario del eje z del sistema de coordenadas del elemento anterior al
J j1
j 2
...
j i
...
j n
i .
de la herramienta y la velocidad de la articulacin i, q
22
j vi z i 1 si la articulacin i es de revolucin.
j vi 0 si es prismtica.
q J ( q ) 1 X
Cabe resaltar que existen configuraciones en las que la matriz jacobiana no es invertible, puesto
que su determinante es cero. Se llaman singularidades. En estos puntos el robot no se
puede desplazar o rotar en algunas direcciones. Pierde algunos grados de libertad.
La medida de manipulabilidad de un robot en una configuracin indica la capacidad del robot de
moverse en todas las direcciones. Viene dada por la siguiente expresin:
(q)
det( J ( q ) J T ( q ))
Hacer un programa de forma que el robot vaya en lnea recta desde el punto (500,
500, 500) al punto (200, 200, 200) con la orientacin del efector final hacia abajo.
La velocidad linear debe ser de 0.1 m/s. La orientacin debe ser constante durante
la trayectoria.
23
TRABAJO OBLIGATORIO I
Modelar el robot que os fue asignado en la prctica anterior mediante una matriz similar a
r1.
Modelar un entorno diferente al de la prctica.
Implementar un programa en Matlab de forma que el robot efecte una tarea de pick&place.
No es necesario que el robot toque la pieza. Basta que el efector final se coloque unos 1020 cm encima de la pieza.
TRABAJO OBLIGATORIO II
Implementar una funcin que solucione la cinemtica inversa para el brazo que os fue
asignado. El dato de entrada debe ser la matriz de transformacin entre el sistema de
coordenadas de la base y el del efector final.
Implementar funciones recta( pto) y arco(pto1, pto2) de manera que el efector final haga
una trayectoria en forma de recta o arco. En ambos casos la trayectoria debe empezar en el
punto donde se encuentra el robot antes de ejecutarla. En caso de una recta, el argumento a
pasarle a la funcin es el punto final. En caso del arco, un punto intermedio del arco y el
punto final.
24