robotica_clase8

Cinematica

Facultad de Ciencias de la Electronica

Benemerita Universidad Autonoma de Puebla

Ingenierıa Mecatronica/Licenciatura en Electronica

Fernando Reyes Cortes

Robotica

ftp://ece.buap.mx/pub/FernandoReyes/robotica primavera2013/

Primavera 2013

Cinematica de robots manipuladores Articulaciones Clasificacion de robots industriales Nomenclatura para robots industriales Convencion Denavit-Hartenberg Robots antropomorfico

Parte II

Cinematica de robots manipuladores

ContenidoRobot industrial

Tipos de articulaciones

Calsificacion de robots industriales

Nomenclatura de robots industriales

Convencion Denavit Hartenberg

Robots antropomorficos

Pendulo robotRobot de 2 gdlRobot de 3 gdl

Fernando Reyes Cortes Facultad de Ciencias de la Electronica

Cinematica Robotica 2 / 54


Clasificacion de robots industriales

Un robot industrial esta compuesto por una serie consecutiva de eslabones y articulaciones para formar una cadenaen cinematica abierta, la cual es la estructura mecanica basica de un robot industrial. La cadena en cinematica abiertaesta formada de la siguiente manera: la primera articulacion sirve para formar la base; a continuacion siguen conexionessucesivas entre articulaciones y eslabones, en el extremo final del ultimo eslabon no hay articulacion, generalmente sedestina a colocar la herramienta de trabajo para llevar a cabo una aplicacion especıfica. El extremo final del robot nose encuentra conectado fısicamente a la base como se muestra en la figura 1:

Figura:

Figura 1: Cadena en cinematica abierta.




ArticulacionesLas articulaciones se construyen por medio de un servomotor y representan la interconexion entre dos eslabonesconsecutivos. Una articulacion puede realizar solo un tipo de movimiento, ya sea lineal, tambien conocida comoprismatica, y rotacional. La figura 2 presenta el tipo de articulaciones:

Figura:

Figura 2: Tipo de articulaciones: rotacional y lineal o prismatica.

Tabla: Clasificacion de robots industriales

Robot CaracterısticasAntropomorfico (RRR) 3 articulaciones rotacionalesSCARA (RRP) 2 articulaciones rotacionales y 1 prismaticaEsferico (RRP) 2 articulaciones rotacionales y 1 prismaticaCilındrico (RPP) 1 articulacion rotacional y 2 prismaticasCartesiano (PPP) 3 articulaciones prismaticas




Clasificacion de robots industrialesTabla: Clasificacion de robots industriales

Robot CaracterısticasAntropomorfico (RRR) 3 articulaciones rotacionalesSCARA (RRP) 2 articulaciones rotacionales y 1 prismaticaEsferico (RRP) 2 articulaciones rotacionales y 1 prismaticaCilındrico (RPP) 1 articulacion rotacional y 2 prismaticasCartesiano (PPP) 3 articulaciones prismaticas

Figura:

Figura 3: Clasificacion 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 siguientearticulacion. En la figura 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 realizartodos sus posibles movimientos. El espacio de trabajo esta determinado por la geometrıa del robot y la naturaleza desus 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 seguridadpara que ninguna persona pueda invadir su area. Un robot industrial puede tener un peso de mas de tres toneladasy alcanzar velocidades superiores a 3000 mm/seg. Mientras el robot esta en movimiento, resulta peligroso para unusuario que se encuentre dentro de su espacio de trabajo.





Figura:

Figura 7: Espacio de trabajo de un robot industrial.





El extremo final (end-effector) es la parte terminal o final del ultimo eslabon, destinado a colocar la herramienta

adecuada para una aplicacion especıfica. La posicion del extremo final se representa por[x , y , z

]Ty su orientacion se

denota a traves de los angulos de Euler.

Figura:

Figura 7: Elementos de un eslabon.




Nomenclatura para robots industrialesLa figura 8 muestra la analogıa entre el brazo humano y un brazo robot o robot industrial. La articulacion de la base correspondea la cintura. La articulacion del hombro (shoulder) debe ser la de mayor capacidad con respecto a las otras articulaciones, yaque es la que mueve y soporta el peso de la articulacion del codo (elbow) y de la herramienta de trabajo, ası como la carga deobjetos 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 ingenierıa, ya que ofreceun 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 representacionmınima de la orientacion y traslacion de robots manipuladores. Consiste en determinar una tabla de parametrosrelacionados con los eslabones del robot. La convencion Denavit-Hartenberg toma como referencia el diagrama de unrobot manipulador en cadena cinematica abierta como se muestra en la figura 10.

Las variables articulares en la representacion Denavit-Hartenberg se denotan con:

θi para el tipo rotacional.

