ROBOTIK 2012

Trajectory modification considering dynamic constraints

of autonomous robots
Christoph Rösmann, Wendelin Feiten, Thomas Wösch
Siemens Corporate Technology, Intelligent Systems and Control, Germany

Frank Hoffmann, Torsten Bertram

Institute of Control Theory and Systems Engineering, Technische Universität Dortmund, Germany

Topic: Research and Development / modelling, planning and control

Keywords: Trajectory modification, timed elastic band, dynamics, kinematics, autonomous robots

Abstract
The classic "elastic band" deforms a path generated by a global planner with respect to the shortest path length while
avoiding contact with obstacles. It does not take any dynamic constraints of the underlying robot into account directly.
This contribution introduces a new approach called "timed elastic band" which explicitly considers temporal aspects of
the motion in terms of dynamic constraints such as limited robot velocities and accelerations. The "timed elastic band"
problem is formulated in a weighted multi-objective optimization framework. Most objectives are local as they depend
on a few neighboring intermediate configurations. This results in a sparse system matrix for which efficient large-scale
constrained least squares optimization methods exist.
Results from simulations and experiments with a real robot demonstrate that the approach is robust and computationally
efficient to generate optimal robot trajectories in real time. The "timed elastic band" converts an initial path composed of
a sequence of way points into a trajectory with explicit dependence on time which enables the control of the robot in real
time. Due to its modular formulation the approach is easily extended to incorporate additional objectives and constraints.

1 Introduction to obtain dynamically feasible trajectories.

Motion planning is concerned with finding of a collision Task

free trajectory that respects the kinematic and dynamic mo-
tion constraints.
World Model Path Planning
In the context of motion planning this paper focuses on
local path modification assuming that an initial path has
Trajectory
been generated by a global planner [1]. In particular in the Local
Timed Elastic Band

Generation &
Obstacles
context of service robotics the modification of a path is a Modification

preferable approach due to the inherent uncertainty of the

dynamic environment since the environment may be dy- Position &
Control
Velocity
namic. Also, the model of the environment may subject to
change due to partial, incomplete maps and dynamic ob-
stacles. Furthermore, the (re-)computation of a large scale
Robot
global path is often not feasible in real-time applications. &
Environment
This observation leads to approaches which modify a path
locally, such as the "elastic band" proposed by [2, 3]. The
main idea of the "elastic band" approach is to deform an
Figure 1: Robot system with "timed elastic band"
originally given path by considering it as an elastic rubber
band subject to internal and external forces which balance Our approach, called "timed elastic band" is novel as it
each other in the attempt to contract the path while keeping explicitly augments "elastic band" with temporal informa-
a distance from obstacles. tion, thus allowing the consideration of the robot’s dy-
Later this approach was extended to non-holonomic kine- namic constraints and direct modification of trajectories
matics [4, 5, 6], robotic systems with many degrees of free- rather than paths. Figure 1 shows the architecture of a
dom [7] and dynamics obstacles [8]. However, to our best robot system with the "timed elastic band". By considering
knowledge dynamic motion constraints have not yet been the temporal information, the "timed elastic band" can be
considered as an objective in path deformation. The typical used to control also the velocites and accelerations of the
approach is to smoothen the path for example with splines robot. The new approach is suitable for high dimensional

