Research Article

Trajectory Planning in Complex Environments for Static and Moving Obstacles

Abstract: This paper presents a piecewise-linear approach for motion planning in uncertain environments punctured with static and moving obstacles. In such environments robots may encounter obstacles with unknown sizes, shapes, and locations. The probability of a robot colliding with an unknown obstacle entails a risk. The proposed approach offers a trajectory planning method that outputs a continuous-time optimal (shortest) trajectory that is guaranteed to have a bounded risk over the planning horizon. The optimal length of the path is determined by a moment optimization approach, and a hierarchy of semidefinite programs that yield increasingly finer lower bounds. In our method, we do not require any time discretization to handle continuous constraints. Using convex methods based on sum of squares (SOS) optimization, we produce continuous-time risk bounded trajectories without time discretization by solving the obtained non-convex time-varying optimization problem. The presented approach can be used for online trajectory planning problems, and it takes into account arbitrary probabilistic uncertainties and non-convex and nonlinear obstacles.