Robotica1-2017

Algoritmo de Newton-Euler

NE-1 Asignar a cada eslabón un sistema de referencia de acuerdo con las normas de D-H
𝑖−1
NE-2 Obtener las matrices de rotación 𝑅𝑖 y sus inversas 𝑖𝑅𝑖−1

cos 𝜃𝑖 − cos 𝛼𝑖 ∗ sin 𝜃𝑖 sin 𝛼𝑖 ∗ sin 𝜃𝑖

𝑖−1
𝑅𝑖 = [ sin 𝜃𝑖 cos 𝛼𝑖 ∗ cos 𝜃𝑖 − sin 𝛼𝑖 ∗ cos 𝜃𝑖 ]
0 sin 𝛼𝑖 cos 𝛼𝑖

NE-3 Establecer las condiciones iniciales:

Para el sistema de la base {𝑆0 }:

0
𝜔0 : 𝑣𝑒𝑙𝑜𝑐𝑖𝑑𝑎𝑑 𝑎𝑛𝑔𝑢𝑙𝑎𝑟 = [0, 0, 0]𝑇
0 ̇
𝜔̇ 0 = 𝑎𝑐𝑒𝑙𝑒𝑟𝑎𝑐𝑖𝑜𝑛 𝑎𝑛𝑔𝑢𝑙𝑎𝑟 = [0, 0, 0]𝑇
0
𝑣0 : 𝑣𝑒𝑙𝑜𝑐𝑖𝑑𝑎𝑑 𝑙𝑖𝑛𝑒𝑎𝑙 = [0, 0, 0]𝑇
0 𝑇
𝑣̇ 0 = 𝑎𝑐𝑒𝑙𝑒𝑟𝑎𝑐𝑖𝑜𝑛 𝑙𝑖𝑛𝑒𝑎𝑙 = [𝑔𝑥 , 𝑔𝑦 , 𝑔𝑧 ]
0
𝜔0 , 0𝜔̇ 0 , 0𝑣0 Son típicamente nulos salvo que la base del robot este en movimiento. Para el
extremo del robot se conocerá la fuerza y el para ejercidos externamente 𝒏+𝟏𝒇𝒏+𝟏 y 𝒏+𝟏𝒏𝒏+𝟏
𝑧0 = [0 ,0 ,1]𝑇
𝑖
𝑝𝑖 = [𝑎 𝑑 sin(𝛼) 𝑑 cos(𝛼)]
= 𝑐𝑜𝑜𝑟𝑑𝑒𝑛𝑎𝑑𝑎𝑠 𝑑𝑒𝑙 𝑣𝑒𝑐𝑡𝑜𝑟 𝑞𝑢𝑒 𝑢𝑛𝑒 𝑎𝑙 𝑠𝑖𝑠𝑡𝑒𝑚𝑎 𝑠𝑖−1 𝑐𝑜𝑛 𝑆𝑖 𝑒𝑥𝑝𝑟𝑒𝑠𝑎𝑑𝑎𝑠 𝑒𝑛 𝑒𝑙 𝑠𝑖𝑡𝑒𝑚𝑎 𝑆𝑖
𝑖
𝑆𝑖 = 𝐶𝑜𝑜𝑟𝑑𝑒𝑛𝑎𝑑𝑎𝑠 𝑑𝑒𝑙 𝑐𝑒𝑛𝑡𝑟𝑜 𝑑𝑒 𝑚𝑎𝑠𝑎𝑠 𝑑𝑒𝑙 𝑒𝑠𝑙𝑎𝑏𝑜𝑛 𝑖 𝑟𝑒𝑠𝑝𝑒𝑐𝑡𝑜 𝑑𝑒𝑙 𝑠𝑖𝑠𝑡𝑒𝑚𝑎 {𝑖}
𝑖
𝐼𝑖 = 𝑀𝑎𝑡𝑟𝑖𝑧 𝑑𝑒 𝑖𝑛𝑒𝑟𝑐𝑖𝑎 𝑑𝑒𝑙 𝑒𝑠𝑙𝑎𝑏𝑜𝑛 𝑖 𝑟𝑒𝑠𝑝𝑒𝑐𝑡𝑜 𝑑𝑒𝑙 𝑠𝑖𝑠𝑡𝑒𝑚𝑎 {𝑖}

La aceleración lineal de la base 0𝑣̇ 0 de la base se inicializará con el inverso del vector de
gravedad 𝑔 expresado en el sistema {0} (generalmente 𝑔 = [0 0 − 9.8]𝑇 ).
Para 𝑖 = 1 … 𝑛 realizar los pasos 4 a 7

NE-4. Obtener la velocidad angular del sistema {𝑆𝑖 }

