Parámetros D-H robot planar

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

𝑖−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

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
Matriz que describe el comportamiento del último elemento respecto a la base
 𝐶(𝑞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

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

0
𝑑𝑒𝑡[ ]
𝐴1 = −𝑙1 𝐶𝑞1 [ 0 0 1] + 𝑙1 𝑆𝑞1 [ 0 0 1] − 0 [ 𝑆𝑞1 𝐶𝑞1 0]
0 0 0 0 0 0 0 0 0
𝐶𝑞1 −𝑆𝑞1 0
+ 1 [ 𝑆𝑞1 𝐶𝑞1 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

 0
𝐴𝑑𝑗[ 𝐴1 ]𝑇
𝐶𝑞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
=
𝑆𝑞 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 −𝑆𝑞2 0 𝑙2 𝐶𝑞2

−𝑆𝑞1 𝐶𝑞1 0 0 ] [ 𝑛𝑦 𝑂𝑦 𝑎𝑦 𝑃𝑦 𝐶𝑞2 0 𝑙2 𝑆𝑞2
[ ] = 𝑆𝑞2
0 0 1 0 𝑛𝑧 𝑂𝑧 𝑎𝑧 𝑃𝑧 0 0 1 0
0 0 0 1 0 0 0 1 [ 0 0 0 1]

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

𝐶𝑞1 𝑃𝑦 − 𝑆𝑞1 𝑃𝑥 = 𝑙2 𝑆𝑞2
𝑃𝑧 = 0