ME 597: AUTONOMOUS MOBILE ROBOTICS SECTION 2 –...
Transcript of ME 597: AUTONOMOUS MOBILE ROBOTICS SECTION 2 –...
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
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