Prismatica o lineal por di ; este parametro di tambien hace el papel de representar el ancho del servomotor de laarticulacion rotacional mas el espesor de la placa metalica del eslabon, en este caso se denota por el sımbolo βi .

La longitud del eslabon se representa con li .

El angulo de separacion entre los ejes zi y zi−1 se denota con αi .





Figura:

Figura 10: Convencion Denavit-Hartenberg para un robot manipulador.

El angulo θi es el angulo entre los ejes xi−1 y xi medido alrededor del eje zi−1.

di es la distancia del origen del sistema de referencia i − 1 a la interseccion del eje xi con el eje zi−1. Su medicionse realiza a lo largo del eje zi−1.

El parametro li se define como la distancia a lo largo del eje xi desde el origen del sistema de referencia coordenadoi − 1 hasta la interseccion del eje zi−1 con el eje xi .

El angulo entre los ejes zi y zi−1 se denota por αi , su medicion es respecto a un plano normal a xi . Una medicionde angulo positivo para αi se toma en direccion del eje zi−1 hacia zi .





Figura:

Figura 11: Convencion para medir θi y αi .




Seleccion de sistemas de referencia DH

Procedimiento:

El eje zi se asigna rıgidamente a la articulacion i + 1. Es decir, z0 es el eje de la articulacion 1, z1 es el eje de laarticulacion 2, y ası sucesivamente.

Localizar el origen o i del sistema de referencia Σi

(xi , yi , zi

)en la interseccion del eje zi con la normal comun a

los ejes zi−1 y zi .

Seleccionar el eje xi−1 sobre la normal que une los ejes zi−1 y zi en direccion de la articulacion i − 1 hacia laarticulacion i .

Definir el angulo de torsion αi , este es el angulo entre los ejes zi y zi−1 y se mide con valor positivo en el sentidode las manecillas del reloj sobre el eje xi .

Seleccionar el eje yi 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

(x0, y0, z0

)solo la direccion del eje z0 es especificada, entonces su origen o0 y el

eje x0 pueden ser seleccionados de manera arbitraria.

Para el sistema de referencia Σn

(xn , yn , zn

)no existe la articulacion n+1, entonces el eje zn no esta completamente

definido, mientras que el eje xn es normal al eje zn−1. Tıpicamente la n-esima articulacion es rotatoria, por lotanto zn se alinea en la direccion de zn−1.

Cuando dos ejes consecutivos zi y zi−1 son paralelos entre sı, la normal comun entre ellos no es unica.

Cuando dos ejes consecutivos zi y zi−1 se interceptan, la direccion del eje xi es arbitraria.

Cuando la articulacion i -esima es lineal o prismatica, entonces la direccion de zi es arbitraria.




1. li es la longitud del i -esimo eslabon, es la distancia del eje zi−1 hacia el eje zi medida sobre el eje xi−1.

2. αi es el angulo de torsion, el cual representa el angulo entre los ejes zi−1 a zi medido en el sentido de lasmanecillas del reloj sobre el eje xi .

3. di se emplea en articulaciones lineales o prismaticas y representa el desplazamiento lineal. Cuando laarticulacion es rotacional, entonces representa el offset o espesor del servomotor (la distancia de xi−1 axi medido sobre el eje zi−1), se denota por βi .

4. θi es el desplazamiento rotacional de xi−1 a xi medido alrededor del eje zi−1. El signo positivo de θi es elsentido contrario a las manecillas del reloj.

Observese que li y βi siempre seran positivos puesto que corresponden a longitudes, mientras que αi , di , θi representancantidades con signo.

Tabla: Parametros Denavit-Hartenberg

Caracterısticas de eslabones

li Longitud del eslabon i-esimo.

di

Articulaciones lineales o prismaticas. Tam-

bıen representa el espesor del servomotor

(βi).

αi Angulo entre los ejes zi−1 y zi medido con respecto al eje xi.

θi

articulaciones rotacionales; representa el an-

gulo entre los ejes xi−1 y xi medido alrededor

del eje zi−1.




Representacion Denavit-Hartenberg con transformaciones homogeneas H ii−1

La representacion DH es a traves del producto de cuatro transformaciones basicas:

H ii−1 = HRzi−1

(θi)HTzi−1(di ; βi)HTxi−1

(li)HRxi−1(αi)

=

cos(θi) − sen(θi) 0 0sen(θi) cos(θi) 0 0

0 0 1 10 0 0 1

1 0 0 00 1 0 00 0 1 di (βi)0 0 0 1

1 0 0 li0 1 0 00 0 1 00 0 0 1

1 0 0 00 cos(αi) − sen(αi) 00 sen(αi) cos(αi) 00 0 0 1

=

cos(θi) − sen(θi) cos(αi) sen(θi) sen(αi) li cos(θi)sen(θi) cos(θi) cos(αi) − cos(θi) sen(αi) li sen(θi)

0 sen(αi) cos(αi) di (βi)0 0 0 1

