Universidad Nacional de San Agustín

Autor: Lizardo Pari

CAPÍTULO 3

MODELO CINEMÁTICO DEL ROBOT

3.1.- PROBLEMA CINEMÁTICO DIRECTO
“Dados la estructura mecánica y variables articulares del robot, determinar la posición y
orientación del elemento terminal respecto a la base, en el espacio cartesiano”.

3.1.1.- CADENA CINEMÁTICA

3.1.1.1.- ROBOT (SERIE)
Sistema mecánico formado por eslabones articulados entre sí.

q3 F2 qn
F1
⋅⋅⋅ n
q2
Fn −1 Fn
2
q1 1
F0 0
Figura 1

3.1.1.2.- REGLAS
- La articulación i conecta con el eslabón i con el eslabón i − 1
- Hay tantos frames (sistemas de coordenadas) como articulaciones haya, más el frame
base Fo.
- Se asume que cada articulación qi es rotacional o prismática (de traslación).
- Existe una transformación A i por cada frame que sólo depende de qi :

A i = A i ( qi ) ( 1)

- Se debe determinar:

 0 Rn 0
tn  ( 2)
=
0
Tn 0
=
Hn  T  , con n : número de articulaciones
0 1 

Donde:

30
 Universidad Nacional de San Agustín
Autor: Lizardo Pari

=
0
Tn A1 ( q1 ) ⋅ A 2 ( q2 )  A n ( qn ) ( 3)

Donde cada transformación homogénea A i tiene la forma:

 i −1 R i ti 
i −1 ( 4)
i −1
=
Ai =
Ai  T 
 0 1 

Ejemplo 1

a. Determinar la matriz homogénea para encontrar la posición del extremo (TCP: Punto
Central Terminal) del robot plano de 3 grados de libertad (GDL) (Figura 2):
b. La posición respecto a la base del punto (3,2,-1) perteneciente al frame 3.

X2 Y3
Y2
l3
q3 X3
l2
X1 TCP
q2 3
2
Y1

l1 Y0 1
q1
X0
0

Figura 2

Su posición de reposo (todos sus valores articulares =0):

Y0 Y1 Y2 Y3

X0 X1 X2
TCP X3

Figura 3.- Posición de reposo del robot

Solución a):
0
La transformación del frame 3 respecto al frame base 0 T3 es:

=
0
T3 rot ( z0 , q1 ) ⋅ trasl (l1 , 0, 0) ⋅ rot ( z1 , − q2 ) ⋅ trasl (l2 , 0, 0) ⋅ rot ( z2 , − q3 ) ⋅ trasl (l3 , 0, 0)
El punto TCP (posición (0,0,0) en el sistema del frame 3), posee las coordenadas homogéneas:

31
Universidad Nacional de San Agustín
Autor: Lizardo Pari

0 
0 
3
p TCP = 
0 
 
1 
La posición de 3
p TCP respecto a la base es:
0
=
p TCP 0
T3 ⋅ 3 p TCP

0 
0 
0
= T3 ⋅ 
p TCP 0 
0 
 
1 
b.- El punto (3,2,-1) del frame 3 respecto a la base es:

3
2
0
p = 0 Τ3 ⋅  
 −1
 
1

3.2.- MODELO DENAVIT-HARTENBERG

Busca una elección sistemática de los frames de cada eslabón. Cada transformación
homogénea A i es el resultado de cuatro transformaciones:

A i = rot ( zi −1 , θi ) ⋅ trasl ( zi −1 , di ) ⋅ trasl ( xi , ai ) ⋅ rot ( xi , α i ) ( 5)

cθi − sθi ⋅ cα i sθi ⋅ sα i ai ⋅ cθi 

 sθ cθi ⋅ cα i −cθi ⋅ sα i ai ⋅ sθi  ( 6)
Ai =  i
0 sα i cα i di 
 
0 0 0 1 

Donde α i , ai , θi , di , son parámetros asociados a cada eslabón i y articulación i . Definen la

localización espacial relativa de este eslabón respecto al anterior.

32
Universidad Nacional de San Agustín
Autor: Lizardo Pari
Tabla 1.- Nombres de los parámetros D-H

Parám. Nombre
ai Longitud de eslabón
αi Torsión de eslabón
di Distancia entre eslabones
θi Angulo entre eslabones

3.2.1.- ASIGNACIÓN DE LAS REFERENCIAS (FRAMES) ASIGNADAS A CADA ESLABÓN.

3.2.1.1.- REFERENCIA BASE ( i = 0 ).
1.- Fijar Z 0 sobre el eje de movimiento de la articulación 1.

2.- Completar X 0 , Y0 por sistema dextrógiro (MANO DERECHA).

3.2.1.2.- REFERENCIAS ( 0 < i < n )

1.- Colocar el eje Z i en el eje de movimiento de la articulación i + 1 .

2.- Colocar el origen Oi en la intersección de Z i y Z i −1 . En caso no se encontrasen, colocar Oi

en la intersección de Z i con una normal a ambos ejes.

3.- Colocar X i normal simultáneamente a Z i y Z i −1 de forma que los interseque.

4.- Completar Yi por sistema dextrógiro.

3.2.1.3.- REFERENCIAS ( i = n )
1.- Colocar Z n apuntando hacia afuera del robot.

2.- Colocar X n perpendicular al movimiento de apertura de la pinza.

3.- Completar Yn por sistema dextrógiro.

3.2.2.- TABLA DE PARÁMETROS D-H.

Se debe completar la siguiente tabla:

Tabla 2

i θi di ai αi
1
2

n
θi : Angulo de rotación de X i −1 en torno a Z i −1 para fijarlo coincidente o paralelo a X i .

di : Distancia entre Oi −1 y la intersección de Z i −1 con X i .

ai : Distancia entre Oi y la intersección de Z i −1 con X i .

α i : Angulo de rotación de Z i −1 en torno a X i para fijarlo coincidente o paralelo a Z i .

33
Universidad Nacional de San Agustín
Autor: Lizardo Pari

Ejemplo 2

Considere el robot de 4 GDL (PPRR) de la Figura 4 ,

1. Hallar el modelo cinemático D-H, es decir:

a) Asignar frames
b) Encontrar la tabla D-H.
c) Hallar las matrices A i
d) Hallar la transformación 0 T4 .
2. Graficar X 0 , Y0 , Z 0 y X 4 , Y4 , Z 4 para los casos de la Tabla 3, e indique que rotaciones
básicas se realizaron para conseguir 0 R 4 .

Tabla 3

Caso d1 d2 θ3 θ4
1 0.4m 0.3m 0 0
2 0.4m 0.3m −π / 2 0

0.2
d2 θ4
1 2 3
4

0.15
d1 0
0.1
Z0 θ3
Y0 TCP
X0
Figura 4

Solución 1 a)

Aplicamos las reglas de asignación de frames, se obtiene la siguiente figura

34
Universidad Nacional de San Agustín
Autor: Lizardo Pari

0.2
d2 Z2 Z3 θ4
Y3
O1 O2 Y2 X3
Z1 O3
X1 0.15
X2
d1 Y1 O4 0.1
Z0 θ3
Y0 TCP X4

X0 Y4 Z4
O0

Figura 5

Solución 1 b):

Se obtiene la Tabla D-H según los frames asignados y las reglas correspondientes:

i θi di ai αi
1 −π / 2 d1 0 −π / 2
2 0 d2 0 π /2
3 θ3 + π / 2 0 0.2 0
4 θ4 −0.25 0 π

Solución 1 c)

0 0 1 0 1 0 0 0
 −1 0 0 0  0 0 −1 0 
A1 =  ; A2 = 
 0 −1 0 d1  0 1 0 d2 
   
0 0 0 1 0 0 0 1

cos θ3' − sin θ3' 0 0.2 cos θ3' 

 
 sin θ3' cos θ3' 0 0.2sin θ3'  π
A3 = con θ=
'
θ3 +
 0 0 1 0  3
2
 
 0 0 0 1 

35
 Universidad Nacional de San Agustín
Autor: Lizardo Pari

cos θ 4 sin θ 4 0 0 
 
sin θ 4 − cos θ 4 0 0 
A4 = 
 0 0 −1 −0.25
 
 0 0 0 1 

Solución 1 d)

 0 0 1 0  1 0 0 0   0 1 0 d2 
 −1 0 0 0  0 0 −1 0   −1 0 0 0 
0
A2 = 
A1A 2 = ⋅ =
 0 −1 0 d1  0 1 0 d 2   0 0 1 d1 
     
 0 0 0 1  0 0 0 1 0 0 0 1

0 1 0 d 2  cos θ3' − sin θ3' 0 0.2 cos θ3' 

 −1  
