Sailing Through Point Clouds: Safe Navigation Using Point Cloud Based Control Barrier Functions

Bolun Dai, Rooholla Khorrambakht, Prashanth Krishnamurthy, Farshad Khorrami

Abstract

The capability to navigate safely in unstructured environments is crucial for deploying robotic systems in real-world scenarios. Control Barrier Function (CBF) based approaches have recently emerged as highly effective tools for synthesizing safety-critical controllers. However, traditional CBF methods often struggle with raw sensor data integration and the tendency to get stuck in local minima. In this work, we propose a novel CBF-based local planner comprised of two components: Vessel and Mariner. Vessel is a novel scaling factor-based CBF formulation that synthesizes safety constraints directly from raw point cloud data using higher-order ellipsoids. Mariner is a GPU-accelerated preview control framework designed to mitigate spurious equilibria during navigation. We demonstrate the efficacy of our approach through comparative simulations and experimental studies on Unitree B1 and Unitree Go2 quadruped robots.

Introduction: The Challenges of Local Planning

Navigating a robot safely usually requires a trade-off between computational speed and path accuracy. While global planners like RRT* are effective for long-range pathfinding, they are often too computationally expensive to run in real-time as a robot encounters dynamic or previously unmapped obstacles. Local planners, particularly those utilizing Control Barrier Functions (CBFs), are popular because they transform complex safety constraints into simple linear inequalities, enabling high-frequency control loops.

Despite their advantages, standard CBFs face two major hurdles:

  1. The Representation Gap: Most CBFs require the environment to be simplified into geometric primitives (e.g., spheres or boxes). Converting raw LiDAR point clouds into these shapes is computationally slow and prone to errors that can lead to collisions or overly conservative behavior.
  2. Myopia: CBFs are inherently "myopic," typically looking only one step ahead. This often causes robots to get stuck in "spurious equilibria," where the robot stops in front of an obstacle because it lacks the look-ahead capability to find a path around it.

Methodology

Vessel: A Differentiable Point-Cloud CBF

The Vessel component addresses the representation gap by operating directly on raw 3D point cloud data. Instead of simplifying the environment, Vessel represents the robot as a higher-order ellipsoid—a flexible geometric shape that can be tuned to look like a rounded box.

By calculating a scaling factor for this ellipsoid relative to every point in the incoming LiDAR cloud, the system determines the robot's proximity to obstacles. To ensure the formulation is usable for optimization, the authors employ a "softmin" function. This allows the controller to calculate smooth gradients across thousands of discrete points, maintaining a "safe set" without the need for pre-processing data into a traditional map.

Evolution of the higher-order ellipsoid primitive as the degree d increases, allowing for tighter robot bounding.

As shown in the figure above, increasing the degree $d$ allows the primitive to more tightly encapsulate the robot's actual geometry. This precision prevents the "overbounding" problem common with spherical approximations, enabling the robot to navigate through tighter gaps safely.

Mariner: GPU-Accelerated Preview Control

To solve the problem of myopia, Mariner acts as a high-speed preview controller. It utilizes a "needle" approach, projecting hundreds of potential paths into the configuration space simultaneously.

The Mariner preview controller projects multiple 'needles' to identify collision-free paths toward a local target.

Leveraging NVIDIA Warp for GPU acceleration, the system checks these needles against the point cloud in parallel. Mariner identifies which needles are collision-free and selects the one that brings the robot closest to its global goal. This look-ahead provides the necessary context to guide the CBF-based controller around obstacles rather than simply stopping in front of them.

Experimental Evaluation and Results

The framework was evaluated using a combination of simulation and real-world hardware:

  • Simulation: 50 randomized environments were created in the MuJoCo physics engine, featuring varied obstacle densities.
  • Hardware: Experiments were conducted on Unitree B1 and Unitree Go2 quadruped robots equipped with 16-channel 3D LiDAR.
  • Integration: The local planner was integrated with the Open Motion Planning Library (OMPL) implementation of RRT* for global guidance and the SLAM toolbox for localization.

Key Results

  • Computational Efficiency: The Vessel CBF computation takes less than 1ms on an NVIDIA Jetson Orin, even when processing 100,000 points.
  • Navigation Success: In comparative simulations, the Vessel + Mariner combination achieved a 100% success rate, producing paths that were significantly shorter and smoother than existing point-cloud CBF baselines.
  • Real-World Robustness: Physical tests confirmed the robot could successfully navigate around "unseen" obstacles—such as boxes placed in its path after the global map was generated—by relying on real-time LiDAR processing.

Conclusion and Future Work

The Vessel and Mariner framework demonstrates that direct processing of point clouds, combined with high-speed preview control, can significantly enhance the safety and agility of autonomous robots. While the system greatly reduces the likelihood of getting stuck, the authors note that it does not yet provide absolute mathematical guarantees against spurious equilibria in extremely complex "trap" geometries.

Future research will focus on incorporating explicit actuator constraints (e.g., torque limits) into the CBF-QP (Quadratic Program) and exploring more complex needle shapes to enable even more agile maneuvering in highly dynamic environments.

Summary updated

Abstract

The capability to navigate safely in an unstructured environment is crucial when deploying robotic systems in real-world scenarios. Recently, control barrier function (CBF) based approaches have been highly effective in synthesizing safety-critical controllers. In this work, we propose a novel CBF-based local planner comprised of two components: Vessel and Mariner. The Vessel is a novel scaling factor based CBF formulation that synthesizes CBFs using only point cloud data. The Mariner is a CBF-based preview control framework that is used to mitigate getting stuck in spurious equilibria during navigation. To demonstrate the efficacy of our proposed approach, we first compare the proposed point cloud based CBF formulation with other point cloud based CBF formulations. Then, we demonstrate the performance of our proposed approach and its integration with global planners using experimental studies on the Unitree B1 and Unitree Go2 quadruped robots in various environments.
BibTeX
@article{dai2024sailing,
  author = {Bolun Dai and Rooholla Khorrambakht and Prashanth Krishnamurthy and Farshad Khorrami},
  title = {{Sailing Through Point Clouds: Safe Navigation Using Point Cloud Based Control Barrier Functions}},
  journal = {IEEE Robotics Autom. Lett.},
  year = {2024},
  url = {https://openreview.net/forum?id=XQwoLbvhQt},
}
Figure