SP Runk 2008

Faculty of Applied Sciences
Department of Computer Science

Autonomous Intelligent Systems Lab
Prof. Dr. Wolfram Burgard
Student Project
Planning Motion Trajectories for

Mobile Robots Using Splines
Christoph Sprunk
sprunkc@informatik.uni-freiburg.de
October 2008
Supervisor: Dipl. Inf. Boris Lau

Abstract
Motion planning for wheeled mobile robots (WMR) in controlled environments is con-
sidered a solved problem. Typical solutions are path planning on a 2D grid and reactive
collision avoidance. Active research deals with issues regarding the integration of ad-
ditional constraints such as dynamics, narrow spaces, or smoothness requirements.
Current approaches to motion planning mainly consider a finite number of actions
that can be carried out by the robot at a given time. In this work we present an
approach that resorts to a parametric trajectory representation to overcome these lim-
itations. As representation we choose Quintic Bezier Splines. We conduct global,
explicit planning for velocities along the robots trajectory a prerequisite if smooth
kinodynamics along the path are to be included into the planning process, yet mainly
unaddressed in current work. Our approach guarantees velocity profiles to respect
several kinodynamic constraints and resolves accelerational interdependencies without
iteration.
The proposed approach optimizes a curvature continuous trajectory and maintains
anytime capability. For actual execution the trajectory is forwarded to a separate
feedback controller. The suggested method is able to smoothly update a trajectory to
account for unmapped obstacles, moderate errors in localization, or erroneous plan exe-
cution. Evaluation of our method on real robots shows its applicability and advantages
over a Dynamic Window-based approach.
Contents
1 Introduction 6
2 Related Work 9
3 Spline Based Trajectories 11

3.1 2D Paths Using Splines . . . . . . . . . . . . . . . . . . . . . . . . . . . 11
3.1.1 Desired Properties . . . . . . . . . . . . . . . . . . . . . . . . . . 11
3.1.2 Choosing a Spline Type . . . . . . . . . . . . . . . . . . . . . . . 13
3.1.3 Separating Spline and Velocity Profile . . . . . . . . . . . . . . . 17
3.2 Numerical Velocity Profile . . . . . . . . . . . . . . . . . . . . . . . . . . 18
3.2.1 Overview . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18
3.2.2 Respected constraints . . . . . . . . . . . . . . . . . . . . . . . . 21
3.2.3 Definitions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21
3.2.4 Movement Equations . . . . . . . . . . . . . . . . . . . . . . . . . 21
3.2.5 Isolated constraints . . . . . . . . . . . . . . . . . . . . . . . . . . 24
3.2.6 Accelerational constraints . . . . . . . . . . . . . . . . . . . . . . 25
3.2.7 Constraint Satisfaction . . . . . . . . . . . . . . . . . . . . . . . . 29
4 Trajectory Generation 32
4.1 Initial Trajectory . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32
4.1.1 First Derivative Heuristic . . . . . . . . . . . . . . . . . . . . . . 32
4.1.2 Second Derivative Heuristic . . . . . . . . . . . . . . . . . . . . . 35
4.1.3 Generation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 36
4.2 Optimization . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37
4.2.1 Parameters for Spline Optimization . . . . . . . . . . . . . . . . 37
4.2.2 Optimization Method . . . . . . . . . . . . . . . . . . . . . . . . 38
4.2.3 Examination of the Search Space . . . . . . . . . . . . . . . . . . 40
4.2.4 Runtime . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 47
4.3 Updating Plans . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 47
4.3.1 Odometry Errors . . . . . . . . . . . . . . . . . . . . . . . . . . . 48
4.3.2 Unmapped Obstacles . . . . . . . . . . . . . . . . . . . . . . . . . 49
4.3.3 Subdivision of Long Paths . . . . . . . . . . . . . . . . . . . . . . 50
4.3.4 Generating an Updated Trajectory . . . . . . . . . . . . . . . . . 50
5 Experiments 53
5.1 Experimental Setup . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 53
5.2 Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 55
5.2.1 Unmapped Obstacles . . . . . . . . . . . . . . . . . . . . . . . . . 59
5.3 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 61
6 Discussion 62
4
Contents
6.1 Customization . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 62
6.2 Improvements . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 62
6.3 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 64
Bibliography 70
A Splines 71
A.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 71
A.2 Cubic Hermite Spline . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 72
A.3 Cubic Bezier Splines . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 74
A.4 Catmull-Rom Splines . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 76
A.5 B-Splines . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 77
A.6 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 79
B Solving the Overlap Problem 80

B.1 Derivation of the Bound . . . . . . . . . . . . . . . . . . . . . . . . . . . 80
B.2 Pseudo Code . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 108
C Sobel Operator for Gradients 110
5
1 Introduction
One of the major ambitions in mobile robotics is to enable robots to follow high-level
task instructions, i.e., task instructions that do not contain detailed information on the
steps to be taken for task fulfillment. To reach this goal, autonomous navigation is a
fundamental prerequisite.
The task to navigate autonomously can be divided into the areas of mapping, local-
ization and motion control. The latter is the domain of interest for this work. The
localization within a map is herein assumed to be given, but errors have to be accounted
for. Furthermore, the problem domain includes the presence of a moderate amount of
unmapped obstacles.
The research community has conducted a lot of work in this domain, see for example
[15] for an overview. As a result, basic navigation of a mobile robot (i.e., motion from a
start to a goal position without any further constraints) is considered a solved problem.
However, practical applications advance to more complex problem domains. Un-
manned autonomous vehicles used for heavy cargo load need motion planning to ac-
count for their extreme kinodynamics, for example. One observes that with the prac-
tical use of mobile robots in increasingly complex scenarios, controllability and pre-
dictability of the robots exact behavior gain importance.
Furthermore, wheeled mobile robots (WMR) are expected to navigate safely and
successfully in tight spaces that they share with other moving entities. The ability to
plan and predict the state of the robot more precisely with respect to time and position
is crucial for navigation in narrow spaces and environments that contain unmapped
obstacles for which movements can be anticipated to a certain degree.
In general, increasing weight is put on how the robot reaches its goal, planned paths
are required to consider costs in domains like smoothness, time of travel, and energy
consumption. Car-like robots for instance have strong smoothness requirements if
passenger comfort or safety of impulse sensitive payload are accounted for.
Therefore the research area is still active [9, 16, 19, 37, 39] as the classical approaches
to mobile robot navigation and motion reach their limits with respect to the require-
ments stated above.
Limits of existing approaches Reactive approaches to mobile robot navigation like

the Dynamic Window [8] and Potential Field methods [12] are unsuitable for application
to the more ambitious problem domain. The reactive nature implies a limited path
predictability. In addition to that, the Dynamic Window suffers from overshooting [32]
and for Potential Field methods local minima are a major issue [14].
The well known 2D A* algorithm and its extension D* provide cost optimal paths on
gridmaps. These paths however consist of straight line segments and ignore kinematic
constraints. To exactly follow these paths the robot is required to exclusively execute
either strictly translational or rotational movement (i.e., to drive straight or turn on
the spot). While the action set to move to one of the eight neighbor cells in a 2D
6
grid is perfectly suited for costs only considering the paths length, it is insufficient
for applications aiming to optimize more complex measures like time of travel, energy
consumption, or path smoothness. Because of this and due to not accounting for
kinematic constraints, the paths optimality is lost when it is taken to the real problem
domain.
To cope with the problem of straight line paths and ignored kinematics, applications
of A* to higher dimensional state spaces have been suggested. The state space is for
instance augmented by the robots heading and a two-dimensional velocity component.
In order to keep the algorithm computationally tractable, heavy discretization has to be
applied to the state space and the action set has a limited size to control the branching
factor of the search tree. Stachniss and Burgard [32] present an approach where a
higher dimensional A* search is guided by a lower dimensional one to further constrain
the search space.
The paths computed by these approaches are in general continuous in the robots
heading but still have discontinuities in curvature. This means that for instance car-like
robots have to cope with a non-continuous steering angle. While the actual motion of
the vehicle is smoothened by inertia and low level controllers, this induces deviations
from the assumed path. Only accounting for these effects during planning can improve
the predictability of the robots behavior.
Precise manipulator control There is a domain however, where exact, time dependent
motion planning and execution has been reached: the area of multi degree of freedom
actuators and manipulators, mostly in the form of robotic arms [30]. While these
manipulators in general are more capable of executing commands with high precision
than WMR, it is believed that the overall accuracy in trajectory execution is also due
to the small feedback loop involved.
Therefore, recent work tries to transfer some of the precision potential to the domain
of motion planning for WMR by decoupling trajectory planning and execution. A
separate representation of the trajectory is maintained and executed by a feedback
controller. This way the feedback loop is small and one can also benefit from the results
and progress made in the field of control theory [13, 18]. Furthermore, separation of
trajectory and execution of the latter adds a level of abstraction that provides more
flexibility, e.g., easy change of the used controller.
Recent developments By the introduction of a separate and smaller feedback loop,

(part of) the current work treats a design disadvantage of previously existing motion
planning methods: their feedback loop includes the global path planner with the con-
sequence of negative effects on stability. To generate a motion command if the robot
has deviated from the previously computed path it is necessary to re-execute the high-
level planning method. This takes time and can therefore only be done at a relatively
limited frequency. Note that while this limitation amongst others applies to the A*
based approaches, methods exist that precompute actions for all positions, e.g., Value
Iteration based planners [35].
In the majority of recent work, motion planning is approached by searching in a
space that consists of suitable motion primitives that are stitched together. The major
challenge for these approaches is to keep the number of primitives within a limit that
yields computationally tractable search trees.
7
1 Introduction
An aspect not covered by most of the current approaches is the necessity to globally
plan for the velocities along the path: thereby it is possible to slow down early enough
when approaching a curve and to account for the effects that changes in the curvature
have on kinodynamics.
A detailed discussion of the related work will be given in the next chapter. First, we
want to motivate the use of splines for locomotion.
Spline based locomotion planning As means of representation for the trajectories we

choose splines (piecewise polynomial parametric curves) because they offer the following
favorable properties: splines constitute a compact representation of smooth paths. This
allows the generation of paths that can be followed by a robot with high precision.
Furthermore, discretization is moved to another dimension. The path between start
and goal can be described with arbitrary precision, the actions are not limited by any
fixed number. This is gained by only considering curves of a parameterized family for
robot movement but does not overconstrain the set of possible paths, which constitutes
a major difference to the search based approaches.
Contributions We shortly outline the system we propose for WMR motion planning.
Our system takes as inputs a map of the environment and sparse waypoints that
describe a collision free line path from the current to the goal position. Based on
Quintic Bezier Splines it then creates an initial trajectory along the waypoints and
continues to optimize it as long as planning time is available. During the optimization
velocities along the curve are explicitly planned for to obtain a 2D path that together
with its velocity profile optimizes a cost function, such as the time of travel for example.
When planning velocities, kinodynamics of the robot are accounted for.
The output of the proposed system is a time-parameterized trajectory together with
its first and second derivatives that can be used by a controller to determine location,
speed, and acceleration along the trajectory for every point in time.
As will be shown, the proposed system has anytime capability and we also present a
way to account for unmapped obstacles. Finally, it has to be noted that we evaluated
our approach with experiments on real robots.
Organization The remainder of this work is organized as follows. Chapter 2 gives an

overview about further related work in the domain of spline based motion planning.
Chapter 3 provides the theoretical foundations for the suggested approach which is
described in detail in Chapter 4. Experiments and corresponding results are presented
in Chapter 5. Chapter 6 finally discusses the proposed system and concludes.
8
2 Related Work
In this section the proposed system will be compared to related work in the area of
spline based motion planning for mobile robots.
There exists a variety of motion planning approaches that use splines to represent
trajectories. They all concentrate on several aspects of the problem but none accounts
for all requirements that we consider important: a path that can be exactly followed
by the robot (i.e., curvature continuity), explicit planning for velocities with consider-
ation of kinodynamics, anytime capability, an optimization not prone to local optima,
treatment of unmapped obstacles, and an evaluation of the system on a real robot.
Hwang et al. [11] use Bezier Splines as trajectory model. They present an approach
that extracts significant points from a touch screen based path input and uses these
as pass-through points for a Cubic Bezier Spline. While unmapped obstacles are ac-
counted for, the paths generated by this approach are discontinuous in curvature. The
work of Hwang et al. is used by Mandel and Frese [21] for autonomous wheelchair nav-
igation, in their work they provide a non-simulated evaluation of the approach. Our
approach relies on Bezier Splines as well but uses a higher degree of these polynomials
to overcome the curvature discontinuities.
Another approach to wheelchair navigation is presented by Gulati and Kuipers [10].
They emphasize the need for smooth and comfortable motion and choose B-Splines
as trajectory representation. However, the approach only applies to navigation in
predefined situations without unmapped obstacles.
B-Splines are also used by Shiller and Gwo [31]. In their work they treat the motion
planning task extended to 3D with the help of B-Spline patches and curves. The
evaluation of their approach, that accounts for kinodynamic constraints, is based on
simulation.
The principle of optimizing a spline based trajectory can be found in the work of
Magid et al. [20]. Here, an initially straight-line shaped path is optimized with respect
to a cost function by the Nelder Mead Simplex method. The difference to the approach
we suggest is that the initial path is not obstacle free and that local minima are a
problem for this method, as the simulative evaluation shows.
Sahraei et al. [29] rely on a Voronoi graph to provide information for the generation of
a Bezier Curve based path. The path is not continuous in curvature and the approach
ignores unmapped obstacles, but it has been evaluated by application on a robot.
Another spline related approach is suggested by Thompson and Kagami [34]. Here,
curvature is a polynomial function of the current traveled distance. With these curva-
ture polynomials Thompson and Kagami define obstacle avoiding paths with ad-hoc
speed profiles.
An approach that has received extensive evaluation is the work of Arras et al. [1].
It uses Elastic Bands [26] as part of the navigation function for an extension of the
Dynamic Window approach and showed reliable performance on an overall traveled
distance of more than 3 000 kilometers. With the use of the Dynamic Window, the
approach is subject to the aforementioned drawbacks, but the combination with Elastic
9
2 Related Work
Bands as a form of trajectory representation can be interpreted as a first step to

separating the trajectory generation from its execution. Here, the Dynamic Window
can be seen as controller that is fed the trajectory through the navigation function.
Most of the current work separates trajectory generation and execution even more
strongly and employs real controllers, in the sense that these try to exactly follow the
trajectory instead of just being guided by it. This considerably improves and simplifies
the predictability of the robots behavior.
A lot of examples for such approaches can be found in the work triggered by the
DARPA Grand and Urban Challenge [3, 16, 36, 39]. All these systems have undergone
a lot of practical testing and show the small feedback loop that results from decoupling
trajectory generation and execution.
Likhachev and Ferguson [16] stitch together actions from a precalculated set, the
action space is searched by anytime dynamic A*. The curvature discontinuities in the
resulting paths are claimed to be non-critical and handled by reducing speed at high
curvatures. A step in the direction of velocity planning is done by a lookahead mech-
anism that ensures smooth deceleration upon a change of vehicle movement direction
(forwards vs. backwards).
Curvature continuous paths are generated by Ziegler et al. [39] through incorporation
of the steering angle into the state space. The path is constructed via an A* search,
the state space does not contain any velocity information. Path execution is handled
by two separate controllers, one for steering angle and velocity each.
An example for searching the space of stitched paths with Rapidly-exploring Random
Trees is given by the work of Macek et al. [19] who emphasize safety and collision
avoidance. Here, translational velocity is part of the state space, and to treat the
costly search an approach called Partial Motion Planning is applied.
There are approaches that, like our suggestion, do not have to cope with limiting
the action set, as they rely on parametric trajectory representations [3, 36].
Thrun et al. [36] generate a Cubic Bezier Spline from GPS data describing the course
of the road. Contrary to our approach the vehicle does not exactly follow the spline
but travels at an intended varying lateral offset instead. Offset changes are employed
for obstacle avoidance and realized through a discrete set of maneuvers resembling lane
changes. While in this approach a spline is used to smoothly interpolate between given
waypoints, in our approach optimization of the spline is part of the planning process.
The approach of Connors and Elkaim [3, 4] also uses a parametric trajectory represen-
tation. Paths are generated by starting with straight-line segments between waypoints.
Control points are then moved out of obstacles repeatedly until a collision free path is
generated. A drawback is that a solution cannot be guaranteed by this method.
Summary As shown in this literature review, the research community investigates the
benefits of separating the trajectory planning from its feedback execution, yielding a
smaller feedback loop. One also finds that smooth, curvature continuous trajectories
are possible to employ, yet not a priority for the successfully tested approaches, e.g.,
the unmanned DARPA challenge vehicles. Advantages of a parameterized trajectory
representation over one that is stitched together from a finite set of precomputed parts
have emerged. Furthermore, we have seen that little is done in the domain of explicit
velocity planning and optimization.
10
3 Spline Based Trajectories
For our system we will use splines as means to determine the exact 2D path for the
robot. The actual trajectory that controls the exact speed and acceleration at every
single 2D point will then consist of this 2D path and a separately created velocity
profile, thus generating a 3D representation over space and time.
Without reducing the scope of the approach, we only consider forward motion of the
robot along the 2D path. Therefore, the non-smooth 2D path shapes that correspond
to forward motion interrupted by backward motion, e.g., to a 3-point turn, are neither
needed nor desired for our application.
3.1 2D Paths Using Splines

For this work, we define a spline as a piecewise polynomial parametric curve in the two-
dimensional plane Q(t) R2 . All segments of the spline are required to be polynomials
of the same degree n. A central aspect of splines is the fulfillment of certain continuity
constraints (see for instance [6, p. 480]).
Parametric continuity A parametric curve is said to have n-th degree parametric con-
dn
tinuity in parameter t, if its n-th derivative dt n Q(t) is continuous. It is then also
called C n continuous.
Geometric continuity n-th degree geometric continuity does not require the left-hand
dn
limit and right-hand limit of the n-th derivative dtn Q(t) to be equal but demands
the direction of these vectors to match, i.e., they have to be scalar multiplies of
each other. Note that geometric continuity is trivial for one-dimensional para-
metric curves (e.g., Q(t) R).
For points on a polynomial these continuity constraints are always fulfilled (as deriva-
tives of arbitrary degree are continuous), so the join points of the polynomial segments
are the critical points. In general geometric continuity is a consequence of parametric
continuity of the same degree, unless the particular derivative yields the zero vector.
Then situations can arise in which the left-hand limit points in a different direction
than the right-hand limit although both evaluate to the zero vector at the join point.
3.1.1 Desired Properties

To ensure that the robot is able to follow a spline based reference path and that the
path can be efficiently planned, the splines are required to have several properties. The
properties necessary for successful employment of splines in the domain of trajectories
are discussed in the following.
11
3.1.1.1 Drivability
Several properties are necessary to guarantee that a wheeled robot can exactly follow
a trajectory defined by a spline. For the most part, these are continuity properties.
Continuity in heading In addition to the self-evident C 0 continuity it makes sense to

require the spline to be G1 or C 1 continuous, which has the effect that the heading of
the robot changes continuously. If this wasnt the case the robot would have to be able
to execute infinite rotational accelerations.
Continuity in curvature Trajectories intended to be driven by wheeled robots should

also be continuous in their curvature. A discontinuity in curvature would require in-
finitely high accelerations for robots equipped with synchro drive, differential drive and
even for holonomic robots. For car like robots it would correspond to a discontinuity
in steering angle. Apart from this, even if a robot can approximately cope with these
accelerations, curvature discontinuities imply strong, abrupt forces to the robot itself
as well as to potentially fragile payload. With unfavorable consequences such as possi-
ble skidding or damaged payload we want to prohibit curvature discontinuities in our
trajectories.
The curvature c is the inverse of a curves radius and defined in [33, Sect. 13.3] as
follows:
dT
c= ,
ds
where T is the tangent vector function to the curve and s the curves arc length. [33,
Sect. 13.3, Theorem 10] allows a formulation in an arbitrary parameterization of the
curve Q:
Q0 (t) Q00 (t)
c(t) = . (3.1)
|Q0 (t)|3
Note that, in difference to the original definition, we drop the application of the absolute
value in the numerator to allow a distinction of the curvatures direction (in relation to
an increasing curve parameter t). When moving forward, positive values for curvature
correspond to turning left while negative values indicate a right turn.
As can be seen in Eq. (3.1), control over the first and second derivatives of a para-
metric curve provides control over its curvature. Choosing first and second derivative
suitable for C 2 continuity implies curvature continuity of the spline.
The requirements for a robot driveable spline are summarized and guaranteed with
the demand of C 2 continuity. Note however, that curvature continuity can also be
reached when falling back to G1 continuity. For this, the second derivative has to meet
certain constraints, for details refer to Section 4.3.4.2.
First and second derivative at start To smoothly update a trajectory that is already
being carried out by a robot, one needs to be able to generate trajectories for a moving
robot. More specifically, it must be possible to match the first and second derivatives
to the required heading and orientation/steering angle of the robot.
12
Property Hermite Cubic Bezier Catmull-Rom B-Spline

stitching manual manual inherent inherent
C 1 continuity X X X X
C 2 continuity - - - X
strong correlation - X X -
localism X X X X
freely set 1st deriv. start X X X X
freely set 2nd deriv. start - - - -
Table 3.1: Key properties of cubic spline families presented in Appendix A. None of
them has all of the desired properties, therefore, higher-order splines are
considered.
3.1.1.2 Planning
In addition to the properties that enable a wheeled robot to follow a trajectory defined
by a spline we also require some properties that will facilitate the process of planning
such a spline.
Localism The first property is called localism. Localism is a property that becomes
important when modifying one segment of a multi-segment spline. We will say that
a spline has localism if changes made at a single segment of the spline only affect
a bounded neighborhood of this particular segment and do not result in changes for
all segments of the spline. This property provides the possibility to locally change the
shape of a spline (for instance to circumvent an obstacle) without the need to reconsider
its global shape. This property can also translate into a computational gain: a large
part of the splines parameters will remain the same after a local change, so one can
expect the computation for the update to be less costly than for a global change of the
splines shape.
Correlation between shape and parameters The second property that will facilitate
planning trajectories with splines is the extent to which the parameters of a spline
correspond to its shape on an intuitive level. When planning a trajectory through an
environment we face the task to define a spline that passes through certain points and
stays clear of certain areas. This task is facilitated a lot if there are relatively direct
correlations between the splines shape and its parameters. Otherwise involved inverse
math had to be employed to retrieve spline instances with given shape properties.
3.1.2 Choosing a Spline Type

An introduction to splines and an examination of specific families can be found in
Appendix A. The discussion is summarized by Tab. 3.1 which lists the key properties
of the presented cubic spline families.
Hermite Splines and Cubic Bezier Splines are defined on a per-segment basis, i.e.,
to generate splines that consist of multiple segments (multiple polynomials) manual
stitching is required. In contrast, Catmull-Rom Splines and B-Splines are defined in
a way that already incorporates the stitching process. As can be seen in the table,
13
no spline family possesses all required properties. While B-Splines are the only family
to provide C 2 continuity they lack a strong and intuitive correlation between shape
and control points, which is on the other hand provided by Bezier and Catmull-Rom
Splines.
Note that when giving up on localism during the stitching process in Cubic Bezier
Splines and Hermite Splines it is possible to connect them in a C 2 continuous fashion,
however, the resulting splines are highly unstable and respond with heavy oscillation
to single parameter changes. Furthermore, for all spline families in Tab. 3.1 setting the
start points second derivative influences the remaining derivatives of the segment.
Since all of the splines discussed so far consist of cubic polynomials, they have the
same expressive power. To explain why none of the presented families is able to fulfill all
our requirements and that this is impossible for cubic polynomials, we closer examine
the degrees of freedom available.
3.1.2.1 Degrees of Freedom

A cubic spline consists out of segments Si that are cubic polynomials, generally defined
as
Si (t) = ai t3 + bi t2 + ci t + di , t [0, 1]. (3.2)
Each segment therefore introduces four degrees of freedom. Assuming our spline con-
sists of m segments Si , i [0, m 1], there are 4m degrees of freedom.
First, C 0 continuity is demanded, i.e.,
Si (1) = Si+1 (0), i [0, m 2]. (3.3)
Analogously we demand C 1 and C 2 continuity:
Si0 (1) = Si+1

0
(0), i [0, m 2], (3.4)
Si00 (1) = Si+1
00
(0), i [0, m 2]. (3.5)
Each of the continuity levels states m 1 conditions, reducing the degrees of freedom
still available to 4m 3(m 1) = m + 3. If additionally the start and end points of
the segments are demanded to have specific values (e.g., to match a control point)
S0 (0) = p0 , Si (1) = pi+1 , i [0, m 1], (3.6)
this results in m + 1 additional constraints, leaving 2 degrees of freedom.

