Coordinate Frames Introduction

When representing the position and orientation of a rigid body in space, we need to establish a common frame of reference in which coordinates and orientations are written. Combining and transforming between different coordinate frames allows us to define positions in convenient ways and represent the robot pose correctly.

For Kingfisher, we define an unmoving spatial or world frame as well as a robot frame and sensor frames within which positions are reported. We can represent positions and orientations with matrices and can use matrix transformations to convert between two different coordinate frames. For more information on the mathematics behind frames and transforms, see "Configuration Space" and "Rigid-Body Motions" in Modern Robotics (Lynch).

Important Coordinate Frames

world frame

The world frame is the unmoving frame which we fix somewhere in space (like in the corner of the pool or edge of the water tank, for example) for the duration of a water test or run.

odom frame

The odom frame defines where the origin of state estimation is. In other words, when state estimation is turned on but before the robot has moved, the state estimation node will report that we are at the origin, $(0,0)$.

vehicle and sensor frames

The vehicle frame is rigidly attached to the submarine. Similarly, a sensor frame is rigidly attached to a sensor onboard the vehicle and does not move with respect to the vehicle frame. For example, the oakd_bottom frame is rigidly attached to the bottom OAK-D camera and does not move w.r.t the vehicle frame.

Important Coordinate Frame Transforms

world->odom

The world->odom transform describes, in world coordinates, where the odom frame origin is. Physically, this represents where the submarine started state estimation w.r.t the world origin. We can also use this transform to account for state estimation drift. This means that, if we detect state estimation drift (either in translation, rotation, or both), we can account for this drift by changing the world->odom transform to correct the world frame representation of the submarine's position. The choice to change specifically world->odom to account for drift is just convention and works well in practice.

odom->vehicle

The odom->vehicle is the transform reported by state estimation. This transform describes, in odom coordinates, where the submarine is w.r.t the origin of the odom frame.

world->vehicle

The world->vehicle transform describes, in world coordinates, where the submarine is w.r.t the origin of the world frame. Knowing this transform is useful for trajectory planning, since we mostly plan trajectories in world coordinates, so knowing where the submarine is in world coordinates is necessary for controlling the vehicle. We interpret world->vehicle as the "ground truth" position of the vehicle which has accounted for any detected drift in state estimation.

Examples

Example 1.1: No Drift in State Estimation

Coordinates are written in terms of $(x, y, \theta)$. We set the origin of the world frame to be the bottom left corner of the pool, $(0,0,0^{\circ})_w$. We then throw the submarine in the water and start state estimation at $(12, 0, 0^{\circ})_w$. The transform which describes the origin of the odom frame is written as world->odom = (12, 0, 0) We drive the sub around to the position $(20, 20, -45^{\circ})_w [^1]$. The transform which describes where the submarine is w.r.t odom is written as odom->vehicle = (8, 20, -45). The transform which describes where the submarine is w.r.t world is written as world->vehicle = (20, 20, -45).

Note that we do not directly measure world->vehicle, and we instead only get odom->vehicle from state estimation. Since we know world->odom, we can get world->vehicle by left-multiplying the odom->vehicle transform by the world->odom transform. (Try this for yourself to verify)

$[^1]$: The angle $\theta$ is negative because of the right hand rule. Look up how to use the RHR to determine positive and negative rotations -- its very important!!!

Example 2.1: Drift in State Estimation

Continuing example 1.1, let's say that state estimation has gathered significant drift in heading, but the translational components of our pose are okay. Let's call the submarine's current pose $(x, y, \theta)_w$ We want to "re-zero" our heading by putting the submarine in a known orientation, like flat up against the wall facing true 0 degrees, and then telling the coordinate system to interpret the submarine's current orientation as a 0 degree heading, ignoring whatever heading it was previously reporting.

We can perform "re-zeroeing" by changing the world->odom offset. For this example, let's say the submarine's heading has drifted by 15 degrees. This means that if the submarine reported a 0 degree heading, it is really at a 15 degree heading. We know $T_{w,o} T_{o,v} = T_{w, v}$, so $T_{w,o} = T_{w,v}' (T_{o,v})^{-1}$, where $T_{w,v}'$ is the desired world->vehicle transform.

Note: we are writing the transformations as 3-tuples when in reality they are $3\times3$ homogenous matrices -- we are just extracting the $x, y, \theta$ from the matrix to write it in a more compact form).