La transformacion homogenea total se obtiene como H n

0 = H 10 H 2

1 · · ·H n−1n−2 H n

n−1. La cinematica directa es la forma

general de transformaciones homogeneas que concatena los sistemas de referencia cartesianos asociados a los eslabonesdel robot, todos relativos al sistema de referencia fijo Σ0.




Algoritmo Denavit-Hartenberg1. Localizar la direccion de los ejes z0, z1, · · · , zn−1.

2. Establecer el sistema de referencia cartesiano fijo Σ0

(x0, y0, z0

)cuyo origen es colocado sobre el sistema

de referencia en la base del robot. Los ejes x0, y0 son determinados de acuerdo con la regla de la manoderecha.Iniciar un proceso iterativo en el cual se define el sistema de referencia Σi

(xi , yi , zi

)usando el sistema

de referencia Σi−1(xi−1,yi−1, zi−1), iniciando con el sistema de referencia Σ1

(x1, y1, z1

). 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 comun que une al eje zi con el eje zi−1. Si el eje ziintercepta al eje zi−1 colocar o i en la intercepcion. Para el caso en que los ejes zi y zi−1 son paralelos:

Si la articulacion i -esima es rotacional, colocar el origen o i sobre la articulacion i -esima, tal quedi = 0.Si la articulacion i -esima es prismatica, colocar el origen o i en un punto lımite fısico de laarticulacion i -esima, por ejemplo en un punto extremo.

4. Seleccionar el eje xi a lo largo de la normal comun que une a los ejes zi−1 y zi , en direccion de laarticulacion i − 1 hacia la articulacion i .

5. Determinar yi por la regla de la mano derecha.

6. Establecer el sistema de referencia del extremo final Σn

(xn , yn , zn

).

Si la articulacion n-esima es rotatoria, entonces alinear el eje zn con el eje zn−1

Si la articulacion n-esima es prismatica, entonces seleccionar el eje zn de forma arbitraria. El eje xn

debe cumplir el paso 4.

7. Establecer la tabla de parametros de eslabones.

8. Obtener las matrices de transformaciones homogeneas: H ii−1 para i = 1, 2, · · · , n − 1.




Pendulo robot

Como un caso particular del robot de 3 grados de libertad antropomorfico, se encuentra el pendulo robot de 1 gradode libertad formado por un servomotor que tiene acoplado mecanicamente una barra metalica de longitud l1 y estasometido al fenomeno de gravedad. El movimiento del pendulo se encuentra en el plano vertical x0−y0 como se indicaen la figura 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(x0, y0, z0) se coloca sobre la articulacion del pendulo (en el respaldodel estator del servomotor), el eje z0 se alinea con el eje de giro del servomotor el cual es perpendicular al plano x0−y0.El servomotor del pendulo tiene un espesor de longitud β1 el cual ya incluye el ancho de la barra metalica. El origendel sistema de referencia Σ1(x1, y1, z1) se coloca en el extremo final de la barra y se mueve de manera conjunta alpendulo, el origen Σ1 tiene las siguientes coordenadas con respecto al origen de Σ0(x0, y0, z0): [l1 cos(q1), l1 sen(q1), β1].Debido al espesor β1 el origen de Σ1 mantiene una distancia β1 sobre el eje z0. Los ejes z1 y z0 son paralelos entre sı,por lo tanto el angulo α1 que existe entre ellos es α1 = 0.

El plano x1− y1 tiene una rotacion de q1 grados con respecto al plano x0− y0 del sistema fijo Σ0(x0, y0, z0), y la matrizRz0

(q1

)determina dicha rotacion:

R10 = Rz0

(q1

)=

cos(q1) − sen(q1) 0sen(q1) cos(q1) 0

0 0 1

.

Tabla: DH del pendulo

Eslabon l α d θ1 l1 0 β1 q1




H 10 = HR(z0, q1)HT (z0, β1)HT (x0, l1)HR(x0, 0)

=

cos(q1) − sen(q1) 0 l1 cos(q1)sen(q1) cos(q1) 0 l1 sen(q1)

0 0 1 β1

0 0 0 1

.

Las coordenadas cartesianas del extremo final del pendulo robot (cinematica directa cartesiana), es decir el origen delsistema Σ1(x1, y1, z1) con respecto al sistema fijo Σ0(x0, y0, z0) se encuentra determinado por:x0

y0

z0

=

l1 cos(q1)l1 sen(q1)

β1

.




Las coordenadas cartesianas del extremo final del pendulo dependen de la ubicacion de la posicion de casa (homeposition), la cual es el punto de reposo u origen del pendulo que corresponde a q1 = 0; la posicion de casa estacolocada sobre el eje x0+ . La matriz de rotacion Rz0

(q1

)que determina la rotacion entre los sistemas de referencia fijo

