Universidad Nacional de San Agustín

Autor: Lizardo Pari

CAPÍTULO 2

MODELO CINEMÁTICO DEL ROBOT

2.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”.

2.1.1.- CADENA CINEMÁTICA

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

2.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

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

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

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

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

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

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

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

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

Tabla 5

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 5 e indique que rotaciones
básicas se realizaron para conseguir 0 R 3 .

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

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 14

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

43
 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

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

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

Rotaciones básicas:

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

2.3.- CINEMÁTICA INVERSA

Es “encontrar las posiciones articulares a partir de la orientación y posición del efector final”,
(ver Figura 17). 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 17

2.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 5

46
Universidad Nacional de San Agustín
Autor: Lizardo Pari
Hallar la cinemática inversa del robot plano de 2 GDL sabiendo que su posición (x,y) de su
extremo es conocido, Figura 18.

( x, y )
l2

θ2
Y0 l1

θ1
Z0
X0

Figura 18

Solución.

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

Aplicando ley de cosenos en el triángulo l − l1 − l2 : (ver Figura 19)

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

Despejando:

x 2 + y 2 − l12 − l22 ( 8)
cos θ 2 =
2l1l2

( x, y )

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

β l1
α θ1
Z0
X0
Figura 19

Cálculo de θ1

Según la Figura 19:

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

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

2.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 (p.e. la muñeca esférica de 3 GDL), por lo que el problema cinemático puede ser
separado (desacoplarse) en dos subproblemas 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 (Figura
20), cuya posición en el sistema base F0 ( 0d 4 ), se encuentra mediante un enfoque geométrico.

Z4

Z5
0
d4
Z0 Z6

X0 Y0

Figura 20

2.3.3.- ENFOQUE MATEMÁTICO

2.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): existen infinitas soluciones.

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

• (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 ( 10)

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

x = A +b ( 11)

Donde:

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

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

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

2.3.3.3.- FORMULACIÓN GENERAL DE LA CINEMÁTICA INVERSA

Dada una matriz de transformación homogénea H d del efector final:

R t  ( 13)
=
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 ) ( 14)

Donde
0
T=
n A1 ⋅ A 2  A n , proviene de la cinemática directa.

La ecuación ( 14) 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 ( 15)

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 ( 14) puede reescribirse
como:

A1−1 ⋅ H d =
A2  An

49
Universidad Nacional de San Agustín
Autor: Lizardo Pari
ó ( 16)
−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 ( 14), 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 6

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

donde:

 r11 r12 r13 px 

r r r33 p y 

H d = 21 22 ( 17)
 r31 r32 r33 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 21

SOLUCIÓN:

50
Universidad Nacional de San Agustín
Autor: Lizardo Pari
De acuerdo a la ecuación ( 14), se tiene el siguiente sistema de ecuaciones:

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

r12 = sin(θ3 + π2 ) sin θ 4 − cos(θ3 + π2 ) cos θ 4
r13 = 0
− cos(θ3 + π2 ) cos θ 4 + sin(θ3 + π2 ) sin θ 4
r21 =
− cos(θ3 + π2 ) sin θ 4 − sin(θ3 + π2 ) cos θ 4
r22 =
r23 = 0 ( 18)
r31 = 0
r32 = 0
r33 = −1
d 2 + 0.2sin(θ3 + π2 )
px =
p y = −0.2 cos(θ3 + π2 )
pz =
−0.25 + d1

De las tres últimas ecuaciones, se tiene:

d=
1 pz + 0.25
 p  π ( 19)
=θ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 ( 18):

Bx + Ay =
r11
Ax − By =
r12 ( 20)
Ax − By =
r21
− Bx − Ay = r22
=
Donde A sin(θ3 + = π
2), B cos(θ3 + 2 ) , x = sin θ 4 , y = cos θ 4 . Se forma el sistema Ax = b ,
π

con

B A  r11 
 A −B r 
 x ( 21)
A=   ; x= , b= 
12

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

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

Ejemplo 7

Hallar la cinemática inversa del robot PRR de 3 GDL del Ejemplo 4, 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 22

Solución:

A partir de la cinemática directa realizada en el Ejemplo 4, 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θ sθ3
'
cθ 2' 0.1cθ 2' + d1 
 
2

 0 0 0 1 
Aplicando la ecuación ( 14):

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

Igualando la parte traslacional:

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

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:

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

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

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

A partir de las matrices A i del Ejemplo 4, 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 ( 23):

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

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

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

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

Del Ejemplo 4, se sabe que

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 ( 24):

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 4:

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

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

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 ( 11), encontrándose θ3 con el arco tangente:

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

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

2.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 ( 25)

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  ( 26)
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.

2.4.1.- CÁLCULO DEL JACOBIANO.

La ecuación ( 26) puede reescribirse como:

J  J Pn  ( 27)
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 .

2.4.1.1.- CONTRIBUCIÓN A LA VELOCIDAD ANGULAR

a) Articulación prismática:

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

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

2.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 23

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  ( 28)
J  =  0  Articulación prismática
 Oi   

 J Pi   Z i −1 × ( P − Pi −1 )  ( 29)
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
-

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

2.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  ( 30)
=q J= X J −1  
ω 

Ejemplo 8

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 24

Solución:

Al ser un robot PPRR, según las ecuaciones ( 28) y ( 29):

Z Z1 Z 2 × ( P − P2 ) Z 3 × ( P − P3 )  ( 31)
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 

58
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 

59
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=  ( 32)
0 0 0 0
0 0 0 0
 
0 0 1 1 

Ejemplo 9

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 25

Solución:

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

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

 Z × ( P − P0 ) Z1 × ( P − P1 )  ( 33)
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 ( 33):

61
 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=  ( 34)
 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

2.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 ( 35)

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

cinemática inversa.

2.5.1.- JACOBIAN CONDITION NUMBER (C)

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

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 ,

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

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

l2

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

Figura 26

Solución:

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

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

J= 2 1 2 
 l2 c (θ1 + θ 2 ) + l1cθ1 l2 c (θ1 + θ 2 ) 
Aplicando la ecuación ( 35):

  −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 27)

l2

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

Figura 27

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

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

(ecuación ( 37)), 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.

R1

Figura 28

( 38)

64