Trajectory optimization on manifolds with applications to quadrotor systems

Manifolds are used in almost all robotics applications even if they are not modeled explicitly. We propose a differential geometric approach for optimizing trajectories on a Riemannian manifold with obstacles. The optimization problem depends on a metric and collision function specific to a manifold. We then propose our safe corridor on manifolds (SCM) method of computationally optimizing trajectories for robotics applications via a constrained optimization problem. Our method does not need equality constraints, which eliminates the need to project back to a feasible manifold during optimization. We then demonstrate how this algorithm works on an example problem on SO(3) and a perception-aware planning example for visual-inertially guided robots navigating in three dimensions. Formulating field of view constraints naturally results in modeling with the manifold R3xS2 , which cannot be modeled as a Lie group. We also demonstrate the example of planning trajectories on SE(3) for a formation of quadrotors within an obstacle filled environment.