Differential Drive Kinematics: How Two-Wheeled Robots Move
The differential drive is the most common locomotion system in mobile robotics. From the Roomba vacuuming your floor to the TurtleBot in your university lab to million-dollar warehouse AGVs, differential drive robots are everywhere. The reason is simplicity: two independently driven wheels and an optional caster for balance. No steering linkage, no complex gearbox, no omnidirectional rollers.
But simplicity in hardware means you need to understand the math. How do you convert desired robot motion (move forward at 0.5 m/s while turning left) into individual wheel speeds? How do you estimate the robot's position from wheel encoder readings? These are the forward and inverse kinematics problems for differential drive, and this tutorial covers both with derivations, intuition, and Python code.
What Is a Differential Drive Robot?
A differential drive robot has two wheels mounted on a common axis, each driven by its own motor. The robot steers by varying the relative speed of the two wheels:
- Both wheels at the same speed: The robot drives straight.
- Left wheel faster than right: The robot curves to the right.
- Right wheel faster than left: The robot curves to the left.
- Wheels at equal speed in opposite directions: The robot spins in place.
The distance between the two wheel contact points is called the wheel base (or track width), denoted L. Each wheel has radius r. These two parameters, along with the wheel angular velocities, completely determine the robot's motion.
A third contact point (a caster wheel, ball caster, or skid) prevents the robot from tipping but plays no role in the kinematics.
The Robot's State
We describe the robot's position and orientation in a 2D plane using three variables:
- x: position along the world x-axis (meters)
- y: position along the world y-axis (meters)
- theta: heading angle measured from the positive x-axis (radians)
Together, (x, y, theta) is the robot's pose. The kinematics equations tell us how the pose changes over time as a function of wheel velocities.
Forward Kinematics: From Wheels to Robot
Forward kinematics answers the question: given the angular velocities of the left and right wheels, what is the resulting linear and angular velocity of the robot?
Let omega_L and omega_R be the angular velocities (rad/s) of the left and right wheels, respectively. The linear velocity of each wheel's contact point with the ground is:
v_L = r * omega_L
v_R = r * omega_R
The robot's linear velocity v (at the midpoint between the wheels) and angular velocity omega are:
v = (v_R + v_L) / 2
omega = (v_R - v_L) / L
These two equations are the core of differential drive forward kinematics. The linear velocity is the average of the two wheel velocities. The angular velocity is the difference divided by the wheel base.
The Instantaneous Center of Curvature (ICC)
When the two wheels spin at different speeds, the robot follows a circular arc. The center of this arc is called the Instantaneous Center of Curvature (ICC). It lies along the line connecting the two wheels, at a distance R from the midpoint:
R = v / omega = (L / 2) * (v_R + v_L) / (v_R - v_L)
When v_R = v_L (straight-line motion), R approaches infinity -- the robot follows a circle of infinite radius, which is a straight line. When v_R = -v_L (pure rotation), R = 0 -- the ICC is at the midpoint between the wheels, and the robot spins in place.
The ICC position in world coordinates is:
ICC_x = x - R * sin(theta)
ICC_y = y + R * cos(theta)
Understanding the ICC is valuable for trajectory prediction. If you know the current wheel speeds, you can predict the circular arc the robot will follow and determine where it will be after any time interval.
Pose Update Equations
Given wheel velocities and a small time step dt, the robot's pose updates as follows.
When omega is not zero (curved motion):
x_new = x + v/omega * (sin(theta + omega*dt) - sin(theta))
y_new = y + v/omega * (cos(theta) - cos(theta + omega*dt))
theta_new = theta + omega * dt
When omega is approximately zero (straight-line motion):
x_new = x + v * cos(theta) * dt
y_new = y + v * sin(theta) * dt
theta_new = theta
The straight-line case is needed because the curved-motion formula involves division by omega, which is undefined when omega is zero. In practice, you check whether |omega| is below a small threshold (e.g., 1e-6) and use the appropriate formula.
Euler Approximation
For small dt, a simpler Euler integration works well and is used in most real-time systems:
theta_new = theta + omega * dt
x_new = x + v * cos(theta_new) * dt
y_new = y + v * sin(theta_new) * dt
This approximation introduces some drift over long distances but is adequate for control loops running at 50 Hz or faster, especially when combined with sensor corrections.
Inverse Kinematics: From Robot to Wheels
Inverse kinematics answers the opposite question: given a desired linear velocity v and angular velocity omega for the robot, what wheel angular velocities are needed?
Rearranging the forward kinematics equations:
v_L = v - (omega * L) / 2
v_R = v + (omega * L) / 2
And converting to wheel angular velocities:
omega_L = v_L / r = (v - omega * L / 2) / r
omega_R = v_R / r = (v + omega * L / 2) / r
This is how a motion controller works. The navigation stack outputs a desired (v, omega) command (often called a Twist message in ROS). The low-level controller converts it to individual wheel speeds using these equations, then commands each motor accordingly.
Velocity Constraints
Real motors have maximum speeds. If the computed wheel velocity exceeds the motor's limit, you need to scale both wheels proportionally to maintain the desired turning radius:
def constrain_velocities(omega_l, omega_r, max_omega):
"""Scale wheel velocities to respect motor limits while preserving ratio."""
max_abs = max(abs(omega_l), abs(omega_r))
if max_abs > max_omega:
scale = max_omega / max_abs
omega_l *= scale
omega_r *= scale
return omega_l, omega_rThis preserves the ratio between wheel speeds, so the robot follows the intended arc at a reduced speed rather than changing direction.
Python Implementation
Here is a complete differential drive kinematics class with forward kinematics, inverse kinematics, odometry integration, and a simulation loop:
import math
class DifferentialDrive:
"""Differential drive kinematics model."""
def __init__(self, wheel_radius: float, wheel_base: float):
"""
Args:
wheel_radius: Radius of each wheel in meters.
wheel_base: Distance between wheel contact points in meters.
"""
self.r = wheel_radius
self.L = wheel_base
# Robot pose
self.x = 0.0
self.y = 0.0
self.theta = 0.0
# Velocity state
self.v = 0.0
self.omega = 0.0
def forward_kinematics(
self, omega_left: float, omega_right: float
) -> tuple[float, float]:
"""
Compute robot velocity from wheel angular velocities.
Args:
omega_left: Left wheel angular velocity (rad/s).
omega_right: Right wheel angular velocity (rad/s).
Returns:
(v, omega): Linear velocity (m/s) and angular velocity (rad/s).
"""
v_left = self.r * omega_left
v_right = self.r * omega_right
v = (v_right + v_left) / 2.0
omega = (v_right - v_left) / self.L
return v, omega
def inverse_kinematics(
self, v: float, omega: float
) -> tuple[float, float]:
"""
Compute wheel angular velocities from desired robot velocity.
Args:
v: Desired linear velocity (m/s).
omega: Desired angular velocity (rad/s).
Returns:
(omega_left, omega_right): Wheel angular velocities (rad/s).
"""
v_left = v - (omega * self.L) / 2.0
v_right = v + (omega * self.L) / 2.0
omega_left = v_left / self.r
omega_right = v_right / self.r
return omega_left, omega_right
def icc(self) -> tuple[float, float] | None:
"""
Compute the Instantaneous Center of Curvature.
Returns:
(icc_x, icc_y) or None if the robot is moving straight.
"""
if abs(self.omega) < 1e-9:
return None
radius = self.v / self.omega
icc_x = self.x - radius * math.sin(self.theta)
icc_y = self.y + radius * math.cos(self.theta)
return icc_x, icc_y
def update(self, omega_left: float, omega_right: float, dt: float):
"""
Update the robot pose given wheel velocities and a time step.
Args:
omega_left: Left wheel angular velocity (rad/s).
omega_right: Right wheel angular velocity (rad/s).
dt: Time step in seconds.
"""
self.v, self.omega = self.forward_kinematics(omega_left, omega_right)
if abs(self.omega) < 1e-9:
# Straight-line motion
self.x += self.v * math.cos(self.theta) * dt
self.y += self.v * math.sin(self.theta) * dt
else:
# Curved motion (exact integration)
self.x += (self.v / self.omega) * (
math.sin(self.theta + self.omega * dt) - math.sin(self.theta)
)
self.y += (self.v / self.omega) * (
math.cos(self.theta) - math.cos(self.theta + self.omega * dt)
)
self.theta += self.omega * dt
# Normalize theta to [-pi, pi]
self.theta = math.atan2(math.sin(self.theta), math.cos(self.theta))
@property
def pose(self) -> tuple[float, float, float]:
"""Current pose as (x, y, theta)."""
return self.x, self.y, self.theta
# Simulation example
if __name__ == "__main__":
# Create a robot with 3.3 cm wheel radius, 16 cm wheel base
# (similar to a TurtleBot3 Burger)
robot = DifferentialDrive(wheel_radius=0.033, wheel_base=0.16)
dt = 0.02 # 50 Hz control loop
duration = 10.0 # seconds
steps = int(duration / dt)
# Drive in a circle: right wheel faster than left
omega_left = 5.0 # rad/s
omega_right = 8.0 # rad/s
print("Simulating differential drive for 10 seconds...")
print(f"Wheel velocities: left={omega_left} rad/s, right={omega_right} rad/s")
v, w = robot.forward_kinematics(omega_left, omega_right)
print(f"Robot velocity: v={v:.3f} m/s, omega={w:.3f} rad/s")
omega_l_check, omega_r_check = robot.inverse_kinematics(v, w)
print(f"Inverse check: left={omega_l_check:.3f}, right={omega_r_check:.3f}")
for i in range(steps):
robot.update(omega_left, omega_right, dt)
if (i + 1) % 50 == 0: # Print every second
x, y, theta = robot.pose
print(
f" t={dt * (i+1):5.2f}s "
f"x={x:7.3f} y={y:7.3f} "
f"theta={math.degrees(theta):7.1f} deg"
)Running this simulation shows the robot tracing a circle, which is exactly what you expect when the wheel speeds are constant but unequal.
Odometry from Wheel Encoders
In practice, you do not command wheel angular velocities directly. Instead, you read wheel encoder ticks and compute angular displacements over each time step.
A typical wheel encoder produces N ticks per revolution. Given tick counts delta_left and delta_right over a time interval dt:
def encoder_to_velocity(delta_left: int, delta_right: int, dt: float,
ticks_per_rev: int, wheel_radius: float) -> tuple[float, float]:
"""Convert encoder tick deltas to wheel angular velocities."""
rads_per_tick = (2.0 * math.pi) / ticks_per_rev
omega_left = (delta_left * rads_per_tick) / dt
omega_right = (delta_right * rads_per_tick) / dt
return omega_left, omega_rightFeed these into the update method above, and you have dead-reckoning odometry. This accumulates error over time due to wheel slip, uneven surfaces, and measurement noise, which is why real robots fuse odometry with IMU data and/or SLAM corrections.
Common Pitfalls
1. Forgetting to Normalize Theta
After updating theta, always wrap it to [-pi, pi] or [0, 2*pi]. Without normalization, theta grows without bound and causes numerical issues in trigonometric functions after long operation.
2. Confusing Left and Right
Define a convention and stick to it. The standard convention is: positive omega (angular velocity) means counter-clockwise rotation. If the right wheel is faster, the robot turns left (positive omega). Be explicit about which wheel is which in your code.
3. Using Euler Integration at Low Rates
Euler integration (the simple x += v*cos(theta)*dt approximation) works at 50 Hz or higher. At 10 Hz, the error accumulates noticeably. Use the exact arc-based update equations shown above, or switch to Runge-Kutta 4th order if you need even higher accuracy.
4. Ignoring Wheel Slip
The kinematic model assumes pure rolling contact: no slip. On smooth tiles with rubber wheels, this is a reasonable assumption. On carpet, gravel, or wet surfaces, wheel slip makes odometry unreliable. Fuse with an IMU to compensate.
Beyond Two Wheels
The differential drive model generalizes to several related designs:
- Tracked robots: Treat the tracks as very wide wheels. The kinematics are the same, but wheel slip is much higher due to skid steering.
- Ackermann steering: Cars and car-like robots use a steering linkage instead of differential wheel speeds. The kinematics are different and involve a steering angle rather than independent wheel velocities.
- Omnidirectional drives: Three or four omniwheels (or mecanum wheels) allow motion in any direction without rotating first. The kinematics involve solving a matrix equation that maps wheel velocities to body velocities in x, y, and theta simultaneously.
Each of these has its own kinematic model, but differential drive is the starting point for understanding all of them.
Summary
Differential drive kinematics is one of the first topics every roboticist should master. The math is straightforward -- two equations for forward kinematics, two for inverse -- and the concepts (ICC, pose integration, encoder-based odometry) transfer directly to more complex platforms.
The key equations to remember:
- Forward: v = (v_R + v_L) / 2, omega = (v_R - v_L) / L
- Inverse: v_L = v - omegaL/2, v_R = v + omegaL/2
- Pose update: Use exact arc integration when omega is nonzero, straight-line when it is zero.
The Python class in this tutorial is a complete, working implementation that you can drop into your own projects or use as a reference when working with ROS 2 Twist messages and odometry.
Ready to see differential drive in action? Try our Mobile Robot Kinematics lesson where you can control a simulated two-wheeled robot in the browser and watch how wheel speeds translate to motion in real time.