Foundations of Robotics: 2018-I

Dynamics
- Part II -
Prof. Oscar E. Ramos, Ph.D.
 Review …

• Lagrangian:
(q, q )   (q, q )   (q)
Kinetic Potential
energy energy

• Equations of Motion (Lagrange):

d    
   i
dt  qi  qi

• Dynamics of a robot manipulator:

  C (q, q )q  g(q)  τ tot
M (q)q

2
 Outline

1. Euler-Lagrange Formulation using DH

2. Newton-Euler Formulation

3. Dynamics of Aerial Robots

- Dynamics (part II) -

Prof. Oscar E. Ramos, Ph.D. 3
 Euler-Lagrange using DH
Introduction

• It uses the matrices obtained using the Denavit-Hartenberg convention

- It does matrix products

• Characteristics
- It leads to well structured equations
- It is computationally “inefficient”: redundant information

• The model is expressed as:

  c(q, q )  g(q)  τ tot
M (q)q

• Assumption:
- The transformation matrices 0Ti for each link (according to the DH convention)
are known

4
 Euler-Lagrange using DH
Algorithm

1. Inertia Matrix M:
 m11 m12  m1n 
m m22  m2 n  n
M   21 where each mij   Tr U kj J kU kiT 
      element is: k  max{i , j }
 
 mn1 mn 2  mnn 

a) Matrices Uij:
0 j 1
 0Ti  T j 1Q j Ti , ji
U ij  
q j  0, j i

0 1 0 0 0 0 0 0
1 0 0 0  0 0 0 0 
Qj   Qj  
0 0 0 0 0 0 0 1
   
0 0 0 0 0 0 0 0

If the j-th joint is If the j-th joint is

revolute prismatic
5
 Euler-Lagrange using DH
Algorithm

1. Inertia Matrix M:
 m11 m12  m1n 
m m22  m2 n  n
M   21 where each mij   Tr U kj J kU kiT 
      element is: k  max{i , j }
 
 mn1 mn 2  mnn 

b) Pseudo-inertia matrices Ji:

 xi2 dm
  x y dm
i i  x z dm
i i  x dm 
i

 yi xi dm  xi 
 y dm 
2

Ji  
  y dm
i  y z dm
i i i
y  Coordinates of the CoM
 z x dm of i with respect to the
 z dm   i
2
 i i  z y dm
i i  z dm
i i
 zi 
frame of link i
 x dm 
  i  y dm
i  z dm
i  dm 
- Note that Ji is positive definite

6
 Euler-Lagrange using DH
Algorithm

2. Coriolis and centrifugal forces Vector c:

 c1  n n
where each
c     c j   cikm qk qm
element is:
k 1 m 1
cn 

- Christoffel symbols:
n
cikm   Tr U jkm J jU Tji  , i, k , m  1, , n
j  max{i , k , m}

- Matrices Uijk:
 0T j 1Q j j 1Tk 1Qk k 1Ti , i  k  j
U ij 0 k 1 j 1
U ijk   T
 k 1 k Q T Q Ti , i  j  k
qk j 1 j
 0, k 1 o j  i


7
 Euler-Lagrange using DH
Algorithm

3. Gravity vector g:

 g1  n
where each
g     element is:
g j   m j g 0U ji j p j
j 1
 g n 
g0: gravity in a base frame
jp : homogeneous coordinates of the CoM of j in frame j
j

4. Dynamic Model:
  c(q, q )  g (q)  τ tot
M (q)q

8
 Outline

1. Euler-Lagrange Formulation using DH

2. Newton-Euler Formulation

3. Dynamics of Aerial Robots

- Dynamics (part II) -

Prof. Oscar E. Ramos, Ph.D. 9
 Newton-Euler Formulation
Idea: Analyze forces and torques

μ f
μ f

-f -μ

μ3 link 3
f3
3
-f3
2 μ2 -μ3
-f2
link 2
1 f1
-μ1 f2
-μ2
μ: moment (torque)
μ1 link 1
-f1 f: force
10
 Newton-Euler Formulation
