Cinemática Inversa

del Robot

1
 Cinemática Inversa
El objetivo del problema cinemático inverso consiste en
encontrar los valores que deben de adoptar las
coordenadas articulares del robot q=[q1,q2,...,qn]T para que
su extremo se posicione y oriente según una determinada
localización espacial.
Se han desarrollado algunos procedimientos genéricos
suceptibles a ser programados, de modo que el
computador pueda a partir del conocimiento cinemático
del robot (con sus parámetros D-H) obtener los valores
articulares que posicionen y orienten su extremo. El
inconveniente de estos procedimientos es que se trata de
métodos numéricos iterativos, cuya velocidad de
convergencia no siempre esta garantizada.
2
Cuando se trata de resolver el problema cinemático
inverso es más adecuado encontrar una solución cerrada,
es decir una relación matemática de la forma:

q k  f k (x, y, z, α, β, γ); k  1,2,..., n (DOF)

Ventajas:
1. La solución del problema cinemático inverso en tiempo
real (por ejemplo, seguimiento de trayectoria). Una
solución de tipo iterativo no garantiza tener solución
en el momento adecuado.
2. La solución del problema cinemático inverso no es
único, existen n pociones que orientan el extremo del
robot. En este caso una solución cerrada es más
conveniente.
3
Una alternativa para resolver el mismo problema, es
recurrir a manipular directamente las ecuaciones
correspondientes al problema cinemático directo.

n o a p
 0 0 0 1   t ij   (1)
 

Donde los elementos tij son función de las coordenadas

articulares [q1,q2,...,qn]T, es posible pensar que mediante
ciertas combinaciones de las ecuaciones planteadas en
(1) se pueden despejar las n variables articulares qi en
función de las componentes n, o, a y p.

4
 Resolución PCI Método Geométrico
Este procedimiento es adecuado para robots de pocos
grados de libertad (3DOF). Aplicaremos el problema
cinemático inverso (PCI) al robot de 3DOF de rotación.

El robot tiene coordenadas

iniciales (px,py,pz) referidas
al {S0}. Se quiere posicionar
su extremo.

5
El robot posee una estructura planar, quedando definido
por el ángulo de la primera variable articular q1. El valor
de q1 se obtiene mediante:
 Px 
q1  tan  
1
P 
 y
Considerando ahora los elementos 2 y 3 que están
situados en el plano, y usando el teorema del coseno se
tiene:

6
 r  p 2x  p 2y
r  p 2z  l22  l32  2l2l3cosq 3
p 2x  p 2y  p z2  l22  l32
cosq 3 
2l2 l3
Esta expresión permite obtener q3 en función del vector
de posición p. Respecto a las ventajas computacionales,
es más conveniente usar la expresión de la tan-1.
De la expresión: sinq 3   1  cos 2 q 3

Se tiene: 
1   1  cos q 3
2 
q 3  tan 
 cosq 3 
 
7
Existen dos posibles soluciones para q3 según se tome el
signo positivo o el signo negativo en la raíz.
El cálculo de q2, se hace a partir de la diferencia de -:
q2  β  α
 
1  z 
p 1  pz 
β  tan    tan
 r    p2  p2 
 x y 
 l3sinq 3 
α  tan 
1

 l 2  l3cosq 3 
 
Luego:
q 2  tan 1  pz   tan 1  l3sinq 3 
  p2  p2   l  l cosq 
 x y   2 3 3 

8
 Resolución de PCI Matriz Homogénea
En principio es tratar de obtener el modelo cinemático
inverso de un robot a partir del conocimiento de su modelo
directo, es decir, son conocidas las relaciones que expresan
el valor de la posición y orientación del extremo del robot en
función de sus coordenadas articulares.

Sea un robot de 3DOF de

configuración RRP (2 giros y
un desplazamiento). El robot
queda contenido en un plano
determinado por el ángulo q1.

9
En primer término obtenemos la expresión que
corresponde al robot mediante la matriz de
transformación homogénea T que relaciona al sistema de
referencia {S0} asociado a la base con el sistema de
referencia {S3} asociado a su extremo.

La figura muestra los

criterios de asignación de
los sistemas de referencia
según D-H con el robot
situado en (q1=q2=0). A
partir de esto obtenemos
las matrices A y T.

