A wheeled robot must travel from a start pose to a goal in a bounded 2D environment with circular obstacles. The problem is to find a collision-free trajectory that reaches the goal efficiently while keeping steering inputs reasonably smooth. This simplified task is relevant to local motion planning in mobile robotics and autonomous driving.
The robot has planar state (x, y, theta) and follows a
unicycle model with forward velocity v and angular velocity
omega:
dx/dt = v cos(theta)dy/dt = v sin(theta)dtheta/dt = omegaWe approximate the motion using a fixed time step
dt = 0.14 s and midpoint integration. A nominal heading
controller points toward the goal, and the optimizer adds a smooth,
sampled angular correction. An input trajectory is represented by 12
steering-correction values interpolated across the time horizon.
Velocity and steering rate have upper bounds.
This implementation uses the Cross-Entropy Method (CEM), a form of population-based Monte Carlo optimization:
N candidate control trajectories
(independently).H steps using the robot
model.This is a simple serial C++17 baseline, not a GPU implementation and not the full Model Predictive Path Integral (MPPI) controller. It uses no external C++ libraries.
Build with the standard GCC compiler:
g++ -std=c++17 -O3 -Wall -Wextra main.cpp -o trajectoryExample run (in the code/ directory):
./trajectory --samples 1400 --steps 90 --iterations 7 --seed 42 --output trajectory.ppmRuntime options:
--samples N: sampled trajectories per optimization
iteration (default 1400)--steps H: motion-integration steps per trajectory
(default 90)--iterations K: CEM optimization iterations (default
7)--seed S: deterministic random seed (default
42)--output FILE.ppm: PPM image path (default
trajectory.ppm)Outputs are:
trajectory.ppm: static 2D image showing ALL candidate
trajectories from every CEM iteration with a light-to-dark blue color
gradient and a numeric iteration legend, plus the green selected path
and obstaclesbest_trajectory.csv: selected path with time, position
and headingsummary.txt: best cost, terminal error, obstacle
clearance, collision step count, execution time, and visualized
iteration/trajectory countsConvert the PPM to PNG using the provided notebook command,
convert code/trajectory.ppm code/trajectory.png.
For the default seed and parameters, the generated outputs can be checked by:
Obstacle clearance is measured from the robot boundary to the physical obstacle boundary; positive clearance indicates separation at the sampled trajectory states. This discrete check does not guarantee continuous-time safety between integration steps.
The visualization shows all Monte Carlo candidate trajectories, using lighter blue for earlier iterations and darker blue for later iterations. A numeric color legend maps colors to iteration numbers. It also shows a green selected best trajectory, red physical obstacles, orange inflated exclusion boundaries, a blue start marker and a yellow goal marker.
The dominant computation is evaluating independent candidate
trajectories. During each CEM iteration, different candidates can be
simulated and scored in parallel using OpenMP on CPU
cores or CUDA on GPU threads. The small
distribution-update step is a reduction over the elite samples. Larger
values of --samples, --steps, and
--iterations scale the workload and will allow performance
and speedup experiments in later course assignments.