Idea of the Algorithm

Link N

Link 0

Recursive - Velocities and accelerations: 0 to N

algorithm - Forces: N to 0

11
 Newton-Euler Formulation
Laws of Motion for 1 particle

• Newton’s 2nd law: translation

The rate of change of the linear moment is equal to the total force (ftot)
applied to the body
d
 f  ftot   mp 
dt

• Euler’s 2nd law: rotation

The rate of change of the angular momentum is equal to the total moment
(μtot) applied to the body
d
 μ  μtot   p  mp 
dt
where
h  p  mp angular moment of a particle

12
 Newton-Euler Formulation
Angular Moment for 1 Rigid Body

• Velocity: for a point of the rigid body

v  p  ω  p
mi
• Angular momentum:
- For a point ω p
h i  p  mi p  p  mi (ω  p)
- For a rigid body mi  dm
{0} dm   dV
h   hi   p  mi (ω  p)
i i

  p  (ω  p)  dV    p  (p  ω)  dV
V V

 
   pˆ (pω
ˆ )  dV   pˆ T pˆ  dV  ω h  ω
V V
 
 Inertia tensor
(in the system of the base 0)
13
 Newton-Euler Formulation
Derivative of the Angular Moment

• Angular momentum
- For a rigid body
h  ω mi

- Transformation of the inertia tensor ω p

  R RT
• Derivative
{0}
d d d
 h    ω   R RT ω
 
dt dt dt
 R RT ω  R R T ω  R RT ω
  : inertia tensor in the body frame
(constant)

