ME 597: AUTONOMOUS MOBILE ROBOTICS SECTION 2 COORDINATE TRANSFORMS. Prof. Steven Waslander

Transcription

1 ME 597: AUTONOMOUS MOILE ROOTICS SECTION 2 COORDINATE TRANSFORMS Prof. Steven Waslander

2 OUTLINE Coordinate Frames and Transforms Rotation Matrices Euler Angles Quaternions Homogeneous Transforms 2

3 COORDINATE FRAMES 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) ody/sensor frame attached to vehicle/sensor, useful for expressing motion/measurements ody origin at vehicle CG, sensor at optical center Y x Z X z y 3

4 COORDINATE FRAMES 2D Rigid ody 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 Y I Z I O X I 4

5 COORDINATE FRAMES Inertial Frame: X Vehicle State: I, Y I [ xy,, ] I Y I y O x X I 5

6 COORDINATE FRAMES ody Frame: Vehicle State: X, Y [0,0,0] Y I y Y X O x X I 6

7 COORDINATE FRAMES ody frame useful for understanding sensor measurements, environment relative to vehicle earing and range to an obstacle: x y object, object, cos sin X Y 7

8 ROTATION MATRICES Conversion between Inertial and ody coordinates is done with a translation vector and a rotation matrix Rotate vectors using 2X2 rotation matrix R I ( ) cos sin sin cos Full transformation is translation and then rotation Y I y O I Y p O x X X I p R ( ) p O I I I I p R ( ) p O I I 8

9 COORDINATE TRANSFORMATION To map the location of the obstacle in a local map, need to transform the current measurement into the map reference frame: I p R ( ) p O I I Y I Y o xr 2 o yr 2.5 X o 1 o x I x x I R o I o yi y yi y 1 r m O r xm 4 r m X I

10 ROTATION MATRICES In fact, the rotation can also be seen as a 3D rotation, about the Z I axis. Y I y cos sin 0 R( ) sin cos O x X I Can be generalized to arbitrary rotations about any axis (Euler angles) Rotation matrices are orthogonal R 1 T ( ) R ( ) 10

11 3D COORDINATE TRANSFORMS 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 x, a homogenous form of the state, x x 1 Combine into a homogeneous transform I I I x I R t x T x

12 TURTLEOT TRANSFORMS I Inertial ody K Kinect K K T I T K Y T I Z I X 12

13 QUADROTOR TRANSFORM EXTREME CASE E Earth Fixed Q Current quadrotor pose C Camera frame M Model frame M d Target fixed frame Q d Desired quadrotor pose Quadrotor inertial pose error equation: E Q E Q C M M d T () t R T T T T () t Q Q C M M d d d Q 13

14 ROTATIONS IN 3D 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. 14

15 EULER ANGLES 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 Euler Angles 15

16 EULER ANGLES Aero convention: Euler Angles Yaw, Pitch, Roll:,, Rotation Matrices 3 - Yaw 2- Pitch 1- Roll cos sin 0 R( ) sin cos cos 0 sin R( ) sin 0 cos R( ) 0 cos sin 0 sin cos 16

17 EULER ANGLES Rotation Matrix ( often Direction Cosine Matrix (DCM)) All three rotations combined cos cos cossin sin RI R,1 R,2 R,3 sin sincos cossin sinsinsin coscos sincos cos sincos sinsin cossinsin sincos coscos Rotate from inertial to body coordinates To rotate from body to inertial, inverse mapping Recall, inverse = transpose R I R I T 17

18 ANGULAR RATE ROTATIONS 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. p q 0 0 cos sin r 0 0 sin cos cos 0 sin0 0 cos sin sin cos sin 0 cos 18

19 ANGULAR RATE ROTATIONS Resulting transformations R e p 1 0 sin q 0 cos cossin r 0 sin coscos R e 1 sintan costan p 0 cos sin q sin cos r 0 cos cos 19

20 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 i j k ijk 1 Everything but commutative multiplication works for quaternions (almost a field)

21 QUATERNIONS Quaternions are a 4-tuple, divided into a scalar and a 3-vector Let i j k Then a quaternion q ( q, q, q, q ) can be written as qq qiq jqk ( q, q) Addition simply adds the elements q p ( q p ) ( q p ) i( q p ) j( q p ) k

22 QUATERNIONS Unit quaternions can be related to an angle (and a vector), which enables them to represent rotations q q 1 cos sin 1 0 Therefore, there must exist an angle (, ] defined by a quaternion q, such that sinθ= q and q q u q sin And we can express the unit quaternion and its conjugate as q cos usin q * cos usin 22

23 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 (0, ω ) The quaternion update equation becomes 1 q q 2 23

24 CONVERSIONS CODED FOR YOU 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

25 EXTRA SLIDES 25

26 QUATERNIONS Quaternions are a 4-tuple, divided into a scalar and a 3-vector Multiplication by a constant cq cq cq icq jcq k The product of two quaternions is defined by Hamilton s rule Which implies i j k ijk 1 ij k ji jk ikj ki j ik

27 QUATERNIONS To get the rule for multiplication, do it out longhand and simplify i j k i j k pq p p p p q q q q Let p ( p, p), q( q, q) 0 0, then r pq pq pq pqqppq Scalar part, r 0 Vector part, r In matrix form, r p p p p q p p p p q pq p p p p q p p p p q

28 QUATERNIONS FOR ROTATIONS Theorem: The quaternion rotation operator R q (v) = qvq* performs a rotation of vector v by 2θ about axis q. v 2 n u a q n R ( ) q n

29 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 2 2( q0 q1) 1 2qq 1 2 2qq 0 3 2qq 1 32qq v' 2qq 1 22qq 0 3 2( q0q2) 1 2qq 2 32qq 0 1 v 2 2 2qq 1 32qq 0 2 2qq 2 3 2qq 0 1 2( q0 q3) 1 Rv