Formulario de Cinemática y Dinámica de Robots

Contents
1 Cinemática del Manipulador 1
1.1 Parámetros Denavit–Hartenberg . . . . . . . . . . . . . . . . . . . . . . . 1
1.2 Transformaciones Homogéneas . . . . . . . . . . . . . . . . . . . . . . . . 2
1.3 Cinemática Directa y Jacobianos . . . . . . . . . . . . . . . . . . . . . . 2

2 Dinámica por el Método de Lagrange 3

2.1 Energı́a Cinética T . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3
2.2 Energı́a Potencial V . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3
2.3 Lagrangiano y Ecuaciones de Movimiento . . . . . . . . . . . . . . . . . . 4

3 Dinámica por el Método de Newton–Euler 5

3.1 Variables y Notación . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5
3.2 Recursión Hacia Adelante (Forward Recursion) . . . . . . . . . . . . . . 6
3.3 Recursión Hacia Atrás (Backward Recursion) . . . . . . . . . . . . . . . 6

1 Cinemática del Manipulador

1.1 Parámetros Denavit–Hartenberg
Para un robot de n grados de libertad {1, . . . , n} con articulaciones rotacionales o prismáticas,
la convención de Denavit–Hartenberg (DH) asocia a cada eslabón i los siguientes parámetros:

Articulación i ai−1 α i−1 di θi (Tipo: R/P)

1 a0 α0 d1 θ1 (R ó P)
2 a1 α1 d2 θ2 (R ó P)
.. .. .. .. .. ..
. . . . . .
n an−1 αn−1 dn θn (R ó P)

Interpretación de los parámetros:

• ai−1 : distancia sobre xi−1 entre los orı́genes de los ejes zi−1 y zi .

• α i−1 : ángulo entre zi−1 y zi , medido sobre xi−1 .

• di : distancia sobre zi entre los orı́genes de los ejes xi−1 y xi .

1
 • θi : ángulo entre xi−1 y xi , medido sobre zi .

• (Tipo: R/P): indica si la articulación es rotacional (R) o prismática (P). En artic-

ulaciones R, θi es variable y di es constante. En articulaciones P, di es variable y θi
es constante.

1.2 Transformaciones Homogéneas

La transformación homogénea entre el frame i − 1 y el frame i se denota
i−1 i−1
!
i−1
Ri p i
Ti = ,
000 1

donde
 
cos θi − sin θi cos α i−1 sin θi sin α i−1
 
a i−1
i−1
  i−1
 sin θi
Ri =  cos θi cos α i−1 − cos θi sin α i−1 
, pi = − sin α i−1 di  .
 

0 sin α i−1 cos α i−1 cos α i−1 di

Entonces la cinemática directa global del manipulador se obtiene por

0
Tn = 0 T1 1 T2 2 T3 · · · n−1
Tn .

La posición y orientación del efector final (frame n) con respecto al frame base 0 quedan
dadas por la parte rotacional 0 Rn y traslacional 0 pn de 0 Tn .

1.3 Cinemática Directa y Jacobianos

Sea 0 pi la posición del origen del frame i en coordenadas del frame 0. El centro de masa
de cada eslabón i se ubica en una posición relativa rCi expresada en el propio frame i.
i

Entonces la posición del centro de masa Ci en el frame base es

0
rCi = 0 Ri rC
i
i
+ 0 pi , i = 1, 2, . . . , n.

Velocidades lineales:
   
q1 q̇1
 q2   q̇2 
0 r˙ = Jv,i (q) q̇, q =  ..  ,
 
q̇ =  ..  ,
 
Ci
. .
qn q̇n

∂(0 rC
x ) ∂(0 rC
x ) ∂(0 rC
x )
 
i i
··· i
 ∂q1 ∂q2 ∂qn 
 ∂(0 ry ) y
∂(0 rC ) y
∂(0 rC )
Jv,i (q) = 
 ∂q1
Ci
∂q2
i
··· ∂qn
i

.
 0z 
∂( rC ) ∂(0 rC
z ) ∂(0 rC
z )

∂q1
i
∂q2
i
··· ∂qn
i

2
Velocidad angular del eslabón i:


ω i = Jω,i (q) q̇, Jω,i (q) = z0 z1 · · · zi−1 0 · · · 0 (3×n) ,

donde zk es el eje de rotación de la articulación k + 1 expresado en el frame base. Para

manipuladores con todas las articulaciones rotacionales, ω i es la suma de las velocidades
angulares proyectadas en el frame base:
 