𝑖
𝑖 𝑅𝑖−1 ( 𝑖−1𝜔𝑖−1 + 𝑧0 𝑞̇ 𝑖 ) … 𝑆𝑖 𝑒𝑙 𝑒𝑠𝑙𝑎𝑏𝑜𝑛 𝑒𝑠 𝑑𝑒 𝑟𝑜𝑡𝑎𝑐𝑖𝑜𝑛
𝜔𝑖 = { 𝑖 𝑖−1
𝑅𝑖−1 𝜔𝑖−1 … 𝑆𝑖 𝑒𝑙 𝑒𝑠𝑙𝑎𝑏𝑜𝑛 𝑒𝑠 𝑑𝑒 𝑡𝑟𝑎𝑠𝑙𝑎𝑐𝑖𝑜𝑛
 Robotica1-2017

NE-5 Obtener la aceleración angular del sistema {𝑆𝑖 }

𝑖
𝑖 𝑅𝑖−1 ( 𝑖−1𝜔̇ 𝑖−1 + 𝑧0 𝑞̈ 𝑖 ) + 𝑖−1𝜔𝑖−1 × 𝑧0 𝑞̇ 𝑖 … 𝑆𝑖 𝑒𝑙 𝑒𝑠𝑙𝑎𝑏𝑜𝑛 𝑒𝑠 𝑑𝑒 𝑟𝑜𝑡𝑎𝑐𝑖𝑜𝑛
𝜔̇ 𝑖 = { 𝑖 𝑖−1
𝑅𝑖−1 𝜔̇ 𝑖−1 … 𝑆𝑖 𝑒𝑙 𝑒𝑠𝑙𝑎𝑏𝑜𝑛 𝑒𝑠 𝑑𝑒 𝑡𝑟𝑎𝑠𝑙𝑎𝑐𝑖𝑜𝑛

NE-6 Obtener la aceleración lineal del sistema i:

𝑖
𝜔̇ 𝑖 × 𝑖𝑝𝑖 + 𝑖𝜔𝑖 × ( 𝑖𝜔𝑖 × 𝑖𝑝𝑖 ) + 𝑖𝑅𝑖−1 𝑖−1𝑣̇ 𝑖−1 … 𝑆𝑖 𝑒𝑙 𝑒𝑠𝑙𝑎𝑏𝑜𝑛 𝑒𝑠 𝑑𝑒 𝑟𝑜𝑡𝑎𝑐𝑖𝑜𝑛
𝑖
𝑣̇ 𝑖 = 𝑖
𝑅𝑖−1 (𝑧0 𝑞̈ 𝑖 + 𝑖−1𝑣̇ 𝑖−1 ) + 𝑖𝜔̇ 𝑖 × 𝑖𝑝𝑖 + 2 𝑖𝜔𝑖 × 𝑖𝑅𝑖−1 𝑧0 𝑞̇ 𝑖 +
𝑖
{ 𝜔𝑖 × ( 𝑖𝜔𝑖 × 𝑖𝑝𝑖 ) … 𝑆𝑖 𝑒𝑙 𝑒𝑠𝑙𝑎𝑏𝑜𝑛 𝑒𝑠 𝑡𝑟𝑎𝑠𝑙𝑎𝑐𝑖𝑜𝑛

NE-7 Obtener la aceleración lineal del centro de gravedad

𝑖
𝑎𝑖 = 𝑖𝜔̇ 𝑖 × 𝑖𝑠𝑖 + 𝑖𝜔𝑖 × ( 𝑖𝜔𝑖 × 𝑖𝑠𝑖 ) + 𝑖𝑣̇ 𝑖

Para 𝑖 = 𝑛 … 1 realizar los pasos 8 a 10

NE-8 Obtener la fuerza ejercida sobre el eslabón 𝑖:

𝑖
𝑓𝑖 = 𝑖𝑅𝑖+1 𝑖+1𝑓𝑖+1 + 𝑚𝑖 𝑖𝑎𝑖

NE-9 Obtener el par ejercido sobre el eslabón 𝑖

𝑖
𝑛𝑖 = 𝑖𝑅𝑖+1 [ 𝑖𝑛𝑖 + ( 𝑖+1𝑅𝑖 𝑖𝑝𝑖 ) × 𝑖+1𝑓𝑖+1 ] + ( 𝑖𝑝𝑖 + 𝑖𝑠𝑖 ) × 𝑚𝑖 𝑖𝑎𝑖 + 𝑖𝐼𝑖 𝑖𝜔̇ 𝑖 + 𝑖𝜔𝑖 × ( 𝑖𝐼𝑖 𝑖𝜔𝑖 )

Donde:
𝐼𝑥𝑥 𝐼𝑥𝑦 𝐼𝑥𝑧
𝑐 𝐼 𝐼𝑦𝑦 𝐼𝑦𝑧 ]
𝐼 = [ 𝑥𝑦
𝐼𝑥𝑧 𝐼𝑦𝑧 𝐼𝑧𝑧

NE-10 Obtener la fuerza o par aplicado a la articulación 𝑖

𝑖 𝑇 𝑖
𝑛𝑖 𝑅𝑖−1 𝑧0 … 𝑆𝑖 𝑒𝑙 𝑒𝑠𝑙𝑎𝑏𝑜𝑛 𝑒𝑠 𝑑𝑒 𝑟𝑜𝑡𝑎𝑐𝑖𝑜𝑛
𝜏𝑖 = { 𝑖 𝑇 𝑖
𝑓𝑖 𝑅𝑖−1 𝑧0 … 𝑆𝑖 𝑒𝑙 𝑒𝑠𝑙𝑎𝑏𝑜𝑛 𝑒𝑠 𝑑𝑒 𝑡𝑟𝑎𝑠𝑙𝑎𝑐𝑖𝑜𝑛

Donde 𝜏 es el par o fuerza efectivo (par motor menos pares de rozamiento o perturbación)

A tener en cuenta:
cos(𝜃1 + 𝜃2 ) = 𝑐𝑜𝑠𝜃1 𝑐𝑜𝑠𝜃2 − 𝑠𝑒𝑛𝜃1 𝑠𝑒𝑛𝜃2
sen(𝜃1 + 𝜃2 ) = 𝑠𝑒𝑛𝜃1 𝑐𝑜𝑠𝜃2 + 𝑐𝑜𝑠𝜃1 𝑠𝑒𝑛𝜃2