Cinemática Inversa

Cinemática Inversa El objetivo del problema cinemático inverso consiste en encontrar los valores que deben adoptar las coordenadas articulares del robot q = [q1, q2, …, qn]T para que su extremo se posicione y oriente según una determinada posición espacial.

Así como es posible abordar el problema cinemático directo de una manera sistemática a partir de la utilización de matrices de transformación homogéneas, e independientemente de la configuración del robot, no ocurre lo mismo con el problema cinemático inverso, siendo el procedimiento de obtención de las ecuaciones fuertemente dependiente de la configuración del robot.

Cinemática Inversa Se han desarrollado algunos procedimientos genéricos susceptibles de ser programados, de modo que un computador pueda, a partir del conocimiento de la cinemática del robot (con sus parámetros de DH, por ejemplo) obtener la n-upla de valores articulares que posicionan y orientan su extremo. El inconveniente de estos procedimientos es que se trata de métodos numéricos iterativos, cuya velocidad de convergencia e incluso su convergencia en sí no esta siempre garantizada. A la hora de resolver el problema cinemático inverso es mucho más adecuado encontrar una solución cerrada. Esto es, encontrar una relación matemática explicita de la forma:

qk = fk( x, y, z, α, β, γ)

k = 1 ... n ( grados de libertad )

Cinemática Inversa Este tipo de solución presenta, entre otras, las siguientes ventajas:

1.  En muchas aplicaciones, el problema cinemático inverso ha de resolverse en tiempo real (por ejemplo, en el seguimiento de una determinada trayectoria). Una solución de tipo iterativo no garantiza tener la solución en el momento adecuado.

2.  Al contrario de lo que ocurría en el problema cinemático directo, con cierta frecuencia la solución del problema cinemático inverso no es única; existiendo diferentes n-uplas [q1, ... , qn]T que posicionan y orientan el extremo del robot del mismo modo. En estos casos una solución cerrada permite incluir determinadas reglas o restricciones que aseguren que la solución obtenida sea la mas adecuada posible.

Cinemática Inversa No obstante, a pesar de las dificultades comentadas, la mayor parte de los robots poseen cinemáticas relativamente simples que facilitan en cierta medida la resolución de su problema cinemático inverso.

Por ejemplo si se consideran solo tres primeros grados de libertad de muchos robots, estos tienen una estructura planar, esto es, los tres primeros elementos quedan contenidos en un plano. Esta circunstancia facilita la resolución del problema. Asimismo, en muchos robots se da la circunstancia de que los tres grados de libertad últimos, dedicados fundamentalmente a orientar el extremo del robot, corresponden a giros sobre los ejes que se cortan en un punto.

De nuevo esta situación facilita el calculo de la n-upla [q1, ... , qn]T correspondiente a la posición y orientación deseadas. Por lo tanto, para los casos citados y otros, es posible establecer ciertas pautas generales que permitan plantear y resolver el problema cinemático inverso de una manera sistemática.

Cinemática Inversa Los métodos geométricos permiten tener normalmente los valores de las primeras variables articulares, que son las que consiguen posicionar el robot. Para ello utilizan relaciones trigonometrías y geométricas sobre los elementos del robot. Se suele recurrir a la resolución de triángulos formados por los elementos y articulaciones del robot.

Como alternativa para resolver el mismo problema se puede recurrir a manipular directamente las ecuaciones correspondientes al problema cinemático directo. Es decir, puesto que este establece la relación:

donde los elementos tij son funciones de las coordenadas articulares [q1, ... , qn]T, es posible pensar que mediante ciertas combinaciones de las ecuaciones planteadas se puedan despejar las n variables articulares qi en función de las componentes de los vectores n, o, a y p.

[ ]ijt1000=⎥

⎦

⎤⎢⎣

⎡ paon

Cinemática Inversa Por ultimo, si se consideran robots con capacidad de posicionar y orientar su extremo en el espacio, esto es, robots con 6 grados de libertad, el método de desacoplamiento cinemático permite, para determinados tipos de robots, resolver los primeros grados de libertad, dedicados al posicionamiento, de una manera independiente a la resolución de los últimos grados de libertad, dedicados a la orientación. Cada uno de estos dos problemas simples podrá ser tratado y resuelto por cualquier procedimiento.

Resolución del problema cinemático inverso por métodos geométricos

Este procedimiento es adecuado para robots de pocos grados de libertad o para el caso de que se consideren solo los primeros grados de libertad, dedicados a posicionar el extremo.

El procedimiento en si se basa en encontrar suficiente número de relaciones geométricas en las que intervendrán las coordenadas del extremo del robot, sus coordenadas articulares y las dimensiones físicas de sus elementos.

