LAUSR

Trajectory optimization methods for motion planning attempt to generate trajectories that minimize a suitable objective function. While such methods efficiently find solutions in static environments, they need to be ran from scratch multiple times in the presence of moving obstacles, which incurs unnecessary computation and slows down execution. In this paper, we propose a trajectory optimization algorithm that anticipates the movement of obstacles and solves the planning problem in an iterative manner. We employ continuous-time Gaussian processes as trajectory representations both for the mobile robot and moving obstacles for which future locations are predicted according to a given model. We formulate the simultaneous moving obstacles tracking and mobile robot motion planning problem as probabilistic inference on a factor graph. Since trajectories of moving obstacles are optimized concurrently to motion planning, the proposed approach works in a predictive manner. After computing the initial solution, we use incremental inference for online replanning after an estimate of the moving obstacle position is provided. Our experimental evaluation demonstrates that the proposed approach supports online motion generation in the presence of moving obstacles.