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

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 general affine control systems. This work uses CBF for safety guarantees and LQRs for optimal control synthesis during edge extensions. Traditional CBF methods involve Quadratic Programs (QPs), which add computational overhead and can sometimes be infeasible. Conversely, LQR-based controllers typically employ first-order Taylor approximations for nonlinear systems, necessitating consistent recalculations. To enhance motion planning efficiency, our framework directly verifies CBF constraints during the planning process, thereby eliminating the need for QP solutions. Additionally, we cache optimal LQR gain matrices in a hash table to bypass re-computation during local linearizations in the rewiring phase. To further boost sampling efficiency, we integrate the Cross-Entropy Method. Our results demonstrate that the proposed planner outperforms existing algorithms in computational efficiency and exhibits robust performance in real-world experiments.