0 0 0   sin θ3' cos θ3' 0 0.2sin θ3' 
=
0
A3 =
0
A 2 A3 
A 2 A 3 A1= =
0 0 1 d1   0 0 1 0 
  
0 0 0 1  0 0 0 1 
 sin θ3' cos θ3' 0 0.2sin θ3' + d 2 
 
− cos θ3' sin θ3' 0 −0.2 cos θ3' 
=
0
A3 0 = A 2 A3 
A 2 A 3 A1=
 0 0 1 d1 
 
 0 0 0 1 

 sin θ3' cos θ3' 0 0.2sin θ3' + d 2  cos θ 4 sin θ 4 0 0 

 
− cos θ3' sin θ3' 0 −0.2 cos θ3'   sin θ 4 − cos θ 4 0 0 
=
0
T4 =
0
A3 A 4 
A 3 A 4 A1A 2=
 0 0 1 d1  0 0 −1 −0.25
  
 0 0 0 1  0 0 0 1 
 sin(θ3' + θ 4 ) − cos(θ3' + θ 4 ) 0 0.2sin θ3' + d 2 
 
 − cos(θ3' + θ 4 ) − sin(θ3' + θ 4 ) 0 −0.2 cos θ3' 
=
0
T4 =
0
A 3 A 4 A1A 2=
A3 A 4
 0 0 −1 −0.25 + d1 
 
 0 0 0 1 

Solución 2.1)

Reemplazando los valores de las 4 variables para el caso 1 en la expresión de 0 T4

1 0 0 0.5 
0 −1 0 0 
0
T4 = 
0 0 −1 0.15
 
0 0 0 1 

36
Universidad Nacional de San Agustín
Autor: Lizardo Pari

De donde se recupera 0 R 4 y 0 t 4 :

1 0 0   0.5 
0
=R 4 0 −1 0  , y 0
t 4 =  0 
0 0 −1 0.15

Gráficamente:
0
0
R4 t4
X4
Z0 Y4
Y0 0.15
X0 Z4
0.5
Figura 6

Rotaciones básicas:

1 0 0 
0
= π ) 0 −1 0 
R 4 rot ( x,=
0 0 −1

Solución 2.2)

Reemplazando en 0 T4

 0 −1 0 0.3 
 −1 0 0 −0.2 
0
T4 =  
 0 0 −1 0.15 
 
0 0 0 1 

De donde se recupera 0 R 4 y 0 t 4 :

 0 −1 0   0.3 
0
R 4 =  −1 0 0  , y 0
t 4 =  −0.2 
 0 0 −1  0.15 

Gráficamente:

37
Universidad Nacional de San Agustín
Autor: Lizardo Pari
0
0
R4 t4

Z0
Y0 Y4
0.3
X0 0.15
X4 0.2
Z4

Figura 7

Rotaciones básicas:

1 0 0  0 −1 0   0 −1 0 
π 
0
rot ( x, π ) ⋅ rot ( z , ) =−
R4 =  0 1 0  ⋅ 1 0 0  =
 −1 0 0 
 
2
0 0 −1 0 0 1   0 0 −1

Ejemplo 3

Considere el robot plano de 2 GDL (RR) de la Figura 8,

1. Hallar el modelo D-H, es decir:

a) Asignar frames
b) Encontrar la tabla D-H.
c) Hallar las matrices A i
d) Hallar la transformación 0 T2 .
2. Graficar X 0 , Y0 , Z 0 y X 2 , Y2 , Z 2 para los casos de la Tabla 4 , e indique que rotaciones
básicas se realizaron para conseguir 0 R 2 .

Tabla 4

Caso θ1 θ2
1 0 0
2 0 π /2

38
Universidad Nacional de San Agustín
Autor: Lizardo Pari

l2

2
θ2
l1 Y0 1
θ1 X
0
Z0 0

Figura 8

Solución 1.a)

( θ1 0,=
La posición de reposo del robot= θ 2 0 ):
Y0

X0

Z0
l1 l2

Figura 9

Aplicando las reglas de asignación de frames del modelo D-H:

X2
Y2

l2 Z2
X1

Y1 Z1 θ
2

l1 Y0
θ1
X0
Z0

Figura 10

39
Universidad Nacional de San Agustín
Autor: Lizardo Pari
Solución 1.b)

Se obtiene la Tabla D-H:

i θi di ai αi
1 θ1 0 l1 0
2 θ2 0 l2 0

Solución 1.c)

Las matrices A i

cθ1 − sθ1 0 l1cθ1 

 sθ cθ1 0 l1sθ1  cos θ1 = cθ1
A1 =  1 donde
0 0 1 0  sin θ1 = sθ1
 
0 0 0 1 

cθ 2 − sθ 2 0 l2 cθ 2 
 sθ cθ 2 0 l2 sθ 2 
A2 =  2
 0 0 1 0 
 
 0 0 0 1 

Solución 1.d)

Aplicando el producto de matrices y las relaciones del seno y coseno de la suma de dos
ángulos, se tiene:

cθ1 − sθ1 0 l1cθ1  cθ 2 − sθ 2 0 l2 cθ 2 

 sθ cθ1 0 l1sθ1   sθ 2 cθ 2 0 l2 sθ 2 
0
A2 = 
A1A 2 = 1
⋅ =
0 0 1 0   0 0 1 0 
   
0 0 0 1   0 0 0 1 
 c (θ1 + θ 2 ) − s (θ1 + θ 2 ) 0 l2 c (θ1 + θ 2 ) + l1cθ1 
 
=
0
A 2 A=A  s (θ1 + θ 2 ) c (θ1 + θ 2 ) 0 l2 s (θ1 + θ 2 ) + l1sθ1 
1 2
 0 0 1 0 
 
 0 0 0 1 

Solución 2.1)

θ1 0,=
Para= θ2 0 :

40
 Universidad Nacional de San Agustín
Autor: Lizardo Pari

1 0 0 l1 + l2 
0 1 0 0 
0
T2 = 
0 0 1 0 
 
0 0 0 1 

De donde se recupera 0 R 2 y 0 t 2 :

1 0 0  l1 + l2 
0
R 2 = 0 1 0  , y 0
t 2 =  0 
0 0 1   0 

Gráficamente:
0
0
R2 t2
Z2
Z0
Y0 Y2
X0
X2
l1 + l2

Figura 11

Rotaciones básicas:

1 0 0 
=0
R 2 =0 1 0  I 3
0 0 1 

Solución 2.2)

θ1 0,=
Para= θ2 π / 2 :

0 −1 0 l1 
1 0 0 l2 
0
T2 = 
0 0 1 0
 
0 0 0 1

De donde se recupera 0 R 2 y 0 t 2 :

41
 Universidad Nacional de San Agustín
Autor: Lizardo Pari

0 −1 0   l1 
0
R 2 = 1 0 0  , y 0
t 2 = l2 
0 0 1   0 

Gráficamente:

Z2
0
0
R2 t2 X2
Y2
Z0
Y0
l2
X0
l1
Figura 12

Rotaciones básicas:

0 −1 0 
 π
=R 2 =
0
1 0 0  rot  z , 
 2
0 0 1 

Ejemplo 4

1.- Resolver el problema cinemático directo para el robot PRR (Figura 13).

2.a.-Calcular la posición y orientación del extremo del robot (respecto a la base) para los casos
de la Tabla 5

Tabla 5

Caso d1 θ2 θ3
1 0.3m 0 −π / 2
2 0.5m −π / 2 π /2
2.b.- Graficar X 0 , Y0 , Z 0 y X 3 , Y3 , Z 3 para los casos de la Tabla 5 e indique que rotaciones
básicas se realizaron para conseguir 0 R 3 .

42
Universidad Nacional de San Agustín
Autor: Lizardo Pari

l1 l2 l3

TCP

2 3
1
θ2 θ3
d1 0
Z0

Y0
X0 O0

Figura 13

Solución 1.-

a) Asignación de frames:

l1 l2 l3

Z1 Z2 Z3
Y1 Y2 Y3
X1 X 2 TCP
O2 X3
O1 O3

2 3
1
θ2 θ3
d1 0
Z0

Y0
X0 O0

Figura 14

43
 Universidad Nacional de San Agustín
Autor: Lizardo Pari
b) Tabla D-H:

i θi di ai αi
1 π /2 d1 l1 0
2 θ2 0 l2 0
3 θ3 0 l3 0

c) Matrices A i

0 −1 0 0 cθ 2 − sθ 2 0 l2 cθ 2  cθ3 − sθ3 0 l3cθ3 

1 0 0 l1   sθ cθ 2 0 l2 sθ 2   sθ cθ3 0 l3 sθ3 
A1 =  ; A2 =  2 ; A3 = 
3

