
Efficient Trajectory Optimization using a Sparse Model
Christoph R
¨
osmann
1
, Wendelin Feiten
2
, Thomas W
¨
osch
2
, Frank Hoffmann
1
and Torsten Bertram
1
Abstract— The ”timed elastic band” approach optimizes
robot trajectories by subsequent modification of an initial tra-
jectory 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.
I. INTRODUCTION
Trajectory planning finds an optimal collision free trajec-
tory that complies with the robots kinematic and dynamic
motion constraints. This paper focuses on trajectory modi-
fication assuming that a global planner generated an initial
feasible path beforehand [1]. In particular in the context of
service robotics the dynamic modification of a preplanned
path is preferable over offline trajectory planning. Online
modification copes with changes of a dynamic environment
by incorporating the most recent sensor data for local re-
finement of the trajectory. In most realistic applications the
model of the environment is subject to continuous change
due to partial, incomplete maps and dynamic obstacles.
Furthermore, the (re-)computation of a large scale global
path is often not feasible in real-time applications. This
observation leads to approaches which modify a path locally,
such as the ”elastic band” proposed by [2], [3].
Later the original approach was extended to nonholonomic
kinematics [4], [5], [6] and robotic systems with many
degrees of freedom [7]. [8] proposed a method, where an
initial path is deformed using optimization techniques. The
trajectory, i.e. the velocities along the path, are not optimized.
The time parameter is used to control the modifications of
1
Institute of Control Theory and Systems Engineering, Technische Uni-
versit
¨
at Dortmund, Germany
2
Siemens Corporate Technology, Research Group Robotics, Germany
the path as the optimization proceeds. The planner considers
the nonholonomic constraints.
[9] deals with the deformation of trajectories rather than
paths by an explicit consideration of temporal information.
The deformation is decomposed into an obstacle avoidance
step using repulsive forces and a connectivity maintenance
step. Based on this work [10] proposes a single step approach
that combines external deformation with internal connec-
tivity forces. Both methods support general state transition
models and allow for spatial-temporal obstacle avoidance.
In contrast, our approach is based on a graph-optimization
formulation, operates with general optimization solvers and
time optimality is an explicit objective. Other methods which
directly optimize trajectories are presented in [11], [12].
In our case, a parametric path is augmented with velocity
profiles that respect the kinodynamic constraints of the
platform. The approach starts with an initial path found by
a global planner and represents it by a compact spline-based
path model also used in [13]. This path model exposes a set
of higher-level parameters to the optimization that iteratively
adapts the shape of the curvature continuous path to reduce
an objective function such as the time of travel. The main
difference to our work is that it trades in the precision of the
analytic model for a discretized trajectory model that allows
it to employ a highly sophisticated, efficient optimization
algorithm, enabling trajectory refinement in real-time.
Most recent approaches for trajectory modification dealing
with robot arms with many degrees of freedom use a
discretized representation of the trajectory in configuration
space (see [15], [16]). The proposed objective function
contains a finite difference matrix to smooth the resulting
trajectory and to additionally satisfy constraints like obstacle
avoidance. The CHOMP algorithm relies on a covariant
gradient descent method which explicitly requires the gra-
dients of each objective, whereas STOMP uses a stochastic
trajectory optimization technique without explicit knowledge
of gradients. Both approaches include temporal information
only in implicit manner by defining a specific discretization
and task duration. The differences to our approach are
detailed in Section IV.
In [17] the authors introduced a new approach called
”timed elastic band” which explicitly augments ”elastic
band” with temporal information. The proposed extension
allows the consideration of the robot’s dynamic constraints
and direct modification of trajectories rather than paths.
The ”timed elastic band” is formulated as a scalarized
multi-objective optimization. The structure of the underlying
optimization problem is sparse as most objectives are local
in that they only depend on a few consecutive configurations
2013 European Conference on Mobile Robots (ECMR)
Barcelona, Spain, September 25-27, 2013
978-1-4799-0263-7/13/$31.00 ©2013 IEEE 138