PRCTICA DE ROBTICA

MODELADO CINEMTICO DE UN
SISTEMA ROBOTIZADO

Departamento de Ingeniera de Sistemas y

Automtica
(DISA)
Universidad Politcnica de Valencia
(UPV)

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:

1- Comprender la utilidad y las formas de operar con transformaciones homogneas

2- Dominar el modelo matemtico de representacin cinemtica propuesto por Denavit y
Hartenberg.

3- Familiarizarse con la herramienta CAE Matlab de gran utilidad en Robtica y en otras reas
de Ingeniera de Sistemas y Automtica.

4- Tener unos primeros conceptos de generacin de trayectorias.

Modelado Cinemtico de un Sistema

Robotizado

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

Modelado Cinemtico de un Sistema

Robotizado

>>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

Dibuja la funcion q=t^2;

30
20
10
0
0

10

12

Modelado Cinemtico de un Sistema

Robotizado

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

Modelado Cinemtico de un Sistema

Robotizado

3.1 TRANSFORMACIONES HOMOGNEAS

A cada elemento del Sistema Robotizado se le va a asociar una Matriz de Transformacin
Homognea que defina su posicin y orientacin en el espacio. Necesitamos definirnos una
serie de funciones, para poder operar convenientemente con matrices de Transformacin
Homognea.
Por ejemplo, para realizar una rotacin en el eje X est definida una funcin rotax(alpha), en el
fichero C:\Matlab\Practcin\rotax.m como la siguiente:

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

se puede introducir en Matlab de forma:

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

Modelado Cinemtico de un Sistema

Robotizado

Ha sido implementada la funcin punto(vector) que genera la matriz homognea del

punto partiendo de su vector de posicin.
Para rotar este punto, por ejemplo, de pi/4 alrededor del eje X, se debe premultiplicar por la
matiz homognea correspondiente:

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', [x1;x2], 'ydata', [y1;y2], 'zdata', [z1;z2], 'color', 'green');

Ejemplo
Para crear una lnea roja desde el punto [0,0,0] al punto [1000,1000,1000] que
llamaremos linuno teclear:

linuno=line('xdata',[0;1000],'ydata',[0;1000],'zdata',[0;1000],'color','red')
7

Modelado Cinemtico de un Sistema

Robotizado

Abriendo la ventana grfica se podr ver la lnea.

Notar que al crear la lnea el Matlab nos devuelve un nmero.

linuno =
78.0001
Es el handle del objeto lnea creado. Para
importancia.

el desarrollo de la prctica no tiene ninguna

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

Modelado Cinemtico de un Sistema

Robotizado

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.

Modelado Cinemtico de un Sistema

Robotizado

Si se quiere desplazar una lnea o girar alrededor de un eje, basta con

premultiplicar por la matriz homognea correspondiente el punto inicial y el punto
final.
Desafortunadamente, para multiplicar un punto por una matriz homognea necesitamos sus
coordenadas en forma de matriz de transformacin, y el Matlab guarda las coordenadas en
formato [x1,x2], [y1,y2], [z1,z2].
Para ayudar a pasar de un formato al otro se han implementado las siguientes funciones:
[P1, P2]=extrae(linea): extrae las matrices de transformacin de los puntos inicial y
final.
modlin(linea,P1,P2): modifica los puntos inicial de la lnea indicada a los puntos P1 y
P2. Adicionalmente, aade a las lneas un atributo adicional erasemode,xor, til
para las animaciones que se harn posteriormente.
Entonces para rotar o desplazar una lnea existente la secuencia sera la siguiente:
1. Extraer el punto inicial y final.
2. Premultiplicar los puntos por la matriz de transformacin correspondiente.
3. Asignar los valores obtenidos al punto inicial y final de la lnea.
Ejemplo:
Para rotar la lnea lintres de 90 grados alrededor del eje z:

[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

Modelado Cinemtico de un Sistema

Robotizado

La mesa consiste de la tabla ( 1200X1200) y cuatro patas de longitud 1000 uds.

Empecemos por crear la tabla de forma que su centro se encuentre en el origen del sistema de
coordenadas.
La tabla consiste de cuatro lneas.
Creamos la primera:

tabla1=crealin([eye(3) [600;600;1000];0 0 0 1], [eye(3) [600;-600;1000] ;0 0 0 1],

[0.1 0.1 1])
Las dems se podran crear de la misma forma, pero lo vamos a hacer mediante rotaciones.
Para obtener la segunda lnea, extraeremos los puntos de la primera y les aplicaremos una
rotacin de 90 grados alrededor del eje z:

[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.

Crear las lneas tabla3 y tabla4 aplicando las transformaciones necesarias a

tabla1 y tabla2.
Crear el objeto tabla=[tabla1;tabla2;tabla3; tabla4].
Verificar la validez de la funcin dibuobj(objeto, matriz) haciendo pruebas de
rotaciones y traslaciones con la tabla.
Implementar una funcin copilin(lnea, matriz) que permite crear una nueva
lnea a partir de una ya existente, rotndola o trasladndola, y conservando el
color original. A partir de ella crear la funcin copiobj(objeto, matriz) que hace
lo mismo con un objeto o vector de lneas.
Crear las patas de la mesa, la primera mediante crealin, y las posteriores
mediante copilin a partir de la primera.
Crear el objeto mesa =[tabla;pata1,....pata4].
Mover la mesa hacia la esquina del dibujo en direccin positiva de los ejes x e y
mediante matrices homogneas y la orden dibuobj.

Ahora vamos a introducir la cinta transportadora. Para ello es suficiente teclear creacinta
puesto que el fichero ya est implementado .

Elevar la cinta a un metro del suelo y desplazarla de un metro en el sentido

negativo del eje x.

De la misma forma tecleando creacubo y creapieza se introducen los elementos siguientes.

Mover la pieza encima de la cinta y el cubo a la esquina anterior derecha.

11

Modelado Cinemtico de un Sistema

Robotizado

Ejecutando el fichero mpieza se mover a lo largo de la cinta. Leer y eventualmente modificar

este fichero para que el movimiento se efecte encima de la cinta.

12

Modelado Cinemtico de un Sistema

Robotizado

3.3 MODELO CINEMTICO DIRECTO DE UN BRAZO

En esta prctica se va a modelar un robot PRR con mueca en lnea. Este tipo de brazo no es
frecuente, pero se ha elegido por no coincidir con los modelos asignados a los alumnos.
Nuestro propsito es describir el robot de forma que se pueda dibujar en cualquier
configuracin. Lo vamos a realizar mediante la matriz r1 que se encuentra en el fichero prr.m.
La primera fila de la matriz corresponde a la base del robot. Las siguientes corresponden a un
elemento del robot cada una. Cada elemento se va a representar con una lnea de color diferente.
Los primeros cuatro elementos de cada lnea (excepto para la base) representan los parmetros
de Denavit-Hartenberg del elemento. Notar que los valores de theta para articulaciones de
rotacin y d para articulaciones prismticas corresponden a las variables de la articulacin. Esto
significa que sus valores cambian al moverse el robot. De momento pondremos sus valores a
cero en la matriz, y posteriormente crearemos una funcin que les asigne los valores segn las
coordenadas en el espacio de articulaciones.
El quinto elemento de cada lnea es un bit que define el tipo de articulacin: 0 para las de
revolucin y 1 para las prismticas.
Los ltimos nueve elementos de cada lnea nos suministran la informacin necesaria para
dibujar el robot en la pantalla. Los elementos 6,7 y 8 son las coordenadas del punto inicial del
elemento en su sistema de coordenadas adjunto. Los elementos 9,10 y 11 son las coordenadas
del punto final. Los elementos 12, 13 y 14 son los valores RGB del color que hemos elegido.
Ahora vamos a ver detalladamente como se ha obtenido la matriz r1. Es conveniente tener
abierto el fichero prr.m mientras se lee la siguiente parte del enunciado.
En nuestro caso la base del robot es una barra de 2000 mm. Encima de ella se desplaza un carro
de 200 mm de longitud, que es el primer elemento del robot.
Pondremos el eje z0 de la base en direccin del movimiento del eje prismtico (ver imagen).
Elegiremos el origen del sistema coordenadas de la base por ejemplo en la esquina
izquierda del elemento. Los otros dos ejes se pueden elegir de cualquier forma con tal de
que el sistema sea dextrgiro. El punto inicial de la base coincide con el origen de su
sistema de coordenadas, mientras que su punto final tiene las coordenadas [0, 0, 2000].
El sistema de coordenadas 1 se encuentra en la segunda articulacin y est asociado al primer
elemento, es decir al carro. El eje z1 debe tener la direccin perpendicular al plano de
rotacin de la articulacin. El eje x1 se debe poner en la normal comn de z0 y z1. Sin
embargo, como en este caso z0 y z1 coinciden, como eje x1 se puede elegir cualquier
recta perpendicular a ellos. Lo elegimos paralelo a x0 porque as se reduce el clculo.
Las coordenadas del punto inicial y final del carro en su sistema de coordenadas asociado son
[0, 0, -100] y [0, 0, 100].

13

Modelado Cinemtico de un Sistema

Robotizado

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

Modelado Cinemtico de un Sistema

Robotizado

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.

El cuarto eje, segundo de la mueca es perpendicular al anterior. El origen o4 est en la

interseccin entre z3 y z4, y x4 es perpendicular a ambos.
El parmetro theta es la variable de la articulacin. El parmetro a es cero puesto que no hay
distancia entre los orgenes a lo largo de x4. El parmetro d corresponde a la distancia entre los
orgenes a lo largo de z3, es decir a la suma de las longitudes de los elementos 3 y 4, o sea
1000mm. El parmetro alpha es el ngulo que se debe girar z3 alrededor de x4 para estar
alineado con z4, es decir 90 grados.
Las coordenadas del punto inicial y final del cuarto elemento en el sistema asociado sern [0,
-100, 0] y [0, 0, 0], respectivamente.
El eje z5 es perpendicular a z4 (mueca en lnea).

15

Modelado Cinemtico de un Sistema

Robotizado

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

Modelado Cinemtico de un Sistema

Robotizado

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

crearob(robot, vector de coordenadas generalizadas): crea las lneas que

corresponden a los elementos del robot en la configuracin requerida.

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

modrob(robot, vector de coordenadas generalizadas, vector de lneas): dibuja el

robot en las nuevas coordenadas generalizadas. Para que la funcin lo pueda
efectuar es tambin necesario pasarle el vector de lneas asociado al robot en
cuestin.

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)

Hacer varias pruebas modificando la configuracin del robot.

Leer y entender los ficheros anteriores.
Implementar la funcin dircin(robot,q), que resuelve la cinemtica
directa del robot. Es decir, para una configuracin dada de un robot
dado, devuelve la posicin y orientacin del efector final en el sistema de
coordenadas de la base, en forma de matriz de transformacin.
17

Modelado Cinemtico de un Sistema

Robotizado

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.

Implementar una versin generalizada de la funcin dircin,

dircin_gen(robot, q1, MH1, MH2) que solucione la cinemtica directa
desde el sistema de coordenadas de Matlab hasta el de la herramienta
para un robot colocado cualquier posicin/ orientacin. MH1 representa
la matriz de transformacin desde el s.c de Matlab al de la base del
robot. MH2 representa matriz de transformacin de la herramienta.

3.4 GENERACIN DE TRAYECTORIAS EN EL ESPACIO DE

ARTICULACIONES
La funcin de un generador de trayectorias es generar las entradas de referencia para el sistema
de control de movimiento, que se encarga de garantizar que el robot ejecute el recorrido
planificado en el tiempo planificado.
Las trayectorias pueden especificarse:
-en espacio cartesiano: pasar de la localizacin L1= ( x1 , y1 , z1 , 1 , 1 , 1 ) a la localizacin
L2= ( x 2 , y 2 , z 2 , 2 , 2 , 2 )
-en espacio de articulaciones: pasar de la configuracin de variables de articulacin
Q1 ( q11 , q 21 , q 31 , q 41 , q 51 ) a otra Q 2 ( q12 , q 22 , q 32 , q 42 , q 52 )
La generacin de trayectorias en espacio de articulaciones es ms sencilla, y permite generar
funciones lineales para cada uno de los ejes (por ejemplo, trapezoides). Sin embargo, genera un
movimiento no lineal del efector final.
Si la trayectoria se genera en espacio cartesiano, el movimiento del efector final ser lineal, lo
que provoca que cada articulacin realice un movimiento no-lineal. La generacin de
trayectorias en espacio cartesiano es ms cmoda para el usuario, ya que puede ver el recorrido
que realizar el efector final. El inconveniente principal es que hay que resolver la cinemtica
inversa para cada punto de la trayectoria, con el consiguiente coste computacional. Este coste es
una de las causas de que la mayora de los robots industriales funcionen de 3 a 4 veces ms
despacio cuando generan trayectorias cartesianas a cuando lo hacen en espacio de
articulaciones.
Uno de los mtodos comunes de generar trayectorias en espacio de articulaciones es el
movimiento punto a punto. En este tipo de movimiento se utilizan polinomios cbicos para
definir la trayectoria. La forma mnima de hacerlo es mediante polinomios de tercer grado. La
posicin de la articulacin se determina mediante el siguiente polinomio.
q (t ) at 3 bt 2 ct d

El perfil de la velocidad y la aceleracin se obtiene derivando:

q (t ) 3at 2 2bt c
18

Modelado Cinemtico de un Sistema

Robotizado

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.

Implementad la funcin [q] = gentray(qi,qf,tf) que devuelva en q la trayectoria

polinomial punto a punto que lleva desde qi hasta qf en tiempo tf. Dividir el
tiempo en intervales de 0.1 segundo. La variable q devuelta tendr forma de
matriz y ser como la siguiente:

q =[

qi1 qi2 qi3 qi4 qi5 qi6

q1 q2 q3 q4 q5 q6
q1 q2 q3 q4 q5 q6
...
qf1 qf2 qf3 qf4qf5 qf6]

para t=0
para t=1
para t=2
para t=tf

Implementar la funcin dibrob(robot, vector de lneas, trayectoria), a la que se

le pasa una trayectoria, el robot y su vector de lneas asociado. La funcin
mostrar el robot movindose en pantalla a lo largo de la trayectoria
especificada.
Ampliar la funcin anterior para que se le pueda asociar un objeto (p.e. la
pieza) al efector final del robot, de forma que este objeto se mueva junto con el
robot.
Implementar los movimientos de pick & place desde la cinta hasta el cubo y la
mesa.

3.5 CINEMTICA INVERSA

La mayora de los manipuladores existentes tienen una cinemtica sencilla puesto que
estn formados por un brazo (de tres articulaciones) y una mueca en lnea. Se elige esta
configuracin mecnica por la dificultad de encontrar solucin al problema cinemtico
inverso para un caso general.

19

Modelado Cinemtico de un Sistema

Robotizado

Partiendo de posiciones y orientaciones que si que son alcanzables, la cinemtica

inversa de un manipulador PRR con una mueca en lnea puede resolverse siguiendo el
siguiente algoritmo:
Datos de entrada:
Posicin y orientacin a alcanzar expresados en forma de matriz homognea:
nx
n
TH y
nz

sx
sy
sz
0

ax
ay
az
0

px
p y R p

p z 0 1

1-Obtener la posicin de la mueca: p w p d 6 a

2-Resolver la cinemtica inversa para las tres primeras articulaciones (q1, q2, q3).
Est implementada en Matlab la funcin ciPRR1_3 que resuelve este problema par
el robot PRR por el mtodo geomtrico. Est funcin devuelve las dos posibles
soluciones para las tres primeras articulaciones.
3-Calcular la cinemtica directa con los q1, q2 y q3 obtenidos, es decir obtener
0

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.

Encontrar las cuatro soluciones de la cinemtica inversa la una matriz

homognea MH=rotax(pi)*despl(800,-800,-1000).
Representar grficamente el robot en las cuatro configuraciones.

3.6 GENERACIN DE TRAYECTORIAS EN ESPACIO CARTESIANO

20

Modelado Cinemtico de un Sistema

Robotizado

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

Puede verse fcilmente que:

p (0) p i
p (1) p f

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 )