Para mostrar el procedimiento a seguir se va a aplicar el método a la resolución del problema cinemático inverso de un robot con 3 grados de libertad de rotación (estructura típica articular). El dato de partida son las coordenadas (px, py, pz) referidas a {S0} en las que se requiere posicionar su extremo, el robot se muestra en la siguiente figura.

Resolución del problema cinemático inverso por métodos geométricos

Robot articular

Resolución del problema cinemático inverso por métodos geométricos

Como se ve este robot posee una estructura planar, quedando este plano definido por el ángulo de la primera variable articular q1.

El valor de q1 se obtiene inmediatamente como:

Considerando ahora únicamente los dos elementos 2 y 3 que están situados en un plano y utilizando el teorema del coseno, se tendrá:

⎟⎟⎠

⎞⎜⎜⎝

⎛=

x

y

pp

q arctan1

32

23

22

222

3

33223

22

22

222

2cos

cos2

llllppp

q

qllllpr

ppr

zyx

z

yx

−−++=

++=+

+=

Resolución del problema cinemático inverso por métodos geométricos

Esta expresión permite obtener q3 en función del vector de posición del extremo p. No obstante, por motivos de ventajas computacionales, es más conveniente utilizar la expresión de la arcotangente en lugar del arcoseno.

Puesto que:

se tendrá que:

32

3 cos1 qqsen −±=

32

23

22

222

3

3

32

3

2cos

coscos1

arctan

llllppp

q

qq

q

zyx −−++=

⎟⎟⎠

⎞⎜⎜⎝

⎛ −±=

Resolución del problema cinemático inverso por métodos geométricos

Como se ve, existen dos posibles soluciones para q3 según se tome el signo positivo o negativo de la raíz. Estas corresponden a las configuraciones de codo arriba y codo abajo del robot.

Elementos 2 y 3 del robot articular contenidos en un plano y en a) configuración codo abajo y b) configuración codo arriba.

Resolución del problema cinemático inverso por métodos geométricos

El cálculo de q2 se hace a partir de la diferencia entre β y α:

Siendo:

Luego, finalmente:

αβ −=2q

⎟⎟⎠

⎞⎜⎜⎝

⎛+

=

⎟⎟

⎠

⎞⎜⎜

⎝

⎛

+±=⎟

⎠⎞⎜

⎝⎛=

332

33

22

cosarctan

arctanarctan

qllqsenl

ppp

rp

yx

zz

α

β

⎟⎟⎠

⎞⎜⎜⎝

⎛+

−⎟⎟

⎠

⎞⎜⎜

⎝

⎛

+±=

332

33222 cos

arctanarctanqllqsenl

pppq

yx

z

Resolución del problema cinemático inverso por métodos geométricos

De nuevo los dos posibles valores según la elección del signo dan lugar a dos valores diferentes de q2 correspondientes a las configuraciones codo arriba y abajo.

Con las expresiones anteriores se resuelve el problema cinemático inverso para el robot de 3 GDL considerado.

Resolución del problema cinemático inverso a partir de la matriz de transformación homogénea

En principio es posible tratar de obtener el modelo cinemático inverso de un robot a partir del conocimiento de su modelo directo. Es decir, suponiendo 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, obtener por manipulación de aquéllas las relaciones inversas.

Sin embargo, en la practica esta tarea no es trivial siendo en muchas ocasiones tan compleja que obliga a desecharla. Además, puesto que el problema cinemático directo, resuelto a través de tij contiene en el caso de un robot de 6 grados de libertad 12 ecuaciones, y se buscan solo 6 relaciones (una por cada grado de libertad), existirán, necesariamente ciertas dependencias entre las 12 expresiones de partida con lo cual la elección de las ecuaciones debe hacerse con sumo cuidado.

Se va a aplicar este procedimiento al robot de 3 grados de libertad de configuración esférica (2 giros y un desplazamiento) mostrado en la figura. El robot queda siempre contenido en un plano determinado por el ángulo q1.

Resolución del problema cinemático inverso a partir de la matriz de transformación homogénea

Robot polar de 3 GDL.

Resolución del problema cinemático inverso a partir de la matriz de transformación homogénea

El primer paso a dar para resolver el problema cinemático inverso es obtener tij correspondiente a este robot. Es decir, obtener la matriz T que relaciona el sistema de referencia {S0} asociado a la base con el sistema de referencia {S3} asociado a su extremo. La siguiente figura muestra la asignación de sistemas de referencia según los criterios de DH con el robot situado en su posición de partida (q1 = q2 = 0), y la tabla muestra los valores de los parámetros de DH.