0 0 1 d1   0 0 1 0   0 0 1 0 
     
0 0 0 1  0 0 0 1   0 0 0 1 

d) Transformación 0 T3 :

cθ 2 − sθ 2 0 l2 cθ 2  cθ3 − sθ3 0 l3cθ3 

 sθ   
1
A3 =  2 cθ 2 0 l2 sθ 2  ⋅  sθ3 cθ3 0 l3 sθ3  =
A 2 A3 =
 0 0 1 0   0 0 1 0 
   
 0 0 0 1   0 0 0 1 
c (θ 2 + θ3 ) − s (θ 2 + θ3 ) 0 l3c (θ 2 + θ3 ) + l2 cθ 2 
 
 s (θ 2 + θ3 ) c (θ 2 + θ3 ) 0 l3 s (θ 2 + θ3 ) + l2 sθ 2 
=
1
A 3 A=2 A3
 0 0 1 0 
 
 0 0 0 1 

=
0
T3 A=1
1 A3 A1=
A 2 A3
0 −1 0 0  c (θ 2 + θ3 ) − s (θ 2 + θ3 ) 0 l3c (θ 2 + θ3 ) + l2 cθ 2 
1 0 0 l   
= 1   s (θ 2 + θ 3 ) c (θ 2 + θ3 ) 0 l3 s (θ 2 + θ3 ) + l2 sθ 2 
0 0 1 d1   0 0 1 0 
  
0 0 0 1   0 0 0 1 
− s (θ 2 + θ3 ) −c (θ 2 + θ3 ) 0 −l3 s (θ 2 + θ3 ) − l2 sθ 2 
 
c (θ 2 + θ3 ) − s (θ 2 + θ3 ) 0 l1 + l3c (θ 2 + θ3 ) + l2 cθ 2 
=
 0 0 1 d1 
 
 0 0 0 1 

Solución 2.a)-

Caso 1: d1 =0.3m;θ 2 =0;θ3 =−90°

44
Universidad Nacional de San Agustín
Autor: Lizardo Pari

1 0 0 l3 
0 1 0 l1 + l2 
0
T3 = 
0 0 1 0.3 
 
0 0 0 1 

De donde se recupera 0 R 3 y 0 t 3 :

1 0 0   l3 
0
R 3 = 0 1 0  , y 0
t=
3
 
l1 + l2 
0 0 1   0.3 

Caso 2: d1 =0.5m;θ 2 =−90°;θ3 =90°

0 −1 0 l2 
1 0 0 l1 + l3 
0
T3 = 
0 0 1 0.5 
 
0 0 0 1 

De donde se recupera 0 R 3 y 0 t 3 :

0 −1 0   l2 
0
R 3 = 1 0 0  , y 0
t=
3
l + l 
 1 3
0 0 1   0.5 

Solución 2.b)

Caso 1: d1 =0.3m;θ 2 =0;θ3 =−90°

l1 + l2
Z3
0 l3
0.3
Z0
0
R3 t3
Y3
Y0
X3
X0

Figura 15

45
 Universidad Nacional de San Agustín
Autor: Lizardo Pari

1 0 0 
0
R= I= 0 1 0 
3 3  
0 0 1 

Caso 2: d1 =0.5m;θ 2 =−90°;θ3 =90°

l1
Z3
l2 Y3
0.5
Z0 l3 X3
0
Y0 R3 0 t3
X0

Figura 16

0 −1 0 
=
0
,90°) 1 0 0 
R 3 rot ( z=
0 0 1 

Ejemplo 5

Se tiene un robot 3 GDL (PRR) (Figura 17 ).

1. Hallar la cinemática directa.

2. Hallar la posición y orientación del tercer frame respecto al frame base para cada uno
de los casos de la Tabla 6.

Tabla 6

Caso d1 θ2 θ3
1 0.3m 0 0
2 0.3m π / 2 0
3. Graficar X 0 , Y0 , Z 0 y X 3 , Y3 , Z 3 para los casos de la Tabla 6 e indique que rotaciones
básicas se realizaron para conseguir 0 R 3 .

46
Universidad Nacional de San Agustín
Autor: Lizardo Pari

TCP

0.5
0.1

d1
X0 θ2 3
θ3
2
Z0
Y0
O0
1
Figura 17

Solución 1a)

Asignación de frames:

Z3

TCP Y3
X3 O3
X1 0.5 Z2 0.1

O1 Z1 Y2
d1 O2
X0 Y1 θ2 X2
θ3
Z0
Y0
O0

Figura 18

Solución 1b)

Tabla D-H:

i θi di ai αi
1 0 d1 0 −π / 2
2 θ2 + π / 2 0.5 0 π /2
3 θ3 0.1 0 0

47
 Universidad Nacional de San Agustín
Autor: Lizardo Pari
Solución 1c)

Matrices A i :

1 0 0 0 cθ 2' 0 sθ 2' 0 cθ3 − sθ3 0 0

0 0  ' 
1 0  sθ 0 −cθ 2' 0
 sθ cθ3 0 0 
A1 =  ; A2 =  2 ; A3 = 
3

0 −1 0 d1   0 1 0 0.5  0 0 1 0.1
     
0 0 0 1  0 0 0 1   0 0 0 1

π
Nota: θ=
'
2 θ2 +
2

Solución 1d)

Transformación 0 T3 :

1 0 0 0  cθ 2' 0 sθ 2' 0

0 0 1 0   ' 
sθ 0 −cθ 2' 0
0
A1A 2 = ⋅  2
A2 =  =
0 −1 0 d1   0 1 0 0.5
   
0 0 0 1   0 0 0 1 
 cθ 2' 0 sθ 2' 0
 
=
0
A 2 A=A  0 1 0 0.5
1 2
 − sθ 2' 0 cθ 2' d1 
 
 0 0 0 1 

 cθ 2' 0 sθ 2' 0  cθ3 − sθ3 0 0

  
 0 1 0 0.5  sθ3 cθ3 0 0 
0
T3 =
0
A 2 A3 =A1A 2 A 3 = ⋅ =
 − sθ 2' 0 cθ 2' d1   0 0 1 0.1
   
 0 0 0 1   0 0 0 1
 cθ 2' cθ3 −cθ 2' sθ3 sθ 2' 0.1sθ 2' 
 
 sθ3 cθ3 0 0.5 
=
0
T3 =
0
A 2 A 3 A1=
A 2 A3
 − sθ 2' cθ3 sθ 2' sθ3 cθ 2' 0.1cθ 2' + d1 
 
 0 0 0 1 

Solución 2

π
Caso 1:= ;θ 2 0;=
d1 0.3m= θ3 0 , es decir θ 2' =
2

48
Universidad Nacional de San Agustín
Autor: Lizardo Pari

0 0 1 0.1
0 1 0 0.5
0
T3 = 
 −1 0 0 0.3
 
0 0 0 1 

De donde se recupera 0 R 3 y 0 t 3 :

 0 0 1  0.1
0
R 3 =  0 1 0  , y 0
t 3 = 0.5
 −1 0 0   0.3

Caso 2: d1 =0.3m;θ 2 =90°;θ3 =0 , es decir θ 2' = π

 −1 0 0 0 
0 1 0 0.5
0
T3 = 
0 0 −1 0.2 
 
0 0 0 1 

De donde se recupera 0 R 3 y 0 t 3 :

 −1 0 0  0 
0
R 3 =  0 1 0  , y 0
t 3 =  0.5
 0 0 −1 0.2 

Solución 3

Caso 1:

Z3
0
0
R3 t3
Y3
X3 0.1
0.5
X0
Z0 0.3

Y0
Figura 19

49
 Universidad Nacional de San Agustín
Autor: Lizardo Pari
Rotaciones básicas:

 0 0 1
