Multi-modal motion planning using composite pose graph optimization

Abstract

This work presents a motion planning framework for multi-modal vehicle dynamics. An approach for transcribing cost function, vehicle dynamics, and state and control constraints into a sparse factor graph is introduced. By formulating the motion planning problem in pose graph form, the motion planning problem can be addressed using efficient optimization techniques, similar to those already widely applied in dual estimation problems, e.g., pose graph optimization for simultaneous localization and mapping (SLAM). Optimization of trajectories for vehicles under various dynamics models is demonstrated. The motion planner is able to optimize the location of mode transitions, and is guided by the pose graph optimization process to eliminate unnecessary mode transitions, enabling efficient discovery of optimized mode sequences from rough initial guesses. This functionality is demonstrated by using our planner to optimize multi-modal trajectories for vehicles such as an airplane which can both taxi on the ground or fly. Extensive experiments validate the use of the proposed motion planning framework in both simulation and real-life flight experiments using a vertical take-off and landing (VTOL) fixed-wing aircraft that can transition between hover and horizontal flight modes.