10
Recordar:
i Es el ángulo que forman los ejes xi-1 y xi medido en un plano
perpendicular al eje zi-1, usando la regla de la mano derecha. Se
trata de un parámetro variable en articulaciones giratorias.
di Es la distancia a lo largo del eje zi-1 desde el origen del sistema de
coordenadas (i-1)-ésimo hasta la intersección del eje zi-1 con el eje
xi. Se trata de un parámetro variable en articulaciones prismáticas.
ai Es la distancia a lo largo del eje xi que va desde la intersección del
eje zi-1 con el eje xi hasta el origen del sistema i-ésimo, en el caso
de articulaciones giratorias. En el caso de articulaciones
prismáticas, se calcula como la distancia más corta entre los ejes
zi-1 y zi.
i Es el ángulo de separación del eje zi-1 y el eje zi, medido en un
plano perpendicular al eje xi, utilizando la regla de la mano
derecha.

11
 i 1
Ai  T ( z, i )T (0,0, d i )T (ai ,0,0)T ( x, i )

C i  S i 0 0 1 00 0  1 0 0 ai  1 0 0 0
 S C i 0 0 0 10 0  0 1 0 0  0 C i  S i 0
i 1
Ai   i
 0 0 1 0  0 01 d i  0 0 1 0  0 S i C i 0
    
 0 0 0 1  0 00 1  0 0 0 1  0 0 0 1
C i  C i S i S i S i ai C i 
 S C i C i  S i C i ai S i 
i 1
Ai   i
 0 S i C i di 
 
 0 0 0 1 

12
 Parámetros D-H del robot

Link  d a 
1 q1 l1 0 90°
2 q2 0 0 -90°
3 0 q3 0 0°

C1 0 S1 0  C 2 0  S2 0
S 
0  C1 0  1  S2 0 C1 
0
0
A1   1
A2  
0 1 0 l1  0 1 0 0
   
0 0 0 1 0 0 0 1

13
 1 0 0 0 C1C 2 - S1  C1S2 0
0 1 
0 0 0  S1C 2 C1 - S1S2 0 
2
A3   ; A2  
0 0 1 q3   S2 0 C2 l1 
   
0 0 0 1  0 0 0 1

C1C 2  S1  C1S2  q 3C1S2 

S C C1  S1S2  q 3S1S2  
T0 A3   1 2
 S2 0 C2 q 3C 2  l1 
 
 0 0 0 1 

14
Obtenida la matriz de T en función de las coordenadas
articulares (q1, q2, q3), y supuesta una localización de
destino para el extremo del robot definida por los
vectores n, o, a y p se puede intentar manipular
directamente las ecuaciones resultantes a fin de
despejar q1, q2, y q3 en función de n, o, a, p.
Sin embargo este procedimiento directo es complicado,
en lugar de ello aplicaremos el siguiente procedimiento:
0 1 2
Se tiene que: T A1 A 2 A 3

A  A
1
2
1 0
1
1
T2 A3
Puesto que T es conocido, los miembros a la izquierda
son función de las variables articulares (q1,...,qk), mientras
que los miembros de la derecha los son de (qk+1,...,qn).
15
Para aplicar este procedimiento, es necesario obtener las
inversas de las matrices i-1Ai.
1
n x ox ax px  n x ny nz  n T p
n   T 
 y oy ay py   ox oy oz  o p

n z oz az pz  a x ay az a pT 

   
0 0 0 1  0 0 0 1 
Luego obtenemos:
C1 0 S1 0  C1 S1 0 0
S 0  C1 0   0 0 1 - l1 
A1   1
1
0

0 1 0 l1   S1  C1 0 0
   
0 0 0 1  0 0 0 1
16
 C 2 0 - S2 0  C 2 S2 0 0
S 0 C2 
0  0 0 -1 
0
1 1
A2   2

0 -1 0 0  - S 2 C2 0 0
   
0 0 0 1  0 0 0 1

1 0 0 0  1 0 0 0 
1 1 0 0  0 1 0 0 
A1  
1
0

0 1 1 q 3  0 0 1 - q3 
   
0 0 0 1  0 0 0 1 

17
Utilizando la primera de las ecuaciones se tiene:

