UNIVERSIDAD NACIONAL DE TRUJILLO

Facultad de Ingeniería
Programa de Ingeniería Mecatrónica
INFORME N°03

“EJERCICIOS DE CINEMATICA INVERSA”

_____________________________________________________

DESARROLLO DE EJERCICIOS

ROBOTICA
ESTUDIANTE(S) :

1. NARVAEZ GRIJALBA, MARTIN ALONSO

DOCENTE :

Dr. Ing. RICARDO J. PALOMARES ORIHUELA

CICLO :
2021 - I

Trujillo, Perú
2021
EJERCICIO 1

La primera ecuación que debemos tomar en cuenta es

𝜙𝑒 = 𝜃1 + 𝜃2 + 𝜃3

Podemos tomar una recta a 𝑂𝐵 teniendo como coordenadas 𝑃𝑏𝑥, 𝑃𝑏𝑦

𝑃𝑏𝑥 = 𝑥𝑒 − 𝑙3 ∗ 𝑐𝑜𝑠𝜙𝑒
𝑃𝑏𝑦 = 𝑦𝑒 − 𝑙3 ∗ 𝑠𝑒𝑛𝜙𝑒

Después para hallar 𝜃1 y𝜃2, nos apoyamos de la ley de cosenos

𝑃𝑏𝑥 2 + 𝑃𝑏𝑦 2 = 𝑙12 + 𝑙22 + 2 ∗ 𝑙1 ∗ 𝑙2 ∗ 𝑐𝑜𝑠𝜃2

𝑃𝑏𝑥 2 + 𝑃𝑏𝑦 2 − 𝑙12 − 𝑙22

𝑞2 = cos −1 ( )
2 ∗ 𝑙1 ∗ 𝑙2
 𝜃1 = 𝛽 − 𝛼

Donde
𝑃𝑏𝑦
𝛽 = 𝑎𝑟𝑐𝑡𝑔 ( )
±𝑃𝑏𝑥
𝑙2 ∗ 𝑠𝑒𝑛𝜃2
𝛼 = 𝑎𝑟𝑐𝑡𝑔 ( )
𝑙1 + 𝑙2 ∗ 𝑐𝑜𝑠𝜃2

Por lo tanto
𝑃𝑏𝑦 𝑙2 ∗ 𝑠𝑒𝑛𝜃2
𝜃1 = 𝑎𝑟𝑐𝑡𝑔 ( ) − 𝑎𝑟𝑐𝑡𝑔 ( )
±𝑃𝑏𝑥 𝑙1 + 𝑙2 ∗ 𝑐𝑜𝑠𝜃2

Por último
𝜃3 = 𝜙𝑒 − 𝜃1 − 𝜃2

EJERCICIO 2
Donde

Debemos sacar los parámetros de

𝜃1 , 𝑑2 , 𝑑3 ,

𝑇 = 𝐴10 ∗ 𝐴12 ∗ 𝐴23 ∗ 𝐴34

−𝑐𝑜𝑠𝜃4 𝑠𝑒𝑛𝜃1 𝑠𝑒𝑛𝜃1 𝑠𝑒𝑛𝜃4 𝑐𝑜𝑠𝜃1 𝑑3 𝑐𝑜𝑠𝜃1 + 𝑙4 𝑜𝑠𝜃1
𝑐𝑜𝑠𝜃1 𝑐𝑜𝑠𝜃4 −𝑐𝑜𝑠𝜃1 𝑠𝑒𝑛𝜃4 𝑠𝑒𝑛𝜃1 𝑑3 𝑠𝑒𝑛𝜃1 + 𝑙4 𝑠𝑒𝑛𝜃1
𝑇=[ ] … (𝐼)
𝑠𝑒𝑛𝜃4 𝑐𝑜𝑠𝜃4 0 𝑑2 + 𝑙1
0 0 0 1

(𝐴10 )−1 ∗ 𝑇 = 𝐴12 ∗ 𝐴23 ∗ 𝐴34

𝑐𝑜𝑠𝜃1 𝑠𝑒𝑛𝜃1 0 0 0 0 1 𝑑3 + 𝑙4
−𝑠𝑒𝑛𝜃1 𝑐𝑜𝑠𝜃1 0 0 𝑐𝑜𝑠𝜃4 −𝑠𝑒𝑛𝜃4 0 0
[ ]𝑇 = [ ] … (𝐼𝐼)
0 0 1 −𝑙1 𝑠𝑒𝑛𝜃4 𝑐𝑜𝑠𝜃4 0 𝑑2
0 0 0 1 0 0 0 1

(𝐴12 )−1 (𝐴10 )−1 ∗ 𝑇 = 𝐴23 ∗ 𝐴34

