Notebook

Odometry-based Localization¶

for the differential-drive robot¶

Localization is based on the kinematics model of the robot and the readings of its wheel encoders. The basic steps for implementing it are:

We start from a known pose: for example, (𝑥=0,𝑦=0, 𝜑=0).
By counting encoder pulses, we calculate the angular speeds of each wheel 𝜔_𝑟, 𝜔_𝑙.
The linear and angular speeds of the robot (𝑢, 𝜔) are calculated.
The displacement of the robot (∆𝑥,∆𝑦, ∆𝜑) is obtained.
Finally, the values of position 𝑥, 𝑦 and orientation 𝜑 are updated at every cycle by integrating the results.

Assuming the encoder readings are stored in the lists encoderValues and oldEncoderValues, the following function can be used to calculate the speed of each wheel. The variable delta_t contains the time interval (in seconds) between function calls, and pulses_per_turn indicates how many pulses each encoder generates per full turn of each wheel.

In [1]:

import numpy as np

In [2]:

def get_wheels_speed(encoderValues, oldEncoderValues, pulses_per_turn, delta_t):
    """Computes speed of the wheels based on encoder readings
    """
    # Calculate the change in angular position of the wheels:
    ang_diff_l = 2*np.pi*(encoderValues[0] - oldEncoderValues[0])/pulses_per_turn
    ang_diff_r = 2*np.pi*(encoderValues[1] - oldEncoderValues[1])/pulses_per_turn

    # Calculate the angular speeds:
    wl = ang_diff_l/delta_t
    wr = ang_diff_r/delta_t

    return wl, wr

Test the function with different encoder values and delta_t.

In [3]:

pulses_per_turn = 72
delta_t = 0.1  # time step in seconds
encoderValues = [1506, 1515]  # Accumulated number of pulses for the left [0] and right [1] encoders.
oldEncoderValues = [1500, 1500]     # Accumulated pulses for the left and right encoders in the previous step

wl, wr = get_wheels_speed(encoderValues, oldEncoderValues, pulses_per_turn, delta_t)

print(f'Left wheel speed  = {wl} rad/s.')
print(f'Right wheel speed = {wr} rad/s.')

Left wheel speed  = 5.235987755982988 rad/s.
Right wheel speed = 13.089969389957469 rad/s.

The function below calculates the linear and angular speeds of the robot based on the speeds of its wheels and its physical parameters: R is the radius of the wheels and D is the distance between the left and right wheels.

In [4]:

def get_robot_speeds(wl, wr, R, D):
    """Computes robot linear and angular speeds"""
    u = R/2.0 * (wr + wl)
    w = R/D * (wr - wl)
    
    return u, w

In [5]:

# Physical parameters of the robot for the kinematics model
R = 0.10    # radius of the wheels of the e-puck robot: 20.5mm 
D = 0.40    # distance between the wheels of the e-puck robot: 52mm

u, w = get_robot_speeds(wl, wr, R, D)

print(f"Robot linear speed  = {u} m/s")
print(f"Robot angular speed = {w} rad/s")

Robot linear speed  = 0.9162978572970228 m/s
Robot angular speed = 1.9634954084936203 rad/s

Finally, the new robot pose can be calculated based on the kinematics model, robot speed and previous pose.

In [6]:

def get_robot_pose(u, w, x_old, y_old, phi_old, delta_t):
    """Updates robot pose based on heading and linear and angular speeds"""
    delta_phi = w * delta_t
    phi = phi_old + delta_phi
    
    if phi >= np.pi:
        phi = phi - 2*np.pi
    elif phi < -np.pi:
        phi = phi + 2*np.pi

    delta_x = u * np.cos(phi) * delta_t
    delta_y = u * np.sin(phi) * delta_t
    x = x_old + delta_x
    y = y_old + delta_y
    
    return x, y, phi

Test the function with different speeds and poses.

In [7]:

x_old, y_old, phi_old = 2.0, 4.0, -np.pi/2  # Robot pose in the previous step
u = 0.2         # m/s
w = 0.15        # rad/s
delta_t = 0.1   # s

x, y, phi = get_robot_pose(u, w, x_old, y_old, phi_old, delta_t)

print(f"The new robot pose is: {x:.3f} m, {y:.3f} m, {phi*180/np.pi:.3f} deg.")

The new robot pose is: 2.000 m, 3.980 m, -89.141 deg.

Conclusion¶

After completing this notebook, you should understand how to build functions to implement odometry-based localization for a differential-drive robot.