Therefore a spline should be possible that consists of cubic polynomials, passes
through control points and is C 2 continuous. While this indeed is possible, the task be-
comes unachievable when demanding localism at the same time. Localism requires the
individual segments to be independent in a way that changing the constraints for one
segment will only affect control points of segments in the direct neighborhood. This is
impossible when demanding C 2 continuity for splines consisting of cubic polynomials,
as will be shown.
To maintain localism, a change at segment Si may only propagate to the segments
Si1 and Si+1 . With cubic polynomials, four degrees of freedom are at disposal for
each segment. This is enough to independently set start and end point as well as the
tangents at these locations. As thereby all degrees of freedom are used, the second
derivative depends on the values for start/end point and tangents. Therefore a change
14
in any of these four constraints will immediately result in a change of the second
derivative at both, the start and end point of the segment. To maintain C 2 continuity
changes are necessary to the following second derivatives: Si+1 00
(0) and Si1
00
(1). As
these cannot be set independently from the other parameters, this introduces shape
changes in Si1 , Si+1 that themselves lead to changes of the second derivatives Si1
00
(0)
and Si+1 (1). These changes now propagate across the whole spline according to the
00
same mechanism.
In conclusion, splines consisting of cubic segments can never provide all of the re-
quired properties at the same time.
3.1.2.2 Higher Order Splines
The degree of the polynomials forming the spline has to be raised for the spline to meet
all our requirements. While quintic polynomials will turn out to be the solution, quartic
polynomials do not suffice: with the additional degree of freedom that a polynomial
of degree four introduces it is possible to set the second derivative independently from
start and end point and respective tangents. However, the second derivatives at start
and end point depend on each other. If it was now necessary to change the second
derivative at one single point in the spline this change would propagate over the spline
in the same way as before, only that it is not mediated by the parameters for start
and end points and their respective tangents this time. Due to these instabilities and
because changing the second derivative is for instance necessary when a start curvature
has to be matched for a moving robot, quartic polynomials are discarded as segment
type.
As mentioned above, quintic polynomials are the segment type of choice to obtain
both, localism and C 2 continuity. The additional two degrees of freedom introduced
compared to cubic polynomials allow to set the second derivative at start and end point
independently from each other. As the Bezier formulation of splines additionally to the
demanded properties provides the convex hull property (Appendix A.3), Quintic Bezier
Splines will be introduced as the final choice of spline type for trajectory planning.
3.1.2.3 Quintic B
ezier Splines
Intuition Quintic Bezier Splines follow the same intuition as Cubic Bezier Splines.
The first and last control point constitute the start and end point of a segment. The
tangents at start and end are proportional to difference vectors of two particular con-
trol points with the first and last control point, respectively. The second derivatives
at beginning and end of a segment are defined by the corresponding tangent and a
difference vector induced by an additional control point each.
Definition Cubic Bezier Splines can be formulated in the basis spline notation with
the help of the Bernstein polynomials of third degree (refer to Eq. (A.19)). Taking the
15
BQB,0 BQB,5

4 4 BQB,2
5 BQB,1 BQB,3

2 2

3 3
10 5 5 BQB,4
0 1 2 3 4
5 5 5 5 1
Figure 3.1: The basis splines of a Quintic Bezier Splines segment, also known as the
Bernstein Polynomials of fifth degree. They are defined in Eq. (3.7).
Bernstein polynomials to degree five, one obtains the Quintic Bezier basis splines
t t
BQB,0 (1 t)5
BQB,1 5(1 t)4 t

BQB,2 10(1 t)3 t2
BQB =
= 10(1 t)2 t3 , (3.7)
BQB,3
BQB,4 5(1 t)t 4
BQB,5 t5
which Fig. 3.1 provides a visualization for. Using six control points, a Quintic Bezier
segment is defined as
S(t) = (1 t)5 P0 + 5(1 t)4 tP1 + 10(1 t)3 t2 P2

(3.8)
+ 10(1 t)2 t3 P3 + 5(1 t)t4 P4 + t5 P5 .
One can verify that the convex hull property as introduced with the Cubic Bezier
Splines (see Appendix A.3) still holds with Bernstein polynomials of fifth degree as
basis splines. Factoring out the powers of the parameter yields formulation with the
Quintic Bezier coefficient matrix :
S(t) = T CQB

P0 + 5P1 10P2 + 10P3 5P4 + P5
5P0 20P1 + 30P2 20P3 + 5P4

10P0 + 30P1 30P2 + 10P3
= t5 t4 t3 t2 t 1

.

10P0 20P1 + 10P2
5P0 + 5P1
P0
(3.9)
Derivatives Derivation of Eq. (3.8) yields
S 0 (0) = 5(P1 P0 ), S 0 (1) = 5(P5 P4 ) (3.10)

S (0) = 20(P0 2P1 + P2 ),
00
S (1) = 20(P3 2P4 + P5 ).
00
(3.11)
16
P3
P4
P2
P5
P1
P0
Figure 3.2: Quintic Bezier Spline segment. The tangents are depicted in red, for dis-
play the blue second derivatives in the figure have been shortened in their
magnitude by factor 5, the tangents by factor 2, respectively. Please refer
to Eqs. (3.10) and (3.11) for correlation between the control points and the
derivatives.
Fig. 3.2 depicts a Quintic Bezier Spline segment and its tangents at the start and
end point.
During trajectory planning the task arises to set specific tangents and second deriva-
tives at start and end point, respectively. A short inverse calculation shows how to set
the control points to retrieve a requested first derivative ts and second derivative as at
the start point and te , ae at the end point:
1 1
P1 = ts + P0 , P4 = P5 te (3.12)
5 5
1 1
P2 = as + 2P1 P0 , P3 = ae + 2P4 P5 . (3.13)
20 20
We now have the means to define a two-dimensional trajectory for the robot to drive
on. In the remainder of this chapter we address the issue of determining the robots
speed along the trajectory.
3.1.3 Separating Spline and Velocity Profile

As introduced in Section 1, a feedback controller is used to let the robot follow the
trajectory defined by the spline. For precise error feedback we provide to the controller
the 2D position and the velocities and accelerations of the robot at any point along the
trajectory.
An intuitive approach is to use the splines first and second derivatives to determine
translational velocity and acceleration. For this the spline, which is a function of
its internal parameter, has to be reparameterized to be a function over time. A nave
approach to this is to linearly map time to the internal spline parameter. This approach
has a serious drawback: Fulfillment of kinodynamic constraints hast to be established
by tuning one scalar factor. If accelerational limits are exceeded at a sharp curve for
instance, there is no option other than to change the mapping to yield a slower traversal
of the whole trajectory.
Because we want closer control over velocities and thereby constraint fulfillment, we
split the spline and the velocity profile. A separate structure will be maintained to
17
vi vi+1
planned velocities
constant acceler-
ation/deceleration
Q(ui )
Q(ui+1 ) spline Q(u) with
planning points (x,y)
s
arc length s
internal spline
ui ui+1 parameter u
Figure 3.3: Planning points for the velocity profile. Placed at equidistant arc length s
along the curve, velocities are planned for these points and constant accel-
eration/deceleration is assumed between them. Dashed lines connect plan-
ning points to their corresponding values for arc length and internal spline
parameter u. In practice, planning points are more densely distributed.
determine which speed and acceleration the robot should have at which point of the
trajectory. By doing so we loose the compact and intuitive character of the spline
induced velocity profile. The critical advantage however is, that we gain strong con-
trol over the velocities and therefore can actively assure compliance with the robots
dynamic constraints.
The next section introduces a method to compute such a separate velocity profile.
3.2 Numerical Velocity Profile

This section describes a way to generate the velocity profile that corresponds to the
fastest traversal of the trajectory while obeying constraints. The method applies to any
parametric curve for which the curve function itself and its first and second derivatives
are given.
3.2.1 Overview
3.2.1.1 Planning for Discrete Points
A velocity profile consists of discrete planning points pi = Q(ui ) distributed along the
trajectory Q(u) and planned translational velocities vi assigned to them. As visualized
by Fig. 3.3, the planning points are distributed equidistant with respect to arc length,
two adjacent planning points pi , pi+1 inscribe an interval on the trajectory with arc
18
length s . For this way of planning point placement, the splines arc length has to be
determined by numerical integration.
Between the planning points, the robot is assumed to move with constant acceleration
or deceleration. Once the velocities vi , vi+1 are planned for an interval bound by the
planning points pi , pi+1 , the assumption of constant accelerations and the intervals
arc length s can be used to determine the time the robot needs to travel through
the interval. Then it is possible to associate with each planning point pi the time ti at
which the robot will arrive at the planning point. As a consequence, a velocity profile
immediately provides the total time of travel for a trajectory.
Association of both, a time ti and the internal spline parameter ui with planning
points pi = Q(ui ) constitutes the basis for a reparameterization of the spline to be a
function over time: as in practice the planning points are deployed more densely than
shown in Fig. 3.3, appropriate interpolation can be used to obtain this reparameteri-
zation.
3.2.1.2 Constraint Respecting Profile

To ensure that the velocity profile respects the kinodynamic constraints of the robot,
these constraints have to be transformed into constraints on the translational veloc-
ities at the planning points. We distinguish two kinds of constraints, isolated and
accelerational constraints.
Isolated constraints at a planning point are independent from the translational veloc-
ities set at neighboring planning points. An example is the maximum centripetal force
allowed for the robot: it depends solely on the curvature and translational velocity at
the particular planning point, velocities at neighboring planning points do not affect
it.
Accelerational constraints in contrast depend on the velocities set for neighboring
planning points. These constraints ensure for instance that the translational velocity
at planning point pi+1 stays within the bounds determined by maximum translational
acceleration or deceleration and the translational velocity at pi .
Profile generation To generate a constraint respecting velocity profile, in a first step

the maximum translational velocities that respect the isolated constraints are computed
for each planning point. These maximum velocities form an initial velocity profile.
Fig. 3.4 provides illustrations for this and the upcoming steps, the initial velocity
profile is shown by Fig. 3.4a.
The next step assures that the velocity at any planning point pi can be reached by
accelerating from the previous planning point pi1 . We also refer to this step as estab-
lishing forward consistency for the profile. The process is started by setting the start
velocity for the profile. In the example in Fig. 3.4b it is set to zero. Then the maximum
velocity at the next planning point is computed (red arrow) and the planned velocity
is corrected, if necessary. Now, forward consistency has been established between the
first two planning points. The procedure is repeated starting with the second planning
point and forward consistency is propagated through the profile this way. Note the
velocities that remain unchanged in Fig. 3.4b because the planned velocity is below
the one reachable through maximum acceleration.
The last phase is visualized by Fig. 3.4c and establishes backward consistency. The
last planning points velocity is set to the desired end speed, i.e., zero in this example.
19
planned
velocity
arc length
(a) Isolated constraints: in a first step the maximum translational velocity for
each planning point has been computed. These velocities are independent from
the ones at neighbor points.
planned
velocity
arc length
(b) Establishing forward consistency by decreasing planned velocities, so that
they meet acceleration constraints (red arrows). Maximum velocity at a plan-
ning point depends on the one set for the previous point in time. Constraints
are being propagated beginning at the start point (left), whose velocity is set
to zero in this example.
planned
velocity
arc length
(c) Establishing backward consistency: The spline is traversed in reverse direc-
tion. The velocity at end point (right) is set to zero, now preceding velocities
are decreased to meet deceleration constraints (blue arrows). Maximum veloc-
ity at a point depends on the one set for the next point in time. The velocity
profile is obtained by interpolating between the velocities at the planning points
(black dotted lines).
Figure 3.4: A simplified, schematic depiction of velocity profile generation. The discrete
planning points for velocities have been set equidistant regarding arc length.
Bigger gray dots stand for values inherited from the previous step. This
example accounts for one accelerational constraint only.
20
Now it is ensured that the velocity at each planning point pi can be reached from the
preceding one pi1 with admissible deceleration. Starting at the end of the curve, the
consistency is now being propagated in reversed curve direction. The resulting velocity
profile is shown with dotted lines in Fig. 3.4c, the velocity at any point on the curve
can be retrieved by appropriate interpolation between planning points, as we assume
constant accelerations.
Note that for multiple accelerational constraints establishing forward and backward
consistency gets more complex, this situation will be accounted for below and is ne-
glected in this overview and the example in Fig. 3.4.
3.2.2 Respected constraints

Our method for velocity profile generation respects the following constraints. The iso-
lated constraints obeyed are maximum translational and rotational velocities, a max-
imum centripetal force and a maximum translational velocity that depends on the
distance to the closest obstacle. As accelerational constraints both, maximum transla-
tional and rotational acceleration are respected. The symbols for these quantities can
be found in Tab. 3.2.
The remainder of this section will present the generation of a velocity profile in detail.
3.2.3 Definitions
We start the presentation of details on velocity profile generation with the introduction
of some notational conventions and definitions. Tab. 3.2 summarizes the ones that will
be used throughout this section. The maximum values for rotational velocity and
accelerations are given by absolute values, we do not distinguish different maxima
for rotation to the left and right, respectively. Furthermore, we assume translational
acceleration and deceleration to be symmetric which is expressed by a common upper
bound atmax given as an absolute value, too.
We will use the convention that a quantitys symbol indexed by i stands for its value
at pi and that we will denote the maximum translational velocity that a constraint
on the quantity imposes as vmax |. The maximum translational velocity due to
maximum centripetal force fmax is for instance written as vmax |f .
3.2.4 Movement Equations

As mentioned above, we assume constant accelerations between planning points. There-
fore, the following equations should hold within the planning interval bounded by pi1
and pi :
1 2
s(t) = at t + vi1 t, t [0, t ], t := ti ti1 (3.14)
2
vi vi1
at = (3.15)
t
s = s(t ) (3.16)
i i1
a = (3.17)
t
21
Symbol Description
v translational velocity
vmax maximum translational velocity
at translational acceleration
atmax maximum translational acceleration (absolute value)
rotational velocity
max maximum rotational velocity (absolute value)
a rotational acceleration
amax maximum rotational acceleration (absolute value)
f centripetal force
fmax maximum centripetal force
c curvature as introduced in Eq. (3.1)
dobst distance to closest obstacle
t time passed since the beginning of the curve
pi i-th planning point on the curve
ui spline parameter for pi = Q(ui ) R ui
s arc length between planning points, i.e., ui1 kQ0 (u)kdu
i value of quantity at pi
vmax | maximum translational velocity due to constraint on quantity
Table 3.2: Abbreviations and variables used throughout the discussion of velocity pro-
file generation.
Here s(t) stands for the arc length traveled since the beginning of the interval and at is
the translational acceleration that is assumed to be constant within the interval. The
total arc length of the interval is given by s (see also Fig. 3.3).
We will now derive a closed form expression for the time t = ti ti1 traveled
between planning points pi1 and pi and one for the velocity vi at planning point pi .
Both will be used in the remainder of the section.
Closed forms for t , vi With Eq. (3.16) and by solving Eq. (3.14) after t one obtains
s
vi1 vi1 2 2s
t = 2 + . (3.18)
at at at
When ignoring negative values for time and velocities, the solutions for Eq. (3.18) are
given by
q
vi1 vi1 2 2s

at + + at > 0
a2t at
s
t = at = 0 (3.19)

vi1 q
vi1 vi1 2
+ 2s
at < 0, vi1 2s at .
at a2t at
22

Note that vi1 2s at is a direct consequence of vi 0. For the cases where
at 6= 0 we substitute it using Eq. (3.15) and thus
s
vi1 t vi1 2 2t 2s t
t = + sign(at ) +
vi vi1 (vi vi1 )2 vi vi1
s
vi1 vi1 2 2t 2s t
1+ t = sign(at ) + .
Both sides of the equation have the same sign in any case, therefore
2
vi1 vi1 2 2t 2s t
1+ 2t = +

2vi1 2s
1+ 2t t = 0
vi vi1 vi vi1
(vi + vi1 ) 2t 2s t = 0
2s
2t t = 0.
vi + vi1
Ignoring the negative solution for t we obtain
2s
t = . (3.20)
vi + vi1
A closer look at Eq. (3.20) reveals that this also holds for the case at = 0, as it implies
vi = vi1 . The side condition that arised for the case at < 0 can be safely dropped, as
it is always ture if vi 0. To see this, we derive an expression for vi using Eqs. (3.15)
and (3.19). For the cases at 6= 0 it is
s !
vi1 vi1 2 2s
vi = vi1 + at + sign(at ) +
at a2t at
s
vi1 2 2s
vi = sign(at )at +
a2t at
which can be simplified to p

vi = vi1 2 + 2s at . (3.21)
Again, this equation also holds for at = 0. The equation also provides the fulfillment
of the side condition in Eq. (3.19) by the existence of a positive vi . For this
vi1 2 + 2s at 0
vi1 2 2s at

has to hold, which is trivial for at 0 and translates to vi1 2s at for at < 0.
Note that given a velocity profile, Eq. (3.20) provides the time of travel for each
planning point interval and therefore for the complete trajectory.
23
3.2.5 Isolated constraints

As introduced above, isolated constraints are independent from the velocities set for
neighboring planning points. Of course all velocities must obey the robots maximum
translational velocity vmax . As we prohibit backward motion this means
vi [0, vmax ]. (3.22)
For our method we assume independence of maximum translational velocity vmax

and maximum rotational velocity max . This is the case for synchro drive robots but
not true for other drive types, e.g., the differential drive. Without this assumption, an
additional constraint has to be included.
With being the product of curvature c and translational velocity v, an upper
bound on rotational velocity max imposes a limit on translational velocities vmax | :
max
vi [0, vmax | ], vmax | = . (3.23)
|ci |
For some robots it might be necessary to limit the occurring centripetal forces, which
are given by f = m|c|v 2 . Translational velocities that respect a maximum centripetal
force fmax , fulfill s
fmax
vi [0, vmax |f ], vmax |f = , (3.24)
m|ci |
where m refers to the mass of the robot.
Slow down near obstacles For safety reasons it might be appropriate to require the
robot to drive slower when in proximity of an obstacle. We derive a maximum velocity
vmax |dobst that depends on the distance to the nearest obstacle dobst .
The robot should be able to completely stop before hitting any obstacle. The stop-
ping distance sbrake depends on the stopping time tbrake :
1
sbrake = vtbrake atmax t2brake . (3.25)
2
!
Via v atmax tbrake = 0 we get tbrake = v/atmax and thereby obtain sbrake = v 2 /2atmax .
Under consideration of the distance sreact = vtreact that the robot travels during its
reaction time treact , we now demand:
sreact + sbrake dobst

v2
vtreact + dobst
2atmax
v 2 + 2atmax treact v 2atmax dobst
q
v atmax treact + a2tmax t2react + 2atmax dobst ,
or differently noted
q
vi [0, vmax |dobst ], vmax |dobst = atmax treact + a2tmax t2react + 2atmax dobst . (3.26)
24
3.2.6 Accelerational constraints

The fulfillment of accelerational constraints at pi additionally depends on the velocities
set at pi1 . Here, the point of view for establishing forward consistency is presented,
the equations for backward consistency are obtained by assuming a reversed motion:
for backward consistency constraints between pi1 and pi one simply swaps their roles
in the corresponding equations for forward consistency.
3.2.6.1 Translational Acceleration

For further simplification we expect accelerational constraints to be symmetric, i.e.,
the absolute values of maximum acceleration and deceleration to be identical, denoted
atmax . Note that this does only affect the planned trajectory that we want to be smooth,
for constraints like safety margins for example it is still possible to assume different
values for deceleration and acceleration.
Through atmax only vi [vmin|at , vmax |at ] can be reached, with
vmin|at = vi1 atmax t , (3.27)
vmax |at = vi1 + atmax t . (3.28)

