An Introduction to

3D Orientations and Quaternions

Autonomous Systems Lab Michael Blösch | 23/09/2015 | 1

The 2D Robotic World

Autonomous Systems Lab Michael Blösch | 23/09/2015 | 2

3D Orientations

WORK CAREFULLY!

Autonomous Systems Lab Michael Blösch | 23/09/2015 | 3

Objectives of this Lecture

 Understand the special characteristics of 3D orientations and be

aware of potential pitfalls
 Know to make use of quaternions for modeling the orientation of
robotic systems
 Understand the basic algebraic tools for 3D orientations and
quaternion and be able to apply them to robotic tasks
 These slides are self-contained and can be used as look-up

Autonomous Systems Lab Michael Blösch | 23/09/2015 | 4

Definition of Orientations

 Given a 3D rigid body with attached coordinate system 𝐵, its orientation 𝑪𝐵𝐼
w.r.t. a reference coordinate system 𝐼, can be defined as the mapping which
maps the coordinates of any fixed vector 𝒓 from 𝐼 to 𝐵, i.e.:
𝐵𝒓 = 𝑪𝐵𝐼 𝐼𝒓 = 𝑹𝐵𝐼 ∙ 𝐼𝒓
 This is independent of the location!

𝒓
1.5
𝑒𝑥𝐼 𝑒𝑥𝐵
𝒓 𝐵𝒓 = −0.2
1.5 𝑒𝑧𝐵 0.8
𝐼𝒓 = 0.8 𝐵
0.2
𝐼 𝑒𝑦𝐼 𝑒𝑦𝐵

𝑒𝑧𝐼
Autonomous Systems Lab Michael Blösch | 23/09/2015 | 5
Properties and Challenges of Orientations

 Orientations are members of SO(3) 𝑪𝐵𝐼 + 𝑪𝑀𝐵

 There is no addition operation on SO(3)
 But there is a multiplication operation
 The multiplication is non-commutative 𝑪𝑀𝐼 = 𝑪𝑀𝐵 ∙ 𝑪𝐵𝐼 ≠ 𝑪𝐵𝐼 ∙ 𝑪𝑀𝐵
 It corresponds to the concatenation of rotations
 There are many different parametrizations for orientations:
 Euler Angles
 Angle Axis
 Rotation matrix
 Unit Quaternions

Autonomous Systems Lab Michael Blösch | 23/09/2015 | 6

Other Parametrizations

 Euler-angles (include Tait-Bryan angles)

 Sequence of rotation around three given axes, e.g., z-x-z or z-y-x (yaw-pitch-roll)
 Many convention, Singularities
𝑹𝐵𝐼 𝛼, 𝛽, 𝛾 = 𝑹𝑥 (𝛼)𝑹𝑦 (𝛽)𝑹𝑧 (𝛾)
 Angle-axis
 Every rotation can be represented as an axis of rotation (𝒂 = 𝐵𝒂 = 𝐼 𝒂) and an angle 𝛼.
 Rodrigues’ Formula (passive form): 𝐵
× 𝑇
𝑒𝑥 𝑒𝑥𝐼
𝑹𝐵𝐼 𝛼, 𝒂 = cos 𝛼 𝑰 − sin 𝛼 𝒂 + (𝟏 − cos 𝛼 )𝒂𝒂
𝛼𝒂
 Rotation matrix
 Linear mapping of vector coordinates 𝑒𝑧𝐼
 Orthonormal matrix 𝑹𝐵𝐼 with det(𝑹𝐵𝐼 ) = 1 𝑒𝑦𝐵
𝐼 𝐼 𝐼
𝑹𝐵𝐼 = 𝐵𝑒𝑥 , 𝐵𝑒𝑦 , 𝐵𝑒𝑧
𝑒𝑧𝐵 𝑒𝑦𝐼

Autonomous Systems Lab Michael Blösch | 23/09/2015 | 7

Quaternions

 Hamilton, 1843
 Extension of complex numbers to 4D
 𝒒 = 𝑞0 + 𝑞1 𝒊 + 𝑞2 𝒋 + 𝑞3 𝒌 = 𝑞0 , 𝑞1 , 𝑞2 , 𝑞3 ∈ 𝐇
 𝒊2 = 𝒋2 = 𝒌2 = 𝒊𝒋𝒌 = −1 (Hamilton convention)
 𝒊2 = 𝒋2 = 𝒌2 = 𝒌𝒋𝒊 = −1 (JPL convention)
 Real and imaginary parts:
 𝑅𝑒 𝒒 = 𝑞0 ∈ 𝑅
 𝐼𝑚 𝒒 = 𝑞1 , 𝑞2 , 𝑞3 = 𝒒 ∈ 𝑅 3
