Differential-drive kinematics and odometry

Many indoor mobile robots, from research platforms to robot vacuums, drive on two independently powered wheels with one or more passive casters for balance. This layout is called differential drive. It is cheap, it turns on the spot, and its kinematics fit on one line. This lesson is for readers who have met 2-D frames and rotations in lesson 2 and noisy sensors in lesson 6. You will convert between wheel speeds and body motion, integrate wheel encoder readings into a pose estimate (odometry), and see how two small calibration errors make that estimate wander off by most of a metre over three laps of a room.

The robot and its frames

Put the body frame base_link at the midpoint of the axle that joins the two drive wheels. Following REP 103, x points forward, y points to the robot’s left and z points up. The heading theta is the angle of the body x axis measured anticlockwise from the x axis of the world frame. Two numbers describe the geometry: the wheel radius r and the track width b, the distance between the two wheel contact points.

Wheel speeds to body motion and back

If neither wheel slips, each wheel’s contact point moves at its ground speed: v_l = r * omega_l on the left and v_r = r * omega_r on the right, where the omegas are wheel angular speeds in rad/s. The body’s forward speed is the average of the two, and its yaw rate is their difference divided by the track width:

• v = (v_r + v_l) / 2
• omega = (v_r - v_l) / b

Inverting gives the wheel speeds a motor controller needs for a commanded body velocity: v_l = v - omega * b / 2 and v_r = v + omega * b / 2. The pair (v, omega) is the body twist, the same quantity a ROS 2 geometry_msgs/msg/Twist command carries in its linear.x and angular.z fields.

When the wheel speeds differ, the robot drives in a circle of radius v / omega around a point on the axle line called the instantaneous centre of rotation. Equal speeds put that point at infinity, so the robot drives straight. Equal and opposite speeds put it at base_link, so the robot spins on the spot. The block uses a wheel radius of 33 mm and a track width of 160 mm, round numbers for a small indoor robot.

import numpy as np

WHEEL_RADIUS = 0.033   # m
TRACK_WIDTH = 0.160    # m, distance between the wheel contact points

def wheels_to_twist(omega_l: float, omega_r: float, r: float = WHEEL_RADIUS, b: float = TRACK_WIDTH):
    """Wheel angular speeds (rad/s) to body linear speed v (m/s) and yaw rate w (rad/s)."""
    v_l, v_r = r * omega_l, r * omega_r
    return (v_r + v_l) / 2.0, (v_r - v_l) / b

def twist_to_wheels(v: float, w: float, r: float = WHEEL_RADIUS, b: float = TRACK_WIDTH):
    """Body twist to the wheel angular speeds that produce it."""
    return (v - w * b / 2.0) / r, (v + w * b / 2.0) / r

for v_cmd, w_cmd in [(0.2, 0.0), (0.0, 1.0), (0.2, 0.5)]:
    wl, wr = twist_to_wheels(v_cmd, w_cmd)
    v, w = wheels_to_twist(wl, wr)
    radius = v / w if w != 0 else float("inf")
    print(f"v={v_cmd:.2f} m/s w={w_cmd:.2f} rad/s -> wheels L {wl:6.2f} R {wr:6.2f} rad/s "
          f"-> back to v={v:.2f} w={w:.2f}, turn radius {radius:.3f} m")

v=0.20 m/s w=0.00 rad/s -> wheels L   6.06 R   6.06 rad/s -> back to v=0.20 w=0.00, turn radius inf m
v=0.00 m/s w=1.00 rad/s -> wheels L  -2.42 R   2.42 rad/s -> back to v=0.00 w=1.00, turn radius 0.000 m
v=0.20 m/s w=0.50 rad/s -> wheels L   4.85 R   7.27 rad/s -> back to v=0.20 w=0.50, turn radius 0.400 m

The round trip is exact, as it should be for two linear equations. Turning on the spot at 1 rad/s needs only 2.42 rad/s at each wheel, in opposite directions. Driving at 0.2 m/s while turning at 0.5 rad/s gives a 0.4 m turn radius.

The unicycle model

In the world frame, the pose (x, y, theta) changes as

• dx/dt = v * cos(theta)
• dy/dt = v * sin(theta)
• dtheta/dt = omega

This is the unicycle model. It has three state variables and only two inputs, and there is no input that moves the robot along its own y axis. The wheels roll forward and back but cannot slide sideways. A constraint of this kind, on velocity rather than on position, is called non-holonomic. It is why parallel parking takes several manoeuvres: the robot can reach any pose in the plane, but not by moving straight towards it. Planners for wheeled robots have to respect this constraint, which is one reason the grid planner in lesson 8 is only a first step.

Integrating odometry

Odometry is the running estimate of the pose from the wheel encoders alone. Each control cycle you read how far each wheel turned since the last cycle, convert the increments to ds (distance) and dtheta (heading change), and add them to the pose.