= ,90°)  0 1 0 
R 3 rot ( y=
0

 −1 0 0 

Caso 2:

0.5
X0 0 0.1
Z0 0.3 0
R3 t3 Y3
Y0 Z3 X3

Figura 20

Rotaciones básicas:

 −1 0 0 
= 0
=
R 3 rot ( y, π )  0 1 0 
 0 0 −1

Ejemplo 6

Se tiene el robot de tres GDL (RRP) de la Figura 21,

1.- Halle la cinemática directa.

2.- para los casos de la Tabla 7, halle:

a.- La transformación entre el tercer frame y la base, recuperando la rotación y traslación.

b.- Grafique X 0 , Y0 , Z 0 y X 3 , Y3 , Z 3 para los casos de la Tabla 7 e indique que rotaciones

básicas se realizaron para conseguir 0 R 3 .

Tabla 7

Caso θ1 θ2 d3
1 0 π /2 0.5
2 π /2 π /2 0.5

50
Universidad Nacional de San Agustín
Autor: Lizardo Pari

TCP

d3 3

θ2

1
0
Z0

l1
Y0
O0
X0
θ1

Figura 21

Solución 1.a

Se asignaron las siguientes frames:

Z3
TCP Y3
O3
X3

d3
Z2
Z1 Y2
O2
X2
O1
X1

θ2 Y1

Z0

l1
Y0
O0
X0
θ1

Figura 22

51
 Universidad Nacional de San Agustín
Autor: Lizardo Pari
Solución 1.b

Tabla D-H:

i θi di ai αi
1 θ1 l1 0 −π / 2
2 θ2 0 0 π /2
3 0 d3 0 0

Solución 1.c

Matrices A i :

cθ1 0 − sθ1 0 cθ 2 0 sθ 2 0 1 0 0 0

 sθ 0 cθ 0   sθ 0 −cθ 2 
0 0 1 0 0 
A1 =  1 1
; A2 = 
2
; A3 = 
 0 −1 0 l1   0 1 0 0 0 0 1 d3 
     
0 0 0 1  0 0 0 1 0 0 0 1

Solución 1.d:

Transformación 0 T3 :

cθ1 0 − sθ1 0  cθ 2 0 sθ 2 0

 sθ 0 cθ 0   sθ 2 0 −cθ 2 0 
0
A2 =  1
A1A 2 = 1
⋅ =
 0 −1 0 l1   0 1 0 0
   
0 0 0 1  0 0 0 1
cθ1cθ 2 − sθ1 cθ1sθ 2 0
 sθ cθ cθ1 sθ1sθ 2 0 
=
0
A 2 A=A  1 2
1 2
 − sθ 2 0 cθ 2 l1 
 
 0 0 0 1

cθ1cθ 2 − sθ1 cθ1sθ 2 0  1 0 0 0

 sθ cθ cθ1 sθ1sθ 2 0  0 1 0 0 
0
T3 ==
0 
A 2 A 3 A1A 2 A 3 = 1 2
⋅ =
 − sθ 2 0 cθ 2 l1  0 0 1 d3 
   
 0 0 0 1  0 0 0 1
cθ1cθ 2 − sθ1 cθ1sθ 2 d3cθ1sθ 2 
 sθ cθ cθ1 sθ1sθ 2 d3 sθ1sθ 2 
=
0
T3 0= A 2 A3  1 2
A 2 A 3 A1=
 − sθ 2 0 cθ 2 d3cθ 2 + l1 
 
 0 0 0 1 

52
 Universidad Nacional de San Agustín
Autor: Lizardo Pari
Solución 2

π
θ1 0;=
Caso 1.a:= θ2 =
; d3 0.5m; ,
2

0 0 1 0.5
0 1 0 0 
0
T3 = 
 −1 0 0 l1 
 
0 0 0 1 

De donde se recupera 0 R 3 y 0 t 3 :

 0 0 1 0.5
0
R 3 =  0 1 0  , y
 0
t 3 =  0 
 −1 0 0   l1 

Caso 1.b.- Gráficamente:

0.5
Y3

l1 0
Z3
R3 0 t3
Z0
Y0 X3
O0
X0

Figura 23

Rotaciones básicas:

 0 0 1
= ,90°)  0 1 0 
R 3 rot ( y=
0

 −1 0 0 

π π
=
Caso 2.a: θ1 = ;θ 2 = ; d3 0.5m; ,
2 2

53
 Universidad Nacional de San Agustín
Autor: Lizardo Pari

 0 −1 0 0
0 0 1 0.5
0
T3 = 
 −1 0 0 l1 
 
0 0 0 1 

De donde se recupera 0 R 3 y 0 t 3 :

 0 −1 0  0
0
R 3 =  0 0 1  , y 0
t 3 = 0.5
 −1 0 0   l1 

Caso 2.b: Gráficamente:

Y3 Z3

0.5
X3
0
0 t3
R3

l1
Z0
Y0
O0
X0
Figura 24

Rotaciones básicas:

=
0
R 3 rot ( y,90°) ⋅ rot ( x,=
−90°)
 0 0 1  1 0 0   0 −1 0 
0
R 3 =  0 1 0  ⋅ 0 0 1  =  0 0 1 
 −1 0 0  0 −1 0   −1 0 0 

54
Universidad Nacional de San Agustín
Autor: Lizardo Pari

3.3.- CINEMÁTICA INVERSA

Es “encontrar las posiciones articulares a partir de la orientación y posición del efector final”,
(ver Figura 25). Es la parte más interesante desde el punto de vista práctico.

Cinemática directa
Posición y
Posiciones
articulares Robot orientación
del extremo
Cinemática inversa

Figura 25

3.3.1.- ENFOQUE GEOMÉTRICO

Se aplican a robots con diseño cinemático simple (generalmente de pocos GDL), se explota al
máximo la particular estructura cinemática del robot, mediante expresiones trigonométricas se
hallan los valores articulares del robot.

Ejemplo 7

Hallar la cinemática inversa del robot plano de 2 GDL sabiendo que su posición (x,y) de su
extremo es conocido, Figura 26.

( x, y )
l2

θ2
Y0 l1

θ1
Z0
X0

Figura 26

Solución.

Cálculo de θ 2 :

Dibujamos en un plano cartesiano XY la geometría de los eslabones del robot (Figura 27):

Aplicando ley de cosenos en el triángulo l − l1 − l2 :

l 2 = l12 + l22 − 2l1l2 cos A ( 7)

Pero l=
2
cos(180 − θ 2 ) =
x 2 + y 2 y cos A = − cos θ 2

En ( 7)

x 2 + y 2 = l12 + l22 + 2l1l2 cos θ 2

55
Universidad Nacional de San Agustín
Autor: Lizardo Pari
Despejando:

x 2 + y 2 − l12 − l22 ( 8)
cos θ 2 =
2l1l2
1 , se tiene: sin θ =
De la identidad trigonométrica: sin 2 θ + cos 2 θ = ± 1 − cos 2 θ , por lo
que el ángulo θ 2 es:

 1 − cos 2 θ  ( 9)
θ 2 a tan  ±
= 2

 cos θ 2 

Donde el signo positivo significa configuración codo abajo y negativo codo arriba.

( x, y )

l2 sin θ 2
l2
l
Y0
θ 2 l cos θ
A 2 2

β l1
α θ1
Z0
X0
Figura 27

Cálculo de θ1

Según la Figura 27:

θ1= α − β
 y ( 10)
α = a tan  
x 
 l sin θ 2 
β = a tan  2 
 l1 + l2 cos θ 2 

3.3.2.- DESACOPLO CINEMÁTICO.

Usualmente en robots de 6 GDL, los tres primeros GDL determinan la posición del efector final,
mientras que los otros 3 GDL cuyos ejes de rotación se intersecan en solo punto, determinan su
orientación (tal es el caso de la muñeca esférica de 3 GDL), por lo que el problema cinemático
puede ser separado (desacoplarse) en dos sub-problemas más simples: problema cinemático
inverso de posición y problema cinemático inverso de orientación (también llamado “solución
de Pieper”). En este caso el origen de los tres últimos GDL se encuentran en el origen del frame
F4, cuya posición en el sistema base F0 ( 0d 4 , ver Figura 28) se encuentra mediante un enfoque
geométrico.

56
Universidad Nacional de San Agustín
Autor: Lizardo Pari

Z4

Z5
0
d4
Z0 Z6

X0 Y0

Figura 28

3.3.3.- ENFOQUE MATEMÁTICO

3.3.3.1.- SOLUCIÓN CERRADA.-
Se desea obtener las variables articulares mediante una solución cerrada, es decir que la
solución dependa explícitamente de las variables de entrada, aparte de su sencillez, su ventaja
radica en la velocidad de procesamiento, a diferencia de los métodos iterativos.

METODO DE MÍNIMOS CUADRADOS: Si se tiene un sistema de m ecuaciones con n variables, se

presentan las siguientes situaciones:

• (m=n): Se obtiene una solución única.

• (m>n): La solución es una aproximación que satisface al mismo tiempo a las m
ecuaciones. Su resolución se realiza mediante el método de mínimos cuadrados: si se
tiene un sistema de la forma:

Ax = b ( 11)

Donde A matriz de dimensión mXn, x vector nX1, y b vector mX1. La solución es:

x = A +b ( 12)

Donde:

A + = ( AT A ) AT ( 13)
−1

Donde A + es una matriz nXm, es la matriz pseudoinversa de A.

3.3.3.2.- SOLUCIÓN MEDIANTE MÉTODOS ITERATIVOS.-

Al presentarse sistemas de ecuaciones no lineales, generalmente no se puede desacoplar las
variables de salida para que dependan explícitamente de las de entrada, entonces se recurre a
métodos iterativos, los cuales son métodos que a partir de una solución de inicio,
progresivamente van calculando aproximaciones a la solución de las variables, en cada iteración
dicha aproximación va siendo más exacta. Una desventaja es su costo computacional que se
traduce en mayor tiempo de procesamiento, y la posibilidad de caer en mínimos locales.

57
Universidad Nacional de San Agustín
Autor: Lizardo Pari
3.3.3.3.- FORMULACIÓN
Dada una matriz de transformación homogénea H d del efector final:

R t  ( 14)
=
Hd   ∈ E (3) con R ∈ SO(3)
 0 1
Encontrar (una o todas) las soluciones a la ecuación:

H d = 0 Tn (q1 ,  , qn ) ( 15)

Donde
0
T=
n A1 ⋅ A 2  A n

La ecuación ( 15) resulta en un sistema de 12 ecuaciones no lineales con n variables que pueden
escribirse como:

H ij = Tij (q1 ,  , qn ) =
con i 1,=
2,3; j 1, 2,3, 4 ( 16)

Donde Tij y H ij , son los 12 elementos no triviales de 0 Tn y H d respectivamente.

Según la necesidad, para despejar las variables necesarias, la ecuación ( 15) puede reescribirse
como:

A1−1 ⋅ H d =
A2  An
ó ( 17)
−1 −1
A ⋅ A ⋅ Hd =
2 A3  A n
1
ó
−1 −1 −1
A ⋅ A ⋅ A ⋅ Hd =
3 2 A4  An1

OBSERVACIONES:

- En la cinemática directa existe una solución única, mientras que en la cinemática

inversa, puede haber múltiples soluciones o no haber solución, estas soluciones deben
discriminarse adecuadamente según la estructura mecánica del robot, rango de
operación de las variables, espacio de trabajo, etc.
- No todas las soluciones pueden ser físicamente realizables.
- Se debe explotar la particular estructura del robot.
- En la ecuación ( 15), de preferencia, igualar con la parte de posición ( px , p y , pz ) de H d
, dado que la parte rotacional es muy sensible a errores.

Ejemplo 8

Considere los resultados del Ejemplo 2, y suponga que es conocido H d , hallar d1 , d 2 , θ3 y θ 4

donde:

58
Universidad Nacional de San Agustín
Autor: Lizardo Pari

 nx ox ax px 
n oy ay p y 
Hd =  y ( 18)
 nz oz az pz 
 
0 0 0 1

0.2
d2 Z2 Z3 θ4
Y3
O1 O2 Y2 X3
Z1 O3
X1 0.15
X2
d1 Y1 O4 0.1
Z0 θ3
Y0 TCP X4

X0 Y4 Z4
O0
Figura 29

SOLUCIÓN:

De acuerdo a la ecuación ( 15), se tiene el siguiente sistema de ecuaciones:

nx = sin(θ3 + π2 ) cos θ 4 + cos(θ3 + π2 ) sin θ 4

ox = sin(θ3 + π2 ) sin θ 4 − cos(θ3 + π2 ) cos θ 4
ax = 0
− cos(θ3 + π2 ) cos θ 4 + sin(θ3 + π2 ) sin θ 4
ny =
− cos(θ3 + π2 ) sin θ 4 − sin(θ3 + π2 ) cos θ 4
oy =
ay = 0 ( 19)

nz = 0
oz = 0
az = −1
d 2 + 0.2sin(θ3 + π2 )
px =
−0.2 cos(θ3 + π2 )
py =
pz = −0.25 + d1

De las tres últimas ecuaciones, se tiene:

59
Universidad Nacional de San Agustín
Autor: Lizardo Pari

d=
1 pz + 0.25
 p  π ( 20)
=θ3 a cos  y  −
 −0.2  2
px − 0.2sin(θ3 + π2 )
d2 =

Cálculo de θ 4 :

Al no haber una solución cerrada, se hallará una aproximación a través del método de mínimos
cuadrados. Reacomodando las primeras cinco ecuaciones de ( 19):

Bx + Ay =
nx
Ax − By =
ox ( 21)
Ax − By =
ny
− Bx − Ay =
oy
=
Donde A sin(θ3 + =
π
,
2) B cos(θ3 + π2 ) , y = sin θ 4 , x = cos θ 4 . Se forma el sistema Ax = b ,
con

B A  nx 
 A −B o 
 y ( 22)
A=   ; x= , b= 
x

 A −B  x ny 
   
 − B − A  o y 
Se aplica la solución por mínimos cuadrados (ecuación ( 12)). Hallado el valor de x y y , se
halla θ 4 a través del arco tangente.

Ejemplo 9

Hallar la cinemática inversa del Ejemplo 4, si se conoce H d

l1 l2 l3

Z1 Z2 Z3
Y1 Y2 Y3
X1 X 2 TCP
O2 X3
O1 O3

2 3
1
θ2 θ3
d1 0
Z0

Y0
X0 O0

Figura 30

60
Universidad Nacional de San Agustín
Autor: Lizardo Pari
Solución:

Aplicando la ecuación ( 15) a los elementos de la posición, se tiene:

−l3 s (θ3 + θ 2 ) − l2 sθ 2
px = ( 23)
p y =l1 + l3c(θ3 + θ 2 ) + l2 cθ 2
pz = d1
De donde

d1 = pz ( 24)

θ 2 y θ3 están fuertemente acoplados, por lo que aplicaremos la primera de las ecuaciones (

17):

A1−1 ⋅ H d =
A 2 A3 ( 25)

A partir de las ecuaciones A1 del Ejemplo 4, se obtiene:

0 1 0 −l1 
 −1 0 0 0 
A1−1 = 
0 0 1 − d1 
 
0 0 0 1 

Aplicando en ( 25), conociendo A 2 A 3 del Ejemplo 4:

0 1 0 −l1   nx ox ax px 
 −1 0 0 0   n y o y ay p y 
 =
0 0 1 −d1   nz oz az pz 
  
0 0 0 1  0 0 0 1
c (θ 2 + θ3 ) − s (θ 2 + θ3 ) 0 l3c (θ 2 + θ3 ) + l2 cθ 2 
 
s (θ + θ ) c (θ 2 + θ3 ) 0 l3 s (θ 2 + θ3 ) + l2 sθ 2 
= 2 3
 0 0 1 0 
 
 0 0 0 1 
Igualando la parte de posición, se tiene:

l1 l3c (θ 2 + θ3 ) + l2 cθ 2
p y −=
− p=
x l3 s (θ 2 + θ3 ) + l2 sθ 2
pz − d1 =
0

Donde se observa nueva mente que θ 2 y θ3 siguen fuertemente acoplados, por lo que
bajaremos un nivel de las ecuaciones ( 17), a la forma:

A −21A1−1 ⋅ H d =
A3 ( 26)

61
Universidad Nacional de San Agustín
Autor: Lizardo Pari
Del Ejemplo 4,

 cθ2 sθ 2 0 −l2 
 − sθ cθ 2 0 0 
A 2−1 =  2

 0 0 1 0 
 
 0 0 0 1 

por lo que:

 c θ 2 sθ 2 0 −l2   0 1 0 −l1   − sθ 2 cθ 2 0 −l1cθ 2 − l2 

 − sθ cθ 0 0   −1 0 0 0   −cθ − sθ 2 0 l1sθ 2 
A −21A1−1 = 2 2    2

 0 0 1 0   0 0 1 −d1   0 0 1 −d1 
    
 0 0 0 1  0 0 0 1   0 0 0 1 

Reemplazando A −21A1−1 en ( 26):

 − sθ 2 cθ 2 0 −l1cθ 2 − l2   r11 r12 r13 px  cθ3 − sθ3 0 l3cθ3 

 −cθ − sθ 2 0 l1sθ 2   r21 r22 r23 p y   sθ3 cθ3 0 l3 sθ3 
 2
=
 0 0 1 − d1   r31 r32 r33 pz   0 0 1 0 
    
 0 0 0 1  0 0 0 1  0 0 0 1 

Igualando los elementos de la posición:

− px sθ 2 + p y cθ 2 − l1cθ 2 − l2 =
l3cθ3
− px cθ 2 − p y sθ 2 + l1sθ 2 =
l3 sθ3
pz − d1 =
0

Acomodando términos en las dos primeras ecuaciones:

− px sθ 2 +  p y − l1  cθ 2 = l3cθ3 + l2 ( 27)

px cθ 2 +  p y − l1  sθ 2 =
−l3 sθ3
Elevando cada ecuación al cuadrado:
2
px2 s 2θ 2 − 2 px sθ 2  p y − l1  cθ 2 +  p y − l1  c 2θ 2= l32 c 2θ3 + 2l2l3cθ3 + l22
2
px2 c 2θ 2 + 2 px cθ 2  p y − l1  sθ 2 +  p y − l1  s 2θ 2 =
l32 s 2θ3

Sumando ambas ecuaciones:

2
px2 +  p y − l1  =l32 + 2l2l3cθ3 + l22

Se despeja θ3 :

 p2 +  p − l 2 − l 2 − l 2  ( 28)
θ3 = a cos 
x  y 1 2 3

 2l2l3 
 

62
Universidad Nacional de San Agustín
Autor: Lizardo Pari

Cálculo de θ 2 :

A fin de eliminar sθ 2 , multiplicamos la primera ecuación de ( 27) por [ p y − l1 ] y la segunda por

px :
2
−  p y − l1  px sθ 2 +  p y − l1  cθ 2 =  p y − l1  l3cθ3 +  p y − l1  l2
px2 cθ 2 +  p y − l1  px sθ 2 = −l3 sθ3 px

Sumando ambas ecuaciones:

2
px2 cθ 2 +  p y − l1  cθ 2 =  p y − l1  l3cθ3 +  p y − l1  l2 − px l3 sθ3

Factorizando [ p y − l1 ] y despejando θ 2 :

  p − l  ( l + l cθ ) − p l sθ  ( 29)
θ 2 = a cos  
1 2
y 3 3 x 3 3

 px +  p y − l1 
2 2

 

63
Universidad Nacional de San Agustín
Autor: Lizardo Pari
Ejemplo 10

Hallar la cinemática inversa del robot PRR de 3 GDL del Ejemplo 5, si se conoce Hd, como:

 nx ox ax px 
n oy ay p y 
Hd =  y
 nz oz az pz 
 
0 0 0 1

TCP

0.5
0.1

d1
X0 θ2 3
θ3
2
Z0
Y0
O0
1
Figura 31

Solución:

A partir de la cinemática directa realizada en el Ejemplo 5, se obtuvo la transformación total:

 cθ 2' cθ3 −cθ 2' sθ3 sθ 2' 0.1sθ 2' 

 
sθ3 cθ3 0 0.5 
0
T3 = 
 − sθ 2' cθ3 sθ 2' sθ3 cθ 2' 0.1cθ 2' + d1 
 
 0 0 0 1 
Aplicando la ecuación ( 15):

H d = 0 T3 = A1 ⋅ A 2 ⋅ A 3

Igualando la parte traslacional:

px = 0.1sθ 2'
p y = 0.5
=pz 0.1cθ 2' + d1

Donde θ=
2
'
(θ 2 + π2 ) . A partir de la primera y tercera ecuación, se tiene respectivamente:

64
Universidad Nacional de San Agustín
Autor: Lizardo Pari

p  ( 30)
θ 2' = a sin  x 
 0.1 
d=1 pz − 0.1cθ 2'
Cálculo de θ3 :

A partir de la primera de las ecuaciones ( 17):

A1−1 ⋅ H d = A 2 ⋅ A 3 ( 31)

A partir de las matrices A i del Ejemplo 5, se obtiene:

1 0 0 0
0 0 −1 d1 
A1−1 = 
0 1 0 0
 
0 0 0 1

cθ 2' 0 sθ 2' 0  cθ3 − sθ3 0 0

 '  
sθ 0 −cθ 2' 0   sθ3 cθ3 0 0 
A 2 A3 =  2 ⋅ =
 0 1 0 0.5  0 0 1 0.1
   
 0 0 0 1   0 0 0 1
cθ 2' cθ3 −cθ 2' sθ3 sθ 2' 0.1sθ 2' 
 ' 
sθ cθ − sθ 2' sθ3 −cθ 2' −0.1cθ 2' 
A 2 A3 =  2 3
 sθ3 cθ3 0 0.5 
 
 0 0 0 1 

Reemplazando en ( 31):

1 0 0 0   nx ox ax px  cθ 2' cθ3 −cθ 2' sθ3 sθ 2' 0.1sθ 2' 

0  
 0 −1 d1   n y oy ay p y   sθ 2' cθ3 − sθ 2' sθ3 −cθ 2' −0.1cθ 2' 
=
0 1 0 0   nz oz az pz   sθ3 cθ3 0 0.5 
    
0 0 0 1 0 0 0 1  0 0 0 1 

Igualando la parte traslacional:

px = 0.1sθ 2'
− pz + d1 =−0.1cθ 2'
p y = 0.5

Se observa que no hay ecuaciones dependientes de θ3 , por lo que bajamos un nivel en las
ecuaciones ( 17):

A −21A1−1H d = A 3 ( 32)

Del Ejemplo 5, se sabe que

65
Universidad Nacional de San Agustín
Autor: Lizardo Pari

cθ 2' sθ 2' 0 0 

 
0 0 1 −0.5
A2 =  '
−1
 sθ 2 −cθ 2' 0 0 
 
 0 0 0 1 

cθ3 − sθ3 0 0 
 sθ cθ3 0 0 
A3 =  3
 0 0 1 0.1
 
 0 0 0 1

cθ 2' sθ 2' 0 0  1 0 0 0  cθ 2' 0 − sθ 2' d1sθ 2' 

    
= 0 0 1 −0.5 0 0 −1 d1   0 1 0 −0.5 
A −21A1−1
 sθ 2' −cθ 2' 0 0  0 1 0 0   sθ 2' 0 cθ 2' −d1cθ 2' 
    
 0 0 0 1  0 0 0 1   0 0 0 1 

Reemplazando en la ecuación ( 32):

cθ 2' 0 − sθ 2' d1sθ 2'   nx ox ax px  cθ3 − sθ3 0 0 

 
 0 1 0 −0.5   n y oy ay p y   sθ3 cθ3 0 0 
=
 sθ 2' 0 cθ 2' −d1cθ 2'   nz oz az pz   0 0 1 0.1
    
 0 0 0 1  0 0 0 1  0 0 0 1

Igualando la parte traslacional:

cθ 2' px − sθ 2' pz + sθ 2' d1 =

0
p y − 0.5 =
0
sθ 2' px + cθ 2' pz − cθ 2' d1 =
0.1

Se observa que en ninguna ecuación hay dependencia de la variable θ3 , por lo que no se

puede obtener θ3 a partir de la parte traslacional, por lo que debe recurrirse al parte
rotacional:

Cálculo por mínimos cuadrados:

A partir de 0 T3 del Ejemplo 5:

 nx ox ax px   cθ 2' cθ3 −cθ 2' sθ3 sθ 2' 0.1sθ 2' 

n  
 y oy ay p y   sθ3 cθ3 0 0.5 
=
 nz oz az pz   − sθ 2' cθ3 sθ 2 sθ3 cθ 2'
'
0.1cθ 2' + d1 
   
0 0 0 1  0 0 0 1 

Igualando elementos que contienen θ3 :

66
Universidad Nacional de San Agustín
Autor: Lizardo Pari

nx = cθ 2' cθ3
ox = −cθ 2' sθ3
n y = sθ3
o y = cθ3
nz = − sθ 2' cθ3
oz = sθ 2' sθ3

Sumando pares de ecuaciones, y haciendo cθ3 = x , y sθ3 = y , y pasando términos

independientes a la derecha:

cθ 2' x − cθ 2' y =nx + ox

x + y = oy + ny
− sθ 2' x + sθ 2' y =nz + oz

Pasando a la forma matricial de la forma Ax = b , donde:

 cθ 2' −cθ 2'   nx + ox 

   x
A= 1 1 ; x=  ; b  n y + o y 
y=
 − sθ 2' sθ 2'   y  nz + oz 


Se encuentra x según la ecuación ( 12), encontrándose θ3 con el arco tangente:

 y
θ3 = a tan  
x 

67
Universidad Nacional de San Agustín
Autor: Lizardo Pari
Ejemplo 11

Hallar la cinemática inversa del Ejemplo 6 si se conoce H d

 nx ox ax px 
n oy ay p y 
Hd =  y
 nz oz az pz 
 
0 0 0 1

TCP

d3 3

θ2

1
0
Z0

l1
Y0
O0
X0
θ1

Figura 32

Solución:

A partir de la cinemática directa realizada en el Ejemplo 6, se obtuvo la transformación total:

cθ1cθ 2 − sθ1 cθ1sθ 2 d3cθ1sθ 2 

 sθ cθ cθ1 sθ1sθ 2 d3 sθ1sθ 2 
0 
T3 = 1 2
 − sθ 2 0 cθ 2 d3cθ 2 + l1 
 
 0 0 0 1 
Aplicando la ecuación ( 15):

H d = 0 T3

Igualando la parte traslacional:

68
Universidad Nacional de San Agustín
Autor: Lizardo Pari

px = d3cθ1sθ 2 ( 33)
p y = d3 sθ1sθ 2
pz d3cθ 2 + l1
=

De la primera y segunda ecuación de ( 33):

p 
θ1 = a tan  y 
 px 

Cálculo de θ 2 :

En las mismas ecuaciones ( 33), multiplicando la primera ecuación por cθ1 , y la segunda por
sθ1 , y sumando luego estas dos ecuaciones:

px cθ1 = d3c 2θ1sθ 2

p y sθ1 = d3 s 2θ1sθ 2
−−−−−−−−−−−
px cθ1 + p y sθ1 =
d3 sθ 2

Dividiendo entre la tercera ecuación de ( 33), y eliminando d3 , se tiene:

 p cθ + p y sθ1 
θ 2 = a tan  x 1 
 pz − l1 

Cálculo de d3 :

De la tercera ecuación de ( 33):

pz − l1
d3 =
cθ 2

69
 Universidad Nacional de San Agustín
Autor: Lizardo Pari

3.4.- CINEMÁTICA DIFERENCIAL (CINEMÁTICA DE MOVIMIENTO)

Es encontrar la relación entre la velocidad (lineal y angular) del efector final y la velocidad de las
articulaciones.

Formulación:

 = Jq
X ( 34)

Donde

 p x 
 
 px 
 p x   V 
=
X =    es el vector 6X1 con la velocidad lineal y angular (del efector final) y
 ψ  ω 
 θ 
 
φ 
 q1 
q 
q =  2  es el vector nX1 con las velocidades articulares.

 
 qn 
J es una matriz 6Xn llamada “jacobiano del robot”, que es función de las articulaciones del
robot.

En algunas situaciones J puede ser reescrito como:

J  ( 35)
J =  P
JO 
Donde J P es una matriz 3Xn relativa a la contribución de q a la velocidad lineal y J O es una
matriz 3Xn relativa a la contribución de q a la velocidad angular.

3.4.1.- CÁLCULO DEL JACOBIANO.

La ecuación ( 35) puede reescribirse como:

J  J Pn  ( 36)
J =  P1 
 J O1  J On 
Donde J Pi y J Oi son vectores 3X1.

NOTA: qi representa una articulación general que puede ser prismática (de traslación): di , ó
de rotación: θi .

3.4.1.1.- CONTRIBUCIÓN A LA VELOCIDAD ANGULAR

a) Articulación prismática:

