3D-Online Generalized Sensed Shape Expansion: A Probabilistically Complete Motion Planner in Obstacle-Cluttered Unknown Environments

Abstract

We present an online motion planning algorithm (3D-OGSSE) for generating smooth, collision-free trajectories over multiple planning iterations for a 3-D agent operating in an unknown, obstacle-cluttered, 3-D environment. In each planning iteration, 3D-OGSSE constructs an obstacle-free region termed “generalized sensed shape” based on the locally-sensed environment information and the notion of generalized shape. A collision-free path is computed by sampling points in the generalized sensed shape and is used to generate a smooth, time-parametrized trajectory by minimizing snap. The generated trajectory at every planning iteration is constrained to lie within generalized sensed shape, which ensures the agent maneuvers in locally obstacle-free space. As the agent reaches the boundary of the generalized sensed shape in a planning iteration, a re-plan is triggered by a receding horizon planning mechanism that also enables the initialization of the next planning iteration. We also present a theoretical guarantee for probabilistic completeness of the developed algorithm over the entire environment and for completely collision-free trajectory generation. We evaluate the proposed method in simulation on complex 3-D environments with varied obstacle-densities. Further, we also evaluate it in scenarios with sensor noise and constraints on the on-board sensor's field-of-view (FOV). We observe that each planning iteration computation takes <inline-formula xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink"><tex-math notation="LaTeX">$\sim 14$</tex-math></inline-formula> milliseconds on a single thread of an Intel Core i5-8500 3.0 GHz CPU, which is significantly faster than several existing algorithms. In addition, we also observe 3D-OGSSE to be less conservative in complex scenarios such as narrow passages.