Close-Range Stereo Planning & Navigation for ARTEMIS

Developed the proximity package for the ARTEMIS humanoid robot, a close-range planning and navigation suite that runs at 60 Hz and serves as a critical fallback when the primary vision pipeline fails or encounters blackout scenarios during competitive matches. This work was part of RoMeLa's winning entry at the RoboCup 2024 Humanoids League (Adult Size) held in Eindhoven, Netherlands.

System Architecture

The proximity package comprises three major components: a stereo image pre-processing pipeline, the main depth processing loop, and an inference recorder for diagnostics. Raw depth data from a ZED stereo camera is processed through a series of geometric filters, connected-component analysis, and coordinate transformations to produce obstacle bounding boxes in real-time.

Stereo Image Pre-processing

The DepthHandler module handles raw depth data from the ZED stereo camera through the following pipeline:

• Downsampling & FOV masking: The depth map is downsampled by a configurable factor, vertical angles are computed per pixel, and a field-of-view mask is applied to reject regions outside the camera's effective cone.
• Floor distance calculation: Using the known camera height and neck pitch angle, the system computes the expected floor distance at each pixel column. Depth values within a tolerance band of this expected distance are rejected as ground plane.
• Green field filtering: An HSV-based mask removes the green playing field from the RGB frame, preventing the field surface from being detected as an obstacle.
• Region hiding: Based on the robot's current neck pitch and yaw angles, portions of the depth image corresponding to the robot's own body or irrelevant regions are masked out.

Connected Component Analysis for Object Detection

The core detection algorithm performs connected component analysis on the filtered depth map using a Union-Find data structure with 8-connectivity. The algorithm identifies contiguous regions of valid depth values and extracts the k closest objects (limited to two in the competition implementation).

For each detected component, the module computes bounding boxes and extracts key attributes: width, height, distance, and bearing angle. An infinite-component analysis pass separately handles NaN and infinite depth values, identifying large contiguous regions of sensor failure that may indicate occluding objects at very close range.

Coordinate Transformations

Detected obstacle positions are converted from the camera frame to the robot body frame through a chain of transformations involving the neck yaw/pitch angles and the known camera mounting offset. This pipeline uses Euler-to-rotation-matrix conversion:

where \(\psi\) and \(\theta\) are the neck yaw and pitch angles, \(\mathbf{p}_{\text{cam}}\) is the obstacle position in camera coordinates, and \(\mathbf{t}_{\text{mount}}\) is the camera mounting offset.

Bounding Box Fusion

Overlapping or adjacent bounding boxes are merged using a combination of IoU thresholding, coverage analysis, and x-coordinate proximity checks. This prevents a single obstacle from being reported as multiple detections when it spans multiple depth discontinuities.

HALO Mode & Integration

In severe occlusion scenarios (e.g., the robot's camera is blocked by another robot), the system switches to a fallback HALO mode. The robot looks down at the ground at a specific neck angle to identify open navigation paths using the depth map, enabling safe blind navigation.

The proximity package integrates seamlessly with ARTEMIS's mid-level planner, localization system, and high-level strategy planner, providing real-time obstacle information for dynamic path planning during matches.

Key Specifications

ARTEMIS humanoid robot in the lab gantry at RoMeLa

Close-range obstacle detection during a match at RoboCup 2024