View
246
Download
1
Category
Preview:
Citation preview
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
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
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
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 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θ
Recommended