Σ0(x0, y0, z0) y Σ1(x1, y1, z1) fue obtenida con respecto al primer cuadrante, es decir en el plano x0+ − y0+ .

Otra posibilidad para la posicion de casa del pendulo es colocar el origen q1 = 0 sobre el lado negativo del eje y0 comose muestra en la figura 14.

Figura:

Figura 14: Cambio de posicion de casa del pendulo sobre el eje y0− .




R10 = Rz0

(−π

2

) cos(q1) − sen(q1) 0sen(q1) cos(q1) 0

0 0 1

=

0 1 0−1 0 00 0 1

cos(q1) − sen(q1) 0sen(q1) cos(q1) 0

0 0 1

=

sen(q1) cos(q1) 0− cos(q1) sen(q1) 0

0 0 1

H 10 =

Rz0

(−π

2

) cos(q1) − sen(q1) 0sen(q1) cos(q1) 0

0 0 1

0T

Rz0

(−π

2

) l cos(q)l sen(q)

β1

1

=

sen(q1) cos(q1) 0 l1 sen(q1)− cos(q1) sen(q1) 0 −l1 cos(q1)

0 0 1 β1

0 0 0 1

.

x0

y0

z0

=

l1 sen(q1)−l1 cos(q1)

β1




Jacobiano del pendulo

Debido a que el pendulo robot es un sistema escalar, no tiene matriz jacobiana. Sin embargo, se puede realizar lasiguiente interpretacion partiendo de la cinematica diferencial:

v =d

dt

[l1 cos(q1)l1 sen(q1)

]=

∂f R(q1)

∂q1

q1 =

[−l1 sen(q1)l1 cos(q1)

]q1

=

[−l1 sen(q1) 0

0 l1 cos(q1)

]︸︷︷︸

J (q1)

[q1

q1

]︸︷︷︸q

= J (q1)q (1)

el jacobiano J (q1) se puede pensar como esta expresado en la ecuacion (1). Notese que existe una singularidad cuandola variable articular q1 = 0,±nπ, ya que su determinante esta dado como: det[J (q1)] = −l21 sen(q1) cos(q1). Tambienexisten singularidades en q1 = ±n π

2, esto ultimo puede verificarse en la cinematica inversa, si q1 = ±n π

2⇒ x0 = 0,

entonces se indefine la funcion arco-tangente (por ejemplo division entre cero).




Funcion transformacion homogenea del pendulo H 10

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 real2

disp(’Parametros Denavit-Hartenberg del pendulo’)3

disp(’[ l alpha d q]’) dh=[l1, 0, beta1, q1];4

disp(dh)5

%H 10 = HRz0

(q1)HTz0(β1)HTx0

