Multi-Modal Motion Planning Using Composite Pose Graph Optimization

This video showcases experiments for our recent paper entitled "Multi-Modal Motion Planning Using Composite Pose Graph Optimization". The paper will be presented at the 2021 International Conference on Robotics and Automation (ICRA 2021)​ and is available at https://arxiv.org/abs/2107.02384. We present a motion planning framework for multi-modal vehicle dynamics. Our approach employs transcription of the cost function, vehicle dynamics, and state and control constraints into a sparse factor graph. By formulating the motion planning problem in pose graph form, we enable the use of 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. We evaluate planned trajectories in real-life flight experiments using a vertical take-off and landing (VTOL) fixed-wing aircraft that can transition between hover and horizontal flight modes.