Sampling-Based & Graph-Search Motion Planning

The Challenge

Planning collision-free paths for a differential-drive robot requires reasoning in configuration space (C-space), not just Euclidean space — the robot's orientation is a third degree of freedom, and circular robot geometry must be inflated into C-space obstacles. Grid-based methods (Dijkstra, A*) are complete but scale poorly; sampling-based methods (PRM, RRT) scale better but are probabilistically complete. The final assignment required deploying the best planner onto a physical iRobot Create navigating a real room mapped from camera images.

Architecture & System Design

Path planning algorithms for robot navigation: Dijkstra and A* search graphs, PRM samples configuration space, RRT explores randomly. All tested on real robot with actual obstacle maps.

Dijkstra and A* operate on a weighted graph derived from a bitmap occupancy map — pixels become nodes, edges connect 8-connected neighbours, and A* uses Euclidean distance as the heuristic. PRM samples random free-space configurations, connects nearby nodes via a straight-line collision check, and queries the resulting roadmap with Dijkstra. The RRT implementation builds a tree by sampling random configs, finding the nearest tree vertex, propagating differential-drive kinematics (xDot, yDot, dirDot from left/right wheel velocities), and checking for map collision at each step. The final path is extracted via Dijkstra on the RRT adjacency matrix and executed on the iRobot Create via a MATLAB serial-port toolbox, with hardware playback scripts.

Code Walkthrough

3-step walk-through of the production implementation — file paths and intent shown above each block.

1. 01

   Step 1 of 3

   C-space setup — bitmap load and obstacle inflation

   planning/cspace_setup.m

   Planning must happen in configuration space, not Euclidean space — a circular robot's body occupies area, so planning through gaps narrower than the robot radius would produce collisions on hardware. Dilating obstacles by the robot radius (Minkowski sum via morphological disk) pre-inflates the map so the planner can treat the robot as a point, making every planned path physically safe.

   matlabCopy

   function inflated_map = build_cspace(map_img_path, robot_radius_px)
   % Load bitmap map and inflate obstacles by robot radius for C-space planning.
   
   raw = imread(map_img_path);
   if ndims(raw) == 3, raw = rgb2gray(raw); end
   binary_map = raw < 128;   % true = obstacle (dark pixels)
   
   % Minkowski sum via morphological dilation — robot becomes a point
   se           = strel('disk', robot_radius_px);
   inflated_map = imdilate(binary_map, se);
   
   % Verify start and goal are reachable after inflation
   if inflated_map(START_ROW, START_COL)
       error('cspace_setup: start position lies inside an inflated obstacle');
   end
   if inflated_map(GOAL_ROW, GOAL_COL)
       error('cspace_setup: goal position lies inside an inflated obstacle');
   end
   
   % Side-by-side debug visualisation
   figure;
   subplot(1,2,1); imshow(~binary_map);   title('Raw map');
   subplot(1,2,2); imshow(~inflated_map); title(sprintf('C-space (r=%d px)', ...
                                                         robot_radius_px));
   end

   Takeaway

   Inflate once at setup, plan as a point — far cheaper than checking a circle-vs-obstacle intersection on every RRT edge test, and guarantees paths are collision-free for the actual robot geometry.

2. 02

   Step 2 of 3

   RRT expansion — unicycle kinematic propagation

   planning/rrt.m

   A differential-drive robot cannot move sideways — its reachable set from any state is constrained by unicycle kinematics. The RRT propagates actual wheel velocity commands (uL, uR) rather than straight-line steps, so every edge in the tree is a feasible trajectory the robot can execute, not just a geometric shortcut that ignores the drive constraints.

   matlabCopy

   % rrt.m — RRT for differential-drive robot on bitmap C-space
   
   while ~done
       % Random sample with 1% goal bias
       sampleq = [-100,-100] + [1000,1400] .* rand(1,2);
       if rand() > 0.99, sampleq = endq; end
   
       % Nearest vertex in tree (Euclidean)
       dists = vecnorm(RM.verts - sampleq, 2, 2);
       [~, minIdx] = min(dists);
   
       % Propagate unicycle kinematics from nearest vertex
       uL = 700 * rand();   uR = 800 * rand();
       xDot   = wheelR * (uL+uR) * cos(RM.dir(minIdx) * pi/180) / 2;
       yDot   = wheelR * (uL+uR) * sin(RM.dir(minIdx) * pi/180) / 2;
       dirDot = 180/pi * wheelR * (uR-uL) / (axleR * 2);
   
       newq   = RM.verts(minIdx,:) + [xDot yDot] * dt;
       newDir = RM.dir(minIdx) + dirDot * dt;
   
       if ~colCreateMap(newq, robot.radius, map)
           RM.verts(end+1,:)      = newq;
           RM.dir(end+1)          = newDir;
           RM.velocities(end+1,:) = [uL uR];
           RM.adjMat(minIdx, end) = 1;
           if norm(newq - endq) < goalThresh, done = true; end
       end
   end

   Takeaway

   Storing the wheel commands (uL, uR) that produced each edge is the key — at execution time you replay the stored velocities rather than re-solving any path-following problem.

3. 03

   Step 3 of 3

   Path extraction and iRobot serial execution

   planning/playback.m

   Once the RRT finds the goal, Dijkstra extracts the shortest path through the tree adjacency matrix, and each stored (uL, uR) command is replayed over Bluetooth serial to the physical iRobot Create. The DRIVE_DIRECT opcode sends independent left/right wheel speeds, so the robot follows the exact kinematic trajectory the planner computed.

   matlabCopy

   % playback.m — Extract RRT path and execute on iRobot Create via BT serial
   
   % Dijkstra on RRT adjacency matrix → ordered vertex indices
   path_idx  = dijkstra2(length(RM.verts), RM.adjMat, 1, endvert);
   path_vels = RM.velocities(path_idx, :);  % [N x 2] [uL, uR] per step
   
   % Open Bluetooth serial (COMx on Windows, /dev/rfcommX on Linux)
   s = serial(IROBOT_PORT, 'BaudRate', 57600);
   fopen(s);
   fwrite(s, [128, 131], 'uint8');  % START + SAFE mode
   
   for k = 1:size(path_vels, 1)
       uL = round(path_vels(k,1));
       uR = round(path_vels(k,2));
       % DRIVE_DIRECT opcode 145: [145][uR_hi][uR_lo][uL_hi][uL_lo]
       fwrite(s, [145, floor(uR/256), mod(uR,256), ...
                       floor(uL/256), mod(uL,256)], 'uint8');
       pause(DT);
   end
   
   fwrite(s, [145, 0, 0, 0, 0], 'uint8');  % Stop
   fclose(s);

   Takeaway

   Storing wheel commands in the tree (not just positions) makes hardware playback a simple replay loop — no closed-loop path-following controller needed for the demo.

Results

All four planners successfully found collision-free paths in simulated bitmap environments. Dijkstra and A* produced optimal paths but were slow on large maps (>10s). RRT found paths in 2–5 seconds with up to 2000 tree vertices. The final assignment deployed RRT on the physical iRobot Create: the robot navigated a real room, executing the planned path via wheel velocity commands over Bluetooth serial. Path execution videos were recorded (LongPath.mp4, ShortPath.mp4).

Gallery & Demos

Click any image or video to expand · ← → keys navigate