SEP SES TecNM

INSTITUTO TECNOLÓGICO DE TOLUCA

INGENIERÍA MECATRÓNICA

ROBÓTICA

EXÁMEN

PRESENTA:
GARCÍA ESCALANTE BORIS
21280908

DOCENTE
ELISEO RIVERA SILVA

METEPEC, MÉXICO, A DICIEMBRE DEL 2025

Parámetros DH
Zi θi Xi αi

ARTICULACION 0.09 0 0 π
1 2
ARTICULACION 0 −π -0.425 0
2 2
ARTICULACION 0 −π -0.39225 0
3 2
ARTICULACION 0.10997 0 0 π
4 2
ARTICULACION 0.09492 0 0 −π
5 2
ARTICULACION 0.08167 0 0 0
6

La matriz de transformación del efector final de forma global requerida para seguir
con los cálculos de cinemática inversa:

( )
0.90628839 −0.11722662 0.40607783 −0.34639949
0 0.41868755 0.11758955 −0.90048510 −0.60862750
6T =
0.05781039 0.98611891 0.15565135 0.51937300
0 0 0 1

Tenemos en cuenta que:

( ) ()
R 11 R 12 R 13 Px
0
6 R = R21 R 11 R 23 P x. y . z= P y
R31 R 32 R 33 Pz

( )
R 11 R 12 R13 P x
0 0 R 21 R11 R23 P y
6T =6 R∗Px . y. z =¿
R 31 R 32 R33 Pz
0 0 0 1

Cálculo de θ1
Conocemos de los parámetros DH:
z 2=z 3=x 1=x 4 =x5 =x6 =0
 π −π
α 1=α 4 = ,α 5= , α 2=α 3=α 6=0
2 2

Como conocemos la posición y rotación final, por lógica la 16T depende de todos
los θi :
0 0 1 2 3 4 5
6 T =1T ∗2T ∗3T ∗4 T ∗5T ∗6T

Para obtener una ecuación que podamos solucionar que dependa de θ1

0 0 1 2 3 4 5
6 T =1T ∗2T ∗3T ∗4 T ∗5T ∗6T

Donde:
0 0 1
6 T =1T ∗6T

( )
cos (θ 1) sin(θ 1) 0 0
0 −1 0 0 1 −z 1
1T =
sin(θ 1) −cos (θ 1) R33 0
0 0 0 1

( )( )
cos (θ 1) sin(θ 1) 0 0x R11 R12 R13 P x
1 0 −1 0 0 0 1 −z 1 R21 R 11 R 23 P y
6T = 1T ∗6T =
sin(θ1) −cos(θ 1) R33 0 R31 R32 R 33 P z
0 0 0 1 0 0 0 1

Esta matriz la llamaremos 16T izq , donde los valores de interés son:

1. 16T izq [2 , 2] = −𝑅23cos(𝜃1) + 𝑅13sin(𝜃1)

2. 16T izq [2 , 3] = −yO6cos(𝜃1) + xO6sin(𝜃1)

Para generar una igual generamos la parte derecha:

1 1 2 3 4 5
6 Tder = 2T ∗3T ∗4T ∗5T ∗6T

Donde:

3. 16T der [2 ,2]= cos(𝜃5)

4. 16T der [ 2 , 3 ] = 𝑧6cos(𝜃5) + 𝑧4

Nos quedaría un sistema de ecuaciones de la siguiente forma:

 { ]} { }
