Robot Motion

Chapter 1: Robot Pose

For a mobile robot on a plane, the pose (or kinematic state) has three components:

where (x, y) is the position in a global coordinate frame, and θ is the heading (orientation). A robot with θ = 0 points along the x-axis; θ = π/2 points along the y-axis.

Chapter 2: Probabilistic Kinematics

Classical kinematics gives a deterministic function: given pose x_t-1 and control u_t, compute the next pose x_t. Probabilistic kinematics extends this to a conditional density:

This density describes the probability of ending up at pose x_t when starting at x_t-1 and executing command u_t. It captures the "cloud" of possible outcomes.

Imagine commanding a robot to drive forward and turn. The true motion is corrupted by noise in translation, rotation, and drift. The resulting probability cloud has a characteristic banana shape: spread along the arc of motion, wider at the end (where rotational error compounds translational error).

There are two ways to use a motion model:

Density evaluation: Given x_t-1, u_t, and x_t, compute p(x_t | u_t, x_t-1). Needed by histogram filters and EKF.

Sampling: Given x_t-1 and u_t, generate a random x_t drawn from p(x_t | u_t, x_t-1). Needed by particle filters.

Chapter 3: Velocity Motion Model

The velocity model assumes the robot is controlled by two velocities:

where v_t is translational velocity (forward/backward) and ω_t is rotational velocity (turning). This is the natural interface for differential-drive, synchro-drive, and Ackerman-drive robots.

Ideal (noise-free) motion: A robot with constant v and ω moves along a circular arc of radius r = v/ω. After time Δt:

Noisy motion: The actual velocities differ from commanded ones. The model adds Gaussian noise to v and ω, controlled by six robot-specific error parameters α₁ through α₆:

Chapter 4: Velocity Sampling

For particle filters, we need to sample from the velocity motion model — generate random poses from p(x_t | u_t, x_t-1). The algorithm is simple:

pseudocode
# Sample from velocity motion model
Input: xt-1 = (x, y, θ), ut = (v, ω)

# Add noise to commanded velocities
v̂ = v + sample(α1|v| + α2|ω|)
ω̂ = ω + sample(α3|v| + α4|ω|)
γ̂ = sample(α5|v| + α6|ω|)

# Apply noisy velocities
x' = x - (v̂/ω̂) sin θ + (v̂/ω̂) sin(θ + ω̂Δt)
y' = y + (v̂/ω̂) cos θ - (v̂/ω̂) cos(θ + ω̂Δt)
θ' = θ + ω̂Δt + γ̂Δt

return (x', y', θ')

The function sample(b) draws from a zero-mean distribution with variance b — typically Gaussian or triangular.

Different noise settings produce very different distributions. High translational noise (α₁, α₂) spreads particles along the arc. High rotational noise (α₃, α₄) spreads them perpendicular to the arc. Balanced noise produces the banana shape.

Chapter 5: Motion Demo

Watch how the velocity motion model spreads particles. The robot starts at the left and executes a "drive forward and turn" command. Particles show the distribution of possible outcomes. Adjust the noise to see banana shapes, wide arcs, and tight clusters.

Chapter 6: Odometry Motion Model

The odometry model uses wheel encoder data instead of velocity commands. Most commercial robots provide odometry: "the left wheel turned X ticks, the right wheel turned Y ticks." From this, we can compute a relative motion estimate.

The odometry model decomposes motion into three phases:

δ_rot1: Initial rotation to face the new position.

δ_trans: Translation to the new position.

δ_rot2: Final rotation to the new heading.

Noise is added to each of the three components independently. The error parameters α₁ through α₄ control how much translational and rotational noise is injected. Since odometry directly measures wheel motion (rather than predicting it from motor commands), it tends to be more accurate than the velocity model.

Chapter 7: Odometry Sampling

Sampling from the odometry model follows the same principle as the velocity model: perturb the parameters, then compute the new pose.

pseudocode
# Sample from odometry motion model
Input: xt-1 = (x, y, θ), ut = (δrot1, δtrans, δrot2)

# Add noise to each component
δ̂rot1 = δrot1 + sample(α1|δrot1| + α2|δtrans|)
δ̂trans = δtrans + sample(α3|δtrans| + α4(|δrot1| + |δrot2|))
δ̂rot2 = δrot2 + sample(α1|δrot2| + α2|δtrans|)

# Compute new pose
x' = x + δ̂trans cos(θ + δ̂rot1)
y' = y + δ̂trans sin(θ + δ̂rot1)
θ' = θ + δ̂rot1 + δ̂rot2

return (x', y', θ')

The noise scales with the magnitude of the motion: larger motions accumulate more error. This is physically reasonable — driving 10 meters has more odometry drift than driving 1 meter.

Chapter 8: Motion and Maps

So far, our motion models ignore obstacles. The robot might sample a pose that's inside a wall! There are two approaches to handling this:

Rejection sampling: Sample poses from the motion model, discard any that land in occupied space, and re-sample. Simple but wasteful if many samples are invalid.

Map-informed models: Modify the motion model itself to assign zero probability to poses inside obstacles. This is cleaner but requires access to the map during the prediction step.

Another subtlety: the Markov assumption says x_t depends only on x_t-1 and u_t. In reality, there are un-modeled effects (people pushing the robot, carpet vs. tile, wheel wear). Over-estimating the motion noise provides a practical buffer against these violations.

Chapter 9: Summary

Probabilistic motion models are the prediction engine of every Bayes filter. They transform the old belief into a predicted belief by accounting for the uncertainty in robot motion.

Key lessons:

• Noise is added to control parameters (velocities or odometry), not directly to the pose.

• Noise magnitude scales with the motion magnitude — bigger moves are noisier.

• Over-estimating noise is safer than under-estimating it.

• Both density evaluation and sampling are needed, depending on the filter type.