#### 13.4. Odometry

This video introduces odometry for omnidirectional and nonholonomic wheeled mobile robots: estimating the motion of the robot’s chassis from the wheel motions.

Odometry is the process of estimating the chassis configuration from wheel motions, essentially integrating the effect of the wheel velocities. Since wheel-rotation sensing is available on all mobile robots, odometry is convenient. Odometry errors tend to accumulate over time, though, due to slipping or skidding of the wheels and numerical integration error. For this reason, odometry is usually supplemented with estimation techniques using exteroceptive sensors, like cameras, range sensors, and GPS.

This video focuses on odometry. In this video, the 6-dimensional twist of the chassis, expressed in the chassis frame {b}, is written V_b6. Since the chassis motion is planar, three of the components are zero. The remaining three velocities in the plane are collectively written as V_b, a planar twist.

With this as background, the odometry process consists of the following steps.

First, we measure the wheel rotations Delta theta since the last timestep, typically using encoders.

Next, we assume that the wheel velocities were constant since the last encoder readings at time Delta t in the past. Because it doesn't matter which units we use to measure time, we can assume Delta t is equal to 1 unit, and therefore theta-dot is equal to Delta theta.

Next, we find the chassis planar twist V_b that corresponds to Delta theta.

Next, we use the matrix exponential to integrate the corresponding 6-dimensional twist V_b6 for time Delta t equal to 1 to find the configuration of the chassis frame at step k-plus-1 relative to the chassis frame at step k.

Finally we express the new chassis frame relative to the space frame.

Since the other steps are straightforward, let's focus on the third step, finding the matrix F relating Delta theta to V_b.

For a car or a diff-drive, the chassis frame {b} is midway between the wheels. The radius of the wheels is r, and positive rotation of the wheels corresponds to forward motion at those wheels. A little geometry shows that the planar twist provided by unit velocity of the left wheel has minus-r over 2d in angular velocity and r over 2 in the x_b direction.
The planar twist for a unit velocity of the right wheel is similar, but with an opposite angular component. The 3-by-2 F matrix relates the wheel increments to the chassis twist V_b.

For omnidirectional mobile robots, the wheel speeds theta-dot are related to the chassis twist V_b by the matrix H of zero, as we learned in a previous video. We can invert this relationship to get V_b equals H-pseudoinverse times theta-dot, which we can write equivalently as F theta-dot or F Delta theta.

For the three omniwheel robot, inverting the H matrix we found in an earlier video produces this 3-by-3 F matrix relating Delta theta to V_b.

For the four mecanum wheel robot, pseudo-inverting the H matrix produces this 3-by-4 matrix.

With the F matrix and the wheel increment Delta theta, we get the planar chassis twist V_b. Expressing this as the 6-dimensional twist V_b6, we can use our standard matrix exponential to calculate the configuration of the chassis frame at step k-plus-1 relative to the configuration at step k. To express this in the space frame, we premultiply the matrix exponential by the transformation matrix representing the chassis frame at step k relative to the space frame. We can then extract the coordinates q_k-plus-1 from T_{s b_k-plus-1}.

Equivalently, we can convert the matrix exponential to an increment of coordinates expressed in the chassis frame, rotate the x and y components to an increment in the space frame, and add the increment to the previous chassis configuration.

In the final video of this chapter, we combine a mobile robot with a manipulator and consider their coordinated control.