CRM Clase2

Cinematica
Facultad de Ciencias de la Electronica

Benemerita Universidad Autonoma de Puebla
Maestra en Ciencias de la Electronica, Opcion en Automatizacion
Fernando Reyes Cortes
Control de Robots Manipuladores
ftp://ece.buap.mx/pub/FernandoReyes/crm primavera2013/
Primavera 2013
Cinematica de robots manipuladores Articulaciones Clasicacion de robots industriales Nomenclatura para robots industriales Convencion Denavit-Hartenberg Robots antropomorco
Parte II
Cinematica de robots manipuladores
Contenido
Robot industrial
Tipos de articulaciones
Calsicacion de robots industriales
Nomenclatura de robots industriales
Convencion Denavit Hartenberg
Robots antropomorcos
Pendulo robot
Robot de 2 gdl
Robot de 3 gdl
Fernando Reyes Cortes Facultad de Ciencias de la Electronica
Cinematica Control de Robots Manipuladores MCEA-20800 2 / 54
Clasicacion de robots industriales
Un robot industrial esta compuesto por una serie consecutiva de eslabones y articulaciones para formar una cadena
en cinematica abierta, la cual es la estructura mecanica basica de un robot industrial. La cadena en cinematica abierta
esta formada de la siguiente manera: la primera articulacion sirve para formar la base; a continuacion siguen conexiones
sucesivas entre articulaciones y eslabones, en el extremo nal del ultimo eslabon no hay articulacion, generalmente se
destina a colocar la herramienta de trabajo para llevar a cabo una aplicacion especca. El extremo nal del robot no
se encuentra conectado fsicamente a la base como se muestra en la gura 1:
Figura:
Articulaciones
Las articulaciones se construyen por medio de un servomotor y representan la interconexion entre dos eslabones
consecutivos. Una articulacion puede realizar solo un tipo de movimiento, ya sea lineal, tambien conocida como
prismatica, y rotacional. La gura 2 presenta el tipo de articulaciones:
Figura:
Figura 2: Tipo de articulaciones: rotacional y lineal o prismatica.
Tabla: Clasicacion de robots industriales
Robot Caractersticas
Antropomorco (RRR) 3 articulaciones rotacionales
SCARA (RRP) 2 articulaciones rotacionales y 1 prismatica
Esferico (RRP) 2 articulaciones rotacionales y 1 prismatica
Cilndrico (RPP) 1 articulacion rotacional y 2 prismaticas
Cartesiano (PPP) 3 articulaciones prismaticas
Clasicacion de robots industriales
Tabla: Clasicacion de robots industriales
Robot Caractersticas
Antropomorco (RRR) 3 articulaciones rotacionales
SCARA (RRP) 2 articulaciones rotacionales y 1 prismatica
Esferico (RRP) 2 articulaciones rotacionales y 1 prismatica
Cilndrico (RPP) 1 articulacion rotacional y 2 prismaticas
Cartesiano (PPP) 3 articulaciones prismaticas
Figura:
Figura 3: Clasicacion de los robots industriales.
Nomenclatura para robots industriales
Un eslabon (link) esta formado por una barra metalica acoplada mecanicamente al rotor y al estator de la siguiente
articulacion. En la gura 7 se muestran las partes que componen a un eslabon de un robot manipulador.
Figura:
Figura 7: Elementos de un eslabon.
El espacio de trabajo (workspace) de un robot manipulador es el espacio o lugar donde el robot puede realizar
todos sus posibles movimientos. El espacio de trabajo esta determinado por la geometra del robot y la naturaleza de
sus articulaciones (lineales y rotacionales).
Figura:
Figura 5: Espacio de trabajo de un robot industrial.
El espacio de trabajo de un robot industrial se encuentra acondicionado por sensores especiales y cercas de seguridad
para que ninguna persona pueda invadir su area. Un robot industrial puede tener un peso de mas de tres toneladas
y alcanzar velocidades superiores a 3000 mm/seg. Mientras el robot esta en movimiento, resulta peligroso para un
usuario que se encuentre dentro de su espacio de trabajo.
Figura:
Figura 7: Espacio de trabajo de un robot industrial.
El extremo nal (end-eector) es la parte terminal o nal del ultimo eslabon, destinado a colocar la herramienta
adecuada para una aplicacion especca. La posicion del extremo nal se representa por
_
x, y, z
T
y su orientacion se
denota a traves de los angulos de Euler.
Figura:
Figura 7: Elementos de un eslabon.
La gura 8 muestra la analoga entre el brazo humano y un brazo robot o robot industrial. La articulacion de la base corresponde
a la cintura. La articulacion del hombro (shoulder) debe ser la de mayor capacidad con respecto a las otras articulaciones, ya
que es la que mueve y soporta el peso de la articulacion del codo (elbow) y de la herramienta de trabajo, as como la carga de
objetos que realice durante una determinada aplicacion.
Figura:
Figura 8: Base, hombro y codo de un robot industrial.
Notacion
Base (primera articulacion).
Hombro (shoulder) la que tiene mayor capacidad (Nm).
Codo (elbow) 10 % de la capacidad del hombro.
Figura:
Figura 9: Robot industrial FANUC.
Convencion Denavit-Hartenberg
El metodo de Denavit-Hartenberg es una herramienta ampliamente conocida en el area de ingeniera, ya que ofrece
un procedimiento sencillo para obtener el modelo cinematico directo cuya estructura queda en terminos de la trans-
formaciones homogeneas.
Jaques Denavit y Richard S. Hartenberg en 1955 presentaron un procedimiento para obtener una representacion
mnima de la orientacion y traslacion de robots manipuladores. Consiste en determinar una tabla de parametros
relacionados con los eslabones del robot. La convencion Denavit-Hartenberg toma como referencia el diagrama de un
robot manipulador en cadena cinematica abierta como se muestra en la gura 10.
Las variables articulares en la representacion Denavit-Hartenberg se denotan con:
i
para el tipo rotacional.
Prismatica o lineal por d
i
; este parametro d
i
tambien hace el papel de representar el ancho del servomotor de la
articulacion rotacional mas el espesor de la placa metalica del eslabon, en este caso se denota por el smbolo
i
.
La longitud del eslabon se representa con l
i
.
El angulo de separacion entre los ejes z
i
y z
i1
se denota con
i
.
Figura:
Figura 10: Convencion Denavit-Hartenberg para un robot manipulador.
El angulo
i
es el angulo entre los ejes x
i1
y x
i
medido alrededor del eje z
i1
.
d
i
es la distancia del origen del sistema de referencia i 1 a la interseccion del eje x
i
con el eje z
i1
. Su medicion
se realiza a lo largo del eje z
i1
.
El parametro l
i
se dene como la distancia a lo largo del eje x
i
desde el origen del sistema de referencia coordenado
i 1 hasta la interseccion del eje z
i1
con el eje x
i
.
El angulo entre los ejes z
i
y z
i1
se denota por
i
, su medicion es respecto a un plano normal a x
i
. Una medicion
de angulo positivo para
i
se toma en direccion del eje z
i1
hacia z
i
.
Figura:
Figura 11: Convencion para medir i y i .
Seleccion de sistemas de referencia DH
Procedimiento:
El eje z
i
se asigna rgidamente a la articulacion i + 1. Es decir, z
0
es el eje de la articulacion 1, z
1
es el eje de la
articulacion 2, y as sucesivamente.
Localizar el origen o
i
del sistema de referencia
i
_
x
i
, y
i
, z
i
_
en la interseccion del eje z
i
con la normal com un a
los ejes z
i1
y z
i
.
Seleccionar el eje x
i1
sobre la normal que une los ejes z
i1
y z
i
en direccion de la articulacion i 1 hacia la
articulacion i .
Denir el angulo de torsion
i
, este es el angulo entre los ejes z
i
y z
i1
y se mide con valor positivo en el sentido
de las manecillas del reloj sobre el eje x
i
.
Seleccionar el eje y
i
por la regla de la mano derecha.
La convencion Denavit-Hartenberg proporciona una representacion no unica para los siguientes casos:
Para el sistema de referencia
0
_
x
0
, y
0
, z
0
_
solo la direccion del eje z
0
es especicada, entonces su origen o
0
y el
eje x
0
pueden ser seleccionados de manera arbitraria.
Para el sistema de referencia
n
_
x
n
, y
n
, z
n
_
no existe la articulacion n+1, entonces el eje z
n
no esta completamente
denido, mientras que el eje x
n
es normal al eje z
n1
. Tpicamente la n-esima articulacion es rotatoria, por lo
tanto z
n
se alinea en la direccion de z
n1
.
Cuando dos ejes consecutivos z
i
y z
i1
son paralelos entre s, la normal com un entre ellos no es unica.
Cuando dos ejes consecutivos z
i
y z
i1
se interceptan, la direccion del eje x
i
es arbitraria.
Cuando la articulacion i -esima es lineal o prismatica, entonces la direccion de z
i
es arbitraria.
1. l
i
es la longitud del i -esimo eslabon, es la distancia del eje z
i1
hacia el eje z
i
medida sobre el eje x
i1
.
2.
i
es el angulo de torsion, el cual representa el angulo entre los ejes z
i1
a z
i
medido en el sentido de las
manecillas del reloj sobre el eje x
i
.
3. d
i
se emplea en articulaciones lineales o prismaticas y representa el desplazamiento lineal. Cuando la
articulacion es rotacional, entonces representa el oset o espesor del servomotor (la distancia de x
i1
a
x
i
medido sobre el eje z
i1
), se denota por
i
.
4.
i
es el desplazamiento rotacional de x
i1
a x
i
medido alrededor del eje z
i1
. El signo positivo de
i
es el
sentido contrario a las manecillas del reloj.
Observese que l
i
y
i
siempre seran positivos puesto que corresponden a longitudes, mientras que
i
, d
i
,
i
representan
cantidades con signo.
Tabla: Parametros Denavit-Hartenberg
Caractersticas de eslabones
l
i
Longitud del eslabon i -esimo.
d
i
Articulaciones lineales o prismaticas. Tam-
ben representa el espesor del servomotor
(
i
).
i