X i 0
ωi = q̇k z0k−1 , z0k−1 = 0 R k−1 0 .
k=1 1

2 Dinámica por el Método de Lagrange

2.1 Energı́a Cinética T
Cada eslabón i tiene:

masa mi , Matriz de inercia en COM Ii ∈ R3×3 ,

expresada en el propio frame i. Entonces su energı́a cinética total es

0 r˙ 2
Ti = 12 mi Ci + 1
2
ω⊤
i Ii ω i .

Sumando sobre todos los eslabones i = 1, . . . , n:

n
X n h
X i
T (q, q̇) = Ti = 1
2
mi q̇ ⊤ Jv,i
⊤
Jv,i q̇ + q̇ ⊤ Jω,i
⊤
Ii Jω,i q̇ .
i=1 i=1

Definimos la matriz de inercia generalizada D(q) como

n h
X i
⊤ ⊤
D(q) = mi Jv,i Jv,i + Jω,i Ii Jω,i , entonces T = 21 q̇ ⊤ D(q) q̇.
i=1

2.2 Energı́a Potencial V

Asumiendo gravedad g = (0, 0, − g)⊤ en el frame base,
n
X n
X
mi g⊤ 0 rCi = − g
0
rCz i .

V = mi
i=1 i=1

n
X
0   
Si rCz i es la coordenada z de Ci , V = mi g hi (q), donde hi (q) ≡ − 0 rCz i es la altura
i=1
de Ci sobre el plano de referencia.

3
2.3 Lagrangiano y Ecuaciones de Movimiento
Definimos la función de Lagrange

L(q, q̇) = T (q, q̇) − V (q).

Para cada coordenada generalizada qi , la ecuación de Lagrange es

d  ∂L  ∂L
− = τi , i = 1, . . . , n,
dt ∂ q̇i ∂qi

donde τi es el torque (o fuerza si la articulación es prismática) aplicado en la articulación

i.

1. Derivada de ∂L/∂ q̇:

∂T ∂V ∂L
= D(q) q̇, =0 =⇒ = D(q) q̇.
∂ q̇ ∂ q̇ ∂ q̇
Luego,
d  ∂L  d 
= D(q) q̇ = Ḋ(q) q̇ + D(q) q̈,
dt ∂ q̇ dt
n
X ∂D
Ḋ(q) = q̇r .
r=1
∂qr

2. Derivada de ∂L/∂q:
∂L ∂T ∂V
= − .
∂q ∂q ∂q
1
P
Dado que T = 2 j,k Djk (q) q̇j q̇k ,
n
∂T 1
X ∂Djk (q)
= 2
q̇j q̇k , i = 1, . . . , n.
∂qi j,k=1
∂q i

Y n

∂V X ∂ 0 rCz k
= mk g .
∂qi k=1
∂qi

3. Ecuaciones de movimiento:
d  ∂L  ∂L h
1

i
− = D(q) q̈ + Ḋ(q) q̇ − 2 ∂q D(q) q̇ q̇ + g(q) = τ,
dt ∂ q̇ ∂q | {z }
C(q, q̇) q̇

donde:
n
X  
1
C(q, q̇) q̇ ⇐⇒ Γ ijk (q) q̇j q̇k , Γ ijk = 2
∂qj Dik + ∂qk Dij − ∂qi Djk .
j,k=1

4
El vector de gravedad g(q) se obtiene de ∂V /∂q:
 
∂q1 V
∂ V 
 q2 
g(q) = 
 ..  .

 . 
∂qn V

Por tanto, la dinámica inversa en forma final es:

D(q) q̈ + C(q, q̇) q̇ + g(q) = τ.

3 Dinámica por el Método de Newton–Euler

En este enfoque se realiza una iteración hacia adelante para calcular velocidades y acel-
eraciones de cada eslabón, seguida de una iteración hacia atrás para hallar fuerzas y
momentos internos, y finalmente los torques en cada articulación.

3.1 Variables y Notación

• ωi ∈ R3 : velocidad angular del eslabón i, expresada en el frame i.

• ω̇i ∈ R3 : aceleración angular del eslabón i.

• vi ∈ R3 : velocidad lineal del origen del frame i.

• ai ∈ R3 : aceleración lineal del origen del frame i.

• aCi ∈ R3 : aceleración lineal del centro de masa Ci .

• Fi ∈ R3 : fuerza neta interna transmitida desde el eslabón i hacia i − 1.

• Ni ∈ R3 : momento interno transmitido desde el eslabón i hacia i − 1.

• R i−1
i
∈ R3×3 , p ii−1 ∈ R3 : rotación y traslación de i−1
Ti .

