Integration of Local Geometry and Metric Information in Sampling-Based Motion Planning

The efficiency of sampling-based motion planning algorithms is dependent on how well a steering procedure is capable of capturing both system dynamics and configuration space geometry to connect sample configurations. This paper considers how metrics describing local system dynamics may be combined with convex subsets of the free space to describe the local behavior of a steering function for sampling-based planners. Subsequently, a framework for using these subsets to extend the steering procedure to incorporate this information is introduced. To demonstrate our framework, three specific metrics are considered: the LQR cost-to-go function, a Gram matrix derived from system linearization, and the Mahalanobis distance of a linear-Gaussian system. Finally, numerical tests are conducted for a second-order linear system, a kinematic unicycle, and a linear-Gaussian system to demonstrate that our framework increases the connectivity of sampling-based planners and allows them to better explore the free space.