74
state spaces even though this paper considers a differential The majority of components of the objective function are
drive mobile robot moving in a planar environment with local with respect to B as they only depend on a few
three global and two local degrees of freedom. number of consecutive configurations rather than the en-
tire band. This property of locality of TEB results in a
sparse system matrix, for which specialized fast and effi-
2 Timed Elastic Band cient large scale numerical optimization methods are avail-
able [11].
The classic "elastic band" is described in terms of a se- The objective functions of the TEB belong to two types:
quence of n intermediate robot poses xi = (xi , yi , βi )T ∈ constraints such as velocity and acceleration limits formu-
R2 × S 1 , in the following denoted as a configuration in- lated in terms of penalty functions and objectives with re-
cluding position xi , yi and orientation βi of the robot in spect to trajectory such as shortest or fastest path (Eq. 18)
the related frame ({map}, Fig. 2): or clearance from obstacles (Eq. 8). Sparse constrained
optimization algorithms are not readily available in robotic
Q = {xi }i=0...n n∈N (1) frameworks (e.g. ROS) in a freely usable implementation.
Therefore, in the context of "timed elastic band" these con-
The "timed elastic band" (TEB) is augmented by the time straints are formulated as objectives in terms of a piecewise
intervals between two consecutive configurations, result- continuous, differentiable cost function that penalize the
ing in a sequence of n − 1 time differences ΔTi : violation of a constraint (Eq. 6).

τ = {ΔTi }i=0...n−1 (2) ( x−(xSr −) )n if x > xr − 
eΓ (x, xr , , S, n)  (6)
Each time difference denotes the time that the robot re- 0 otherwise
quires to transit from one configuration to the next config-
xr denotes the bound. S, n and  affect the accuracy of the
uration in sequence (Fig. 2). The TEB is defined as a tuple
approximation. Especially S expresses the scaling, n the
of both sequences:
polynomial order and  a small translation of the approxi-
mation.
B := (Q, τ ) (3)

The key idea is to adapt and optimize the TEB in terms of 10

both configurations and time intervals by a weighted multi- 9
Approximation 1
Approximation 2
objective optimization in real-time: 8
Ideal constraint

 7
f (B) = γk fk (B) (4) 6
k
e (x)

5
B∗ = argminf (B)
K

(5) 4
B
3

in which B ∗ denotes the optimized TEB, f (B) denotes 2

the objective function. In this paper it is a weighted sum of 1

components fk which capture the various aspects. This is 0

0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8
the most elementary approach to multi-objective optimiza- x

tion, but already it yields very good results. In future work,

more sophisticated approaches might be investigated. Figure 3: Polynomial approximation of constraints

Figure 3 shows two different realizations of Eq. 6. Ap-

proximation 1 results from parameter-set n = 2, S = 0.1,
 = 0.1 and Approximation 2, which is conspicuously a
stronger approximation, result from parameter-set n = 2,
S = 0.05 and  = 0.1. This example shows an approxima-
tion of the constraint xr = 0.4.
An obvious advantage of using a multi-objective optimiza-
tion framework is the modular formulation of objective
functions. The objective functions currently employed in
the TEB are listed below.

2.1 Way points and obstacles

Figure 2: TEB: sequences of configurations and time dif- The TEB simultaneously accounts for the attainment of the
ferences intermediate way points of the original path and the avoid-

75
ance of static or dynamic obstacles. Both objective func- ing time intervals:
tions are similar with the difference that way points attract
2(vi+1 − vi )
the elastic band whereas obstacles repel it. The objective ai = (11)
function rests upon the minimal separation dmin,j between ΔTi + ΔTi+1
the TEB and the way point or obstacle zj (Fig. 4). In the For the sake of clarity, the three consecutive configurations
case of way points the distance is bounded from above by are substituted by their two related velocities in Eq. 11.
a maximal target radius rpmax (Eq. 7) and in case of obsta- The rotational acceleration is computed similar to Eq. 11
cles it is bounded from below by a minimal distance romin by considering rotational velocities instead of translational
(Eq. 8). These constraints are implemented by the penalty ones. Considering a differential drive mobile robot, the
function in Eq. 6. relationship between the wheel velocities and the transla-
tional and rotational velocities vi and ωi of the robot center
point are computed according to:
fpath = eΓ (dmin,j , rpmax , , S, n) (7)
vwr ,i = vi + Lωi (12)
fob = eΓ (−dmin,j , −romin , , S, n) (8)
vwl ,i = vi − Lωi (13)
According to Fig. 3, the signs of the separation dmin,j and
in which the parameter L denotes half of the robot wheel-
the bound romin in Eq. 8 must be swapped to realize a
base.
bounding from below.
Differentiating Eq. 12 and Eq. 13 with respect to time
Notice, that the gradient of these objective functions can be
leads to the corresponding wheel accelerations. The wheel
interpreted as an external force acting on the elastic band.
velocities and acceleration are bounded from above and
below according to the manufacturer specifications. The
translational and rotational inertia of the robot could be in-
cluded in an obvious way, but in this first implementation
we have not yet done so.