The simplest update is an Euler step: move ds along the current heading, then add dtheta. It assumes the heading stayed constant during the step, which is false whenever the robot turns. If instead you assume the wheel speeds were constant during the step, the robot followed a circular arc, and the arc has a closed form. The block compares both against the exact answer for a robot driving at 0.2 m/s and 0.5 rad/s for 12 s.

import numpy as np

def euler_step(pose, v, w, dt):
    x, y, th = pose
    return np.array([x + v * np.cos(th) * dt, y + v * np.sin(th) * dt, th + w * dt])

def arc_step(pose, v, w, dt):
    """Exact integration for constant v and w over dt: the robot follows a circular arc."""
    x, y, th = pose
    if abs(w) < 1e-9:
        return euler_step(pose, v, w, dt)
    th_new = th + w * dt
    return np.array([x + v / w * (np.sin(th_new) - np.sin(th)),
                     y - v / w * (np.cos(th_new) - np.cos(th)),
                     th_new])

v, w, t_end = 0.2, 0.5, 12.0          # 0.4 m turn radius, about 1 lap in 12.57 s
exact_end = arc_step(np.zeros(3), v, w, t_end)   # one big step is exact for constant v, w
print(f"exact pose at {t_end} s: x={exact_end[0]:.4f} y={exact_end[1]:.4f} th={exact_end[2]:.4f}")
for dt in (0.5, 0.1, 0.02):
    pe = np.zeros(3)
    pa = np.zeros(3)
    for _ in range(round(t_end / dt)):
        pe = euler_step(pe, v, w, dt)
        pa = arc_step(pa, v, w, dt)
    err_e = np.hypot(*(pe[:2] - exact_end[:2])) * 1000
    err_a = np.hypot(*(pa[:2] - exact_end[:2])) * 1000
    print(f"dt={dt:5.2f} s  Euler position error {err_e:7.2f} mm   arc error {err_a:.2e} mm")

exact pose at 12.0 s: x=-0.1118 y=0.0159 th=6.0000
dt= 0.50 s  Euler position error   14.12 mm   arc error 1.18e-13 mm
dt= 0.10 s  Euler position error    2.82 mm   arc error 5.43e-12 mm
dt= 0.02 s  Euler position error    0.56 mm   arc error 3.37e-11 mm

The Euler error drops by a factor of 5 when the step drops by a factor of 5, which is what a first-order method does. At 50 Hz (a 0.02 s step) the error after a lap is about half a millimetre, small compared with the errors in the next section. The arc step is exact to rounding error whenever the wheel speeds really were constant over each step. It costs one extra sine and cosine, so there is little reason not to use it. When dtheta is near zero the arc formula divides by almost zero, so the code falls back to the straight-line step.

How calibration errors become drift

The real error budget of wheel odometry is set by the robot, not the integrator. Wheels slip on smooth floors and jump on bumps. Those are random errors. Two systematic errors matter as much: the wheel radius in software is not the true rolling radius (tyres compress, wear and differ from each other), and the track width in software is not the effective distance between the contact patches.

The block drives a real robot around a 2 m square three times, turning on the spot at each corner. The wheel rotations are what the true geometry needs to drive a perfect square. The odometry then integrates those rotations with a wheel radius 0.9 % too large and a track width 3.1 % too small, and prints where it thinks the robot ended up.

import numpy as np

R_TRUE, B_TRUE = 0.0330, 0.160      # what the robot really has
R_ODOM, B_ODOM = 0.0333, 0.155      # what the odometry code believes (+0.9 %, -3.1 %)
SIDE = 2.0                          # m, side of the square the robot drives

def odom_update(pose, d_phi_l, d_phi_r, r, b):
    """Integrate one pair of wheel rotation increments (rad) with the exact-arc formula."""
    x, y, th = pose
    ds_l, ds_r = r * d_phi_l, r * d_phi_r
    ds, dth = (ds_r + ds_l) / 2.0, (ds_r - ds_l) / b
    if abs(dth) < 1e-9:
        return np.array([x + ds * np.cos(th), y + ds * np.sin(th), th])
    rho = ds / dth
    return np.array([x + rho * (np.sin(th + dth) - np.sin(th)),
                     y - rho * (np.cos(th + dth) - np.cos(th)), th + dth])

# The real robot drives a perfect square: straight, turn on the spot 90 deg left, repeat.
# These are the wheel rotations that achieve that with the TRUE radius and track width.
straight = SIDE / R_TRUE
turn = (np.pi / 2) * (B_TRUE / 2) / R_TRUE
moves = [(straight / 2, straight / 2), (straight / 2, straight / 2), (-turn, turn)] * 4

pose = np.zeros(3)
path = [(0.0, 0.0)]
for lap in range(3):
    for d_l, d_r in moves:
        pose = odom_update(pose, d_l, d_r, R_ODOM, B_ODOM)
        if abs(d_l - d_r) < 1e-9:           # record after each half side
            path.append((round(float(pose[0]), 3), round(float(pose[1]), 3)))
    print(f"after lap {lap + 1}: odometry says x={pose[0]:+.3f} m y={pose[1]:+.3f} m "
          f"heading={np.rad2deg(pose[2]) - 360 * (lap + 1):+.1f} deg (truth: 0, 0, 0)")