−𝑠𝑒𝑛𝜃1 𝑐𝑜𝑠𝜃1 0 0 𝑐𝑜𝑠𝜃4 −𝑠𝑒𝑛𝜃4 0 0
0 0 1 −𝑑2 − 𝑙1 𝑠𝑒𝑛𝜃4 𝑐𝑜𝑠𝜃4 0 0
[ ]𝑇 = [ ] … (𝐼𝐼𝐼)
𝑐𝑜𝑠𝜃1 𝑠𝑒𝑛𝜃1 0 0 0 0 1 𝑑3 + 𝑙4
0 0 0 1 0 0 0 1

(𝐴23 )−1 (𝐴12 )−1 (𝐴10 )−1 ∗ 𝑇 = 𝐴34

 −𝑠𝑒𝑛𝜃1 𝑐𝑜𝑠𝜃1 0 0 𝑐𝑜𝑠𝜃4 −𝑠𝑒𝑛𝜃4 0 0
0 0 1 −𝑑2 − 𝑙1 𝑠𝑒𝑛𝜃4 𝑐𝑜𝑠𝜃4 0 0
[ ]𝑇 = [ ] … (𝐼𝑉)
𝑐𝑜𝑠𝜃1 𝑠𝑒𝑛𝜃1 0 −𝑑3 0 0 1 𝑙4
0 0 0 1 0 0 0 1

De (𝐼𝐼) hallamos 𝑑2 :
𝑑2 − 𝑙1 = 𝑃𝑧
𝑑2 = 𝑃𝑧 − 𝑙1

De (𝐼𝐼) y (𝐼𝐼𝐼) vemos que

𝑃𝑥 ∗ 𝑐𝑜𝑠𝜃1 + 𝑃𝑦 ∗ 𝑠𝑒𝑛𝜃1 − 𝑙4 = 𝑑3

De (𝐼𝐼) tenemos que:

−𝑃𝑥 ∗ 𝑠𝑒𝑛𝜃1 + 𝑃𝑦 ∗ 𝑐𝑜𝑠𝜃1 = 0
𝑃𝑥 ∗ 𝑠𝑒𝑛𝜃1 = 𝑃𝑦 ∗ 𝑐𝑜𝑠𝜃1
𝑃𝑦
𝜃1 = 𝑎𝑟𝑐𝑡𝑔 ( )
𝑃𝑥
Conociendo 𝜃1 entonces:
𝑑3 = 𝑃𝑥 ∗ 𝑐𝑜𝑠𝜃1 + 𝑃𝑦 ∗ 𝑠𝑒𝑛𝜃1 − 𝑙4

De (𝐼𝐼𝐼) vemos que

−𝑠𝑒𝑛𝜃1 𝑛𝑥 + 𝑐𝑜𝑠𝜃1 𝑛𝑦 = 𝑐𝑜𝑠𝜃4

−𝑠𝑒𝑛𝜃1 𝑜𝑥 + 𝑐𝑜𝑠𝜃1 𝑜𝑦 = −𝑠𝑒𝑛𝜃4

𝑠𝑒𝑛𝜃1 𝑜𝑥 − 𝑐𝑜𝑠𝜃1 𝑜𝑦
𝜃4 = 𝑎𝑟𝑐𝑡𝑔 ( )
−𝑠𝑒𝑛𝜃1 𝑛𝑥 + 𝑐𝑜𝑠𝜃1 𝑛𝑦

En resumen, tenemos
𝑃𝑦
𝜃1 = 𝑎𝑟𝑐𝑡𝑔 ( )
𝑃𝑥
𝑑2 = 𝑃𝑧 − 𝑙1
𝑑3 = 𝑃𝑥 ∗ 𝑐𝑜𝑠𝜃1 + 𝑃𝑦 ∗ 𝑠𝑒𝑛𝜃1 − 𝑙4
𝑠𝑒𝑛𝜃1 𝑜𝑥 − 𝑐𝑜𝑠𝜃1 𝑜𝑦
𝜃4 = 𝑎𝑟𝑐𝑡𝑔 ( )
−𝑠𝑒𝑛𝜃1 𝑛𝑥 + 𝑐𝑜𝑠𝜃1 𝑛𝑦

EJERCICIO 3
Para este ejercicio se ha desarrollado el laboratorio el cual tiene el siguiente código
Para la prueba se desarrollo
Obteniéndose como resultado de la simulación

Donde como vemos se ingresa unos parámetros aleatorios como vector 𝑞, se desarrollo
la matriz de transformación 𝑇, teniendo con la función de Cinemática Inversa una
respuesta de los 4 parámetros donde se aprecia que es el mismo vector que el de entrada
verificando que la Cinemática esta correcta.