1 1
T izq[2 ,2] =6T der [2, 2] −R 23 cos ( θ 1 ) + R 13 sin ( θ 1 )=cos (θ 5)
6
=
6
1
T izq [ 2 , 3 ] = T der [ 2 , 3
1
6 − yO 6 cos (θ 1 ) + xO 6 sin ( θ 1 )=z 6 cos (θ 5)+ z 4

Solucionamos para θ 1, sustituimos la ecuación 1 en la 2:

− yO 6 cos ( θ 1 ) + xO 6 sin ( θ 1 )=z 6[−R 23 cos ( θ 1 ) + R 13 sin (θ 1 )]+ z 4

Factorizamos para cos (θ 1 ) y sin ( θ1 ):

− yO 6 cos ( θ 1 ) + xO 6 sin ( θ 1 ) + z 6 R 23 cos ( θ 1 ) −z 6 R 13 sin ( θ 1 )=z 4

[ z 6 R 23− yO 6] cos (θ 1 ) + [ xO 6−z 6 R 13] sin ( θ 1 )=z 4

Obtenemos una ecuación del tipo A cos (θ )+ ¿ B sin ( θ ) = C, donde:

A = z 6 R 23− yO 6
B = xO 6−z 6 R 13
C = z4
Calculamos la amplitud:

R=√ A 2+ B ≥ 0
Se define el desfase φ mediante atan 2 para asegurar el cuadrante correcto:
φ=atan 2 ( A , B )
A cos θ+ B sin θ=R cos ( θ−φ )

Entonces queda como:

R cos ( θ−φ )=C
Condiciones de existencia:
 Si |C|> R no hay soluciones reales
 Si R=0 (equivale a A=B=R=0)
o Si C=0 : cualquier θ es solución (infinitas soluciones).
o Si C ≠ 0: sin solución.
Si |C|> R entonces:
C
cos (θ−φ )=
R
Por lo tanto, las soluciones generales son:
 θ−φ=±cos−1 ( Rc )+2 kπ ,k ∈ Z
Es decir, que en un periodo de 2 π hay dos soluciones independientes

θ1=φ+cos−1 ( CR ) , θ =φ−cos ( CR )
2
−1

Entonces si sustituimos los valores

A = z 6 R 23− yO 6 = (0.08167) ( −0.90048510 ) - ¿) = 0.535084881883

B = xO 6−z 6 R 13 = −0.34639949 – (0.08167) (0.40607783 ) = -0.37956386637609996

C = z 4 = 0.10997

R=√ (0.535084881883)2+(−0.37956386637609996)2 =0.6560370107532942

φ=atan 2 ( 0.535084881883 , 0.37956386637609996 )=−0.6169753543353088 rad ≈−35.35008∘
C 0.10997
cos (θ−φ )= = ≈ 0.16767
R 0.6560370107532942
Entonces:

θ(1)
1 =φ+ cos
−1
( CR )=¿−0.6169753543+1.4023734631 = 0.7853981088 rad≈

44.999997°

θ(2)
1 =φ−cos
−1
( CR )=0.61697−1.4023734=−2.0193488175 rad ≈−115.700165 °
Cálculo de θ5
Para calcular este ángulo solo ocupamos las ecuaciones antes dadas, se
despejan para θ5 y conociendo ya θ1 se resuelve:
−R 23 cos ( θ 1 ) + R 13 sin ( θ1 )=cos (θ5 )

θ5 =±cos−1 (−R 23 cos ( θ(11 )) + R 13 sin ( θ(11 ) ) ¿)+ 2 kπ ¿

θ5=± cos−1 (−R 23 cos ( θ(1)

1 ) + R 13 sin ( θ 1 ) ¿)¿ =
(1)

−1
± cos (0.90048510 cos ( 44.999997 ) +0.40607783 sin ( 44.999997 ) ¿)¿ = ±
22.500000834004243°
Cálculo de θ6
Para calcular θ6 necesitamos encontrar una ecuación que dependa de los ángulos

esto de donde conseguimos nuestras ecuaciones de 𝜃1 obtenemos:

que ya conocemos hasta el momento y de θ6 para poder solucionar para θ6 . Para

1
T izq [2 , 0]= −𝑅21cos(𝜃1) +𝑅11sin(𝜃1)
6
1
6T der [2 ,0 ] = cos(𝜃6) sin(𝜃5)

{6T izq[2, 0]=6T der [2, 0] }= {−R 21cos (θ 1)+ R 11sin (θ 1)=cos(θ 6)sin( θ5)}
1 1

−R 21 cos ( θ 1 ) + R 11sin ( θ1 )=cos ( θ 6 ) sin ( θ 5 )

−R 21 cos ( θ 1 ) + R 11sin ( θ1 )
θ 6=± cos−1 ( )
sin ( θ 5 )

=±25.714306979
θ6 =± cos
−1
( 0.41868755 cos ( 44.999997 ) +0.90628839 sin ( 44.999997 )
sin ( 22.500000834004243 ) )
46782°

Cálculo de θ2
Para hacer el cálculo necesitamos ecuaciones que dependan de θ2 y de los
ángulos que conocemos, para esto usaremos:
2 2 2
4Tder =3T ∗3T