print("odometry path (x, y):", path)

after lap 1: odometry says x=+0.279 m y=-0.229 m heading=+15.0 deg (truth: 0, 0, 0)
after lap 2: odometry says x=+0.607 m y=-0.378 m heading=+30.0 deg (truth: 0, 0, 0)
after lap 3: odometry says x=+0.963 m y=-0.437 m heading=+45.0 deg (truth: 0, 0, 0)
odometry path (x, y): [(0.0, 0.0), (1.009, 0.0), (2.018, 0.0), (1.952, 1.007), (1.886, 2.014), (0.886, 1.882), (-0.115, 1.751), (0.082, 0.761), (0.279, -0.229), (1.254, 0.032), (2.228, 0.293), (1.904, 1.249), (1.58, 2.204), (0.648, 1.818), (-0.285, 1.432), (0.161, 0.527), (0.607, -0.378), (1.481, 0.126), (2.355, 0.631), (1.795, 1.47), (1.235, 2.309), (0.434, 1.695), (-0.367, 1.081), (0.298, 0.322), (0.963, -0.437)]

After three laps the robot is back where it started, facing the same way. Odometry says it is 1.06 m away and rotated by 45 degrees.

The heading error is the one to watch. In a turn on the spot both wheels travel the same arc in opposite directions, and the computed angle is the true angle times R_ODOM / R_TRUE times B_TRUE / B_ODOM. Here that ratio is 1.0091 times 1.0323, or 1.0417, so each 90 degree turn is read as 93.75 degrees. Four turns per lap give 15 degrees per lap, exactly what the code printed. The radius error appears in that ratio too, so it contributes to the heading error as well as stretching every distance by 0.9 %. The track-width error contributes more than three times as much to the heading. Heading errors are the dangerous kind because every later metre of travel goes in a slightly wrong direction, so the position error they cause keeps growing with distance.

Calibrating both numbers is cheap and worth doing on any new robot. Drive a known straight distance and compare odometry with a tape measure to fix the radius. Spin a known number of whole turns on the spot and compare the odometry heading to fix the track width. Repeat the test in both turning directions, because the two wheels are rarely identical.

Odometry in ROS 2: REP 105 frames

ROS gives the frames involved standard names in REP 105. base_link is fixed to the robot body. odom is a world-fixed frame in which the robot’s pose comes from odometry: it moves smoothly, never jumps, and drifts without bound, exactly as in the chart above. map is a world-fixed frame in which a localisation system (scan matching against a map, or SLAM) keeps the pose free of long-term drift, at the price of sudden jumps when a correction arrives.

The frames form a chain, map to odom to base_link. The odometry node owns the odom to base_link transform and usually also publishes a nav_msgs/msg/Odometry message with the pose, twist and their covariances. The localisation node publishes map to odom, which is whatever correction makes map to base_link right. A controller that needs smooth local motion, such as one following a wall, works in odom. A planner that needs to reach a room across the building works in map. Lesson 10 publishes and looks up exactly this chain with tf2.

Exercise

1. In the third block, set R_ODOM equal to R_TRUE and run three laps. Then restore it and set B_ODOM equal to B_TRUE instead. Which single error produces more drift after three laps, and why?
2. Change moves so the robot drives the square clockwise (turn right at each corner) and compare the final odometry pose with the anticlockwise result. Use the two results to explain why a calibration run should use both directions.
3. Add random slip: multiply each wheel increment by 1 + rng.normal(0, 0.01) with a seeded rng. Run 20 trials of three laps and report the mean and standard deviation of the final position error.

Summary

• A differential-drive robot’s body twist is v = (v_r + v_l) / 2 and omega = (v_r - v_l) / b, with wheel ground speeds equal to the wheel radius times the wheel angular speed.
• The unicycle model has three states and two inputs. The robot cannot move sideways, a non-holonomic constraint.
• Euler integration of odometry has an error proportional to the step. The exact-arc step is exact when wheel speeds are constant over the step and costs one extra sine and cosine.
• Systematic errors in wheel radius and track width dominate odometry drift. Heading errors grow into position errors in proportion to the distance travelled.
• REP 105 puts odometry in the odom frame: continuous but drifting. A localisation system publishes map to odom to remove the drift.

Further reading

• Modern Robotics, Lynch and Park, Cambridge University Press, 2017 (updated first edition, 2019 preprint): chapter 13 covers wheeled mobile robots, including the kinematics of the unicycle and differential drive and odometry.
• Probabilistic Robotics, Thrun, Burgard and Fox, MIT Press, 2005: chapter 5 develops the velocity and odometry motion models with their noise.
• REP 105, Coordinate Frames for Mobile Platforms, Wim Meeussen: the definitions of base_link, odom and map used by ROS 2.