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:
- 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.
- 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.
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.
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