𝑞0
 𝒒= 𝒒

 The multiplication ⊗ directly results from the above

 It is associative and distributive but not commutative!
 The identity quaternion is 𝒒𝑰 = 1, 0, 0, 0 and fulfils 𝒒𝑰 ⊗ 𝒒 = 𝒒 ⊗ 𝒒𝑰 = 𝒒

Autonomous Systems Lab Michael Blösch | 23/09/2015 | 8

Skew Symmetric Matrix of a 3D Vector

 The skew symmetric matrix of 3D vector 𝒓 is defined as

0 −𝑟𝑧 𝑟𝑦
𝒓× = 𝑟𝑧 0 −𝑟𝑥
−𝑟𝑦 𝑟𝑥 0
 It fulfils the following identities for any vectors 𝒂, 𝒃 and rotation matrix 𝑹:
 𝒂 × 𝒃 = 𝒂× 𝒃 = −𝒃× 𝒂
 𝒂× 𝒂 = 𝟎
 (𝑹𝒂)× = 𝑹𝒂× 𝑹𝑇
 𝒂× 𝒂× = 𝒂𝒂𝑇 − 𝒂 2 𝑰

Autonomous Systems Lab Michael Blösch | 23/09/2015 | 9

Quaternion Multiplication

 Quaternion multiplication: 𝒋𝒊 = −𝒌2 𝒋𝒊 = 𝒌

𝒒 ⊗ 𝒑 = 𝑞0 + 𝑞1 𝒊 + 𝑞2 𝒋 + 𝑞3 𝒌 𝑝0 + 𝑝1 𝒊 + 𝑝2 𝒋 + 𝑝3 𝒌
𝒒 ⊗ 𝒑 = 𝑞0 𝑝0 + 𝑞0 𝑝1 𝒊 + 𝑞0 𝑝2 𝒋 + 𝑞0 𝑝3 𝒌
𝒊𝒌 = 𝒋
𝒒 ⊗ 𝒑 = +𝑞1 𝑝0 𝒊 + 𝑞1 𝑝1 𝒊𝒊 + 𝑞1 𝑝2 𝒊𝒋 + 𝑞1 𝑝3 𝒊𝒌 𝒊𝒋 = −𝒌
𝒒 ⊗ 𝒑 = +𝑞2 𝑝0 𝒋 + 𝑞2 𝑝1 𝒋𝒊 + 𝑞2 𝑝2 𝒋𝒋 + 𝑞2 𝑝3 𝒋𝒌 𝒋𝒌 = −𝒊
𝒒 ⊗ 𝒑 = +𝑞3 𝑝0 𝒌 + 𝑞3 𝑝1 𝒌𝒊 + 𝑞3 𝑝2 𝒌𝒋 + 𝑞3 𝑝3 𝒌𝒌
𝒌𝒊 = −𝒋
𝒒 ⊗ 𝒑 = 𝑞0 𝑝0 − 𝑞1 𝑝1 − 𝑞2 𝑝2 − 𝑞3 𝑝3
𝒒 ⊗ 𝒑 = +(𝑞0 𝑝1 + 𝑞1 𝑝0 − 𝑞2 𝑝3 + 𝑞3 𝑝2 )𝒊 𝒌𝒋 = 𝒊
𝒒 ⊗ 𝒑 = +(𝑞0 𝑝2 + 𝑞1 𝑝3 + 𝑞2 𝑝0 − 𝑞3 𝑝1 )𝒋 !!!! JPL !!!
𝒒 ⊗ 𝒑 = +(𝑞0 𝑝3 − 𝑞1 𝑝2 + 𝑞2 𝑝1 + 𝑞3 𝑝0 )𝒌
𝑞0 −𝑞1 −𝑞2 −𝑞3 𝑝0
𝑞1 𝑞0 𝑞3 −𝑞2 𝑝1 𝑞0 − 𝒒𝑇
𝒒 ⊗ 𝒑 = 𝑞 −𝑞 𝑞0 𝑞1 𝑝2 = 𝒒 𝑞 𝑰 − 𝒒× 𝒑 = 𝑴𝒍 𝒒 𝒑
2 3 0
𝑞3 𝑞2 −𝑞1 𝑞0 𝑝3 =:𝑴𝒍 𝒒
𝑝0 −𝑝1 −𝑝2 −𝑝3 𝑞0
𝑝 𝑝 −𝑝3 𝑝2 𝑞1 𝑝0 − 𝒑𝑇
𝒒 ⊗ 𝒑 = 𝑝1 𝑝0 𝑝0 −𝑝1 𝑞2 = 𝒒 = 𝑴𝒓 𝒑 𝒒
2 3 𝒑 𝑝0 𝑰 + 𝒑×
𝑝3 −𝑝2 𝑝1 𝑝0 𝑞3 =:𝑴 𝒑
𝒓