2.3 Non-holonomic kinematics

Figure 4: Minimal distance between TEB and way point

or obstacle

2.2 Velocity and acceleration

Figure 5: Relationship between configurations on a circle
Dynamic constraints on robot velocity and acceleration
for non-holonomic kinematics
are described by similar penalty function as in the case
of geometric constraints. Figure 2 shows the structure of Robots with a differential drive only possess two local de-
TEB. The mean translational and rotational velocities are grees of freedom. Thus they can only execute motions in
computed according to the euclidean or angular distance the direction of the robot’s current heading. This kinematic
between two consecutive configurations xi , xi+1 and the constraint leads to a smooth path that is composed of arc
time interval ΔTi for the transition between both poses. segments. Thus two adjacent configurations are required
  to be located on a common arc of constant curvature as
1 xi+1 − xi
vi    (9) illustrated in Fig. 5: The angle ϑi between the initial con-
ΔTi yi+1 − yi figuration xi and the direction di,i+1 has to be equal to the
βi+1 − βi corresponding angle ϑi+1 at the final configuration xi+1 .
ωi  (10)
ΔTi If βi denotes the absolute orientation of a robot at the i-th
configuration the arc condition demands:
Due to the vicinity of configurations the euclidean distance
is a sufficient approximation of the true length of the cir- ϑi = ϑi+1 (14)
⎛ ⎞ ⎛ ⎞
cular path between two consecutive poses. The accelera- cos βi cos βi+1
tion relates two consecutive mean velocities, thus consid- ⇔ ⎝ sin βi ⎠ × di,i+1 = di,i+1 × ⎝ sin βi+1 ⎠(15)
ers three consecutive configurations with two correspond- 0 0

76
with the direction vector: Path
⎛ ⎞ {zj}
xi+1 − xi
di,i+1 := ⎝ yi+1 − yi ⎠ (16) Initialization
0
B(Q,τ)

The corresponding objective function Insert/delete B(Q,τ)

Re-Initialization
TEB states

 ⎛ ⎞ ⎛ ⎞
2

cos βi cos βi+1 
B(Q,τ)

⎝

fk (xi , xi+1 ) =
sin βi ⎠ + ⎝ sin βi+1 ⎠ × di,i+1

Trajectory modification
Associate TEB states with

0 0 waypoints/obstacles

(17) Mapping B(Q,τ)

Generate hyper-graph
penalizes the quadratic error in the violation of this con-
straint. A potential 180◦ orientation change is taken care Hyper-graph

of with an extra term. Optimize hyper-graph

B*(Q,τ)

B*(Q,τ)
2.4 Fastest path Possible?:
Verify trajectory
Yes/No
Previous "elastic band" approaches obtain the shortest path
B*(Q,τ)
by internal forces that contract the elastic band. Since our
approach considers temporal information the objective of Calculate control variables
a shortest path we have the option to replace the objective
v,ω
of a shortest path with that of a fastest path, or to combine
those objectives. The objective of a fastest path is easily Obstacles Robot Odometry
&
achieved by minimizing the square of the sum of all time Environment

