The Dynamics Projection Filter (DPF) - real-time nonlinear trajectory optimization using projection operators

Robotic navigation applications often require on-line generation of trajectories that respect underactuated non-linear dynamics, while optimizing a cost function that depends only on a low-dimensional workspace (collision avoidance). Approaches to non-linear optimization, such as differential dynamic programming (DDP), suffer from the drawbacks of slow convergence by being limited to stay within the trust-region of the linearized dynamics and having to integrate the dynamics with fine granularity at each iteration. We address the problem of decoupling the workspace optimization from the enforcement of non-linear constraints. In this paper, we introduce the Dynamics Projection Filter, a nonlinear projection operator based approach that first optimizes a workspace trajectory with reduced constraints and then projects (filters) it to a feasible configuration space trajectory that has a bounded sub-optimality guarantee. We show simulation results for various curvature and curvature-derivatives constrained systems, where the dynamics projection filter is able to, on average, produce similar quality solution 50 times faster than DDP. We also show results from flight tests on an autonomous helicopter that solved these problems on-line while avoiding mountains at high speed as well as trees and buildings as it came in to land.