Autonomous Systems Lab Michael Blösch | 23/09/2015 | 10

Unit Quaternions

 The norm of a quaternion is defined as 𝒒 = 𝑞02 + 𝑞12 + 𝑞22 + 𝑞32

 Unit quaternions have norm 1
 The inverse of a unit quaternion is given by its conjugate:
 𝒒−1 = 𝒒𝑇 = 𝑞0 , −𝑞1 , −𝑞2 , −𝑞3
 𝒒−1 ⊗ 𝒒 = 𝒒 ⊗ 𝒒−1 = 𝒒𝑰
 Taking the inverse of a product reverses the order:
 (𝒒 ⊗ 𝐩)−1 = 𝒑−1 ⊗ 𝒒−1

Autonomous Systems Lab Michael Blösch | 23/09/2015 | 11

Unit Quaternions and Orientations

 Unit quaternions can be associated with orientations

𝛼𝒂

𝛼
𝑞0 cos 2
 𝒒 𝛼, 𝒂 = 𝒒 = 𝛼
sin 𝒂
2
𝛼 𝛼
 Check of norm: 𝒒 = 𝑞02 + 𝒒 2 = cos2 + 𝒂 2 sin2 =1
2 2

 We will write 𝒒𝐵𝐼 for the quaternion describing the orientation of 𝐵 w.r.t. 𝐼.

Autonomous Systems Lab Michael Blösch | 23/09/2015 | 12

Rotating Coordinate Vectors with Quaternions

 The pure (imaginary) quaternion of a coordinate vector 𝐼𝒓 is given by

0
𝒑 𝐼𝒓 =
𝐼𝒓

 Given the unit quaternion 𝒒𝐵𝐼 representing the orientation of 𝐵 w.r.t. 𝐼, one
can show that the following holds:
𝒑 𝐵𝒓 = 𝒒𝐵𝐼 ⊗ 𝒑 𝐼𝒓 ⊗ 𝒒𝑇𝐵𝐼

Autonomous Systems Lab Michael Blösch | 23/09/2015 | 13

Rotation Matrix

 Derivation of rotation matrix (𝒒 = 𝒒𝐵𝐼 ):

0
 𝒑 𝐵𝒓 = 𝒒 ⊗ 𝒑 𝐼 𝒓 ⊗ 𝒒𝑇 = 𝑴𝒍 𝒒 𝑴𝒓 (𝒒𝑇 )
𝐼𝒓
0 𝑞 −𝒒𝑇 𝑞0 𝒒𝑇 0
 = 0
𝐵𝒓 𝒒 𝑞0 𝑰 − 𝒒× −𝒒 𝑞0 𝑰 − 𝒒× 𝐼𝒓

0 𝑞02 + 𝒒 2 𝑞0 𝒒𝑇 − 𝑞0 𝒒𝑇 + 𝒒𝑇 𝒒× 0
 =
𝐵𝒓 𝑞0 𝒒 − 𝑞0 𝒒 + 𝒒× 𝒒 𝒒𝒒𝑇 + 𝑞02 𝑰 − 2𝑞0 𝒒× + 𝒒× 𝒒× 𝐼𝒓
0 1 0 0
 = 2
𝐵𝒓 0 𝑞0 − 𝒒 2 𝑰 − 2𝑞0 𝒒× + 𝟐𝒒𝒒𝑇 𝐼 𝒓
 𝐵𝒓 = 𝑞02 − 𝒒 2
𝑰 − 2𝑞0 𝒒× + 𝟐𝒒𝒒𝑇 𝐼𝒓

 𝑹 𝒒 = 2𝑞02 − 1 𝑰 − 2𝑞0 𝒒× + 𝟐𝒒𝒒𝑇

 Combining this with the angle-axis representation yields Rodrigues’ Formula
𝛼 𝛼 𝛼 𝛼
 𝑹 𝒒 𝛼, 𝒂 = 2 cos2 − 1 𝑰 − 2 cos sin 𝒂× + 𝟐 sin2 𝒂𝒂𝑇
2 2 2 2
= cos 𝛼 𝑰 − sin 𝛼 𝒂× + (𝟏 − cos 𝛼 )𝒂𝒂𝑇
Autonomous Systems Lab Michael Blösch | 23/09/2015 | 14
Why Quaternions?

 Euler-Angle, Angle-Axis
 Singularities!
 Rotation Matrix
 9 scalars, more complex regularization
 Concatenation: 27 multiplications
 Rotating a vector: 9 multiplications
 Quaternion
 4 scalars, easy regularization
 Concatenation: 16 multiplications
 Rotating a vector: 18 multiplications

