LQR-CBF-RRT*: Safe and Optimal Motion Planning (2304.00790v4)

Abstract: We present LQR-CBF-RRT*, an incremental sampling-based algorithm for offline motion planning. Our framework leverages the strength of Control Barrier Functions (CBFs) and Linear Quadratic Regulators (LQR) to generate safety-critical and optimal trajectories for a robot with dynamics described by an affine control system. CBFs are used for safety guarantees, while LQRs are employed for optimal control synthesis during edge extensions. Popular CBF-based formulations for safety critical control require solving Quadratic Programs (QPs), which can be computationally expensive. Moreover, LQR-based controllers require repetitive applications of first-order Taylor approximations for nonlinear systems, which can also create an additional computational burden. To improve the motion planning efficiency, we verify the satisfaction of the CBF constraints directly in edge extension to avoid the burden of solving the QPs. We store computed optimal LQR gain matrices in a hash table to avoid re-computation during the local linearization of the rewiring procedure. Lastly, we utilize the Cross-Entropy Method for importance sampling to improve sampling efficiency. Our results show that the proposed planner surpasses its counterparts in computational efficiency and performs well in an experimental setup.

• The paper demonstrates that integrating CBFs and LQRs into RRT* enhances both trajectory safety and optimality.
• It introduces computational efficiencies by avoiding continuous QP solving with direct CBF constraint checks and storing optimal LQR gains.
• Adaptive sampling via the Cross-Entropy Method significantly speeds up convergence, as validated by simulations and hardware experiments.

Safe and Optimal Motion Planning with LQR-CBF-RRT*

The paper presents a novel approach to offline motion planning through the development of the LQR-CBF-RRT* algorithm. This method advances motion planning by integrating Control Barrier Functions (CBFs) and Linear Quadratic Regulators (LQR) into a sampling-based planner to generate trajectories that are both safe and optimal for systems with affine control dynamics.

Key Methodological Contributions

1. Incorporation of CBFs and LQRs: Traditional motion planning frameworks often struggle with maintaining safety constraints and optimizing control inputs concurrently. This paper leverages CBFs to uphold safety by ensuring that the generated paths remain within predefined safe sets. Simultaneously, LQRs are employed to compute optimal control inputs during the edge extension process, optimizing a quadratic cost function over state and control variables. This dual approach not only maintains safety but also minimizes the path cost function effectively.
2. Efficiency Improvements for Motion Planning: The framework addresses key computational challenges associated with CBF and LQR usage. CBF-based approaches typically require solving Quadratic Programs (QPs), which can be computationally expensive. The authors circumvent this by directly checking CBF constraints during edge extensions, thus eliminating the need for continuous QP solving. For LQRs, which require recalculations through Taylor approximations in nonlinear settings, computed optimal LQR gain matrices are stored in a hash table, enhancing computational efficiency by avoiding redundant calculations.
3. Adaptive Sampling via Cross-Entropy Method: The Cross-Entropy Method is applied for importance sampling, significantly improving the sampling efficiency by adapting to promising regions of the state space, which facilitates faster convergence to optimal solutions.

Evaluation and Results

The algorithm demonstrates superior performance in both benchmark evaluations and a hardware experiment involving an omnidirectional robotic platform. In simulations, LQR-CBF-RRT* consistently outperformed several baseline methods, including naive adaptations and QP-reliant approaches, highlighting its efficiency improvements in terms of reduced computational time without compromising safety or path optimality. For instance, in the unicycle model experiment, the proposed method completed 2000 iterations significantly faster than approaches not employing hash-table-based optimizations or adaptive sampling. Furthermore, by avoiding QP solving during execution, the method achieved an 86% improvement in computational speed for linear systems in comparison to traditional QP-based methods.

In hardware, employing LQR-CBF-RRT* before transitioning to online trajectory tracking demonstrated its practical applicability. The experiments validated the algorithm’s capability to generate feasible, collision-free paths in cluttered environments. The generated paths, when followed by real-world robots equipped with online controllers, allowed for successful navigation in dynamic environments while adhering to safety constraints.

Theoretical Implications

From a theoretical standpoint, the probabilistic completeness and asymptotic optimality of LQR-CBF-RRT* align with canonical results in sampling-based motion planning. By leveraging local consistent extensions through LQR and utilizing state feedback constrained by CBFs, the planner effectively samples and optimizes over the space of admissible paths, ensuring scalability to higher-dimensional spaces and more complex dynamical systems.

Future Directions

Given its efficient handling of safety and optimality, future work may explore the adaptation of LQR-CBF-RRT* to real-time applications involving dynamic obstacles and the incorporation of model uncertainties. The investigation into more sophisticated CBF and LQR integration mechanisms with additional types of constraints, such as those arising from multiple agent interactions, could further enhance the applicability of this approach in complex robotic tasks.

In conclusion, the LQR-CBF-RRT* algorithm offers a robust, efficient, and mathematically grounded framework for safe and optimal motion planning, providing substantial improvements over existing methods in both computational efficiency and applicability to real-world scenarios.