differences.
n
fk = ( ΔTi )2 (18)
i=1 Figure 6: Control flow of TEB-implementation
This objective leads to a fastest path in which the interme-
diate configurations are uniformly separated in time rather
than space. The required hyper-graph is a graph in which the amount of
connected nodes of one single edge is not limited. There-
2.5 Implementation fore an edge can connect more than two nodes. The TEB
problem (Eq. 4) can be transformed into a hyper-graph that
Figure 6 shows the control flow of the implemented TEB. has configurations and time differences as nodes. They are
In the initialization-phase an initial path is enhanced to an connected with edges representing given objective func-
initial trajectory by adding default timing information re- tions fk or constraint functions. Figure 7 shows an ex-
specting the dynamic and kinematic constraints. In our ample hyper-graph with two configurations, one time dif-
case the initial trajectory is composed of piecewise linear ference and a point shaped obstacle. The velocity bound-
segments with a pure rotation followed by a translation. ing objective function requires the mean velocity which re-
Such a path representation in terms of a polygon is com- lates to the euclidean distance between two configurations
monly provided by probabilistic roadmap planners [9]. Al- and the required travel time. Hence it forms an edge con-
ternatively, Reeds-Shepp-paths are easily enhanced to ad- necting those states of B. The obstacle requires one edge
missible trajectories [10]. which is connected to the nearest configuration. The node
At each iteration, the algorithm dynamically adds new con- representing the obstacle is fixed (double circle), thus its
figurations or deletes previous ones in order to adjust the parameters (position) cannot be changed by optimization
spatial and temporal resolution to the remaining trajectory algorithms.
length or planning horizon. A hysteresis is implemented
to avoid oscillations. The optimization problem is trans- After verifying the optimized TEB, control variables v and
formed into a hyper-graph and solved with large scale op- ω can be calculated to directly command the robot drive
timization algorithms for sparse systems which are con- system. Before every new iteration, the re-initialization-
tained in the "g2o-framework" [11]. phase checks new and changing way-points which can be
useful if way-points are received after analyzing short-
range camera or laser-scan data.

77


x0 Velocity fvel x1

b
fo



tacle
ΔT0

Obs


o1



Figure 7: Velocity and obstacle objective function formu- Figure 10: Trajectory with way points: constraint
lated as a hyper-graph approx. 2

Figure 9 and 10 show a scenario with four intermediate

way points. In the second scenario, the TEB employs a
3 Experiments and results stronger penalty for the violation of geometric constraints
according to Fig. 3 (Approximation 2), thus robot traverses
In this section we give a short outline of the experimental the way points more accurately. However, the solution that
results. employs a weaker penalty for constraint violations often
results in a smoother trajectory with less overshoot. De-
pending on the specific applications, tuning the weights al-
0 lows for shifting the emphasis between a more accurate
or a smoother and thereby faster trajectory. The dynamic
limitations (vmax = 1.4 ms , amax = 0.4 sm2 ) are taken into
50
account as shown in Fig. 11. A scenario with avoidance of
static obstacles is illustrated in Fig. 12.
100

1.4
Right wheel
150 Left wheel
1.2

1
0 50 100 150
nz = 5481
0.8
v [m/s]

Figure 8: TEB: Sparse system-matrix of a TEB realization 0.6

As illustrated in Fig. 8, the resulting TEB system-matrix is 0.4

still sparse with 15 percent of non-zero elements. In this 0.2

example, the first 141 states corresponds to 47 configura-
0
tions xi and states 142-189 are the related time differences 0 5 10 15 20 25 30
t [s]
ΔTi . Those last states are related to the objective func-
tion which aim is to achieve the fastest trajectory, thus this Figure 11: Velocity profile for scenario shown in fig.10
block is dense and grows quadratically with the dimension
of the TEB. The augmentation of elastic bands with temporal informa-
tion and the availability of efficient large scale optimizers


for sparse systems allow for real-time trajectory adaptation
and control of the robot.
Figure 13 shows a sequence of snapshots from an exper-
iment with a Pioneer 2 robot in which a person walks

 through the scene. The Pioneer 2 is controlled by a
