Motion planning with dynamic constraints through pose graph optimization

Abstract

This contribution is an optimization-based method for robotic path-planning that is able to recover vehicle controls in addition to discovering an optimized, feasible trajectory from start to goal for vehicles with arbitrary dynamics. The motion planner extends the application of factor-graph optimization commonly used in simultaneous localization and mapping tasks to the path-planning task, specifically the "timed elastic band" trajectory optimization approach [1] for control input extraction functionality. This is achieved by the introduction of control input-dependent vertices into the factor-graph along with a way to systematically design dynamics violation costs without relying on hand-picked geometric parameters. An implementation of the planner successfully recovers vehicle control inputs and produces feasible trajectories in simulation testing.