Obtener Matriz de Transformacin Homognea

1
0
Calcular cinemtica inversa, obteniendo q1,q2,q3,q4 y q5
Fpara
Implementad la funcin function [ q ] = gentrayc(Ti,pf, tf), a la que pasaremos como
parmetros la localizacin inicial, la posicin final, y el tiempo total de la trayectoria
(nmero de pasos en los que se lleva a cabo). La funcin devolver en cada fila de la
matriz q las posiciones de las articulaciones para cada instante.
q =[

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

3.7. CINEMTICA DE VELOCIDADES

La

cinemtica

de

velocidades

q q 1 q 2 ... q n
X v x v y v z x

con

relaciona
las

z .

las

velocidades

Estn relacionadas por la siguiente expresin:

21

velocidades
cartesianas

de

las
del

articulaciones
efector

final

Modelado Cinemtico de un Sistema

Robotizado

vx
v
y
vz

x
y

z

q 1

q
v
2
J (q ) J (q )q
...

q n

Dnde J es la matriz jacobiana del robot.

Como se puede apreciar en la expresin anterior, los valores de los elementos de la matriz
jacobiana dependen de la configuracin q del robot, q.
Las dimensiones de la matriz jacobiana para un robot de n articulaciones son 6Xn.
La matriz Jacobiana se puede dividir en dos matrices menores:

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

Dnde j vi es de dimensin 3X1 y representa la relacin entre el vector de velocidad linear de

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

i, p i 1 su vector de posicin y p n el vector de posicin del origen del s.c. de la

herramienta con respecto al de la base.

De la misma forma, la matriz J se puede escribir:

J j1

j 2

...

j i

...

j n

Dnde ji es de dimensin 3X1 y representa la relacin entre el vector de velocidad angular

i .
de la herramienta y la velocidad de la articulacin i, q

22

Modelado Cinemtico de un Sistema

Robotizado

Se puede deducir que:

j vi z i 1 si la articulacin i es de revolucin.

j vi 0 si es prismtica.

Est implementada en Matlab la funcin jacobiana(robot, q) que devuelve la matriz jacobiana

de un robot para la configuracin dada.
Si el robot es de seis articulaciones, se puede resolver la cinemtica inversa de velocidades de la
siguiente forma:

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 ))

Encontrar la Jacobiana del robot en la configuracin [0 0 0 0 0 0] y en la

configuracin [0 0 0 0 pi/2 0]. Encontrar el determinanate y la manipulabilidad en
ambos casos. Que se puede concluir?

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

Modelado Cinemtico de un Sistema

Robotizado

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.

Este Trabajo valdr 20% de la nota final.

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.

Este Trabajo valdr 20% de la nota final.

24