Parámetros DH del robot polar de 3 GDL

Articulación q d a a

1 q1 I1 0 90° 2 q2 0 0 -90° 3 0 q3 0 0

Resolución del problema cinemático inverso a partir de la matriz de transformación homogénea

Asignación de los sistemas de referencia del robot polar

Resolución del problema cinemático inverso a partir de la matriz de transformación homogénea

A partir de éstos es inmediato obtener las matrices A y la matriz T.

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡ −

=

1000010

0cos000cos

1

11

11

10

lqqsenqsenq

A

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡

+−−−−−

==

10000 12322

21321121

21321121

30

lCqCSSSqSSCCSSCqSCSCC

AT

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡

=

1000100

00100001

33

2

qA

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡

−

−

=

100000100cos000cos

22

22

21 qqsen

qsenq

A

Resolución del problema cinemático inverso a partir de la matriz de transformación homogénea

Obtenida la expresión 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 podría intentar manipular directamente las 12 ecuaciones resultantes de T a fin de despejar q1, q2, y q3 en función de n, o, a y p.

Sin embargo, este procedimiento directo es complicado, apareciendo ecuaciones trascendentes. En lugar de ello, suele ser más adecuado aplicar el siguiente procedimiento:

Puesto que T = 0A1 1A2 2A3 , se tendrá que:

(0A1)-1 T = 1A2 2A3

(1A2)-1 (0A1)-1 T = 2A3

Resolución del problema cinemático inverso a partir de la matriz de transformación homogénea

Puesto que: es conocida, los miembros a la izquierda

en las expresiones anteriores son función de las variables articulares (q1, …, qk) mientras que los miembros de la derecha son de las variables articulares (qk+1, ... , qn).

De este modo, de la primera de las expresiones se tendrá q1 aislado del resto de las variables articulares y tal vez será posible obtener su valor sin la complejidad que se tendría abordando directamente la manipulación de la expresión T. A su vez, una vez obtenida q1, la segunda expresión anterior (2A3), permitirá tener el valor de q2 aislado respecto de q3. Por ultimo, conocidos q1 y q2 se podrá obtener q3 de la expresión T sin excesiva dificultad.

⎥⎦

⎤⎢⎣

⎡=

1000paon

T

Resolución del problema cinemático inverso a partir de la matriz de transformación homogénea

Para poder aplicar este procedimiento, es necesario en primer lugar obtener las inversas de las matrices, i-1Ai. Esto es sencillo si se considera que la inversa de una matriz viene dada por:

⎥⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢⎢

⎣

⎡

−−−

=

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡

10001000papopn

Tzyx

Tzyx

Tzyx

zzzz

yyyy

xxxx

aaaooonnn

paonpaonpaon

Resolución del problema cinemático inverso a partir de la matriz de transformación homogénea

Por lo tanto, utilizando la primera de las ecuaciones definidas anteriormente, se tiene que:

( )

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡

−

−−

=

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡

−

−

=

=

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡

−−

==−

10000010

00

1000100

00100001

100000100000

1000100000

10000

3222

3222

3

22

22

11

1

11

322

111

0

qCCSqSSC

qCSSC

paonpaonpaon

CSl

SC

zzzz

yyyy

xxxx

AATA

Resolución del problema cinemático inverso a partir de la matriz de transformación homogénea

De las 12 relaciones establecidas en la ecuación anterior, interesan aquellas que expresan q1 en función de constantes (y no de q2 y q3). Así por ejemplo, tomando el elemento (3,4) se tiene:

( )

⎟⎟⎠

⎞⎜⎜⎝

⎛=

⎟⎟⎠

⎞⎜⎜⎝

⎛=

=−

x

y

x

y

yx

pp

q

pp

q

pCpS

arctan

tan

0

1

1

11

Resolución del problema cinemático inverso a partir de la matriz de transformación homogénea Utilizando la segunda de las ecuaciones definidas anteriormente se tendrá:

( ) ( )

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡

=

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡

−−−−

−

=

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡

=

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡

−−

⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢

⎣

⎡−

==−−

1000100

00100001

100010000

0

1000100

00100001

1000100000

10000

100000010000

3121212

211

2221212

311

1

11

22

22

321

101

21

qpaonpaonpaon

lCSSCSCCS

SlSSCCC

qpaonpaonpaon

CSl

SC

CS

SC

zzzz

yyyy

xxxx

zzzz

yyyy

xxxx

ATAA

Resolución del problema cinemático inverso a partir de la matriz de transformación homogénea

