Efficient trajectory optimization using a sparse model

The "timed elastic band" approach optimizes robot trajectories by subsequent modification of an initial trajectory generated by a global planner. The objectives considered in the trajectory optimization include but are not limited to the overall path length, trajectory execution time, separation from obstacles, passing through intermediate way points and compliance with the robots dynamic, kinematic and geometric constraints. "Timed elastic bands" explicitly consider spatial-temporal aspects of the motion in terms of dynamic constraints such as limited robot velocities and accelerations. The trajectory planning operates in real time such that "timed elastic bands" cope with dynamic obstacles and motion constraints. The "timed elastic band problem" is formulated as a scalarized multi-objective optimization problem. Most objectives are local and relate to only a small subset of parameters as they only depend on a few consecutive robot states. This local structure results in a sparse system matrix, which allows the utilization of fast and efficient optimization techniques such as the open-source framework "g2o" for solving "timed elastic band" problems. The "g2o" sparse system solvers have been successfully applied to VSLAM problems. This contribution describes the application and adaptation of the g2o-framework in the context of trajectory modification with the "timed elastic band". Results from simulations and experiments with a real robot demonstrate that the implementation is robust and computationally efficient.