Autonomous Systems Lab Michael Blösch | 9/23/2015 | 15

Small Angle Approximations

 Linear approximations for small angles:

𝛼
cos 1
2
 lim 𝒒 𝛼, 𝒂 = lim 𝛼
= 𝛼
𝒂
𝛼→0 𝛼→0
sin 𝒂 2
2
×
 lim 𝑹 𝒒 𝛼, 𝒂 = 𝑰 − 𝛼𝒂
𝛼→0
 For smooth motions we also have:
1
𝐵𝝎𝐼𝐵 𝑡
 lim 𝒒𝐵 𝑡+∆𝑡 𝐵(𝑡) = lim 𝒒 ∆𝑡 𝐵𝝎𝐼𝐵 𝑡 , = lim 𝒒 ∆𝑡
∆𝑡→0 ∆𝑡→0 𝐵𝝎𝐼𝐵 𝑡 ∆𝑡→0 𝝎 𝑡
2 𝐵 𝐼𝐵
𝐵 𝑡+∆𝑡 𝐵(𝑡)
𝑒𝑥 𝑒𝑥
∆𝑡 𝐵𝝎𝐼𝐵 (𝑡)
𝐵(𝑡)
𝑒𝑧 𝐵 𝑡+∆𝑡
𝑒𝑦

𝐵 𝑡+∆𝑡
Autonomous Systems Lab
𝑒𝑧 𝑒𝑦
𝐵(𝑡)
Michael Blösch | 23/09/2015 | 16
Time Derivatives of Unit Quaternions

 Given the local rotational velocity 𝐵𝝎𝐼𝐵 (𝑡) of the coordinate system 𝐵 w.r.t.
coordinate system 𝐼, we can derive:
1 1
∆𝑡 ⊗ 𝒒𝐵𝐼 (𝑡) − ⊗ 𝒒𝐵𝐼 (𝑡)
𝒒𝐵𝐼 𝑡 + ∆𝑡 − 𝒒𝐵𝐼 (𝑡) 𝝎
𝐵 𝐼𝐵 𝑡 𝟎
𝒒𝐵𝐼 (𝑡) = lim = lim 2
∆𝑡→0 ∆𝑡 ∆𝑡→0 ∆𝑡
0
∆𝑡 ⊗ 𝒒𝐵𝐼 𝑡
𝐵 𝝎𝐼𝐵 𝑡 1 0
𝒒𝐵𝐼 𝑡 = lim 2 = ⊗ 𝒒𝐵𝐼 𝑡
∆𝑡→0 ∆𝑡 2 𝐵𝝎𝐼𝐵 𝑡
1 0 1
𝒒𝐵𝐼 𝑡 = 𝑴𝒍 𝒒𝐵𝐼 𝑡 = 𝜴( 𝐵𝝎𝐼𝐵 𝑡 )𝒒𝐵𝐼 𝑡
2 𝐵𝝎𝐼𝐵 𝑡 2
=:𝜴( 𝐵𝝎𝐼𝐵 𝑡 )

Autonomous Systems Lab Michael Blösch | 23/09/2015 | 17

Relation to Local Rotational Velocity

 A relation to the local rotational velocity is given by:

1 0 1 0
𝒒𝐵𝐼 𝑡 = ⊗ 𝒒𝐵𝐼 𝑡 = 𝑴𝒓 𝒒𝐵𝐼 𝑡
2 𝐵𝝎𝐼𝐵 𝑡 2 𝐵𝝎𝐼𝐵 𝑡
1 −𝒒𝐵𝐼 𝑇 1
𝒒𝐵𝐼 𝑡 = 𝐵𝝎𝐼𝐵 𝑡 = 𝜩(𝒒𝐵𝐼 ) 𝐵𝝎𝐼𝐵 𝑡
2 𝑞𝐵𝐼,0 𝑰 + 𝒒𝐵𝐼 × 2

=:𝜩(𝒒𝐵𝐼 )

 Which can also be inverted by considering 𝜩𝑇 𝒒𝐵𝐼 𝜩 𝒒𝐵𝐼 = 𝑰:

𝐵𝝎𝐼𝐵 𝑡 = 2𝜩𝑇 (𝒒𝐵𝐼 )𝒒𝐵𝐼 𝑡

Autonomous Systems Lab Michael Blösch | 9/23/2015 | 18

Recapitulation

 Specific attributes of orientations

 Definition of quaternions
 Multiplication, identity quaternion
 Norm of quaternions, unit quaternion
 Relation between unit quaternions and orientations
 Rotation of coordinate vectors, derivation of rotation matrix
 Small angle approximations, time derivatives of quaternions

Autonomous Systems Lab Michael Blösch | 23/09/2015 | 19