Continuous-Safe Navigation in Unknown Cluttered Environments via Direction-Aware Convex Free-Region Generation

Overview

Convex free regions provide a structured and optimization-friendly representation of collision-free space, but enlarging local free space only according to surrounding obstacle geometry can be insufficient in cluttered environments. In narrow passages, the generated region may fail to both accommodate robot geometry and preserve traversable extension along candidate motion directions. In addition, when robot geometry is modeled explicitly, checking safety only at discretized trajectory samples does not guarantee continuously collision-free motion.

To address these issues, FRGraph jointly incorporates candidate motion directions and robot geometry into convex free-region generation, and achieves continuously collision-free motion through continuous-safe trajectory generation. Within each region, the framework performs geometry-aware target pose selection and trajectory generation, together with Lipschitz-based continuous safety certification and local refinement. The resulting free regions and candidate motions are maintained in a region-based graph to support incremental planning.

---

Key Features

• Direction-aware convex free-region generation
  Candidate navigation directions are incorporated into obstacle selection and hyperplane optimization through a direction-biased QP formulation, producing free regions that better preserve traversable space while accommodating robot geometry.

• Continuous-safe trajectory generation
  The framework combines Bézier trajectory parameterization with Lipschitz-based adaptive interval subdivision for continuous safety certification, together with a local QP-based control-point refinement scheme for iterative violation correction.

• Region-based graph for incremental planning
  Generated free regions and candidate motions are organized in a region-based navigation graph, enabling incremental planning and recovery from locally unproductive branches.

---

Demo

2D Navigation in Cluttered Environments

The proposed framework enables reliable collision-free navigation in cluttered 2D environments across a wide range of obstacle densities.

---

3D Navigation with Dead-End Handling

The maintained region-based graph allows the planner to backtrack from dead-end branches and continue navigation toward the goal.

---

Real-World Experiments

The framework transfers effectively to real robots and maintains collision-free navigation in cluttered real-world scenarios.

---

Results at a Glance

• Quantitative 2D experiments show that the proposed method generates free regions better aligned with downstream traversal and enables reliable collision-free navigation.
• Additional 3D experiments demonstrate dead-end handling and backtracking enabled by the maintained region-based graph.
• Real-world experiments validate the practical applicability of the framework in both 2D and 3D cluttered environments.

---

Installation

This project is built in a ROS catkin workspace.

Dependencies

Before building FRGraph, please make sure the following dependency is available:

• catkin_simple

In addition, the DecompROS module used in this project is adapted from the original implementation:

• DecompROS

Build Instructions

1. Clone this repository into your catkin workspace:

cd ~/catkin_ws/src
git clone <your_repo_url>

2. Clone catkin_simple into the third_party directory:

cd FRGraph/third_party
git clone https://github.com/catkin/catkin_simple.git

3. Go back to the workspace root and build the decomp_* packages first:

cd ~/catkin_ws
catkin build decomp_*

4. Build the whole workspace:

catkin build

If all steps finish successfully, FRGraph is ready to use.

---

Quick Start (2D Example)

roslaunch simple_robot simple_robot.launch if_3d_lidar:=0
roslaunch planner_manager planner.launch

---

Usage

FRGraph consists of two main components:

• A robot interface (simulation or real robot)
• The planner (planner_manager)

1. Launch the Robot

You can launch the provided example robot:

roslaunch simple_robot simple_robot.launch if_3d_lidar:=0

This launch file provides a simple robot setup and can be replaced with your own robot system.

---

2. Launch the Planner

roslaunch planner_manager planner.launch

---

Running in 2D vs 3D Environments

2D Mode

To run in a 2D environment:

1. Launch the robot with 2D LiDAR:

roslaunch simple_robot simple_robot.launch if_3d_lidar:=0

2. Set the environment type in:

src/planner_manager/config/config.yaml

env_type: 0

3. Set robot geometry:

• Modify the robot vertex coordinates in the configuration file according to your robot shape.

4. Required topics:

• Point cloud (2D LiDAR):
  /scan
• Odometry:
  /odom

---

3D Mode

To run in a 3D environment:

1. Launch the robot with 3D LiDAR:

roslaunch simple_robot simple_robot.launch if_3d_lidar:=1

2. Set the environment type:

env_type: 1

3. Set robot geometry:

• Update the robot vertex coordinates to match your robot model.

4. Required topics:

• Point cloud (3D LiDAR):
  /velodyne_points
• Odometry:
  /odom

---

Input Requirements

The planner requires the following inputs:

• Sensor data (single-frame point cloud)

  • 2D mode: /scan
  • 3D mode: /velodyne_points

• Robot state (odometry)

  • /odom

Make sure these topics are correctly published before launching the planner.

---

Notes

• The provided simple_robot is only for demonstration. You can replace it with your own robot as long as the required topics are provided.
• The robot geometry (vertex representation) must be consistent with your actual robot to ensure correct collision checking.
• Ensure that the topic names match or are properly remapped if different in your system.

---

Acknowledgment

This project builds upon the open-source project DecompROS, which provides the foundation for convex decomposition and free-region generation.
We thank the authors for making their implementation publicly available.

The DecompROS module in this repository has been adapted and modified to fit our framework.

---