💻 computer science

SE(2) Navigation Mesh

This paper introduces the SE(2) Navigation Mesh, a polygonal representation that encodes yaw-dependent traversability for non-circular robots in complex multi-level environments, coupled with an A*-String Pulling-A* pathfinding strategy and an online update mechanism that significantly outperforms existing methods in capturing traversable areas and navigating constrained spaces.

Original authors: Shuyang Shi, Kaixian Qu, Changan Chen, Ines Kast, Yuntao Ma, Marco Hutter

Published 2026-07-03

📖 5 min read🧠 Deep dive

CC BY 4.0

Original authors: Shuyang Shi, Kaixian Qu, Changan Chen, Ines Kast, Yuntao Ma, Marco Hutter

Original paper licensed under CC BY 4.0 (http://creativecommons.org/licenses/by/4.0/). ✨ This is an AI-generated explanation of the paper below. It is not written or endorsed by the authors. For technical accuracy, refer to the original paper. Read full disclaimer

Imagine you are trying to guide a very specific robot through a complex, multi-story building filled with narrow hallways, stairs, and overhanging obstacles. The robot isn't a simple ball that can roll in any direction; it's more like a rectangular box (or a dog) that has a specific "front" and "back." If it tries to squeeze through a narrow door sideways, it might fit, but if it tries to go through head-first, it might get stuck.

This paper introduces a new way of drawing the "map" for this robot, called the SE(2) Navigation Mesh. Here is how it works, broken down into simple concepts:

1. The Problem: The Old Maps Were Too Simple

Previous methods for robot navigation used maps that treated the robot like a perfect circle (a cylinder).

The Flaw: Imagine a long, narrow hallway that is just wide enough for a rectangular robot to pass through sideways, but not wide enough for it to pass through head-first.
The Old Map's Mistake: Because the old map assumed the robot was a circle, it would look at that hallway and say, "This is too narrow for a circle, so the robot can't go there." It would block off the path entirely, even though the robot could actually make it if it turned its body the right way.
The Result: The robot would get stuck or take huge, inefficient detours because the map didn't understand that the robot's shape changes depending on which way it is facing.

2. The Solution: A "3D" Map with Direction

The authors created a new map that doesn't just track where the robot is (left/right, forward/back), but also which way it is facing (its "yaw").

The Analogy: Think of the old map as a flat 2D floor plan. The new map is like a multi-layered cake.
- Each "layer" of the cake represents the map from a different angle (e.g., Layer 1 is the map if the robot faces North, Layer 2 if it faces Northeast, etc.).
- If a hallway is too narrow for the robot facing North, that hallway is "blocked" on the North layer.
- But if the robot can fit through that same hallway facing East, that hallway is "open" on the East layer.
The Magic: The map connects these layers. It knows that the robot can move from the "North layer" to the "East layer" by spinning in place. This allows the robot to plan a path where it might need to turn a corner before entering a narrow passage to fit through.

3. How the Robot Finds Its Way (The "ASA" Strategy)

Once the map is built, the robot needs to find a path. The authors use a three-step strategy called ASA (A*-String Pulling-A*):

The Rough Sketch (A Search):* The robot first finds a rough path through the "cake layers." It figures out which rooms and hallways to visit and which way to face in each one.
Straightening the Rope (String Pulling): The rough path is often zigzaggy because it was forced to follow the edges of the map layers. The robot then "pulls a string" tight between the start and finish, smoothing out the path to make it as straight as possible, like pulling a rope taut through a series of poles.
Fine-Tuning the Turn (Yaw Refinement): Now that the path is straight, the robot re-checks its turning angles. It makes sure that at every point on the straight line, the robot is actually facing a direction that fits through the walls. It adjusts the turns to be perfectly efficient.

4. Building the Map While Walking (Online Generation)

Usually, you need a complete 3D scan of a building before you can make a map. This paper introduces a way to build the map while the robot is walking.

The Analogy: Imagine the robot is painting a mural on a wall as it walks. Instead of repainting the entire wall every time it takes one step (which would be slow), it only repaints the small section of the wall it just saw.
The Result: The robot can explore a new, unknown building, build its own map in real-time, and start navigating immediately, even as it discovers new rooms or stairs.

5. What They Proved

The authors tested this on a real robot (a legged robot that looks like a dog) and in computer simulations:

More Space: Their new map found 50% more usable space than old maps. It successfully identified narrow passages that the old maps thought were impossible to cross.
Better Paths: The robot took shorter, smoother paths and spent less time planning.
Real-World Success: They successfully navigated the robot up stairs, through narrow doorways, and under overhanging obstacles in real life, all while building the map on the fly.

In summary: This paper gives robots a smarter map that understands their body shape and direction. It stops the robot from getting "confused" by narrow spaces and allows it to navigate complex, multi-story environments much more efficiently than before.

Technical Summary: SE(2) Navigation Mesh

Problem Statement

Global navigation for ground robots in complex, multi-level environments (e.g., industrial facilities with stairs, overhangs, and narrow passages) requires representations that accurately capture traversable regions while enabling efficient path planning. Existing approaches face significant limitations:

Point clouds and volumetric maps (e.g., occupancy grids, TSDFs) lack explicit surface structure, making fine-grained traversability estimation computationally expensive and requiring additional geometric inference.
Direct pathfinding on dense triangle meshes is computationally prohibitive for real-time planning.
Classical Navigation Meshes (NavMeshes) abstract traversable regions into convex polygons but assume yaw-invariant traversability. By approximating robots as cylinders and requiring feasibility for all headings, they fail to capture the specific constraints of non-circular robots (e.g., rectangular legged robots) in narrow spaces, often discarding valid paths or failing to represent multi-level structures effectively.

Methodology

The authors propose the SE(2) Navigation Mesh (SE(2) NavMesh), a polygonal representation that explicitly encodes yaw-dependent traversability. The system operates in two modes: offline generation from pre-built maps and online incremental generation from streaming point clouds.

1. Representation: SE(2) NavMesh

The method extends the classical NavMesh by incorporating the robot's yaw ( $\psi$ ) into the mesh construction, creating a state space $X = M \times S^1$ (surface $\times$ orientation).

Robot Model: Unlike the cylindrical approximation in classical NavMeshes, this method uses a cuboid approximation (length, width, height) to better represent the geometry of legged robots.
Yaw Channels: The continuous yaw interval $[0, 2\pi)$ is discretized into $N_\Psi$ channels.
Footprint Masks: Traversability is evaluated using continuous-yaw footprint masks. These masks represent the union of occupied areas swept by the robot footprint over a yaw interval, ensuring safe rotation between adjacent headings.
Layered Structure: The traversable space is partitioned into yaw-specific traversability layers ( $L_i$ $L_{i}$ ). Each layer contains regions where the robot can traverse with a specific heading.
- Safe Regions: Feasible for all discretized yaw angles.
- Restricted Regions: Feasible only for a subset of yaw angles.
Connectivity: The navigation graph includes two types of edges:
- Translational Connectivity: Between regions within the same yaw layer (movement without changing heading).
- Rotational Connectivity: Between layer-specific regions of the same spatial location but different yaw layers (in-place rotation).

2. Generation Pipeline

Offline: The environment (triangle mesh) is voxelized. A distance map is computed to classify voxels as safe, restricted, or inaccessible based on the robot's footprint and traversal constraints (step height, slope). A watershed algorithm partitions these voxels into convex polygons, which are then merged across tiles to form the global mesh.
Online: The system uses Voxblox for incremental geometry reconstruction from point clouds. To maintain real-time performance, updates are local. The map is divided into vertical slabs; only slabs affected by new geometry and their immediate neighbors are re-processed. This avoids the linear latency growth associated with global re-generation.

3. Pathfinding Strategy: ASA

The authors introduce a three-stage A-String Pulling-A (ASA)** strategy to optimize both position and heading:

Initial Search: An A* search on the navigation graph finds a feasible sequence of layer-specific regions and states.
Path Straightening: A string-pulling algorithm geometrically refines the path to remove zigzagging, producing a straightened path within the corridor defined by the region sequence.
Yaw Refinement: A second A* search optimizes the robot's heading along the straightened path, ensuring the final trajectory respects yaw-dependent traversability constraints.

Key Contributions

SE(2) NavMesh Representation: A novel polygonal representation that encodes yaw-dependent feasibility using footprint masks and a layered graph structure. It captures significantly more traversable area (especially in narrow passages) than classical NavMeshes.
ASA Pathfinding Strategy: A hierarchical planning approach that decouples geometric path straightening from heading optimization, reducing geometric path length by 6.2% and final path cost to 87% of the initial search cost.
Online Generation Framework: An efficient method for incrementally constructing and updating SE(2) NavMeshes from streaming point clouds using local slab-based updates, enabling real-time deployment on onboard hardware.

Experimental Results

Simulation (Traversability): Across six diverse indoor environments (including multi-level scenes), SE(2) NavMesh captured over 50% more traversable area than classical NavMeshes. It successfully bridged previously disconnected safe regions by utilizing restricted regions.
Simulation (Planning): The ASA pipeline consistently outperformed sampling-based baselines (RRT, RRT*, PRM) in constrained environments, achieving higher Success Weighted by Path Cost (SPC) and lower planning times. In open spaces, sampling-based methods were faster but produced higher-cost paths.
Real-World Deployment: Experiments on an ANYmal legged robot demonstrated successful online generation and navigation in complex scenarios, including:
- Multi-level environments with stair traversal.
- Narrow corridors (0.8m wide, only 0.27m wider than the robot).
- Navigation under overhanging obstacles.
- The system maintained a 4 Hz update rate using local updates, whereas global updates failed to sustain real-time performance as the map grew.

Significance and Claims

The paper claims that SE(2) NavMesh addresses a critical gap in ground robot navigation by explicitly modeling the relationship between robot heading and traversability. By moving beyond the "circular footprint" assumption of classical NavMeshes, the method enables reliable navigation for non-circular robots in constrained, multi-level environments where traditional approaches fail or are overly conservative. The integration of an online generation pipeline further establishes the feasibility of using this representation for autonomous exploration in unknown, dynamic environments without prior maps. The work is presented as a step toward more robust global navigation for legged robots in industrial and infrastructure inspection tasks.