Multimodal trajectory optimization for motion planning

Existing motion planning methods often have two drawbacks: (1) goal configurations need to be specified by a user, and (2) only a single solution is generated under a given condition. In practice, multiple possible goal configurations exist to achieve a task. Although the choice of the goal configuration significantly affects the quality of the resulting trajectory, it is not trivial for a user to specify the optimal goal configuration. In addition, the objective function used in the trajectory optimization is often non-convex, and it can have multiple solutions that achieve comparable costs. In this study, we propose a framework that determines multiple trajectories that correspond to the different modes of the cost function. We reduce the problem of identifying the modes of the cost function to that of estimating the density induced by a distribution based on the cost function. The proposed framework enables users to select a preferable solution from multiple candidate trajectories, thereby making it easier to tune the cost function and obtain a satisfactory solution. We evaluated our proposed method with motion planning tasks in 2D and 3D space. Our experiments show that the proposed algorithm is capable of determining multiple solutions for those tasks.