ME 597: AUTONOMOUS MOBILE ROBOTICSSECTION 2 – COORDINATE TRANSFORMS

Prof. Steven Waslander

Coordinate Frames and Transforms Rotation Matrices Euler Angles Quaternions Homogeneous Transforms

2

OUTLINE

Used to define environment, vehicle motion Right-handed by convention

Inertial frame – fixed, usually relative to the earth GPS: Earth Centered Earth Fixed (ECEF), Latitude,

Longitude, Altitude (LLA), East North Up (ENU) Aeronautics: North East Down (NED)

Body/sensor frame – attached to vehicle/sensor, useful for expressing motion/measurements Body origin at vehicle CG, sensor at optical center

3

COORDINATE FRAMES

X

Y

Z

x

y

z

2D Rigid Body Motion For ground robots, two dimensional motion definition

often enough If robot has a heading, third axis is implicit Right hand rule defines direction of rotation

4

COORDINATE FRAMES

O

IY

IX

IZ

Inertial Frame: Vehicle State:

5

COORDINATE FRAMES

O

IY

IX

x

y

,I IX Y

[ , , ]I x y

Body Frame: Vehicle State:

6

COORDINATE FRAMES

O

BY BX

x

,B BX Y

O

IY

IX

x

y

[0,0,0]B

Body frame useful for understanding sensor measurements, environment relative to vehicle Bearing and range to an obstacle:

7

COORDINATE FRAMES

BYBX

,

,

cossin

object B

object B

xy

BYBX

Conversion between Inertial and Body coordinates is done with a translation vector and a rotation matrix Rotate vectors using 2X2

rotation matrix

Full transformation is translation and then rotation

8

ROTATION MATRICES

cos sin( )

sin cosBIR

( )

( )

B IB I I B

I BI B B I

p R p O

p R p O

OI

IY

IXx

y

p

OB

To map the location of the obstacle in a local map, need to transform the current measurement into the map reference frame:

9

COORDINATE TRANSFORMATION

BYBX

O

IY

IX4rmx

1rmy

2orx

2.5ory

60rm

1

0.5 0.866 2 40.866 .5 2.5 1

o o BI B IB

Io o BI B I

x x xR

y y y

1.165 42.982 1

2.8353.982

( )I BI B B Ip R p O

In fact, the rotation can also be seen as a 3D rotation, about the ZI axis.

Can be generalized to arbitrary rotations about any axis (Euler angles)

Rotation matrices are orthogonal

10

ROTATION MATRICES

cos sin 0( ) sin cos 0

0 0 1R

O

IY

IXx

y

1( ) ( )TR R

Often handy to concisely define a transformation between coordinate frames Define

t, the translation vector between the origins of the two frames

R, a 3X3 rotation matrix from one frame to the next , a homogenous form of the state,

Combine into a homogeneous transform

11

3D COORDINATE TRANSFORMS

1x

x

x

1 0 1 1

I I I BI B B B

Bx R t x

T x

I – Inertial B – Body K – Kinect

12

TURTLEBOT TRANSFORMS

X

Y

ZI

B

K

BKT

BIT

KIT

E – Earth Fixed Q – Current

quadrotor pose C – Camera frame M – Model frame Md – Target fixed

frame Qd – Desired

quadrotor pose

Quadrotor inertial pose error equation:

13

QUADROTOR TRANSFORM – EXTREME CASE

( ) ( )d

d d d

MQ E Q C MQ Q C M

E

M QT t R T T T T t

There are at least 3 ways to represent rotations in 3D Euler angles

Intuitive, easy to understand, sequence of rotations Have singularity known as “gimbal lock” where rotation

cannot be properly represented Quaternions

Represents rotations as a 4 element unit quaternion Easy to update, no singularities Requires unit norm, not intuitive

Rotation Matrix Complete, exact, unique, symmetric 3X3 matrix Also easy to update, no singularities Has to have a unit determinant, not intuitive

Others include Rodriguez, modified Rodriguez, etc.

ROTATIONS IN 3D

14

Given First Axes (xyz), rotate to Second Axes (XYZ) through 3 successive rotations, using 3D rotation matrix. Rotation 1: About z by alpha

Rotation 2: About N by beta

Rotation 3: About Z by gamma

Known as 3-1-3 Euler Angles

15

EULER ANGLES

Aero convention: 3-2-1 Euler Angles Yaw, Pitch, Roll:

Rotation Matrices 3 - Yaw

2- Pitch

1- Roll

16

EULER ANGLES

, ,

cos sin 0( ) sin cos 0

0 0 1R

cos 0 sin( ) 0 1 0

sin 0 cosR

1 0 0( ) 0 cos sin

0 sin cosR

