# Computing Odometry from two velocities

I am looking to determine how to compute odometry for my robot. It is a skid-steer platform with one motor on each side (left/right) and each side reports an RPM which is then turned into a velocity.

I have not been able to find any examples of how to use these two velocities to computer the linear and angular components of the twist portion of my odometry message. I have found a way to compute the change in x & y given an angle theta as shown in the RobotSetup/Odom tutorial but I don't know how to get to the theta value.

Any help would be appreciated!

edit retag close merge delete

Sort by » oldest newest most voted

The kinematic equations can be found for example by searching for differential drive robots. E.g. one of the first results: diff-drive kinematics.

Your velocities at the left and right wheel are denoted as v_l and v_r respectively.

Velocities in the robot frame (which are located in the center of rotation, e.g. base_link frame)

v_rx = ( v_right + v_left ) /2;
v_ry = 0; // we have a non-holonomic constraint (for a holonomic robot, this is non-zero)
omega_r = ( v_right - v_left ) / d; // d denotes the distance between both wheels (track)


Subscript r indicates the robot frame.

Note, after converting your encoder values to rotational velocities at each wheel (omega_left, omega_right), you can transform them to the translational velocity at each wheel using the radius of the wheel: v_left = omega_left * radius.

Velocities in the odom frame

The velocities expressed in the robot base frame can be transformed into another coordinate system (e.g. odom). For transforming velocities (in this simple setting with scalar values), we only need to rotate them according to the current robot orientation. Here we apply the 2D rotation matrix (we are operating in the x-y plane). We assume that we already know the current orientation (theta).

v_wx = v_rx * cos(theta) - v_ry * sin(theta);
v_wy = v_rx * sin(theta) + v_ry * cos(theta);


For your robot kinematics it is v_ry=0. Subscript w indicates the world/odom frame.

Computing the current robot pose

In order to obtain the current robot position, numerical integration can be applied. The velocities are integrated in order to achieve the current pose (x, y, theta). We define the pose in the odom frame, and start with x=y=theta=0 (origin of the odom frame).

Applying Explicit/Forward Euler Integration leads to

x_kp1 = x_k + v_wx * dt; // kp1 := k+1
y_kp1 = y_k + v_wy * dt; // by the way, the last term is delta_y from the equations in your comment
theta_kp1 = theta_k + thetadot * dt;


In your implementation, you can simply share the same variables for x_kp1 and x_k etc. dt denotes the step width and should be choosen according to the time elapsed since the last integration step (here you might see, that your rate significantly influences your integration accuracy. But the integration rate is limited by the rate of your sensors (wheel encoders).

The odometry message contains the pose and the twist information. Now you should have everything to compute the values. The pose represents your current robot pose w.r.t the odom frame (x_k, y_k, theta_k). You must convert theta_k to a Quaternion, e.g. by calling the related functions of the tf package. The twist part usually contains the velocities expressed in your robot frame (there is an extra parameter where you can insert the frame name (child_frame_id), but not all drivers are filling it with a value. Here you just need the linear velocity (linear.x = v_rx and the angular velocity (around the z-axis, angular.z = omega_r). All other values are zero.

You can find much information on those things!

• kinematics: just ...
more

Thanks that helped a lot! Do you know how I could break those values out to use in these lines from the Odometry example:

double dt = (current_time - last_time).toSec();
double delta_x = (vx * cos(th) - vy * sin(th)) * dt;
double delta_y = (vx * sin(th) + vy * cos(th)) * dt;

1

Thank you so much with this very helpful walkthrough my robot is now producing usable wheel odometry!

I have been working with the new code and while the forsward and backward tracking seem spot on it doesn't pick up rotation at all

Update: I got it to work by multiply my rotation in z by 100 but I cannot figure out why I needed to do this.

I just have to say this is by far the best answer I've found on this topic. Very complete, yet extremely succinct, with references. Thank you!