System Architecture

Task Pipeline

LISTEN — Listen to human instruction; Record the robot's home position

SEARCH — Spin and explore the environment using frontier-based navigation until the target object is found via YOLO

PLAN_GRASP — Localize the object in 3D using depth back-projection and compute an optimal top-down grasp pose

PRE_GRASP — Move the arm to a position 10 cm above the object

GRASP — Descend, close the gripper, and lift

VERIFY_GRASP — Confirm the object is held using a second YOLO detection pass

RETURN — Navigate back to the recorded home position

DONE — Mission complete

Navigation States

State Machine

Voice

Object Detection via YOLOv8

Grasping

GraspNet + GPD Experimentation

Implemented Grasp Approach

Grasp Validation using YOLO

Navigation

Depth Mapping

Building the occupancy map with depth rays necessitates some additional considerations. Firstly, only obstacles with heights between 10cm and 80cm in world-space are stored in the map; this prevents the floor (and potentially ceiling or the underside of tables) from being detected and generating false positives in the map. Additionally, limiting the range of each ray between 0.4m and 5m helped filter out self-hits and noisy, long-distance readings. All cells along a ray, between the camera and the obstacle, are marked as free (using Bresenham's line algorithm).

Navigation via Frontier Exploration

Frontier-based exploration:

After each 360º scan, if the object was not yet seen or not yet reachable (no A* path), the robot created frontiers, which were free cells that bordered unknown space. The system grouped these frontier cells into clusters using flood fill, then filtered them. It discarded clusters with fewer than 8 cells, skipped clusters within 1 m of the robot to avoid revisiting nearby space, and rejected clusters that were not reachable via connected free space or were too close to obstacles (within a 35 cm clearance buffer). It ranked the remaining frontiers by the amount of surrounding unknown space divided by the distance from the robot, so the robot preferred to explore areas where it could learn the most per meter of travel. If the robot had already glimpsed the target object through YOLO but couldn't make an A* path to it yet, the scoring shifted to heavily favor frontiers near where it last saw the object.

To navigate to a frontier, the system ran A* search on the occupancy grid with 8-directional movement (cost 1.0 for cardinal, √2 for diagonal), using Euclidean distance as the heuristic and capping the search at 50,000 node expansions. It inflated obstacles by the robot's clearance radius during planning to ensure paths remained a safe distance from obstacles. It then simplified the resulting path to one waypoint per meter, and the robot followed these waypoints using proportional heading control with reactive depth-based obstacle checks.

Object approach:

When YOLO detected the target object in at least two frames within a 2-second window (guarding against false positives), the system confirmed the detection and estimated the object's world position by back-projecting its pixel location through the depth image. If possible, it then planned an A* path to that position and entered an approach phase. Otherwise, it would keep exploring until the map was full enough to map a path to the object. 

During the object approach phase, we continuously checked whether the object stayed centered in the camera frame. If it drifted more than 60 pixels off-center, the robot stopped completely and rotated in place until it brought the object back within 40 pixels of center. The gap between these two thresholds prevented the robot from flickering between driving and correcting. After re-centering, the system re-planned the path from the robot's current position using the freshened estimate. 

Pre-grasp positioning:

Once the robot was within 35 cm of the object, it did a sweep of camera tilt positions until the object was centered vertically in the frame. Then executed a top-down grasp using depth-based point cloud segmentation and inverse kinematics (detailed in the Grasping section). The full pipeline from voice command to exploration to detection to grasp ran end-to-end in simulation (yay!).

Navigating the Real World:

On the real robot, we unfortunately ran into several issues that made the map-based approach unworkable in our time frame (aw man..). The lab's dense clutter of tables and chairs added noise to the occupancy grid, which sometimes led to unreliable paths. In addition, the robot's motors drifted slightly, and the camera feed lagged behind the robot's actual position. We slowed the robot down to compensate, but that made each test run much longer and slowed our debugging cycle. There was also a real-life-only misalignment between the depth and RGB camera streams, which led to issues because the system built the occupancy grid from depth but pulled object positions from YOLO running on RGB, so the detected object landed at the wrong place in the world map. 

With limited time left to fix the depth-RGB registration, we simplified the navigation to a purely vision-based method. Instead of building a map, the robot simply rotated in place to scan for the unoccluded target using YOLO, AprilTag, or face detection (depending on the target object), then drove toward it while continuously steering to keep the detection centered in the frame. This gave up obstacle avoidance and occlusion-robustness, but it proved far more reliable on the real hardware because it depended only on what the camera saw in the moment rather than on an accumulated map that had drifted out of alignment with reality.

Task 3: Fetch and Sort

Facial Recognition

Bin Sorting via AprilTags

For the full codebase, please refer to the GitHub Repository below in the Footer.