(l1)HRx0(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

end11




Cinematica inversa del pendulo

q1 = atan

(y0

x0

).

Codigo Fuente 2 cinv pendulo



cinv pendulo.m MATLAB version 2012a

function q1=cinv_pendulo(x0,y0)1

%cinematica inversa del pendulo2

q1=atan(y0/x0);3

end4




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 simbolico5

x0=l1*cos(q1);6

y0=l1*sin(q1);7

z0=beta1;8

end9

else%caso numerico10

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

end14




Ejemplo

Desarrollar un programa para MATLAB que presente de manera simbolica los parametros DH, cinematica cartesianay el jacobiano del pendulo. Ademas, contemple el analisis donde la posicion de casa se traslada del primer al cuartocuadrante.

Codigo Fuente 4 pendulo



pendulo.m MATLAB version 2012aclc;1clear all;2close all;3format short4syms beta1 l1 q1 real5

%H10 = HRz0

(q1)HTz0(β1)HTx0

(l1)HRx0(0)6

H10=H pendulo() %H107

%obtiene la matriz de rotacion R10 y cinematica directa f r (q)8

[R10, frq pendulo, cero, c]=H DH(H10)%R10, f R(β1, l1, q1)9

jac pendulo=jacobian(frq pendulo, q1)10H10a=HRz(-pi/2)*H10 %H1

0 a = HRz0

`−π

2´H1

011%ejemplo numerico12q1=55*pi/180; %posicion angular del pendulo13beta1=0.1;%ancho del servomotor mas espesor de la barra14l1=0.45; beta1=0.1; %longitud del pendulo y espesor del servomotor15%cinematica cartesiana cuadrante I: f RI

(β1, l1, q1)16[x0,y0,z0]=cinematica pendulo(beta1,l1,q1)17%cinematica directa en el cuadrante IV18%f RIV

(β1, l1, q1) = Rz (−π2 )f RI

(β1, l1, q1)19

Rz(-pi/2)*cinematica pendulo(beta1,l1,q1)20




Robot antropomorfico de 2 gdl

La figura 15 muestra un robot de dos grados de libertad con articulaciones rotacionales que se mueve en el planovertical x0 − y0. El sistema de referencia fijo Σ0(x0, y0, z0) se coloca en el respaldo del servomotor del hombro, de talforma que el eje z0 coincida con el eje de rotacion (perpendicular al plano de la hoja). El ancho de cada servomotory espesor de la barra metalica estan determinados por β1 y β2. Los ejes x0, y0 se seleccionan con la regla de la manoderecha.

Figura:

Figura 15: Robot planar vertical de dos gdl.

Parametros DH

Tabla: Parametros DH para el robot de 2gdl

Eslabon li αi di θi

1 l1 0 β1 q1

2 l2 0 β2 q2





Transformaciones homogeneas del robot de 2 gdl

H 10 = HRz0

(q1

)HTz0

(β1

)HTx0

(l1)HRx0

(0)

=


0 0 1 β1

0 0 0 1

H 2

1 = HRz1

(q2

)HTz1

(β2

)HTx1

(l2)HRx1

(0)

=


0 0 1 β2

0 0 0 1

H 2

0 = H 10 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

].

x0

y0

z0

= 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 10 y0−

= HRz0

(−π

2

)H 1

0

=

[0 1 0 0−1 0 0 00 0 1 00 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 10 y0−

=

[sen(q1) cos(q1) 0 l1 sen(q1)− cos(q1) sen(q1) 0 −l1 cos(q1)

0 0 1 β10 0 0 1

]H 2

1 y1−= HRz1

(−π

2

)H 2

1

=

[0 1 0 0−1 0 0 00 0 1 00 0 0 1

][cos(q2) − sen(q2) 0 l2 cos(q2)sen(q2) cos(q2) 0 l2 sen(q2)

0 0 1 β2

0 0 0 1

]

=

[sen(q2) cos(q2) 0 l2 sen(q2)− cos(q2) sen(q2) 0 −l2 cos(q2)

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 + β20 0 0 1

]




Jacobiano del brazo robot de 2 gdl

El jacobiano analıtico del robot de 2 gdl se obtiene como:

J (q) =∂f R(β1, β2, l1, l2, q)

∂q

Matriz jacobiana

Considerando la posicion de casa sobre el eje x0 (primer cuadrante).

Sean β1, β2, l1, l2 los parametros geometricos.

J (q) =

[−l1 sen(q1)− l2 sen(q1 + q2) −l2 sen(q1 + q2)l1 cos(q1) + l2 cos(q1 + q2) l2 cos(q1 + q2)

]∈ IR2×2

El determinante del jacobiano esta dado por: det[J (q)] = l1l2 sen(q2).

El determinante es cero para q2 = 0,±nπ y q1 cualquier valor.

Cuando la articulacion del codo tiene alguno de esos valores para q2 el robot entra en una singularidad.

Problemas: aplicaciones donde interviene v = J (q)q ⇒ q = J (q)−1v .




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 dedos grados de libertad se obtiene por procedimiento geometrico (ver figura 17).

Figura:

Figura 17: Cinematica inversa de un robot planar de dos grados de libertad.

x 20 + y2

0 = [l1 + l2 cos(q2)]2 + l22 sen2(q2)

= l21 + l22��:1

[cos2(q2) + sen2(q2)] + 2l1l2 cos(q2)

= l21 + l22 + 2l1l2 cos(q2)

q2 = acos(

x20 +y2

0−l21−l222l1l2

)Observe que el angulo ϑ satisface:

ϑ = atan

(l2 sen(q2)

l1 + l2 cos(q2)

)




ϑ + q1 = atan

(y0

x0

)Entonces, la variable articular q1 adquiere la siguiente forma:

q1 = atan

(y0

x0

)− ϑ

= atan

(y0

x0

)− atan

(l2 sen(q2)

l1 + l2 cos(q2)

)Por lo tanto, la cinematica inversa de un robot de 2 gdl no depende de β1 y β2 debido a que no hay proyecciones deestos parametros sobre el plano x0 − y0:

Cinematica inversa del robot de 2 gdl

q2 = acos(

x20 +y2

0−l21−l222l1l2

)q1 = atan

(y0

x0

)− atan

(l2 sen(q2)

l1 + l2 cos(q2)

)




Funcion cinematica directa del robot de 2 gdl[x0, y0, z0]=cinematica r2gdl(β1, l1, q1, β2, l2, q2)

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 simbolico8

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

else%caso numerico12

x0=l1*cos(q1)+l2*cos(q1+q2);13

y0=l1*sin(q1)+l2*sin(q1+q2);14

z0=beta1+beta2;15

end16

end17Fernando Reyes Cortes Facultad de Ciencias de la Electronica



Funcion transformacion homogenea del robot 2 gdl H 20

H 20 =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 real2

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 10 = HRz0

(q1)HTz0(β1)HTx0

(l1)HRx0(0)7

H10=HRz(q1)*HTz(beta1)*HTx(l1)*HRx(0)8

%H 21 = HRz0

(q2)HTz0(β2)HTx0

(l2)HRx0(0)9

H21=HRz(q2)*HTz(beta2)*HTx(l2)*HRx(0)10

H20=simplify(H10*H21);% H 20 = H 1

0 H 2111

[R20, cinemat r2gdl, cero, c]=H DH(H20)12

H=[R20, cinemat r2gdl; % R20(q1, q2), f R(q1, q2)13

cero, c];14

end15




Funcion cinematica inversa del robot de 2 gdl

[q1, q2]=cinv_r2gdl(l1, l2, x0, y0)

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

end4




Aplicacion

Ejemplo

Disenar un programa para MATLAB que permita desplegar en forma simbolica los parametros DH, cinematicacartesiana y el jacobiano del robot vertical planar de 2 gdl. Ademas, programar una aplicacion donde el extremo

final del robot trace un cırculo de radio r = 0.2m, con centro en[xc, yc

]T=[0.3,−0.3

]Tm, con periodo de

movimiento de 6.28 segundos. [xy

]=

[xc + r sen(t)yc + r cos(t)

]donde xc, yc representan el centro del cırculo, 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 2012aclc; clear all ;close all;1format short2syms q1 q2 beta1 beta2 l1 l2 alpha1 alpha2 real3H20=H r2gdl()4disp(’Transformacion homogenea del robot 2 gdl’);5disp(H20);6[R20, cinemat r2gdl,cero, c]=H DH(H20) ;7disp(’Matriz de rotacion’); disp(R20);8disp(’cinematica directa’);9disp(cinemat r2gdl);10[x0, y0, z0]=cinematica r2gdl(beta1,l1,q1,beta2,l2,q2)11jac r2gdl=jacobian([x0; y0], [q1;q2])12det r2gdl=simplify(det(jac r2gdl))% det[J]= l1l2 sen(q2)13

%ejemplo numerico14

t=0:0.001:100;%parametros del cırculo:ˆxc , yc

˜T=

ˆ0.3,−0.3

˜Ty radio r = 0.215

xc=0.3; yc=-0.3; r=0.20;16l1=0.45; l2=0.45;17beta1=0.1; beta2=0.1;18q1=[]; q2=[];19

% ecuacion del cırculo20x=xc+r*sin(t) ;21y=yc+r*cos(t) ;22

% cinematica inversa23[q1,q2]=cinv r2gdl(l1,l2,x,y) ;24

%coordenas cartesianas del extremo final del robot de 2 gdl25[x0, y0, z0]=cinematica r2gdl(beta1,l1,q1,beta2,l2,q2) ;26

plot(x0,y0)27




Robot antropomorfico (RRR)La gran mayorıa de los robots industriales tienen la configuracion antropomorfica (brazos robots) ya que presentanmayor destreza en su espacio de trabajo debido a que sus eslabones estan unidos por articulaciones rotacionales. Elespacio de trabajo de la configuracion antropomorfica corresponde a una esfera, cuyo radio corresponde a la sumade longitudes de los eslabones. Por similitud con la forma anatomica con el brazo humano, la segunda articulacionse conoce como hombro (shoulder) y la tercera articulacion se llama codo (elbow). La figura 19 muestra ejemplosde robots industriales antropomorficos de la companıa ABB y FANUC que se utilizan en aplicaciones de traslado deobjetos.

Figura:

Figura 19: Robots industriales en configuracion antropomorfica: ABB y FANUC.




Cinematica directa cartesiana del brazo robot de 3 gdl

Figura:

Figura 20: Robot antropomorfico.





Figura:

Figura 21: Robot antropomorfico.

Tabla: DH del robot de 3 gdl

Eslabon li αi di θi

1 0 π2

l1 + β1 q1

2 l2 0 β2 q2

3 l3 0 β3 q3




Matriz de transformacion homogenea H 30 robot antropomorfico de 3 gdl

H 10 = HRz0

(q1

)HTz0

(l1 + β1

)HTx0

(0)HRx0

(π2

)=

[cos(q1) 0 sen(q1) 0sen(q1) 0 − cos(q1) 0

0 1 0 l1 + β1

0 0 0 1

]H 2

1 = HRz1

(q2

)HTz1

(β2

)HTx1

(l2)HRx1

(0)

=

[cos(q2) − sen(q2) 0 l2 cos(q2)sen(q2) cos(q2) 0 l2 sen(q2)

0 0 1 β2

0 0 0 1

]H 3

2 = HRz2

(q3

)HTz2

(β2

)HTx2

(l3)HRx2

(0)

=

[cos(q3) − sen(q3) 0 l3 cos(q3)sen(q3) cos(q3) 0 l3 sen(q3)

0 0 1 β3

0 0 0 1

]Por lo tanto, la matriz homogenea del robot antropomorfico de 3 gdl moviendose en su espacio tridimensional es:

H 30 = H 1

0 H 21 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

]





x0

y0

z0

=

[β2 + β3

]sen(q1) + cos(q1)

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

]−[β2 + β3

]cos(q1) + sen(q1)

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

]l1 + β1 + l2 sen(q2) + l3 sen(q2 + q3)

[x0, y0, z0]=cinematica_r3gdl(β1, l1, q1, β2, l2, q2, β3, l3, q3)

Codigo Fuente 9 cinematica r3gdl



cinematica r3gdl.m MATLAB version 2012afunction [x0, y0, z0]=cinematica_r3gdl(beta1,l1,q1,beta2,l2,q2,beta3,l3,q3)1

dato1=whos(’beta1’); dato2=whos(’l1’); dato3=whos(’q1’);2dato4=whos(’beta2’); dato5=whos(’l2’); dato6=whos(’q2’);3dato7=whos(’beta3’); dato8=whos(’l3’); dato9=whos(’q3’);4v1=strcmp(dato1.class, ’sym’);v2=strcmp(dato2.class, ’sym’);5v3=strcmp(dato3.class, ’sym’);v4=strcmp(dato4.class, ’sym’);6v5=strcmp(dato5.class, ’sym’);v6=strcmp(dato6.class, ’sym’);7v7=strcmp(dato7.class, ’sym’);v8=strcmp(dato8.class, ’sym’);8v9=strcmp(dato9.class, ’sym’); digits(3);9if (v1 & v2 & v3 & v4 & v5 & v6 & v7 & v8 & v9) %caso simbolico10

x0=simplify(vpa((beta2+beta3)*sin(q1)+cos(q1)*(l3*cos(q2 + q3) + l2*cos(q2)),3));11y0=simplify(vpa(-(beta2+beta3)*cos(q1)+sin(q1)*(l3*cos(q2 + q3) + l2*cos(q2)),3));12z0=simplify(vpa( l1 +beta1+ l3*sin(q2 + q3) + l2*sin(q2) ,3));13x0=vpa(x0); y0=vpa(y0);14else%caso numerico15x0=(beta2+beta3)*sin(q1)+cos(q1).*(l3*cos(q2 + q3) + l2*cos(q2));16y0=-(beta2+beta3)*cos(q1)+sin(q1).*(l3*cos(q2 + q3) + l2*cos(q2));17z0=l1 +beta1+ l3*sin(q2 + q3) + l2*sin(q2);18

end19end20




Cinematica inversa del robot antropomorfico de 3 gdl

Figura:

Figura 22: Metodo geometrico de la cinematica inversa.

ϑ = ρ + q1 ⇒ q1 = ϑ− ρ.

ϑ = atan(

y0

x0

).

ρ = atan

β2 + β3√x 20 + y2

0 −(β2 + β3

)2

q1 = ϑ− ρ = atan

(y0

x0

)− atan

β2 + β3√x 20 + y2

0 −(β2 + β3

)2





Figura:

Figura 23: Metodo geometrico de la cinematica inversa.

x 20 + y2

0 + z 20 = [l2 + l3 cos(q3)]

2 + l23 sen2(q3)

= l22 + l23��:1

[cos2(q3) + sen2(q3)] + 2l2l3 cos(q3)

= l22 + l23 + 2l2l3 cos(q3)

cos(q3) =x 20 + y2

0 + z 20 − l22 − l23

2l2l3

sen(q3) =

√1−

[x 20 + y2

0 + z 20 − l22 − l23

2l2l3

]2

q3 = atan

√

1−[

x20 +y2

0+z20−l22−l23

2l2l3

]2x20 +y2

0+z20−l22−l23

2l2l3





tan (ϑ + q2) =tan (q2) + tan (ϑ)

1− tan (q2) tan (ϑ)=

z0√x 20 + y2

0

Realizando sencillos pasos algebraicos, para determinar la funcion tangente de q2:

tan (q2) + tan ϑ =z0√

x 20 + y2

0

− z0√x 20 + y2

0

tan (q2) tan (ϑ)

tan (q2)[1 + z0√

x20 +y2

0

tan (ϑ)]

=z0√

x 20 + y2

0

− tan ϑ

tan (q2) =

z0√x20 +y2

0

− tan ϑ[1 + z0√

x20 +y2

0

tan (ϑ)] =

z0√x20 +y2

0

− l3 sen(q3)l2+l3 cos(q3)[

1 + z0√x20 +y2

0

l3 sen(q3)l2+l3 cos(q3)

]=

[l2 + l3 cos(q3)

]z0 − l3 sen(q3)

√x 20 + y2

0√x 20 + y2

0

[l2 + l3 cos(q3)

]+ z0l3 sen(q3)





Cinematica inversa

q1 = atan

(y0

x0

)− atan

β2 + β3√x 20 + y2

0 −(β2 + β3

)2

q2 = atan

([l2 + l3 cos(q3)

]z0 − l3 sen(q3)

√x 20 + y2

0√x 20 + y2

0

[l2 + l3 cos(q3)

]+ z0l3 sen(q3)

)

q3 = atan

√

(2l2l3)2 − [x 20 + y2

0 + z 20 − l22 − l23 ]

2

x 20 + y2

0 + z 20 − l22 − l23




Jacobiano del brazo robot (antropomorfico) de 3 gdl

Jacobiando del robot J (q) =∂f (q )

∂q :

J (q) =

j11 j12 j13j21 j22 j23j31 j32 j33

j11 =

[β2 + β3

]cos(q1)− sen(q1)

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

]j12 = − cos(q1)

[l2 sen(q2) + l3 sen(q2 + q3)

]j13 = −l3 cos(q1) sen(q2 + q3)

j21 =[β2 + β3

]sen(q1) + cos(q1)

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

]j22 = − sen(q1)

[l2 sen(q2) + l3 sen(q2 + q3)

]j23 = −l3 sen(q1) sen(q2 + q3)

j31 = 0, j32 = l2 cos(q2) + l3 cos(q2 + q3), j33 = l3 cos(q2 + q3)

El determinante del jacobiano del brazo robot de 3 gdl esta dado por:

det[J (q)] = −l22 l3 cos(q2) sen(q3)− l2l23 sen(q2) cos(q3)2 − l2l23 cos(q2) sen(q3) cos(q3) + l2l23 sen(q2) (2)

existen un numero infinito de configuraciones singulares para q2 = 0 ± nπ y q3 = 0 ± nπ. Sin embargo, el angulo q1

no interviene con los puntos singulares.




Funcion transformacion homogenea del robot antropomorfico de 3 gdl H 30

H 30 =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 real2

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 10 = HRz0

(q1)HTz0(l1 + β1)HTx0

(0)HRx0(π

2 )6

H10=simplify(HRz(q1)*HTz(l1+beta1)*HTx(0)*HRx(pi/2));7

% H 21 = HRz1

(q2)HTz1(β2)HTx1

(l2)HRx1(0)8

H21=simplify(HRz(q2)*HTz(beta2)*HTx(l2)*HRx(0)) ;9

% H 32 = HRz2

(q3)HTz2(β3)HTx2

(l3)HRx2(0)10

H32=simplify(HRz(q3)*HTz(beta3)*HTx(l3)*HRx(0));11

H30=simplify(H10*H21*H32,3); % H 30 = H 1

0 H 21 H 3

212

[R30, cinemat r3gdl, cero, c]=H DH(H30);% R30 , f R(q1, q2, q3)13

H=[R30, cinemat r3gdl;14

cero, c];15

end16




Ejemplo

Disenar un programa en lenguaje MATLAB que permita desplegar en forma simbolica los parametros DH,cinematica cartesiana y el jacobiano del robot antropomorfico de 3 gdl. Asimismo programar una aplicacion dondeel extremo final del robot trace trayectorias circulares sobre el eje z0.

Figura:

Figura 24: Trazo que realiza el extremo final del brazo robot de 3 gdl.




Codigo Fuente 11 cap5 r3gdl



cap5 r3gdl.m MATLAB version 2012aclc;1clear all;2close all;3format short4syms q1 q2 q3 beta1 beta2 beta3 l1 l2 l3 alpha1 alpha2 alpha3 real5H30=H r3gdl() ;6disp(’Transformacion homogenea del robot antropomorfico de 3 gdl’);7disp(H30);8[R30, cinemat r3gdl,cero, c]=H DH(H30) ;9disp(’Matriz de rotacion’);10disp(R30);11disp(’cinematica directa’);12disp(cinemat r3gdl);13[x0, y0,z0]= cinematica r3gdl(beta1,l1,q1,beta2,l2,q2,beta3,l3,q3); disp([x0; y0;z0])14jac r3gdl=jacobian([x0; y0;z0], [q1;q2;q3])15det r3gdl=simplify(vpa(det(jac r3gdl),3))16

%ejemplo numerico17l1=0; l2=0.45; l3=0.45;18t=0:0.001:100;19xc=0.3; yc=-0.3; r=0.20;20beta1=0.12; beta2=0.01; beta3=0.01;21q1=[]; q2=[]; q3=[];22x=xc+r*sin(t) ;23y=yc+r*cos(t);24z=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 centrıfuga como el que se muestra en en la figura 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: Centrıfuga.



robotica_clase8

Documents

matlab version 2012aclc

matlab version 2012afunction

los ejes x0

posgrado en automatizacion

q1 q2

q1 q2

q2 q3

q2 q3