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