Articulación 𝜽𝒊 d a 𝜶

1 𝜃1 L1 0 90°
2 𝜃2 0 L2 0
3 𝜃3 0 L3 0

Y1 Q1 Y2
Y3
Q2 Q3

X1 X2 X3

90° L2 l3

Z1 l1
Z2 Z3

Q0
Z0

Y0

X0

Matrices homogéneas del robot

𝑐1 0 𝑠1 0
𝑠 0 −𝐶1 0
𝐴1 = [ 1 ] 2.2
0 1 0 𝑑1
0 0 0 1
𝑐2 𝑐1 𝑠2 𝑐1 𝑠1 𝑎2 𝑐2 𝑠1
𝑐2 𝑠1 𝑠2 𝑠1 𝐶1 𝑎2 𝑐2 𝑐1
𝐴2 = [ ] 2.3
𝑠2 𝑐2 0 𝑎2 𝑐2 + 𝑑1
0 0 0 1

𝑐3 𝑐1 −𝑠3 𝑐1 𝑠1 (𝑎3 𝑐23 +𝑎2 𝑐2 )𝑐1

𝑐 𝑠 −𝑠3 𝑠1 −𝐶1 (𝑎3 𝑐23 +𝑎2 𝑐2 )𝑠1
𝐴3 = [ 3 1 ] 2.4
𝑠3 𝑐3 0 𝑎3 𝑠23 + 𝑎2 𝑠2 + 𝑑1
0 0 0 1

𝑥 = (𝑎3 𝑐23 +𝑎2 𝑐2 )𝑐1 2.5

𝑦 = (𝑎3 𝑐23 +𝑎2 𝑐2 )𝑠1 2.6

𝑧 = 𝑎3 𝑠23 + 𝑎2 𝑠2 + 𝑑1 2.7
x
a
Podemos decir que el Angulo a es equivalente a atan(x,y).

Para 𝑞1:
𝑎 = atan2(𝑍 − 𝑑1, 𝐿)
𝑏 = atan2(𝑙3 sin(𝑞2 ) , 𝑙3 cos(𝑞2 ) + 𝑙3 )

𝑟 2 + 𝑙22 − 𝑙32
𝑞1 = 𝑎𝑐𝑜𝑠
2 ∗ 𝑟 ∗ 𝑙2
𝑞2 = 𝑎 − 𝑏
Para q2

𝑅 2 = 𝑙22 + 𝑙32 + 2𝑙2 𝑙3 cos(𝑐)

Despejando el coseno del ángulo para obtener el valor:
𝑅 2 − 𝑙22 + 𝑙32
𝑎 cos(𝑐) =
2𝑙2 𝑙3