Angulo entre los ejes z
i1
y z
i
medido con respecto al eje x
i
.
i
articulaciones rotacionales; representa el an-
gulo entre los ejes x
i1
y x
i
medido alrededor
del eje z
i1
.
Representacion Denavit-Hartenberg con transformaciones homogeneas H
i
i 1
La representacion DH es a traves del producto de cuatro transformaciones basicas:
H
i
i1
= H
Rz
i1
(
i
)H
Tz
i1
(d
i
;
i
)H
Tx
i1
(l
i
)H
Rx
i1
(
i
)
=
_
_
cos(
i
) sen(
i
) 0 0
sen(
i
) cos(
i
) 0 0
0 0 1 1
0 0 0 1
_
_
_
_
1 0 0 0
0 1 0 0
0 0 1 d
i
(
i
)
0 0 0 1
_
_
_
_
1 0 0 l
i
0 1 0 0
0 0 1 0
0 0 0 1
_
_
_
_
1 0 0 0
0 cos(
i
) sen(
i
) 0
0 sen(
i
) cos(
i
) 0
0 0 0 1
_
_
=
_
_
cos(
i
) sen(
i
) cos(
i
) sen(
i
) sen(
i
) l
i
cos(
i
)
sen(
i
) cos(
i
) cos(
i
) cos(
i
) sen(
i
) l
i
sen(
i
)
0 sen(
i
) cos(
i
) d
i
(
i
)
0 0 0 1
_
_
La transformacion homogenea total se obtiene como H
n
0
= H
1
0
H
2
1
H
n1
n2
H
n
n1
. La cinematica directa es la forma
general de transformaciones homogeneas que concatena los sistemas de referencia cartesianos asociados a los eslabones
del robot, todos relativos al sistema de referencia jo
0
.
Algoritmo Denavit-Hartenberg
1. Localizar la direccion de los ejes z
0
, z
1
, , z
n1
.
2. Establecer el sistema de referencia cartesiano jo
0
_
x
0
, y
0
, z
0
_
cuyo origen es colocado sobre el sistema
de referencia en la base del robot. Los ejes x
0
, y
0
son determinados de acuerdo con la regla de la mano
derecha.
Iniciar un proceso iterativo en el cual se dene el sistema de referencia
i
_
x
i
, y
i
, z
i
_
usando el sistema
de referencia
i1
(x
i1
,y
i1
, z
i1
), iniciando con el sistema de referencia
1
_
x
1
, y
1
, z
1
_
. Llevar a cabo los
pasos 3 al 5 para la articulaciones i = 1, , n 1.
3. Localizar el origen o
i
en la interseccion de la normal com un que une al eje z
i
con el eje z
i1
. Si el eje z
i
intercepta al eje z
i1
colocar o
i
en la intercepcion. Para el caso en que los ejes z
i
y z
i1
son paralelos:
Si la articulacion i -esima es rotacional, colocar el origen o
i
sobre la articulacion i -esima, tal que
d
i
= 0.
Si la articulacion i -esima es prismatica, colocar el origen o
i
en un punto lmite fsico de la
articulacion i -esima, por ejemplo en un punto extremo.
4. Seleccionar el eje x
i
a lo largo de la normal com un que une a los ejes z
i1
y z
i
, en direccion de la
articulacion i 1 hacia la articulacion i .
5. Determinar y
i
por la regla de la mano derecha.
6. Establecer el sistema de referencia del extremo nal
n
_
x
n
, y
n
, z
n
_
.
Si la articulacion n-esima es rotatoria, entonces alinear el eje z
n
con el eje z
n1
Si la articulacion n-esima es prismatica, entonces seleccionar el eje z
n
de forma arbitraria. El eje x
n
debe cumplir el paso 4.
7. Establecer la tabla de parametros de eslabones.
8. Obtener las matrices de transformaciones homogeneas: H
i
i1
para i = 1, 2, , n 1.
Pendulo robot
Como un caso particular del robot de 3 grados de libertad antropomorco, se encuentra el pendulo robot de 1 grado
de libertad formado por un servomotor que tiene acoplado mecanicamente una barra metalica de longitud l
1
y esta
sometido al fenomeno de gravedad. El movimiento del pendulo se encuentra en el plano vertical x
0
y
0
como se indica
en la gura 12.
Figura:
Figura 12: Pendulo robot.
Pendulo robot experimental
Figura:
Figura 13: Prototipo experimental pendulo robot.
Cinematica directa cartesiana del pendulo robot
El origen del sistema de referencia cartesiano
0
(x
0
, y
0
, z
0
) se coloca sobre la articulacion del pendulo (en el respaldo
del estator del servomotor), el eje z
0
se alinea con el eje de giro del servomotor el cual es perpendicular al plano x
0
y
0
.
El servomotor del pendulo tiene un espesor de longitud
1
el cual ya incluye el ancho de la barra metalica. El origen
del sistema de referencia
1
(x
1
, y
1
, z
1
) se coloca en el extremo nal de la barra y se mueve de manera conjunta al
pendulo, el origen
1
tiene las siguientes coordenadas con respecto al origen de
0
(x
0
, y
0
, z
0
): [l
1
cos(q
1
), l
1
sen(q
1
),
1
].
Debido al espesor
1
el origen de
1
mantiene una distancia
1
sobre el eje z
0
. Los ejes z
1
y z
0
son paralelos entre s,
por lo tanto el angulo
1
que existe entre ellos es
1
= 0.
El plano x
1
y
1
tiene una rotacion de q
1
grados con respecto al plano x
0
y
0
del sistema jo
0
(x
0
, y
0
, z
0
), y la matriz
R
z0
_
q
1
_
determina dicha rotacion:
R
1
0
= R
z0
_
q
1
_
=
_
_
cos(q
1
) sen(q
1
) 0
sen(q
1
) cos(q
1
) 0
0 0 1
_
_
.
Tabla: DH del pendulo
Eslabon l d
1 l
1
0
1
q
1
H
1
0
= H
R
(z
0
, q
1
)H
T
(z
0
,
1
)H
T
(x
0
, l
1
)H
R
(x
0
, 0)
=
_
_
cos(q
1
) sen(q
1
) 0 l
1
cos(q
1
)
sen(q
1
) cos(q
1
) 0 l
1
sen(q
1
)
0 0 1
1
0 0 0 1
_
_
.
Las coordenadas cartesianas del extremo nal del pendulo robot (cinematica directa cartesiana), es decir el origen del
sistema
1
(x
1
, y
1
, z
1
) con respecto al sistema jo
0
(x
0
, y
0
, z
0
) se encuentra determinado por:
_
_
x
0
y
0
z
0
_
_
=
_
_
l
1
cos(q
1
)
l
1
sen(q
1
)
1
_
_
.
Las coordenadas cartesianas del extremo nal del pendulo dependen de la ubicacion de la posicion de casa (home
position), la cual es el punto de reposo u origen del pendulo que corresponde a q
1
= 0; la posicion de casa esta
colocada sobre el eje x
0+
. La matriz de rotacion R
z0
_
q
1
_
que determina la rotacion entre los sistemas de referencia jo
0
(x
0
, y
0
, z
0
) y
1
(x
1
, y
1
, z
1
) fue obtenida con respecto al primer cuadrante, es decir en el plano x
0+
y
0+
.
Otra posibilidad para la posicion de casa del pendulo es colocar el origen q
1
= 0 sobre el lado negativo del eje y
0
como
se muestra en la gura 14.
Figura:
Figura 14: Cambio de posicion de casa del pendulo sobre el eje y0
.
R
1
0
= R
z0
_
2
_
_
_
cos(q
1
) sen(q
1
) 0
sen(q
1
) cos(q
1
) 0
0 0 1
_
_
=
_
_
0 1 0
1 0 0
0 0 1
_
_
_
_
cos(q
1
) sen(q
1
) 0
sen(q
1
) cos(q
1
) 0
0 0 1
_
_
=
_
_
sen(q
1
) cos(q
1
) 0
cos(q
1
) sen(q
1
) 0
0 0 1
_
_
H
1
0
=
_
_
R
z0
_
2
_
_
_
cos(q
1
) sen(q
1
) 0
sen(q
1
) cos(q
1
) 0
0 0 1
_
_
0
T
R
z0
_
2
_
_
_
l cos(q)
l sen(q)
1
_
_
1
_
_
=
_
_
sen(q
1
) cos(q
1
) 0 l
1
sen(q
1
)
cos(q
1
) sen(q
1
) 0 l
1
cos(q
1
)
0 0 1
1
0 0 0 1
_
_
.
_
_
x
0
y
0
z
0
_
_
=
_
_
l
1
sen(q
1
)
l
1
cos(q
1
)
1
_
_
Jacobiano del pendulo
Debido a que el pendulo robot es un sistema escalar, no tiene matriz jacobiana. Sin embargo, se puede realizar la
siguiente interpretacion partiendo de la cinematica diferencial:
v =
d
dt
_
l
1
cos(q
1
)
l
1
sen(q
1
)
_
=
f
R
(q
1
)
q
1
q
1
=
_
l
1
sen(q
1
)
l
1
cos(q
1
)
_
q
1
=
_
l
1
sen(q
1
) 0
0 l
1
cos(q
1
)
_
. .
J(q1)
_
q
1
q
1
_
..
q
= J(q
1
) q (1)
el jacobiano J(q
1
) se puede pensar como esta expresado en la ecuacion (1). Notese que existe una singularidad cuando
la variable articular q
1
= 0, n, ya que su determinante esta dado como: det[J(q
1
)] = l
2
1
sen(q
1
) cos(q
1
). Tambien
existen singularidades en q
1
= n
2
, esto ultimo puede vericarse en la cinematica inversa, si q
1
= n
2
x
0
= 0,
entonces se indene la funcion arco-tangente (por ejemplo division entre cero).
Funcion transformacion homogenea del pendulo H
1
0
Codigo Fuente 1 H pendulo
Control de Robots Manipuladores, primavera 2013
Fernando Reyes Cortes. Posgrado en Automatizacion, Facultad de Ciencias de la Electronica BUAP.
H pendulo.m MATLAB version 2012a
function H=H_pendulo() 1
syms l1 q1 beta1 real 2
disp(Parametros Denavit-Hartenberg del pendulo) 3
disp([ l alpha d q]) dh=[l1, 0, beta1, q1]; 4
disp(dh) 5
%H
1
0
= H
Rz
0
(q
1
)H
Tz
0
(
1
)H
Tx
0
(l
1
)H
Rx
0
(0) 6
H10=HRz(q1)*HTz(beta1)*HTx(l1)*HRx(0) ; 7
[R10, cinemat pendulo, cero, c]=H DH(H10); 8
H=[R10, cinemat pendulo; 9
cero, c]; 10
end 11
Cinematica inversa del pendulo
q
1
= atan
_
y
0
x
0
_
.
Codigo Fuente 2 cinv pendulo
cinv pendulo.m MATLAB version 2012a
function q1=cinv_pendulo(x0,y0) 1
%cinematica inversa del pendulo 2
q1=atan(y0/x0); 3
end 4
Funcion cinematica directa cartesiana del pendulo
Codigo Fuente 3 cinematica pendulo
cinematica pendulo.m MATLAB version 2012a
function [x0, y0, z0]=cinematica_pendulo(beta1,l1,q1) 1
dato1=whos(beta1); dato2=whos(l1); dato3=whos(q1); 2
v1=strcmp(dato1.class, sym); v2=strcmp(dato2.class, sym); 3
v3=strcmp(dato3.class, sym); digits(3); 4
if (v1 & v2 & v3) %caso simbolico 5
x0=l1*cos(q1); 6
y0=l1*sin(q1); 7
z0=beta1; 8
end 9
else %caso numerico 10
x0=double(simplify(vpa(l1*cos(q1),3))); 11
y0= double(simplify(vpa(l1*sin(q1),3))); 12
z0=double(simplify(vpa(beta1,3))); 13
end 14
Ejemplo
Desarrollar un programa para MATLAB que presente de manera simbolica los parametros DH, cinematica cartesiana
y el jacobiano del pendulo. Ademas, contemple el analisis donde la posicion de casa se traslada del primer al cuarto
cuadrante.
Codigo Fuente 4 pendulo
pendulo.m MATLAB version 2012a
clc; 1
clear all; 2
close all; 3
format short 4
syms beta1 l1 q1 real 5
%H
1
0
= H
Rz
0
(q1)H
Tz
0
(1)H
Tx
0
(l1)H
Rx
0
(0)
6
H10=H pendulo() %H
1
0
7
%obtiene la matriz de rotacion R
1
0
y cinematica directa f
r
(q) 8
[R10, frq pendulo, cero, c]=H DH(H10) %R
1
0
, f
R
(1, l1, q1) 9
jac pendulo=jacobian(frq pendulo, q1) 10
H10a=HRz(-pi/2)*H10 %H
1
0 a
= H
Rz
0
`
H
1
0 11
%ejemplo numerico 12
q1=55*pi/180; %posicion angular del pendulo 13
beta1=0.1; %ancho del servomotor mas espesor de la barra 14
l1=0.45; beta1=0.1; %longitud del pendulo y espesor del servomotor 15
%cinematica cartesiana cuadrante I: f
R
I
(1, l1, q1)
16
[x0,y0,z0]=cinematica pendulo(beta1,l1,q1) 17
%cinematica directa en el cuadrante IV 18
%f
R
IV
(1, l1, q1) = Rz (
2
)f
R
I
(1, l1, q1)
19
Rz(-pi/2)*cinematica pendulo(beta1,l1,q1) 20
Robot antropomorco de 2 gdl
La gura 15 muestra un robot de dos grados de libertad con articulaciones rotacionales que se mueve en el plano
vertical x
0
y
0
. El sistema de referencia jo
0
(x
0
, y
0
, z
0
) se coloca en el respaldo del servomotor del hombro, de tal
forma que el eje z
0
coincida con el eje de rotacion (perpendicular al plano de la hoja). El ancho de cada servomotor
y espesor de la barra metalica estan determinados por
1
y
2
. Los ejes x
0
, y
0
se seleccionan con la regla de la mano
derecha.
Figura:
Figura 15: Robot planar vertical de dos gdl.
Parametros DH
Tabla: Parametros DH para el robot de 2
gdl
Eslabon l
i

i
d
i

i
1 l
1
0
1
q
1
2 l
2
0
2
q
2
Transformaciones homogeneas del robot de 2 gdl
H
1
0
= H
Rz
0
_
q
1
_
H
Tz
0
_
1
_
H
Tx
0
_
l
1
_
H
Rx
0
_
0
_
=
_
_
cos(q
1
) sen(q
1
) 0 l
1
cos(q
1
)
sen(q
1
) cos(q
1
) 0 l
1
sen(q
1
)
0 0 1
1
0 0 0 1
_
_
H
2
1
= H
Rz
1
_
q
2
_
H
Tz
1
_
2
_
H
Tx
1
_
l
2
_
H
Rx
1
_
0
_
=
_
_
cos(q
2
) sen(q
2
) 0 l
2
cos(q
2
)
sen(q
2
) cos(q
2
) 0 l
2
sen(q
2
)
0 0 1
2
0 0 0 1
_
_
H
2
0
= H
1
0
H
2
1
=
_
cos(q1 + q2) sen(q1 + q2) 0 l1 cos(q1) + l2 cos(q1 + q2)
sen(q1 + q2) cos(q1 + q2) 0 l1 sen(q1) + l2 sen(q1 + q2)
0 0 1 1 + 2
0 0 0 1
_
.
_
_
x
0
y
0
z
0
_
_
= f
R
(q) =
_
l1 cos(q1) + l2 cos(q1 + q2)
l1 sen(q1) + l2 sen(q1 + q2)
1 + 2
_
.
Figura:
Figura 16: Cambio de posicion de casa.
H
1
0 y0
= H
Rz
0
_
2
_
H
1
0
=
_
0 1 0 0
1 0 0 0
0 0 1 0
0 0 0 1
__
cos(q1) sen(q1) 0 l1 cos(q1)
sen(q1) cos(q1) 0 l1 sen(q1)
0 0 1 1
0 0 0 1
_
Transformacion homogenea robot 2 gdl
H
1
0 y0
=
_
0 0 1 1
0 0 0 1
_
H
2
1 y1
= H
Rz
1
_
2
_
H
2
1
=
_
0 1 0 0
1 0 0 0
0 0 1 0
0 0 0 1
__
0 0 1 2
0 0 0 1
_
=
_
0 0 1 2
0 0 0 1
_
H
2
0 y0
= H
1
0 y0
H
2
1 y1
=
_
sen(q1 + q2) cos(q1 + q2) 0 l1 sen(q1) + l2 sen(q1 + q2)
cos(q1 + q2) sen(q1 + q2) 0 l1 cos(q1) l2 cos(q1 + q2)
0 0 1 1 + 2
0 0 0 1
_
Jacobiano del brazo robot de 2 gdl
El jacobiano analtico del robot de 2 gdl se obtiene como:
J(q) =
f
R
(
1
,
2
, l
1
, l
2
, q)
q
Matriz jacobiana
Considerando la posicion de casa sobre el eje x
0
(primer cuadrante).
Sean
1
,
2
, l
1
, l
2
los parametros geometricos.
J(q) =
_
l
1
sen(q
1
) l
2
sen(q
1
+ q
2
) l
2
sen(q
1
+ q
2
)
l
1
cos(q
1
) + l
2
cos(q
1
+ q
2
) l
2
cos(q
1
+ q
2
)
_
IR
22
El determinante del jacobiano esta dado por: det[J(q)] = l
1
l
2
sen(q
2
).
El determinante es cero para q
2
= 0, n y q
1
cualquier valor.
Cuando la articulacion del codo tiene alguno de esos valores para q
2
el robot entra en una singularidad.
Problemas: aplicaciones donde interviene v = J(q) q q = J(q)
1
v.
Cinematica inversa del brazo robot de 2 gdl
La cinematica inversa (sin tomar en cuenta la orientacion de la herramienta de trabajo) de un robot manipulador de
dos grados de libertad se obtiene por procedimiento geometrico (ver gura 17).
Figura:
Figura 17: Cinematica inversa de un robot planar de dos grados de libertad.
x
2
0
+ y
2
0
= [l
1
+ l
2
cos(q
2
)]
2
+ l
2
2
sen
2
(q
2
)
= l
2
1
+ l
2
2
:
1
[cos
2
(q
2
) + sen
2
(q
2
)] + 2l
1
l
2
cos(q
2
)
= l
2
1
+ l
2
2
+ 2l
1
l
2
cos(q
2
)
q
2
= acos
_
x
2
0
+y
2
0
l
2
1
l
2
2
2l1l2
_
Observe que el angulo satisface:
= atan
_
l
2
sen(q
2
)
l
1
+ l
2
cos(q
2
)
_
+ q
1
= atan
_
y
0
x
0
_
Entonces, la variable articular q
1
adquiere la siguiente forma:
q
1
= atan
_
y
0
x
0
_
= atan
_
y
0
x
0
_
atan
_
l
2
sen(q
2
)
l
1
+ l
2
cos(q
2
)
_
Por lo tanto, la cinematica inversa de un robot de 2 gdl no depende de
1
y
2
debido a que no hay proyecciones de
estos parametros sobre el plano x
0
y
0
:
Cinematica inversa del robot de 2 gdl
q
2
= acos
_
x
2
0
+y
2
0
l
2
1
l
2
2
2l1l2
_
q
1
= atan
_
y
0
x
0
_
atan
_
l
2
sen(q
2
)
l
1
+ l
2
cos(q
2
)
_
Funcion cinematica directa del robot de 2 gdl
[x
0
, y
0
, z
0
]=cinematica r2gdl(
1
, l
1
, q
1
,
2
, l
2
, q
2
)
Codigo Fuente 5 cinematica r2gdl
cinematica r2gdl.m MATLAB version 2012a
function [x0, y0, z0]=cinematica_r2gdl(beta1,l1,q1,beta2,l2,q2) 1
digits(3); 7
if (v1 & v2 & v3 & v4 &v5 & v6) %caso simbolico 8
x0=simplify(vpa(l1*cos(q1)+l2*cos(q1+q2),3)); 9
y0=simplify(vpa(l1*sin(q1)+l2*sin(q1+q2),3)); 10
z0=vpa(beta1+beta2,3); 11
x0=l1*cos(q1)+l2*cos(q1+q2); 13
y0=l1*sin(q1)+l2*sin(q1+q2); 14
z0=beta1+beta2; 15
end 16
end 17
Funcion transformacion homogenea del robot 2 gdl H
2
0
H
2
0
=H r2gdl()
Codigo Fuente 6 H r2gdl
H r2gdl.m MATLAB version 2012a
function H=H_r2gdl() 1
syms q1 q2 beta1 beta2 l1 l2 alpha1 alpha2 real 2
disp(Parametros Denavit-Hartenberg del robot planar vertical de 2 gdl) 3
disp([ l alpha d q]) 4
dh=[l1, 0, beta1, q1; l2, 0, beta2, q2]; 5
disp(dh) 6
%H
1
0
= H
Rz
0
(q
1
)H
Tz
0
(
1
)H
Tx
0
(l
1
)H
Rx
0
(0) 7
H10=HRz(q1)*HTz(beta1)*HTx(l1)*HRx(0) 8
%H
2
1
= H
Rz
0
(q
2
)H
Tz
0
(
2
)H
Tx
0
(l
2
)H
Rx
0
(0) 9
H21=HRz(q2)*HTz(beta2)*HTx(l2)*HRx(0) 10
H20=simplify(H10*H21); % H
2
0
= H
1
0
H
2
1
11
[R20, cinemat r2gdl, cero, c]=H DH(H20) 12
H=[R20, cinemat r2gdl; % R
2
0
(q
1
, q
2
), f
R
(q
1
, q
2
) 13
cero, c]; 14
end 15
Funcion cinematica inversa del robot de 2 gdl
[q
1
, q
2
]=cinv_r2gdl(l
1
, l
2
, x
0
, y
0
)
Codigo Fuente 7 cinv r2gdl
cinv r2gdl.m MATLAB version 2012a
function [q1 q2]=cinv_r2gdl(l1,l2,x0,y0) 1
q2=acos((x0.*x0+y0.*y0-l1*l1-l2*l2)/(2*l1*l2)); 2
q1=atan(y0./x0)-atan((l2*sin(q2))./(l1+l2*cos(q2))); 3
end 4
Aplicacion
Ejemplo
Dise nar un programa para MATLAB que permita desplegar en forma simbolica los parametros DH, cinematica
cartesiana y el jacobiano del robot vertical planar de 2 gdl. Ademas, programar una aplicacion donde el extremo
nal del robot trace un crculo de radio r = 0.2m, con centro en
_
x
c
, y
c
T
=
_
0.3, 0.3
T
m, con periodo de
movimiento de 6.28 segundos.
_
x
y
_
=
_
x
c
+ r sen(t)
y
c
+ r cos(t)
_
donde x
c
, y
c
representan el centro del crculo, r es el radio.
Figura:
Figura 18: Trayectoria circular que traza el robot de 2 gdl.
Codigo Fuente 8 cap5 robot2gdl
cap5 robot2gdl.m MATLAB version 2012a
clc; clear all ;close all; 1
format short 2
syms q1 q2 beta1 beta2 l1 l2 alpha1 alpha2 real 3
H20=H r2gdl() 4
disp(Transformacion homogenea del robot 2 gdl); 5
disp(H20); 6
[R20, cinemat r2gdl,cero, c]=H DH(H20) ; 7
disp(Matriz de rotacion); disp(R20); 8
disp(cinematica directa); 9
disp(cinemat r2gdl); 10
[x0, y0, z0]=cinematica r2gdl(beta1,l1,q1,beta2,l2,q2) 11
jac r2gdl=jacobian([x0; y0], [q1;q2]) 12
det r2gdl=simplify(det(jac r2gdl)) % det[J]= l1l2 sen(q2) 13
t=0:0.001:100; %parametros del crculo:
xc, yc
T
=
0.3, 0.3
T
y radio r = 0.2 15
xc=0.3; yc=-0.3; r=0.20; 16
l1=0.45; l2=0.45; 17
beta1=0.1; beta2=0.1; 18
q1=[]; q2=[]; 19
% ecuacion del crculo 20
x=xc+r*sin(t) ; 21
y=yc+r*cos(t) ; 22
% cinematica inversa 23
[q1,q2]=cinv r2gdl(l1,l2,x,y) ; 24
%coordenas cartesianas del extremo nal del robot de 2 gdl 25
[x0, y0, z0]=cinematica r2gdl(beta1,l1,q1,beta2,l2,q2) ; 26
plot(x0,y0) 27
Robot antropomorco (RRR)
La gran mayora de los robots industriales tienen la conguracion antropomorca (brazos robots) ya que presentan
mayor destreza en su espacio de trabajo debido a que sus eslabones estan unidos por articulaciones rotacionales. El
espacio de trabajo de la conguracion antropomorca corresponde a una esfera, cuyo radio corresponde a la suma
de longitudes de los eslabones. Por similitud con la forma anatomica con el brazo humano, la segunda articulacion
se conoce como hombro (shoulder) y la tercera articulacion se llama codo (elbow). La gura 19 muestra ejemplos
de robots industriales antropomorcos de la compa na ABB y FANUC que se utilizan en aplicaciones de traslado de
objetos.
Figura:
Figura 19: Robots industriales en conguracion antropomorca: ABB y FANUC.
Cinematica directa cartesiana del brazo robot de 3 gdl
Figura:
Figura 20: Robot antropomorco.
Figura:
Figura 21: Robot antropomorco.
Tabla: DH del robot de 3 gdl
Eslabon l
i

i
d
i

i
1 0

2
l
1
+
1
q
1
2 l
2
0
2
q
2
3 l
3
0
3
q
3
Matriz de transformacion homogenea H
3
0
robot antropomorco de 3 gdl
H
1
0
= H
Rz
0
_
q
1
_
H
Tz
0
_
l
1
+
1
_
H
Tx
0
_
0
_
H
Rx
0
_
2
_
=
_
cos(q1) 0 sen(q1) 0
sen(q1) 0 cos(q1) 0
0 1 0 l1 + 1
0 0 0 1
_
H
2
1
= H
Rz
1
_
q
2
_
H
Tz
1
_
2
_
H
Tx
1
_
l
2
_
H
Rx
1
_
0
_
=
_
0 0 1 2
0 0 0 1
_
H
3
2
= H
Rz
2
_
q
3
_
H
Tz
2
_
2
_
H
Tx
2
_
l
3
_
H
Rx
2
_
0
_
=
_
0 0 1 3
0 0 0 1
_
Por lo tanto, la matriz homogenea del robot antropomorco de 3 gdl moviendose en su espacio tridimensional es:
H
3
0
= H
1
0
H
2
1
H
3
2
=
_
cos(q1) cos(q2 + q3) cos(q1) sen(q2 + q3) sen(q1)
2 + 3
sen(q1) + cos(q1)
l2 cos(q2) + l3 cos(q2 + q3)
sen(q1) cos(q2 + q3) sen(q1) sen(q2 + q3) cos(q1)
2 + 3
cos(q1) + sen(q1)
l2 cos(q2) + l3 cos(q2 + q3)
sen(q2 + q3) cos(q2 + q3) 0 l1 + 1 + l2 sen(q2) + l3 sen(q2 + q3)

0 0 0 1
_
_
_
x
0
y
0
z
0
_
_
=
_
_
_
2
+
3
sen(q
1
) + cos(q
1
)
_
l
2
cos(q
2
) + l
3
cos(q
2
+ q
3
)
2
+
3
cos(q
1
) + sen(q
1
)
_
l
2
cos(q
2
) + l
3
cos(q
2
+ q
3
)
l
1
+
1
+ l
2
sen(q
2
) + l
3
sen(q
2
+ q
3
)
_
_
[x
0
, y
0
, z
0
]=cinematica_r3gdl(
1
, l
1
, q
1
,
2
, l
2
, q
2
,
3
, l
3
, q
3
)
Codigo Fuente 9 cinematica r3gdl
cinematica r3gdl.m MATLAB version 2012a
function [x0, y0, z0]=cinematica_r3gdl(beta1,l1,q1,beta2,l2,q2,beta3,l3,q3) 1
v1=strcmp(dato1.class, sym);v2=strcmp(dato2.class, sym); 5
v9=strcmp(dato9.class, sym); digits(3); 9
if (v1 & v2 & v3 & v4 & v5 & v6 & v7 & v8 & v9) %caso simbolico 10
x0=simplify(vpa((beta2+beta3)*sin(q1)+cos(q1)*(l3*cos(q2 + q3) + l2*cos(q2)),3)); 11
y0=simplify(vpa(-(beta2+beta3)*cos(q1)+sin(q1)*(l3*cos(q2 + q3) + l2*cos(q2)),3)); 12
z0=simplify(vpa( l1 +beta1+ l3*sin(q2 + q3) + l2*sin(q2) ,3)); 13
x0=vpa(x0); y0=vpa(y0); 14
x0=(beta2+beta3)*sin(q1)+cos(q1).*(l3*cos(q2 + q3) + l2*cos(q2)); 16
y0=-(beta2+beta3)*cos(q1)+sin(q1).*(l3*cos(q2 + q3) + l2*cos(q2)); 17
z0=l1 +beta1+ l3*sin(q2 + q3) + l2*sin(q2); 18
end 19
end 20
Cinematica inversa del robot antropomorco de 3 gdl
Figura:
Figura 22: Metodo geometrico de la cinematica inversa.
= + q
1
q
1
= .
= atan
_
y0
x0
_
.
= atan
_
_

2
+
3
_
x
2
0
+ y
2
0

_
2
+
3
_
2
_
_
q
1
= = atan
_
y
0
x
0
_
atan
_
_

2
+
3
_
x
2
0
+ y
2
0

_
2
+
3
_
2
_
_
Figura:
Figura 23: Metodo geometrico de la cinematica inversa.
x
2
0
+ y
2
0
+ z
2
0
= [l
2
+ l
3
cos(q
3
)]
2
+ l
2
3
sen
2
(q
3
)
= l
2
2
+ l
2
3
:
1
[cos
2
(q
3
) + sen
2
(q
3
)] + 2l
2
l
3
cos(q
3
)
= l
2
2
+ l
2
3
+ 2l
2
l
3
cos(q
3
)
cos(q
3
) =
x
2
0
+ y
2
0
+ z
2
0
l
2
2
l
2
3
2l
2
l
3
sen(q
3
) =
1
_
x
2
0
+ y
2
0
+ z
2
0
l
2
2
l
2
3
2l
2
l
3
_
2
q
3
= atan
_
_
_
_
_
1
_
x
2
0
+y
2
0
+z
2
0
l
2
2
l
2
3
2l2l3
_
2
x
2
0
+y
2
0
+z
2
0
l
2
2
l
2
3
2l2l3
_
_
_
_
tan ( + q
2
) =
tan (q
2
) + tan ()
1 tan (q
2
) tan ()
=
z
0
_
x
2
0
+ y
2
0
Realizando sencillos pasos algebraicos, para determinar la funcion tangente de q
2
:
tan (q
2
) + tan =
z
0
_
x
2
0
+ y
2
0
z
0
_
x
2
0
+ y
2
0
tan (q
2
) tan ()
tan (q
2
)
_
1 +
z0
x
2
0
+y
2
0
tan ()
_
=
z
0
_
x
2
0
+ y
2
0
tan
tan (q
2
) =
z0
x
2
0
+y
2
0
tan
_
1 +
z0
x
2
0
+y
2
0
tan ()
_ =
z0
x
2
0
+y
2
0
l3 sen(q3)
l2+l3 cos(q3)
_
1 +
z0
x
2
0
+y
2
0
l3 sen(q3)
l2+l3 cos(q3)
_
=
_
l
2
+ l
3
cos(q
3
)
z
0
l
3
sen(q
3
)
_
x
2
0
+ y
2
0
_
x
2
0
+ y
2
0
_
l
2
+ l
3
cos(q
3
)
+ z
0
l
3
sen(q
3
)
Cinematica inversa
q
1
= atan
_
y
0
x
0
_
atan
_
_

2
+
3
_
x
2
0
+ y
2
0

_
2
+
3
_
2
_
_
q
2
= atan
_
_
l
2
+ l
3
cos(q
3
)
z
0
l
3
sen(q
3
)
_
x
2
0
+ y
2
0
_
x
2
0
+ y
2
0
_
l
2
+ l
3
cos(q
3
)
+ z
0
l
3
sen(q
3
)
_
q
3
= atan
_
_
_
(2l
2
l
3
)
2
[x
2
0
+ y
2
0
+ z
2
0
l
2
2
l
2
3
]
2
x
2
0
+ y
2
0
+ z
2
0
l
2
2
l
2
3
_
_
Jacobiano del brazo robot (antropomorco) de 3 gdl
Jacobiando del robot J(q) =
f (q)
q
:
J(q) =
_
_
j
11
j
12
j
13
j
21
j
22
j
23
j
31
j
32
j
33
_
_
j
11
=
_
2
+
3
cos(q
1
) sen(q
1
)
_
l
2
cos(q
2
) + l
3
cos(q
2
+ q
3
)
j
12
= cos(q
1
)
_
l
2
sen(q
2
) + l
3
sen(q
2
+ q
3
)
j
13
= l
3
cos(q
1
) sen(q
2
+ q
3
)
j
21
=
_
2
+
3
sen(q
1
) + cos(q
1
)
_
l
2
cos(q
2
) + l
3
cos(q
2
+ q
3
)
j
22
= sen(q
1
)
_
l
2
sen(q
2
) + l
3
sen(q
2
+ q
3
)
j
23
= l
3
sen(q
1
) sen(q
2
+ q
3
)
j
31
= 0, j
32
= l
2
cos(q
2
) + l
3
cos(q
2
+ q
3
), j
33
= l
3
cos(q
2
+ q
3
)
El determinante del jacobiano del brazo robot de 3 gdl esta dado por:
det[J(q)] = l
2
2
l3 cos(q2) sen(q3) l2l
2
3
sen(q2) cos(q3)
2
l2l
2
3
cos(q2) sen(q3) cos(q3) + l2l
2
3
sen(q2) (2)
existen un n umero innito de conguraciones singulares para q
2
= 0 n y q
3
= 0 n. Sin embargo, el angulo q
1
no interviene con los puntos singulares.
Funcion transformacion homogenea del robot antropomorco de 3 gdl H
3
0
H
3
0
=H_r3gdl()
Codigo Fuente 10 H r3gdl
H r3gdl.m MATLAB version 2012a
function H=H_r3gdl() 1
syms q1 q2 q3 beta1 beta2 beta3 l1 l2 l3 alpha1 alpha2 alpha3 real 2
disp(Parametros Denavit-Hartenberg del robot planar vertical de 3 gdl) 3
disp([ l alpha d q]) 4
dh=[l1, pi/2, l1+beta1, q1; l2, 0, beta2, q2; l3, 0, beta3, q3]; disp(dh) 5
% H
1
0
= H
Rz
0
(q
1
)H
Tz
0
(l
1
+
1
)H
Tx
0
(0)H
Rx
0
(
2
) 6
H10=simplify(HRz(q1)*HTz(l1+beta1)*HTx(0)*HRx(pi/2)); 7
% H
2
1
= H
Rz
1
(q
2
)H
Tz
1
(
2
)H
Tx
1
(l
2
)H
Rx
1
(0) 8
H21=simplify(HRz(q2)*HTz(beta2)*HTx(l2)*HRx(0)) ; 9
% H
3
2
= H
Rz
2
(q
3
)H
Tz
2
(
3
)H
Tx
2
(l
3
)H
Rx
2
(0) 10
H32=simplify(HRz(q3)*HTz(beta3)*HTx(l3)*HRx(0)); 11
H30=simplify(H10*H21*H32,3); % H
3
0
= H
1
0
H
2
1
H
3
2
12
[R30, cinemat r3gdl, cero, c]=H DH(H30); % R
3
0
, f
R
(q1, q2, q3) 13
H=[R30, cinemat r3gdl; 14
cero, c]; 15
end 16
Ejemplo
Dise nar un programa en lenguaje MATLAB que permita desplegar en forma simbolica los parametros DH,
cinematica cartesiana y el jacobiano del robot antropomorco de 3 gdl. Asimismo programar una aplicacion donde
el extremo nal del robot trace trayectorias circulares sobre el eje z
0
.
Figura:
Figura 24: Trazo que realiza el extremo nal del brazo robot de 3 gdl.
Codigo Fuente 11 cap5 r3gdl
cap5 r3gdl.m MATLAB version 2012a
clc; 1
clear all; 2
close all; 3
format short 4
syms q1 q2 q3 beta1 beta2 beta3 l1 l2 l3 alpha1 alpha2 alpha3 real 5
H30=H r3gdl() ; 6
disp(Transformacion homogenea del robot antropomorco de 3 gdl); 7
disp(H30); 8
[R30, cinemat r3gdl,cero, c]=H DH(H30) ; 9
disp(Matriz de rotacion); 10
disp(R30); 11
disp(cinematica directa); 12
disp(cinemat r3gdl); 13
[x0, y0,z0]= cinematica r3gdl(beta1,l1,q1,beta2,l2,q2,beta3,l3,q3); disp([x0; y0;z0]) 14
jac r3gdl=jacobian([x0; y0;z0], [q1;q2;q3]) 15
det r3gdl=simplify(vpa(det(jac r3gdl),3)) 16
l1=0; l2=0.45; l3=0.45; 18
t=0:0.001:100; 19
xc=0.3; yc=-0.3; r=0.20; 20
beta1=0.12; beta2=0.01; beta3=0.01; 21
q1=[]; q2=[]; q3=[]; 22
x=xc+r*sin(t) ; 23
y=yc+r*cos(t); 24
z=t; 25
[q1, q2, q3]=cinv r3gdl(beta2,beta3,l2,l3,x,y,z); 26
[x0, y0,z0]=cinematica r3gdl(beta1,l1,q1,beta2,l2,q2,beta3,l3,q3); 27
plot3(x0,y0,z0) 28
Tarea
Considere un sistema mecatronico denominado centrfuga como el que se muestra en en la gura 25.
Obtener:
(a) La tabla de parametros DH.
(b) La matriz homogenea.
(c) Cinematica directa.
(d) Cinematica inversa.
Asimismo, realizar un programa que despliegue en forma simbolica los anteriores incisos.
Figura:
Figura 25: Centrfuga.

CRM Clase2

Încărcat de

Informații document

Titlu original

Drepturi de autor

Formate disponibile

Partajați acest document

Partajați sau inserați document

Opțiuni de partajare

Vi se pare util acest document?

Este necorespunzător acest conținut?

Drepturi de autor:

Formate disponibile

CRM Clase2

Încărcat de

Drepturi de autor:

Formate disponibile

Cinematica

Facultad de Ciencias de la Electronica

l2 cos(q2) + l3 cos(q2 + q3)

sen(q1) cos(q2 + q3) sen(q1) sen(q2 + q3) cos(q1)

l2 cos(q2) + l3 cos(q2 + q3)

sen(q2 + q3) cos(q2 + q3) 0 l1 + 1 + l2 sen(q2) + l3 sen(q2 + q3)

S-ar putea să vă placă și