• rC 3
i ∈ R : vector desde el origen del frame i al centro de masa Ci en el frame i.
i

• rii+1 ∈ R3 : vector desde el origen del frame i al origen del frame i + 1, expresado
en coordenadas de i.

• ω0 = ω̇0 = 0, v0 = 0, a0 = − g (base estacionaria).

• Fuerzas externas en el eslabón n + 1: F n+1 = 0, N n+1 = 0 si no hay carga.

5
3.2 Recursión Hacia Adelante (Forward Recursion)
Para i = 1, 2, . . . , n:
 
0
i ⊤
ωi = R i−1 ω i−1 + 0  ,

q̇i
   
0 0
i ⊤
ω̇ i = R i−1 ω̇ i−1 +  0  + ω i ×  0  ,
q̈i q̇i
i ⊤
ω i−1 × p ii−1 + v i−1 ,
 
vi = R i−1
i ⊤
ω̇ i−1 × p ii−1 + ω i−1 × ω i−1 × p ii−1 + a i−1 ,


ai = R i−1

= ai + ω̇ i × rCi i + ω i × ω i × rCi i .


aCi

3.3 Recursión Hacia Atrás (Backward Recursion)

Para i = n, n − 1, . . . , 1:
F i = mi aCi + R ii+1 F i+1 ,

N i = Ii ω̇ i + ω i × Ii ω i + rCi i × mi aCi + R ii+1 N i+1 + r i+1 × R ii+1 F i+1 .

i

Finalmente, el torque en la articulación i es la componente del momento N i a lo largo

del eje zi−1 :  
0
⊤  
 
τi = N i 0 = N i z.
1 i

Interpretación:
• En la recursión hacia adelante, se calculan ωi , ω̇i , ai , aCi conociendo los valores para
el eslabón i − 1.
• En la recursión hacia atrás, se propagan las fuerzas y momentos generados por
cada eslabón i hacia el anterior, sumando la inercia rotocéntrica, Coriolis, término
gravitacional (si no se incluyó antes), y los pares que se aplican en las articulaciones.
• La salida final {τ1 , . . . , τn } corresponde al vector de torques que debe aplicar cada
actuador para lograr los perfiles {qi , q̇i , q̈i }.

Resumen de Fórmulas
1. DH:
 
cos θi − sin θi cos α i−1 sin θi sin α i−1 a i−1
i−1
 sin θi cos θi cos α i−1 − cos θi sin α i−1 − sin α i−1 di 
Ti = 
 0
.
sin α i−1 cos α i−1 cos α i−1 di 
0 0 0 1

6
 2. Cinemática directa:
n
Y
0 i−1 0
Tn = Ti , Rn , 0 pn extraı́dos de 0 Tn .
i=1

3. Jacobianos:
0 r˙ = Jv,i (q) q̇, ωi = Jω,i (q) q̇.
Ci

4. Energı́a cinética:
n h
X i
1 ⊤ ⊤ ⊤
T = q̇ D(q) q̇,
2
D(q) = mi Jv,i Jv,i + Jω,i Ii Jω,i .
i=1

5. Energı́a potencial:
n
X 0
rCz i .

V = −g mi
i=1

6. Ecuaciones de Lagrange:

D(q) q̈ + C(q, q̇) q̇ + g(q) = τ,

donde C(q, q̇) q̇ i = nj,k=1 Γijk (q) q̇j q̇k , Γijk = 12 (∂qj Dik + ∂qk Dij − ∂qi Djk ).
  P

7. Newton–Euler: Forward Recursion (cinemática):

 
0
i ⊤
ωi = R i−1 ω i−1 +  0  ,
q̇i
   
0 0
i ⊤
ω̇i = R i−1 ω̇ i−1 + 0 + ωi × 0  ,
  
q̈i q̇i
i ⊤ i
 
vi = R i−1 ω i−1 × p i−1 + v i−1 ,
i ⊤
ω̇ i−1 × p ii−1 + ω i−1 × (ω i−1 × p ii−1 ) + a i−1 ,
 
ai = R i−1
aCi = ai + ω̇i × rCi i + ωi × (ωi × rCi i ).

8. Newton–Euler: Backward Recursion (dinámica):

F i = mi aCi + R ii+1 F i+1 ,

N i = Ii ω̇ i + ω i × (Ii ω i ) + rCi i × (mi aCi ) + R ii+1 N i+1 + r i+1 × R ii+1 F i+1 .

i
 
0
⊤  
τi = N i 0 = [N i ]z .
1

Fin del Formulario.