Documentos de Académico
Documentos de Profesional
Documentos de Cultura
Cinemática Inversa Ejemplo1 PDF
Cinemática Inversa Ejemplo1 PDF
Parámetros de
Denavit Hartenberg
qi θi di ai αi
1 q1 L1 0 π/ 2
2 q2 0 L2 0
3 q3 0 0 π/ 2
4 q 4 L 3 0 π/ 2
5 q5 0 0 π/ 2
6 q6 L4 0 0
i 1
Con base en la tabla 1 se obtienen las respectivas matrices de transformación Ai
0
A1 Trz q1 Tt 0 0 L1 Tt 0 0 0 Trx π 2
1
A 2 Trz q 2 Tt 0 0 0 Tt L 2 0 0 Trx 0
2
A 3 Trz q 3 Tt 0 0 0 Tt 0 0 0 Trx π 2
3
A 4 Trz q 4 Tt 0 0 L3 Tt 0 0 0 Trx π 2
4
A 5 Trz q 5 Tt 0 0 0 Tt 0 0 0 Trx π 2
5
A 6 Trz q 6 Tt 0 0 L 4 Tt 0 0 0 Trx 0
0 1 0 L3 0 1 0 0 0 0 1 L4
0 0 0 1 0 0 0 1 0 0 0 1
La matriz de transformación ThSimb que determina la posición y orientación del sistema S 6
ubicado en el extremo del robot respecto al sistema de la base S 0 , se obtiene mediante el
i 1
producto de las matrices Ai :
ThSimb 0 A11 A 2 2 A 3 3 A 4 4 A 5 5 A 6
El subíndice hSimb se refiere a ‘ h ’ por extremo del robot o efector final y ‘ Simb ’ porque
dicha matriz se obtiene a partir del producto de las matrices simbólicas i 1 A i . Dado el
contexto de cinemática inversa se suponen las 12 posiciones de la matriz Th conocidas ya
que allí es donde se desea posicionar el robot. La matriz conocida se representa por la letra
ThObj , cuyo subíndice ‘ hObj ’ se refiere al objetivo o 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
Ambas matrices son equivalentes, sólo que de una de éstas se conocen sus valores y la otra
es simbólica o está en función de las variables articulares:
ThObj ThSimb
ThObj 0 A11 A 2 2 A3 3 A 4 4 A5 5 A6
DESACOPLO CINEMÁTICO
Se divide el problema en dos partes: la primera parte consiste en encontrar las primeras
variables articulares q1 , q 2 y q 3 las cuales determinan prácticamente la posición del robot,
basándose en la posición Pm Pmx , Pmy , Pmz de un punto en especial del robot denominado
muñeca (denotada por el subíndice m ). En la segunda parte se calculan las variables
articulares finales q 4 , q 5 y q 6 las cuales determinan en gran medida la orientación del efector
final. Dichos cálculos se realizan con base en la orientación nh , oh , ah deseada para la
herramienta, y las articulaciones q1 , q 2 y q 3 obtenidas en la primera parte. El punto que
representa la muñeca en la mayoría de robots es sencillo de ubicar.
2
Generalmente para brazos robóticos de 6GDL, éste se encuentra en el lugar donde se cortan
los ejes de los tres últimos grados de libertad q 4 , q 5 , q 6 . En la figura 2 se muestra el punto
Pm del brazo robótico de la figura 1:
a) b)
Figura 2
En este caso el punto Pm donde se cortan las tres últimas articulaciones, está justo sobre el
origen O 5 del sistema S 5 y cuyo vector asociado al sistema S 0 de la base del robot es:
Pm O 0O 5 Pmx Pmy Pmz
T
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
3
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
Pmx Phx a hx Phx L 4a hx
Pm Pmy Phy L 4 a hy Phy L 4a hy
P P a P L a
mz hz hz hz 4 hz
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 L1 0 π/ 2
2 q2 0 L2 0
3 q 3 π/ 2 0 L3 0
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. 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 simbólica TmSimb
que representa el extremo del robot de la figura 3, en este caso la muñeca se obtiene:
4
C1 0 S1 0 C 2 S2 0 L 2 C 2
S 0 C1 0 1 S C 2 0 L 2S2
0
A1 1 A2 2
0 1 0 L1 0 0 1 0
0 0 0 1 0 0 0 1
C(q3 π 2 ) S(q3 π 2 ) 0 L3C(q 3 π 2 ) S3 C3 0 L3S3
S(q π 2 ) C(q π 2 ) 0 L3S(q3 π 2 ) C S3 0 L3C3
2
A 3 aux 3 3
Simplificando: 2 A 3 aux 3
0 0 1 0 0 0 1 0
0 0 0 1 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
L 2C1C 2 L3C1C 2S3 L3C1S2C3 Pm x
L S C L S C S L S S C P
2 1 2 3 1 2 3 3 1 2 3 my
L1 L 2S2 L3S2S3 L3C 2C3 Pm z
Haciendo a L 3C 3 y b L 2 L 3S3 :
C1 aS2 bC 2 Pm x (1.1)
S1 aS2 bC 2 Pm y (1.2)
bS2 aC 2 Pm z L1 (1.3)
5
• Obtención de q1
Pm x Pm y
C1 S1
aS2 bC 2 aS2 bC 2
Sabiendo que:
Pmx Phx L 4 a hx Pmy Phy L 4 a hy
Phx L 4 a hx Phy L 4 a hy
C1 S1
aS2 bC 2 aS2 bC 2
Phy L 4 a hy Phx L 4 a hx
Usando atan2: q1 atan 2S1 , C1 atan 2 ,
aS2 bC 2 aS2 bC 2
• Obtención de q 3
Elevando al cuadrado las ecuaciones 1.1, 1.2 y 1.3 se obtienen las ecuaciones 1.5, 1.6 y 1.7
respectivamente:
C1 a 2S2 2 abS2 C 2 b 2 C 2 Pm x
2 2 2
2
(1.5)
S1 a 2S 2 2 abS 2 C 2 b 2 C 2
2 2 2
Pmy
2
(1.6)
S 1
2 2
C1 a 2S2 2 abS2 C 2 b 2 C 2 Pm x Pm y
2 2
2 2
a 2S2 2 abS2 C 2 b 2 C 2 Pm x Pm y
2 2 2 2
6
Sumando el resultado anterior con la ecuación 1.7:
2
a 2 S2 C 2 b 2 S2 C 2 Pm x Pm y Pm z L1
2 2 2
2 2 2
a 2 b 2 Pm x Pm y Pm z L1
2 2 2
Reemplazando a L 3C 3 y b L 2 L 3S 3 :
Despejando S 3 :
Pm x Pm y Pm z L1 L 2 L 3
2 2 2 2 2
S3
2 L 2 L3
atan 2S , 1 S
2
q 3 II 3 3
• Obtención de q 2
Pm x
aS2 bC2 Pm z L1 bS2 aC 2 (1.9)
C1
Aplicando la regla de Cramer al sistema de ecuaciones 1.9 se obtienen dos expresiones, una
para S 2 y otra para C 2 :
7
Pm x C1 a b S2
P L
m z 1 b a C2
L 2 L3S3 Phz L 4a hz L1 L3C3 (Phx L4a hx ) C1 L3C3 Phz L4a hz L1 L 2 L3S3 Phx L4a hx C1
q 2 atan 2 ,
L C
3 3 2
L 2 L S
3 3 2
L C
3 3 2
L 2 L S
3 3 2
La solución anterior se puede simplificar aún más. Observe que en la función atan2 la
expresión L3C3 L 2 L3S3 es el denominador tanto de la parte real como imaginaria,
2 2
como en el caso de q1 . También se aprecia que su resultado siempre será positivo, lo cual
además advierte que no influye sobre el cuadrante de q 2 . Por lo tanto se puede prescindir
(si se está seguro de lo que se está haciendo) de dicha expresión:
q 2 atan 2 L2 L3S3 L1 L4a hz Phz
L3C3 (Phx L4a hx ) L L3S3 Phx L4a hx
, L3C3 Phz L 4a hz L1 2 (1.10)
C1 C1
q 2 tiene solución única. Es función f q1 , q 3
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 :
8
QhSimb 0 Q 6 0 Q11 Q 2 2 Q3 3 Q 4 4 Q5 5 Q 6
Donde:
C1 0 S1 C 2 S 2 0 C3 0 S3
0
Q1 S1 0 C1 1
Q 2 S2 C 2 0 2
Q 3 S3 0 C3
0 1 0 0 0 1 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
3
Q6 Q
0
3
1
QhObj (1.12)
• 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
S5 t13 t 23
2 2
q 5 I atan 2 t132 t 23 2 , t 33 (1.16)
q 5 II atan 2 t132 t 232 , t
33
10
• 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 Q3 QhObj
1
t 23 t13 (1.17)
q 4 atan 2 ,
S 5 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.18)
q 6 atan 2 , 31
S5 S5
Solución única para q 6 . Es función f q1 , q 2 , q 3 , q 5
Una vez resuelto el problema cinemático inverso, es necesario establecer un orden para
calcular cada una de las variables articulares. Esto se debe a que pueden existir variables
que son independientes, y otras que son funciones de por lo menos una de las demás. A
continuación se muestra una tabla con las soluciones obtenidas anteriormente para cada una
de las variables. De allí se podrá establecer la relación que existe entre éstas:
11
qi SOLUCIONES PARA LAS VARIABLES ARTICULARES f q ,.., q
i n
q 3 I atan 2S 3 , 1 S3
2
Phx L4a hx 2 Phy L4a hy 2 Phz L4a hz L1 2 L2 2 L32
q3 S3
q3 II atan 2S 3 , 1 S3
2
2 L 2 L3
q6 q6 atan 2 t 32
,
t 31
t 0 Q3 QhObj
1
q1 , q 2 , q 3 , q 5
S5 S5
Tabla 3
12
La tabla 3 muestra que las articulaciones q 1 y q 3 son completamente independientes,
además de hacer parte del conjunto de entradas para las demás variables. Por lo tanto las
demás variables se deben evaluar para cada una de las cuatro posibles combinaciones de
q 1 y q 3 . Con base a esto se llegó a un total de 8 posibles soluciones que pueden satisfacer
la configuración deseada para el robot. En la figura 4 se puede ver el árbol de las 8 posibles
combinaciones:
q4 q 6 COMBINACIÓN 1
I
q q
1I 3I q 2 q 5
II
q4 q 6 COMBINACIÓN 2
q4 q 6 COMBINACIÓN 3
I
q q
1I 3 II q 2 q 5
II
q4 q 6 COMBINACIÓN 4
q4 q 6 COMBINACIÓN 5
I
q q
1 II 3I q 2 q 5
II
q4 q 6 COMBINACIÓN 6
q4 q 6 COMBINACIÓN 7
I
q q
1 II 3 II q 2 q 5
II q4 q 6 COMBINACIÓN 8
Figura 4
COMPROBACIÓN
Para el robot del ejemplo comprobar si todas las posibles combinaciones de las variables
articulares q i satisfacen la matriz objetivo ThObj :
Resultados:
13
======================= COMBINACIÓN 1 =======================
q1 q2 q3 q4 q5 q6
0.7877 0.3907 -1.0461 1.0465 1.5755 -0.3913
ThCalculada=
0.7189 -0.6110 -0.3315 7.6550
0.4462 0.0400 0.8940 19.9603
-0.5330 -0.7907 0.3014 47.4436
0 0 0 1.0000
ThObj=
0.7200 -0.6100 -0.3300 7.6700
0.4500 0.0400 0.8900 19.9200
-0.5300 -0.7900 0.3000 47.4300
0 0 0 1.0000
q1 q2 q3 q4 q5 q6
0.7877 0.3907 -1.0461 -2.0951 -1.5755 2.7503
ThCalculada=
0.7189 -0.6110 -0.3315 7.6550
0.4462 0.0400 0.8940 19.9603
-0.5330 -0.7907 0.3014 47.4436
0 0 0 1.0000
ThObj=
0.7200 -0.6100 -0.3300 7.6700
0.4500 0.0400 0.8900 19.9200
-0.5300 -0.7900 0.3000 47.4300
0 0 0 1.0000
q1 q2 q3 q4 q5 q6
0.7877 -0.9458 -2.0955 1.9723 1.9176 2.0644
ThCalculada=
0.7206 -0.6090 -0.3315 7.6550
0.4461 0.0412 0.8940 19.9603
-0.5308 -0.7921 0.3014 47.4436
0 0 0 1.0000
ThObj=
0.7200 -0.6100 -0.3300 7.6700
0.4500 0.0400 0.8900 19.9200
-0.5300 -0.7900 0.3000 47.4300
0 0 0 1.0000
======================= COMBINACIÓN 4 =======================
q1 q2 q3 q4 q5 q6
0.7877 -0.9458 -2.0955 -1.1693 -1.9176 -1.0772
ThCalculada=
0.7206 -0.6090 -0.3315 7.6550
0.4461 0.0412 0.8940 19.9603
-0.5308 -0.7921 0.3014 47.4436
0 0 0 1.0000
ThObj=
0.7200 -0.6100 -0.3300 7.6700
0.4500 0.0400 0.8900 19.9200
-0.5300 -0.7900 0.3000 47.4300
0 0 0 1.0000
14
======================= COMBINACIÓN 5 =======================
q1 q2 q3 q4 q5 q6
-2.3539 -2.1958 -1.0461 -1.1693 1.9176 2.0644
ThCalculada=
0.7206 -0.6090 -0.3315 7.6550
0.4461 0.0412 0.8940 19.9603
-0.5308 -0.7921 0.3014 47.4436
0 0 0 1.0000
ThObj=
0.7200 -0.6100 -0.3300 7.6700
0.4500 0.0400 0.8900 19.9200
-0.5300 -0.7900 0.3000 47.4300
0 0 0 1.0000
q1 q2 q3 q4 q5 q6
-2.3539 -2.1958 -1.0461 1.9723 -1.9176 -1.0772
ThCalculada=
0.7206 -0.6090 -0.3315 7.6550
0.4461 0.0412 0.8940 19.9603
-0.5308 -0.7921 0.3014 47.4436
0 0 0 1.0000
ThObj=
0.7200 -0.6100 -0.3300 7.6700
0.4500 0.0400 0.8900 19.9200
-0.5300 -0.7900 0.3000 47.4300
0 0 0 1.0000
q1 q2 q3 q4 q5 q6
-2.3539 2.7509 -2.0955 -2.0951 1.5755 -0.3913
ThCalculada=
0.7189 -0.6110 -0.3315 7.6550
0.4462 0.0400 0.8940 19.9603
-0.5330 -0.7907 0.3014 47.4436
0 0 0 1.0000
ThObj=
0.7200 -0.6100 -0.3300 7.6700
0.4500 0.0400 0.8900 19.9200
-0.5300 -0.7900 0.3000 47.4300
0 0 0 1.0000
======================= COMBINACIÓN 8 =======================
q1 q2 q3 q4 q5 q6
-2.3539 2.7509 -2.0955 1.0465 -1.5755 2.7503
ThCalculada=
0.7189 -0.6110 -0.3315 7.6550
0.4462 0.0400 0.8940 19.9603
-0.5330 -0.7907 0.3014 47.4436
0 0 0 1.0000
ThObj=
0.7200 -0.6100 -0.3300 7.6700
0.4500 0.0400 0.8900 19.9200
-0.5300 -0.7900 0.3000 47.4300
0 0 0 1.0000
15
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
16