Eq. (3.21) allows to obtain the boundaries for translational velocities that comply with
atmax :
(p
vi1 2 2atmax s vi1 2 > 2atmax s
vmin|at = (3.29)
0 else,
p
vmax |at = vi1 2 + 2atmax s . (3.30)
3.2.6.2 Rotational Acceleration

Our method assumes constraints on translational and rotational acceleration to be
independent from each other. While this is true for synchro drives, it is certainly
not for the differential drive although suggested so by many controllers. Here, one
theoretically has to consider the translational accelerations of the left and right wheel
of a differential drive. However, there is a good reason to make the assumption of
independent accelerations: this way one can account for the differences in moments
of inertia between rotational and translational movements of the robot by imposing
appropriate limits for the respective accelerations. These differences in moments of
inertia depend on the shape of the robot and the resulting mass distribution and
therefore vary a lot among different types of robots.
Despite the fact that it introduces a lot of complexity into velocity profile generation
we choose to make the assumption because the errors caused by not accounting for
different moments of inertia usually are much bigger than the ones introduced by
assumed independence of rotational and translational accelerations.
From the parametric curve it is expected that its curvature does change approxi-
mately linearly between the planning points. A prerequisite that for any meaningful
curve in the domain of trajectory planning is easily achieved by a sufficient density of
planning points.
25
2s amax 2s amax 2s amax
v2 v1 v2 v2 v1 v1
v1 v2 v1 v1 v2 v2
(a) one interval (b) two intervals (c) empty solution set
Figure 3.5: Schematic figures for the shape of the parabola in vi defined by Eq. (3.31).
The upper row depicts the case ci > 0 (left curve), the lower row the case
ci < 0 (right curve). For ci > 0 the parabolas opening is directed upwards,
while it is directed downwards for ci < 0. For the solution set (depicted
by braces) one can distinguish three basic cases, depicted by (a)-(c). The
explicit interval borders are given by Eq. (3.32) and Eq. (3.33).
With the assumptions introduced above, translational acceleration has an effect on

the rotational acceleration between pi1 and pi . As the rotational acceleration is
bounded by amax , this also implies boundaries on vi that are mediated through = cv
to Eq. (3.17).
The following has to hold
vi ci vi1 ci1
amax amax ,
t
which with the help of Eq. (3.20) can be transformed to
2s amax ci vi 2 + (ci ci1 )vi1 vi ci1 vi1 2 2s amax . (3.31)
This quadratic inequality in vi describes a parabola whose basic shape is determined

by the sign of ci . Depending on the actual parameters there will be one or two intervals
that contain values for vi that fulfill Eq. (3.31). An illustration is given by Fig. 3.5:
the upper row shows the case ci > 0 (left curve) while the lower row depicts the case
ci < 0 (right curve). It will will now be shown that the case depicted in Fig. 3.5 (c) can
never occur and an explicit formulation for the solution set is derived with the help of
fall differentiation.
First, general conditions for intersection of the parabola with the upper and lower
bound will be derived. The case ci = 0 (driving straight) will at first be ignored and is
accounted for later.
26
Intersection with upper bound (2s amax )
ci vi 2 + (ci ci1 )vi1 vi ci1 vi1 2 = 2s amax

(ci ci1 )vi1 ci1 2s amax
vi 2 + vi vi1 2 =0
ci ci ci
s
(ci ci1 )vi1 (ci ci1 )2 vi1 2 ci1 2s amax
vi1,2 = + vi1 2 +
2ci 4ci 2 ci ci
s
(ci ci1 )vi1 (ci + ci1 )2 vi1 2 8s amax
vi1,2 = 2
+
2ci 4ci 4ci
(ci ci1 )vi1 1 p
vi1,2 = (ci + ci1 )2 vi1 2 + 8ci s amax
2ci 2|ci |
1 p
vi1,2 = (ci1 ci )vi1 (ci + ci1 )2 vi1 2 + 8ci s amax
2ci
For later use, we restate the result, naming both values for vi :
1 p
v1 = (ci + ci1 )2 vi1 2 + 8ci s amax ,
(ci1 ci )vi1 +
2ci
p (3.32)
1
v2 = (ci1 ci )vi1 (ci + ci1 )2 vi1 2 + 8ci s amax .
2ci
Intersection with lower bound (2s amax ) Analogously to the derivation of

Eq. (3.32) we obtain:
ci vi 2 + (ci ci1 )vi1 vi ci1 vi1 2 = 2s amax

...
1 p
vi1,2 = (ci1 ci )vi1 (ci + ci1 )2 vi1 2 8ci s amax .
2ci
We again name the result:
1 p
v1 = (ci + ci1 )2 vi1 2 8ci s amax ,
(ci1 ci )vi1 +
2ci
p (3.33)
1
v2 =

(ci1 ci )vi1 (ci + ci1 )2 vi1 2 8ci s amax .
2ci
Existence of intersections We briefly comment on when the intersections derived

above actually exist. The case ci > 0 is depicted by the upper row of Fig. 3.5. For
ci > 0 the discriminants in Eq. (3.32) are always positive, the intersections with the
upper bound (Eq. (3.32)) therefore always exist and consequently the case in Fig. 3.5
(c) cannot occur.
Whether or not the solution set is split into two intervals (Fig. 3.5 (b) vs. (a))
depends on the existence of intersections with the lower bound (Eq. (3.33)):

v1 , v2 exist (ci + ci1 )2 vi1 2 8ci amax s 0 . (3.34)
27
The lower row of Fig. 3.5 shows the case ci < 0. Here the discriminants in Eq. (3.33)
are always positive, therefore the existence of intersections with the lower bound
(Eq. (3.33)) is guaranteed. The case in Fig. 3.5 (c) is hereby proved to be irrelevant.
The existence of intersections with the upper bound (Eq. (3.32)) determines whether
or not the solution set is split into two intervals (Fig. 3.5 (b) vs. (a)):

v1 , v2 exist (ci + ci1 )2 vi1 2 + 8ci amax s 0 . (3.35)
The degenerate case ci = 0 For ci = 0, which corresponds to a straight movement,

Eq. (3.31) simplifies to
2s amax ci1 vi1 vi ci1 vi1 2 2s amax . (3.36)
This is a linear function in vi which intersects the upper bound in the point v1 , the
lower bound in v2 :
2s amax 2s amax
v1 = vi1 , v2 = vi1 . (3.37)
ci1 vi1 ci1 vi1
Consequently, the solution set for this case is

[
v1 , v2 ] ci1 > 0

[v2 , v1 ] ci1 < 0
vi

R ci1 = 0

else.
Note that for ci = ci1 = 0 any vi is admissible, as i = i1 = 0 always holds and

implies a zero rotational acceleration.
Solution set Summarizing all previous results, the solution set Ia0 can be stated as
(

[v2 , v1 ] (ci + ci1 )2 vi1 2 8ci amax s < 0

ci > 0

[v2 , v2 ] [v1 , v1 ] (ci + ci1 )2 vi1 2 8ci amax s 0

(

[v1 , v2 ] (ci + ci1 )2 vi1 2 + 8ci amax s < 0

ci < 0
[v , v ] [v2 , v2 ] (ci + ci1 )2 vi1 2 + 8ci amax s 0
vi Ia0 := 1 1 (3.38)

[
v1 , v2 ] ci1 > 0

[v2 , v1 ] ci1 < 0

ci = 0,

ci1 = 0

R
else
with v1 , v2 , v1 , v2 , v1 , v2 according to Eqs. (3.32), (3.33), and (3.37).

Note that we only want to consider positive velocities and therefore define the final
solution set as the intersection with the non-negative real numbers:
Ia = Ia0 R+
0. (3.39)
28
planned
velocity
atmax
amax
pi1 pi
Figure 3.6: Situation in which intervals of admissible translational velocities defined by

accelerational constraints atmax and amax overlap. Translational velocity
has to be reduced due to increasing curvature. Consider for example a car
approaching a curve.
3.2.7 Constraint Satisfaction

All of the isolated constraints are easily satisfied as they yield admissible velocity
intervals of the form [0, b], b R+ . One takes the minimum over all upper bounds,
i.e.,
vi min{vmax , vmax | , vmax |f , vmax |dobst }. (3.40)
When additionally accounting for accelerational constraints the situation gets more
complex: each accelerational constraint defines at least one interval of admissible trans-
lational velocities and thereby in general demands a minimum translational velocity for
a planning point. In case the intersection of these intervals is non-empty, the maximum
velocity contained in the intersection is chosen.
An example is shown in Fig. 3.6: the situation corresponds to a car approaching
a curve. The preceeding planning point has a lower curvature and a relatively high
translational velocity planned for it. As the next planning point lies within the curve,
an increased curvature at this point requires the car to slow down in order to not
exceed the maximum rotational acceleration. Here, the intervals defined by maximum
translational and rotational accelerations overlap and the translational velocity for the
planning point is set to the maximum contained in the intersection.
There is no guarantee, however, that the intervals [vmin|at , vmax |at ] and Ia overlap
in general. This means that when trying to establish forward consistency one can run
into a situation where due to the velocity vi1 planned for pi1 and the curvatures
ci1 and ci there is no velocity vi that respects both, translational and rotational
acceleration constraints. The analog is true for backward consistency.
Suppose for instance a car approaching a curve at a high speed: Even if skidding
is not a problem, it would either have to decelerate quickly or to abruptly change
its steering angle in order to stay on the road. If neither is feasible according to the
limiting constraints of the car, it will not be able to follow the curve. Fig. 3.7 shows
a a schematic, non-exhaustive visualization of the problem. Similar to Fig. 3.6 the
intervals of translational velocities that comply maximum translational and rotational
acceleration are shown, but they do not overlap.
An ad-hoc solution for this problem would be to iterate the process of readjusting
(i.e., decreasing) velocities until an admissible velocity profile has been found. The
drawback of this approach would be a potentially long and to some degree unpredictable
29
planned
velocity
atmax
amax
pi1 pi pi1 pi pi1 pi pi1 pi
Figure 3.7: Schematic, non-exhaustive depiction of the overlap problem. During veloc-
ity profile generation a situation can occur, where the admissible transla-
tional velocities respecting constraints for translational acceleration (red)
and the ones for rotational acceleration (blue) do not overlap and therefore
no admissible vi or vi1 can be found. This is due to the current combi-
nation of the curvatures ci1 and ci and the planned velocity vi1 or vi ,
respectively.
execution time. However, there is another solution that forgoes iteration: given the
curvatures ci1 and ci one can analytically determine the biggest upper bound vi1 for
vi1 such that for all vi1 vi1 an overlap of [vmin|at , vmax |at ] and Ia is guaranteed.
To assure that vi then lies within the overlap, the same method can be applied in a
backward fashion due to our symmetry assumptions.
The bounds derived thereby can be treated as additional isolated constraints for vi1
and vi and they are therefore easily integrated into the first phase of velocity profile
generation. As the derivation of the bounds is quite lengthy, we skip it here and refer
the reader to the appendix. The derivation of the upper bound for vi1 can be found
in Appendix B.1, while Appendix B.2 presents pseudo code for its computation.
With the overlap of the intervals guaranteed by isolated constraints we can establish
forward and backward consistency as presented in Section 3.2.1.2. The difference is that
one additionally has to establish consistency with respect to rotational acceleration.
Note that attention has to be paid to choosing the appropriate interval from Ia when
determining the maximum admissible velocity respecting rotational acceleration.
Fig. 3.8 provides a visualization of velocity profiles. It depicts velocity profiles to-
gether with the different isolated constraints for two differently shaped trajectories. The
plots show how the final velocity profile respects constraints on translational velocities
imposed by maximum translational and rotational velocity vmax and max , maximum
centripetal force fmax , obstacle distance dobst , and the aforementioned additional con-
straint that guarantees overlapping of [vmin|at , vmax |at ] and Ia . Furthermore, it can
be observed how accelerational limits affect the final velocity profile: it accelerates and
decelerates smoothly between the minima of the isolated constraints and the start and
end velocity of zero.
The corresponding splines for the velocity profiles can be found in Fig. 4.5, the
smoother spline was generated from the one with sharper curves by the optimization
method presented in Chapter 4. Note how the optimization drastically reduces the
effects imposed by the constraints.
30
translational
velocity [m/s]
0.8 overlap
obstacles
centripetal
0.6 rotational
vmax final profile
0.4
0.2
0
arc length
(a) Values for a trajectory with sharp curves, see Fig. 4.5a.
translational
velocity [m/s]
0.8 overlap
obstacles
centripetal
0.6 rotational
vmax final profile
0.4
0.2
0
arc length
(b) Values for a smooth trajectory, see Fig. 4.5b.
Figure 3.8: Different sources of isolated constraints imposed on translational velocity.

The final velocity profile obeys these constraints as well as accelerational
constraints, as can be seen for example by the smooth acceleration from
starting velocity zero. The profiles are for the trajectories shown in Fig. 4.5.
(a) shows the data for a trajectory with sharp curves whereas the plot
in (b) corresponds to a smoothened spline (Fig. 4.5b) generated by the
optimization method presented in Chapter 4.
31
4 Trajectory Generation
This chapter presents how trajectories can be obtained and optimized. Recall that we
are given a map and sparse waypoints as an input to our system. The line path defined
by the waypoints is required to be traversable by the robot, i.e., it is expected to be
free of obstacles.
At first, a trajectory will be generated that closely follows the line path induced by
the waypoints. This trajectory will then be iteratively optimized in 2D shape by moving
around control points and elongating spline tangents at the latter. Optimization will
be stopped if either the optimal trajectory has been found or time constraints prohibit
further computation. With this design our system is able to generate an admissible
trajectory instantly (anytime capability) while efficiently using available computational
resources. Fig. 4.1 provides a schematic overview over this system.
The next section covers the generation of the initial trajectory through the use of
heuristics. The remaining sections provide details on the optimization of the initial
trajectory and the issue of planning a new trajectory while still following the old one.
4.1 Initial Trajectory

The intuition behind the initial trajectory is to correspond to traversing the line path
induced by the waypoints in a drive and turn fashion. As a consequence, we expect the
highest curvature near the waypoints, since these are the turning points of the path.
For the generation of the initial trajectory the given waypoints will be used as start
and end points of the segments of a Quintic Bezier Spline. To specify tangents and
second derivatives at these points, heuristics will be used that extract suitable values
from the geometric shape of the line path induced by the waypoints.
4.1.1 First Derivative Heuristic

The first derivatives, or tangents, at inner waypoints (i.e., all but the starting and the
end point) are defined by employment of the following heuristic.
In the following we assume computation of the tangent for the waypoint Pi . As it is
an inner waypoint, it inscribes an angle with its neighbor points, = Pi1 Pi Pi+1 . The
tangent will be set perpendicular to the angular bisector of , pointing in the direction
of travel. To prevent unnecessary oscillations of the spline, the tangent magnitude
is set to half of the minimum Euclidean distance to a neighbor control point, i.e.,
1
2 min {|Pi1 Pi |, |Pi Pi+1 |}. Fig. 4.2 provides an exemplary visualization of the heuristic.
Application of the heuristic means that half of the heading change between the straight
line connections Pi1 Pi and Pi Pi+1 has taken place when reaching Pi . This heuristic
is well known to yield smooth curves and is for instance referred to in [17, p. 318].
For the first waypoint the tangents direction is given by the robots heading, for the
last waypoint it is set to match the one of the straight line connection to the previous
32
Sparse Waypoints Map
Initial Trajectory
Optimization
System
Final Trajectory
Figure 4.1: Schematic system overview with processes/functional entities (rounded cor-
ners) and objects (normal rectangles). With sparse waypoints and a map
as input the system first generates an initial trajectory. This trajectory is
then optimized in an iterative fashion.
B (5,5)

2 2

2

2 C (7,2)
D (4,1)
A (0,0)
Figure 4.2: Visualization of the tangent (first derivative) heuristic applied to a Quintic
Bezier Spline. The tangents at control points B and C (red arrows) have
been set according to the heuristic: they are perpendicular to the angu-
lar bisector of the angle inscribed by the particular control points and its
neighbors (dashed blue). Furthermore their magnitude is proportional to
the distance to the nearest neighbor control point.
33
B (2,4)
C (5,2)
A (0,0)
(a) Splines
curvature
0
curve param.
-1
-2
-3
-4
-5
(b) Curvature profiles
Figure 4.3: Second derivative heuristic visualization. Black: a Quintic Bezier Spline,
red: two Cubic Bezier Splines. (a) shows the splines while (b) displays
their curvature profiles. The splines start at point A and B, respectively.
The Quintic Bezier Splines second derivative at point B has been set to a
weighted average of the two Cubic Bezier splines. Except for the second
derivative at point B the Quintic Bezier Splines first and second derivatives
match the ones of the particular Cubic Bezier Splines. Note the curvature
discontinuity at the join point B for the Cubic Bezier Splines.
34
waypoint. Similar to the inner waypoints the tangents magnitudes are set to half of
the Euclidean distance to the neighbor waypoint.
The suitability of the employed heuristic is shown below during treatment of trajec-
tory optimization (Section 4.2.3).
4.1.2 Second Derivative Heuristic

With the second derivative our goal is to retrieve a spline whose curvature does not
change too much. As proved in [24, Sect. 2.3, Corollary 2.8], cubic splines minimize the
integral of the second derivatives absolute value. In general, this corresponds to small
changes in curvature. Although preconditions are not exactly met (second derivative
at start and end point do not necessarily equal zero), we try to mimic the behavior of
cubic splines.
If we connected the waypoints Pi1 , Pi and Pi , Pi+1 with Cubic Bezier Splines each,
we would have minimal curvature change on each of the segments. As discussed in
Section 3.1.2.1, however, this would result in a curvature discontinuity. The intention
now is to set the Quintic Bezier Splines second derivatives at the join point to a
weighted average of the values that two Cubic Bezier Splines would have. See Fig. 4.3
for a visualization of this heuristic.
For simplicity we adopt the notation used in Fig. 4.3 for the derivation. Our goal is to
determine the second derivative at the point B for the Cubic Bezier Spline connecting
A and B (SAB ), as well as for the one from B to C (SBC ). We consider the tangents
tA , tB , and tC at the points A, B, and C to be given.
With this information, the control points of the two Cubic Splines compute to (see
Eq. (A.22))
1 1
SAB : A, A + tA , B tB , B
3 3
1 1
SBC : B, B + tB , C tC , C.
3 3
For the second derivatives of the splines at the point B this yields (refer to Eq. (A.23)):

1 1
SAB (1) = 6
00
A + tA 2 B tB + B = 6A + 2tA + 4tB 6B
3 3

1 1
SBC (0) = 6 B 2 B + tB + C tC
00
= 6B 4tB 2tC + 6C.
3 3
While one could now compute a simple average of the two second derivatives, it makes
even more sense to weight the second derivatives according to the relative size of their
respective segments. We will do this in an inversely proportional fashion, which might
seem counterintuitive at first glance but yields the effect that a long segment will not
dramatically deform a much shorter adjacent segment.
Let the generic weighted average of the second derivatives be
a = SAB
00
(1) + SBC
00
(0) = (6A + 2tA + 4tB 6B) + (6B 4tB 2tC + 6C) .
To define weights with the above mentioned properties, let dAB be the distance
between A and B, say dAB = |AB|, dBC analogously. The weights for the segments
35
Start (0,9)
(3,5.5)
(5.5,5) End
(1.5,4)
Figure 4.4: Initial trajectory generation: The given waypoints and the induced line
path are depicted in blue. The red arrow indicates the robots heading at
the start position. The generated initial trajectory is given by the black
solid curve. The black markers indicate the control points of the Quintic
Spline that are not given waypoints. Note that the tangent length has been
reduced to a quarter of the minimum adjacent segment length to achieve
even less deviation from the line path than in Fig. 4.2 for example.
are then given by

dBC dAB
= , = .
dAB + dBC dAB + dBC
As for the first and last waypoint only one segment is available, one simply takes the
second derivative corresponding to that segment. Note that the second derivative at
the first waypoint might also be predetermined when planning a new trajectory while
the robot is already moving.
4.1.3 Generation
To generate an initial trajectory, the given waypoints are chosen as start and end
points of Quintic Bezier Spline segments. The remaining control points, that determine
first and second derivatives at the waypoints, are obtained by means of the respective
heuristics presented above. Compared to the original tangent heuristic, tangents are
shortened by factor 2 to achieve less deviation from the waypoint induced line path.
Fig. 4.4 gives an example for an initial trajectory. It shows the given waypoints, the
induced line path, and the initial trajectory as well as the initial heading of the robot.
We now have a trajectory that closely follows the line path induced by the waypoints
in a drive straight and turn fashion and is therefore not colliding with any obstacles.
For this trajectory a velocity profile could be instantly generated and together with
the trajectory provided to a controller for execution. The next section describes how
36
4.2 Optimization
start start
p2
p1
(a) Initial trajectory (b) Optimized trajectory
Figure 4.5: Trajectory before and after optimization. (a) shows the initial trajectory
with the gradient induced coordinate system for waypoint translation (black
arrows). In (b) the result of the optimization process is shown. Red markers
stand for spline control points, tangents at inner waypoints are depicted by
red lines. Trajectories are shown in blue above the distance map, where
darker values are closer to obstacles.
this initial trajectory can be optimized iteratively until an optimal solution has been
found or time constraints enforce returning the best trajectory found so far.
4.2 Optimization
The goal of the optimization is to change the initial trajectorys shape in a way that
it yields lower costs, for this work this means that it can be traversed faster. This can
be reached by smoothing the trajectory to have less sharp curves and by reducing the
overall arc length, for example. The optimization has to balance these two potentially
conflicting means to reach its goal of a minimum traversal time while respecting the
robots kinematic and obstacle distance constraints.
In this section we will first introduce the parameters of the spline we are going to
optimize and present the method that is used to do so. Afterwards, an empirical
examination of the search space is conducted to assure the suitability of the employed
optimization method.
For a rough idea of what the method does refer to Fig. 4.5. It shows a trajectory
before and after optimization, the grayscale background of the figures is the correspond-
ing distance map. The optimized trajectory can be traversed faster and the occurring
kinematic constraints are less harsh, as can bee seen in Fig. 3.8. It shows the velocity
profiles and isolated constraints for the splines in Fig. 4.5.
4.2.1 Parameters for Spline Optimization

As mentioned above, to influence the shape of the initial trajectory we choose to use
the 2D location of the inner waypoints and the tangent magnitude at the waypoints
37
as optimization parameters. This leaves us with three degrees of freedom per inner
waypoint.
Waypoint 2D position For the parameterization of a waypoints translation we choose

the first parameter to be the translation of the waypoint in the direction of the distance
maps gradient at the waypoint location. The second parameter will be the translational
component orthogonal to this direction. This way we use a local coordinate system that
is induced by the distance map gradient. This corresponds to moving a waypoint closer
to or further away from an obstacle, and translating the waypoint parallel to the closest
obstacle, respectively. With the use of this coordinate system the two parameters for
translation now have a meaning, especially with respect to the part of the cost function
that addresses obstacle proximity. Note that whether using the meaningful coordinate
system or the one provided by the distance map, the reachable 2D translations are the
same, of course. In the left part of Fig. 4.5 the induced coordinate systems are shown
by black arrows at the inner waypoints p1 and p2. Details on the Sobel Operator based
computation of the gradients can be found in Appendix C.
Tangent magnitude The parameterization of the tangent magnitude will be repre-

sented by the tangent elongation factor, a scalar value indicating by which factor the
initial tangent has to be multiplied to retrieve the desired tangent. Elongation of a
tangent causes the curve at the particular join point to become less sharp and therefore
in general allows faster traversal of the trajectory. See for instance the change in shape
of the curve at p1 in Fig. 4.5 and in the magnitude of respective tangents.
One might ask oneself why the tangents direction is not considered as optimization
parameter, the answer is that we found the heuristic introduced in the previous section
to yield results that hardly need optimization. Support for this claim will be given
below. Note that when translating an inner waypoint, the tangent direction at this
point and its neighbors will be corrected according to the heuristic while keeping the
elongation factors.
Search space topology The space that we will search for the optimized trajectory
is spanned around the initial trajectory by continuous variation of the above men-
tioned parameters for each of the inner waypoints. The evaluation criterion for each
combination of parameters is the total traversal time. For splines that collide with
the environment an infinite traversal time is assumed. This causes the optimization
procedure to discard these parameter combinations.
Note that we furthermore restrict the search space in the following manner: we do not
continue the search in directions that yield collision causing parameter combinations.
In practice this scenario occurs if there are two paths around an obstacle: we will only
consider the one suggested by the initial trajectory and not incrementally translate
waypoints through the obstacle to reach the other path, a part of the search space that
is disconnected by colliding splines.
4.2.2 Optimization Method

The optimization method we use to search for the optimal trajectory is inspired by the
RPROP algorithm. RPROP stands for resilient backpropagation and is an algorithm
38
4.2 Optimization
1 P parameters to be optimized;
2 BestTrajectory initial trajectory;
3 while time left do
4 forall p P do
5 CurrentTrajectory BestTrajectory;
6 repeat
7 (, CurrentTrajectory) RPROP(p, CurrentTrajectory);
8 if evaluate(CurrentTrajectory) > evaluate(BestTrajectory) then
9 BestTrajectory CurrentTrajectory;
10 break;
11 until < ;
12 tryWaypointDeletion(BestTrajectory)
Figure 4.6: Pseudo code for the optimization method based on the RPROP algorithm.
for weight adaption in neural networks. A brief introduction can be found in a technical
report [27], while [28] is the original publication of the algorithm. It is a gradient
descend method that only uses the sign of partial derivatives to determine the direction
of the weight change. The magnitudes of the derivatives are ignored, the actual amount
of change in the weights is determined by an additional heuristic. As the sign of partial
derivatives can be computed by pointwise evaluation of the cost function, this renders
RPROP a derivative free optimization algorithm. Further advantages of the algorithm
are its robustness with respect to local minima and its fast convergence compared to
standard gradient descent methods.
We base our optimization method on RPROP because of its robust and fast con-
vergence and because it is derivative free. The derivative of our cost function is too
involved to be obtained in closed form: deriving the influence of shape parameters
on time of travel on its own involves deriving arc length and curvature functions and
further complexity is introduced with the influence of the distance map on admissible
velocities.
The major difference in our adoption of RPROP is that we do not update all param-
eters at the same time but choose to iterate through them and treat each parameter
as a one-dimensional optimization problem. While doing so, we interweave the opti-
mization of the different parameters, i.e., we do not fully optimize a parameter before
proceeding to the next one. The treatment of one dimensional subproblems is chosen
to reduce the number of gradient sign computations and thereby the number of velocity
profile generations.
Fig. 4.6 gives the pseudo code for the proposed optimization method. Starting with
the initial trajectory it always keeps a reference to the best trajectory so far. As long
as there is time left for optimization the method cycles through the parameters P of
the spline (2D location of the inner waypoints and tangent elongation) and triggers a
one-dimensional optimization using RPROP. As soon as a RPROP step successfully
improved the trajectory, optimization of the parameter is stopped and the method
moves on to the next parameter. The same is true for the case that the step size
computed by RPROP falls below a certain threshold ( in the algorithm). In Fig. 4.6
the function RPROP stands for one step of RPROP optimization that returns a new step
39
(a) Scene 1 (b) Scene 2 (c) Scene 3
(d) Scene 4 (e) Scene 5 (f) Scene 6
(g) Scene 7
Figure 4.7: Scenes used for search space inspection. Obstacles are shown in black, the
robots start position is blue and the goal is indicated by a yellow dot.
The waypoints and the line path induced by them are shown in blue. For
examination of Scene 1 the path has been cut after the first four waypoints
(compare Fig. 4.5, it shows initial and optimized trajectory for Scene 1).
size and an updated trajectory. The evaluate function rates a trajectory with respect
to a cost function. The trajectory with the shorter time of travel is chosen. At this
point, other cost measures like anticipated energy consumption or steering effort could
be used instead.
Deletion of inner waypoints In addition to parameter optimization using RPROP,

we employ the deletion of inner waypoints as mean of optimization. Not seldom, such
a deletion results in a more optimal path, as complexity is reduced due to one less
waypoint to pass through. Another side effect is that in total less parameters remain
to be optimized, which considerably speeds up the process.
We choose to delete a waypoint only if the resulting trajectory is collision free and
considered a better one with respect to our cost function. Note that when deleting
an inner waypoint, tangent directions at the neighboring waypoints are not updated
according to the heuristic because this might cause a collision with the environment.
Tests for possible deletion of inner waypoints are conducted after every complete
pass through all parameters, the pseudo code in Fig. 4.6 refers to it as the function
tryWaypointDeletion.
4.2.3 Examination of the Search Space

In this section we will examine the search space defined above with respect to the
applicability of the suggested optimization method. Evidence is presented that the
optimization method converges to near optimal results and that it is not disturbed
40
4.2 Optimization
by local minima in the search space. Furthermore, we elaborate on how changes in

optimization parameters affect the spline and associated costs.
To examine the search space, we rely on an empirical inspection, as the general space
is difficult to handle. Parameter combinations are evaluated in an exhaustive fashion
for several scenes. The hereby gained discretized views of the search space are then
examined. The input basis for this is constituted by the seven scenes shown in Fig. 4.7.
As we expect the search space to be smooth and well behaved for scenes without close
obstacles or sharp turns in the waypoint path, we choose scenes that represent critical
situations for our examination. Paths have to navigate around obstacles and their
shapes are very diverse. For the most part, the obstacles shape cannot be suitably
approximated by a circumcircle. This induces sharp turns into the waypoint path.
Note that for Scene 1 the input path was pruned to contain only the first three
segments in order to keep the exhaustive search computationally tractable. This way all
input scenes contain paths with two inner waypoints, which leads to a six-dimensional
search space (three dimensions for each inner waypoint).
We start the examination with the subspaces the optimization parameters induce
when modified exclusively.
Tangent elongation Fig. 4.8 shows the influence of tangent elongation in the initial
trajectory for several scenes. For combinations of tangent elongations for both inner
waypoints p1 and p2 the durations of the velocity profiles are shown.
Initial trajectories have a tangent elongation factor of 0.5 for both inner waypoints
p1 and p2, this combination is found in the the plots lower left corner. The plots
show that elongating tangents in principle allows the spline to be traversed faster.
Beyond a certain point, however, further elongation of the tangents again increases the
durations or even yields trajectories that collide with the environment (white areas).
Increasing durations occur as soon as the trajectory cannot be traversed any faster but
the widened curves lead to a bigger overall arc length instead.
It can be seen that the space is smooth and free of local minima. The plots for the
remaining scenes in Fig. 4.8 look similar.
Waypoint translation Translating inner waypoints can affect the time needed to tra-
verse a trajectory in several ways. Firstly, translating waypoints has a direct effect on
overall arc length and can furthermore lead to a change in curvature at the join points.
Secondly, by translating waypoints, the trajectory can change its distance to nearby
obstacles, influencing induced velocity limits (see Section 3.2.5).
Fig. 4.9 shows how translation of the first inner waypoint (p1) in the initial trajectory
changes time of travel for several scenes. The figure shows the corresponding initial
trajectories and distance maps.
The plots axes represent movement along the gradient direction and orthogonal to
it, this gradient induced coordinate system is visualized with black arrows in the cor-
responding distance maps. Looking at these coordinate systems in the right column of
Fig. 4.9 one observes that movement along gradient direction generally corresponds to
increasing obstacle distance while orthogonal movement corresponds to lateral trans-
lation with respect to the closest obstacle.
The relation between waypoint translation and time of travel is observable in Scene 4
(Fig. 4.9b). From the plot one can see that the optimal position for the waypoint p1 is
41
duration [s]
10.5 36
34
8.5
tangent elongation p2
32
6.5 30
4.5 28
26
2.5
24
0.5 22
0.5 2.5 4.5 6.5 8.5 10.5
(a) Scene 1
duration [s]
10.5 28
26
8.5
24
6.5
22
4.5
20
2.5
18
0.5 16
0.5 2.5 4.5 6.5 8.5 10.5
(b) Scene 4
duration [s]
10.5 38
36
8.5
34
6.5 32
4.5 30
28
2.5
26
0.5 24
0.5 2.5 4.5 6.5 8.5 10.5
(c) Scene 7
Figure 4.8: Elongation of tangents for initial trajectory and its effects on time of travel.
The initial trajectory has elongation factors of 0.5 for both tangents (lower
left corner). Darker is better, contours have been added for intervals of
0.5 seconds. Parameters that correspond to white areas yield collisions
with the environment. Figures look similar for the remaining scenes.
42
4.2 Optimization
duration [s]
38
0.8
36
orthogonal movement
0.4
opt
34
start
0
32
p2
-0.4
30
p1
-0.8 opt
28
-0.8 -0.4 0 0.4 0.8
movement in gradient direction
(a) Scene 1
duration [s]
36
0.8 opt
34
orthogonal movement
0.4 32
30
start
0 28
p2
26
-0.4 24 opt
22
-0.8 p1
20
-0.8 -0.4 0 0.4 0.8
(b) Scene 4
duration [s]
40
0.8
38
orthogonal movement
36
0.4 p2
34
start 32
0
30 opt
opt p1
28
-0.4
26
24
-0.8
22
-0.8 -0.4 0 0.4 0.8
(c) Scene 5
Figure 4.9: Translation of waypoint p1 for initial trajectory and its effects on duration.
Right part shows corresponding initial trajectories. In the left part darker
is better, contours have been added for intervals of 0.5 seconds. White
areas correspond to parameter combinations that lead to collisions with
the environment. Optima and start positions are marked. Figures look
similar for the remaining scenes.
43
in negative gradient direction and positive orthogonal direction from the start position.
It is indicated by a white marker in the plot and a black one in the distance map in
the right part of the figure.
We also observe that moving p1 closer to the obstacle, i.e., movement in negative
gradient direction, is not feasible without prior lateral movement along the obstacle,
here in positive orthogonal direction. As can be seen in the distance map, moving p1
towards the obstacle leads to a collision of the trajectory on the right side of p1. In the
plot these collisions correspond to the triangular white area in the lower left. Lateral
movement of p1 centers the point below the obstacle and thereby allows passing it
without collision when moving p1 towards it.
The parameter combinations that yield the rectangular white area in the right part
of the plot correspond to moving p1 away from the obstacle beyond a certain threshold.
A look at the distance map reveals that a collision of the trajectory with the obstacle
at p2 is inevitable when moving p1 in positive gradient direction, regardless of any
additional movement in orthogonal direction.
One observes that the search space is smooth and free of local minima for translation
of the first inner waypoint. The plots for the remaining scenes in Fig. 4.9 look similar
to the presented ones and analog observations have been made for the translation of
the second inner waypoint p2.
Combined search space The spaces discussed above only constitute two-dimensional
subspaces of the actual search space, which consists of two inner waypoints with three
parameters each and therefore has six dimensions. As visualization gets difficult for
this dimensionality we have to rely on different means for inspection of the complete
space.
The smoothness of the subspaces suggests that this is also the case for the com-
bined search space. However, interaction effects might occur. We therefore conduct an
exhaustive search in the discretized six-dimensional space for each scene.
For Scene 5 the discretized space contains only one minimum. For the other scenes
several local minima are detected. However, they either yield durations within 0.2 sec-
onds range of the optimum, which is roughly 1% of the total travel time, or they
correspond to extreme parameter settings that yield a considerably higher duration.
As another mean to assess the applicability of the optimization method in the com-
bined space we compare its results to the ones obtained by the aforementioned ex-
haustive search. Fig. 4.10 shows how the proposed optimization method performs in
each scene. It is always able to find a trajectory in the vicinity of the optimum in the
discretized space. In the left part the duration ratio of the searchs trajectory and the
one that is the optimum found by the exhaustive search are plotted against the num-
ber of optimization steps. As soon as this ratio approaches 1, the search has found a
trajectory that is as good as the discretized optimum. For Scene 4 the ratio even drops
considerably below 1. Due to not being limited by discretization the search method
was able to find a better trajectory than the discretized exhaustive search. The table
in the right part of the figure states the absolute numbers. It lists duration of the
initial trajectory, the best one found by the discretized exhaustive search, and the one
the search method converges to.
44
4.2 Optimization
duration
ratio
1.6 Scene 1
Scene 2
1.5 Scene 3 durations [s]
Scene 4 Scene initial exh. search optimized
1.4 Scene 5 1 28.73 20.89 20.78
Scene 6 2 18.10 12.01 11.92
1.3 Scene 7
3 26.75 19.29 19.27
4 23.10 15.90 14.92
1.2
5 22.85 15.76 15.72
1.1 6 21.76 13.69 13.68
7 30.25 23.53 23.48
1
0.9
0 100 200 300 iter-
ations
Figure 4.10: Optimization process for Scenes 17. The plot shows the ratio of duration
and optimum duration found by the (discretized) exhaustive search against
the number of iterations. The table lists the absolute values.
Tangent rotation To conclude the examination of the search space we provide some
evidence for the suitability of the employed tangent heuristic. While one could include
tangent rotation as an additional parameter in the optimization, we chose to rely on
the heuristic for complexity reasons, as the use of the heuristic decreases the number
of optimization parameters.
To ensure that by not considering tangent orientation for parametric optimization
not too much potential is given away, an exhaustive search of the space including
tangent rotation is conducted. The now eight-dimensional space has to be heavily
discretized to keep the search tractable.
Tab. 4.1 shows the differences in optimal trajectory duration if considering tangent
rotation or not. Note that data was generated with a different discretization and
slightly different parameters for obstacle imposed velocity limits, therefore the optima
listed in Tab. 4.1 do not exactly match the ones in the right part of Fig. 4.10.
In Tab. 4.1 one observes that the durations for the optima of the exhaustive search
do not differ dramatically, including tangent rotation into the search space did not
yield optima with considerably better durations.
More evidence that tangent rotation is set to a sensible value by the heuristic is given
in Fig. 4.11. It shows the two-dimensional space spanned by tangent rotation at both
inner waypoints around the trajectory the optimization method converged to. Darker
values stand for less time of travel and contours have been added to the figure at levels
of 0.5 seconds. White areas correspond to values for rotation that yield trajectories
colliding with the environment. One observes that deviation from the rotation set by
the heuristic can hardly improve the duration of the trajectory. In fact, examination
of the space for Scenes 17 results in a maximum improvement of 0.09 seconds, i.e.,
45
duration [s]
28
tangent rotation at p2 in degrees

40
27
30
20 26
10 25
heuristic
0
24
-10
-20 23
-30 22
-40
21
-40 -30 -20 -10 0 10 20 30 40
(a) Scene 1
duration [s]
21
40
20
30
19
20
18
10
heuristic 17
0
16
-10
15
-20
14
-30
13
-40
12
-40 -30 -20 -10 0 10 20 30 40
(b) Scene 2
duration [s]
36
40
30 34
20
32
10
heuristic 30
0
-10
28
-20
-30 26
-40
24
-40 -30 -20 -10 0 10 20 30 40
(c) Scene 7
Figure 4.11: Tangent rotation for first and second inner waypoint for final optimized
trajectory and its effects on time of travel. Darker colors stand for shorter
durations. Contours have been plotted for levels of 0.5 seconds. The
tangent heuristic corresponds to values of 0 for both rotations. Figures
for scenes remaining look similar.
46
4.3 Updating Plans
exh. search best duration [s]

Scene with rotation without rotation difference [s]
1 20.71 20.80 0.09
2 11.92 12.15 0.23
3 19.24 19.31 0.07
4 15.72 16.16 0.44
5 15.56 15.58 0.02
6 13.60 13.60 0.00
7 23.50 23.52 0.02
Table 4.1: Differences in optimal duration regarding consideration of tangent rotation

for optimization. The data is based on an exhaustive search in the discretized
eight-dimensional space.
0.5% of the total time of travel, if one considers tangent rotation only (numbers not
shown in the figure). For the evaluation, rotation up to 45 degrees in both directions
has been considered, values beyond that tend to yield unfavorably deformed splines as
tangents increasingly point against the general direction of travel.
4.2.4 Runtime
We briefly describe the runtime of the proposed method. The predominant part of
computational expense is consumed by the generation of the velocity profile. Herein,
the numerical integration of the 2D path of the trajectory constitutes constant costs
for each segment. Calculations at the planning points yield costs that are linear in
a segments arc length. Costs for calculating the spline from its parameters and for
collision checks are linearly depending on the number of spline segments. Consequently,
the runtime is linear in the trajectory length and in the number of waypoints provided
to the method.
In most cases around 500 RPROP steps can be executed within 0.4 seconds for paths
consisting of four waypoints. Significant improvements are in general achieved earlier
in the optimization process, as can be seen in Fig. 4.10 as well, the major improvements
take place within the first 100 RPROP steps, which corresponds to 0.1 seconds. Note
again the anytime capability of the proposed method, i.e., it provides the continuously
improving trajectory at any time during the optimization.
The runtimes correspond to execution on an Intel Pentium 2 GHz dual processor.
The optimization method used to gather this data can be further improved regarding
implementational and conceptual aspects (see also Section 6.2).
4.3 Updating Plans

During the execution of the planned trajectory, it becomes necessary to update the
active plan for mainly two reasons. First, sensors might have detected an unmapped
obstacle that leads to a collision in the active path or at least to changes in costs.
The second reason is that accumulating odometry error leads to a displacement of the
47
3 4
5
2
Figure 4.12: Schematic visualization of the effects of accumulated odometry error and
a plan update as countermeasure. The reference frame is the ground
truth, localization errors are ignored. For details please refer to the text
(Section 4.3.1).
robot with respect to the planned trajectory in case that the employed controller uses
odometry as feedback exclusively.
This is the case for most controllers and is also due to the fact that in high precision
robots, odometry readings are available at a considerably higher frequency than high
level position estimates generated by the employed localization method. We now show
how updating the active plan can account for the accumulation of odometry errors and
for unmapped obstacles.
4.3.1 Odometry Errors

Eventually the robot pose estimated by accumulated odometry readings will deviate
from the estimates by localization. As a result, derivation from the planned trajectory
increases over time.
Fig. 4.12 provides a schematic visualization for this situation: the reference frame
for this figure is the ground truth, localization errors are ignored. The robot starts
at position 1. While the black solid line depicts the planned trajectory, the dashed
black line stands for the execution of the planned trajectory in case the plan is not
updated. Over time the deviation from the planned path increases due to accumulating
odometry error and leads to a collision.
At position 3 an update of the plan is initiated. At this time the robot is located
at position 3 but the estimate by the odometry based controller is at the originally
planned position 2. During the update, the position estimate based on the odometry
is reinitialized to the current localization, thus the accumulated error vanishes.
To account for the time consumed by updating the plan, a lookahead is done to
determine the anticipated location of the robot at the time the updated plan will be
available. In Fig. 4.12 this position is marked with the number 4, the blue solid line
represents the anticipated movement of the robot during the planning phase. A new
48
4.3 Updating Plans
plan (red solid line) is therefore computed to smoothly fit at this position and prevent
the impending collision.
The odometry error that accumulates during the planning phase for the updated
trajectory is assumed to be small compared to the one building up to the beginning of
the phase. It results in the robot actually being at position 5 at execution time of the
updated trajectory. As a consequence the actual execution of the updated trajectory
(depicted as dashed red line) slightly differs from the planned trajectory.
Note that Fig. 4.12 only constitutes a schematic visualization of the situation, i.e.,
it does not claim proportionality. For easier reference we restate the meaning of line
colors and numbers in the figure:
black solid original trajectory
black dashed robots execution of trajectory due to odometry error
blue solid anticipated robot movement during the planning phase
red solid updated plan
red dashed execution of updated plan (due to to a small odometry error during re-
planning phase)
1 initial robot position
2 ideal robot position at replanning time (controller thinks robot is here)
3 real robot position at replanning time (due to odometry error)
4 robots assumed position at start time of new plan
5 robots real position at new plan execute time (due to small odometry error during
replanning phase)
4.3.2 Unmapped Obstacles

Through updating the trajectory as described above one can also account for unmapped
obstacles. This is done by simply incorporating current sensor readings into the map
just before planning the new trajectory. As a result, the updated trajectory accounts
for obstacles that are not contained in the original map of the environment but sensed
at planning time.
For our work we employ a simple version of sensor data incorporation, only the most
current laser scan is added to the original map of the environment.
Adding currently sensed obstacles to the environment map before updating the tra-
jectory not only accounts for unmapped obstacles, moderate localization errors are
treated by this technique, too. A displaced position estimate results in accordingly
expanded obstacles within the sensor field of view that are then safely circumvented
by the updated trajectory. Extending sensor incorporation to not only add obstacles
but also mark unexpectedly clear areas of the environment would further improve the
systems capabilities to cope with localization errors but is not addressed in this work.
49
4.3.3 Subdivision of Long Paths

For planning situations where the path to the goal is long, planning the complete path
takes a lot of time, as the computational expenses are linear in path length and number
of waypoints (see Section 4.2.4). Furthermore, motion planning beyond the sensors
field of view is of limited use only.
Therefore, a trajectory is only planned for the first part of a long path. During plan
updates, new trajectory segments are appended to the existing trajectory with smooth
join, i.e., with continuity in heading, curvature, and velocity. This way, a smooth path
from start to goal is computed iteratively over time in an efficient way.
4.3.4 Generating an Updated Trajectory

The basic idea for the generation of an updated trajectory is to determine the state of
the robot, which consists of its current position, velocity, and acceleration, at the point
in time that corresponds to the start of the new trajectory. From this states position
a new set of waypoints is requested as a basis for trajectory generation as described
earlier in this chapter. Two aspects require attention herein: firstly, the robot state
estimation has to be corrected according to the current localization and secondly, a
smooth fitting of the updated trajectory to the existing one has to be realized.
We will now develop several aspects of this smooth fitting.
4.3.4.1 Continuity of Derivatives

To ensure continuity in tangent direction and curvature, continuity in first and second
derivative (i.e., C 2 continuity) is a sufficient condition. The use of Quintic Bezier
Splines allows us to set arbitrary values for the first and second derivative (Eqs. (3.12)
and (3.13)), which makes establishing continuity of the derivatives straightforward.
However, it is also possible to achieve tangent direction and curvature continuity
with less strict demands.
4.3.4.2 Geometric Continuity of Derivatives

Achieving tangent direction and curvature continuity by geometric continuity instead
of parametric continuity of derivatives gives us the possibility to modify the magnitude
of the tangent at the new trajectorys starting point. We include this additional di-
mension into the optimization method. By that, it is possible to straighten out initial
trajectories that start with sharp curves, which is of benefit when establishing velocity
profile continuity (see below). In the following, constraints are derived for geometric
continuity of the derivatives to yield continuity of tangent direction and curvature.
Let P be the active trajectory and Q the updated one, both at the join point of the
trajectories. Instead of planning with exactly continuous first and second derivative,
say Q0 = P 0 and Q00 = P 00 , one can also consider planning with geometric continuity,
i.e., first and second derivatives should point in the same direction, but magnitudes do
not have to be identical (G2 continuity, c.f. Section 3.1). When taking into account a
continuity in curvature (Eq. (3.1)), this results in
Q0 Q00 P 0 P 00
cQ = cP = .
|Q0 |3 |P 0 |3
50
4.3 Updating Plans
Assuming geometric continuity in the first and second derivative, i.e., Q0 = aP 0 ,

Q00 = bP 00 , a, b > 0:
aP 0 bP 00 P 0 P 00
3
=
|aP | 0 |P 0 |3
ab(P P )
0 00
P 0 P 00
=
a3 |P 0 |3 |P 0 |3
b(P P )
0 00
P P 00
0
=
a2 |P 0 |3 |P 0 |3
Now the constraints for the first and second derivative are:
Q0 = aP 00 (4.1)
2
Q =a P .
00 00
(4.2)
If these constraints are met, the two curves P and Q fit smoothly with G2 and curvature
continuity.
4.3.4.3 Velocity Profile Continuity

In addition to continuity regarding the trajectorys shape, continuity in the planned
velocities has to be established as well. There are two situations which require more
attention than simply setting the start velocity of the updated trajectory to the required
value, which are described in the following.
Obstacle proximity Suppose the critical constraint of the active trajectorys velocity
at the join point was obstacle proximity. Furthermore, an error in trajectory execution
now leads to an anticipated join point that is closer to the obstacle. In this situation,
the velocity required at the join point would not be admissible due to obstacle proximity
restrictions.
We resolve this situation by allowing the required velocity to be set at the updated
trajectorys starting point but force the velocity profile to decelerate at maximum rate
until the limit imposed by nearby obstacles is obeyed.
Unfavorable path shape Depending on how sophisticated the method providing the
waypoints is, it might be the case that the waypoints for the new trajectory define
an initial trajectory that cannot be traversed starting with the required velocity. An
example for this situation is a sharp curve that cannot be slowed down for appropriately.
Velocity profile generation is therefore unable to maintain the initially set starting
velocity.
This issue can be treated by using the difference between the required velocity and
the maximum starting velocity as a cost function for the optimization method. The
method will then modify the trajectorys shape to allow a higher starting velocity.
Once it is possible to set the required velocity, the cost function can be switched back
to the normal one.
Note that when treating starting velocity discontinuities with the optimization me-
thod, the discontinuity is mostly resolved by elongating the tangent at the start point
using geometric continuity as presented above. For this specific case it therefore makes
51
sense to not switch to the next optimization dimension as soon as an improvement

was observed but to fully optimize the start tangents magnitude before proceeding
(compare Section 4.2.2).
52
5 Experiments
The previous chapter has already presented a simulation based validation of the op-
timization method and its applicability to the search space. Having found that the
optimization method is suitable and the search space well behaved, in this chapter we
assess the performance of the overall system on real robots.
A number of experiments were conducted to evaluate the performance of our spline
based method including a comparison to a Dynamic Window-based approach.
5.1 Experimental Setup

The robots used for our experiments are Albert, a RWI B21r robot with synchro drive,
and Friedrich, a differential drive Pioneer 2 system, shown in Fig. 5.1. For environment
sensing we rely on laser range finders that both robots are equipped with.
Software The implementation of our approach is integrated into the CARMEN robo-
tics framework [22]. Waypoints for the initial trajectory are supplied by the frameworks
value iteration based path planner [35].
To execute the planned trajectories we rely on a dynamic feedback linearization con-
troller presented by Oriolo et al. [23]. Note that as this controller relies on odometry
readings as error feedback, its performance directly depends on the quality and fre-
quency of odometry readings. It is, like all controllers of its kind, intended for use with
robotic hardware that supplies frequent, reliable odometry readings with accurate time
stamps.
We compare our method to an implementation of the Dynamic Window Approach
(DWA) [8]. For the spline based system used for the experiments, only the first four
waypoints are used, longer paths are cut after the fourth waypoint. The time assigned
to planning and optimizing a trajectory is 0.4 seconds. Furthermore, updating of
trajectories is triggered approximately every second. As mentioned in Section 4.3.3 we
thereby account for goal locations that are beyond the sensors field of view by iterative
replanning.
Experimental situations Experiments have been conducted in two different situations

shown in Fig. 5.2. The maps show a hallway of building 079 of the Department for
Computer Science, University of Freiburg. Three boxes are used as obstacles in two
different configurations. The one in Fig. 5.2a is referred to as normal obstacles, the
situation depicted by Fig. 5.2b is called close obstacles.
The situations have been designed to correspond to a hard setting, where balancing
out travel velocity, path length, and obstacle distance is crucial and difficult. In the
situation with close obstacles, the most narrow part of the map is the passage between
the leftmost box and the one in the middle. When subtracting a safety margin of 5 cm,
within which the collision avoidance immediately stops the robot, the side distance to
53
5 Experiments
(a) Albert (b) Friedrich
Figure 5.1: Robots used for experiments. Synchro drive robot Albert and differential
drive robot Friedrich.
(a) Normal obstacles (b) Close obstacles
Figure 5.2: Experiment situations. Hallway of building 079, University of Freiburg.

Three boxes are used as obstacles. (a) is referred to as situation with
normal obstacles, obstacles in (b) are closer to each other. The robot
starting position is marked in blue, the goal is represented by a yellow dot.
54
5.2 Results
close normal
DWA Splines DWA Splines
avg. traveled distance [m] 6.83 6.62 7.05 6.43
standard deviation [m] 0.13 0.15 0.20 0.18
Table 5.1: Average traveled distance for Albert in situation with close and normal
obstacles. This is data for the runs depicted by Fig. 5.3
the closest obstacle is 5 cm for Albert and 12 cm for Friedrich when driving exactly in
the middle of the passage.
As the approach has been successfully applied to a wider range of situations in
simulations, we chose one of the hardest settings to be evaluated on real robots.
5.2 Results
In the situations introduced above, several runs were executed for the spline based
and the Dynamic Window based approach with both robots, Albert and Friedrich. A
general observation for the spline based approach is that the paths driven by the robots
were smooth and the updating of trajectories was not perceivable by a human observer,
i.e., updated trajectories did fit very smoothly.
More details on the runs will now be presented along with a comparison to the Dy-
namic Window-based approach that has been evaluated on exactly the same situations
as the one based on splines.
Albert With the synchro drive robot Albert, 11 runs were executed for both exper-
imental situations and approaches, each. Fig. 5.3 shows the recorded times of travel.
Each run is represented by a marker in the scatter plots. The left part of the figure
depicts the data for the situation with close obstacles, the right part for the one with
normal obstacles.
In general, time of travel is higher for the situation with close obstacles than for
the other situation, where obstacles are further apart. This is due to the fact that the
robot is required to drive slowly when in proximity of obstacles. In both situations the
median time of travel is lower for the spline based approach, the medians are connected
by a dashed line in both plots. Furthermore, the variance in time of travel is higher
for the Dynamic Window, especially for the situation with close obstacles.
The average distance traveled for the runs presented by Fig. 5.3 is given by Tab. 5.1.
In both situations the spline based approach travels a shorter path than the Dynamic
Window does. Note that this occurs although both approaches are granted the same
minimal distance to obstacles.
We could observe that the actions executed by the spline based method are smoother
than the ones by the Dynamic Window. Support for this observation is given in
Fig. 5.4a. It shows the translational velocity commands generated by both approaches
on the first particular run in the close obstacles situation with Albert. The commands
generated by the spline based method are clearly smoother and lack the heavy oscil-
lation found in the Dynamic Window. For the remaining runs, the plots show similar
55
5 Experiments
time [s]
33
32
time [s]
31 29
30 28
29 27
28 26
27 25
26 24
DWA Splines DWA Splines

(a) Close obstacles (b) Normal obstacles
Figure 5.3: Results for Albert in two situations with differently spaced obstacles (see
Fig. 5.2). Depicted is the total time of travel for 11 runs each. The dashed
lines connect the medians of runs with the Dynamic Window approach and
the proposed spline based method, respectively. With the proposed method
the robot reaches the goal faster and the total time of travel shows lower
variance, especially in the situation with close obstacles (a). Note that the
time axes have been cropped.
56
5.2 Results
velocity
[m/s]
0.3
0.2
0.1
0
0 5 10 15 20 25 30 time [s]
(a) DWA
velocity
[m/s]
0.3
0.2
0.1
0
0 5 10 15 20 25 30 time [s]
(b) Splines
Figure 5.4: Translational velocity commands sent to Albert by (a) the Dynamic Win-
dow approach and (b) the proposed spline based method. The data is
taken from a run for the close obstacles situation (see Fig. 5.2b). Note that
the abrupt decelerations at the goal are caused by the global path planner
which stops the robot when in vicinity of the goal.
57
5 Experiments
time [s]
27
26
25
24
23
DWA Splines
Figure 5.5: Results for Friedrich in the close obstacles situation. The scatter plot shows
the total time of travel for every run (14 for each approach). The dashed
line connects the median of runs with the Dynamic Window approach and
the proposed spline based method, respectively. Note that the time axis
has been cropped.
differences. The abrupt deceleration at the end is due to the global path planner
which stops the robot when it is near the goal. Note that this suppresses the smooth
decelerations that the spline based approach originally plans (see for instance Fig. 3.8).
It has to be noted that the implementation of the Dynamic Window does not employ
any smoothing. Of course this can be done to receive smoother velocity commands.
However, by smoothing the commands one also smoothens the constraints that cause
the abrupt commands, i.e., one loosens the constraints as their fulfillment is delayed
by the command smoothing.
In the situation with close obstacles, the Dynamic Window-based approach using
Albert showed the following behavior: right before passing through the first two boxes
(most narrow part of the path) it stops and turns left and right on the spot before
entering the narrow passage between the two boxes. This can also be seen in Fig. 5.4a,
where translational velocities drop to 0 around the time of 10 seconds. The behavior
is caused by the fact that the Dynamic Window is not looking into the future and can
therefore not easily find and head towards the narrow corridor of admissible velocities
leading through the passage.
Friedrich For the differential drive robot Friedrich, experiments were only conducted
for the situation with close obstacles. The observations match those made for Albert,
the difficulties of the Dynamic Window however were less evident, yet still observable
to a certain degree. This is due to the lower diameter of Friedrich in comparison to
Albert which renders the narrow passage less constrained.
Fig. 5.5 shows the times of travel for the runs conducted in the close obstacles
situation, the corresponding average traveled distances are presented in Tab. 5.2. As
58
5.2 Results
DWA Splines
avg. traveled distance [m] 6.64 6.13
standard deviation [m] 0.07 0.06
Table 5.2: Average traveled distance for Friedrich in situation with close obstacles. This
is data for the runs depicted by Fig. 5.5
DWA
1m Splines
1m
Figure 5.6: CARMEN localization output for the first run of Friedrich in the close
obstacle situation (Fig. 5.2b) for the Dynamic Window (black) and the
spline based approach (red), respectively. Note that the actually driven
path is smoother than the localization output which is subject to errors.
mentioned above, the spline based approach yields lower times of travel together with
shorter overall pathlength. For the particular first runs, Fig. 5.6 shows the output
of the CARMEN localization for the Dynamic Window approach and the spline based
method, respectively. One can observe how the curves taken by the spline based method
are less sharp and the curves are in general less distinct, yielding the reported lower
traveled distance (compare Tab. 5.2). Note that the noise in the shown trajectories is
caused by the localization method, the actually driven paths are smooth.
5.2.1 Unmapped Obstacles

To test how our approach accounts for unmapped obstacles, we conducted several runs
with Friedrich in the close obstacles situation (Fig. 5.2b). While the robot was traveling
from right to left in the map, we confronted it with an unmapped obstacle near the
leftmost of the three boxes.
As Fig. 5.7 shows, the robot successfully evaded the obstacle with a smoothly fitting
updated trajectory. The figure visualizes the effect of replanning in four subsequent
situations. The blue circles stand for the current belief of the CARMEN localization
about the robots position. The currently active trajectory is depicted in green and
the result of the triggered trajectory update is shown in red.
In Fig. 5.7b the robot senses the unmapped obstacle that has not been present at
the previous timestep (Fig. 5.7a). As a result the updated trajectory circumvents it.
The updated trajectory in Fig. 5.7b is shorter than the other trajectories because here
the waypoint planner did provide more densely distributed waypoints. Recall that for
the experiments we limited the number of processed waypoints to four.
59
5 Experiments
(a) (b)
(c) (d)
Figure 5.7: Reaction to an unmapped obstacle. The plots show new (red) and ac-
tive (green) trajectories above the current distance map from (a)(d) in
chronological order. The robot travels from right to left. In (b) it senses an
unmapped obstacle near the leftmost box and plans an evasive trajectory.
The current belief of the CARMEN localization about the robots position
is depicted with a blue circle. The underlying map is the close obstacles
situation (Fig. 5.2b), the data was recorded with Friedrich.
60
5.3 Summary
5.3 Summary
The experiments have shown that our spline based approach is applicable to real robots,
it yields smooth paths with respect to both, velocity and shape. The approach has
been successfully applied to a synchro drive and a differential drive robot. Updating
the trajectory while executing it is possible seamlessly, enabling the method to cope
with unmapped obstacles and accumulating odometry error.
Comparison to a Dynamic Window-based approach shows that paths generated by
the spline based method are shorter and can be traversed faster. At the same time
they respect the very same constraints in kinodynamics as well as regarding obstacle
distance and yet yield a smoother execution behavior of the robot. Furthermore, less
variance is observed in the spline based runs.
However, we belive that the full potential of the approach could not be displayed
by the experiments. As mentioned above, for any controller that relies on odometry
feedback it is crucial that these odometry readings are both frequent and accurate. The
robots at our disposal for the experiments lack this high quality odometry. First of all,
due to system architecture time stamps for the odometry readings do not correspond
to the measurement itself but to points in time that variably deviate up to one reading
cycle. In addition to that, readings are only available at fairly low frequencies (v 10 Hz
for Friedrich, v 20 Hz for Albert). As a last drawback, the quality of the odometry
readings is comparatively low for the used robots.
With improved odometry capabilities we expect our approach to perform even better,
especially with increasing velocities, as the odometry errors impact increases there.
Note that with higher frequency odometry the original Dynamic Window approach
is expected to perform worse as its search window shrinks with increasing execution
frequency.
61
6 Discussion
This chapter comments on how the proposed method can be customized and presents
possible improvements before it finally gives a conclusion.
6.1 Customization
The idea to optimize a parametric trajectory representation with respect to a cost
function is very generic and therefore highly customizable. All parts can be exchanged
and modified to suit a specific need or situation.
When starting from the method proposed here, three main parts emerge for cus-
tomization: cost function, waypoint input, and controller.
In case that travel time is not the only cost measure of interest for a specific applica-
tion, the cost function for the optimization method is easily changed to correspond to
different measures like anticipated energy consumption or overall steering effort. Costs
depending on the curvature integral of the spline are easily computed as curvatures are
already calculated for velocity profile generation in the current approach.
Also the method that computes the input waypoints for the approach is of course
easily exchanged. Instead of using the employed value iteration planner one can think
of using Voronoi graphs as a high level planner, as pursued by [29].
An advantage of the proposed method is that the controller used can be easily
exchanged. This way one can benefit from progress in control theory and adapt the
controller to specific needs. As long as the assumption of independent translational
and rotational velocities and accelerations is a valid approximation (see Section 3.2)
one can also easily port the method to different drive types this way without changing
velocity profile generation.
6.2 Improvements
The proposed method has a lot of potential for further improvement. Apart from im-
plementational issues there are a number of ideas that are able to extend the methods
functionality or considerably improve its performance, which we want to present here.
Spline subdivision Bezier Spline segments have the property that they can be effi-
ciently split into two Bezier segments of the same degree that yield the same curve. A
derivation and description of this subdivision property can be found in [25, Chapt. 3.3].
So far, our method does not take advantage of this property. However, there are two
potential applications for it.
Together with the convex hull property (c.f. Appendix A.3) one can implement col-
lision checking in a hierarchical fashion, applying polygon clipping algorithms to the
62
6.2 Improvements
respective convex hulls that can be iteratively refined to localize a collision. This hier-
archical collision checking entails a lot of speed up potential compared to the currently
employed system of pointwise collision checking along the trajectory.
The subdivision property can also be exploited for re-using previously planned tra-
jectories when planning a new trajectory. The current system always computes a com-
pletely new trajectory starting at the replanning point. Similar to seeding the queue
in A* based algorithms, one can re-use existing segments or with the help of the
subdivision property even parts of them and append new segments. This speeds up
the planning process if already existing trajectory parts are still valid and admissible.
Trajectory updating In the current implementation updating of the trajectory is done

in a relatively nave fashion at a fixed time interval. A more sophisticated way would
consider the current state and take different appropriate actions. It would distinguish
whether the environment drastically changed or the robot is just a bit off the trajectory
due to odometry error. Smart trajectory updating could therefore decide whether to
reuse the existing trajectory and expand the planning horizon or to discard the current
trajectory and plan an evasive maneuver. This would put computational resources to
a more efficient use than repeatedly recalculating most part of the trajectory.
When extending the planning horizon by this method one can also exploit the lo-
calism of Bezier Splines to speed up calculations, as changes to a spline segment only
affect a limited neighborhood.
Time dependent maps As the proposed method generates time parameterized tra-
jectories it can be extended to use a time dependent map to navigate among obstacles
for which movements can be anticipated.
Obstacle distance When computing the velocity limit that nearby obstacles impose
on the robot, the current method does not take into account the robots heading. This
way the robot is also forced to drive at a low speed when moving away from obstacles.
Accounting for the robots heading would remove this unnecessary constraint. Fur-
thermore, the system can be extended to account for the robots actual shape instead
of approximating it by a circle.
Spline representation In the current approach, splines are subject to discretization

by numerical integration and the planning points. Therefore, also curvature extrema
are subject to discretization. These and other extrema can be computed analytically, if
splines are represented as done by Connors and Elkaim [3]. They construct a separate
coordinate reference frame for each segment and restrict the shape of a segment to a
polynomial of the form R R. This can be exploited to minimize discretization effects
by placing planning points at curvature extrema.
Furthermore, integration of polynomials that have a one dimensional range and do-
main can be done analytically to avoid discretization errors that accompany numerical
integration.
63
6 Discussion
6.3 Conclusion
We have proposed a method for wheeled mobile robot motion planning that features a
parametric representation of the planned actions and thereby overcomes the limitations
imposed by finite action sets, which a majority of the current work has to cope with.
The suggested approach plans curvature continuous paths in a time parametric fash-
ion that allows prediction of the robots position and kinematic state for any given
point in time along the trajectory. Therefore, a variety of controllers can be used to
execute the trajectory, which allows for precise navigation through a small odometry
feedback loop. Furthermore, optimization is employed to receive trajectories that are
near optimal with respect to a cost function such as the time of travel.
The input to the proposed method is constituted by a map and a set of sparse way-
points out of which an admissible trajectory is created instantly (anytime capability).
The initial trajectory is then subject to optimization in a continuous parameter space
for the remainder of the designated planning time. After optimization the time para-
metric trajectory together with its first two derivatives are forwarded to a controller
for execution.
In contrast to Cubic Bezier Splines the Quintic variant is able to provide the desired
properties for a trajectory representation, which are localism, curvature continuity,
and a strong correlation between control points and shape. The additional degrees of
freedom in form of second derivatives at start and end point are handled by a heuristic
that mimics the minimizing behavior of Cubic Bezier Splines. Furthermore, Bezier
Splines provide the subdivision property which is of use for future extensions of the
method.
We have shown how under certain assumptions the fastest constraint respecting
velocity profile can be computed for a trajectory without resorting to iteration to resolve
interdependencies between translational and rotational acceleration constraints.
The RPROP based optimization of trajectories worked well in both simulation and
experiments on real robots. It not only shortens the length of the planned trajectory
but also permits faster traversal of the latter by deforming it to reduce the constraints
impact as has been seen in Fig. 3.8. This has positive effects on the smoothness of
driving behavior.
Our experiments have shown the potential of the proposed method and its real
time applicability to robots: the approach works on both synchro and differential
drive robots and is able to smoothly update plans to avoid unmapped obstacles. In
comparison, the spline based method yields faster and smoother trajectories than a
Dynamic Window-based approach. Part of the advantage is due to the lookahead
our method performs, but even if a lookahead mechanism was added to the Dynamic
Window, it would still suffer from the negative effects of a finite action set.
As a first step to a more precise navigation in constrained spaces the majority of
current work has decoupled trajectory generation from execution. While we follow this
step, we also believe that a necessary next step is to abandon finite action sets.
Finite action sets and their optimal discretization receive substantial attention in
current state of the art work. A lot of effort is put into keeping the size of the action
set in bounds, i.e., keeping the resulting search tree tractable, and at the same time
spanning the relevant action space.
Our work is able to overcome the limitations of discretization by the choice of a
parametric trajectory representation instead of the predominant finite action sets.
64
6.3 Conclusion
Furthermore, we argue that the employment of an optimization method that only

provides near optimal results is not to be considered a major drawback regarding opti-
mality compared to existing approaches. Firstly, existing A* based search approaches
on 2D gridmaps have to abandon their optimality claims when their solutions are taken
to the real, usually higher dimensional and continuous problem domain that involves
kinematics and dynamics. Secondly, a strong discretization of the action space and
the use of approximate or heuristic costs challenge optimality claims with respect to
the original problem, although justified in the discrete subproblem regarding the used
costs.
We therefore recommend the presented application of an optimization in a continuous
parameter space with respect to the costs of interest. This is a goal oriented approach
to trajectory generation when exhaustive search is prohibitive and heuristics on their
own do not yield sufficient results. In addition to that, explicit planning for velocities
along the trajectory is a prerequisite for consideration of kinodynamics along the path.
The use of a trajectory that defines position, velocity, and acceleration over time
not only facilitates the use of state of the art controllers, it also opens the door to the
domain of planning with time dependent maps. Solutions are within reach that can
efficiently avoid obstacles that follow trajectories which are known to a certain degree.
65
List of Figures
3.1 Basis splines for Quintic Bezier Spline segment . . . . . . . . . . . . . . 16

3.2 Quintic Bezier Spline segment . . . . . . . . . . . . . . . . . . . . . . . . 17
3.3 Planning points for the velocity profile . . . . . . . . . . . . . . . . . . . 18
3.4 Velocity profile generation . . . . . . . . . . . . . . . . . . . . . . . . . . 20
3.5 Parabolas in vi defined by rotational acceleration constraints . . . . . . 26
3.6 Overlap of accelerational constraint intervals . . . . . . . . . . . . . . . 29
3.7 Overlap problem for accelerational constraints . . . . . . . . . . . . . . . 30
3.8 Velocity profiles and isolated constraints . . . . . . . . . . . . . . . . . . 31
4.1 System overview . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33

4.2 Tangent heuristic . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33
4.3 Second derivative heuristic . . . . . . . . . . . . . . . . . . . . . . . . . . 34
4.4 Initial trajectory generation . . . . . . . . . . . . . . . . . . . . . . . . . 36
4.5 Initial and optimized trajectory . . . . . . . . . . . . . . . . . . . . . . . 37
4.6 Optimization method pseudo code . . . . . . . . . . . . . . . . . . . . . 39
4.7 Scenes for search space inspection . . . . . . . . . . . . . . . . . . . . . . 40
4.8 Search space: tangent elongation . . . . . . . . . . . . . . . . . . . . . . 42
4.9 Search space: waypoint translation . . . . . . . . . . . . . . . . . . . . . 43
4.10 Optimization process . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 45
4.11 Search space: tangent rotation . . . . . . . . . . . . . . . . . . . . . . . 46
4.12 Odometry error and plan updating . . . . . . . . . . . . . . . . . . . . . 48
5.1 Robots used for experiments . . . . . . . . . . . . . . . . . . . . . . . . . 54

5.2 Experiment situations . . . . . . . . . . . . . . . . . . . . . . . . . . . . 54
5.3 Results for Albert . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 56
5.4 Translational velocity commands . . . . . . . . . . . . . . . . . . . . . . 57
5.5 Results for Friedrich . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 58
5.6 Driven paths for Friedrich . . . . . . . . . . . . . . . . . . . . . . . . . . 59
5.7 Reaction to unmapped obstacle . . . . . . . . . . . . . . . . . . . . . . . 60
A.1 Basis splines for Cubic Bezier Spline segment . . . . . . . . . . . . . . . 74

A.2 Cubic Bezier Spline . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 75
A.3 B-Spline . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 78
66
Bibliography
[1] K. O. Arras, R. Philippsen, N. Tomatis, M. de Battista, M. Schilt, and R. Siegwart.
A navigation framework for multiple mobile robots and its application at the
Expo.02 exhibition. In Proc. IEEE International Conference on Robotics and
Automation (ICRA03), Taipei, Taiwan, 2003.
[2] R. H. Bartels, J. C. Beatty, and B. A. Barsky. An introduction to splines for use

in computer graphics & geometric modeling. Morgan Kaufmann Publishers Inc.,
San Francisco, CA, USA, 1987.
[3] J. Connors and G. Elkaim. Analysis of a spline based, obstacle avoiding path plan-
ning algorithm. In Proc. IEEE 65th Vehicular Technology Conference, VTC2007-
Spring, pages 25652569, 2225 April 2007.
[4] J. Connors and G. Elkaim. Manipulating B-Spline based paths for obstacle avoid-
ance in autonomous ground vehicles. In Proc. of the 2007 National Technical
Meeting of the Institute of Navigation, pages 10811088, San Diego, California,
2007.
[5] R. O. Duda and P. E. Hart. Pattern Classification and Scene Analysis. John
Wiley & Sons, New York, 1973.
[6] J. D. Foley, A. van Dam, S. K. Feiner, and J. F. Hughes. Computer graphics:

principles and practice (2nd ed.). Addison-Wesley Longman Publishing Co., Inc.,
Boston, MA, USA, 1990.
[7] J. D. Foley, R. L. Phillips, J. F. Hughes, A. van Dam, and S. K. Feiner. Introduction

to Computer Graphics. Addison-Wesley Longman Publishing Co., Inc., Boston,
MA, USA, 1994.
[8] D. Fox, W. Burgard, and S. Thrun. The dynamic window approach to collision
avoidance. IEEE Robotics & Automation Magazine, 4(1):2333, March 1997.
[9] H. Gonz alez-Ba

nos, D. Hsu, and J. Latombe. Motion planning: Recent devel-
opments. In S. Ge and F. Lewis, editors, Autonomous Mobile Robots: Sensing,
Control, Decision-Making and Applications, chapter 10. CRC Press, 2006.
[10] S. Gulati and B. Kuipers. High performance control for graceful motion of an
intelligent wheelchair. In Proc. IEEE International Conference on Robotics and
Automation ICRA 2008, pages 39323938, 1923 May 2008.
[11] J.-H. Hwang, R. C. Arkin, and D.-S. Kwon. Mobile robots at your fingertip:
Bezier curve on-line trajectory generation for supervisory control. In Proc. of
the IEEE/RSJ International Conference on Intelligent Robots and Systems, pages
14441449, 2003.
67
Bibliography
[12] O. Khatib. Real-time obstacle avoidance for manipulators and mobile robots.
International Journal of Robotics Research, 5(1):9098, 1986.
[13] I. Kolmanovsky and N. McClamroch. Developments in nonholonomic control prob-

lems. IEEE Control Systems, 15(6):2036, Dec. 1995.
[14] Y. Koren and J. Borenstein. Potential field methods and their inherent limitations
for mobile robot navigation. In IEEE International Conference on Robotics and
Automation, pages 13981404, 1991.
[15] J.-C. Latombe. Robot Motion Planning. Kluwer Academic Publishers, Norwell,
MA, USA, 1991.
[16] M. Likhachev and D. Ferguson. Planning long dynamically-feasible maneuvers

for autonomous vehicles. In Proc. of Robotics: Science and Systems IV, Zurich,
Switzerland, June 2008.
[17] J. Loustau and M. Dillon. Linear Geometry with Computer Graphics. CRC Press,
1992.
[18] A. Luca and G. Oriolo. Kinematics and Dynamics of Multi-Body Systems, chap-
ter Modelling and control of nonholonomic mechanical systems. Springer-Verlag,
Wien, 1995.
[19] K. Macek, A. Vasquez, T. Fraichard, and R. Siegwart. A hierarchical approach to

safe vehicle navigation in dynamic urban scenarios. In 10th International Confer-
ence on Control, Automation, Robotics and Vision (ICARCV), 2008. To appear.
[20] E. Magid, D. Keren, E. Rivlin, and I. Yavneh. Spline-based robot navigation.

In Proc. IEEE/RSJ International Conference on Intelligent Robots and Systems,
pages 22962301, Oct. 2006.
[21] C. Mandel and U. Frese. Comparison of wheelchair user interfaces for the paral-
ysed: Head-joystick vs. verbal path selection from an offered route-set. In Proc.
of the 3rd European Conference on Mobile Robotics (ECMR), 2007.
[22] M. Montemerlo, N. Roy, and S. Thrun. Perspectives on standardization in mo-

bile robot programming: the Carnegie Mellon Navigation (CARMEN) Toolkit.
In Proc. IEEE/RSJ International Conference on Intelligent Robots and Systems
(IROS 2003), volume 3, pages 24362441, 2731 Oct. 2003.
[23] G. Oriolo, A. De Luca, and M. Vendittelli. WMR control via dynamic feedback
linearization: design, implementation, and experimental validation. IEEE Trans-
actions on Control Systems Technology, 10(6):835852, Nov. 2002.
[24] R. Plato. Concise Numerical Mathematics, volume 57 of Graduate studies in

mathematics. American Mathematical Society, 2003.
[25] H. Prautzsch, W. Boehm, and M. Paluszny. Bezier and B-Spline techniques.

Springer, 2002.
68
Bibliography
[26] S. Quinlan and O. Khatib. Elastic bands: Connecting path planning and control.
In Proc. of the International Conference on Robotics and Automation, pages 802
807, 1993.
[27] M. Riedmiller. Rprop description and implementation details. Technical report,

University of Karlsruhe, January 1994.
[28] M. Riedmiller and H. Braun. A direct adaptive method for faster backpropagation
learning: The RPROP algorithm. In Proc. of the IEEE International Conference
on Neural Networks, pages 586591, San Francisco, CA, 1993.
[29] A. Sahraei, M. T. Manzuri, M. R. Razvan, M. Tajfard, and S. Khoshbakht. AI*IA

2007: Artificial Intelligence and Human-Oriented Computing, chapter Real-Time
Trajectory Generation for Mobile Robots, pages 459470. Springer, 2007.
[30] L. Sciavicco and B. Siciliano. Modelling and Control of Robot Manipulators. Ad-
vanced textbooks in control and signal processing. Springer, 2nd edition, January
2005.
[31] Z. Shiller and Y. Gwo. Dynamic motion planning of autonomous vehicles. IEEE
Transactions on Robotics and Automation, 7:241249.
[32] C. Stachniss and W. Burgard. An integrated approach to goal-directed obsta-

cle avoidance under dynamic constraints for dynamic environments. In Proc.
IEEE/RSJ International Conference on Intelligent Robots and System, volume 1,
pages 508513, 30 Sept.5 Oct. 2002.
[33] J. Stewart. Calculus: Early Transcendentals. Brooks Cole, Pacific Grove, 5th
edition, 2002.
[34] S. Thompson and S. Kagami. Continuous curvature trajectory generation with

obstacle avoidance for car-like robots. In Proc. International Conference on Com-
putational Intelligence for Modelling, Control and Automation and International
Conference on Intelligent Agents, Web Technologies and Internet Commerce, vol-
ume 1, pages 863870, 2830 Nov. 2005.
[35] S. Thrun and A. Bucken. Learning maps for indoor mobile robot navigation.
Artificial Intelligence, 99:2171, 1998.
[36] S. Thrun, M. Montemerlo, H. Dahlkamp, D. Stavens, A. Aron, J. Diebel, P. Fong,

J. Gale, M. Halpenny, G. Hoffmann, K. Lau, C. Oakley, M. Palatucci, V. Pratt,
P. Stang, S. Strohband, C. Dupont, L.-E. Jendrossek, C. Koelen, C. Markey,
C. Rummel, J. van Niekerk, E. Jensen, P. Alessandrini, G. Bradski, B. Davies,
S. Ettinger, A. Kaehler, A. Nefian, and P. Mahoney. Stanley: The robot that won
the DARPA Grand Challenge. Journal of Field Robotics, 23(9):661692, June
2006.
[37] K. I. Tsianos, I. A. Sucan, and L. E. Kavraki. Sampling-based robot motion plan-

ning: Towards realistic applications. Computer Science Review, 1:211, August
2007.
69
Bibliography
[38] A. H. Watt and M. Watt. Advanced Animation and Rendering Techniques: Theory
and Practice. Addison-Wesley Publishing Company, Inc., New York, 1992.
[39] J. Ziegler, M. Werling, and J. Schroder. Navigating car-like robots in unstruc-

tured environments using an obstacle sensitive cost function. In IEEE Intelligent
Vehicles Symposium (IV 08), 2008.
70
A Splines
This chapter introduces splines as used throughout this work and presents several cubic
spline families along with a discussion of their key properties.
A.1 Introduction
We introduce a spline as a piecewise polynomial parametric curve Q(t). In general
splines can exist in other spaces as well, for this application however, we restrict them
to live in a two-dimensional plane: Q(t) R2 . Note that nevertheless most of the
following is true in arbitrary spaces. Typically one considers splines whose segments
are polynomials of identical degree n.
There are three commonly used ways in the literature to define the polynomial
segments of a spline. We will briefly introduce them with the example of a cubic
segment with domain R2 .
Polynomial in parameter t The most straightforward way is of course the standard

notation for a polynomial. The segment S(t) can be expressed by individual cubic
polynomials for each dimension, as the following equation shows:

x(t) ax t3 + bx t2 + cx t + dx
S(t) = =
y(t) ay t3 + by t2 + cy t + dy

ax ay (A.1)
bx by
= t3 t2 t 1 cx cy
=T C
dx dy
Eq. (A.1) defines the so called coefficient matrix C which serves as the characterization
of a specific cubic curve in this notation. Furthermore, it introduces the abbreviation
T for the vector containing the parameters powers. Note that in the literature also a
reversed variant of the parameter vector is used, e.g., [2]. We adopt the notation used
in [6, 7], and [38, Chapt. 3.5.2].
Weighted sum of control vertices S(t) can also be expressed as a weighted sum of
control vertices pi R2 , for cubic polynomials there are four of them:
3
X
S(t) = pi Bi (t) (A.2)
i=0
This notation allows the interpretation of the cubic polynomial as a linear combination
of the control vertices pi R2 , weighted by the so called basis splines Bi (t), which are
cubic polynomials. A particular segment is here characterized by its control vertices
71
A Splines
whereas the basis splines can be interpreted as defining the family of segments it belongs
to.
Product of matrices Combining the ideas of the precedent two notations one can
also express a segment through the product of three matrices and vectors, respectively:
S(t) = T M G (A.3)
T is the vector containing the parameters potencies as introduced in Eq. (A.1). Further
comparison with the definition in Eq. (A.1) shows that the coefficient matrix C has
been split into a basis matrix M and a geometry matrix G by C = M G. Defined as

m00 m01 m02 m03 G0
m10 m11 m12 m13
M = , G = G1 (A.4)
m20 m21 m22 m23 G2
m30 m31 m32 m33 G3
the geometry matrix characterizes the segment by four control vertices {Gi | i 0 . . . 3}
whereas the basis matrix can be regarded to define the family the curve belongs to.
Having a look at Eq. (A.2)
again, one realizes that the product of the parameter vector
T = t3 t2 t 1 with the basis matrix M is identical with a vector containing
the basis splines from Eq. (A.2)
t
m00 t3 + m10 t2 + m20 t + m30
m01 t3 + m11 t2 + m21 t + m31
T M =
m02 t3 + m12 t2 + m22 t + m32 ,
m03 t3 + m13 t2 + m23 t + m33
when identifying control vertices pi with Gi . At denotes the transposed of A.

We will now introduce some well known spline types for later discussion about their
suitability for trajectory planning. Note that a huge domain of theory that uses splines
for interpolation is of no use for us, as it prescribes the location of the sampling points.
In this application however, we deal with sparse sampling points at fixed locations.
For the following splines the parameter t lies in [0, 1] for each segment S(t). We do
not introduce a unified notation for mapping the global spline parameter t in Q(t) to
the segment parameters in order to perform segment selection at this point.
A.2 Cubic Hermite Spline

Intuition A Cubic Hermite Spline consists of segments that are cubic polynomials.
Each segment connects two control points. The two remaining degrees of freedom are
controlled by specifying the tangent vectors at both, the start and the end point of the
segment. At join points, the tangent vectors are manually set to match, resulting in a
C 1 continuous spline.
Definition The following definitions are derived in [6, Sect. 11.2.1]. Note that, de-
viating from conventions in mathematics, we use 0 as the first index to comply with
72
A.2 Cubic Hermite Spline
notation used in Computer Science. The Hermite geometry matrix is defined as

P0
P3
GH =
R0 , (A.5)
R3
where P0 is the start point of a segment and P3 specifies its end point. R0 and R3 are the
tangent vectors at the start and the end point, respectively. We follow the somewhat
unintuitive notation of [6] for the same reason they introduced it: consistency with
notation that will be introduced later on. The Hermite basis matrix evaluates to

2 2 1 1
3 3 2 1
MH = 0
. (A.6)
0 1 0
1 0 0 0
Now, with Eq. (A.3), the Hermite form segment can be noted as
S(t) = T MH GH . (A.7)
This equation can easily be transformed into the corresponding instance of Eq. (A.2):
S(t) = (2t3 3t2 + 1)P0 + (2t3 + 3t2 )P3 + (t3 2t2 + t)R0 + (t3 t2 )R3 , (A.8)
which yields the vector of the Hermite basis splines

t
2t3 3t2 + 1
2t3 + 3t2
BH =
t3 2t2 + t . (A.9)
t3 t2
To get the polynomial formulation of our segment, we compute the coefficient matrix
CH = MH GH and proceed according to Eq. (A.1). We get:

2P0 2P3 + R0 + R3
3P0 + 3P3 2R0 R3
CH =

(A.10)
R0
P0
and therefore
S(t) = (2P0 2P3 + R0 + R3 )t3 + (3P0 + 3P3 2R0 R3 )t2 + R0 t + P0 . (A.11)
The drawback of the Hermite form for the splines segments is that the tangent
specification by vectors somewhat differs from the specification of the start and end
points by position vectors. Furthermore the relation of the tangents directions and
magnitudes to the segments shape is not very obvious. These considerations lead to
the definition of Cubic Bezier Splines.
73
A Splines
BB,0 BB,3
4 BB,1 BB,2
9
0 1 2
3 3 1
Figure A.1: The basis splines of a Cubic Bezier Splines segment, also known as the
Bernstein Polynomials of third degree. For a definition see Eq. (A.16).
A.3 Cubic B
ezier Splines
Intuition Cubic Bezier Splines also consist of cubic polynomials and therefore have the
same expressive power as the Cubic Hermite Splines. They differ from Hermite Splines
in the formulation of the segments. Instead of explicitly defining the tangent vectors
at start and end point, these vectors are extracted from two additional control vertices
in the case of Cubic Bezier Splines segments. C 1 continuity can be reached for Cubic
Bezier Splines by arranging the control vertices for the tangents in an appropriate way.
Definition The definitions stated here can be found in [6, Chapt. 11.2.2] together
with a derivation. Again we adapt indexing to Computer Sciences conventions. The
Bezier geometry matrix is
P0
P1
GB = P2
(A.12)
P3
and the Bezier basis matrix is

1 3 3 1
3 6 3 0
MB =
3
. (A.13)
3 0 0
1 0 0 0
Following Eq. (A.3) we get the Bezier segment defined by the four control vertices in
GB as
S(t) = T MB GB . (A.14)
Further transformation yields a formulation with the help of the Bezier basis splines
t t t
BB,0 t3 + 3t2 3t + 1 (1 t)3
BB,1 3t3 6t2 + 3t t)2
BB = = = 3t(1
BB,2 3t3 + 3t2 3t (1 t) ,
2 (A.15)
BB,3 t3 t3
74
A.3 Cubic Bezier Splines
P2 P3 P4
P9
S0 S1 P5
P1
S2
P6 P8
P0
P7
Figure A.2: Cubic Bezier Spline consisting of three segments S0 , S1 , and S2 . The
tangents at the join points are depicted in red with half their magnitude
and the control points have been arranged for C 1 continuity. The grey
areas show the convex hull of the respective segments control points and
thereby the convex hull property of a Cubic Bezier Spline.
which are also known as the Bernstein polynomials of third degree:
S(t) = (1 t)3 P0 + 3t(1 t)2 P1 + 3t2 (1 t)P2 + t3 P3 . (A.16)
They have the property to always sum up to 1 and to never leave the interval [0, 1]:
3
X
BB,i = 1, BB,i [0, 1], i = 0 . . . 3. (A.17)
i=0
As a consequence the linear combination of the control vertices is an interpolation of

the control vertices, a property which is referred to as convex hull property. Fig. A.1
provides a visualization of the basis splines. Calculating the Bezier coefficient matrix
from MB and GB ,
P0 + 3P1 3P2 + P3
3P0 6P1 + 3P2
CB =
,
(A.18)
3P0 + 3P1
P0
enables us to state the instantiation of Eq. (A.1) for a Cubic Bezier segment, the
formulation as polynomial in parameter t:
S(t) = (P0 + 3P1 3P2 + P3 )t3 + (3P0 6P1 + 3P2 )t2 + (3P0 + 3P1 )t + P0 . (A.19)
Derivatives Eq. (A.19) makes it easy to state the first and second derivatives of a
Cubic Bezier Segment:
S 0 (t) = (3P0 + 9P1 9P2 + 3P3 )t2 + (6P0 12P1 + 6P2 )t 3P0 + 3P1 (A.20)
S (t) = (6P0 + 18P1 18P2 + 6P3 )t + 6P0 12P1 + 6P2
00
(A.21)
75
A Splines
For the start and end point of a segment, these evaluate to

S 0 (0) = 3(P1 P0 ), S 0 (1) = 3(P3 P2 ), (A.22)
S (0) = 6(P0 2P1 + P2 ),
00
S 00 (1) = 6(P1 2P2 + P3 ). (A.23)
These equations clarify the relation between the inner control points of a segment
and its derivatives at the start and end point, respectively. Fig. A.2 shows Cubic
Bezier Spline with three segments. The light gray polygons visualize the convex hull
property of the segments: as the Bezier basis splines compute a weighted average of
the control vertices, each point of the segment is contained in the convex hull of its
control vertices, i.e., the polygon created by them. For the spline in the figure, the
segments inner control points have been arranged to yield C 1 continuity.
This is achieved by establishing
Si0 (1) = Si+1
0
(0)
3(P3i+3 P3i+2 ) = 3(P3(i+1)+1 P3(i+1) )
P3i+3 P3i+2 = P3i+4 P3i+3 .
A.4 Catmull-Rom Splines

So far, to reach C 1 continuity it has been necessary to manually adjust the tangents at
the join points. Catmull-Rom Splines overcome this by being inherently C 1 continuous.
We will introduce Catmull-Rom Splines according to [6, Chapt. 11.2.6], [38, Chapt.
3.10]. Note that [2] uses the term to refer to a more generalized family of splines
that interpolate vector valued functions instead of control vertices. The splines named
Catmull-Rom Splines here and in [6] are a member of this family, see [2, Chapt. 21.4]
for a definition of this family and [2, p. 427] for a remark on the membership relation.
Intuition A Catmull-Rom Spline is defined by a sequence of control points P0 , . . . , Pm .

It passes through the control points P1 , . . . , Pm1 and the tangent vector at point Pi
is parallel to the vector Pi1 Pi+1 . Catmull-Rom Splines are designed for inherent C 1
continuity and are composed of segments that are cubic polynomials. To reach C 1
continuity, control points are shared between segments to a certain degree.
The curve segment Si (t) connects the control points Pi2 and Pi1 via Si (0) =
Pi2 and Si (1) = Pi1 . However, it is defined by the control points Pi3 , . . . , Pi , the
additional points are used to determine the tangent at Pi2 and Pi1 , respectively. As
a consequence, the very first and last control point of a sequence are not connected to
the curve, it reaches from P1 to Pm1 .
A.4.0.1 Definition
The i-th segment (i [3, m]) of a Catmull-Rom Spline defined by control points
P0 , . . . , Pm is given by
Si (t) = T MCR GiCR

1 3 3 1 Pi3
1 2 5 4 1 Pi2 (A.24)
= t3 t2 t 1 .
2 1 0 1 0 Pi1
0 2 0 0 Pi
76
A.5 B-Splines
The vector containing the basis splines can be derived as

t
t3 + 2t2 t
1 3t3 5t2 + 2
BCR = . (A.25)
2 3t3 + 4t2 + t
t3 t2
We realize that although the basis splines sum up to 1, they do exceed the interval
[0, 1] and therefore Catmull-Rom Splines do not possess the convex hull property.
Calculating the coefficient matrix

Pi3 + 3Pi2 3Pi1 + Pi
1 2Pi3 5Pi2 + 4Pi1 Pi
i
CCR = MCR GiCR = (A.26)
2 Pi3 + Pi1
2Pi2
allows us to write Equation (A.24) as a polynomial:
1
Si (t) = (Pi3 + 3Pi2 3Pi1 + Pi ) t3
2
1
+ (2Pi3 5Pi2 + 4Pi1 Pi ) t2 (A.27)
2
1
+ (Pi3 + Pi1 ) t + Pi2 .
2
Derivatives Now, by inserting 0 for the start and 1 for the end point, respectively,
we obtain
Si (0) = Pi2 Si (1) = Pi1 (A.28)
1 1
Si0 (0) = (Pi1 Pi3 ) Si0 (1) = (Pi Pi2 ) (A.29)
2 2
and thereby confirm the C 1 continuity, as Si1 (1) = Si (0) and Si1
0
(1) = Si0 (0). Fur-
thermore, we see that the tangent at point Pi is parallel to the vector Pi1 Pi+1 . Cal-
culating the second derivative for a segment of a Catmull-Rom Spline will reveal that
they are generally not C 2 continuous.
Catmull-Rom Splines guarantee C 1 continuity at the cost of strictly prescribing
tangent direction and magnitude at every joint point.
A.5 B-Splines
For the above presented spline classes we were unable to reach our initial goal of C 2
continuity. B-Splines inherently possess this property. However, this comes at the
additional cost of not passing through any of their control points.
Intuition Like Catmull-Rom Splines, to ensure continuity constraints at the joint

points, a B-Splines segments have to share some of their control points. A B-Spline is
defined by its control points P0 , . . . , Pm . Each segment is a cubic polynomial, the i-th
segment (i [0, m 3]) is defined by the control points Pi , . . . , Pi+3 . The following def-
inition can be found in [38, Chapt. 3.5]. A remark that addresses the transformability
of B-Splines and Bezier curves can be found in [6, p. 510].
77
A Splines
P3
S1 S2
P4
P2
S3
S0 P5 P9
P1
S4
P6 P8
P0 S5
P7 S6
Figure A.3: B-Spline consisting of seven segments S0 , . . . , S6 . The overlapping convex

hulls of the segments control points are depicted in gray, the one of segment
S2 (control points P2 , . . . , P5 ) is highlighted in red. It is apparent that a
B-Spline in general does not pass through any of its control points.
Definition The i-th segment (i [0, m 3]) of a B-Spline defined by control points
P0 , . . . , Pm is given by
Si (t) = T MBS GiBS

1 3 3 1 Pi
1 3 6 3 0 Pi+1 (A.30)
= t3 t2 t 1 .
6 3 0 3 0 Pi+2
1 4 1 0 Pi+3
We calculate the vector containing the basis splines

t t
BBS,0 t3 + 3t2 3t + 1
BBS,1 1 3t3 6t2 + 4
BBS =
BBS,2 = 6 3t3 + 3t2 + 3t + 1
,
(A.31)
BBS,3 t3
P
3
where BBS,i = 1, BBS,i [0, 1], i = 0, . . . , 3, which is the sufficient condition
i=0
for the convex hull property that was introduced with the Cubic Bezier Splines in
Appendix A.3. Fig. A.3 shows an instance of a B-Spline with seven segments. The
convex hulls of the segments overlap, the one of segment S2 (corresponds to control
points P2 , . . . , P5 ) is highlighted in the figure.
We continue with the calculation of the coefficient matrix

Pi + 3Pi+1 3Pi+2 + Pi+3
3Pi 6Pi+1 + 3Pi+2
i
CBS = MBS GiBS =
,
(A.32)
3Pi + 3Pi+2
Pi + 4Pi+1 + Pi+2
78
A.6 Summary
which leads to the polynomial formulation:

1 1
Si (t) = (Pi + 3Pi+1 3Pi+2 + Pi+3 ) t3 + (3Pi 6Pi+1 + 3Pi+2 ) t2
6 6 (A.33)
1 1
+ (3Pi + 3Pi+2 ) t + (Pi + 4Pi+1 + Pi+2 ) .
6 6
Derivation is now straightforward and confirms the C 2 continuity. The fact that
B-Splines do not pass through any of their control points (compare Fig. A.3) can be
revealed by a closer look at the basis splines: none of them ever reaches the value
1, any point on a segment therefore contains information from more than one control
point. However, there is a way to enforce passing through control points for a B-
Spline: repeated insertion of a control point will lead the spline through this point.
The drawback of this method is a resulting discontinuity in the derivatives.
While B-Splines have favorable properties such as the desired C 2 continuity and
even the convex hull property, not passing through control points decreases the degree
of intuitive correlation between control points and spline shape. We consider this an
exclusion criterion.
A.6 Summary
The properties of the cubic spline families presented in this chapter and the ones of
Quintic Bezier Splines, which are introduced in Section 3.1.2.3, are summarized by the
following table:
Cubic Quintic
om
l-R
y
e
ite
rt
lin
ul
er
r
e
m
m
ie
p
op
at
ez
-S
ez
er
B
Pr
stitching manual manual inherent inherent manual

C 1 continuity X X X X X
C 2 continuity - - - X X
visit control points X X X - X
strong correlation - X X - X
localism X X X X X
freely set 1st deriv. start X X X X X
freely set 2nd deriv. start - - - - X
convex hull - X - X X
Only Quintic Bezier Splines can provide all properties as they are constructed from
quintic polynomials, whereas the remaining presented spline families resort to cubic
polynomials.
79
B Solving the Overlap Problem
During velocity profile generation, compliance of the planned velocities with translational and rotational
acceleration constraints has to be ensured. Due to a combination of curvature change and translational
velocity planned for an adjacent planning point, situations can arise where no velocity exists for a planning
point that respects both constraints (Section 3.2.7). Suppose for instance a car approaching a curve at a
high speed: As descibed in Section 3.2.7, even if skidding is not a problem, it would either have to decelerate
quickly or to abruptly change its steering angle in order to stay on the road. If neither is feasible according
to the limiting constraints of the car, it will not be able to follow the curve.
More formally, the intervals [vmin|at , vmax |at ] and Ia , defined by Eqs. (3.29), (3.30), and (3.39), do not
overlap in general. This chapter provides a solution to the problem: it derives the biggest upper bound for
vi1 that guarantees a non-empty intersection for all velocities smaller than this bound. To ensure that
admissible velocity intervals overlap for backward consistency, too, the same bound can be applied in a
reverse fashion, assuming motion from pi to pi1 .
The computation of the bound derived in the following section is summarized by a pseudo code algorithm
presented in Appendix B.2. Once the bound has been computed, it can be seamlessly integrated into velocity
profile generation as an additional isolated constraint.
B.1 Derivation of the Bound

In this section the biggest upper bound for vi1 is derived that guarantees Ia [vmin|at , vmax |at ] 6= .
There are many different possible situations regarding the curvature change between two planning points:
curvature can increase or decrease, the curve can be oriented to the left or the right, or it can even change
orientation from left to right or vice versa. Due to the occurence of square roots and quadratic expressions
in the definitions of Ia ,vmin|at , and vmax |at (Eqs. (3.29), (3.30), and (3.39) and in consequence Eqs. (3.32),
(3.33), and (3.37)), it is important to know the signs of terms during the derivations to conduct them
in a mathematically correct way. We therefore have to resort to excessive case differentiation, especially
regarding curvature orientation and sign of its change ci ci1 between planning points.
In the following case differentiation positive values for curvatures ci1 and ci correspond to a left curve,
negative ones yield a right curve. The cases where ci > ci1 correspond to an increasing curvature for left
curves, for right curves they correspond to a decreasing curvature. For ci < ci1 these correspondences are
swapped. A curvature of zero corresponds to strictly straight movement, cases where the signs of ci1 and
ci differ correspond to transition from a right to a left curve or vice versa.
B.1.1 Case 1, ci > 0, ci1 0

B.1.1.1 Case 1.1, ci > ci1
We will first show that v2 < 0, and v2 exists v2 < 0 which makes both values irrelevant for intersection
as vmin|at 0.
From the case definition it follows that ci1 ci < 0. Looking at the definitions of v2 and v2 in Equations
Eq. (3.32), Eq. (3.33) respectively, the claim immediately follows.
80
The interval that remains to be intersected with [vmin|at , vmax |at ] is now [v1 , v1 ] in case v1 does exist and
[0, v1 ] in case it doesnt.
We show that v1 < vmax |at : since vmax |at > 0 (Eq. (3.30)), this is clear for v1 < 0. We can therefore
assume v1 0 and obtain:
v1 < vmax |at
q
1 2
p
(ci1 ci ) vi1 + (ci + ci1 ) vi1 8ci s amax < vi1 2 + 2atmax s .
2
2ci
As both sides are greater or equal zero due vmax |at > 0 and the assumption above, they can be squared:

(ci1 ci )2 vi1 2 + 2(ci1 ci )vi1 . . . + (ci + ci1 )2 vi1 2 8ci s amax < 4ci 2 vi1 2 + 8ci 2 atmax s

2 ci1 2 ci 2 vi1 2 8ci s (amax + ci atmax ) < 2 (ci1 ci ) vi1 . . ..
The inequality is always true, as the rhs is always positive whereas the lhs always yields a negative value.
Therefore v1 < vmax |at holds.
We have to distinguish two cases for vmin|at : For the case that vmin|at = 0 the intersection is always
nonempty, as v1 > 0:
1 p
v1 = (ci1 ci )vi1 + (ci + ci1 )2 vi1 2 + 8ci s amax
2ci
1 p
> (ci1 ci )vi1 + (ci + ci1 )2 vi1 2
2ci
1
= 2ci1 vi1
2ci
0.
The next case is vmin|at > 0. The intersection will be non-empty as soon as vmin|at v1 . We derive a
condition for this:
v1 vmin|at
1 p p
(ci1 ci )vi1 + (ci + ci1 )2 vi1 2 + 8ci s amax vi1 2 2atmax s .
2ci
With both sides of the inequality greater than zero, squaring both sides is an equivalence transformation:

(ci1 ci )2 vi1 2 + 2(ci1 ci )vi1 . . . + (ci + ci1 )2 vi1 2 + 8ci amax s 4ci 2 vi1 2 8ci 2 atmax s

2 ci1 2 ci 2 vi1 2 + 8ci s (amax + ci atmax ) 2 (ci1 ci ) vi1 . . .
4ci s (amax + ci atmax )
(ci1 + ci ) vi1 + . . ..
(ci1 ci ) vi1
For this to have any chance to hold, the lhs has to be positive. This translates into a constraint for vi1 :
(ci1 + ci ) vi1 + 0
(ci1 ci ) vi1
(ci1 + ci ) vi1 2
ci1 ci
vi1 2 .
(ci1 ci ) (ci1 + ci )
81
We keep this constraint in mind and assume for now that vi1 fulfills it. Therefore both sides of the
original inequality are now positive and we can square them. Note that the term (ci1 + ci )2 vi1 2 cancels
out immediately:
4ci s (amax + ci atmax ) 16ci 2 2s (amax + ci atmax )2 )
2(ci1 + ci ) + +8ci amax s
ci1 ci (ci1 ci )2 vi1 2
8ci (ci1 + ci )s (amax + ci atmax ) 16ci 2 2s (amax + ci atmax )2
vi1 2 + 8ci amax s vi1 2
ci1 ci (ci1 ci )2
8ci s ((ci1 + ci )(amax + ci atmax ) (ci1 ci )amax ) 16ci 2 2s (amax + ci atmax )2
vi1 2
ci1 ci (ci1 ci )2
2ci s (amax + ci atmax )2
((ci1 + ci )(amax + ci atmax ) (ci1 ci )amax ) vi1 2
ci1 ci
2
2c (a
i s max + c a
i tmax )
(2ci amax + (ci1 + ci )ci atmax ) vi1 2
ci1 ci
2s (amax + ci atmax )2
vi1 2 .
(ci ci1 ) (2amax + (ci1 + ci )atmax )
We remember that there was an additional constraint on vi1 but it is weaker than the final constraint:
It is
4ci s (amax + ci atmax ) 2s (amax + ci atmax )2
>
(ci1 ci )(ci1 + ci ) (ci ci1 ) (2amax + (ci + ci1 )atmax )
2ci amax + ci atmax amax + ci atmax
> = .
ci1 + ci 2amax + (ci + ci1 )atmax a + ci atmax + amax + ci1 atmax
| {z } | max {z }
>1 <1
A last consideration for this case reassures that the computed bound fulfills the assumption of vmin|at > 0
(see the second case of Eq. (3.29)):
s
2s (amax + ci atmax )2 p
> 2s atmax
(ci ci1 ) ((2amax + (ci1 + ci )atmax ))
(amax + ci atmax )2 > atmax ((ci ci1 ) (2amax + (ci1 + ci )atmax ))
a2max + 2ci amax atmax + ci 2 a2tmax > a2tmax (ci 2 ci1 2 ) + atmax (ci ci1 )2amax
a2max + 2amax atmax ci1 + a2tmax ci1 2 > 0.
The last line is a tautology if case assumptions are considered.

We can now conclude for this case:

(ci > 0) (ci1 0) (ci > ci1 )
s !
Ia [vmin|at , vmax |at ] 6= vi1 . (B.1)
(ci ci1 ) (2amax + (ci1 + ci )atmax )
B.1.1.2 Case 1.2, ci < ci1

Note that the case assumption implies ci1 > 0. With ci1 ci > 0 as a direct consequence of the case
assumptions v1 > 0 is easily verified by its definition in Eq. (3.32).
82
For v2 it holds
1 p
v2 = (ci1 ci )vi1 (ci + ci1 )2 vi1 2 + 8ci amax s
2ci
1 p
< (ci1 ci )vi1 (ci + ci1 )2 vi1 2
2ci
1 p
= (ci1 ci )vi1 (ci + ci1 )2 vi1 2
2ci
1
= 2ci vi1
2ci
0.
Therefore it is Ia = [0, v2 ] [v1 , v1 ] or Ia = [0, v1 ], depending on whether or not v1 , v2 exist. For

intersection with [vmin|at , vmax |at ] it will be shown, that v1 vmin|at :
v1 vmin|at
1 p p
(ci1 ci )vi1 + (ci + ci1 )2 vi1 2 + 8ci amax s vi1 2 2atmax s .
2ci
As both sides of the inequality are non-negative they can be squared without causing any harm:

(ci1 ci )2 vi1 2 + 2(ci1 ci )vi1 . . . + (ci + ci1 )2 vi1 2 + 8ci amax s 4ci 2 vi1 2 8ci 2 atmax s

2(ci1 2 ci 2 )vi1 2 + 2(ci1 ci )vi1 . . . + 8ci s (amax + ci atmax ) 0.
The lhs of the inequality is easily verified to be non-negative through the case assumptions.
As a first result we state that a non-empty intersection exists as soon as v1 , v2 do not exist and have
distinct values:
(exv1sv2s) Ia [vmin|at , vmax |at ] 6= , (B.2)
where s !
8ci amax s
exv1sv2s = vi1 > . (B.3)
(ci + ci1 )2
We assume that v1 , v2 do exist and are not equal (exv1sv2s). The first possibility for an intersection is
v1 vmax |at
1 p p
(ci1 ci )vi1 + (ci + ci1 )2 vi1 2 8ci amax s vi1 2 + 2atmax s .
2ci
Squaring is an equivalence transformation as both, lhs and rhs are non-negative:

(ci1 ci )2 vi1 2 + 2(ci1 ci )vi1 . . . + (ci1 + ci )2 vi1 2 8ci amax s 4ci 2 vi1 2 + 8ci 2 atmax s

2(ci1 2 ci 2 ) 8ci s (ci atmax + amax ) 2(ci1 ci )vi1 . . .
4ci s (ci atmax + amax )
(ci1 + ci )vi1 + . . ..
(ci1 ci )vi1
83
If the inequalitys lhs is negative there wont be an intersection. We assume to lhs to be non-negative. This
is the case if
(ci1 + ci )vi1 + 0
(ci1 ci )vi1
(ci1 + ci )vi1 2
ci1 ci
4c (c a
i s i tmax + a max )
vi1 2
(ci1 ci )(ci1 + ci )
s
vi1 .
With the assumption of the lhs being non-negative we continue transforming the inequality by squaring
both sides, note that the term (ci1 + ci )2 vi1 2 immediately cancels out:
8(ci1 + ci )ci s (ci atmax + amax ) 16ci 2 2s (ci atmax + amax )2
+ 8ci amax s
8(ci1 + ci )ci s (ci atmax + amax ) + 8ci (ci1 ci )amax s 16ci 2 2s (ci atmax + amax )2
vi1 2
ci1 ci (ci1 ci )2
2s (ci atmax + amax )2
(2amax + (ci1 + ci )atmax )vi1 2
ci1 ci
s
vi1 .
(ci1 ci )(2amax + (ci1 + ci )atmax )
We state another intermediary result:

s !
exv1sv2s vi1
s !!
vi1
Ia [vmin|at , vmax |at ] 6= . (B.4)
Note that also holds:
(exv1sv2s
s ! s !!!
4ci s (ci atmax + amax ) 2s (ci atmax + amax )2
vi1 vi1
(ci1 ci )(ci1 + ci ) (ci1 ci )(2amax + (ci1 + ci )atmax )
[v1 , v1 ] [vmin|at , vmax |at ] = . (B.5)
The second possibility for an intersection is that [0, v2 ][vmin|at , vmax |at ] 6= . This is true for v2 vmin|at .
Before examining this condition we first demand v2 to be positive:
v2 > 0
p
(ci1 ci )vi1 > (ci + ci1 )2 vi1 2 8ci amax s .
84
Taking both sides to the power of two:
4ci1 ci vi1 2 > 8ci amax s

2amax s
vi1 2 <
ci1
s
2amax s
vi1 < .
ci1
We define s !
2amax s
v2spos = vi1 < . (B.6)
ci1
We have to distinguish two cases for vmin|at : vmin|at = 0 and vmin|at > 0. A non-empty intersection
exists for the case vmin|at = 0 and v2 > 0 and of course for the case vmin|at , v2 > 0 and v2 vmin|at .
Assume vmin|at = 0. This means
p
vmatzero = vi1 2atmax s . (B.7)
We state an intermediary result:
(exv1sv2s vmatzero v2spos) Ia [vmin|at , vmax |at ] 6= . (B.8)
Now, assuming v2 > 0 and vmin|at > 0:
v2 vmin|at
1 p p
(ci1 ci )vi1 (ci + ci1 )2 vi1 2 8ci amax s vi1 2 2atmax s
2ci
to the power of two

(ci1 ci )2 vi1 2 2(ci1 ci )vi1 . . . + (ci + ci1 )2 vi1 2 8ci amax s 4ci 2 vi1 2 8ci 2 atmax s

2(ci1 2 ci 2 )vi1 2 + 8ci s (ci atmax amax ) 2(ci1 ci )vi1 . . .
4ci s (ci atmax amax ) p
(ci1 + ci )vi1 + (ci + ci1 )2 vi1 2 8ci amax s .
(ci1 ci )vi1
A necessary condition for this inequality to hold is the lhs to be non-negative:
4ci s (ci atmax amax )

(ci1 + ci )vi1 + 0
(ci1 ci )vi1
vi1 2 .
We call this
2 4ci s (ci atmax amax )
precond1 = vi1 . (B.9)
85
Assuming precond1 to hold, we proceed with the original inequality by squaring both sides, note that
the term (ci1 ci )2 vi1 2 immediately cancels out:
8(ci1 + ci )ci s (ci atmax amax ) 16ci 2 2s (ci atmax amax )2
+ 8ci amax s
8(ci1 + ci )ci s (ci atmax amax ) + 8(ci1 ci )ci amax s 16ci 2 2s (ci atmax amax )2
vi1 2
ci1 ci (ci1 ci )2
2
2s (ci atmax amax )
((ci1 + ci )atmax 2amax ) vi1 2 .
ci1 ci
As the rhs is negative, this would be tautology for (ci1 + ci )atmax 2amax 0, for the complementary case
(ci1 + ci )atmax 2amax < 0 it translates to:
s
2s (ci atmax amax )2
vi1 .
(ci1 ci )(2amax (ci1 + ci )amax )
A short derivation shows that it is always (ci1 + ci )atmax 2amax < 0: So far, we assumed v2hpos and
vmatzero to hold. A consequence of this is:
s
2amax s p
> 2atmax s
ci1
amax
> atmax
ci1
ci1 atmax < amax
with the case assumption ci < ci1
(ci1 + ci )atmax < 2amax .

Before concluding for this case we state some more intermediary results:
s !!
exv1sv2s vmatzero v2spos precond1 vi1
(ci1 ci )(2amax (ci1 + ci )atmax )
Analogously to the first possibility for intersection it also holds:
exv1sv2s vmatzero
s !! !
v2spos precond1 vi1
[0, v2 ] [vmin|at , vmax |at ] = . (B.11)
We can now conclude for this case:

(ci > 0) (ci1 0) (ci < ci1 )

Ia [vmin|at , vmax |at ] 6= (exv1sv2s (exv1sv2s (isect1 isect2 isect3 ))) , (B.12)
86
where
s !
8ci amax s
exv1sv2s = vi1 >
(ci + ci1 )2
s ! s !!
4ci s (ci atmax + amax ) 2s (ci atmax + amax )2
isect1 = vi1 vi1
isect2 = (vmatzero v2spos)
s !!
isect3 = vmatzero v2spos precond1 vi1
p
vmatzero = vi1 2atmax s
s !
2amax s
v2spos = vi1 <
ci1

precond1 = vi1 2 .
A quick repetition of the semantics:

isect1 true iff intersection occurs with [v1 , v1 ]
isect2 true iff intersection occurs with [0, v2 ] and vmin|at = 0
isect3 true iff intersection occurs with [0, v2 ] and vmin|at > 0
As some of the conditions are lower bounds to vi1 it cannot be taken for granted that once a suitable
vi1 has been found, decreasing it will never result in an empty intersection. We will now argue, however,
that this is the case.
For the lower bound introduced by exv1sv2s this is obvious, as exv1sv2s on its own is sufficient for a
non-empty intersection. A closer look is needed for the lower bounds active in isect3 :
We assume to have found a vi1 that fulfills isect3 and while decreasing vi1 , vmatzero is the first
condition to become false. As can be verified, the condition isect2 immediately becomes true and decreasing
vi1 is harmless from now on.
For the more complex case that precond1 is the first condition to become false while decreasing vi1 , it
will now be shown that for this case isect1 will always be true.
In case precond1 is true, it holds:
4ci s (ci atmax amax ) 4ci s (amax ci atmax ) 4ci s (amax + ci atmax )
vi1 2 < = <
(ci1 ci )(ci1 + ci ) (ci1 ci )(ci1 + ci ) (ci1 ci )(ci1 + ci )
s
vi1 .
This means that the first part of isect1 is fulfilled. For the truth of the second part we introduce a case
distinction:
2 (c a a )2 2 (c a +a )2
s i tmax
Subcase 1 (ci1 ci )(2a max
max (ci1 +ci )atmax )
s i tmax
(ci1 ci )(2a max
max +(ci1 +ci )atmax )
: For isect3 to have a possibility
to hold, the lower bound introduced by precond1 has to be lower than any upper bound, in particular it
87
has to hold:
4ci s (ci atmax amax ) 2s (ci atmax amax )2
.
(ci1 ci )(ci1 + ci ) (ci1 ci )(2amax (ci1 + ci )atmax )
Together with the subcase assumption the truth of the second part of isect1 results.
2 (c a a )2 2 (c a +a )2
s i tmax
Subcase 2 (ci1 ci )(2a max
> (ci1 ci )(2a
s i tmax max
: We will first transform the
subcase assumption to retrieve a statement that will be of use later on.
2s (ci atmax amax )2 2s (ci atmax + amax )2

>
(ci1 ci )(2amax (ci1 + ci )atmax ) (ci1 ci )(2amax + (ci1 + ci )atmax )
(ci atmax amax )2 (ci atmax + amax )2
>
2amax (ci1 + ci )atmax 2amax + (ci1 + ci )atmax
(ci atmax amax )2 (2amax + (ci1 + ci )atmax ) > (ci atmax + amax )2 (2amax (ci1 + ci )atmax )
8ci atmax a2max + 2(ci 2 a2tmax + a2max )(ci1 + ci )atmax > 0
4ci a2max + (ci1 + ci )(ci 2 a2tmax + a2max ) > 0.
With a last transformation step we can state the subcase assumption as
ci 2 (ci1 + ci )a2tmax > (3ci ci1 )a2max . (B.13)
For the fulfillment of the second part of isect1 it is needed:
4ci s (ci atmax amax ) 2s (ci atmax + amax )2

2ci (amax ci atmax ) (ci atmax + amax )2

ci1 + ci 2amax + (ci1 + ci )atmax
(2ci amax 2ci 2 atmax )(2amax + (ci1 + ci )atmax ) (ci 2 a2tmax + 2ci atmax amax + a2max )(ci1 + ci )
4ci a2max 4ci 2 atmax amax 2ci 2 (ci1 + ci )a2tmax ci 2 (ci1 + ci )a2tmax + (ci1 + ci )a2max
(4ci ci1 ci )a2max 4ci 2 atmax amax + (2ci 2 (ci1 + ci ) ci 2 (ci1 + ci ))a2tmax 0
(3ci ci1 )a2max 3ci 2 (ci1 + ci )a2tmax 4ci 2 atmax amax 0.
| {z }| {z }
Eq. (B.13) 0 0
As indicated by the braces, the subcase assumption leads to the lhs always being negative, which means
that the second part of isect1 is fulfilled.
We have shown for this case that once a suitable vi1 has been found, decreasing it has no effect on the
non-emptiness of the intersection.
B.1.1.3 Case 1.3, ci = ci1

For ci = ci1 it is v2 < 0 and (if exists) v2 < 0 as can be seen in the respective definitions (Eqs. (3.32) and
(3.33)). Furthermore v1 and v1 degenerate according to
r r
2
2amax s 2amax s
v1 = vi1 + , v1 = vi1 2

.
ci ci
88
Conviction that the intersection of Ia and [vmin|at , vmax |at ] is never empty is now achieved through the
definitions in Eqs. (3.30) and (3.29): One interval is always contained in the other one.
Therefore, we conclude for this case

(ci > 0) (ci1 0) (ci = ci1 ) Ia [vmin|at , vmax |at ] 6= . (B.14)
B.1.2 Case 2, ci < 0, ci1 0

This case has structural similarities to case 1: case 2.1 corresponds to case 1.2, and case 2.2 to case 1.1,
respectively.
B.1.2.1 Case 2.1, ci > ci1

Case assumptions lead to ci1 < 0, ci1 ci < 0 and to the unconditioned existence of v1 and v2 .
Furthermore, v2 > 0 always holds.
Additionally it is always v1 < 0:
v1 < 0
1 p
(ci1 ci )vi1 + (ci + ci1 )2 vi1 2 8ci s amax < 0
2ci
p
(ci1 ci )vi1 + (ci + ci1 )2 vi1 2 8ci s amax > 0.
This is derived by:

p
(ci1 ci )vi1 + (ci + ci1 )2 vi1 2 8ci s amax
p
> (ci1 ci )vi1 + (ci + ci1 )2 vi1 2
= (ci1 ci )vi1 + |ci + ci1 |vi1
= (ci1 ci + |ci | + |ci1 |)vi1
= 2|ci |vi1
> 0.
Depending on the existence of v1 , v2 it is now Ia = [0, v2 ] or Ia = [0, v1 ] [v2 , v2 ]. For further discussion
of interval overlapping we first derive v2 vmin|at :
v2 vmin|at
1 p p
(ci1 ci )vi1 (ci1 + ci )2 vi1 2 8ci s amax vi1 2 2atmax s
2ci

(ci1 ci )2 vi1 2 2(ci1 ci )vi1 . . . + (ci1 + ci )2 vi1 2 8ci s amax 4ci 2 vi1 2 8ci 2 atmax s

(ci1 + ci )vi1 + . . ..
(ci1 ci )vi1
As the lhs is clearly negative due to case assumptions the last line always holds.
We state an intermediary result: the existence of a non-empty overlap as soon as v1 and v2 do not exist
with different values.
(exv1v2 ) Ia [vmin|at , vmax |at ] 6= , (B.15)
89
where s !
8ci amax s
exv1v2 = vi1 > . (B.16)
(ci + ci1 )2
From now on exv1v2 and thereby the existence and non-equality of v1 and v2 is assumed. With Ia =
[0, v1 ] [v2 , v2 ] the first possibility for an overlap emerges for v2 vmax |at . Note that v2 > 0 is guaranteed
by case assumptions.
v2 vmax |at
1 p p
(ci1 ci )vi1 (ci + ci1 )2 vi1 2 + 8ci s amax vi1 2 + 2s atmax .
2ci
With both sides guaranteed to be positive, squaring becomes a equivalence transformation:

(ci1 ci )2 vi1 2 2(ci1 ci )vi1 . . . + (ci + ci1 )2 vi1 2 + 8ci s amax 4ci 2 vi1 2 + 8ci 2 s atmax

2(ci1 2 ci 2 )vi1 2 + 8ci s (amax ci atmax ) 2(ci1 ci )vi1 . . .
4ci s (amax ci atmax )
(ci1 + ci )vi1 + . . ..
(ci1 ci )vi1
For the inequality to be able to hold, its lhs must be non-negative. This is the case for:

(ci1 + ci )vi1 + 0
(ci1 ci )vi1
(ci1 + ci )vi1 2
(ci1 ci )
vi1 2
(ci1 + ci )(ci1 ci )
s
vi1 .
Assuming the lhs to be non-negative, we proceed with the original inequality by squaring both sides,
again the term (ci1 + ci )2 vi1 2 appears on both sides and therefore cancels out:
8ci s (ci1 + ci )(amax ci atmax ) 16ci 2 2s (amax ci atmax )2

+ 8ci s amax
8ci s ((ci1 + ci )(amax ci atmax ) (ci1 ci )amax ) 16ci 2 2s (amax ci atmax )2
vi1 2
ci1 ci (ci1 ci )2
2
2ci amax ci (ci1 + ci )atmax 2ci s (amax ci atmax )
vi1 2
ci1 ci (ci1 ci )2
2s (amax ci atmax )2
(2amax (ci1 + ci )atmax )vi1 2
ci1 ci
s
vi1 .
90
This leads to another intermediary result for this case:

s !
exv1v2 vi1
s !!
vi1
Note that also holds:
s !
exv1v2 vi1
s ! !!
vi1
[v2 , v2 ] [vmin|at , vmax |at ] = . (B.18)
Now we proceed to the second possibility for an intersection: v1 vmin|at . For this to be possible, v1
has to be non-negative:
v1 0
1 p
(ci1 ci )vi1 + (ci + ci1 )2 vi1 2 + 8ci s amax 0
2ci
p
(ci1 ci )vi1 (ci + ci1 )2 vi1 2 + 8ci s amax
p
both sides are non-negative, squaring them
(ci1 ci )2 vi1 2 (ci + ci1 )2 vi1 2 + 8ci s amax

4ci1 ci vi1 2 8ci s amax
2s amax
vi1 2
ci1
s
2s amax
vi1 .
ci1
This condition will be named s !

2s amax
v1pos = vi1 (B.19)
ci1
and is assumed to hold from now on.
For decision on v1 vmin|at there are two cases to be distinguished for vmin|at : vmin|at = 0 and vmin|at >
0.
In case vmin|at = 0, which means
p
vmatzero = vi1 2s atmax , (B.20)
91
we have a non-empty intersection as soon as v1 exists with v1 0. We state this as an intermediary result:
(exv1v2 vmatzero v1pos) Ia [vmin|at , vmax |at ] 6= . (B.21)
We assume vmin|at > 0 (i.e., vmatzero). For a non-empty intersection it is needed:
v1 vmin|at
1 p p
(ci1 ci )vi1 + (ci + ci1 )2 vi1 2 + 8ci s amax vi1 2 2s atmax
2ci
both sides are assumed to be positive, they can be squared

(ci1 ci )2 vi1 2 + 2(ci1 ci )vi1 . . . + (ci + ci1 )2 vi1 2 + 8ci s amax 4ci 2 vi1 2 8ci 2 s atmax

2(ci1 2 ci 2 )vi1 2 + 8ci s (amax + ci atmax ) 2(ci1 ci )vi1 . . .
(ci1 + ci )vi1 . . ..
(ci1 ci )vi1
We want to assume the lhs to be positive and translate this into a condition for vi1 :

(ci1 + ci )vi1 0
(ci1 ci )vi1
vi1 2
We call it
2 4ci s (amax + ci atmax )
precond1 = vi1 . (B.22)
Assuming precond1 to hold we continue to process the original inequality by squaring both sides, as they
now share signs. Note that the term (ci1 + ci )2 vi1 2 immediately cancels out.
8ci s (amax + ci atmax )(ci1 + ci ) 16ci 2 2s (amax + ci atmax )2

+ 8ci s amax
8ci s ((amax + ci atmax )(ci1 + ci ) (ci1 ci )amax ) 16ci 2 2s (amax + ci atmax )2

2 2 2 2
ci s (2amax + (ci1 + ci )atmax ) 2ci s (amax + ci atmax )

(2amax + (ci1 + ci )atmax )
(ci1 ci )vi1 2
(2amax + (ci1 + ci )atmax )vi1 2 .
ci1 ci
This inequality is a tautology for the case (2amax + (ci1 + ci )atmax ) 0 as the rhs is always non-negative.
For the complementary case (2amax + (ci1 + ci )atmax ) > 0 it translates to:
s
vi1 .
92
The case (2amax +(ci1 +ci )atmax ) 0 can never occur in this context: We assumed v1pos and vmatzero
to hold, which has as a consequence:
s
2amax s p
> 2atmax s
ci1
amax
> atmax
ci1
amax > ci1 atmax
amax + ci1 atmax > 0.
The case assumption ci > ci1 leads to
2amax + (ci1 + ci )atmax > 0.
Prior to concluding for this case, we state some intermediary results:
s !!
exv1v2 vmatzero v1pos precond1 vi1
Note, that analogously to the first intersection possibility also this holds:
exv1v2 vmatzero
s !! !
v1pos precond1 vi1
[0, v1 ] [vmin|at , vmax |at ] = . (B.24)
Concluding for this case, we can state:

(ci < 0) (ci1 0) (ci > ci1 )

Ia [vmin|at , vmax |at ] 6= (exv1v2 (exv1v2 (isect1 isect2 isect3 ))) , (B.25)
93
where
s !
8ci amax s
exv1v2 = vi1 >
(ci + ci1 )2
s ! s !!
4ci s (amax ci atmax ) 2s (amax ci atmax )2
isect1 = vi1 vi1
(ci1 + ci )(ci1 ci ) (ci1 ci )(2amax (ci1 + ci )atmax )
isect2 = (vmatzero v1pos)
s !!
isect3 = vmatzero v1pos precond1 vi1
p
vmatzero = vi1 2s atmax
s !
2s amax
v1pos = vi1
ci1

2 4ci s (amax + ci atmax )
precond1 = vi1 .
The abbreviations meanings are:

isect1 true iff intersection occurs with [v2 , v2 ]
isect2 true iff intersection occurs with [0, v1 ] and vmin|at = 0
isect3 true iff intersection occurs with [0, v1 ] and vmin|at > 0
As some of the conditions are lower bounds to vi1 it cannot be taken for granted that once a suitable
vi1 has been found, decreasing it will never result in an empty intersection. We will now argue, however,
that this is the case.
For the lower bound introduced by exv1v2 this is obvious, as exv1v2 on its own is sufficient for a
non-empty intersection. More derivations are needed for the lower bounds active in isect3 :
Assume we have found a vi1 that fulfills isect3 and while decreasing vi1 , vmatzero is the first condition
to become false. The condition isect2 immediately becomes true and decreasing vi1 is harmless from now
on.
For the more complex case that precond1 is the first condition to become false while decreasing vi1 , it
will now be shown that for this case isect1 will always be true.
For the case that precond1 holds, it is (also due to ci < 0):
4ci s (amax + ci atmax ) 4ci s (amax ci atmax )

vi1 2 < <
(ci1 ci )(ci1 + ci ) (ci1 ci )(ci1 + ci )
s
vi1 .
This guarantees the truth of the first part of isect1 . For the second part we will distinguish two cases:
2 (a +c a )2 2 (a c a )2
s
Subcase 1 (ci1 ci )(2a max i tmax
s
(ci1 ci )(2a max i tmax
: For isect3 to be possible to
hold the lower bound imposed by precond1 has to be smaller than any active upper bound, in particular it
94
has to hold:
4ci s (amax + ci atmax ) 2s (amax + ci atmax )2
.
Taking the subcase assumption into account, the fulfillment of the second part of isect1 is obvious.
2 (a +c a )2 2s (amax ci atmax )2
s
Subcase 2 (ci1 ci )(2a max i tmax
> (ci1 ci )(2amax (ci1 +ci )atmax ) : First, we will transform the
subcase assumption for later use:
2s (amax + ci atmax )2 2s (amax ci atmax )2

>
(ci1 ci )(2amax + (ci1 + ci )atmax ) (ci1 ci )(2amax (ci1 + ci )atmax )
(amax + ci atmax )2 (amax ci atmax )2
>
2amax + (ci1 + ci )atmax 2amax (ci1 + ci )atmax
(amax + ci atmax )2 (2amax (ci1 + ci )atmax ) > (amax ci atmax )2 (2amax + (ci1 + ci )atmax )
8ci atmax a2max 2(a2max + ci 2 a2tmax )(ci1 + ci )atmax > 0
4ci a2max (ci1 + ci )(a2max + ci 2 a2tmax ) > 0.
A last step allows to state the subcase assumption as
(3ci ci1 )a2max > ci 2 (ci1 + ci )a2tmax . (B.26)
For the truth of the second part of isect1 we need:
4ci s (amax + ci atmax ) 2s (amax ci atmax )2

(ci1 ci )(ci1 + ci ) (ci1 ci )(2amax (ci1 + ci )atmax )
2ci (amax + ci atmax ) (amax ci atmax )2

ci1 + ci 2amax (ci1 + ci )atmax
(2ci amax + 2ci 2 atmax )(2amax (ci1 + ci )atmax ) (a2max 2ci atmax amax + ci 2 a2tmax )(ci1 + ci )
4ci a2max + 4ci 2 atmax amax 2ci 2 (ci1 + ci )a2tmax (ci1 + ci )a2max + ci 2 (ci1 + ci )a2tmax
(4ci ci1 ci )a2max + 4ci 2 atmax amax 3ci 2 (ci1 + ci )a2tmax 0
(3ci ci1 )a2max 3ci 2 (ci1 + ci )a2tmax + 4ci 2 atmax amax 0.
| {z } | {z }
Eq. (B.26) 0 0
As the braces indicate, application of the subcase assumption leads to the lhs being always positive. This
means that the second part of isect1 is true.
A short summary: It has been shown that once an admissible vi1 has been found for this case, it can
be decreased without any effect on the non-emptiness of the intersection.
B.1.2.2 Case 2.2, ci < ci1

Via ci1 ci > 0 the case assumptions lead to the unconditioned existence of v1 and v2 with v1 < 0.
Likewise, v1 exists v1 < 0 follows directly from the definition of v1 and the case assumptions.
The relevant intervals for intersection are now [v2 , v2 ] and [0, v2 ], respectively, depending on the existence
of v2 .
95
We derive that v2 vmax |at : since vmax |at > 0 by definition (Eq. (3.30)), this is clear for v2 < 0 and we
can therefore assume v2 0.
v2 vmax |at
1 p p
(ci1 ci )vi1 (ci + ci1 )2 vi1 2 + 8ci s amax vi1 2 + 2s atmax
2ci
both sides are non-negative due to the above assumption, therefore they can be squared:

(ci1 ci )2 vi1 2 2(ci1 ci )vi1 . . . + (ci + ci1 )2 vi1 2 + 8ci s amax 4ci 2 vi1 2 + 8ci 2 s atmax

2(ci1 2 ci 2 )vi1 2 + 8ci s (amax ci atmax ) 2(ci1 ci )vi1 . . .
(ci1 + ci )vi1 + . . ..
(ci1 ci )vi1
As the lhs is never positive according to case assumptions, the inequality can easily be identified as a
tautology. Therefore, v2 vmax |at holds.
Combining all information gained so far, we receive a non-empty intersection as soon as vmin|at v2
holds. We will now derive the condition for this:
For the case that vmin|at = 0 it is always vmin|at v2 as v2 > 0:
v2 > 0
1 p
(ci1 ci )vi1 (ci + ci1 )2 vi1 2 8ci s amax > 0
2ci
p
(ci1 ci )vi1 (ci + ci1 )2 vi1 2 8ci s amax < 0.
This can be derived as follows:

p
(ci1 ci )vi1 (ci + ci1 )2 vi1 2 8ci s amax
p
< (ci1 ci )vi1 (ci + ci1 )2 vi1 2
= (ci1 ci )vi1 |ci + ci1 |vi1
= (|ci1 | + |ci | |ci | |ci1 |)vi1
= 2ci1 vi1
< 0.
We proceed to the more general case vmin|at > 0:
v2 vmin|at
1 p p
(ci1 ci )vi1 (ci + ci1 )2 vi1 2 8ci s amax vi1 2 2s atmax
2ci
non-negative lhs and rhs allow squaring

(ci1 ci )2 vi1 2 2(ci1 ci )vi1 . . . + (ci + ci1 )2 vi1 2 8ci s amax 4ci 2 vi1 2 8ci 2 s atmax

(ci1 + ci )vi1 + . . ..
(ci1 ci )vi1
96
For this inequality to hold, the positiveness of the lhs is a vital precondition:

(ci1 + ci )vi1 + 0
(ci1 ci )vi1
(ci1 + ci )vi1 2
ci1 ci
4c (c a
i s i tmax amax )
vi1 2
s
vi1
Under the assumption of this condition to be fulfilled, we continue processing the original inequality by
squaring it. Note that the term (ci1 + ci )2 vi1 2 cancels out immediately.
8(ci1 + ci )ci s (ci atmax amax ) 16ci 2 2s (ci atmax amax )2

+ 8ci s amax
(ci1 ci ) (ci1 ci )2 vi1 2
8ci s ((ci1 + ci )(ci atmax amax ) + (ci1 ci )amax ) 16ci 2 2s (ci atmax amax )2

(ci1 ci ) (ci1 ci )2 vi1 2
ci 2 ((ci1 + ci )atmax 2amax ) 2ci 2 s (ci atmax amax )2

(ci1 ci ) (ci1 ci )2 vi1 2
((ci1 + ci )atmax 2amax )vi1 2
(ci1 ci )
2
2s (ci atmax amax )
vi1 2
(ci1 ci )((ci1 + ci )atmax 2amax )
s
vi1 .
We show that the precondition on vi1 can be safely dropped, as values that fulfill the last inequality
always fulfill that precondition:
4ci s (ci atmax amax ) 2s (ci atmax amax )2

(ci1 ci )(ci1 + ci ) (ci1 ci )((ci1 + ci )atmax 2amax )
2ci ci atmax amax

ci1 + ci (ci1 + ci )atmax 2amax
2|ci | |ci |atmax + amax
.
|ci1 | + |ci | |ci1 |atmax + amax + |ci |atmax + amax
| {z } | {z }
>1 <1
As a last consideration for this case it will be shown that the computed bound for the case vmin|at > 0
97
p
always respects this condition, namely vi1 > 2s atmax :
s
2s (ci atmax amax )2 p
> 2s atmax
(ci atmax amax )2
> atmax
(ci atmax amax )2 < atmax (ci1 ci )((ci1 + ci )atmax 2amax )
ci 2 a2tmax + 2ci atmax amax a2max < a2tmax (ci1 2 ci 2 ) 2(ci1 ci )atmax amax
ci1 2 a2tmax + 2ci1 atmax amax a2max < 0.
The lhs is negative through the case assumptions.
Combining all knowledge gained we conclude for this case:

(ci < 0) (ci1 0) (ci < ci1 )
s !
Ia [vmin|at , vmax |at ] 6= vi1 . (B.27)
(ci1 ci ) ((ci1 + ci )atmax 2amax )
B.1.2.3 Case 2.3, ci = ci1

The definitions of v1 and v1 (Eqs. (3.32) and (3.33)) reveal that v1 < 0 (if exists) and v1 < 0 for the case
ci = ci1 . They also show that v2 and v2 degenerate according to:
r r
2
2amax s 2amax s
v2 = vi1 + , v2 = vi1 2

.
ci ci
The intersection of Ia and [vmin|at , vmax |at ] is never empty because one interval is always contained in the
other one, as can be seen in the definitions in Eqs. (3.29) and (3.30).
Therefore, we conclude for this case

(ci < 0) (ci1 0) (ci = ci1 ) Ia [vmin|at , vmax |at ] 6= . (B.28)
B.1.3 Case 3, ci < 0, ci1 > 0

This case yields ci1 ci > 0 and therefore its assumptions have v1 < 0 and v1 < 0 (if exists) as a
consequence (Definitions are in in Eqs. (3.32) and (3.33)). Furthermore, it is v2 exists v2 < 0:
v2 < 0
1 p
(ci1 ci )vi1 (ci + ci1 )2 vi1 2 + 8ci s amax < 0
2ci
p
(ci1 ci )vi1 (ci + ci1 )2 vi1 2 + 8ci s amax > 0.
This is derived through:
p
p
> (ci1 ci )vi1 (ci + ci1 )2 vi1 2
= (ci1 ci |ci + ci1 |)vi1
= (|ci1 | + |ci | ||ci | + |ci1 ||)vi1
> 0.
98
The interval to intersect with [vmin|at , vmax |at ] therefore reduces to [0, v2 ]. A first precondition for a
non-empty intersection will be v2 > 0:
v2 > 0
1 p
(ci1 ci )vi1 (ci + ci1 )2 vi1 2 8ci s amax > 0
2ci
p
(ci1 ci )vi1 < (ci + ci1 )2 vi1 2 8ci s amax
we take both sides to the power of two
(ci1 ci )2 vi1 2 < (ci + ci1 )2 vi1 2 8ci s amax

4ci ci1 vi1 2 < 8ci s amax
2s amax
vi1 2 <
ci1
s
2s amax
vi1 < .
ci1
As before, we name this condition

s !
2s amax
v2spos = vi1 < . (B.29)
ci1
An intersection will occur as soon as v2 vmin|at . For the case vmin|at = 0 the intersection is non-empty
as soon as v2 is positive (v2spos):
(vmatzero v2spos) Ia [vmin|at , vmax |at ] 6= , (B.30)
where p
vmatzero = vi1 2s atmax . (B.31)
For the case vmin|at > 0 (vmatzero), the following has to hold:
v2 vmin|at
1 p p
(ci1 ci )vi1 (ci + ci1 )2 vi1 2 8ci s amax vi1 2 2s atmax
2ci
squaring an inequality which has sign-sharing sides is an equivalence operation

(ci1 ci )2 vi1 2 2(ci1 ci )vi1 . . . + (ci + ci1 )2 vi1 2 8ci s amax 4ci 2 vi1 2 8ci 2 s atmax

(ci1 + ci )vi1 + . . ..
(ci1 ci )vi1
For the inequality to hold the lhs has to be non-negative:
(ci1 + ci )vi1 + >0
(ci1 ci )vi1
(ci1 + ci )vi1 2 > .
ci1 ci
99
We have to distinguish two cases here: ci1 + ci 0 and ci1 + ci < 0. For the former the inequality always
hold and for the latter it translates to:

vi1 2 <
s
vi1 < .
We state the condition in a unified, more formal way:

s !!!
precond1 = (ci1 + ci 0) (ci1 + ci < 0) vi1 < . (B.32)
Note that this could be simplified, but we resist to do that in order to explicitly state what is true for which
case. Assuming now that the condition precond1 is fulfilled and the lhs of the original inequality therefore
is non-negative, we proceed by squaring it. Note that the term (ci1 + ci )2 vi1 2 immediately cancels out.
8ci s (ci1 + ci )(ci atmax amax ) 16ci 2 2s (ci atmax amax )2

+ 8ci s amax
8ci s ((ci1 + ci )(ci atmax amax ) + (ci1 ci )amax ) 16ci 2 2s (ci atmax amax )2

2ci s (ci atmax amax )2
2
(ci 2 ((ci1 + ci )atmax 2amax ))vi1 2
ci1 ci
((ci1 + ci )atmax 2amax )vi1 2
ci1 ci
As will be shown below, it is ((ci1 + ci )atmax 2amax ) < 0

vi1 2
s
vi1 .
We claimed ((ci1 +ci )atmax 2amax ) < 0, which is a consequence of our assumptions vmatzero v2spos.
For both assumptions to hold, it has to be:
2s amax
2s atmax <
ci1
ci1 atmax < amax
With ci < 0 by case assumption
(ci1 + ci )atmax 2amax < 0.
100
Combining the two possibilities for an intersection, we finally conclude for this case:

(ci < 0) (ci1 > 0) Ia [vmin|at , vmax |at ] 6=
v2spos vmatzero vmatzero precond1

s !!!!!
vi1 , (B.33)
where
s !
2s amax
v2spos = vi1 <
ci1
p
s !!!
precond1 = (ci1 + ci 0) (ci1 + ci < 0) vi1 < .
B.1.4 Case 4, ci > 0, ci1 < 0

This case bears some structural analogy to case 3. The case assumptions directly lead to ci1 ci < 0,
v2 < 0 and v2 < 0 (if exists). Furthermore, it can be shown that v1 exists v1 < 0:
1 p
v1 = (ci1 ci )vi1 + (ci + ci1 )2 vi1 2 8ci s amax
2ci
1 p
< (ci1 ci )vi1 + (ci + ci1 )2 vi1 2
2ci
1
= ((|ci1 | |ci | + ||ci | |ci1 ||)vi1 )
2|ci |
1
= (((|ci1 | + |ci |) + ||ci | |ci1 ||)vi1 )
2|ci |
< 0.
The interval remaining for intersection with [vmin|at , vmax |at ] therefore reduces to [0, v1 ]. For a non-empty
intersection we demand v1 to be positive:
v1 0
1 p
(ci1 ci )vi1 + (ci + ci1 )2 vi1 2 + 8ci s amax 0
2ci
p
(ci1 ci )vi1 (ci + ci1 )2 vi1 2 + 8ci s amax .
101
As both sides of the inequality are non-negative, we can square it:
(ci1 ci )2 vi1 2 (ci + ci1 )2 vi1 2 + 8ci s amax

4ci1 ci vi1 2 8ci s amax
2s amax
vi1 2
ci1
s
2s amax
vi1 .
ci1
We will call this condition s !

2s amax
v1pos = vi1 . (B.34)
ci1
A non-empty intersection will occur as soon as v1 vmin|at . For the case vmin|at = 0 this is the case as
soon as v1pos holds:
(vmatzero v1pos) Ia [vmin|at , vmax |at ] 6= , (B.35)
where p
We now consider the case vmin|at > 0 (i.e., vmatzero), assuming v1pos to hold:
v1 vmin|at
1 p p
(ci1 ci )vi1 + (ci1 + ci )2 vi1 2 + 8ci s amax vi1 2 2s atmax .
2ci
Both sides are non-negative, so they can be squared:

(ci1 ci )2 vi1 2 + 2(ci1 ci ) . . . + (ci1 + ci )2 vi1 2 + 8ci s amax 4ci 2 vi1 2 8ci 2 s atmax

2(ci1 2 ci 2 )vi1 2 + 8ci s (amax + ci atmax ) 2(ci1 ci )vi1 . . .
(ci1 + ci )vi1 . . ..
(ci1 ci )vi1
For the inequality to hold the lhs has to be non-negative:
(ci1 + ci )vi1 0
(ci1 ci )vi1
(ci1 + ci )vi1 2 .
ci1 ci
As the rhs is negative, this is a tautology for ci1 + ci 0. For the complementary case ci1 + ci > 0 it
translates to
vi1 2
s
vi1 .
102
We restate this precondition as

s !!!
precond1 = (ci1 + ci 0) (ci1 + ci > 0) vi1 . (B.37)
Assuming precond1 to hold, we can now continue with the original inequality by squaring it. Note that
the term (ci + ci1 )2 vi1 2 cancels out immediately:
8(ci1 + ci )ci s (amax + ci atmax ) 16ci 2 2s (amax + ci atmax )2

+ 8ci s amax
(ci1 ci ) (ci1 ci )2 vi1 2
8ci s ((ci1 + ci )(amax + ci atmax ) (ci1 ci )amax ) 16ci 2 2s (amax + ci atmax )2

(ci1 ci ) (ci1 ci )2 vi1 2
(ci1 + ci )atmax + 2amax 2s (amax + ci atmax )2

(ci1 ci ) (ci1 ci )2 vi1 2
((ci1 + ci )atmax + 2amax )vi1 2
ci1 ci
Due to (ci1 + ci )atmax + 2amax > 0 (see below for derivation):
vi1 2
(ci1 ci )((ci1 + ci )atmax + 2amax )
s
vi1 .
The claim (ci1 + ci )atmax + 2amax > 0 can be justified as a consequence of the previous assumptions
vmatzero v1pos. For both of them to hold, it has to be
2s amax
2s atmax <
ci1
ci1 atmax < amax
ci1 atmax + amax > 0
With ci > 0 by case assumption we then retrieve
(ci1 + ci )atmax + 2amax > 0.
Summarizing the two possibilities for an intersection, we conclude:

(ci > 0) (ci1 < 0) Ia [vmin|at , vmax |at ] 6=
v1pos vmatzero vmatzero precond1

s !!!!!
vi1 , (B.38)
103
where
s !
2s amax
v1pos = vi1
ci1
p
s !!!
precond1 = (ci1 + ci 0) (ci1 + ci > 0) vi1 .
B.1.5 Case 5, ci = 0, ci1 6= 0

This case treats the condition ci = 0, recall that the interval to be intersected with [vmin|at , vmax |at ] now
has the borders v1 and v2 which are defined by Eq. (3.37).
B.1.5.1 Case 5.1, ci1 > 0
For this case the interval to be intersected with [vmin|at , vmax |at ] simplifies to [0, v2 ] as the lower interval
border v1 can be verified to be negative by the case assumptions.
For a non-empty intersection we demand the upper bound v2 to be non-negative:
v2 0
2s amax
vi1 0
ci1 vi1
2s amax ci1 vi1 2
s
2s amax
vi1 .
ci1
We call this condition

s !
2s amax
v2hpos = vi1 . (B.39)
ci1
In order to retrieve a non-empty intersection v2 vmin|at has to hold. In case vmin|at = 0 this is guaranteed
by v2hpos:
(vmatzero v2hpos) Ia [vmin|at , vmax |at ] 6= , (B.40)
where
p
Assuming now vmin|at > 0 (i.e., vmatzero) we more closely examine the condition
v2 vmin|at
2s amax p
vi1 vi1 2 2s atmax .
ci1 vi1
104
Due to our assumptions both sides are non-negative, so they can be squared:
42s a2max 4s amax

2 2
+ vi1 2 vi1 2 2s atmax
ci1 vi1 ci1
42s a2max 4s amax
2
+ (2s atmax )vi1 2 + 0
ci1 ci1
(2s atmax ci1 2 4s amax ci1 )vi1 2 + 42s a2max
(ci1 atmax 2amax )ci1 vi1 2 + 2s a2max .
As it is ci1 atmax 2amax < 0 (derivation to follow)
2s a2max
vi1 2
(ci1 atmax 2amax )ci1
s
2s a2max
vi1 .
The derivation of ci1 atmax 2amax < 0 is obtained through the previous assumptions vmatzero v1hpos.
In case both are true, it holds
2s amax
2s atmax <
ci1
ci1 atmax < amax
ci1 atmax + 2amax > 0.
Combining all knowledge gained so far, we can conclude for this case:

(ci = 0) (ci1 > 0) Ia [vmin|at , vmax |at ] 6=
s !!! !!
2s a2max
v2hpos vmatzero vmatzero vi1 , (B.42)
where
s !
2s amax
v2hpos = vi1
ci1
p
vmatzero = vi1 2s atmax .
105
B.1.5.2 Case 5.2, ci1 < 0

For this case v2 is negative and hence the interval to be intersected with [svmin|at , vmax |at ] reduces to [0, v1 ].
For the intersection to be non-empty, we demand v1 to be positive:
v1 0
2s amax
vi1 0
ci1 vi1
2s amax ci1 vi1 2
s
2s amax
vi1 .
ci1
This will be abbreviated by s !

2s amax
v1hpos = vi1 . (B.43)
ci1
For the intersection with [vmin|at , vmax |at ] to be non-empty, v1 vmin|at has to hold. For the case vmin|at = 0
this is true, as soon as v1hpos is fulfilled:
(v1hpos vmatzero) Ia [vmin|at , vmax |at ] 6= , (B.44)
with p
For vmin|at > 0 (i.e., vmatzero) the condition for a non-empty intersection is derived as follows:
v1 vmin|at
2s amax p
vi1 vi1 2 2s atmax .
ci1 vi1
Both sides of the inequality share signs, so they can be squared:
42s a2max 4s amax

2 2
+ + vi1 2 vi1 2 2s atmax
ci1 vi1 ci1
2s a2max 2amax
2 2
+ atmax
ci1 vi1 ci1
2s a2max + (2amax ci1 + ci1 2 atmax )vi1 2 0
(2amax + ci1 atmax )ci1 vi1 2 2s a2max .
Claiming 2amax + ci1 atmax > 0 (proof below), we retrieve
2s a2max
vi1 2
(2a + ci1 atmax )ci1
s max
2s a2max
vi1 .
ci1 (2amax + ci1 atmax )
106
The previously made assumptions vmatzero v1hpos lead to the above claimed 2amax + ci1 atmax > 0.
For the truth of both it is required:
2s amax
2s atmax <
ci1
ci1 atmax < amax
ci1 atmax + 2amax > 0.
Concluding for this case we summarize the conditions for a non-empty intersection:

(ci = 0) (ci1 < 0) Ia [vmin|at , vmax |at ] 6=
v1hpos vmatzero vmatzero

s !!!!!
2s a2max
vi1 , (B.46)
ci1 (2amax + ci1 atmax )
where
s !
2s amax
v1hpos = vi1
ci1
p
vmatzero = vi1 2s atmax .
B.1.6 Case 6, ci = ci1 = 0

Similarly to cases 1.3 and 2.3 the intersection with [vmin|at , vmax |at ] is always non-empty. This is due to
the fact that Ia = R+0 for this case (see Eq. (3.31)). The conclusion for this case therefore is:

(ci = 0) (ci1 = 0) Ia [vmin|at , vmax |at ] 6= . (B.47)
We have now derived the biggest bound to vi1 that guarantees a non-empty intersection of [vmin|at ,
vmax |at ] and Ia for all smaller values. This constraint can be treated as an isolated constraint and is
accounted for in the first phase of velocity profile generation. Note that it is also crucial to ensure that
vi lies within the now guaranteed overlap. To do this, we apply the same method backwards, assuming
to move from pi to pi1 . This is possible due to our symmetry assumptions that imply identical absolute
values for accelerational and decelerational constraints.
107
B.2 Pseudo Code

This section presents a pseudo code algorithm that aggregates the results of the previ-
ous section. It computes an upper bound for vi1 that guarantees overlap of [vmin|at ,
vmax |at ] and Ia at pi for all smaller values. Note that for implementation the algorithm
can be optimized by delaying the application of the square root at various locations.
To maintain coherence with the derivations this has not been done here.
1 if ci > 0 ci1 0 then // Case 1
2 if ci > ci1 then
q // Case 1.1
2s (amax +ci atmax )2
3 thresh (atmax (ci +ci1 )+2amax )(ci ci1 ) ;
4 if ci < ci1 then
q // Case 1.2
8ci amax s
thresh1 2 ;
q (ci1 +ci )
5
4ci s (ci atmax +amax
6 tmp1 (ci1 ci )2 ;
q
2s (ci atmax +amax )2
7 tmp2 (ci1 ci )(2a +(ci1 +ci )at
max max );
8 thresh tmp1 min(tmp1,tmp2);
q
2amax s p
9 thresh tmp2 min( ci1 , 2atmax s );
10 thresh tmp3 ;
2amax s 2s (ci atmax amax )2
11 tmp min( ci1 , (ci1 ci )(2a max (ci1 +ci )atmax )
);
12 if tmp > (ci1 ci )(ci1 +ci ) tmp > 2atmax s then
13 thresh tmp3 tmp
14 thresh max(max(thresh1,thresh tmp1),max(thresh tmp2,thresh tmp3))
15 if ci = ci1 then // Case 1.3
16 thresh
17 if ci < 0 ci1 0 then // Case 2
18 if ci > ci1 then
q // Case 2.1
8c a
2 ;
i max s
thresh1
q (ci1 +ci )
19
4ci s (amax ci atmax
tmp1 ;
q (ci1 +ci )(ci1 ci )
20
21 tmp2 (ci1 ci )(2a (ci1 +ci )at
max max );
22 thresh tmp1 min(tmp1,tmp2);
q
2amax s p
23 thresh tmp2 min( ci1 , 2atmax s );
24 thresh tmp3 ;
2amax s 2s (amax +ci atmax )2
25 tmp min( ci1 , (ci1 ci )(2a max +(ci1 +ci )atmax )
);
4ci s (amax +ci atmax )
26 if tmp > (ci1 ci
)(ci1 +ci ) tmp > 2atmax s then
27 thresh tmp3 tmp
28 thresh max(max(thresh1,thresh tmp1),max(thresh tmp2,thresh tmp3))
29 if ci < ci1 then
q // Case 2.2
30 thresh (ci1 ci )((ci +ci1 )atmax 2amax )
31 if ci = ci1 then // Case 2.3
32 thresh
108
B.2 Pseudo Code
33 if ci < 0 ci1 > 0qthen // Case 3

2s amax
34 vtwostarpos ci1 ;
35 precond ;
36 if ci1 + ci < 0qthen
37 precond (ci1 ci )(ci1 +ci ) ;
q
38 thresh tmp min(precond, (ci1 ci )((ci1 +ci )atmax 2amax ) );
p
39 thresh tmp max(thresh tmp, 2s atmax );
40 thresh min(thresh tmp, vtwostarpos)
41 if ci > 0 ci1 < 0qthen // Case 4
2 amax
42 vonestarpos csi1 ;
43 precond ;
44 if ci1 + ci > 0qthen
4ci s (amax +ci atmax )
45 precond (ci1 ci )(ci1 +ci ) ;
q
2s (amax +ci atmax )2
46 thresh tmp min(precond, (ci1 ci )((ci1 +ci )atmax +2amax ) );
p
47 thresh tmp max(thresh tmp, 2s atmax );
48 thresh min(thresh tmp, vonestarpos)
49 if ci = 0 then // Case 5
50 if ci1 > 0 then q // Case 5.1
2s amax
51 vtwohatpos ;
ci1
p q
2s a2
52 thresh tmp max( 2s atmax , ci1 (ci1 at max
2a ) );
max max
53 thresh min(vtwohatpos, thresh tmp);
54 if ci1 < 0 then q // Case 5.2
2 amax
55 vonehatpos ;
csi1
p q
2s a2
56 thresh tmp max( 2s atmax , ci1 (ci1 at max
+2a ) );
max max
57 thresh min(vonehatpos, thresh tmp);
58 if ci = 0 ci1 = 0 then // Case 6
59 thresh ;
60 return thresh;
109
C Sobel Operator for Gradients
For waypoint translation we chose to use a coordinate system derived from the gradient
of the distance map. Thereby the axes of the coordinate system correspond to changing
the waypoints obstacle distance and its lateral displacement with respect to the closest
obstacle. To obtain the the distance maps gradient at a location on the map, we employ
the Sobel Operator.
The Sobel Operator is a well known and commonly used method for 2D gradient
computation on discrete data. It computes the gradient by averaging and differencing
in a window around the desired coordinate, estimating a separate value for the x- and
the y-component of the gradient. An introduction can be found in [5, Chapt. 7.3], the
original Sobel Operator is introduced in [5, pp. 271-2].
To increase robustness, we use a 5x5 window instead of the original 3x3 one. The
following equation displays the weights we use to compute the x-component Sx and
the y-component Sy of the gradient:

1 2 0 2 1 1 4 6 4 1
4 8 0 8 4 2 8 12 8 2

Sx = 6 12 0 12 6 , Sy = 0
0 0 0 0 . (C.1)
4 8 0 8 4 2 8 12 8 2
1 2 0 2 1 1 4 6 4 1
For our application the gradient magnitude is not important, so we can skip normal-
ization and directly compute the angle [0, 2) of the gradient as

arctan Sy Sy 0
= Sx (C.2)
arctan Sy + else.
Sx
110

SP Runk 2008

Uploaded by

Copyright:

Available Formats

SP Runk 2008

Uploaded by

Document Information

Original Description:

Original Title

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

SP Runk 2008

Uploaded by

Copyright:

Available Formats

Faculty of Applied Sciences

Department of Computer Science

Planning Motion Trajectories for

Supervisor: Dipl. Inf. Boris Lau

3 Spline Based Trajectories 11

B Solving the Overlap Problem 80

C Sobel Operator for Gradients 110

Limits of existing approaches Reactive approaches to mobile robot navigation like

Recent developments By the introduction of a separate and smaller feedback loop,

Spline based locomotion planning As means of representation for the trajectories we

Organization The remainder of this work is organized as follows. Chapter 2 gives an

Bands as a form of trajectory representation can be interpreted as a first step to

3.1 2D Paths Using Splines

3.1.1 Desired Properties

Continuity in heading In addition to the self-evident C 0 continuity it makes sense to

Continuity in curvature Trajectories intended to be driven by wheeled robots should

Property Hermite Cubic Bezier Catmull-Rom B-Spline

3.1.2 Choosing a Spline Type

3.1.2.1 Degrees of Freedom

Si (1) = Si+1 (0), i [0, m 2]. (3.3)

Analogously we demand C 1 and C 2 continuity:

Si0 (1) = Si+1

S0 (0) = p0 , Si (1) = pi+1 , i [0, m 1], (3.6)

this results in m + 1 additional constraints, leaving 2 degrees of freedom.

3.1.2.2 Higher Order Splines

S(t) = (1 t)5 P0 + 5(1 t)4 tP1 + 10(1 t)3 t2 P2

Derivatives Derivation of Eq. (3.8) yields

S 0 (0) = 5(P1 P0 ), S 0 (1) = 5(P5 P4 ) (3.10)

3.1.3 Separating Spline and Velocity Profile

3.2 Numerical Velocity Profile

3.2.1.2 Constraint Respecting Profile

Profile generation To generate a constraint respecting velocity profile, in a first step

3.2.2 Respected constraints

3.2.4 Movement Equations

Ignoring the negative solution for t we obtain

which can be simplified to p

3.2.5 Isolated constraints

vi [0, vmax ]. (3.22)

For our method we assume independence of maximum translational velocity vmax

sreact + sbrake dobst

3.2.6 Accelerational constraints

3.2.6.1 Translational Acceleration

vmin|at = vi1 atmax t , (3.27)

vmax |at = vi1 + atmax t . (3.28)

3.2.6.2 Rotational Acceleration

2s amax 2s amax 2s amax

2s amax 2s amax 2s amax

2s amax 2s amax 2s amax

2s amax 2s amax 2s amax

With the assumptions introduced above, translational acceleration has an effect on

which with the help of Eq. (3.20) can be transformed to

2s amax ci vi 2 + (ci ci1 )vi1 vi ci1 vi1 2 2s amax . (3.31)

This quadratic inequality in vi describes a parabola whose basic shape is determined

Intersection with upper bound (2s amax )

ci vi 2 + (ci ci1 )vi1 vi ci1 vi1 2 = 2s amax

Intersection with lower bound (2s amax ) Analogously to the derivation of

ci vi 2 + (ci ci1 )vi1 vi ci1 vi1 2 = 2s amax

We again name the result: