Documentos de Académico
Documentos de Profesional
Documentos de Cultura
Cinemática Inversa Ejemplo2 PDF
Cinemática Inversa Ejemplo2 PDF
Robot con seis grados de libertad y cuyos parámetros de Denavit-Hartenberg están dados en
la tabla 1:
Parámetros de
Denavit-Hartenberg
qi θi di ai αi
1 q1 0 0 π/ 2
2 q2 L1 0 π/ 2
3 q 3 π 2 0 L 2 π/ 2
4 q4 L3 0 π/ 2
5 q5 0 0 π/ 2
6 q6 L4 0 0
Figura 1. Imagen tomada de Fundamentos De Robótica [2] Tabla 1
Con base en los parámetros de la tabla se generan las diferentes matrices de transformación
i 1
Ai :
C1 0 S1 0 C 2 0 S 2 0
S
0 C1 0 1 S2 0 C 2 0
0
A1 1
A2
0 1 0 0 0 1 0 L1
0 0 0 1 0 0 0 1
C(q 3 π 2 ) 0 S(q 3 π 2 ) L 2 C(q 3 π 2 ) S3 0 C3 L 2S3
S(q π 2 ) 0 C(q 3 π 2 ) L 2S(q 3 π 2 ) C 0 S3 L 2 C3
2
A3 3 Simplifica ndo 2 A 3 3
0 1 0 0 0 1 0 0
0 0 0 1 0 0 0 1
1
C 4 0 S4 0 C 5 0 S5 0 C 6 S6 0 0
S 0 C4
0 4 S 0 C5
0 5 S C6 0 0
3
A4 4 A5 5 A6 6
0 1 0 L3 0 1 0 0 0 0 1 L4
0 0 0 1 0 0 0 1 0 0 0 1
ThSimb 0 A11 A 2 2 A 3 3 A 4 4 A 5 5 A 6
De igual manera la matriz objetivo ThObj para la configuración deseada para la herramienta:
n hx o hx a hx Phx
n o hy a hy Phy QhObj Ph
hy
Phz 0 1
ThObj
n hz o hz a hz
0 0 0 1
ThObj ThSimb
ThObj 0 A 1 1 A 2 2 A 3 3 A 4 4 A 5 5 A 6
DESACOPLO CINEMÁTICO
Las coordenadas de Ph , vector conocido que indica la posición que se desea para el extremo
del robot:
Ph O0O6 Phx Phy Phz
T
En la figura 2 se observa que la magnitud del vector Pd O 5O 6 que va desde la muñeca hasta
el extremo del robot es L 4 , su dirección es la misma que la del conocido vector unitario
Z 6 ah a hx a hy a hz ; por lo tanto:
T
Pd O5O6 L 4 a hx a hy a hz
T
2
Figura 2
A partir de los vectores Ph y Pd , se puede obtener Pm mediante una suma de lazo vectorial:
Pm Pd Ph 0
Pm Ph Pd
3
PARTE 1: OBTENCIÓN DE q1 , q 2 y q 3
En esta parte, para el análisis sólo se consideran las tres primeras articulaciones, tomando
únicamente la parte de la estructura del robot que va desde su base hasta el punto que
representa la muñeca (ver figura 3):
Parámetros de
Denavit Hartenberg
qi θi di ai αi
π
1 q1 0 0
2
π
2 q2 L1 0
2
π L
3 q3 atan 3 0 L 2 L3
2 2
0
2 L2
Figura 3 Tabla 2
Al realizar la nueva asignación de los sistemas S i para el robot de la figura 3, se observa que
los sistemas S 0 , S1 y S 2 son los mismos que los del robot de la figura 1, no siendo así el sistema
S 3aux que en este caso indica el extremo del robot de la figura 3. Se puede observar que la
dirección del eje x 3 aux del sistema final es algo particular y, esto se debe a que al aplicar las
reglas para obtener los parámetros de D.H. el eje x i es la línea normal que corta los ejes z i y
z i1 . Por otra parte, la articulación cuatro y que hace girar el eslabón cuatro, puesto que no se
considera en esta parte, se toman los eslabones tres y cuatro como un solo eslabón rígido.
Por tanto los parámetros de Denavit Hartenberg para este robot serán las dos primeras filas
de la tabla 1. La última es necesario hallarla. La matriz de transformación homogénea
4
simbólica TmSimb que representa el extremo del robot de la figura 3, en este caso la muñeca se
obtiene:
C1 0 S1 0 C 2 0 S2 0
S 0 C1 0 1 S 0 C2 0
0
A1 1 A2 2
0 1 0 0 0 1 0 L1
0 0 0 1 0 0 0 1
3 3 2 3 3 2
S(q 3 π 2 atan( L3 L 2 )) C(q 3 π 2 atan( L3 L 2 )) L 2 L3
2 2
2
A 3aux 0 S(q 3 π 2 atan( L3 L 2 ))
0 0 1 0
0 0 0 1
3 3 2 3 3 2
C(q 3 atan( L3 L 2 )) S(q 3 atan( L3 L 2 )) L 2 L3
2 2
2
A 3aux 0 C(q 3 atan( L3 L 2 ))
0 0 1 0
0 0 0 1
n m x om x amx Pm x
0Q n Pm y
PmSimb om y amy
TmSimb 0 A11 A 2 2 A 3 aux 3aux TmObj my
0 1 n mz om z a mz Pm z
0 0 0 1
De la matriz TmObj sólo se conoce su última columna PmObj , la cual representa el vector posición
de la muñeca Pm . Ambas matrices son equivalentes ( TmSimb TmObj ), sólo que una es simbólica
y la otra es la matriz objetivo para la muñeca. Recordando que para la primera parte del
desacoplo cinemático sólo se consideran las posiciones y no las orientaciones, se extraen los
vectores de posición (las tres primeras filas de la cuarta columna) de ambas matrices y se
igualan:
PmSimb Pm
Pm z
2 2
S 2 S (q 3 atan( L L
3 2 )) L 2 L 3
5
De manera que se tienen tres ecuaciones con tres incógnitas. Por comodidad, dado el tamaño
de las ecuaciones anteriores, se simplificarán haciendo α q 3 atan L 3 L 2 y b L 2 L 3 :
2 2
S2Sα b Pm z (1.3)
Obtención de q 3
Elevando al cuadrado las ecuaciones 1.1, 1.2 y 1.3 se obtienen las ecuaciones 1.4, 1.5 y 1.6
respectivamente:
2
S1 L1 Cα b 2 2 Cα L1b 2 S1C1C 2Sα L1 Cα b b C1 C 2 Sα b 2 Pm x
2 2 2 2 2 2
(1.4)
2
C1 L1 Cα b 2 2 Cα L1b 2 S1C1C 2Sα L1 Cα b b S1 C 2 Sα b 2 Pm y
2 2 2 2 2 2
(1.5)
S2Sα b 2 Pm z 2
S2 Sα b 2 Pm z
2 2 2
(1.6)
S 1
2
2
C1 L1 C α b 2 2 C α L1b S1 C1 C 2 Sα b 2 Pm x Pm y
2 2 2 2
2 2 2 2
C α b 2 C 2 Sα b 2 2 C α L1b Pm x Pm y L1
2 2 2 2 2 2
C α
2
Sα b 2 2 Cα L1b Pm x Pm y Pm z L1
2 2 2 2 2
b 2 2 Cα L1b Pm x Pm y Pm z L1
2 2 2 2
Despejando Cα :
Pm x Pm y Pm z L1 b 2
2 2 2 2
Cα
2 L1b
q 3 I atan 2 1 Cα 2 , Cα atan L3
L2
(1.7)
q 3 II atan 2 1 Cα 2 , Cα atan L3
L2
El término 1 Cα
2
impone dos soluciones para q 3 al considerar ambos signos. Por otra
parte, ésta no depende de otras articulaciones.
Obtención de q 2
7
Pm z Phz L 4a hz
S2
L2
Sα b S q 3 atan L3 L 2 2 L3 2
q 2 atan 2S2 , C 2 atan 2 S2 , 1 S2 2
Phz L 4 a hz
S2
S q 3 atan
L3
L2 L 2 L3
2 2
q 2 I atan 2 S2 , 1 S2 2 (1.8)
q 2 II atan 2S 2 , 1 S2
2
Arrojando dos soluciones para q 2 . Es función f q 3 .
Obtención de q1
Pm x L1 Cα b S1 C 2Sα b C1
Pm y C 2Sα b S1 L1 Cα b C1
Pm x L1 C α b C 2Sα b S1
P
m y C 2Sα b L1 Cα b C1
Con las dos expresiones anteriores, una para S1 y otra para C1 ; se puede encontrar una
solución de tipo q1 atan 2S1 , C1 :
8
Si se desea, la solución para q1 se puede simplificar aún más. Observe que la expresión
L1 Cα b 2 C 2Sα b 2 es el denominador tanto de la parte real como imaginaria (como se vio en
el ejemplo 1, no influye en la magnitud de q1 ). Además su resultado siempre es positivo, lo
cual indica que tampoco influye sobre su cuadrante. De manera que se puede prescindir (ante
una situación como esta, si no se está seguro sobre si influye o no en la magnitud y
cuadrante, es mejor abstenerse de hacerlo) de dicha expresión:
α q 3 atan L 3 L 2 , b L 2 L 3
2 2
q1 atan 2 Phx L 4 a hx L1 C α b C 2Sα Phy L 4 a hy b , Phy L 4 a hy L1 C α b C 2Sα Phx L 4 a hx b
(1.9)
PARTE 2: OBTENCIÓN DE q 4 , q 5 y q 6
Una vez se conocen los valores para las tres primeras articulaciones, se obtienen las
posteriores que definen la orientación del robot. Retomando la matriz ThSimb generada con los
parámetros de la tabla 1:
ThSimb 0 A11 A 2 2 A 3 3 A 4 4 A 5 5 A 6
i 1
Tomando únicamente las submatrices de orientación de las matrices Ai :
QhSimb 0 Q 6 0 Q11 Q 2 2 Q3 3 Q 4 4 Q5 5 Q 6
Donde:
C1 0 S1 C 2 0 S 2 S3 0 C3
0
Q1 S1 0 C1 1
Q 2 S2 0 C 2 2
Q 3 C3 0 S3
0 1 0 0 1 0 0 1 0
C 4 0 S 4 C5 0 S5 C 6 S6 0
3
Q 4 S4 0 C 4 4
Q 5 S5 0 C5 5
Q 6 S6 C 6 0
0 1 0 0 1 0 0 0 1
9
3
Q6 Q
0
3
1
Donde 0 Q3 0 Q11 Q 2 2 Q3 y
QhObj 3
Q 6 3 Q 4 4 Q5 5 Q 6 (1.10)
Resolviendo la parte izquierda de la ecuación 1.10:
S4S6 C 4 C5C6 S4C6 C 4 C5S6 C 4S5
3
Q 6 C 4S6 + S4 C5C6 C 4C6 S4C5S6 S4S5 (1.11)
S5C6 S5S6 C5
Ahora nombrando como t el resultado de la parte derecha de la ecuación 1.10:
t11 t12 t13
t Q3 QhObj t 21 t 22 t 23
0 1
(1.12)
t 31 t 32 t 33
donde:
t11 n hx S1C3 C1C 2S3 + n hy S1C 2S3 C1C3 n hz S2S3
t12 o hx S1C3 C1C 2S3 + o hy S1C 2S3 C1C3 o hz S2S3
t13 a hx S1C3 C1C 2S3 + a hy S1C 2S3 C1C3 a hz S2S3
t 21 n hx C1S2 n hyS1S2 n hz C 2
t 22 o hx C1S2 o hyS1S2 o hz C 2
t 23 a hx C1S2 a hyS1S2 a hz C 2
t 31 n hx S1S3 C1C 2 C3 n hy S1C 2 C3 C1S3 n hz S2 C3
t 32 o hx S1S3 C1C 2 C3 o hy S1C 2C3 C1S3 o hz S2 C3
t 33 a hx S1S3 C1C 2 C3 a hy S1C 2 C3 C1S3 a hz S2 C3
Obtención de q 5
Tomando las posiciones C4S5 t13 y S4S5 t 23 de la ecuación 1.15, elevándolas al cuadrado y
luego sumando ambos resultados:
C 4 S5 t13
2 2 2
S4 S5 t 23
2 2 2
S 4
2 2
C 4 S5 t13 t 23
2 2 2
S5 t13 t 23
2 2 2
10
S5 t13 t 23
2 2
t 0 Q 3 QhObj
1
q 5 I atan 2 t132 t 23 2 , t 33 (1.14)
q 5 II atan 2 t132 t 232 , t
33
Obtención de q 4
Una vez q 5 es conocida, q 4 se puede hallar tomando las posiciones C4S5 t13 y S4S5 t 23 para
despejar C 4 y S 4 respectivamente:
t13
C4S5 t13 C4
S5
t 23
S4S5 t 23 S4
S5
q 4 atan 2S4 , C4
t 0 Q 3 QhObj
1
t 23 t13 (1.15)
q 4 atan 2 ,
S5 S5
Solución única para q 4 . Es función f q1 , q 2 , q 3 , q 5
Obtención de q 6
t 0 Q3 QhObj
1
t 32 t (1.16)
q 6 atan 2 , 31
S5 S5
11
Solución única para q 6 . Es función f q1 , q 2 , q 3 , q 5
12
RECOPILACIÓN DE LOS RESULTADOS
qi TABLA 3: SOLUCIONES PARA LAS VARIABLES ARTICULARES f q ,.., q
i n
q 3 I atan 2 1 C α 2 , C α atan L3
L2 Phx L 4a hx 2 Phy L 4a hy 2 Phz L 4a hz 2 L12 L 2 2 L3 2
q3 Cα
2 L1 L 2 L 3
2 2
L
q 3 II atan 2 1 Cα , Cα atan 3
2
L2
q 2 I atan 2 S2 , 1 S2 2 Phz L 4a hz
q2 S2
q3
atan 2S
L3
S q 3 atan L 2 L3
2 2
L2
, 1 S2
2
q 2 II 2
q1 atan 2 Phx L 4a hx L1 Cα b C 2Sα Phy L 4a hy b , Phy L 4a hy L1 Cα b C 2Sα Phx L 4a hx b
q1 L q2 ,q3
α q 3 atan 3 b L 2 L3
2 2
L2
q 5 I atan 2 t132 t 232 , t 33
q5 t 0 Q3 QhObj
1
q1 , q 2 , q 3
q 5 II atan 2 t13 t 23 , t 33
2 2
t 23 t13
q4 t 0 Q3 QhObj
1
q 4 atan 2 , q1 , q 2 , q 3 , q 5
S5 S5
t 32 t
t 0 Q3 QhObj
1
q6 q 6 atan 2 , 31 q1 , q 2 , q 3 , q 5
S5 S5
13
La tabla 3 muestra que la articulación q 3 es independiente, mientras que las demás variables
son funciones de otras. En la figura 4 se puede ver un árbol de 8 soluciones que pueden
satisfacer la configuración deseada para el robot:
I
q4 q 6 COMBINACIÓN 1
q1 q 5
I II q4 q 6 COMBINACIÓN 2
q 2
I II I
q4 q 6 COMBINACIÓN 3
q1 q 5
II q4 q 6 COMBINACIÓN 4
q 3
I
q4 q 6 COMBINACIÓN 5
q1 q 5
II I II q4 q 6 COMBINACIÓN 6
q 2
II I
q4 q 6 COMBINACIÓN 7
q1 q 5
II q4 q 6 COMBINACIÓN 8
Figura 4
COMPROBACIÓN
Con el fin de comprobar si las ocho combinaciones posibles satisfacen los parámetros de la
matriz Tr dada, sea entonces:
Resultados:
14
======================= COMBINACIÓN 1 =======================
q1 q2 q3 q4 q5 q6
0.3569 1.2088 -0.7728 -2.6310 0.3994 -0.3479
ThCalculada=
0.0487 -0.7859 -0.6164 -34.9540
0.4505 -0.5335 0.7158 76.7482
-0.8914 -0.3126 0.3281 10.3109
0 0 0 1.0000
ThObj=
0.0500 -0.7900 -0.6200 -34.9900
0.4500 -0.5400 0.7200 76.7900
-0.8900 -0.3100 0.3300 10.3300
0 0 0 1.0000
q1 q2 q3 q4 q5 q6
0.3569 1.2088 -0.7728 0.5106 -0.3994 2.7937
ThCalculada=
0.0487 -0.7859 -0.6164 -34.9540
0.4505 -0.5335 0.7158 76.7482
-0.8914 -0.3126 0.3281 10.3109
0 0 0 1.0000
ThObj=
0.0500 -0.7900 -0.6200 -34.9900
0.4500 -0.5400 0.7200 76.7900
-0.8900 -0.3100 0.3300 10.3300
0 0 0 1.0000
q1 q2 q3 q4 q5 q6
0.4276 1.9328 -0.7728 -2.5139 0.6666 -1.1285
ThCalculada=
0.0508 -0.7858 -0.6164 -34.9540
0.4520 -0.5323 0.7158 76.7482
-0.8906 -0.3150 0.3281 10.3109
0 0 0 1.0000
ThObj=
0.0500 -0.7900 -0.6200 -34.9900
0.4500 -0.5400 0.7200 76.7900
-0.8900 -0.3100 0.3300 10.3300
0 0 0 1.0000
======================= COMBINACIÓN 4 =======================
q1 q2 q3 q4 q5 q6
0.4276 1.9328 -0.7728 0.6277 -0.6666 2.0131
ThCalculada=
0.0508 -0.7858 -0.6164 -34.9540
0.4520 -0.5323 0.7158 76.7482
-0.8906 -0.3150 0.3281 10.3109
0 0 0 1.0000
ThObj=
0.0500 -0.7900 -0.6200 -34.9900
0.4500 -0.5400 0.7200 76.7900
-0.8900 -0.3100 0.3300 10.3300
0 0 0 1.0000
15
======================= COMBINACIÓN 5 =======================
q1 q2 q3 q4 q5 q6
0.4276 -1.2088 -1.1928 2.5435 0.7009 3.0199
ThCalculada=
0.0441 -0.7862 -0.6164 -34.9540
0.4474 -0.5361 0.7158 76.7482
-0.8932 -0.3074 0.3281 10.3109
0 0 0 1.0000
ThObj=
0.0500 -0.7900 -0.6200 -34.9900
0.4500 -0.5400 0.7200 76.7900
-0.8900 -0.3100 0.3300 10.3300
0 0 0 1.0000
q1 q2 q3 q4 q5 q6
0.4276 -1.2088 -1.1928 -0.5981 -0.7009 -0.1217
ThCalculada=
0.0441 -0.7862 -0.6164 -34.9540
0.4474 -0.5361 0.7158 76.7482
-0.8932 -0.3074 0.3281 10.3109
0 0 0 1.0000
ThObj=
0.0500 -0.7900 -0.6200 -34.9900
0.4500 -0.5400 0.7200 76.7900
-0.8900 -0.3100 0.3300 10.3300
0 0 0 1.0000
q1 q2 q3 q4 q5 q6
0.3569 -1.9328 -1.1928 2.8835 0.8399 -2.8305
ThCalculada=
0.0423 -0.7863 -0.6164 -34.9540
0.4461 -0.5372 0.7158 76.7482
-0.8940 -0.3053 0.3281 10.3109
0 0 0 1.0000
ThObj=
0.0500 -0.7900 -0.6200 -34.9900
0.4500 -0.5400 0.7200 76.7900
-0.8900 -0.3100 0.3300 10.3300
0 0 0 1.0000
======================= COMBINACIÓN 8 =======================
q1 q2 q3 q4 q5 q6
0.3569 -1.9328 -1.1928 -0.2581 -0.8399 0.3111
ThCalculada=
0.0423 -0.7863 -0.6164 -34.9540
0.4461 -0.5372 0.7158 76.7482
-0.8940 -0.3053 0.3281 10.3109
0 0 0 1.0000
ThObj=
0.0500 -0.7900 -0.6200 -34.9900
0.4500 -0.5400 0.7200 76.7900
-0.8900 -0.3100 0.3300 10.3300
0 0 0 1.0000
16
BIBLIOGRAFÍA:
[1] JOHN J. CRAIG. Robótica. Tercera Edición. Prentice Hall, 2006. ISBN: 9789702607724
[3] SUBIR KUMAR SAHA. Introducción A La Robótica. Primera Edición. McGraw-Hill, 2008.
ISBN: 9786071503138
17