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

Bolun Dai, Rooholla Khorrambakht, Prashanth Krishnamurthy, Farshad Khorrami

TL;DR

This paper introduces a two-part local planning framework—Vessel and Mariner—that allows robots to navigate safely through unstructured environments using raw 3D point cloud data. By combining a novel, differentiable Control Barrier Function (CBF) with a GPU-accelerated preview controller, the system ensures collision avoidance while preventing the robot from getting stuck in the "local minima" common in safety-critical control.

What Problem Does This Paper Address?

Navigating a robot safely usually requires a trade-off between speed and accuracy. Traditional global planners (like RRT) are computationally expensive to run in real-time as a robot moves. Local planners, specifically those using Control Barrier Functions (CBFs)*, are popular because they turn complex safety constraints into simple linear ones, allowing for very fast control loops.

However, CBFs face two major hurdles:

  1. Representation Gap: Most CBFs require the environment to be simplified into geometric primitives (like spheres or boxes). Converting raw sensor data (point clouds) into these shapes is slow and prone to errors.
  2. Myopia: CBFs are "myopic"—they only look one step ahead. This often causes robots to get stuck in "spurious equilibria," where the robot stops dead in front of an obstacle because it doesn't "see" a path around it.

Methods

The authors propose a local planner composed of two novel components:

1. Vessel (The Safety Filter)

Vessel is a point-cloud-based CBF formulation. Instead of simplifying the environment, it represents the robot as a higher-order ellipsoid (a flexible shape that can look like a rounded box).

  • Scaling Factor: It calculates a scaling factor for this ellipsoid relative to every point in the incoming LiDAR cloud.
  • Differentiability: To make this usable for optimization, they use a "softmin" function, which allows the controller to calculate smooth gradients even when dealing with thousands of discrete points. This ensures the robot maintains a "safe set" without needing to pre-process the data into a map.

2. Mariner (The Preview Controller)

To solve the "getting stuck" problem, Mariner acts as a high-speed preview controller.

  • The "Needle" Approach: It projects hundreds of potential paths (called "needles") into the configuration space.
  • GPU Acceleration: Using NVIDIA Warp, the system checks these needles against the point cloud in parallel.
  • Path Selection: It identifies which needles are collision-free and chooses 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 just stopping in front of them.

Datasets

The researchers evaluated their framework using:

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

Key Insights and Ideas

  • Direct Processing is Key: By operating directly on point clouds, the system avoids the "overbounding" problem (where a robot is treated as a giant sphere, preventing it from fitting through tight gaps) and the latency of environment decomposition.
  • Higher-Order Ellipsoids Matter: Moving beyond simple spheres to higher-order ellipsoids allows the "Vessel" to more tightly encapsulate the robot's actual geometry, enabling safer navigation in cramped spaces.
  • Preview vs. Prediction: Instead of solving a heavy Nonlinear Model Predictive Control (NMPC) problem, the "needle" approach provides the benefits of a long-horizon preview at a fraction of the computational cost by discretizing the search space and leveraging the GPU.

Key Results

  • Efficiency: The Vessel CBF computation takes less than 1ms on an NVIDIA Jetson Orin, even with 100,000 points.
  • Performance: In comparative simulations, the Vessel + Mariner combo achieved a 100% success rate. It produced paths that were significantly shorter and smoother than existing point-cloud CBF baselines.
  • Robustness: Real-world tests showed the robot successfully navigating around "unseen" obstacles (like boxes added after the global map was built) by relying on the local planner's real-time LiDAR processing.

Limitations and Future Work

While the framework is highly effective, the authors note a few areas for improvement:

  • No Absolute Guarantees: While Mariner greatly reduces the chance of getting stuck, it does not mathematically guarantee that the robot will never find a spurious equilibrium in extremely complex "trap" geometries.
  • Input Constraints: The current formulation does not explicitly account for the physical limits of the robot's actuators (e.g., maximum torque or motor saturation).
  • Future Directions: The team plans to incorporate these input constraints into the CBF-QP (Quadratic Program) and explore more complex "needle" shapes for even more agile maneuvering.

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 letter, 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},
}

Loading related content...

Figure