Siemens Lifebook s6410 (Core2Duo, 2.4GHz, 2GBRAM)
and is equipped with a Hokuyo Laser Scanner to detect the


dynamic change of obstacle’s position. The TEB adapts
the original robots trajectory (t = 0) in real time and avoids


an imminent collision with the person during the interval
t ∈ [6, 12] by stretching the trajectory away from the ob-
Figure 9: Trajectory with way points: constraint approx. 1 stacle. The control cycle time is 25ms.

78
   approach is to surround an obstacle with a band with op-
posite orientation than the trajectory at the closest point,
glue them at tangential point and cut the resulting trajec-

 
 
tory at this point to obtain one that passes the obstacle on
the other side. After optimizing this trajectory, it is decided



on which side of the obstacle to continue. Note that this is

still a local optimization, not guaranteed to find global op-
  tima.
 

Figure 12: Consider way points (Fig. 9) and obstacles

References
[1] S. M. LaValle. Planning Algorithms, Cambridge
University Press, Cambridge, U.K., 2006.



 [2] Quinlan, S.; Khatib, O. Elastic Bands: Connect-
ing Path Planning and Control, IEEE Int. Conf. on
Robotics and Automation (ICRA) (2), 1993

[3] Quinlan, S. Real-time modification of collision-free

paths, PhD thesis, Stanford University, 1994
(a) t  0 s (b) t  6 s [4] Khatib, M. Sensor-based motion control for mobile
robots, Laboratoire d’Automatique et d’Analyse des
Systèmes LAAS-CNRS, 1996

[5] Khatib, M. et al. Dynamic Path Modification for Car-

Like Nonholonomic Mobile Robots, IEEE Int. Conf.
on Robotics and Automation (ICRA), 1997

[6] Graf, B.; Wandosell, J.M.H.; Schaeffer, C. Flexi-

(c) t  11 s (d) t  16 s ble Path Planning for Nonholonomic Mobile Robots,
Fraunhofer Institute Manufacturing Engineering and
Figure 13: Avoiding a dynamic obstacle by real time adap- Automation (IPA), 2001
tation of the TEB
[7] Brock, O.; Khatib, O. Executing Motion Plans for
Robots with Many Degrees of Freedom in Dynamic
Environments, IEEE Int. Conf. on Robotics and Au-
4 Conclusion and further work tomation (ICRA), 1998, pp. 1-6

This paper presents a novel approach for real time trajec- [8] Fiorini, P.; Shiller, Z. Motion planning in dynamic
tory modification with timed elastic bands. It rests upon environments using velocity obstacles, International
the augmentation of the classic "elastic band" theory by Journal on Robotics Research, Vol. 17, No. 7, July
temporal information. The proposed method makes it pos- 1998, pp. 760-772
sible to not only to consider geometric constraints on the
path but to simultaneously account for dynamic constraints [9] Kavraki, L.E. et al. Probabilistic roadmaps for path
of the mobile robot. The algorithm operates in real-time planning in high-dimensional configuration spaces,
and thereby directly generates commands for the underly- IEEE Transactions on Robotics and Automation, Vol.
ing robot motion controller. The method is highly flexible 12, No.4, August 1996, pp.566-580
and is easily adapted to different robot kinematics and ap- [10] Reeds, J.A.; Shepp L.A. Optimal paths for a car that
plication requirements. goes both forwards and backwards, Pacific Journal
Future work is concerned with the development of a sparse of Mathematics, Vol. 145, No. 2, 1990, pp. 367-393
constraint optimization framework, thus rendering the cur-
rent formulation of constraints in terms of penalty func- [11] Kümmerle, R. et al. g2o: A General Framework for
tions obsolete. A different direction of research is to make Graph Optimization, Proc. of the IEEE Int. Conf. on
the elastic band "jump over obstacles". Technically the Robotics and Automation (ICRA), May 2011

79