Control Barrier Function Augmentation in Sampling-based Control Algorithm for Sample Efficiency

Abstract: For a nonlinear stochastic path planning problem, sampling-based algorithms generate thousands of random sample trajectories to find the optimal path while guaranteeing safety by Lagrangian penalty methods. However, the sampling-based algorithm can perform poorly in obstacle-rich environments because most samples might violate safety constraints, invalidating the corresponding samples. To improve the sample efficiency of sampling-based algorithms in cluttered environments, we propose an algorithm based on model predictive path integral control and control barrier functions. The proposed algorithm needs fewer samples and time-steps and has a better performance in cluttered environments compared to the original model predictive path integral control algorithm.

• The paper integrates Control Barrier Functions into MPPI to guide sampling toward safe trajectories in obstacle-dense scenarios.
• The paper formulates a quadratic programming framework to enforce safe set invariance and adjust sample distributions for higher efficiency.
• The paper demonstrates improved performance in narrow passages, achieving reduced sample counts and time steps compared to standard MPPI.

Control Barrier Function Augmentation in Sampling-based Control Algorithm for Sample Efficiency

Introduction

The paper "Control Barrier Function Augmentation in Sampling-based Control Algorithm for Sample Efficiency" (2111.06974) explores the intersection of path planning and safety in robotics using sampling-based algorithms enhanced with Control Barrier Functions (CBFs). The study addresses the limitations of traditional sampling algorithms in obstacle-rich environments, where many samples may violate safety constraints, thus requiring efficient solutions to improve sample efficiency.

Problem Statement and Background

Stochastic path planning involves generating random sample trajectories to find optimal and safe paths for robots navigating from a start to a target position. Conventional deterministic models for solving these problems, like MPC and Bernstein polynomials, are computationally intensive, especially for nonlinear stochastic systems. In sampling-based methods, algorithms such as RRTs, Probabilistic Roadmaps, and MCMC are popular due to their applicability in parallel computing, allowing fast sampling of trajectories.

However, in scenarios like narrow passage problems, current sampling methods suffer from inefficiency as they spend excessive time identifying safe paths through small regions. Previous attempts have improved upon these issues using adaptive sampling and dynamic domain restriction. The paper proposes a novel hybrid approach combining MPPI with CBF to address these inefficiencies, leveraging the strengths of both methods.

Figure 1: Control architecture combining MPPI control with CBF to compensate for unsafe actions without considering the influence of CBF on the weights of the samples.

Control Barrier Functions in MPPI

Control Barrier Functions provide safety guarantees by ensuring the forward invariance of a predefined safe set. In the proposed methodology, CBF is incorporated into the sampling process of MPPI, controlling the distribution from which samples are drawn, thus guiding the algorithms to favor safe trajectories and improve sample efficiency.

The core principle involves an optimization problem reformulated with CBF constraints:

• CBF-Based QP Optimization: Formulated as a Quadratic Programming problem, it modulates sample distributions to prioritize safety using constraints ensuring the safe set's forward invariance.
• Stochastic Quadratic Programming: Incorporates probabilistic guarantees via chance constraints to ensure samples are drawn from regions likely to satisfy safety requirements.

Through semidefinite programming optimization, the approach recalibrates the sampling process to focus on trajectories within safe regions, thereby aligning the MPPI algorithm with the safety objectives.

Figure 2: We solve the SDP optimization for each sample and at each time step to find a safe mean and variance and sample a new safe control input.

Experimental Results

The paper presents empirical evaluations in two scenarios: single-obstacle and multi-obstacle narrow passage. The experiments demonstrate the algorithm's capability to effectively guarantee safety with improved sample efficiency over the traditional MPPI algorithm.

• Single-Obstacle Scenario: The MPPI-CBF algorithm achieves safety while maintaining optimal path-planning efficiency. This scenario highlighted the conservative nature of MPPI-CBF, avoiding obstacles while reaching the target efficiently.
• Multi-Obstacle Scenario: In cluttered environments, MPPI-CBF demonstrates superior sample efficiency, completing tasks using fewer samples and time steps compared to MPPI, largely due to its focus on sampling in safe regions.

Figure 3: MPPI-CBF algorithm for single-obstacle simulation.

Figure 4: Cost comparison between MPPI and MPPI-CBF algorithms in the single-obstacle scenario, showing similar time efficiency.

Figure 5: Trajectories from MPPI and MPPI-CBF algorithms in a narrow passage problem, demonstrating improved sample efficiency with MPPI-CBF.

Conclusion

The MPPI-CBF algorithm represents a significant step towards enhancing sample efficiency and safety in sampling-based path planning algorithms for robots. By combining MPPI with CBF practices, the approach ensures pathways through narrow passages are efficient and avoid obstacles. Future work suggests further optimization in computational aspects, particularly in solving SDP more efficiently, and theoretical analysis concerning deviations in the sample distributions compared to existing MPPI frameworks.