( )
cos(θ 3)cos (θ 4)−sin(θ 3)sin(θ 4 ) 0 cos(θ 4)sin(θ 3)+cos (θ 3)sin(θ 4) x 3 cos(θ 3)
Tder = cos (θ 4)sin(θ 3)+cos (θ 3)sin (θ 4) 0 −cos (θ 3)cos (θ 4)+sin (θ 3)sin (θ 4) x 3 sin(θ 3)
2
4
0 1 0 z4
0 0 0 1
2 0
Se puede despejar de la ecuación principal a 4T ya que conocemos 6T
0 0 1 2 3 4 5
6 T =1T ∗2T ∗3T ∗4 T ∗5T ∗6T

Al conocer θ1 , θ5 y θ6 tenemos:
2 0 −1 1 −1 4 −1 5 −1 0
4Tizq =1T ∗2T ∗5T ∗6T ∗6T

(la 42Tizq es una matriz de gran tamaño, asi que solo colocaremos los valores que
2
nos interesan y como tenemos que igualar a la 4Tder , observaremos los valores en
la matriz de esta para basarnos en cual necesitaremos para los valores de 42Tizq )

Para generar nuestra ecuación tenemos que:

{ 24T izq [ 0 , 3 ] =24T der [ 0 ,3 ] }