qJ Oi = 0 , ⇒ J Oi = 0

70
Universidad Nacional de San Agustín
Autor: Lizardo Pari
b) Articulación de rotación:

qJ Oi = θi Z i −1 , ⇒ J Oi = Z i −1

NOTA: Z i −1 : vector unitario en la dirección de Z i −1 .

3.4.1.2.- CONTRIBUCIÓN A LA VELOCIDAD LINEAL.

a) Articulación prismática:

qJ Pi = di Z i −1 , ⇒ J Pi = Z i −1

b) Articulación de rotación:

ωi
Zi −1
Yi −1
X i −1

ri −1, n
Pi −1
Zn
Z0
Yn
X0 P
Xn
Y0
Figura 33

qJ Pi = ωi × ri −1,n =θi Z i −1 × ( P − Pi −1 )

⇒ J Pi = Z i −1 × ( P − Pi −1 )

RESUMEN:

 J Pi   Z i −1  ( 37)
J  =  0  Articulación prismática
 Oi   

 J Pi   Z i −1 × ( P − Pi −1 )  ( 38)
J  =   Articulación de rotación
 Oi   Z i −1 
Donde:

Z i −1 : es la tercera columna de 0 R i−1 , para el eslabón cero: Z 0 = [ 0 0 1] .

T
-
- P es la parte traslacional de 0 Tn .
Pi −1 es la parte traslacional de 0 A i−1 , para el eslabón cero: P0 = [ 0 0 0] .
T
-

