Computationally Efficient Motion Planning Algorithms for Agile Autonomous Vehicles in Cluttered Environments

Fast, real-time motion planning of an agile, autonomous vehicle in a cluttered environment, with many geometrically-fixed obstacles, is a very complex problem, especially because of the vehicle dynamics constraints and resourceconstrained computational capabilities onboard the vehicle. In this paper, we present computationally-efficient versions of our novel motion planning algorithm called the Spherical Expansion and Sequential Convex Programming (SE–SCP) algorithm. The SE–SCP algorithm first uses a spherical-expansion-based randomized sampling algorithm to explore the workspace. Once a path is found from the start position to the goal position, the algorithm computes a locally optimal trajectory, within its homotopy class for a desired cost function, by solving a sequence of convex optimization problems. Thus, the SE–SCP algorithm is anytime locally optimal and the trajectory is globally optimal if the number of samples tends to infinity. In this paper, we further enhance the computational efficiency of the SE– SCP algorithm using uni-directional and bi-directional rewiring techniques. We also present a detailed proof of the local optimality characteristics of the new SE–SCP algorithms for a special case of vehicle dynamics. Simulation examples involving quadrotor and spacecraft help demonstrate the effectiveness of our new algorithms.