C1 S1 0 0  n x ox ax px 
0   
 A1  T3   S - C
0 1 0
 0 1
0
l1  n y
0  n z
oy
oz
ay
az
py 
py 
1 1
  
0 0 0 1 0 0 0 1

C 2 0  S2 0 1 0 0 0  C 2 0  S2  S2 q 3 
S 0 C2 0 0 1 0 0   S2 0 C2 C 2 q 3 
 2 
0 1 0 0  0 0 1 q3   0 1 0 0 
    
0 0 0 1  0 0 0 1  0 0 0 1 

18
De las relaciones anteriores, interesan aquellas que
expresan q1 en función de las constantes (y no de q2 y
q3). Por ejemplo si tomamos el elemento (3,4), tenemos:

S1p x  C1p y  0
De la relación:

 py  1  p y 
tan(q 1 )     q1  tan  
 px   px 
Utilizando ahora la segunda de las ecuaciones, tenemos:

A  A
1
2
1 0
1
1
T2 A3 

19
 C2 S2 S2 0  C1 S1 0 0  n x ox ax px 
 0 0 -1 0   0 0 1 -l1   n y oy ay p y 

-S2 C2 0 0   S1 -C1 1 0  nz oz az pz 
   
 0 0 0 1  0 0 0 1  0 0 0 1
1 0 0 0
0 1 0 0 

0 0 1 q3 
 
0 0 0 1

20
 C2C1 C2S1 S2 -l1S2   n x ox ax p x  1 0 0 0
 -S C1 0 0   n y oy ay p y  0 1 0 0 
 1 
-S2C2 -S2S1 C2 -C2l1   n z oz az p z  0 0 1 q3 
    
 0 0 0 1  0 0 0 1  0 0 0 1

Tomando el elemento (1,4) se tiene:

C 2C1p x  C 2S1p y  S2 p z  l1S2  0

C 2 C1p x  S1p y   S2 p z  l1   0
C1p x  S1p y
tan(q 2 )  
p z  l1

21
y considerando: S1p x  C1p y  0

Elevando al cuadrado, tenemos:

S p  C p   S p
1 x 1 y
2 2
1
2
x  C12 p 2y  2S1C1p x p y
1  C p  1  S p
2
1
2
x
2
1
2
y  2S1C1p x p y
C12 p 2x  S12 p 2y  2S1C1p x p y  p 2x  p 2y
C1p x  S1p y  p 2x  p 2y

Se tiene finalmente:
 p 2x  p 2y 
q 2  tan 1  
 l1  p z 
 
22
Por último, tomando de:

 C2C1 C2S1 S2 -l1S2   n x ox ax p x  1 0 0 0

 -S C1 0 0   n y oy ay p y  0 1 0 0 
 1 
-S2C2 -S2S1 C2 -C2l1   n z oz az p z  0 0 1 q3 
    
 0 0 0 1  0 0 0 1  0 0 0 1

el elemento (3,4)
- S2 C1p x  S2S1p y  C 2 p z  C 2 l1  q 3
C 2 p z  l1  - S2 C1p x  S1p y   q 3
q 3  C 2 p z  l1  - S2 p x2  p y2
Las expresiones para q1, q2 y q3 son la solución al
problema
23
 Desacoplo Cinemático
En general no basta posicionar el extremo del robot en un
punto del espacio, sino que casi siempre es preciso
conseguir que la herramienta que porta se oriente de una
manera determinada. Para ello los robots cuentan con
otros 3 grados de libertad adicionales, situados al final de
la cadena cinemática, y cuyos ejes generalmente se cortan
en un punto, que se denomina muñeca del robot.
El método de desacoplo cinemático separa ambos
problemas: posición y orientación, para ello se requiere dar
una orientación y posición deseadas, establecer las
coordenadas del punto de corte de los 3 últimos ejes
(muñeca de robot) calculándose los valores de las 3
variables articulares y conseguir posicionar este punto.
24
En la figura se presenta un robot que reúne las citadas
características, con indicación de los sistemas de
coordenadas asociados según el procedimiento D-H,
cuyos parámetros son dados en la tabla.

Link  d a 
1 1 l1 0 -90°
2 2 0 l2 0°
3 3 0 0 90°
4 4 l3 0 -90°
5 5 0 0 90°
6 6 l4 0 0°