71
Universidad Nacional de San Agustín
Autor: Lizardo Pari

3.4.2.- UTILIDAD DEL JACOBIANO

- Planeamiento y ejecución de trayectorias suaves.

- La ejecución de movimientos coordinados.

- Para la transformación de fuerzas y torques del extremo a las articulaciones del robot.

- En la determinación de configuraciones singulares.

- Para aplicaciones de control (dinámico o cinemático), p.e., en el control cinemático: si se

conoce la velocidad del efector final (velocity skew), se puede conocer el desplazamiento
diferencial del robot:

−1  V  ( 39)
=q J= X J −1  
ω 

Ejemplo 12

Hallar el jacobiano del robot del Ejemplo 2.

0.2
d2 θ4
1 2 3
4

0.15
d1 0
0.1
Z0 θ3
Y0 TCP
X0

Figura 34

Solución:

Al ser un robot PPRR, según las ecuaciones ( 37) y ( 38):

Z Z1 Z 2 × ( P − P2 ) Z 3 × ( P − P3 )  ( 40)
J= 0 
0 0 Z2 Z3 
De los resultados del Ejemplo 2:

De

 sin(θ3' + θ 4 ) − cos(θ3' + θ 4 ) 0 0.2sin θ3' + d 2 

 
 − cos(θ3' + θ 4 ) − sin(θ3' + θ 4 ) 0 −0.2 cos θ3' 