Tomando el elemento (1,4) se tiene:

y considerando que:

( ) ( )

( )1

112

12112

2121212

tan

0

0

lppSpC

q

lpSpSpCCSlpSpSCpCC

z

yx

zyx

zyx

−+

=

=−++

=−++

( )( ) ( )

2211

2211

221

221

1122

122

1

1122

122

12

11

11

2

211

02

0

yxyx

yxyxyx

yxyx

yxyxyx

yx

pppSpC

ppppCSpSpC

ppCSpSpC

ppCSpCpSpCpS

pCpS

+=+

+=++

=−+−

=−−=−

=−

Resolución del problema cinemático inverso a partir de la matriz de transformación homogénea

se tiene finalmente:

Por último, tomado de la ecuación anterior el elemento (3,4) se tiene:

Las ecuaciones anteriores corresponden a la solución del problema cinemático inverso del robot considerado.

⎟⎟

⎠

⎞⎜⎜

⎝

⎛

−+

=z

yx

plpp

q1

22

2 arctan

( ) ( )( ) 22

2123

312112

31221212

yxz

zyx

zyx

ppSlpCq

qlpCpSpCSqlCpCpSSpCS

+−−=

=−+−−

=−+−−

Desacoplamiento cinemático Los procedimientos vistos en los apartados anteriores permiten obtener los valores de las 3 primeras variables articulares del robot, aquellas que posicionan su extremo en las coordenadas (px, py, pz) determinadas, aunque pueden ser igualmente utilizadas para la obtención de las 6 a costa de una mayor complejidad.

No basta con posicionar el extremo del robot en un punto del espacio, sino que casi siempre es preciso también conseguir que la herramienta que aquél porta se oriente de una manera determinada. Para ello, los robots cuentan con otros tres grados de libertad adicionales, situados al final de la cadena cinemática y cuyos ejes, generalmente, se cortan en un punto, que informalmente se denomina muñeca del robot.

Si bien la variación de estos tres últimos grados de libertad origina un cambio en la posición final del extremo real del robot, su verdadero objetivo es poder orientar la herramienta del robot libremente en el espacio.

Desacoplamiento cinemático El método de desacoplo cinemático saca partido de este hecho, separando ambos problemas: posición y orientación.

Para ello, dada una posición y orientación final deseadas, establece las coordenadas del punto de corte de los 3 últimos ejes (muñeca del robot) calculándose los valores de las tres primeras variables articulares (q1, q2, q3) que consiguen posicionar este punto. A continuación, a partir de los datos de orientación y de los ya calculados (q1, q2, q3) obtiene los valores del resto de las variables articulares.

En la figura se representa un robot que reúne las citadas características, con indicación de los sistemas de coordenadas asociados según el procedimiento de Denavit-Hartenberg, cuyos parámetros se pueden observar en la tabla.

Desacoplamiento cinemático

Cinemática del robot IRB2400, cuya inversa se puede desacoplar

Desacoplamiento cinemático

Parámetros DH del robot de la figura.

Articulación q d a a

1 q 1 I1 0 -90°

2 q 2 0 I2 0

3 q 3 0 0 90°

4 q 4 I3 0 -90°

5 q 5 0 0 90°

6 q 6 I4 0 0

Desacoplamiento cinemático El punto central de la muñeca del robot corresponde al origen del sistema {S5}: O5. Por su parte, el punto final del robot será el origen del sistema {S6}: O6. Enseguida se utilizaran los vectores:

que van desde el origen del sistema asociado a la base del robot {S0} hasta los puntos centro de la muñeca y fin del robot, respectivamente.

Puesto que la dirección del eje z6 debe coincidir con la de z5 y la distancia entre O5 y O6 medida a lo largo de z5 es precisamente d4 = l4, se tendrá que:

estando todos los vectores referidos a las coordenadas del sistema {S0}.

60

50

OOOO

r

m

==

pp

64zlrm −= pp

Desacoplamiento cinemático En la expresión pr son las coordenadas del punto donde se pretende que se posicione el robot expresadas en {S0}. Por lo tanto:

El vector director z6 es el vector a correspondiente a la orientación deseada z6 = [ax, ay, az]T y l4 es un parámetro asociado con el robot. Por lo tanto, las coordenadas del punto central de la muñeca (pmx, pmy, pmz) son fácilmente obtenibles.

Es posible, mediante un método geométrico, por ejemplo, calcular los valores de (q1, q2, q3) que consiguen posicionar el robot en el pm deseado.

[ ]Tzyxr ppp ,,=p

