Parámetros D-H robot planar 3GDL

𝐴𝑟𝑡𝑖𝑐𝑢𝑙𝑎𝑐𝑖ó𝑛 𝜃 𝑑 𝑎 𝛼
1 𝑞1 0 𝑙1 0
2 𝑞2 0 𝑙2 0
3 𝑞3 0 𝑙3 0

𝐶𝜃 −𝐶𝛼𝑆𝜃 𝑆𝛼𝑆𝜃 𝑎𝐶𝜃

𝑖−1
𝐴𝑖 = [𝑆𝜃 𝐶𝛼𝐶𝜃 −𝑆𝛼𝐶𝜃 𝑎𝑆𝜃 ]
0 𝑆𝛼 𝐶𝛼 𝑑
0 0 0 1
Matrices de Transformación Homogéneas

𝐶𝑞1 −𝑆𝑞1 0 𝑙1 𝐶𝑞1

0 𝑙1 𝑆𝑞1
0
𝐴1 = [ 𝑆𝑞1 𝐶𝑞1 ]
0 0 1 0
0 0 0 1
𝐶𝑞2 −𝑆𝑞2 0 𝑙2 𝐶𝑞2
0 𝑙2 𝑆𝑞2 ]
1
𝐴2 = [ 𝑆𝑞2 𝐶𝑞2
0 0 1 0
0 0 0 1
𝐶𝑞3 −𝑆𝑞3 0 𝑙3 𝐶𝑞3
0 𝑙3 𝑆𝑞3
2
𝐴3 = [ 𝑆𝑞3 𝐶𝑞3 ]
0 0 1 0
0 0 0 1
0
𝑃𝑟𝑖𝑚𝑒𝑟𝑎 𝑜𝑝𝑒𝑟𝑎𝑐𝑖𝑜𝑛 𝑑𝑒 𝑚𝑎𝑡𝑟𝑖𝑐𝑒𝑠 𝑂𝑐𝑡𝑒𝑛𝑐𝑖𝑜𝑛 𝑑𝑒 𝐴2
0
𝐴2 = 𝑩 = 𝟎 𝑨𝟏 ∗ 𝟏 𝑨𝟐
𝐶𝑞1 −𝑆𝑞1 0 𝑙1 𝐶𝑞1
0 𝑙1 𝑆𝑞1 ]
0
𝐴1 = [ 𝑆𝑞1 𝐶𝑞1
0 0 1 0
0 0 0 1
 𝐶𝑞2 −𝑆𝑞2 0 𝑙2 𝐶𝑞2
0 𝑙2 𝑆𝑞2
1
𝐴2 = [ 𝑆𝑞2 𝐶𝑞2 ]
0 0 1 0
0 0 0 1

𝐶𝑞1 𝐶𝑞2 − 𝑆𝑞1 𝑆𝑞2 −(𝑆𝑞1 𝐶𝑞2 + 𝐶𝑞1 𝑆𝑞2 ) 0 𝐶𝑞1 𝑙2 𝐶𝑞2 + 𝑆𝑞1 𝑙2 𝑆𝑞2 + 𝑙1 𝐶𝑞1
0 𝑆𝑞1 𝑙2 𝑆𝑞2 + 𝐶𝑞1 𝑙2 𝑆𝑞2 + 𝑙1 𝑆𝑞1 ]
𝐵 = [𝑆𝑞1 𝐶𝑞2 + 𝐶𝑞1 𝑆𝑞2 −𝑆𝑞1 𝑆𝑞2 + 𝐶𝑞1 𝐶𝑞2
0 0 1 0
0 0 0 1
𝐶(𝑞1 + 𝑞2 ) −𝑆(𝑞1 + 𝑞2 ) 0 𝑙1 𝐶𝑞1 +𝑙2 𝐶(𝑞1 + 𝑞2 )
0 𝑙1 𝑆𝑞1 + 𝑙2 𝑆(𝑞1 + 𝑞2 )]
𝐵 = [ 𝑆(𝑞1 + 𝑞2 ) 𝐶(𝑞1 + 𝑞2 )
0 0 1 0
0 0 0 1

𝑻 = 𝑩 ∗ 𝟐 𝑨𝟑
𝐶(𝑞1 + 𝑞2 ) −𝑆(𝑞1 + 𝑞2 ) 0 𝑙1 𝐶𝑞1 +𝑙2 𝐶(𝑞1 + 𝑞2 )
0 𝑙1 𝑆𝑞1 + 𝑙2 𝑆(𝑞1 + 𝑞2 )]
𝐵 = [ 𝑆(𝑞1 + 𝑞2 ) 𝐶(𝑞1 + 𝑞2 )
0 0 1 0
0 0 0 1
𝐶𝑞3 −𝑆𝑞3 0 𝑙3 𝐶𝑞3
0 𝑙3 𝑆𝑞3 ]
2
𝐴3 [ 𝑆𝑞3 𝐶𝑞3
0 0 1 0
0 0 0 1
𝐶 (𝑞1 + 𝑞2 )𝐶𝑞3 − 𝑆(𝑞1 + 𝑞2 )𝑆𝑞3 −𝐶(𝑞1 + 𝑞2 )𝑆𝑞3 − 𝑆(𝑞1 + 𝑞2 )𝐶𝑞3 0 𝐶(𝑞1 + 𝑞2 )𝑙3 𝐶𝑞3 − 𝑆(𝑞1 + 𝑞2 )𝑙3 𝑆𝑞3 + 𝑙1 𝐶𝑞1 +𝑙2 𝐶(𝑞1 + 𝑞2 )
[𝑆(𝑞1 + 𝑞2 )𝐶𝑞3 + 𝐶(𝑞1 + 𝑞2 )𝑆𝑞3 −𝑆 (𝑞1 + 𝑞2 )𝑆𝑞3 + 𝐶(𝑞1 + 𝑞2 )𝐶𝑞3 0 𝑆(𝑞1 + 𝑞2 )𝑙3 𝐶𝑞3 + 𝐶(𝑞1 + 𝑞2 )𝑙3 𝑆𝑞3 + 𝑙1 𝑆𝑞1 + 𝑙2 𝑆(𝑞1 + 𝑞2 )]
0 0 1 0
0 0 0 1

Matriz que describe el comportamiento del último elemento respecto a la base

𝐶 (𝑞1 + 𝑞2 + 𝑞 3 ) −𝑆(𝑞1 + 𝑞2 + 𝑞3 ) 0 𝑙1 𝐶𝑞1 +𝑙2 𝐶(𝑞 + 𝑞 ) + 𝑙3 𝐶(𝑞 + 𝑞 + 𝑞 )

1 2 1 2 3
0 𝑙1 𝑆𝑞1 + 𝑙2 𝑆(𝑞1 + 𝑞 ) + 𝑙 𝑆(𝑞 + 𝑞 + 𝑞 )
0
𝐴3 = 𝑇 = 𝑆 ( 𝑞 1 + 𝑞 2 + 𝑞 3 ) 𝐶(𝑞1 + 𝑞2 + 𝑞3 ) 2 3 1 2 3
0 0 1 0
[ 0 0 0 1 ]
 CALCULO DE CADA UNA DE LAS INVERSAS DE LAS MATRICES
𝟏
[𝑨]−𝟏 = 𝑨𝒅𝒋(𝑨𝑻 )
|𝑨|
𝟎
Se procede a calcular la matriz inversa de 𝑨𝟏

𝐶𝑞1 −𝑆𝑞1 0 𝑙1 𝐶𝑞1

0 𝑙1 𝑆𝑞1
0
𝐴1 = [ 𝑆𝑞1 𝐶𝑞1 ]
0 0 1 0
0 0 0 1
𝟎
Calculamos el determinante de la matriz 𝑨𝟏

𝑆𝑞1 𝐶𝑞1 0 𝐶𝑞1 −𝑆𝑞1 0 𝐶𝑞1 −𝑆𝑞1 0 𝐶𝑞1 −𝑆𝑞1 0

0
𝑑𝑒𝑡[ 𝐴1 ] = −𝑙1 𝐶𝑞1 [ 0 0 1] + 𝑙 𝑆𝑞
1 1 [ 0 0 1] − 0 [ 𝑆𝑞1 𝐶𝑞1 0] + 1 [ 𝑆𝑞1 𝐶𝑞1 0]
0 0 0 0 0 0 0 0 0 0 0 1
0
|𝑨| = 𝑑𝑒𝑡 [ 𝐴1 ] = [𝐶2 𝑞1 + 0 + 0 − (0 + 0 − 𝑆2 𝑞1 )] = 1

𝟎
Se procede a calcular la traspuesta de la matriz 𝑨𝟏
𝐶𝑞1 𝑆𝑞1 0 0
−𝑆𝑞1 𝐶𝑞1 0 0
[ 0 𝐴1 ]𝑇 = [ ]
0 0 1 0
𝑙1 𝐶𝑞1 𝑙1 𝑆𝑞1 0 1

Calculamos la matriz adjunta de la traspuesta

𝐶𝑞1 0 0 −𝑆𝑞1 0 0 −𝑆𝑞1 𝐶𝑞1 0 −𝑆𝑞1 𝐶𝑞1 0

+| 0 1 0| −| 0 1 0| +| 0 0 0| − | 0 0 1|
𝑙1 𝑆𝑞1 0 1 𝑙1 𝐶𝑞1 0 1 𝑙1 𝐶𝑞1 𝑙1 𝑆𝑞1 1 𝑙1 𝐶𝑞1 𝑙1 𝑆𝑞1 0
𝑆𝑞1 0 0 𝐶𝑞1 0 0 𝐶𝑞1 𝑆𝑞1 0 𝐶𝑞1 𝑆𝑞1 0
−| 0 1 0| +| 0 1 0| −| 0 0 0| + | 0 0 1|
𝑙1 𝑆𝑞1 0 1 𝑙1 𝐶𝑞1 0 1 𝑙1 𝐶𝑞1 𝑙1 𝑆𝑞1 1 𝑙1 𝐶𝑞1 𝑙1 𝑆𝑞1 0
𝐴𝑑𝑗[ 0 𝐴1 ]𝑇 =
𝑆𝑞 1 0 0 𝐶𝑞1 0 0 𝐶𝑞1 𝑆𝑞1 0 𝐶𝑞1 𝑆𝑞1 0
| 𝐶𝑞1 0 0| | −𝑆𝑞1 0 0| | −𝑆𝑞1 𝐶𝑞1 0| | −𝑆𝑞1 𝐶𝑞1 0|
𝑙1 𝑆𝑞1 0 1 𝑙1 𝐶𝑞1 0 1 𝑙1 𝐶𝑞1 𝑙1 𝑆𝑞1 1 𝑙1 𝐶𝑞1 𝑙1 𝑆𝑞1 0
𝑆𝑞1 0 0 𝐶𝑞1 0 0 𝐶𝑞1 𝑆𝑞1 0 𝐶𝑞1 𝑆𝑞1 0
|𝐶𝑞1 0 0| |−𝑆𝑞1 0 0| |−𝑆𝑞1 𝐶𝑞1 0| |−𝑆𝑞1 𝐶𝑞1 0|
[ 0 1 0 0 1 0 0 0 0 0 0 1 ]

𝐶𝑞1 𝑆𝑞1 0 −𝑙1

0
1 −𝑆𝑞1 𝐶𝑞1 0 0 ]
[ 𝐴1 ]−1 = [
1 0 0 1 0
0 0 0 1

𝟎
Entonces la matriz inversa de 𝑨𝟏 sera
 𝑪𝒒𝟏 𝑺𝒒𝟏 𝟎 −𝒍𝟏
𝟎 −𝑺𝒒 𝑪𝒒𝟏 𝟎 𝟎 ]
[ 𝑨𝟏 ]−𝟏 =[ 𝟏
𝟎 𝟎 𝟏 𝟎
𝟎 𝟎 𝟎 𝟏

𝟎
( 𝑨𝟏 )−𝟏 ∗ 𝟎
𝑻𝟑 = 𝟏
𝑨𝟐 ∗ 𝟐
𝑨𝟑

𝐶(𝑞2 + 𝑞3 ) −𝑆(𝑞2 + 𝑞3 ) 0 𝑙2 𝐶𝑞2 +𝑙3 𝐶(𝑞2 + 𝑞3 )

0 𝑙2 𝑆𝑞2 + 𝑙3 𝑆(𝑞2 + 𝑞3 )]
𝟏
𝑨𝟐 ∗ 𝟐
𝑨𝟑 = [ 𝑆(𝑞2 + 𝑞3 ) 𝐶(𝑞2 + 𝑞3 )
0 0 1 0
0 0 0 1

𝐶𝑞1 𝑆𝑞1 0 −𝑙1 𝒏𝒙 𝑶𝒙 𝒂𝒙 𝑷𝒙 𝐶(𝑞2 + 𝑞3 ) −𝑆(𝑞2 + 𝑞3 ) 0 𝑙2 𝐶𝑞2 +𝑙3 𝐶(𝑞2 + 𝑞3 )

−𝑆𝑞 𝐶𝑞1 0 0 ] [𝒏𝒚 𝑶𝒚 𝒂𝒚 𝑷𝒚 0 𝑙2 𝑆𝑞2 + 𝑙3 𝑆 (𝑞2 + 𝑞3 )
[ 1 ] = [ 𝑆(𝑞2 + 𝑞3 ) 𝐶(𝑞2 + 𝑞3 ) ]
0 0 1 0 𝒏𝒛 𝑶𝒛 𝒂𝒛 𝑷𝒛 0 0 1 0
0 0 0 1 𝟎 𝟎 𝟎 𝟏 0 0 0 1

Tomando el Elemento (𝟑, 𝟒)

𝐶𝑞1 𝑃𝑥 + 𝑆𝑞1 𝑃𝑦 − 𝑙1 = 𝑙2 𝐶𝑞2 + 𝑙3 𝐶(𝑞2 + 𝑞3 )
𝒚 𝑷 ′
𝒔𝒊 𝒒𝟏 = 𝒂𝒓𝒄𝑻 (𝑷 ′ )
𝒙

𝟐 𝟐
𝑬𝒏𝒕𝒐𝒏𝒄𝒆𝒔 𝑪𝒒𝟏 𝑷𝒙 + 𝑺𝒒𝟏 𝑷𝒚 = √𝑷𝒙 ′ + 𝑷𝒚 ′

√𝑷𝒙 ′𝟐 + 𝑷𝒚 ′𝟐 = 𝑙2 𝐶𝑞2 + 𝑙3 𝐶(𝑞2 + 𝑞3 ) − 𝑙1

𝐶𝑞1 𝑃𝑦 − 𝑆𝑞1 𝑃𝑥 = 𝑙2 𝑆𝑞2 + 𝑙3 𝑆(𝑞2 + 𝑞3 )
𝑃𝑧 = 0
1=1

𝐶𝑞2 −𝑆𝑞2 0 𝑙2
1 𝑆𝑞 𝐶𝑞2 0 0]
[ 𝐴2 ]−1 =[ 2
0 0 1 0
0 0 0 1

1
( 𝐴2 )−1 ∗ ( 0 𝐴1 )−1 ∗ 𝑇 = 2 𝐴3
𝐶𝑞2 𝑆𝑞2 0 −𝑙2 𝐶𝑞1 𝑆𝑞1 0 −𝑙1 𝑛𝑥 𝑂𝑥 𝑎𝑥 𝑃𝑥 𝐶𝑞3 −𝑆𝑞3 0 𝑙3 𝐶𝑞3
0 0 ] [𝑛𝑦 𝑂𝑦 𝑎 𝑃 0 𝑙3 𝑆𝑞3 ]
[−𝑆𝑞2 𝐶𝑞2 0 0 ] [−𝑆𝑞1 𝐶𝑞1 𝑦 𝑦
] = [ 𝑆𝑞3 𝐶𝑞3
0 0 1 0 0 0 1 0 𝑛𝑧 𝑂𝑧 𝑎𝑧 𝑃𝑧 0 0 1 0
0 0 0 1 0 0 0 1 0 0 0 1 0 0 0 1
 𝐶(𝑞1 + 𝑞2 ) 𝑆(𝑞1 + 𝑞2 ) 0 −(𝑙1 𝐶𝑞2 + 𝑙2 ) 𝑛𝑥 𝑂𝑥 𝑎𝑥 𝑃𝑥 𝐶𝑞3 −𝑆𝑞3 0 𝑙3 𝐶𝑞3
0 𝑙1 𝑆𝑞2 ] [𝑛𝑦 𝑂𝑦 𝑎𝑦 𝑃𝑦 0 𝑙3 𝑆𝑞3 ]
[−𝑆(𝑞1 + 𝑞2 ) 𝐶(𝑞1 + 𝑞2 ) ] = [ 𝑆𝑞3 𝐶𝑞3
0 0 1 0 𝑛𝑧 𝑂𝑧 𝑎𝑧 𝑃𝑧 0 0 1 0
0 0 0 1 0 0 0 1 0 0 0 1

𝐶 (𝑞1 + 𝑞2 )𝑃𝑥 + 𝑆(𝑞1 + 𝑞2 )𝑃𝑦 − 𝑙1 𝐶𝑞2 − 𝑙2 = 𝑙3 𝐶𝑞3

𝑷𝒚 ′′
𝒔𝒊 (𝒒𝟏 + 𝒒𝟐 ) = 𝒂𝒓𝒄𝑻 ( ′′ )
𝑷𝒙
𝟐 𝟐
𝑬𝒏𝒕𝒐𝒏𝒄𝒆𝒔 𝑪(𝒒𝟏 + 𝒒𝟐 )𝑷𝒙 + 𝑺(𝒒𝟏 + 𝒒𝟐 )𝑷𝒚 = √𝑷𝒙 ′′ + 𝑷𝒚 ′′

√𝑷𝒙 ′′ 𝟐 + 𝑷𝒚 ′′ 𝟐 − 𝒍𝟏 𝑪𝒒𝟐 − 𝒍𝟐 = 𝒍𝟑 𝑪𝒒𝟑

𝐶 (𝑞1 + 𝑞2 )𝑃𝑦 − 𝑆(𝑞1 + 𝑞2 )𝑃𝑥 + 𝑙1 𝑆𝑞2 = 𝑙3 𝑆𝑞3

𝑃𝑧 = 0
1=1