Rotation Matrix ( often Direction Cosine Matrix (DCM)) All three rotations combined

Rotate from inertial to body coordinates

To rotate from body to inertial, inverse mapping Recall, inverse = transpose

17

EULER ANGLES

,1 ,2 ,3

cos cos cos sin sinsin sin cos cos sin sin sin sin cos cos sin coscos sin cos sin sin cos sin sin sin cos cos cos

BIR R R R

TI BB IR R

Angular rates measured in body frame (p,q,r) Euler angles are measured relative to multiple

intermediate coordinate frames (3-2-1), Euler rates used to update Euler angles in motion

Not a rotation matrix Cannot simply transpose for inverse.

18

ANGULAR RATE ROTATIONS

1 0 0 00 0 cos sin0 0 sin cos 0

1 0 0 cos 0 sin 00 cos sin 0 1 0 00 sin cos sin 0 cos

pqr

Resulting transformations

19

ANGULAR RATE ROTATIONS

1 0 sin0 cos cos sin0 sin cos cos

pqr

1 sin tan cos tan0 cos sin

sin cos0cos cos

pqr

eR

eR

QUATERNIONS

An alternative way of representing rotations is through quaternions Hamilton (1843) was looking

for a field of dimension 4 Reals are a field of dimension 1,

complex are a field of dimension 2 While walking with his wife in

Dublin, scribbled the rule of quaternions on a bridge so he would not forget it.

Everything but commutative multiplication works for quaternions (almost a field)

2 2 2 1 i j k ijk

QUATERNIONS

Quaternions are a 4-tuple, divided into a scalar and a 3-vector Let

Then a quaternion can be written as

Addition simply adds the elements

1 0 0

0 1 0

0 0 1

i

j

k

4

0 1 2 3( , , , )q q q q q

0 1 2 3 0( , )q q q q q q i j k q

0 0 1 1 2 2 3 3( ) ( ) ( ) ( )q p q p q p q p q p i j k21

QUATERNIONS

Unit quaternions can be related to an angle (and a vector), which enables them to represent rotations

Therefore, there must exist an angle defined by a quaternion q, such that sinθ=||q|| and

And we can express the unit quaternion and its conjugate as

2 2

0|| || 1q q 2 2cos sin 1

( , ]

|| || sin q quq

*

cos sincos sin

qq

uu

22

QUATERNION UPDATE EQUATIONS

Similar to the Euler angle update, quaternions can be updated directly from body rotation rates

If you measure the body rotation rate and form a quaternion version

The quaternion update equation becomes

(0, )B B ω

12 B

q q

23

Matlab code to switch between representations now included in code package Converts between Rotation Matrix, Quaternion, Euler

angles and Euler vector, angle representations

OUTPUT=SpinCalc(CONVERSION,INPUT,tol,ichk)

Simple code to create 3D rotation matrices

rot.m rotates by an angle about one of 3 principle axes

ROS uses primarily quaternions, but also has built in conversion functions

Geometry/RotationMethods 24

CONVERSIONS – CODED FOR YOU

EXTRA SLIDES

25

QUATERNIONS

Quaternions are a 4-tuple, divided into a scalar and a 3-vector Multiplication by a constant

The product of two quaternions is defined by Hamilton’s rule

Which implies

0 1 2 3cq cq cq cq cq i j k

2 2 2 1 i j k ijk

ij k jijk i kjki j ik

QUATERNIONS

To get the rule for multiplication, do it out longhand and simplify

Let , then

In matrix form,

0 1 2 3 0 1 2 3pq p p p p q q q q i j k i j k

0 0( , ), ( , )p p q q p q

0 0 0 0r pq p q p q p q q p p q

0 1 2 3 0

1 0 3 2 1

2 3 0 1 2

3 2 1 0 3

p p p p qp p p p q

r pqp p p p qp p p p q

Scalar part, r0 Vector part, r

QUATERNIONS FOR ROTATIONS

Theorem: The quaternion rotation operator Rq(v) = qvq*

performs a rotation of vector v by 2θ about axis q.

v

qna

( )qR n

2

n

u

QUATERNIONS FOR ROTATIONS

So now we have a physical interpretation of the quaternion as a combination of a rotation axis q and a rotation angle 2θ

We can write the rotation operator in matrix form and extract a conversion to a rotation matrix

2 20 1 1 2 0 3 1 3 0 2

2 21 2 0 3 0 2 2 3 0 1

2 21 3 0 2 2 3 0 1 0 3

2( ) 1 2 2 2 2' 2 2 2( ) 1 2 2

2 2 2 2 2( ) 1

q q q q q q q q q qq q q q q q q q q qq q q q q q q q q q

R

v v

v