xO6cos(θ1)cos(θ2 ) + yO6cos(θ2 )sin(θ1) + (((R12cos(θ1) + R22sin(θ1))cos(θ2
) + R32sin(θ2 ))cos(θ6 ) + ((R11cos(θ1) + R21sin(θ1))cos(θ2 ) + R31sin(θ2
)sin(θ6 ))z5 - ((R13cos(θ1) + R23sin(θ1))cos(θ2 ) + R33sin(θ2 ))z6 - z1sin(θ2)
+ zO6sin(θ2 ) - x2 = x3cos(θ3)

{ 24T izq [ 1 ,3 ]= 24T der [ 1 , 3 ] }

-xO6cos(θ1)sin(θ2 ) - yO6sin(θ1)sin(θ2) + ((R32cos(θ2 ) - (R12cos(θ1) +
R22sin(θ1))sin(θ2))cos(θ6 ) + (R31cos(θ2) - (R11cos(θ1) + R21sin(θ1))sin(θ2
))sin(θ6 ))z5 - (R33cos(θ2) - (R13cos(θ1) + R23sin(θ1))sin(θ2))z6 - z1cos(θ2 )
+ zO6cos(θ2) = x3sin(θ3 )
Hay que factorizar los coeficientes de cada cos(θ2) y sin(θ2 ), obteniendo un
sistema de ecuaciones de la forma:

{a 11cos (θ2 ) + a 12sin ( θ 2 )−x 2 ¿ x 3 cos (θ3 )

a 21 cos(θ 2) + a 22 sin ( θ2 ) ¿ x 3 sin(θ3 ) }
a11 = ((R12cos(θ1) + R22*sin(θ1))cos(θ6 ) + (R11cos(θ1) + R21sin(θ1))sin(θ6 ))z5 -
(R13cos(θ1) + R23sin(θ1))z6 + xO6cos(θ1) + yO6sin(θ1) = -0.602025461123883

a12 = (R32cos(θ6 ) + R31sin(θ6 ))z5 - R33z6 - z1 + zO6 = 0.517123529420017

a21 = (R32cos(θ6 ) + R31sin(tθ6 6))z5 - R33z6 - z1 + zO6 = 0.517123529420017

a22 = -((R12cos(θ1) + R22sin(θ1))cos(θ6 ) + (R11cos(θ1) + R21sin(θ1))sin(θ6 ))z5 +

(R13cos(θ1) + R23sin(θ1))z6 - xO6cos(θ1) - yO6sin(θ1) = 0.602025461123883

para resolver el sistema de ecuaciones hay que eliminar x 3 cos (θ3 ) y x 3 sin (θ3 )
elevándolos al cuadrado
2 2
(x 3cos ( θ3 ) ) = (a 11cos ( θ2 ) +a 12 sin ( θ2 ) −x 2)
2 2
(x 3sin ( θ 3 ) ) =(a 21 cos (θ2 )+ a 22 sin ( θ2 ))
2 2
(1) (a 11cos ( θ2 ) +a 12 sin ( θ2 ) −x 2) + (a 21 cos (θ2 )+ a 22sin ( θ 2 )) = ( x 3)2

Entonces definimos:
C=cos θ 2 , S=sinθ 2

( a211 + a221) C 2 + (a 212+ a222) S 2+2 ( a11 a22 ) CS+ (−2 x 2 a 11 ) C+ (−2 x 2 a12) S+ ( x 22+7 x 23 )=0
2 2
α =a11 +a 21
 2 2
β=a12+ a22

γ =2(a11 a 12+a 21 a22 )

δ=−2 x 2 a 11

ε =−2 x 2 a11
2 2
ζ =x 2−x 3

La ecuación (1) se escribe de la siguiente manera:

2
(2) α cos θ2 + β sin θ2 + γ cos θ2 sin θ2 +δ cos θ2 +ε sin θ2 +ζ =0
2
θ2 1−t 2t
t=tan , cos θ 2= 2,
sin θ2= 2
2 1+ t 1+t
2
Se sustituye en (2) y se multiplica por ( 1+t 2 ) :

c A =α −S+ζ

c 3=−2 γ +2 ε

c 2=−2 ∝+4 β +2 ζ

c 1=2 γ +2 ε

c 0=α + δ+ζ
4 3 2
(3) c 4 t +c 3 t +c 2 t +c 1 t+c 0 =0

Finalmente:

Rt 2=√ a211 + a212+ a221+ a222

−1
∅ t 2=tan 2 ( a12 +a22 , a11 +a 21)

Tomando los valores aproximadamente de:

θ1 = -115.700164605187 °
θ6 = -13.8542079154836 °
θ2 =102.45508790058184°
2

) + π = -2.0415390896+ π = 63.961112007966°
−1 X3
θ2=tan 2 ( a12 +a22 , a11 + a21) ± cos (
−1
Rt 2

Cálculo de θ3
De las ecuaciones en donde obtuvimos θ2 despejaremos para θ3:
 {a 11cos (θ2 ) + a 12sin ( θ 2 )−x 2 ¿ x 3 cos (θ3 )
a 21 cos(θ 2) + a 22 sin ( θ2 ) ¿ x 3 sin(θ3 ) }
−1
a 11cos ( θ 2 ) +a 12 sin ( θ2 )−x 2
θ3 =±cos ( )
x3

−1 −0.602 cos (−2.0415390896 )+ 0.602 sin (−2.0415390896 )−(−0.425)

θ3 =±cos ( )=
−0.39225
31.94793310280934 °

Cálculo de θ 4
3
Para tener una ecuación para θ 4 ocupamos la transformada principal de 4T que
llamaremos 34T der ya que esta depende de θ 4, obtenemos una ecuación igualando
a 34T izq, que sabemos que:
0 0 1 2 3 4 5
6 T =1T ∗2T ∗3T ∗4 T ∗5T ∗6T
3 0 −1 1 −1 2 −1 4 −1 5 −1 0
4 Tizq =1T ∗2T ∗3T ∗5T ∗6T ∗6T

Entonces:

Tizq [ 1 ,2 ] =−((R32 cos ⁡(θ 2)−(R12 cos ⁡(θ 1)+ R22 sin ⁡(θ 1))sin ⁡(θ2 ))cos ⁡(θ3 )−(( R12 cos ⁡( θ 1)+ R22 sin ⁡(θ 1))cos ⁡(θ 2)+ R
3
4

3
Tder [1 ,2]=4¿ −cos ( θ4 ) ¿

Entonces:

( )
− ( R 32 cos ( θ 2 )−( R 12 cos ( θ 1 )+ R 22 sin ( θ1 ) ) sin ( θ2 ) ) cos ( θ3 )−( ( R 12 cos ( θ 1) + R 22 sin ( θ1 ) ) cos ( θ2 ) + R32 sin ( θ2 ) ) sin ( θ 3 ) c

θ 4=cos (−(−(( R32 cos ( 102.45508790058184 ) −( R12 cos ( 44.99999687118389 )+ R22 sin ( 44.99999687118389 ) )
−1

θ 4=−¿ 120.63656325884975°

=== RESULTADOS FINALES ===

θ1 = 44.99999687118389 °
θ2 = 63.96111200796598 °
θ3 = -31.947933102809248 °
θ4 = 120.63656325884975 °
θ5 = 22.500000834004243 °
θ6 = 25.71430697946782 °
COMPROBACION SAGEMATH
Comprobación coppelia
CODIGOS PARA DESCARGAR: CodigoSAGEMATH