From Path to Trajectory
So far we’ve discussed path planning: finding a sequence of configurations from start to goal.
But a path is not executable:
Path (what): [q1] → [q2] → [q3] → ... → [qn]
(only positions)
Trajectory (how): [q1, v1, a1] → [q2, v2, a2] → ... → [qn, vn, an]
(positions, velocities, accelerations, timestamps)
A trajectory must:
- Follow the path (be feasible)
- Respect dynamics (motors can’t accelerate infinitely)
- Be smooth (no jerky motions)
- Be fast (minimize execution time)
Part 1: Trajectory Optimization
The Problem
Given a path, find the time-optimal trajectory that respects:
- Joint velocity limits: |q̇_i| ≤ v_max
- Joint acceleration limits: |q̈_i| ≤ a_max
- Actuator torque limits: |τ_i| ≤ τ_max
Simplest Approach: Trapezoidal Velocity Profile
Accelerate → Cruise → Decelerate
Figure 1: Trapezoidal Velocity Profile Structure
Velocity
↑
│ Phase 1: Accel Phase 2: Cruise Phase 3: Decel
│ a=a_max a=0 a=-a_max
│ ╱╲╲╲╲╲╲╲╲╲╲╲╱╱╱╱╱╱╱╱╱╱╱╱╱╱
v_max├────────╱ ╲ ╲
│ ╱ ╲ ╲
│ ╱ ╲ ╲
│ ╱ ╲ ╲
│ ╱ ╲ ╲
0 ├╱─────────────────────────────╲──────────────╲─→ Time
t_accel t_cruise t_decel
Total distance: d = d_accel + d_cruise + d_decel
Total time: T = t_accel + t_cruise + t_decel
Key: Minimize T while respecting v_max and a_max limits
Three Phases:
- Acceleration (a_max): Build up speed efficiently
- Cruise (v_max): Constant velocity
- Deceleration (-a_max): Smooth stop at goal
Velocity
↑
v_max├─────────────────
│ ╱╲ ╱╲
│ ╱ ╲ ╱ ╲
│ ╱ ╲ ╱ ╲
│ ╱ ╲ ╱ ╲
│╱ ╲╱ ╲
0 ├──────────┴─────────┴────→ Time
Phase 1: Accel (a = a_max)
Phase 2: Cruise (a = 0, v = v_max)
Phase 3: Decel (a = -a_max)
Trapezoidal Profile Equations
Given:
- Start position: q_0
- End position: q_f
- Distance: d = q_f - q_0
- Velocity limit: v_max
- Acceleration limit: a_max
Minimum time trajectory:
- If d < 2*v_max²/a_max: (no cruise phase)
- Peak velocity: v_peak = sqrt(a_max * d / 2)
- Time: T = 2 * sqrt(d / a_max)
- Else: (has cruise phase)
- Accel time: t_a = v_max / a_max
- Distance during accel: d_a = v_max² / (2 * a_max)
- Cruise time: t_c = (d - 2*d_a) / v_max
- Total time: T = 2*t_a + t_c
Code: Trapezoidal Profiler
class TrapezoidalProfiler:
def __init__(self, v_max, a_max):
self.v_max = v_max
self.a_max = a_max
def plan(self, q_start, q_goal):
"""Generate time-optimal trajectory"""
distance = abs(q_goal - q_start)
direction = 1 if q_goal > q_start else -1
# Check if cruise phase exists
d_accel = self.v_max**2 / (2 * self.a_max)
if distance < 2 * d_accel:
# No cruise phase
v_peak = math.sqrt(self.a_max * distance / 2)
t_accel = v_peak / self.a_max
t_total = 2 * t_accel
trajectory = []
for t in np.linspace(0, t_total, 100):
if t < t_accel:
# Acceleration phase
q = q_start + 0.5 * self.a_max * direction * t**2
v = self.a_max * direction * t
else:
# Deceleration phase
t_decel = t - t_accel
q = q_start + (d_accel - 0.5 * self.a_max * direction * t_decel**2)
v = v_peak * direction - self.a_max * direction * t_decel
trajectory.append((t, q, v))
return trajectory
else:
# Has cruise phase
t_accel = self.v_max / self.a_max
d_accel = self.v_max**2 / (2 * self.a_max)
t_cruise = (distance - 2*d_accel) / self.v_max
t_total = 2*t_accel + t_cruise
trajectory = []
for t in np.linspace(0, t_total, 200):
if t < t_accel:
# Accel
q = q_start + 0.5 * self.a_max * direction * t**2
v = self.a_max * direction * t
elif t < t_accel + t_cruise:
# Cruise
q = q_start + d_accel * direction + self.v_max * direction * (t - t_accel)
v = self.v_max * direction
else:
# Decel
t_decel = t - t_accel - t_cruise
q = q_start + distance * direction - 0.5 * self.a_max * direction * t_decel**2
v = self.v_max * direction - self.a_max * direction * t_decel
trajectory.append((t, q, v))
return trajectory
# Usage
profiler = TrapezoidalProfiler(v_max=1.0, a_max=0.5)
trajectory = profiler.plan(q_start=0.0, q_goal=10.0)
for t, q, v in trajectory:
print(f"t={t:.2f}s, q={q:.2f}m, v={v:.2f}m/s")Visualization: Multi-Joint Trajectory
For a 3-joint robot, generate trapezoidal profiles independently:
Joint 1: q1(t)
├─ Accel phase (0-1s)
├─ Cruise phase (1-3s)
└─ Decel phase (3-4s)
Joint 2: q2(t)
├─ Accel phase (0-0.5s) [faster than joint 1]
├─ Cruise phase (0.5-3.5s)
└─ Decel phase (3.5-4s)
Joint 3: q3(t)
├─ Very short motion
└─ Accel + Decel only (0-2s)
Key insight: Trajectories execute in parallel. Total execution time = max(individual times).
Part 2: Minimum Snap / Minimum Jerk
Better than trapezoidal: optimize for smoothness.
Figure 2: Trajectory Smoothness Comparison
TRAPEZOIDAL PROFILE MINIMUM SNAP PROFILE
(Piecewise linear) (Polynomial optimization)
Acceleration Acceleration
↑ ↑
│ ╱╲ │ ╱╲
│ ╱ ╲ │ ╱ ╲
│╱────╲ │ ╱ ╲
└──────→ Time └─╱──────╲─→ Time
Sharp corners Smooth curves
Position Smoothness: Position Smoothness:
↑ S─ Reaches goal ↑ S─── Smooth arrival
│ ╱╲ │ ╱╲
│╱ ╲ │ ╱╱ ╲
└──────→ Time └╱───────╲─→ Time
Jerky motion Smooth motion
Motor Stress: Motor Stress:
- Acceleration changes abruptly → Jerk spikes
- High motor torque demands - Smooth acceleration → Low jerk
- Vibration & wear - Smooth operation & efficiency
Benefits of Minimum Snap:
- Smoother trajectories (less motor stress)
- More energy efficient
- Reduced vibration
- Better for delicate objects (UAV packages, manipulation)
Jerk = rate of change of acceleration = d³q/dt³
Why Minimize Jerk?
Smoother trajectories:
- Less motor stress
- Reduce vibration
- Better for tracking
- More energy efficient
Minimum Snap (Quadcopter Trajectory)
For UAVs, minimize snap (4th derivative):
Minimize: ∫(d⁴q/dt⁴)² dt
Subject to:
- Pass through waypoints at specific times
- Respect position/velocity constraints
Solution: Piecewise polynomial of degree 7 between waypoints.
class MinimumSnapTrajectory:
def __init__(self, waypoints, times):
"""
waypoints: list of (x, y, z, yaw) positions
times: list of times at which waypoints occur
"""
self.waypoints = waypoints
self.times = times
self.polys = self._compute_polynomials()
def _compute_polynomials(self):
"""Solve for piecewise polynomial coefficients"""
# Between each pair of waypoints, fit degree-7 polynomial
# Constraints: position at endpoints, smooth derivatives
# This is a constrained optimization problem
# Usually solved with quadratic programming
# Simplified: use cubic spline interpolation
from scipy.interpolate import CubicSpline
x_coords = [w[0] for w in self.waypoints]
y_coords = [w[1] for w in self.waypoints]
cs_x = CubicSpline(self.times, x_coords)
cs_y = CubicSpline(self.times, y_coords)
return (cs_x, cs_y)
def evaluate(self, t):
"""Get position at time t"""
cs_x, cs_y = self.polys
return (cs_x(t), cs_y(t))
def evaluate_derivative(self, t, order=1):
"""Get velocity (order=1), acceleration (order=2), etc."""
cs_x, cs_y = self.polys
return (cs_x(t, order), cs_y(t, order))
# Usage
waypoints = [(0, 0, 0, 0), (5, 0, 1, 0), (10, 5, 2, 0)]
times = [0, 2, 4]
traj = MinimumSnapTrajectory(waypoints, times)
for t in np.linspace(0, 4, 100):
x, y = traj.evaluate(t)
vx, vy = traj.evaluate_derivative(t, order=1)
ax, ay = traj.evaluate_derivative(t, order=2)
print(f"t={t:.2f}: pos=({x:.2f}, {y:.2f}), vel=({vx:.2f}, {vy:.2f})")Part 3: Model Predictive Control (MPC)
MPC recomputes optimal trajectory at each time step, incorporating new information.
Figure 3: MPC Receding Horizon Control
TIME STEP 0: Initial Plan
Measurement: Current state
├─ Plan horizon: 10 steps
├─ Optimal control found
└─ Execute step 0
Predicted trajectory
↙ ↘
─────────────────────────────────────
t₀ [Execute] t₁ t₂ t₃ t₄ t₅ ... t₁₀
TIME STEP 1: Replan (New Information)
Measurement: Actual state (≠ predicted!)
├─ Obstacle detected at t₃
├─ Replan avoiding obstacle
└─ Execute step 1
Corrected trajectory
↙ ↖ (obstacle avoidance)
────────────────────────────────────
t₁ [Execute] t₂ t₃ t₄ t₅ ... t₁₁
TIME STEP 2: Replan Again
Measurement: Wind gust, sensor update
├─ Adjust trajectory
└─ Execute step 2
Further refined
↙ ↖
────────────────────────────────────
t₂ [Execute] t₃ t₄ t₅ ... t₁₂
KEY IDEA:
- Never plan too far (horizon = 10 steps ahead)
- Constantly replanning keeps control accurate
- Adapts to model errors and disturbances
- Balances optimization with responsiveness
MPC Advantages:
- Handles model uncertainties (wind, friction changes)
- Responds to new obstacles in real-time
- Constraint-aware (velocity, acceleration limits)
- Optimal over finite horizon
The Concept
At each time step t:
1. Measure current state
2. Predict next N steps
3. Optimize trajectory over N steps
4. Execute only first step
5. Repeat at next time step
This closed-loop replanning handles:
- Modeling errors
- Disturbances
- Unknown obstacles
Simple MPC Example
class SimpleModelPredictiveControl:
def __init__(self, model, horizon=10, dt=0.1):
self.model = model
self.horizon = horizon
self.dt = dt
def compute_control(self, current_state, goal_state):
"""Compute optimal control sequence"""
# Predict future states
predicted_states = []
state = current_state
for _ in range(self.horizon):
predicted_states.append(state)
# Simple model: constant velocity
state = self.model.predict_next(state)
# Optimize: find control that minimizes
# cost = distance_to_goal + energy_used
best_control = None
best_cost = float('inf')
for control in self.possible_controls():
cost = 0
s = current_state
for _ in range(self.horizon):
# Simulate with this control
s = self.model.apply_control(s, control)
# Cost: how far from goal
cost += distance(s, goal_state)
# Cost: energy used
cost += 0.1 * norm(control)
if cost < best_cost:
best_cost = cost
best_control = control
return best_control
def possible_controls(self):
"""Enumerate possible controls"""
# In 2D: velocity in each direction
controls = []
for vx in [-1, -0.5, 0, 0.5, 1]:
for vy in [-1, -0.5, 0, 0.5, 1]:
controls.append([vx, vy])
return controlsMPC Workflow
Time 0:
├─ Measure state: [1, 0, 0.5, 0]
├─ Plan horizon 0-10 steps
├─ Compute optimal control
└─ Execute step 0: v=[0.7, 0.3]
Time 1 (after 0.1s):
├─ Measure new state: [1.07, 0.03, 0.4, 0.1] (actual ≠ predicted)
├─ Replan horizon 1-11 steps
├─ Compute new optimal control (corrected)
└─ Execute step 1
...repeat...
Why MPC?
✅ Advantages:
- Handles model uncertainties
- Adapts to changing environments
- Receding horizon (only optimize what matters)
❌ Disadvantages:
- Computationally expensive (real-time constraint)
- Needs good model
- Horizon limited by compute time
Part 4: Practical Deployment
Integration: Planning → Execution
Figure 4: Complete Motion Planning Pipeline
┌────────────────────────────────────────────────────────────┐
│ PLANNING PIPELINE │
└────────────────────────────────────────────────────────────┘
LEVEL 1: GOAL SPECIFICATION
"Go from [0, 0] to [10, 10]"
↓
LEVEL 2: GLOBAL PLANNING (5 Hz, 100-1000ms)
Algorithm: RRT* or A*
Input: Start, Goal, Environment
Output: Rough waypoint path
┌─────────────────┐
│ (0,0) │
│ \ │
│ \(2,2) │
│ \ │
│ *(5,5) │
│ \ │
│ *(8,8) │
│ \ │
│ (10,10)│
└─────────────────┘
↓
LEVEL 3: TRAJECTORY OPTIMIZATION (2 Hz, 500ms)
Algorithm: Trapezoidal profile or Minimum Snap
Input: Waypoints
Output: Smooth trajectory with velocities
┌──────────────────────────────────┐
│ q(t), v(t), a(t) for each joint │
└──────────────────────────────────┘
↓
LEVEL 4: LOCAL PLANNING (20-50 Hz, 20-50ms)
Algorithm: MPC or Potential Fields
Input: Trajectory, Current state, Local obstacles
Output: Smooth velocity commands
└─ Handles unforeseen obstacles
└─ Adapts to model errors
↓
LEVEL 5: REACTIVE CONTROL (100-200 Hz, 5-10ms)
Algorithm: Bug algorithm / Direct control
Input: Velocity commands
Output: Motor PWM signals, Joint commands
└─ Immediate safety layer
└─ Real-time responsiveness
↓
LEVEL 6: EXECUTION
┌─────────────────────────┐
│ ROBOT EXECUTION │
│ Motors, servos, wheels │
└─────────────────────────┘
Frequency and Latency Budget:
Layer Frequency Latency Purpose
──────────────────────────────────────────────
Global Planning 5 Hz 200ms Long-term strategy
Trajectory Opt 2 Hz 500ms Path smoothing
Local Planning 30 Hz 33ms Obstacle avoidance
Reactive Control 100 Hz 10ms Safety/responsiveness
Real Robot System
Example: Mobile robot with ROS 2
# node_global_planner.py (5 Hz)
class GlobalPlanner:
def plan(self, goal):
# RRT* planning
path = rrt_star(self.current_pose, goal)
self.publish('/global_path', path)
# node_trajectory_optimizer.py (20 Hz)
class TrajectoryOptimizer:
def optimize(self, path):
# Trapezoidal profiler
trajectory = trapezoidal_profile(path)
self.publish('/trajectory', trajectory)
# node_local_planner.py (50 Hz)
class LocalPlanner:
def plan(self, trajectory, obstacles):
# MPC with obstacle avoidance
control = mpc_control(trajectory, obstacles)
self.publish('/cmd_vel', control)
# node_controller.py (100 Hz)
class Controller:
def control(self, velocity_command):
# PID control
motor_commands = pid_control(velocity_command, self.actual_velocity)
self.publish('/motor_commands', motor_commands)Time Budget
Real-time execution requires respecting latencies:
Event Timeline:
t=0ms: Robot sensor update
t=5ms: Global planner decision
t=10ms: Trajectory update
t=15ms: Local planner decision
t=20ms: Motor command execution
────────────────────────────────
Cycle time: 20ms (50 Hz)
Slack: 5ms for unexpected delays
Algorithm Selection Flowchart
Figure 5: Algorithm Selection Decision Tree
START
│
├─ Do you know the full environment?
│ ├─ YES → Use GLOBAL PLANNER
│ │ ├─ Dimension ≤ 3D? → Use A* or Dijkstra
│ │ └─ Dimension > 3D? → Use RRT* or PRM
│ │
│ └─ NO → Is it changing?
│ ├─ SLOWLY → Use PRM (build once, query many)
│ └─ QUICKLY → Use RRT (replan each time)
│
├─ Is path quality critical?
│ ├─ YES → Use RRT* (asymptotically optimal)
│ │ or Trajectory Optimization (smooth)
│ │
│ └─ NO → Use RRT (fast, good enough)
│ or Bug algorithms (reactive)
│
├─ Is computation power limited?
│ ├─ YES → Use Potential Fields or Bug (O(1) per step)
│ │ Very light resources
│ │
│ └─ NO → Use RRT*, MPC (more computation OK)
│
└─ Must adapt to dynamic obstacles?
├─ YES → Use MPC with replanning
│ or Potential Fields + Bug
│
└─ NO → Use static planner (RRT*, A*)
then smooth with trajectory optimizer
DECISION TABLE:
┌──────────────────┬──────┬──────┬──────┬────────────┐
│ Scenario │ Dim │ Time │ Qual │ Algorithm │
├──────────────────┼──────┼──────┼──────┼────────────┤
│ Mobile robot │ 2D │ Med │ Good │ A* │
│ 6D arm │ 6D │ Med │ Good │ RRT* │
│ UAV trajectory │ 4D │ Fast │ V.Good│Min. Snap │
│ Unknown env. │ Any │ Fast │ Fair │ RRT │
│ Real-time safe │ Any │ Fast │ N/A │ Reactive │
│ Many queries │ Any │ Slow │ Good │ PRM │
└──────────────────┴──────┴──────┴──────┴────────────┘
Do you know the full
environment?
├─ YES → Use global planner (RRT*, Grid-based)
│ └─ Time: 1-10 seconds
│
└─ NO → Is it changing?
├─ Slowly → Use PRM + local planner
│ └─ Build once, reuse many times
│
└─ Quickly → Use reactive planner (Bug, Potential Fields)
└─ Real-time response
Is path quality critical?
├─ YES → Use RRT* or MPC
│ └─ Optimize for smoothness
│
└─ NO → Use RRT or Bug0
└─ Fast approximations
Is computation limited?
├─ YES → Use potential fields or Bug algorithms
│ └─ O(1) per step
│
└─ NO → Use RRT*, MPC, or trajectory optimization
└─ Better quality
Real-World Case Studies
Case 1: Autonomous Vehicle
Requirements: High speed, safety, optimality
Solution:
- Global: A* on road network (precomputed)
- Trajectory: Minimum snap for comfort
- Local: MPC with prediction (30Hz)
- Reactive: Potential fields for pedestrians (100Hz)
Case 2: Robotic Manipulator
Requirements: Collision avoidance, speed
Solution:
- Global: RRT* in joint space (1s planning)
- Trajectory: Trapezoidal profile (smooth)
- Local: Impedance control (500Hz)
- Safety: Hard limits on joint velocities
Case 3: Autonomous Quadrotor
Requirements: Dynamic, complex 3D space
Solution:
- Global: RRT* with sampling in 4D (x,y,z,yaw)
- Trajectory: Minimum snap + yaw optimization
- Local: MPC for wind disturbances (50Hz)
- Control: Attitude controller (200Hz)
Performance Summary
Algorithm Dimension Time Optimality Real-time
─────────────────────────────────────────────────────────────
Grid-Based (A*) 2D-3D Fast Optimal Yes
RRT 6D+ Med Fair Yes
RRT* 6D+ Slow V.Good Limited
PRM 6D+ Setup Fair Yes (query)
Trapezoidal Profile 1D/Joint Fast Good Yes
Minimum Snap 3D Slow V.Good Limited
MPC Any Slow Good With limits
Potential Fields Any Fast N/A Yes
Bug Algorithms 2D Fast N/A Yes
Key Takeaways
-
Path ≠ Trajectory
- Path: Just waypoints
- Trajectory: With velocities, accelerations, timing
-
Trajectory optimization matters
- Trapezoidal for speed
- Minimum snap/jerk for smoothness
-
MPC adds robustness
- Replans at each step
- Handles uncertainties
- But computationally expensive
-
Layered architecture wins
- Global planner (slow, optimal)
- Local planner (fast, reactive)
- Each layer has appropriate frequency
-
Always profile timing
- Know your latencies
- Budget computation time
- Understand bottlenecks
Summary: All 4 Parts
| Part | Focus | Key Algorithms |
|---|---|---|
| Part 1 | Fundamentals, low-dim | Dijkstra, A*, Grid-based |
| Part 2 | High-dim sampling | RRT, RRT*, PRM |
| Part 3 | Reactive, online | Bug algorithms, Potential fields |
| Part 4 | Trajectory, deployment | Optimization, MPC, layered systems |
References
-
Mellinger, D., & Kumar, V. (2011). “Minimum snap trajectory generation and control for quadrotors.” ICRA.
-
Camacho, E. F., & Bordons, C. (2013). Model Predictive Control. Springer.
-
Maciejewski, A. A., & Klein, C. A. (1985). “Obstacle avoidance for manipulators and mobile robots.” IEEE Trans. Robotics and Automation.
-
LaValle, S. M. (2006). Planning Algorithms. Cambridge University Press.
-
Thrun, S., Burgard, W., & Fox, D. (2005). Probabilistic Robotics. MIT Press.
Series Complete! June 2026
You now understand:
- ✅ Grid-based planning (Dijkstra, A*)
- ✅ Sampling-based planning (RRT, RRT*, PRM)
- ✅ Reactive methods (Bug, Potential fields)
- ✅ Advanced topics (Trajectory optimization, MPC)
- ✅ Practical deployment strategies
Next steps: Implement these in your projects, benchmark on your robots, adapt to your constraints.
Happy planning! 🤖