0
T4 =
 0 0 −1 −0.25 + d1 
 
 0 0 0 1 

72
Universidad Nacional de San Agustín
Autor: Lizardo Pari

0.2sin θ3' + d 2 
 
⇒ P =  −0.2 cos θ3' 
 −0.25 + d1 
 
De

 sin θ3' cos θ3' 0 0.2sin θ3' + d 2 

  0.2sin θ3' + d 2 
 − cos θ3' sin θ3' 0 −0.2 cos θ3'   
0
A3 = ⇒ P3 =  −0.2 cos θ3' 
 0 0 1 d1 
   d1 
 
 0 0 0 1 
De

0 1 0 d2 
 −1 d2 
0 0 0  0
0
A2 =  ⇒ P2 =
 
0 0 1 d1 
   d1 
0 0 0 1

De

0 0 1 0
 −1 0 1 
0 0  0 
A1 =  ⇒ Z1 =
 
 0 −1 0 d1 
  0 
0 0 0 1

De

0 1 0 d2 
 −1 0
0 0 0  0
0
A2 =  ⇒ Z2 =
 
0 0 1 d1 
  1 
0 0 0 1

De

 sin θ3' cos θ3' 0 0.2sin θ3' + d 2 

  0 
 − cos θ3' sin θ3' 0 −0.2 cos θ3'  0 
0
A3 = ⇒ Z3 =
 
 0 0 1 d1 
  1 
 0 0 0 1 

0 
Además: Z 0 = 0 
1 

73
Universidad Nacional de San Agustín
Autor: Lizardo Pari

 0.2sin θ3' 
 '
 0.2 cos θ3 
P − P2 =−
 −0.25 
 

 0 
 0 
P − P3 =
 
 −0.25

Z 3 × ( P − P3 ) =
0

Z 2 × ( P − P=
2) [ Z 2× ] ( P − P=
2)

0 −1 0   0.2sin θ3  0.2 cos θ3' 

'

   ' 
( P − P2 ) 1 0 0  −0.2=
Z 2 ×= cos θ3'   0.2sin θ3 
0 0 0   −0.25  
 0 


0 1 0.2 cos θ3' 0 

 
0 0 0.2sin θ3' 0 
1 0 0 0
J=  ( 41)
0 0 0 0
0 0 0 0
 
0 0 1 1 

Ejemplo 13

Hallar el jacobiano del robot del robot plano de dos GDL del Ejemplo 3

l2

2
θ2
l1 Y0 1
θ1 X
0
Z0 0

Figura 35

Solución:

Al ser un robot 2 GDL RR, a partir de las ecuaciones ( 37) y ( 38):

74
Universidad Nacional de San Agustín
Autor: Lizardo Pari

 Z × ( P − P0 ) Z1 × ( P − P1 )  ( 42)
J= 0 
 Z0 Z1 
De los resultados del Ejemplo 3:

De

c (θ1 + θ 2 ) − s (θ1 + θ 2 ) 0 l2 c (θ1 + θ 2 ) + l1cθ1 

 
s (θ + θ ) c (θ1 + θ 2 ) 0 l2 s (θ1 + θ 2 ) + l1sθ1 
0
T2 =  1 2
 0 0 1 0 
 
 0 0 0 1 

l2 c (θ1 + θ 2 ) + l1cθ1 

 
P l2 s (θ1 + θ 2 ) + l1sθ1 
=
 0 