Desacoplamiento cinemático Quedan ahora obtener los valores de q4, q5, y q6 que consiguen la orientación deseada. Para ello denominando 0R6 a la submatriz de rotación de 0T6 se tendrá:

0R6 = [n o a] = 0R3 3R6

donde 0R6 es conocida por ser la orientación deseada del extremo del robot, y 0R3 definida por:

0R3 = 0A1 1A2 2A3

también lo será a partir de los valores ya obtenidos de q1, q2 y q3. Por lo tanto:

3R6 = [rij] = (0R3)-1 0R6 = ( 0R)T [n o a]

tendrá sus componentes numéricas conocidas.

Desacoplamiento cinemático Por otra parte, 3R6 corresponde con la submatriz (3X3) de rotación de la matriz de transformación homogénea 3T6 que relaciona el sistema {S3} con el {S6}. Por lo tanto:

3R6 = 3R4 4R5 5R6

donde i-1Ri es la submatriz de rotación de la matriz de Denavit-Hartenberg i-1Ai, cuyos valores son:

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡ −=

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡−=

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡

−

−=

10000

01000

01000

66

66

65

55

55

54

44

44

43 CS

SCCSSC

CSSC

RRR

Desacoplamiento cinemático Luego se tiene que:

donde rij será por valore numéricos conocidos:

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡

−−+−+

−−−=

56565

546465464654

546465464654

63

CSSCSCSCCSCSSCCCSSCCSSCCSSCCC

R

[ ]⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡

−−+−+

−−−=

5r

6565

546465464654

546465464654

CSSCSCSCCSCSSCCCSSCCSSCCSSCCC

ij

Desacoplamiento cinemático De estas nueve relaciones expresadas se puede tomar las correspondientes a r13, r23, r33, r31, r32 :

Del conjunto de ecuaciones es inmediato obtener los parámetros articulares :

Estas expresiones y teniendo en cuenta que las posiciones de cero son distintas, constituyen la solución completa del problema cinemático inverso del robot articular.

65326531

533542354

rr

rrr13

SSCS

CCSSC

=−=

=−==

( )

⎟⎟⎠

⎞⎜⎜⎝

⎛−=

=

⎟⎟⎠

⎞⎜⎜⎝

⎛=

31

326

335

33

234

rrarctan

rarccosrr

q

q

arcsenq

Configuración articulada Considere el manipulador de la siguiente figura.

Configuración articulada Tenemos que:

Si hay un offset d1≠ 0 como se muestra en la siguiente figura entonces el centro de la muñeca no puede intersectar con el eje z0. En este caso, habrá en general, solamente dos soluciones para θ1.

⎟⎟⎠

⎞⎜⎜⎝

⎛=

x

y

pp

arctan1θ

Configuración articulada Estas corresponden a las llamadas configuraciones de brazo izquierdo y brazo derecho como se muestra en las siguientes figuras. Primero se muestra la configuración de brazo izquierdo.

Configuración articulada Observamos geométricamente que:

donde:

αφθ −=1

⎟⎟

⎠

⎞⎜⎜

⎝

⎛ −+=⎟

⎟⎠

⎞⎜⎜⎝

⎛ −=

⎟⎟⎠

⎞⎜⎜⎝

⎛=

1

21

22

1

21

2

arctanarctan

arctan

ddpp

ddr

pp

yx

x

y

α

φ

Configuración articulada La segunda solución dada por la configuración de brazo derecho de la segunda figura está dada por:

Para hallar los ángulos θ2, θ3 para el manipulador, dado θ1, consideremos el plano formado por la segunda y tercera articulación como se muestra en la siguiente figura.

⎟⎟

⎠

⎞⎜⎜

⎝

⎛ −+−⎟⎟⎠

⎞⎜⎜⎝

⎛=1

21

22

1 arctanarctand

dpppxpy yxθ

Configuración articulada Ya que el movimiento de las articulaciones dos y tres es planar, tenemos que:

Por tanto, θ3 está dado por:

Daa

aadpppaa

aasr

zyx =−−−++

=

−−+=

:2

)(

2cos

32

23

22

21

22232

23

22

22

3θ

⎟⎟⎠

⎞⎜⎜⎝

⎛ −±=DD2

31arctanθ

Configuración articulada De manera similar θ2 está dada por:

⎟⎟⎠

⎞⎜⎜⎝

⎛+

−⎟⎟

⎠

⎞⎜⎜

⎝

⎛

+−=

⎟⎟⎠

⎞⎜⎜⎝

⎛+

−⎟⎠⎞⎜

⎝⎛=

332

3322

332

332

arctan1arctan

arctanarctan

caasa

ppdpz

caasa

rs

yx

θ