25
 MATRIZ JACOBIANA
El modelado cinemático de un robot busca las relaciones
entre las variables articulares, la posición y la orientación
del extremo del robot. En esta relación no se tiene en
cuenta las fuerzas o pares que actúan sobre el robot
(actuadores, cargas, fricciones, etc) y que pueden originar
el movimiento del mismo, de ese modo es necesario
conocer la relación.
Así el control del robot debe de establecer que
velocidades se debe de imprimir a cada articulación (a
través de actuadores) y conseguir que el extremo
desarrolle una trayectoria concreta, (p,e línea recta a
velocidad cte). Para ello es necesario disponer de la
relación de las velocidades de las coordenadas articulares
y las de posición y extremo del robot.
26
La relación entre ambos vectores de velocidad se obtiene
a través de la matriz Jacobiana.

La matriz Jacobiana nos permite conocer las velocidades

del extremo del robot a partir de los valores de las
velocidades de cada articulación. Por otro lado la matriz
Jacobiana inversa permitirá conocer las velocidades
articulares necesarias para obtener unas velocidades
determinadas en el extremo del robot.
27
 Relaciones Diferenciales
El método más directo para obtener la relación entre
velocidades articulares y del extremo del robot consiste
en diferenciar las ecuaciones correspondientes al modelo
cinemático directo. Supongamos que se conocen las
ecuaciones que resuelven el problema cinemático directo
de un robot de nDOF.
x  f x (q1 ,..., q n ) y  f y (q1 ,..., q n ) z  f z (q1 ,..., q n )
α  f α (q1 ,..., q n ) β  fβ (q1 ,..., q n ) γ  f γ (q1 ,..., q n )
Derivando con respecto al tiempo el conjunto de
ecuaciones anteriores:
n f 
 n
f x    n
f z 
x qi y   y
qi z   qi
1 q i 1 q i 1 q i
 n
f α   n f β   n f γ 
α qi β   qi γ   qi
1 q i 1 q i 1 q i 28
O de forma matricial:

 f x f x f x 
 

 

   
x q
 1 q1 q 2 q n 
 
 y    
   
 z   J.   J   
    
 
α        
    
β    
 f f γ 
f γ 
  

 γ
 γ  q n    
 q1 q 2 q n 

La matriz J se denomina matriz Jacobiana.

29
Problema: Obtener la matriz Jacobiana del robot SCARA
de la figura. El problema cinemático directo viene
determinado por las ecuaciones:

x  l3C12  l 2 C1
y  l3S12  l 2S1
z  l1  q 3

30
  
  

 x   (l 3S12  l 2S1 )  l 3S12 0   1 
q
y    l 3C12  l 2C1 l C 0  q 
  3 12    
2

 z   0 0  1 q 
   3 
Si el robot se encuentra en un momento determinado en
la posición dada por:

 
q1  rad q2  rad q3  0.75cm
6 4
Moviéndose a una velocidad articular instantánea.

 
 
q1  rad / s q2  rad / s q3  1m / s
2 2
31
y con l2=l3=1 m, la velocidad del extremo será:


 x   1.465  0.965 0   / 2  3.81
y    1.124 0.258 0   / 2   2.17 
     
 z   0 0  1  1    1 
 
Si, mantenemos la velocidad articular, el robot se
encontrase en la posición q1=\3 rad, q2= \2 rad, la
velocidad del extremo sería:

 x    1.36  0.5 0   / 2   2.92 
y    0.366 0.866 0   / 2   1.935
     
 z   0 0  1  1    1 
 
32
 Jacobiano Inverso
Del mismo modo que se ha obtenido la relación directa
que permite obtener las velocidades del extremo a partir
de las velocidades articulares, puede obtenerse la
relación inversa que permite calcular las velocidades
articulares dependiendo de las del extremo.
En la obtención de la relación  
  

x
inversa puede emplear varios  q1  
procedimientos. q   y
 2 
En primer lugar, supuesta conocida     J 1  z 
la relación directa, dada por la  α 
matriz Jacobiana, se puede obtener   
la relación inversa invirtiendo  β 
simbólicamente la matriz.  
q n   γ  33