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

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 1

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

( x, y )
l2

l1 2
Y0

1
Z0
X0
Figura 2

Solución.

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

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

l 2  l12  l22  2l1l2 cos A ( 0)

cos A  cos(180   2 )   cos  2
Pero l  x  y y
2 2 2

En ( 0)

x 2  y 2  l12  l22  2l1l2 cos  2

Despejando:

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

x 2  y 2  l12  l22 ( 0)
cos  2 
2l1l2

( x, y )

l2 sin  2
l2
l
Y0
2
A l2 cos  2
 l1
 1
Z0
X0
Figura 3

Cálculo de
1

Según la Figura 3:

1    
( 0)
 y
  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 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
problemas más simples: problema cinemático inverso de posición y problema cinemático
inverso de orientación. En este caso el origen de los tres últimos GDL se encuentran en el
0
origen del frame F4, cuya posición en el sistema base F0 (
d 4 , ver Figura 4) 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 4

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 ( 0)
Donde A matriz de dimensión mXn, x vector nX1, y b vector mX1. La solución es:

x  Ab ( 0)
Donde:

A    AT A  AT
1 ( 0)

Donde A es una matriz nXm.

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

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

R t  ( 0)
Hd     E (3)
 0 1 con R  SO (3)
Encontrar (una o todas) las soluciones a la ecuación:

H d  0 Tn (q1 ,, qn ) ( 0)
Donde
0
Tn  A1  A 2  A n

La ecuación ( 0) resulta en un sistema de 12 ecuaciones no lineales con n variables que pueden

escribirse como:

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

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

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

A11  H d  A 2  A n
ó ( 0)
A 21  A11  H d  A 3  A n
ó
1 1 1
A  A  A  Hd  A4  An
3 2 1

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.
( px , p y , pz )
- En la ecuación ( 0), de preferencia, igualar con la parte de posición de
H d , dado que la parte rotacional es muy sensible a errores.

Ejemplo 2

Considere los resultados del Error: Reference source not found, 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

 r11 r12 r13 px 

r r r33 p y 
( 0)
H d   21 22
 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 5

SOLUCIÓN:

De acuerdo a la ecuación ( 0), 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
r21   cos( 3  2 ) cos  4  sin( 3  2 ) sin  4 ( 0)

r22   cos( 3  2 ) sin  4  sin( 3  2 ) cos  4

r23  0
r31  0
r32  0
r33  1
px  d 2  0.2sin( 3  2 )
p y  0.2 cos( 3  2 )
pz  0.25  d1

De las tres últimas ecuaciones, se tiene:

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

d1  pz  0.25
( 0)
 p  
3  a cos  y  
 0.2  2
d 2  px  0.2sin(3  2 )

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 ( 0):

Bx  Ay  r11
( 0)
Ax  By  r12
Ax  By  r21
 Bx  Ay  r22
A  sin(3  ) , B  cos( 3  2 ) , x  sin  4 , y  cos  4 . Se forma el sistema Ax  b ,

2
Donde
con

B A  r11 
 A B r  ( 0)
A   b   12 
 A B x  r21 
  x   
  B  A ;  y ,  r22 

Se aplica la solución por mínimos cuadrados, hallado el valor de x y y , se halla 4 a través
del arco tangente.

Ejemplo 3

Hd
Hallar la cinemática inversa del Error: Reference source not found, si se conoce

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 6

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

Solución:

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

px  l3 s (3   2 )  l2 s 2 ( 0)
p y  l1  l3c(3   2 )  l2c 2
pz  d1
De donde

d1  pz ( 0)
 2 y 3 están fuertemente acoplados, por lo que aplicaremos la primera de las ecuaciones
( 0):

A11  H d  A 2 A3 ( 0)
A1 del Error: Reference source not found, se obtiene:
A partir de las ecuaciones

0 1 0 l1 
 1 0 0 0 
A11  
0 0 1  d1 
 
0 0 0 1 

A 2 A 3 del Error: Reference source not found:

Aplicando en ( 0), conociendo

0 1 0 l1   r11 r11 r11 px 

 1 0 0 0   r11 r11 r11 p y 
 
0 0 1 d1   r11 r11 r11 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:

p y  l1  l3c   2   3   l2c 2
 px  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 ( 0), a la forma:

A 21A11  H d  A 3 ( 0)

61
Universidad Nacional de San Agustín
Autor: Lizardo PAri

Del Error: Reference source not found,

 c2 s 2 0 l2 
  s c 2 0 0 
A2  
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 2 0 0   1 0 0 0   c2  s2 0 l1s 2 
A 21A11   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 ( 0):

  s 2 c 2 0 l1c 2  l2   r11 r12 r13 p x  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 ( 0)

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  l32c 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  ( 0)
3  a cos 
x  y 1 2 3

 2l2l3 
 

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

Cálculo de
3 :

s 2 , multiplicamos la primera ecuación de ( 0) por [ p y  l1 ] y la segunda por

A fin de eliminar
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  pxl3s3

[ p y  l1 ] 2 :
Factorizando y despejando

  p  l   l  l c   p l s  ( 0)
 2  a cos  
1 2
y 3 3 x 3 3

  
px   p y  l1 
2 2

 

63