3D-OGSE: Probabilistically Complete Online Safe and Smooth Trajectory Generation using Generalized Shape Expansion in Unknown 3-D Environments

We present an online motion planning algorithm (3D-OGSE) 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-OGSE constructs an obstacle-free region termed 'generalized shape' based on the locally-sensed environment information. A collision-free path is computed by sampling points in the generalized shape and is used to generate a smooth, time-parametrized trajectory by minimizing snap. The generated trajectories are constrained to lie within the generalized shape, which ensures the agent maneuvers in the locally obstacle-free space. As the agent reaches the boundary of the 'sensing 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 present theoretical guarantees for probabilistic completeness 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 in scenarios with sensor noise and constraints on sensor's field-of-view (FOV). We observe that each re-planing computation takes 1.4 milliseconds on a single thread of an Intel Core i5-8500 3.0 GHz CPU, which is significantly faster (4-10 times) than several existing algorithms. In addition, we observe 3D-OGSE to be less conservative in complex scenarios such as narrow passages.