ˆ R) RT ω  R  ( RT ω
 (ω ˆ T )ω  R RT ω
 R : rotation of the body with respect
to the reference frame

ˆ ω  ω
ω

 ω  ω  ω ˆ Tω  0
ω

14
 Newton-Euler Formulation
Laws of Motion for 1 Rigid Body

• Newton’s 2nd law: translation

The rate of change of the linear momentum of the CoM is equal to the total
force (ftot) applied to the rigid body

d c
 mp c   ftot f  f tot  mp
dt

• Euler’s 2nd law: rotation

The rate of change of the angular momentum (h) is equal to the total
momentum (μtot) applied to the rigid body

d
 h   μtot μ  μ tot
  ω  ω
 ω
dt

pc: position of the Center of Mass

15
 Newton-Euler Formulation
Translational Motion for Link i

Joint i Joint i+1

p ci

fi mg
i 0
fi1

• Dynamics of the translational motion (Newton’s 2nd):

f  f  f
i i 1
ci
 mig0  mip
reordering

ci  mi g0
fi  fi1  mip

16
 Newton-Euler Formulation
Rotational Motion for Link i

Joint i Joint i+1

ωi
zi
zi1 μi1
μi ri,ci
ri1,ci

fi mg
i 0
fi1

• Dynamics of the rotational motion with respect to the CoM (Euler’s 2nd):

μ  μ  μi i 1
 i  ωi  iωi
 (ri1,ci )  fi  (ri,ci )  fi1  iω
 i  ωi  iωi
μi  μi1  fi  ri1,ci  fi1  ri,ci  iω
reordering

 i  ωi  iωi
μi  μi1  fi  ri1,ci  fi1  ri,ci  iω
17
 Newton-Euler Formulation
Velocity and Acceleration for Link i

• Velocities: revolute joint

ωi  ωi1  qi zi1 Joint i

p i  p i1  ωi  ri1,i z i1 link i zi

ri1,i
• Angular acceleration:
i ω
ω  i1  qi zi1  qi (ωi1  zi1) pi1 pi
• Linear acceleration:

i  p
p i1  ω
 i  ri1,i  ωi  (ωi  ri1,i ) {0}
With respect to the base frame
• Acceleration of the Center of Mass:
ci  p
p i  ω
 i  ri,ci  ωi  (ωi  ri,ci )

18
 Newton-Euler Formulation
Procedure

1. Forward recursion (in frame i): 0 to N

i
ωi  i Ri1  ωi1  qi zi1 
i
 i  i Ri1  ω
ω  i1  qi zi1  qiωi1  zi1 
i
i  i Ri1p
p i1  ω i  ri1,i  ωi  (ωi  ri1,i )
ci  p
p i  ω
 i  ri,ci  ωi  (ωi  ri,ci )

2. Backward recursion: N to 0
ci  mi g0
fi  fi1  mip
 i  ωi  Iiωi
μi  μi1  fi  (ri1,i  ri,ci )  fi1  ri,ci  Iiω

fiT zi1
Note: generalized forces τi   T
μi zi1
19
 Newton-Euler Formulation
Comments on NE

• Recursive formulas can be computed in two forms: symbolic or

numeric
• Symbolic form:
- Substitute expressions recursively
- The result is the same as the obe obtained using Lagrange
- There are no advantages

• Numeric form:
- Substitute numeric values only (numbers) in every step
- Computational complexity of every step is constant
- Recommended for usage in real time (specially when n is large)

20
 Outline

1. Euler-Lagrange Formulation using DH

2. Newton-Euler Formulation

3. Dynamics of Aerial Robots

- Dynamics (part II) -

Prof. Oscar E. Ramos, Ph.D. 21
 Dynamics of Aerial Robots
Aerial Robots

• Also known as:

- UAVs (Unmanned Aerial Vehicles)
- RPVs (Remotely Piloted Vehicles)
- Aerial Robots
- “Drones”

22
 Dynamics of Aerial Robots
Control of a Quadcopter: Idea

• In general, to control the position and orientation:

- The (angular) velocity of each rotor is controlled

ω3 ω2

ω4

z
y
x ω1

- Note: Two rotors rotate in one direction and the other two rotate in opposite
direction

23
 Dynamics of Aerial Robots
Control of a Quadcopter: Idea

• Hover (keep the same pose):

- “Equal” angular velocities (ω)
- The thrust of the propellers compensates for the gravitational force (the
weight of the robot)
- The torques of the 4 rotors sum to zero

ω
ω
ω
z
y
x ω

Top view

24
 Dynamics of Aerial Robots
Control of a Quadcopter: Idea

• Up and down motion

- Modify the velocity of all the rotors equally
- More velocity → Robot moves up
- Lower velocity → Robot moves down

Ascent Descent

25
 Dynamics of Aerial Robots
Control of a Quadcopter: Idea

• Rotational motion: roll (x) and pitch (y)

ωa ω a > ωb
ωb

ωb ω b > ωa ωb

Note: both rotors are kept at an “intermediate” velocity (they are not modified) 26
 Dynamics of Aerial Robots
Control of a Quadcopter: Idea

• Rotational motion: yaw (z)

Clockwise rotation Counterclockwise rotation

27
 Dynamics of Aerial Robots
Control of a Quadcopter: Idea

• Translational motion

(accelerates)

(decelerates)

28
 Vijay Kumar Lab
[Link]

29
 Dynamics of Aerial Robots
Behavior of a Rotor

• Rotor thrust:  f
m [Nm] [N]
f  k f 2
• Moment of aerodynamic
resistance:
  km 2

• Example:
- Weight of the quadcopter: mg
1
- Thrust for each rotor: mg
4 1
- Compute the velocity of the rotor mg
4
- Compute the aerodynamic resistance
- Choose a motor with an appropriate
torque (τm) 0 
Velocity of the rotor
(rad/s o RPM)

30
 Dynamics of Aerial Robots
Dynamics of a Quadcopter
f3
f2
μ3
f4 μ2

μ4 p3 f1
z p2
p4 y
x p1 μ1

mg0

• Resulting force:
f res  f1  f 2  f3  f 4  mg 0
• Resulting moment (about the CoM):
μ res  μ1  μ 2  μ 3  μ 4  p1  f1  p 2  f 2  p 3  f3  p 4  f 4

31
 Dynamics of Aerial Robots
Dynamics of a Quadcopter

• Dynamics of Translation (Newton’s 2nd law):

f1  f 2  f3  f 4  mg 0  mr

• Dynamics of Rotation (Euler’s 2nd law):

  ω  ω
μ1  μ 2  μ 3  μ 4  p1  f1  p 2  f 2  p3  f3  p 4  f 4  ω

̇ ω

r
{I }

32
 Dynamics of Aerial Robots
Simplified Model

• Moments with respect to the origin of frame {B}

l   0   0  f3
p1  f1  0    0    lf1  f2
0   f1   0 
f4
0   0  lf 2  z
p 2  f 2   l    0    0  l y l
0   f 2   0  l {B} x f1
l
 l   0   0 
p3  f3   0    0   lf 3 
 0   f3   0 

 0   0   lf 4  Frame {B}: Rigidly attached

p 4  f 4   l    0    0  to the body
 0   f 4   0 
Assumes symmetry and same lengths

33
 Dynamics of Aerial Robots
Simplified Model

• Dynamics of Translation
m
r  mg 0  f1  f 2  f3  f 4
m
r  mg 0  I RB ( B f1  B f 2  B f3  B f 4 )

0  0 
mr  m  0   I RB  0 

B B B B
  g   f1  f 2  f3  f 4 

f3
f2
I
RB : Rotation matrix of frame {B} with respect f4
to the inertial frame {I} z l
(for example, using Euler angles) l l y
{B} x f1
fi : Componente i de la fuerza de empuje l

34
 Dynamics of Aerial Robots
Simplified Model

• Dynamics of Rotation
  ω  Iω  μ1  μ 2  μ 3  μ 4  p1  f1  p 2  f 2  p3  f3  p 4  f 4
Iω
 xb   xb   xb   0   0   0   0   0  lf   0   lf 
2 4
        
I  yb    yb   I  yb    0    0    0    0    lf1    0   lf 3    0 
                  0   0   0   0 
 zb   zb   zb   1   2   3   4         

 xb   l ( f  f )   xb   xb 
2 4
       
  yb    l ( f3  l1 )    yb     yb 
              

 zb   1 2 3 4 
 zb   zb 

( xb ,  yb ,  zb ) : Angular acceleration in the frame of the body

35
 Dynamics of Aerial Robots
Simplified Model

• Summary:
0  0 
mr  m  0   I RB  0 

B B B B
  g   f1  f 2  f 3  f 4 

 xb   l ( f  f )   xb   xb 
2 4
       

  yb    l ( f 3  l1 )    yb     yb 
              

 zb   1 2 3 4  
 zb   zb 

• How to compute the parameters (l, m, I)?

- If we know the positions, velocities and accelerations, we can estimate
the parameters
• How to compute the angular velocity in the body frame?
- Converting the velocity from Roll, Pitch, Yaw to angular velocity
36
 Dynamics of Aerial Robots
State Space Model

• Components:
- q: configuration (position and orientation) of the system
T
q   x y z r p  y 
- x: state of the system
q 
x 
q 
• Notes:
- The state vector x has 12 elements (positions and velocities)
q 
- If we want the robot to keep its position and orientation … we use x   
0

Position: Cartesian coordinates

Orientation: roll, pitch, yaw angles (usually)
37
 References

• B. Siciliano, L. Sciavicco, L. Villani, y G. Oriolo. Robotics: modelling, planning

and control. Springer Science & Business Media, 2010 (Chapter 7)

• M.W. Spong, S. Hutchinson, y M. Vidyasagar. Robot Modeling and Control.

John Wiley & Sons, 2006 (Chapter 7)

• Michael, N., Mellinger, D., Lindsey, Q., & Kumar, V. (2010). The GRASP
multiple micro-UAV testbed. Robotics & Automation Magazine, IEEE, 17(3),
56-65.

• Lupashin, S., Hehn, M., Mueller, M. W., Schoellig, A. P., Sherback, M., &
D’Andrea, R. (2014). A platform for aerial robotics research and
demonstration: The Flying Machine Arena. Mechatronics, 24(1), 41-54.

38