De

cθ1 − sθ1 0 l1cθ1 

 sθ cθ1 0 l1sθ1 
A1 =  1
0 0 1 0 
 
0 0 0 1 

l1cθ1  0 
 
P1 = l1sθ1  y Z1 = 0 
 0  1 

Además

0  0  l2 c (θ1 + θ 2 )  l2 c (θ1 + θ 2 ) + l1cθ1 

   
Z 0 = 0  , P0 = 0  , P1 l2 s (θ1 + θ 2 )  ,
P −= y P0 l2 s (θ1 + θ 2 ) + l1sθ1 
P −=
1  0   0   0 

Productos vectoriales:

0 −1 0  l2 c (θ1 + θ 2 ) + l1cθ1   −l2 s (θ1 + θ 2 ) − l1sθ1 

Z 0 × ( P − P= 1 0 0  l s θ + θ + l sθ=   
0)    2 ( 1 2 ) 1 1  l2 c (θ1 + θ 2 ) + l1cθ1 
0 0 0   0   0 

0 −1 0  l2 c (θ1 + θ 2 )   −l2 s (θ1 + θ 2 ) 

   
P1 ) 1 0 0  l2 s (θ1 + θ=
Z1 × ( P −= 2 )  l2 c (θ1 + θ 2 ) 
0 0 0   0   0 
Reemplazando en ( 42):

75
 Universidad Nacional de San Agustín
Autor: Lizardo Pari

−l2 s (θ1 + θ 2 ) − l1sθ1 −l2 s (θ1 + θ 2 ) 

 
 l2 c (θ1 + θ 2 ) + l1cθ1 l2 c (θ1 + θ 2 ) 
 0 0 
J=  ( 43)
 0 0 
 0 0 
 
 1 1 
π
Para θ1 = 0 y θ 2 = , con l1= l2= 1 :
2

 −l2 s ( π2 ) − l1s (0) −l2 s ( π2 )   −1 −1

 π  1 0
 l2 c ( 2 ) + l1c(0) l2 c ( 2 ) 
π
 
 0 0  0 0
=J =   
 0 0  0 0
 0 0  0 0
   
 1 1  1 1

Ejemplo 14

Hallar el jacobiano del robot del Ejemplo 4

l1 l2 l3

TCP

2 3
1
θ2 θ3
d1 0
Z0

Y0
X0 O0

Figura 36

Solución

Robot 3 GDL PRR, su estructura:

Z Z1 × ( P − P1 ) Z 2 × ( P − P2 )  ( 44)
J= 0 
0 Z1 Z2 

76
 Universidad Nacional de San Agustín
Autor: Lizardo Pari
A partir de la cinemática directa, se obtuvo:

− s (θ 2 + θ3 ) −c (θ 2 + θ3 ) 0 −l3 s (θ 2 + θ3 ) − l2 sθ 2 
 
c (θ 2 + θ3 ) − s (θ 2 + θ3 ) 0 l1 + l3c (θ 2 + θ3 ) + l2 cθ 2 
0
T3 = 
 0 0 1 d1 
 
 0 0 0 1 
De donde

 −l3 s (θ 2 + θ3 ) − l2 sθ 2 
 
P = l1 + l3c (θ 2 + θ3 ) + l2 cθ 2 
 d1 

De

0 −1 0 0  cθ 2 − sθ 2 0 l2 cθ 2 
1 0 0 l1   sθ 2 cθ 2 0 l2 sθ 2 
=
0
A 2 A=A  =
1 2
0 0 1 d1   0 0 1 0 
  
0 0 0 1 0 0 0 1 
 − sθ 2 −cθ 2 0 −l2 sθ 2 
 cθ − sθ 2 0 l2 cθ 2 + l1 
=
0
A 2 A=
1A 2
 2
 0 0 1 d1 
 
 0 0 0 1 

 −l2 sθ 2  0
=P2 l2 cθ 2 + l1  y Z 2 = 0 
 
 d1  1 

De

0 −1 0 0
1 0 0 l1 
A1 = 
0 0 1 d1 
 
0 0 0 1

0 0
 
P1 =  l1  y Z1 = 0 
 d1  1 

Además:

 −l3 s (θ 2 + θ3 ) − l2 sθ 2   −l3 s (θ 2 + θ3 )  0 
   
− P1  l3c (θ 2 + θ3 ) + l2 cθ 2 
P= y P2  l3c (θ 2 + θ3 )  ,
P −= Z 0 = 0 
 0   0  1 

77
Universidad Nacional de San Agustín
Autor: Lizardo Pari
Productos vectoriales:

0 −1 0   −l3 s (θ 2 + θ3 ) − l2 sθ 2   −l3c (θ 2 + θ3 ) − l2 cθ 2 
Z1 × ( P − P1 ) =     
1 0 0   l3c (θ 2 + θ3 ) + l2 cθ 2  =−  l3 s (θ 2 + θ3 ) − l2 sθ 2 
0 0 0   0   0 

0 −1 0   −l3 s (θ 2 + θ3 )   −l3c (θ 2 + θ3 ) 
   
Z 2 × ( P − P2 ) =1 0 0   l3c (θ 2 + θ3 )  =− l3 s (θ 2 + θ3 ) 
0 0 0   0   0 

Reemplazando en la estructura ( 44)

0 −l3c (θ 2 + θ3 ) − l2 cθ 2 −l3c (θ 2 + θ3 ) 
 
0 −l3 s (θ 2 + θ3 ) − l2 sθ 2 −l3 s (θ 2 + θ3 ) 
1 0 0 
J=  ( 45)
0 0 0 
0 0 0 
 
0 1 1 

3.5.- SINGULARIDADES CINEMÁTICAS.

Singularidades son aquellas configuraciones donde la movilidad de la estructura es reducida, es
decir no se puede alcanzar, en esta situación, se pierden grados de libertad, por ejemplo, cuando
dos o más ejes de movimiento están alineados.

Se comprueba que en estas configuraciones el rango de J es deficiente, es decir que se cumple

que:

det ( J ) = 0 ( 46)

Cuando la estructura se encuentra en una singularidad, puede haber infinitas soluciones a la

cinemática inversa.

3.5.1.- JACOBIAN CONDITION NUMBER (C)

C = J J −1 donde 1 ≤ C ≤ ∞ ( 47)

Donde, si C es muy grande, entonces se está cerca de la singularidad

Ejemplo:

Considerando que la posición [0,0] es una posición singular del robot plano de dos GDL, se
obtiene:

C (0, π2 ) = 2.618 , C (0, π6 ) = 9.35 , C ([ 0,5°]) =57.26 , C ([ 0,3°]) =95.47 ,

C ([ 0, 0.5°]) =572.95 ,

78
Universidad Nacional de San Agustín
Autor: Lizardo Pari

Ejemplo 15

Hallar las posiciones singulares del manipulador plano de dos GDL Ejemplo 3

l2

2
θ2
l1 Y0 1
θ1 X
0
Z0 0

Figura 37

Solución:

En el Ejemplo 13, se encontró el jacobiano del robot plano de 2 GDL, según la ecuación ( 43):

 −l s (θ + θ ) − l1sθ1 −l2 s (θ1 + θ 2 )  ( 48)

J= 2 1 2 
 l2 c (θ1 + θ 2 ) + l1cθ1 l2 c (θ1 + θ 2 ) 

Aplicando la ecuación ( 46):

  −l s (θ + θ ) − l1sθ1 −l2 s (θ1 + θ 2 )  

det   2 1 2
 l2 c (θ1 + θ 2 ) + l1cθ1  = 0
 l2 c (θ1 + θ 2 )  

Resolviendo, y eliminando algunos términos, se halla:

l1l2 sin θ 2 = 0

Esto se cumple para l1 ≠ 0 y l2 ≠ 0 , y para θ 2 = 0 ó θ 2 = π (ver Figura 38)

79
Universidad Nacional de San Agustín
Autor: Lizardo Pari

l2

Y0 Y0
l1 θ2 = 0 l1 θ2 = π
θ1 θ1
X0 X0
l2

Figura 38

En este caso, se cumple que rank ( J ) = 1 , reemplazando θ 2 = 0 en la ecuación del jacobiano

(ecuación ( 48)), se tiene:

 −(l + l ) sθ −l2 sθ1 

J= 2 1 1
 (l2 + l1 )cθ1 l2 cθ1 

Se ve que J posee dos columnas paralelas.

BIBLIOGRAFÍA

[1]. D.L. Pieper, “The kinematics of manipulators under computer control”, Ph.D. dissertation,
Standfor Univ., Standfor,CA, 1969.

R1

Figura 39

( 49)

80