![Chapter 1
Introduction and Literature Review
As autonomous mobile robots are deployed in ever more demanding tasks, they strive
to complete their tasks while minimizing overall travel time. This requirement is
achieved by maximizing the robot’s speed while maintaining velocity dependant safety
constraints throughout the navigation process. Examples are autonomous mobile
robots operating in sentry duty and surveillance [2, 3], where high speed is required to
complete the task without being detected by potential intruders [19]. Other examples
are self-driving cars [50] and traffic assistance systems [4] that will soon allow vehi-
cles to be piloted autonomously at speeds up to 50 km/h [63]. High speed systems
also include mobile robots carrying cargo in large warehouses [70, 79]. In all of these
applications collision safety along high speed paths depends on velocity constraints,
thus requiring a consideration of the robot’s full position-velocity state in order to
synthesize such paths.
There are two main safety concerns during execution of fast mobile robot tasks
in planar environment. The first is to ensure that the robot will always be able to
reach a full stop without colliding with any of the stationary obstacles or moving
agents [27, 57]. The second is to ensure that the robot will not slide or tip-over while
taking sharp turns at high velocity. These two limitations on robot’s speed are induced
by the robot’s geometry and the power delivered by its actuators as well as by the
friction between the wheels and the ground.
11](https://image.slidesharecdn.com/0468cf6d-4b62-44c0-8a22-ec19452dbac9-150703153321-lva1-app6892/85/Manor_final-18-320.jpg)
![According to the classical path planning approach, one first determines a collision
free path using the robot’s configuration space, then selects a time parametrization
that attempts to minimize total travel time subject to velocity-dependent safety con-
straints [33, 35, 64]. The more general approach of planning a path in a space that in-
cludes the robots position as well as velocity is a relatively unexplored area in robotics.
In this work, the entire process of path planning takes place in the robot’s state space,
thus treating the geometric path planning and the speed profiling in a single frame-
work.
1.1 Literature Review
Next, we provide a survey of relevant works in the literature. The survey is divided
into parts according to the different approaches taken in this field.
1.1.1 Configuration Space
The classical approach for path planning considers planning a collision free path in a
configuration space [40, 46]. Given an arrangement of obstacles and the geometry of
a single rigid robot with no kinematic constraints (see Figure 1.1(a) for example), the
configuration space is computed. As depicted in Figure 1.1(b), for every orientation
of the robot, each obstacle induces a c-obstacle. The c-obstacle includes all the points
in the plane which bring the robot to penetrate the physical obstacle. Note that the
c-space depends on the placement of the robot’s coordinates system. When a 2D
environment of obstacles is considered while assuming a planar robot that can freely
translate and rotate, the induced c-space includes towering columns in IR3
. Each col-
umn, represents forbidden places and orientations that will result an imminent collision
between the robot and the physical obstacle. Generally, the c-space’s size depends on
the degrees of freedom of the robot as well as on the environment’s complexity.
The c-space method allows to compute a collision free path connecting the start
12](https://image.slidesharecdn.com/0468cf6d-4b62-44c0-8a22-ec19452dbac9-150703153321-lva1-app6892/85/Manor_final-19-320.jpg)
![(a) (b)
physical environement c-space
c-obstacle
robot’s reference point
Figure 1.1: (a) An arrangement of polygonal obstacles in the plane. (b) Each obstacle
induces a c-obstacle in the robot’s c-space.
to target using various methods. These include exact methods such as cellular decom-
position where the free c-space is decomposed into simple regions whose connectivity
induces a undirected graph denoted a connectivity graph [17]. Other methods suggest
constructing a road map for the environment such as the Voronoi diagram or visibility
graph [76], then search for a collision free path on this graph.
Although very useful in known environment with unconstrained motion, adding
dynamic constraints, which is the main topic of this thesis, increases the size of the c-
space and hence makes it harder to implement most common exact methods. Moreover,
the addition of non-holonomic constraints on the robot’s motion adds non-integrable
equations involving the configuration parameters and their derivatives which limits the
possible motions of the robot but does not reduce the size of the c-space.
1.1.2 Two-steps Methods
The next approach is a two-steps method that first computes pure geometric collision
free path, then generates a velocity profile in such way that total travel time along the
path is minimized while meeting velocity and higher derivative safety constraints. One
of the original works in this subdivision was introduced by Bobrow et al. in [9]. In their
work, the speed parameterizations of the joints of a serial robot were determined using
13](https://image.slidesharecdn.com/0468cf6d-4b62-44c0-8a22-ec19452dbac9-150703153321-lva1-app6892/85/Manor_final-20-320.jpg)
![a linear controller while following a desired path. These speed profiles are selected such
that actuators’ constraints are met. Their work was later extended by [65] to include
the dynamics of a mobile robot. Other works followed starting with lepetic et al. [45],
that proposed a method for high speed path planning while considering acceleration
limits induced by maximal ground friction forces. Their path is generated by a spline
curve computed over a set of “control via points”, whose positional optimization gives
an approximate time-minimal path.
Velenis and Tsiotras [72], propose a method to determine the robot’s speed in order
to minimize travel time along a pre-specified collision free path. The velocity profile
respects the mobile robot’s acceleration limits. Villagra et al. [73], first compute
collision free paths with continuous curvatures and curvatures’ derivatives, then apply
a speed profiler such that time minimal trajectories are obtained along the curvature-
based paths. Their time minimal trajectories satisfy additional comfort constraints
such as jerk limitations.
Wu et al. [74], first compute a geometric path by taking into account kinematic
constraints such as minimal turning radius. Then, a velocity profiler is applied to
maintain prescribed velocity constraints. All these methods plan the robot’s trajectory
in two separate steps. First a purely geometric collision free path is computed, then
the robot’s speed is selected along the path such that velocity safety concerns are met.
However, when truly time optimal paths are desired under velocity dependent safety
constraints, the geometric path taken by the robot and the speed profile along the
path must be synthesized within a single position-velocity space, which is the topic of
this research.
1.1.3 Reactive Motion Planners
An important class of high speed motion planners operate locally in discrete time steps.
Whereby at each time step the robot selects a local high-speed maneuver that avoids
nearby obstacles. The velocity obstacles method models the robot’s safe velocities as
14](https://image.slidesharecdn.com/0468cf6d-4b62-44c0-8a22-ec19452dbac9-150703153321-lva1-app6892/85/Manor_final-21-320.jpg)
![v ‚ƒWXiPRSUvHar€xSX‘Rhzˆr‘€xY`PRab„`SUP€xWXabfŠ€`tbvd§QTSXSUWX„`WUQRaŒd§QRabv@D¤D4(HF1«ˆr‘ 910h‡PR„2„…tbQ‚£saŸWXa¥V6WUfR‚bY`v Q ‘™†tbv
‰—¨‚“ ™!!pB™W£q“¨U—¡ £¢WU„©€`tbv§ahfbvHœbabv§fŒPR„ F
£¢QICD¤D4(HF1¥¤ 91 { $
B
^
VB
VO
B
V
A
VB
A
^
VGWUfT‚bY…v Q Fy™†tbv2iTvHSUQqd§W’€—‘(QTˆb„p€ePRdHSUv¦ ¦¢ 10
Figure 1.2: The velocity obstacle voB induced by a moving obstacle ˆB and a moving robot
ˆA. The entities’ velocities are vA and vB respectively.
an allowed velocity cone [24]. As depicted in Figure 1.2, each velocity cone determines
a local avoidance maneuver, and a series of such maneuvers is selected until the robot
reaches the target. The velocity obstacles method has been extended to local time
optimal maneuvers, and to motion in the presence of other moving agents such as cars
and pedestrians [39, 69, 78].
The dynamic window approach plans the robot’s path within a finite time horizon
whose size is determined by the robot’s current state [26]. At each time step, the
robot’s trajectory is chosen by maximizing a weighted sum of the robot’s distance
from the obstacles, the robot’s speed, and the current direction to the target. Some
dynamic window implementations explicitly account for safe braking constraints [13,
26]. Another approach is the method of inevitable collision states [27, 56, 57]. This
method encodes the surrounding obstacles as well as the robot’s braking capabilities
into first-order differential equations. Numerical integration of these equations gives
the inevitable collision states that can be incorporated into any fast mobile robot
navigation scheme.
Additionally, let us mention the vector field histogram approach [10, 71]. Using the
robot’s sensors, this method builds a histogram based on the distances to the obstacles
in discrete set of angles. The method then searches for gaps in this histogram that
represent collision-free headings of motion from the robot’s current position. Although
15](https://image.slidesharecdn.com/0468cf6d-4b62-44c0-8a22-ec19452dbac9-150703153321-lva1-app6892/85/Manor_final-22-320.jpg)
![efficient for on-line directional control, this method does not consider explicitly safety
criteria or other high level requirements such as total travel time.
Finally, let us discuss the work presented in [66] which considers static environments
populated by disc obstacles. Instead of computing at each time instant a local maneu-
ver which must avoid the obstacles, each obstacle is considered at a time. This reduces
the computational running time to linear in the number of obstacles. About each in-
dividual obstacle, an optimal bypassing maneuver is computed for a non-holonomic
point robot while assuming symmetrically bounded actuators and double integrator
dynamics. A global path is computed by concatenating a series of such paths. How-
ever, this method does not consider safety limitations which is the main topic of this
research.
This family of reactive motion planners is clearly useful in dynamically evolving
environments. In such environments the robot cannot posses full knowledge of the
obstacles and therefore cannot plan its complete path before travel. Therefore the
robot is forced to use a greedy strategy in which it computes its best and fasted path
with respect to its known surroundings.
1.1.4 Discretized Search Spaces
Other motion planners search for a high speed and safe path in the robot’s discretized
state space. First suggested by the kinodynamics method, which discretizes the robot’s
state space into a regular grid and searches the obstacle-free portion of the grid for
a minimum travel time path subject to velocity dependent safety constraints [21, 37,
42]. The kinodynamics approach requires a huge grid in complex environment [32, 43].
The method’s running time is proportional to the grid size and can be very high in
complex environments.
Karvaki [18] and LaValle and Kuffner [41, 44] introduced the RRT method, or
rapidly-exploring random trees, computed in the robot’s discretized state space. At
each time step until the target is reached, a randomly chosen point is connected to the
16](https://image.slidesharecdn.com/0468cf6d-4b62-44c0-8a22-ec19452dbac9-150703153321-lva1-app6892/85/Manor_final-23-320.jpg)
![current tree with a feasible maneuver that obeys predefined velocity constraints and
minimizes a cost function such as path length or travel time. While the RRT method
was an important breakthrough, there are cases where the algorithm converges to a
non optimal path.
To over come this, Karman and Frazzoli, [36], introduced the RRT*. Beside linking
a randomized chosen point to the current tree with an optimal edge that satisfies the
velocity constraints, in each stage, the RRT* also updates the connections between
the nodes in the current tree. The connections between the nodes are updated in such
way that reduces the total cost of the path between the start to the current node. The
RRT* is proven of being complete and converges to the optimal path as number of
iterations reaches infinity. However, its rate of convergence can be excessively slow in
congested environment. Also note that this class of motion planers usually assumes
fully known environments.
1.1.5 Exact Optimal Paths Using Calculus of Variations
Using calculus of variations, Wein et al. [75] compute collision free paths whose geo-
metric shape is determined by a user specified “path quality” parameter δ. A value
of δ = 0 corresponds to minimal length paths that trace the visibility graph, while
δ = ∞ corresponds to maximal clearance paths that trace the Voronoi diagram of
the environment. Based on this path quality measure, they describe a scheme that
discretizes the environment’s Voronoi diagram into -length segments, then searches
the adjacency graph of these segments for optimal paths in O((Λ2
/ 2
)n5/2
log n) time,
where Λ is the total length of the environment’s Voronoi diagram, and n is the num-
ber of obstacle features in the environment. However, [75] does not explicitly consider
velocity dependent safety constraints or travel time minimization.
17](https://image.slidesharecdn.com/0468cf6d-4b62-44c0-8a22-ec19452dbac9-150703153321-lva1-app6892/85/Manor_final-24-320.jpg)
![1.1.6 Optimal Control Based Methods
The use of optimal control for time optimal path computation is well cited in the
literature. One of the earliest works in this field includes Dubins’s work which was
published in 1957 [22]. In his work, Dubins considered a non-holonomic motion of a car
like vehicle navigating between two end points in an obstacles-free environment while
assuming constant speed. He then proved that the time optimal path (with constant
speed!) between these two end points is constructed by path primitives consisting of
constant radius arcs and linear path segments. It is important to note that the initial
and terminal states of the car like vehicle are determined by its position as well as its
orientation. In [58], the car was also allowed with backward motion which created a
non smooth path with cusps.
Many works were inspired by Dubins’s work. In [31], for e.g., the authors compute
path primitives which minimize overall travel distance on the plane in heterogeneous
environments. In such environments the robot’s permitted speed is determined by its
position. Other works create the path using smooth path primitives such as clothoid
which allows to create smooth connectivity between adjacent segments [25, 77]. Com-
puting time optimal path primitives using optimal control theory was also suggested
in [16]. In this work, path primitives were computed for Dubins airplanes. For such
airplanes, the state space is 4D rather than 3D as in the planar Dubins car. However, non
of these works explicitly account the speed dependant constraints involving braking safety
as well as the turning radius of the robot while navigating with high speed between adjacent
obstacles. Although not fully presented in this thesis, this approach is currently being taken
by us in current work as briefly discussed in Section 4.2.
1.1.7 Potential Field Methods
Last, consider works were potential field methods, or PFMs, are used to navigate the robot.
For such methods each obstacle exerts a repulsive force on the robot while the target imposes
an attracting force. A summation of these forces propels the robot between the obstacles to
the target, for e.g. [55, 67, 28]. Although quiet efficient for global path planning and also
18](https://image.slidesharecdn.com/0468cf6d-4b62-44c0-8a22-ec19452dbac9-150703153321-lva1-app6892/85/Manor_final-25-320.jpg)
![as a local planner, PFMs suffer from two main drawbacks as discussed by Koren et al in
[38]. (1) PFMs may lead the robot to a local minimum which will prevent the robot from
reaching its target. This drawback is well investigated in the literature and many solutions
were suggested, for e.g. [60, 59]. (2) PFMs may bring the robot to lose its stability when
implemented online in narrow corridors or near a long wall obstacle. This loss of stability
is characterized by rapidly growing oscillations, left and right, that bring the robot to lose
its grip with the ground and collide with adjacent obstacles. Koren et al. point out this
drawback of PFMs and suggest a stability criteria that has to be taken into account when
implementing such paths using PFMs.
Although this thesis utilizes ideas also used by PFM methods, the implementation draw-
back, discussed by Koren et al. is not in the scope of this research. In this research we
focus mainly on path and trajectory planning rather then trajectory tracking. These two
problems are complementary but can be isolated and dealt independently from each other.
Few examples for works that concern the tracking problem can be found in [6, 52, 15, 5, 80].
Despite all this, we find it important to mention Koren’s work which enlighten the navigation
problem from a different, rather important, point of view.
1.2 Objectives and Significance of Work
As presented in Section 1.1, the problem of finding a collision free path connecting two points
in occupied environments has no particular optimal solution. Moreover, when safety con-
straints such as braking distance and speed dependant turning radius are imposed, dynamic
constraints on robot’s motion are involved which makes the path planning a more complex
task.
As discussed, when dynamic constraints are involved, common numerical approaches
suggest discretizing the search space into a regular grid then implementing a numerical
solver which in some cases does converges to the optimal path (with respect to some quality
measure) in an infinite time.
This thesis considers a relatively simplified problem in which the robot’s model is a
planar disc navigating in the plane among stationary obstacles. The robot is subjected to
19](https://image.slidesharecdn.com/0468cf6d-4b62-44c0-8a22-ec19452dbac9-150703153321-lva1-app6892/85/Manor_final-26-320.jpg)
![uniform safe braking constraint. The classical Brachistochrone problem describes the time
optimal path between two points in an obstacle free environment subjected to a constant force
field. By encoding the braking safety constraint as a force field surrounding each obstacle, the
method generalizes the Brachistochrone problem into the time optimal navigation problem in
an environment populated by stationary obstacles. The time optimal safe path between any
start and target is first computed on a simplified problem, where the mobile robot navigates
in the vicinity of a single point or a wall segment obstacle. Then, the analysis is extended to
environment containing outer walls and multiple internal polygonal obstacles. First, a Speed
Graph for the environment in constructed, which consists of time optimal edges that connect
critical via points. The speed graph is then used to efficiently compute the homotopy class
of paths which most likely contains the global time optimal path1. In the second stage, the
exact time optimal path is computed within the chosen homotopy class based on convexity
properties of these paths.
The concluding chapter of this thesis, summarizes the results of this work and discusses
future extension beyond the scope of this research. Auxiliary material is also added in the
appendices.
1
Two continuous paths α : [a, b] → IRn
and β : [a, b] → IRn
connecting start and target denoted pS
and pT , belong to the same homotopy class if there is a continuous mapping, F : [a, b] × [0, 1] → IRn
,
such that F(t, 0) = α(t), F(t, 1) = β(t) for t ∈ [a, b] and F(a, s) = pS, F(b, s) = pT for s ∈ [0, 1].
21](https://image.slidesharecdn.com/0468cf6d-4b62-44c0-8a22-ec19452dbac9-150703153321-lva1-app6892/85/Manor_final-28-320.jpg)
![2.1 Introduction
This chapter considers planar environments populated by stationary obstacles whose location
is known to the robot. The assumption of stationary obstacles is a conservative approach,
suitable for environments where emergency events occur along obstacle edges or just beyond
corners obstructed by obstacles. For instance, an autonomous vehicle moving in an urban
environment may detect another vehicle suddenly emerging from its parking space. By
maintaining suitable safe velocity, the moving vehicle can brake and prevent collision with
the emerging vehicle. More generally, any environment in which emergency events can be
detected at a distance exceeding the robot’s braking distance can be modeled according to
the method proposed in this chapter and also published in [48, 47].
As presented in the abstract, there are two main safety concerns during execution of fast
mobile robot tasks. The first is to ensure that the robot will always be able to reach a full
stop without colliding with any of the stationary obstacles or moving agents [27, 57]. The
second is to ensure that the robot will not slide or tip-over while taking sharp turns at high
velocity. In this chapter we model the uniform braking constraint as vc-obstacles in position-
velocity configuration space. The vc-obstacles’ cross section increases monotonically along
the velocity coordinate. We characterize the critical points where two vc-obstacles meet
for the first time and locally disconnect the free position-velocity configuration space. In
the physical environment, these are critical events where the robot’s velocity becomes too
large to support safe passage between neighboring obstacles. Next, we describe a cellular
decomposition of the free position-velocity space which is based on these critical points. An
adjacency graph is constructed for the cellular decomposition, such that its edges project
onto the Voronoi diagram of the planar environment. A specialized search algorithm called
the vc-method finds an approximate time-minimal path along the adjacency graph. Finally,
the path’s vertices are smoothed to ensure safe turning at the prescribed path velocity. The
resulting path leads the robot from start to target with maximum velocity while guaranteeing
safe braking and turning along the path.
The chapter is structured as follows. Section 2.2 describes the main velocity depen-
dent safety constraints governing high-speed mobile robot motion. Section 2.3 introduces
the position-velocity space and the vc-obstacles induced by the safe braking constraint in
23](https://image.slidesharecdn.com/0468cf6d-4b62-44c0-8a22-ec19452dbac9-150703153321-lva1-app6892/85/Manor_final-30-320.jpg)
![shortest braking path is dictated by the robot’s maximal deceleration, a system dependent
parameter which is part of the following definition.
Definition 1 (Linear Acceleration Constraint). The robot’s linear acceleration is con-
strained by system dependent parameters amin and amax, which bound the robot’s acceleration
by
amin ˙v · ˆv amax,
where ˆv is the robot’s linear velocity direction and amin 0 to represent deceleration.
The robot’s shortest braking path is defined as follows.
Definition 2 (Shortest Braking Path). The robot’s shortest braking path is any feasible
path along which the robot applies the maximal deceleration amin.
The distance from the robot’s initial position to the point where it reaches a full stop is
not constant. As the robot follows a wiggly but feasible path, this distance becomes shorter
as depicted in Figure 2.1. The robot’s minimal braking distance, denoted d, is the length
of any of its shortest braking paths along which the robot decelerate with amin. Stretching
these braking paths into a linear path of length d will also give a shortest braking path, but
the one with the largest distance measured between the robot’s initial position and the final
braking point. For safety, the robot must keep at least d away from any of the obstacles.
The minimal braking distance can be computed using energy balance as follows [27]. The
mobile robot is modeled as a cylindrical rigid body moving with fixed orientation. Its kinetic
energy is given by
Ek =
1
2
mν2
, (2.1)
where m is the robot’s mass and ν = ||v|| is the robot’s linear velocity magnitude.
Let α(s), s ∈ [0, 1] be a braking trajectory which brings the robot to a full stop along
any of the shortest braking paths. The net work applied by the static friction force, which
acts between the wheels and the ground in order to bring the robot to a complete stop is
given by
Ef = µ
α(s)
fN ds = µfN d = µmgd, (2.2)
where µ is the static coefficient of friction between the wheels and the ground, g is the
25](https://image.slidesharecdn.com/0468cf6d-4b62-44c0-8a22-ec19452dbac9-150703153321-lva1-app6892/85/Manor_final-32-320.jpg)
![gravitational acceleration, and fN =mg is the net normal force at the contacts. The minimal
braking path, d, occurs when the frictional reaction forces between the wheels and the ground
are aligned with the friction-cone edge opposing the direction of motion. Note that during
this braking phase, the wheels are rolling without sliding at the contacts. A complete stop of
the mobile robot requires that all its initial kinetic energy be absorbed by the braking force,
Ek = Ef . (2.3)
Substituting for Ek and Ef gives 1
2mν2 = µmgd. The minimal braking distance is therefore
d(ν) =
1
2µg
ν2
= kν2
, (2.4)
where k = 1/2µg is a constant.
Note that d(ν) does not depend on the robot’s mass. This assumption is valid only under
an ideal rigid body model. In practice the robot’s wheels are rubber coated and experience
substantial deformations at the contacts. In these cases the friction coefficient µ (and hence
the braking distance d) is strongly influenced by the robot’s mass m [68][chap. 3].
Also note that the actual frictional work is accomplished by the braking pads of the robot
and not by the static friction between the wheels and the ground. However if we assume that
the braking pads can deliver a larger braking force than the friction force at the contacts
then the previous development sustains.
The uniform braking constraint: Uniform braking safety is ensured when the robot
is kept at least d(ν) away from the nearest obstacle. This conservative safety criterion is
illustrated in Figure 2.1. All paths that bring the robot to a full stop while applying the
maximal deceleration amin have the same length d. When the closest obstacle is located at
least d away from the robot’s initial position, the robot can reach a full stop without hitting
any obstacle while following any maximal deceleration path.
The second major velocity dependent constraint is the robot’s minimal turning radius.
While turning, centrifugal forces and lateral moments act on the robot’s body. A practical
means to prevent sliding or tipping-over is to lower bound the robot’s turning radius at any
given velocity, as specified in the following definition.
26](https://image.slidesharecdn.com/0468cf6d-4b62-44c0-8a22-ec19452dbac9-150703153321-lva1-app6892/85/Manor_final-33-320.jpg)
![
x
y
Figure 2.3: The vc-obstacles resemble columns whose cross section expands monotonically
along the ν coordinate.
space Fv shrinks inward. The graph depicted in the (x, y) plane is the Voronoi diagram of
the c-space obstacles and an outer boundary which is not shown. The Voronoi diagram will
play an important role in Section 2.5. ◦
2.4 Cellular Decomposition of Position-Velocity Con-
figuration Space
This section describes a cellular decomposition of the free position-velocity configuration
space, Fv, such that each cell is associated with a range of velocities that can be safely
executed at a particular locality of the environment. Section 2.4.1 introduces the position-
velocity height function, ϕ, and analyzes its critical points. Section 2.4.2 partitions Fv into
3D cells based on the critical points of ϕ.
2.4.1 The Critical Points of the Height Function
The free position-velocity configuration space is a stratified set [30], consisting of the following
manifolds. The interior of Fv is a three-dimensional manifold. The portions of the vc-
obstacle surfaces on the boundary of Fv form two-dimensional manifolds. The intersection
curves of vc-obstacle surfaces form one-dimensional manifolds. The isolated points where
three vc-obstacle surfaces intersect form zero-dimensional manifolds.
29](https://image.slidesharecdn.com/0468cf6d-4b62-44c0-8a22-ec19452dbac9-150703153321-lva1-app6892/85/Manor_final-36-320.jpg)
![The cellular decomposition of Fv will be determined by the critical points of the function
ϕ : Fv → IR , which measures the “height” of points p = (x, y, ν) along the ν coordinate in
position-velocity configuration space,
ϕ(x, y, ν) = ν. (2.9)
The critical points of ϕ in Fv are the union of the critical points of the restriction of ϕ
to the individual manifolds comprising Fv [30]. A point p ∈ Fv is a critical point of ϕ
when ϕ(p) is collinear with the generalized normal to the particular manifold containing
the point p. The generalized normal of the manifolds comprising Fv can be characterized as
follows [20]. In the interior three-dimensional manifold, the generalized normal is the zero
vector. At a point on a vc-obstacle surface, it is the usual surface normal pointing into Fv.
At a point on the intersection curve of two vc-obstacle surfaces, it is the convex combination
of the two surface normals meeting at this point. Last, every zero-dimensional manifold is
automatically a critical point of ϕ. The following theorem asserts that ϕ has two types of
critical points.
Theorem 1. The critical points of ϕ in Fv are located either on the intersection curve of
two vc-obstacle surfaces, or at the intersection point of three vc-obstacle surfaces.
Proof: First consider the interior of Fv. Since ϕ(p) = (0, 0, 1) is a non-zero vector, ϕ
cannot have any critical point in the interior of Fv. Next consider the vc-obstacle surfaces,
denoted bdy(COv
i ) for i = 1 . . . k. Denote by ηi = (ηx, ηy, ην) the normal to bdy(COv
i ) at p.
If p is a critical point of ϕ, ϕ must be collinear with ηi:
0
0
1
×
ηx
ηy
ηv
=
−ηy
ηx
0
=
0
0
0
⇒ η=
0
0
ηv
(2.10)
Thus, a necessary condition for p to be a critical point is that (ηx, ηy) = (0, 0) at this
point. The boundary of COv
i is the zero level-set of the scalar valued function: ψ(x, y, ν) =
dst((x, y), Oi) − kν2 = 0. Therefore ψ is collinear with the vc-obstacle normal ηi, and up
30](https://image.slidesharecdn.com/0468cf6d-4b62-44c0-8a22-ec19452dbac9-150703153321-lva1-app6892/85/Manor_final-37-320.jpg)
![to a multiplicative factor:
ηi(x, y, ν) = ψ(x, y, ν) =
dst (q, Oi)
−2kν
.
The function dst(q, Oi) measures the minimal Euclidean distance between q=(x, y) and Oi.
This function is known to have a unit magnitude gradient, pointing from the neatest point on
Oi’s boundary to q [20][Proposition 2.5.4]1. Since dst(q, Oi) is a non-zero vector, ϕ cannot
have critical points on bdy(COv
i ) for i = 1 . . . k.
Next consider a point p on the intersection curve of adjacent vc-obstacle surfaces. The
generalized normal at p ∈ bdy(COv
i )∩bdy(COv
j ) is the convex combination of the vc-obstacle
normals, ηi and ηj,
η ∈ λ1ηi + λ2ηj 0 ≤ λ1, λ2 ≤ 1, λ1+λ2 =1.
A critical point p must therefore satisfy the condition:
ϕ(p) =
0
0
1
= λ1ηi + λ2ηj for some λ1, λ2 ≥ 0. (2.11)
It follows from (2.11) that ϕ has a critical point at p when the tangent to the intersection
curve at p is purely horizontal in (x, y, ν)-space. Such points occur at the lowest points
along the intersection curves. Finally, every isolated point where three vc-obstacle surfaces
intersect is automatically a critical point of ϕ.
The critical points of ϕ on the intersection curve of vc-obstacle pairs will be called Type
I points, while the critical points of ϕ at the intersection points of vc-obstacle triplets will
be called Type II points. The two types of critical points are either saddles or local maxima,
as stated in the following proposition.
Proposition 2.4.1. The function ϕ:Fv →IR has non-smooth saddles at the Type I critical
points, and non-smooth local maxima at the Type II critical points.
1
One must use the generalized normal of dst(q, Oi) when q has multiple nearest points on bdy(Oi).
31](https://image.slidesharecdn.com/0468cf6d-4b62-44c0-8a22-ec19452dbac9-150703153321-lva1-app6892/85/Manor_final-38-320.jpg)
![P
v
jCOv
iCO
x
y
pv
F
Intersection curve
injn
pv v
pv v
pv v
(a) (b)
Figure 2.4: (a) A Type I critical point is a saddle point of ϕ in Fv. (b) The vc-obstacles
slices in the vicinity of the saddle point.
Proof Sketch: Let p be a Type I critical point of ϕ. Then p ∈ bdy(COv
i ) ∩ bdy(COv
j ),
such that the tangent to the curve bdy(COv
i ) ∩ bdy(COv
j ) is purely horizontal at p. Since
the vc-obstacles form columns whose cross-section expands monotonically along the ν axis,
the point p must be a local minimum of ϕ along the intersection curve. Let Vp be the plane
orthogonal to the curve bdy(COv
i ) ∩ bdy(COv
j ) at p. Let Up be a small three-dimensional
ball centered at p. The free space in Vp ∩ Up looks like an inverted V shape, with p located
at its upper vertex (see Figure 2.4(a)). It follows that ϕ has a local maximum within the set
Vp ∩ Up. Since ϕ has a local minimum and maximum along orthogonal directions at p, it has
a non-smooth saddle at this point.
When p is a Type II critical point of ϕ, three vc-obstacles intersect at this point. The
cross-section of each vc-obstacle expands monotonically along the ν axis. Hence at any fixed-
ν slice above p, the three vc-obstacles strictly penetrate each other. Since Fv is bounded
from above by the three vc-obstacles at p, the point p is a non-smooth local maximum of ϕ.
A Type I critical point p is thus a saddle of ϕ in Fv. Based on stratified Morse theory [30],
a locally connected fixed-ν slice of Fv just below p splits at the saddle and becomes two locally
disconnected sets in the slices just above p.
Example: Figure 2.4(a) shows a saddle point p where two vc-obstacles meet for the first
time along the ν axis. Note that ϕ = (0, 0, 1) is orthogonal to the intersection curve
32](https://image.slidesharecdn.com/0468cf6d-4b62-44c0-8a22-ec19452dbac9-150703153321-lva1-app6892/85/Manor_final-39-320.jpg)
![adjacency graph
Voronoi edges
x
y
v
Figure 2.9: The adjacency graph of the cellular decomposition induced by three polygonal
obstacles and an outer wall.
νmax(pi) and νmax(pj) the speeds at the two local maxima. The minimum travel time along
the underlying Voronoi edge consists of two motion modes. Starting at qi with maximal
speed νmax(pi), the robot traces the Voronoi edge with maximal deceleration amin, until
reaching the (x, y) projection of the saddle point pij. The robot next traces the remaining
portion of the Voronoi edge with maximal acceleration amax, until reaching the endpoint qj
with maximal speed νmax(pj). The Type A edge cost is given by
Type A edge cost =
νmax(pi) − νij
|amin|
+
νmax(pj) − νij
amax
where νij is the lowest speed at the saddle point pij =(qij, νij).
Any shortest path search algorithm on Gv gives a pseudo time optimal chain of 3D cells
connecting the start cell to the target cell. Note that the chain of cells does not necessarily
contains the geometrically shortest path, but rather an approximate minimum-time path that
guides the robot safely from start to target under a uniform braking distance constraint.
Computational complexity of the vc-method: The adjacency graph vertices project
to Voronoi nodes in the (x, y) plane, while the Type A edges project to Voronoi edges in
the (x, y) plane. In general, the Voronoi diagram of a polygonal environment containing n
vertices has O(n) nodes and edges [7, 62]. Each vertical stack in the cellular decomposition
of Fv consists of four 3D cells. Hence there are O(n) vertices in Gv. Each 3D cell is
37](https://image.slidesharecdn.com/0468cf6d-4b62-44c0-8a22-ec19452dbac9-150703153321-lva1-app6892/85/Manor_final-44-320.jpg)
 for
full details.
Path smoothing at the nodes: The Type A edges project to Voronoi edges that meet
non-smoothly at discrete nodes in the (x, y) plane. But the robot must respect a velocity
dependent lower bound on its turning radius, rmin(ν), specified in the appendix. When the
optimal path in Gv passes through a vertex pi = (qi, νi), the robot changes its direction of
motion along a circular arc of radius rmin(νi) which is tangent to the Voronoi edges meeting
at qi. By following this circular arc with velocity magnitude which varies linearly between
its values at the arc’s endpoints, the robot executes a smooth edge transition while satisfying
the minimum turning radius constraint. The path smoothing procedure is illustrated in the
following example.
Example: Figure 2.12(a) depicts three c-obstacles in the (x, y) plane, together with three
40](https://image.slidesharecdn.com/0468cf6d-4b62-44c0-8a22-ec19452dbac9-150703153321-lva1-app6892/85/Manor_final-47-320.jpg)
![
lower passage paths
upper passage paths
t(vc-method)/t(lmin)
3
Figure 2.15: The ratio t(vc-method)/t(lmin) of the vc-method as a function of δ.
2.7.2 Comparison of the VC-Method with the Dynamic Win-
dow Approach
The vc-method is next compared with the dynamic window approach [13, 26]. The dynamic
window approach, or DWA, chooses the path’s velocity within a user specified time-window.
Within each time-window the robot moves along a circular arc of radius r. The selection
of r and the speed ν along this arc is based on maximization of an objective function that
consists of three components:
1. Target heading: a function head(r, ν) measuring the inverse deviation of the robot
heading from the target direction.
2. Obstacle clearance: a function dst(r, ν) measuring the minimal distance to the sur-
rounding obstacles.
3. Forward velocity: a function vel(r, ν) specifying the robot’s normalized speed during
tracing of the current circular arc.
The objective function, denoted G(r, ν), is the weighted sum of the three elements:
G(r, ν) = α · head(r, ν) + β · dst(r, ν) + γ · vel(r, ν)
where α, β, and γ are user-specified weights. In each time-window, G(r, ν) is maximized
over all turning radii r and speeds ν that ensure collision-free trajectories as well as braking
43](https://image.slidesharecdn.com/0468cf6d-4b62-44c0-8a22-ec19452dbac9-150703153321-lva1-app6892/85/Manor_final-50-320.jpg)
![0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1
0
5
10
15
20
25
30
35
(b)
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1
0
5
10
15
20
25
30
35
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1
0
5
10
15
20
25
30
35
(a)
DWA
DWA
vc-method
vc-method
minl
v
s
s
s
v
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1
0
5
10
15
20
25
30
vc-method
v
vc-method
(a)
Figure 2.17: The velocity profiles of the paths illustrated in Figures 2.16(a)-(b).
The second scenario, depicted in Figure 2.16(c), resembles urban city blocks surrounded
by a rectangular boundary. The start and target are located at opposite corners of the envi-
ronment. The vc-method path follows Voronoi edges that maintain wide clearance from the
obstacles. This path permits high speeds subject to the uniform braking distance constraint.
The DWA path passes through a low-clearance passage between adjacent city blocks. While
this is a shorter path, the narrow passage forces the robot to slow down, resulting in a path
whose travel time is almost twice longer than the vc-method travel time (Table 2.1).
The simulations indicate that when the motivation is to reduce travel time rather than
path length, wide clearance paths support higher speeds (and hence reduced travel time)
under the uniform braking constraint. By methodically searching the free cells of position-
velocity configuration space, significant travel time improvements can be obtained relative
to more traditional high-speed navigation methods.
2.8 Experiments
The vc-method was implemented on an Irobot Create 4400 platform. The robot’s mini-
mal deceleration was set to amin = −100 mm/sec2
, the maximal acceleration was set to
amax = 100 mm/sec2
, and the minimal turning radius was set to rmin = 300 mm. These
scaled values reflect those of autonomous high-speed platforms (e.g. [3, 2]). Figure 2.18 de-
picts the experimental setup. The environment contains three rectangular obstacles located
within a rectangular outer boundary. The Voronoi diagram which underlies the adjacency
graph construction is marked on the floor. The environment resembles the obstacle-cluster
environment described in Section 2.7.
45](https://image.slidesharecdn.com/0468cf6d-4b62-44c0-8a22-ec19452dbac9-150703153321-lva1-app6892/85/Manor_final-52-320.jpg)
![3.2 Problem Description and Preliminaries
This chapter considers the following problem, first formulated by Fraichard [27]. A mobile
robot modeled as point or a disc has to navigate with high speed in a planar environment pop-
ulated by static polygonal obstacles. The robot is assumed to move freely in IR2
and its state,
denoted (q, ν) ∈ IR2
× IR+
, consists of its position, q = (x, y), and its speed ν = ||v|| = || ˙q||.
While the robot navigates with high speed in the given environment, emergency events may
occur at any time along obstacle boundaries. For instance, a vehicle driving autonomously
along an urban road must limit its speed to a level allowing the vehicle to brake and reach a
full stop when another car suddenly emerges from its parking spot, or a pedestrian suddenly
emerges between two parked vehicles.
To ensure motion safety, the robot must brake and reach a full stop without hitting any
surrounding obstacles whenever such an event is detected by the robot. Braking safety is
ensured when the robot maintains the following safe braking distance constraint. Note that
some notations where already defined in Section 2.2, however for ease of reading we present
them again.
Definition 9. The robot’s safe braking distance, denoted d, is the length of the shortest
kinematically feasible braking path leading from the robot’s current state (q, ν) to a full stop
at ν = 0.
The shortest braking path may be any kinematically feasible path along which the robot
applies maximal deceleration (a system dependent parameter), eventually reaching a full
stop without hitting any obstacle. By maintaining a safe braking distance with respect to
the surrounding obstacles, the robot can handle emergency events that typically occur along
obstacle boundaries. Note that mobile robots are deployed in environments containing static
as well as dynamic obstacles. The current work assumes static obstacles, with the intention
of incorporating dynamic obstacles in future work.
To derive an expression for the safe braking distance as a function of the robot’s current
speed, first consider the robot’s maximal deceleration, denoted amin. Braking on the verge
of sliding determines the robot’s maximal deceleration. The net frictional reaction force
acting at the robot’s wheels during maximal deceleration is given by µmg, where µ is the
coefficient of friction at the ground contacts, m is the robot’s mass, and g is the gravitational
51](https://image.slidesharecdn.com/0468cf6d-4b62-44c0-8a22-ec19452dbac9-150703153321-lva1-app6892/85/Manor_final-58-320.jpg)
![acceleration. The robot’s dynamics during maximal deceleration is given by the equation
mamin = µmg, and this equation gives the maximal deceleration:
amin = µg. (3.1)
While amin does not depend on the robot’s mass, the coefficient of friction µ usually
depends on m due to local deformations at the robot’s wheels [68][p. 15]. The safe braking
distance is computed using energy balance. When the robot moves along a kinematically
feasible braking path, the frictional reaction forces at the contacts are aligned with the
friction cone edge opposing the direction of motion (note that the wheels are rolling without
sliding during this braking phase). A complete stop of the robot requires that all its initial
kinetic energy be absorbed by the braking force. This energy balance leads to the expression
(see e.g. [27]),
d(ν) =
1
2amin
ν2
(3.2)
where amin is the robot’s maximal deceleration, and ν is the robot’s initial speed. The robot
is required to maintain a uniform braking safety distance from all surrounding obstacles, as
stated in the following definition.
Definition 10. When the mobile robot travels with speed ν, uniform braking safety is
ensured when the robot is kept at least d(ν) away from the nearest obstacle at every state
(q, ν).
Uniform braking safety is clearly a conservative safety criterion for autonomous robots.
To satisfy this requirement, the robot must maintain a circular safety zone in order to allow
safe deceleration to a full stop without hitting any obstacle. Since d(ν) is monotonically
increasing in ν, the circular safety zone increases in size when the robot navigates with
higher speeds, thus limiting the robot’s maximal speed at every point in the environment.
In particular, when the start or target are located at an obstacle boundary point, the robot
must start or reach the respective point with zero velocity.
Note that the actual frictional work is accomplished by the braking pads of the robot and
not by the static friction between the wheels and the ground. However if we assume that the
52](https://image.slidesharecdn.com/0468cf6d-4b62-44c0-8a22-ec19452dbac9-150703153321-lva1-app6892/85/Manor_final-59-320.jpg)
![braking pads can deliver a larger braking force than the friction force at the contacts then
the previous development sustains.
3.3 The Safe Time Optimal Path Near a Single Ob-
stacle Feature
This section formulates the safe time optimal path variational problem, then describes an-
alytic solutions for the safe time optimal path near a single obstacle feature which can be
a wall segment or a point obstacle. These solutions will later be used to compute the safe
time optimal path in polygonal environment.
3.3.1 The Travel Time Functional
The robot’s path can be any piecewise smooth curve α(s) : [0, 1] → IR2
with endpoints
pS = α(0) and pT = α(1). When the geometric parameter s is parameterized by time, s(t),
the robot’s speed along α is given by
ν(s(t)) = d
dt α(s(t)) = α (s)
ds(t)
dt
where α (s) = d
ds α(s).
Solving for dt gives:
dt =
α (s)
ν(s)
ds, (3.3)
Integrating both sides of (3.3) gives the travel time functional:
T(α) =
1
0
α (s)
ν(s)
ds, (3.4)
and its minimization over all safe paths α defines the following variational problem.
Definition 11. The travel time variational problem seeks to minimize T(α) over all
piecewise smooth paths α(s) : [0, 1] → IR2
connecting pS and pT , where the speed ν along α
satisfies the uniform braking constraint d(ν) = ν2/2amin.
53](https://image.slidesharecdn.com/0468cf6d-4b62-44c0-8a22-ec19452dbac9-150703153321-lva1-app6892/85/Manor_final-60-320.jpg)
![Note that T(α) is a path dependent function. The collection of piecewise smooth paths
in IR2
forms a metric space under the d1 metric:
d1(α1, α2) = max
s∈[0,1]
{ α1(s)−α2(s) }+ max
s∈[0,1]
{ α1(s)−α2(s) },
where α1 and α2 are two piecewise smooth paths connecting the same end points.
Moreover, T(α) is a continuous function of α with respect to this metric [29]. The inte-
grant of T(α) in (3.4), denoted F, is given by
F α(s), α (s), s =
α (s)
ν(s)
.
Based on calculus of variations [29], when F is differentiable with respect to its arguments,
any extremal path of T(α) over all piecewise smooth paths connecting pS and pT must satisfy
the Euler-Lagrange equation:
∂F
∂α
−
∂
∂s
∂F
∂α
= 0, (3.5)
where α and α are to be treated as formal variables. The safe time optimal path near a wall
segment and a point obstacle is next computed.
3.3.2 The Safe Time Optimal Path Near a Wall Segment
First consider the case of a point robot traveling near a wall segment. This case is a direct
extension of the classical Brachistochrone problem. The Brachistochrone problem considers
a mass particle sliding under the influence of gravity on a wire, and computes the wire’s
geometric shape that would give minimum travel time between two fixed endpoints [11]. In
our case the robot’s maximal deceleration, amin, replaces the gravitational acceleration as
follows.
Consider a wall segment aligned with the x axis as shown in Figure 3.1(a). Denote by
pS = (xS, yS) and pT = (xT , yT ) the start and target, which are assumed to lie in the upper
54](https://image.slidesharecdn.com/0468cf6d-4b62-44c0-8a22-ec19452dbac9-150703153321-lva1-app6892/85/Manor_final-61-320.jpg)
![half of the infinite strip spanned by the wall segment. To encode the safe braking constraint,
note that the robot’s minimal distance from the wall is given by its y coordinate. Along
a safe time optimal path, the robot must keep a distance of at least d(ν) away from the wall
segment:
y ≥ d(ν).
Substituting d(ν) = ν2/2amin while requiring the robot’s speed to be maximized for travel
time minimization gives:
ν(y) = 2amin · y y ≥ 0. (3.6)
In the case of a wall segment, let us use the horizontal x coordinate as the path param-
eter s. The robot’s path is thus parameterized by α(x) = (x, y(x)) for x ∈ [xS, xT ]. In this
parametrization α (x) dx = 1 + (y (x))2dx, and the safe travel time functional (3.4) is
given by
T(α) =
xT
xS
F y(x), y (x) dx
where
F y, y =
1 + y 2
2amin · y
y ≥ 0, (3.7)
with the end conditions y(xS) = yS and y(xT ) = yT .
Any extremal path of T(α) must satisfy the Euler-Lagrange equation (3.5). Substitut-
ing for F(y, y ) in this equation, while considering the fact that ∂G(y , y, x)/∂x = 0, then
integrating with respect to x, gives the implicit scalar differential equation:
1 = c2
1 + y (x)
2
y(x) (3.8)
where c is yet to be determined. Note that the robot’s maximal deceleration, amin, does
not appear in (3.8). Thus, while the extremal path’s total travel time depends on amin,
55](https://image.slidesharecdn.com/0468cf6d-4b62-44c0-8a22-ec19452dbac9-150703153321-lva1-app6892/85/Manor_final-62-320.jpg)
![0 5 10 15 20 25 30 35
0
5
10
15
20
25
30
y
x
5 10 15 20 25 30 35 40
0
5
10
15
20
25
30
y
x
wall segment
(b)(a)
time optimal path
is a cycloid
Sp
Tp
rolling circle
wall segment
Figure 3.1: (a) A circle rolling on the wall segment generates the cycloid curve. (b) The
safe time optimal path near a wall segment.
its geometric shape is invariant with respect to amin. The extremal path solving (3.8) is
a cycloid, α(ξ) = (x(ξ), y(ξ)), where ξ represents the angle that a point fixed on a rolling
circle spans with respect to the vertical direction (Figure 3.1(a)). The formula for α(ξ) is
specified in the following lemma.
Lemma 3.3.1. The safe extremal path of a point robot moving in a planar environment
near a wall segment aligned with the x axis is the cycloid curve:
α(ξ) =
x0
0
+
1
2c2
ξ − sin ξ
1 − cos ξ
ξ ∈ [ξS, ξT ] (3.9)
where ξS, ξT , x0, and c are determined by the path’s endpoints pS and pT . Moreover, α(ξ) is
a local minimum of T(α).
While a formal proof of the lemma is omitted (see e.g. [29]), the Cycloid curve is known of
being the time optimal path connecting two points under a constant gravity field. Since the
problem formulated in this section is identical to the Brachistochrone problem, this Cycloid
curve is time minimal and unique. Note that the time optimal path is uniquely determined
by its endpoints pS and pT . Also note that the time optimal path reaches the x axis along
the normal direction to this axis (Figure 3.1(a)). Moreover, in addition to keeping a safe
braking distance, this path segment also maintains the safe turning radius constraint that
prevents sliding events (see Appendix D).
Example: The time optimal path of a point robot traveling near a wall segment from pS to
56](https://image.slidesharecdn.com/0468cf6d-4b62-44c0-8a22-ec19452dbac9-150703153321-lva1-app6892/85/Manor_final-63-320.jpg)
![pT is shown Figure 3.1(b). The path is a cycloid segment that bends with convex curvature
away from the wall segment. ◦
To complete the wall segment case, consider a scenario where the robot is a disc of radius
ρ. The time optimal path of the robot’s center is given by the formula:
α(ξ) =
x0
ρ
+
1
2c2
ξ − sin ξ
1 − cos ξ
ξ ∈ [ξS, ξT ]
where ξS, ξT , x0, and c are determined by the endpoints pS and pT .
3.3.3 The Safe Time Optimal Path Near a Point Obstacle
Next consider the case of a point robot traveling near a point obstacle located at the origin
of the (x, y) plane. Using polar coordinates centered at the origin, (r, θ), the robot’s position
is given by (x, y) = (r cos θ, r sin θ). The robot’s distance from the point obstacle is given by
its r coordinate. Braking safety requires that the robot keep a distance of at least d(ν) away
from the point obstacle:
r ≥ d(ν).
Substituting d(ν) = ν2/2amin while maximizing the robot’s speed for travel time minimiza-
tion gives:
ν(r) =
√
2amin · r r ≥ 0.
Let the robot’s path be parameterized by θ, so that α(θ) = (r(θ) cos θ, r(θ) sin θ) for
θ ∈ [θS, θT ]. Under this parametrization α (θ) dθ = r2(θ) + (r (θ))2dθ, and the travel
time functional is given by
T(α) =
θT
θS
F r(θ), r (θ) dθ
57](https://image.slidesharecdn.com/0468cf6d-4b62-44c0-8a22-ec19452dbac9-150703153321-lva1-app6892/85/Manor_final-64-320.jpg)
![where
F(r, r ) =
r2 + r 2
2amin · r
r ≥ 0, (3.10)
with the end conditions r(θS) = rS and r(θT ) = rT . Substituting for F(r, r ) in the Euler-
Lagrange equation (3.5), then integrating with respect to θ, gives the implicit scalar differ-
ential equation:
c2
r2
(θ) + (r (θ))2
= r3
(θ) (3.11)
where c is yet to be determined. Much like the wall segment case, the travel time T(α)
depends on amin, but the geometric shape of the extremal path solving (3.11) is invariant
with respect to amin. The solution of (3.11) is a parabolic curve specified in the following
lemma.
Lemma 3.3.2. The safe extremal path of a point robot moving in a planar environment
near a point obstacle located at the origin is the parabolic curve:
α(θ) = r(θ) cos θ, r(θ) sin θ θ ∈ [θS, θT ]
where r(θ) is given by
r(θ) = 2c2 1+sin(θ−θ0)
cos2(θ − θ0)
θ ∈ [θS, θT ] (3.12)
such that θS, θT , c, and θ0 are determined by the path’s endpoints pS and pT . Moreover,
α(θ) is a local minimum of T(α).
While a formal proof of the lemma is omitted, let us note that ∂2F(r, r )/∂2r 0
and that F(r, r ) is differentiable with respect to r and r . This condition implies that the
extremal path is a local minimum of T(α) [29][Theorem 5.5]. In addition, since this problem
is known by having a global minimum (Lemma. 3.4.2), the path α(θ) which is the unique
extremal path must be that global and unique minimum.
The safe time optimal path is uniquely determined by pS and pT , except when the path’s
endpoints lie on opposite sides of the point obstacle. This special case is illustrated in
the following example. Note that this path segment also maintains the safe turning radius
58](https://image.slidesharecdn.com/0468cf6d-4b62-44c0-8a22-ec19452dbac9-150703153321-lva1-app6892/85/Manor_final-65-320.jpg)
![3.4 Basic Properties of the Safe Time Optimal Path
in Polygonal Environments
This section formulates the safe time optimal variational problem in multiple obstacle envi-
ronments2, then discusses basic properties of the safe travel time functional in these envi-
ronments. These properties will guide the speed graph construction and the optimal path
computation.
3.4.1 The Safe Time Optimal Variational Problem in Polyg-
onal Environments
When a mobile robot is subjected to a uniform braking safety constraint, its maximal allowed
speed is governed by its distance to the nearest obstacles. Consider a polygonal environment
consisting of an outer boundary, O0, populated by internal obstacles O1, . . . , Ok. Let F
denote the obstacle-free portion of the environment together with the obstacles’ boundaries.
The robot’s path can be any piecewise smooth curve, α(s) : [0, 1] → F, such that α(0) = pS
and α(1) = pT . Braking safety requires that as the robot moves along α(s), it must keep a
distance of at least d(ν) away from the nearest obstacles,
min
i=0...k
{dst (α(s), Oi)} ≥ d(ν(s)) s ∈ [0, 1], (3.13)
where dst(α(s), Oi) is the minimal distance between the robot positioned at α(s) and the
obstacle Oi. Travel time minimization requires maximal speed. Substituting the robot’s
maximal allowed speed, d(ν) = ν2/2amin, into (3.13) gives the maximal safe speed:
ν(s) = 2amin· min
i=0...k
{dst (α(s), Oi)} s ∈ [0, 1].
The safe travel time functional to be minimized is thus given by
T(α) =
1
0
F α(s), α (s) ds,
2
The term variational problem refers to optimization of a path functional over a set of admissible
paths.
60](https://image.slidesharecdn.com/0468cf6d-4b62-44c0-8a22-ec19452dbac9-150703153321-lva1-app6892/85/Manor_final-67-320.jpg)
![where F(α, α ) is given by
F α, α =
α
2amin·mini=0...k{dst(α, Oi)}
. (3.14)
The minimization of T(α) over all piecewise smooth paths connecting pS with pT in
F defines the safe time optimal variational problem. Note that F(α , α) is continuously
differentiable with respect to the variables α and α, except at special points which are
equidistant from two or more obstacle features in the environment. Next we define the
energy functional associated with T(α).
Definition 12. The energy functional associated with T(α) is the functional T(α) having
the squared integrant,
T(α) =
1
0
¯F α (s), α(s) ds,
where
¯F α, α =
α 2
2amin·mini=0...k{dst(α, Oi)}
. (3.15)
Although the integrants of T(α) and T(α) are almost identical, there is no correspondence
between their extremal paths. This would have enabled us to project the convexity properties
of T(α) on T(α). Consequently, an alternate approach had been taken to prove monotonicity
of T(α) in the paths α as described in Appendix C.
3.4.2 Basic Properties of the Safe Travel Time Functional
This section describes basic properties of the safe time optimal paths in F. Let us first
review the notion of homotopic paths.
Definition 13. Two continuous paths with endpoints pS and pT , α(s) : [0, 1] → F and
β(s) : [0, 1] → F, belong to the same homotopy class if there exists a continuous mapping,
f(s, t) : [0, 1] × [0, 1] → F, such that f(s, 0) = α(s), f(s, 1) = β(s) for s ∈ [0, 1], with
f(0, t) = pS, f(1, t) = pT for t ∈ [0, 1].
The following example illustrates the notion of path homotopy classes.
61](https://image.slidesharecdn.com/0468cf6d-4b62-44c0-8a22-ec19452dbac9-150703153321-lva1-app6892/85/Manor_final-68-320.jpg)
![Example: Let the point obstacle depicted in Figure 3.2(a) represent a disc obstacle of small
radius. The two paths depicted in this figure belong to distinct homotopy classes, since one
path cannot be continuously deformed into the other without crossing the obstacle. ◦
Two paths belonging to the same path homotopy class can be thought of as “points”
connected by a continuous curve (the path homotopy), in the metric space of piecewise
smooth paths in IR2
. Under this interpretation every path homotopy class forms a connected
set in the metric space. Before proceeding on with the properties of the time optimal path,
a fundamental argument about the continuity of T(α) is presented and summarized by the
next Lemma.
Lemma 3.4.1 (Continuity of T(α)). The travel time functional, T(α), is continuous
with respect to paths connecting pS and pT in the metric space induced by the d1 metric.
Proof. The travel time functional T(α) is given by:
T(α) =
1
0
F α, α ds,
where,
F α, α =
||α (s)||
2amin · mini=0...k{dst (α(s), Oi)}
,
for s ∈ [0, 1]. Note that F is a ratio between two functions which are continuous in α and α.
Hence it is a continuous function in the pair (α, α ) at points where the denominator does
not vanish. Therefore, for every 0 there exists a δ( ) 0 such that for any two paths α
and β that satisfy:
d1 (α(s), β(s)) = max
s∈[0,1]
{||α(s) − β(s)||} + max
s∈[0,1]
{||α (s) − β (s)||} δ,
the integrant F satisfies:
F α, α − F β, β , (3.16)
for every s ∈ [0, 1]. Next, examine the travel time difference for two neighboring paths,
62](https://image.slidesharecdn.com/0468cf6d-4b62-44c0-8a22-ec19452dbac9-150703153321-lva1-app6892/85/Manor_final-69-320.jpg)
![path homotopy class of F. Denote by pi the crossing points between the path α with the
segments ci ∈ M, for i = 1...k, where k is the number of segments crossed by α. Note that
k ≤ m where m = size(M) is upper bounded in every path homotopy class. Next, let us
parameterize the position of every crossing point pi along its respective segment ci with the
parameter ui ∈ [0, 1]. When ui = 0, the crossing point pi is positioned on one end of the
segment ci. As ui grows, the crossing point pi is shifted continuously towards the other end
of ci.
Let α be the concatenation of time optimal path segments connected at the crossing
points. Then for every specific selection of the parameters u1, ..., uk the complete path is
uniquely determined and the travel time along it can be computed. The travel time T(α) can
now be written in terms of the parameters u1, ..., uk, as T(α(u1, ..., uk)), which is continuous
over the compact domain u1 ×...×uk ∈ [0, 1]×, ..., [0, 1], and thus possesses a global minimum
in this domain. Let us accept this global minimality as a fact, and proceed to show that
every local minimum of T(α) lies in the interior of F. Braking safety requires that the robot
must travel with zero speed along any obstacles’ boundary segment, thus giving T(α) = ∞
along any boundary segment. It follows that a local minimum path of T(α) can only touch
isolated obstacle boundary points otherwise T(α) = ∞.
If such an isolated point lies in the interior of a polygon edge or at a concave polygon
vertex, the path can be locally shortened and moved away from the obstacle, resulting in
a reduced total travel time along the modified path. A local minimum path of T(α) can
therefore touch only isolated convex obstacle vertices. Suppose the path touches a convex
obstacle vertex as depicted in Figure 3.3(a). The path locally consists of two cycloids that
reach the vertex along the respective edge normals. Such a path can be moved away from
the vertex as depicted in Figure 3.3(b), resulting in a shorter path that maintains larger
clearance from the obstacle. The modified path has a reduced total travel time, implying
that any local minimum path of T(α) must lie in the interior of F.
Calculus of variations is next used to establish the C(1)
smoothness of the safe time
optimal path.
Proposition 3.4.3 (Optimal Path Smoothness). Every safe time optimal path connect-
ing pS and pT in F is a C(1)
curve having a continuous tangent.
64](https://image.slidesharecdn.com/0468cf6d-4b62-44c0-8a22-ec19452dbac9-150703153321-lva1-app6892/85/Manor_final-71-320.jpg)
![Proof: Every obstacle feature in the environment (a polygon vertex or an edge) defines
a proximity cell, consisting of all points in F which are closest to the given obstacle feature.
The proximity cells partition the free space F, and every path connecting pS and pT in F
consists of segments that lie inside individual proximity cells. Let α∗ be a safe time optimal
path connecting pS and pT in F. This path lies in the interior of F according to Prop 3.4.2.
The path satisfies the Euler-Lagrange equation in each proximity cell, and hence forms a C(1)
curve within each cell. Let two segments of α∗ meet at a corner point, p0 = α∗(s0), located
on the common boundary of two proximity cells in F. The term ∂F(α, α )/∂α appearing
in the Euler-Lagrange equation is a continuous function of the path parameter s along any
extremal path of T(α) [29][Section 15].3 Using the expression for F(α, α ) specified in (3.15),
the derivative ∂F(α, α )/∂α is given by
∂F(α, α )
∂α
=
1
L(α) · α (s)
α (s),
where L(α) = 2amin · mini=0...k {dst(α, Oi)}. Continuity with respect to s at p0 gives:
1
L(α∗((s−
0 ))) · α ∗(s−
0 )
α ∗
(s−
0 ) =
1
L(α∗((s+
0 ))) · α ∗(s+
0 )
α ∗
(s+
0 ). (3.17)
Since L is a continuous function of s, L(α∗(s−
0 )) = L(α∗(s+
0 )), eq. (3.17) implies that
α ∗(s−
0 ) = α ∗(s+
0 ), and α∗ is therefore a C(1)
curve.
The next lemma establishes the minimality of the extremal paths of T(α).
Lemma 3.4.4 (Optimal Path Minimality). Every extremal path of the safe travel time
functional T(α) which lies in the interior of F is a local minimum of T(α).
Proof Sketch: We will use the environment’s x coordinate as the robot’s path parameter
s. The robot’s path is thus parameterized by α(x) = (x, y(x)) for x ∈ [xS, xT ].
Consider the travel time functional T(α),
T(α) =
xT
xS
F x, y(x), y (x) dx.
3
This property is known as Erdman’s corner condition in calculus of variations.
65](https://image.slidesharecdn.com/0468cf6d-4b62-44c0-8a22-ec19452dbac9-150703153321-lva1-app6892/85/Manor_final-72-320.jpg)
![where
F =
1 + y 2(x)
2amax · mini=1...k{dst (α(x), Oi)}
dx.
Sufficient conditions for a path α(x) = (x, y(x)) to be a local minimum of T(α) are:
1. The path α is an extremal of T(α).
2. Along the path α, the second derivative ∂2F
∂α 2 (α, α , s) = ∂2F
∂y 2 (y, y , x) 0.
3. The interval [xS, xT ] contains no points conjugate to xS.
See ([29](chapter 5)) for more details.
The first condition stated above is clearly satisfied since every extremal path under
consideration must be a solution of the Euler-Lagrange equation given by, ∂F
∂α − d
ds
∂F
∂α = 0,
or in cartesian coordinates as follows:
∂F
∂y
−
d
dx
∂F
∂y
= 0.
Moreover, each extremal is C(1) since it satisfies the Erdmann corner condition as described
by Prop. 3.4.3.
Next, consider the functional F = 1+y 2(x)
2amax·mini=1...k{dst(α(x),Oi)}
dx. Its second derivative
with respect to y is given by,
Fy y =
1
2amax · mini=1...k{dst (α(x), Oi)}
·
1
(1 + y 2)3/2
0.
Since Fy y 0, the second condition stated above is met not only along an extremal path
but also along every other path. The condition is also known as the strengthened Legen-
dre Condition. Note that necessary conditions only require semi positiveness of Fy y and
therefore, here the strengthened form is met.
Note that generally, the integrant F(y, y ) must be continuously differentiable with re-
spect to y(x) and y (x). In our case, F is continuously differentiable with respect to y but
only piece-wise differentiable with respect to y. At points where two obstacles are equidistant
F loses its differentiability due the minimum function in its denominator. However, these
points coincide with the corner points of the path and hence in every interval between two
66](https://image.slidesharecdn.com/0468cf6d-4b62-44c0-8a22-ec19452dbac9-150703153321-lva1-app6892/85/Manor_final-73-320.jpg)
,[83](p.58),[29](p.112),[84](p.161)). This allows us to derive the minimality
of α.
Note that although sufficient conditions for a minimum are not fully satisfied, neces-
sary conditions are met. Results appearing in the continuation of this thesis support this
minimality (For e.g. see Sec. 3.6.1).
The following theorem establishes the uniqueness of the safe time optimal path in each
path homotopy class connecting pS with pT in F.
Theorem 2 (Optimal Path Uniqueness). The safe travel time functional, T(α), pos-
sesses a unique time optimal path in each path homotopy class connecting pS with pT in
F.
Proof sketch: According to Prop 3.4.2, T(α) attains a global minimum in every path
homotopy class connecting pS with pT in F. Suppose one of these path homotopy classes
contains two local minimum paths, α∗ and β∗
. Since α∗ and β∗
belong to the same path
homotopy class, the two paths can be joined by a path homotopy, f(s, t) : [0, 1] × [0, 1] → F,
such that f(s, 0) = α∗(s) and f(s, 1) = β∗
(s) for s ∈ [0, 1]. The path homotopy f(s, t) can
be interpreted as a continuous path connecting α∗ with β∗
in the metric space of piecewise
smooth paths in IR2
.
The functional T(α) is a continuous function of α. According to the mountain pass theo-
rem [53, 81], the collection of all continuous paths (each being a path homotopy) connecting
α∗ and β∗
contains a distinguished path along which the maximal value attained by T(α)
is minimized.4 Moreover, the maximal value is attained at an extremal path, γ, which is
necessarily a saddle point of T(α). The path γ must lie in the interior of F based on the
following argument. Braking safety implies that γ can only touch isolated obstacle boundary
4
The mountain pass theorem requires that T(α) be defined on a closed and bounded set of paths
in the d1 metric space [53]. This condition is met on the set of piecewise optimal paths connecting
pS with pT in F.
67](https://image.slidesharecdn.com/0468cf6d-4b62-44c0-8a22-ec19452dbac9-150703153321-lva1-app6892/85/Manor_final-74-320.jpg)
![points. Suppose γ touches an obstacle vertex. Consider a path homotopy between α∗ and
β∗
obtained by locally pulling the path homotopy containing γ away from the vertex as
depicted in Figure 3.3. The travel time along the modified path homotopy is reduced, and
the maximal value attained by T(α) along the modified path homotopy is lower than T(γ).
Therefore γ must lie in the interior of F. This leads to a contradiction, since every extremal
path of T(α) in the interior of F must be a local minimum according to Lemma 3.4.4. It
follows that T(α) attains a unique local minimum in each path homotopy class connecting
pS with pT in F.
The last Prop. considers cases where the time optimal path coincides with a Voronoi arc.
Proposition 3.4.5 (Voronoi edge optimality). Given start and target positioned along
an individual straight or parabolic Voronoi arc e, the time optimal path α∗ coincides with the
segment of e bounded by pS and pT .
Proof Sketch: First, assume that e is a Straight Voronoi arc. When pS and pT belong to
e, the shortest path connecting them is the portion of e bounded by pS and pT . Therefore
this portion of e must be the time optimal path connecting pS and pT since it maximizes the
robot’s clearance and speed and additionally ensures minimal travel distance.
Next consider a parabolic Voronoi arc spanned between a wall segment and a point
obstacle. Since this type of Voronoi arc is known of having a parabolic shape [7], its formula
can be readily parameterized by polar coordinates with the point obstacle being at the origin
as follows:
r(θ) = y0
1 + sin(θ − θ0)
cos2(θ − θ0)
, (3.18)
where y0 is the distance between the wall segment and the point obstacle, θ0 is the polar angle
in which the parabolic arc acquires its minimal clearance from the obstacles and θ ∈ [θS, θT ].
Note that (3.18) is identical to the time optimal path near a point obstacle (3.12) with
2c2 = y0 and hence it is optimal with respect to this obstacle feature.
Consider small perturbations of the path away from e towards the wall segment. Such
small perturbations will increase the total length of the path. Moreover since e is equidis-
tant from the wall segment and the point obstacle, any small perturbation of the path will
68](https://image.slidesharecdn.com/0468cf6d-4b62-44c0-8a22-ec19452dbac9-150703153321-lva1-app6892/85/Manor_final-75-320.jpg)
![1o
(b)
speed graph
edges
speed graph
nodes
Sp
Tp
1e
2e
3e
4e
2o1o
(a)
2o
Figure 3.4: (a) The Voronoi diagram, and (b) The speed graph of an environment populated
by two polygonal obstacles (Voronoi arcs are solid curves, speed graph edges are dashed
curves).
necessary reduce the permitted safe speed. Therefore e is optimal.
3.5 The Speed Graph
This section describes the speed graph, which will allow selection of the path homotopy class
that most likely contains the global time optimal path connecting pS with pT in F. The
speed graph construction will use the following Voronoi diagram of F [8, 54].
Definition 14. The Voronoi diagram of a polygonal environment F is the collection of
points equidistant from at least two non-adjacent obstacle features. The equidistant points
form a network of Voronoi arcs that meet at Voronoi nodes.
The Voronoi diagram of F forms a connected network of curves which retains the path
homotopic structure of F. As shown in Figure 3.4(a), the Voronoi arcs reach every concave
corner in F and consequently partition the free space into distinct Voronoi cells. For instance,
the environment depicted in Figure 3.4(a) is partitioned into six Voronoi cells: two cells
surround the internal obstacles O1 and O2, and four cells are associated with the outer walls.
The Voronoi diagram of a polygonal environment with n obstacle features has O(n) Voronoi
arcs and can be computed in O(n log n) time [8]. This work assumes the Voronoi diagram of
F has been computed and is available for the following speed graph construction. The speed
graph consists of safe time optimal arcs connecting points located on the Voronoi diagram
as next described.
69](https://image.slidesharecdn.com/0468cf6d-4b62-44c0-8a22-ec19452dbac9-150703153321-lva1-app6892/85/Manor_final-76-320.jpg)
![Definition 15. The speed graph of F is the weighted graph having the following structure:
1. The speed graph nodes are points on the Voronoi arcs where the obstacles’ equidis-
tance function attains its minimal value along the respective Voronoi arc (possibly at
its endpoint).
2. The speed graph edges are safe time optimal paths connecting every pair of nodes
that lie on the boundary of a common Voronoi cell of F.
3. The cost of each edge is the value of the safe travel time functional, T(α), along this edge.
There are O(n) Voronoi arcs in a polygonal environment having n obstacle features [8],
and each Voronoi arc contains a single speed graph node. The speed graph thus consists of
O(n) nodes and O(n2) edges.
Example: The environment depicted in Figure 3.4(b) is populated by two polygonal ob-
stacles O1 and O2. The figure shows the speed graph nodes, as well as the principle layout
of the speed graph edges whose construction is discussed below. Note that each Voronoi
arc contains a single speed graph node, and that every pair of adjacent Voronoi arcs is con-
nected by a speed graph edge. Since the Voronoi diagram retains the path homotopy classes
of F, the speed graph contains all the path homotopy classes of F. For instance, the path
homotopy class connecting pS to pT while passing between O1 and O2 in Figure 3.4(b) is
represented by the speed graph path along its edges e1, e2, e3, and e4. ◦
Construction of Speed Graph Edges: Consider two speed graph nodes, p1 and p2,
located on the boundary of a Voronoi cell associated with a polygonal obstacle Oi. As
illustrated in Figure 3.5(a), the edges and vertices of Oi define proximity cells, each consisting
of the points in F closest to the given obstacle feature. The two speed graph nodes are
separated by a finite number, say m + 1, of proximity cells. For instance, the nodes p1 and
p2 in Figure 3.5(a) are separated by three proximity cells associated with a vertex and two
incident edges of Oi. The boundary between adjacent proximity cells forms a line segment
denoted lj for j = 1, . . . , m.
The computation will focus on the piecewise time optimal paths connecting p1 and p2,
consisting of time optimal path segments within each proximity cell and corner points that
can freely vary along the proximity cells’ boundary lines. The corner points are parameterized
70](https://image.slidesharecdn.com/0468cf6d-4b62-44c0-8a22-ec19452dbac9-150703153321-lva1-app6892/85/Manor_final-77-320.jpg)
![10 10.5 11 11.5 12 12.5 13 13.5 14
10
10.5
11
11.5
12
12.5
13
13.5
14
1u
2u
*
1u
*
2u
1p
*
2u
*
1u
(a) (b)
2p
proximity cell
boundary lines
speed
graph
edge
* *
1 2( , )u u
iO
1u
2u
initial path
Figure 3.5: (a) Two speed graph nodes, p1 and p2, are separated by three proximity cells
whose boundary lines are parameterized by (u1, u2). (b)T(γ(u1, u2)) is star shaped, moreover
its contours are convex in the (u1, u2) plane.
by p(uj) : [0, 1] → lj for j = 1, . . . , m. For instance, Figure 3.5(a) depicts two corner points
p(u1) and p(u2). Since each of the time optimal path segments is uniquely determined by
its two endpoints, the safe travel time functional T(α) becomes a function of u1, . . . , um. To
compute the time optimal path between p1 and p2 (and hence the speed graph edge), we will
use the following Monotonic property of the travel time functional T(α). The proof of the
following Lemma appears in Appendix C.
Lemma 3.5.1. Let γ(u1, . . . , um) parameterize the piecewise time optimal path segment con-
necting the nodes p1 and p2 in F while crossing the proximity cell boundary lines l1, . . . , lm.
Let u∗ = (u∗
1, . . . , u∗
m) be the parameter values of the optimal crossing points. The functional
T(γ(u1, . . . , um)) is monotonically increasing along any ray emanating from u∗ in the set
(u1, . . . , um) ∈ [0, 1] × · · · × [0, 1].
The monotonicity of T(γ) leads to the following observation. In general, the level set S of
a scalar valued function f(u) is star shaped when there exists a center point, u0 ∈ IRn
, such
that every point in S lies on a unique ray emanating from u0. In particular, convex level
sets are always star shaped. Since T(γ(u)) is monotonically increasing along rays emanating
from u∗, its level sets form star shaped sets as illustrated in the following example.
Example: Consider the speed graph nodes p1 and p2 depicted in Figure 3.5(a). The two
nodes are separated by three proximity cells. The proximity cell boundary lines are param-
eterized by (u1, u2) ∈ [0, 1] × [0, 1], and the convex contours of T(γ(u1, u2)) are depicted in
Figure 3.5(b). Starting with an initial path α0, convex optimization of T(γ(u1, u2)) yields
71](https://image.slidesharecdn.com/0468cf6d-4b62-44c0-8a22-ec19452dbac9-150703153321-lva1-app6892/85/Manor_final-78-320.jpg)
![the optimal parameter values (u∗
1, u∗
2) corresponding to the speed graph edge connecting p1
and p2. ◦
Based on Lemma 3.5.1, every speed graph edge can be computed as a convex optimiza-
tion of T(γ(u1, . . . , um)) on the set (u1, . . . , um) ∈ [0, 1] × · · · × [0, 1]. Convex optimization
algorithms such as the ellipsoid algorithm run in O(m2 log(1/ )) steps [51], where m is the
number of corner points and is the desired solution accuracy. Convex optimization requires
evaluation of the gradient of T(γ(u1, . . . , um)), a computation that takes O(m) operations
and increases the total run time to O(m3 log(1/ )). It is worth mentioning that our examples
were prepared with the more practical Newton-Raphson’s method. While lacking a global
run time bound, it is easier to implement and rapidly converges to an accurate solution;
see Section 3.6.1 for implementation details.
The Voronoi saturation problem:
In Section 3.3, analytic solutions for time optimal paths near a wall segment obstacle and
a single point obstacle were derived. In a real environment which includes multiple internal
obstacles, the time optimal path under the influence of one obstacle may emerge out of its
Voronoi cell and penetrate into a neighboring Voronoi cell, as illustrated in the next example.
Motivational example: Figure 3.6 depicts a rectangular environment containing three
point obstacles, O1, O2, O3. The points pS and pT are positioned within the same Voronoi
cell. The Voronoi edges passing between the obstacles are also depicted in the figure. Three
possible paths are sketched. The path γ is a time optimal path computed as if only obstacle
O1 is present. Note that γ crosses the Voronoi edge at points a and b and penetrates the
neighboring Voronoi cell. The path β includes three segments. The first and last segments,
are the portions of γ connecting pS with a and pT with b. The middle segment of β, coincides
with the portion of the Voronoi diagram between points a and b. The path α is the true
time optimal path connecting pS with pT in this environment. Note that although γ was
computed analytically, it is not time optimal since path β reduces γ’s total travel time. ◦
In order to find the true time optimal path, a saturation method is proposed:
(i) Compute the time optimal path as if only obstacle O1 exists (Path γ).
(ii) If the computed path does not exceed from the Voronoi cell of O1, it is the time optimal
72](https://image.slidesharecdn.com/0468cf6d-4b62-44c0-8a22-ec19452dbac9-150703153321-lva1-app6892/85/Manor_final-79-320.jpg)
![3.6 Speed Graph Based Computation of the Safe
Time Optimal Path in Polygonal Environments
This section describes a speed-graph based scheme for computing the safe time optimal path bet-
ween two points pS and pT in F. As a preliminary step, pS and pT are connected with safe
time optimal arcs to the speed graph nodes lying on the boundary of their Voronoi cells.
The latter arcs can be computed with the convex optimization scheme used to compute the
speed graph edges in Section 3.5. The minimum cost path from pS and pT is next computed
along the augmented speed graph using any standard graph search algorithm. As the speed
graph contains O(n2) edges, the minimum cost path can be computed in O(n2 log n) time,
where n is number of obstacle features in the environment. The resulting minimum cost
path, denoted αo, consists of safe time optimal arcs (the augmented speed graph edges),
meeting non-smoothly at the speed graph nodes.
The path αo indicates the most promising path homotopy class in terms of safe travel
time from pS and pS in F. It will next serve as the initial path for computing the exact
time optimal path in the chosen path homotopy class. The computation will focus on the
piecewise time optimal paths connecting pS and pT , consisting of safe time optimal path
segments in each Voronoi cell, and corner points that can freely slide along the Voronoi arcs
of F. Our objective would be to slide the corner points of αo along the Voronoi arcs, until
the entire path becomes a C(1) curve. In calculus of variations [29][Section 15], any piecewise
optimal path having continuous tangents at its corner points is automatically an extremal
path of the respective functional. Hence the resulting C(1) curve, denoted α∗, is an extremal
path of T(α). Based on Theorem 2, α∗ is the safe time optimal path connecting pS and pT
in the path homotopy class of αo in F.
The path smoothing scheme first identifies which Voronoi arcs of F, termed the optimal
Voronoi arcs, are crossed by α∗. Let p1, . . . , pm denote the Voronoi crossing points of αo. By
inspecting the acute angle spanned by the tangents of αo at these points, one can determine
in O(m) steps the identity of the optimal Voronoi arcs. This property is formally justified
in Appendix C (see proof of Proposition 3.6.1), and is the basis for the following procedure.
74](https://image.slidesharecdn.com/0468cf6d-4b62-44c0-8a22-ec19452dbac9-150703153321-lva1-app6892/85/Manor_final-81-320.jpg)
![Let e1, . . . , en for n ≥ m denote the optimal Voronoi arcs identified by the above proce-
dure. For instance, two such optimal Voronoi arcs, e1 and e2, are depicted in Figure 3.8(a).
The computation of α∗ will focus on the piecewise time optimal paths connecting pS with
pT while crossing the Voronoi arcs e1, . . . , en at points that freely slide along these arcs.
The Voronoi crossing points are parameterized by p(ui) : [0, 1] → ej for i = 1, . . . , n (Fig-
ure 3.8(a)). Since each of the piecewise time optimal paths connecting pS and pT in F is
uniquely determined by its Voronoi crossing points p(ui) for i = 1, . . . , n, the safe travel time
functional T(α) becomes a function of u1, . . . , un. The computation of the safe time optimal
path will use the following monotonicity property of T(α).
Proposition 3.6.1. Let α(u1, . . . , un) parameterize the piecewise time optimal paths con-
necting pS and pT in F while crossing the optimal Voronoi arcs e1, . . . , en. Let u∗ =
(u∗
1, . . . , u∗
n) be the parameter values of the optimal Voronoi crossing points. The functional
T(α(u1, . . . , un)) is monotonically increasing along any ray emanating from u∗ in the set
(u1, . . . , un) ∈ [0, 1] × · · · × [0, 1].
The proof of Proposition 3.6.1 appears in Appendix C. The monotonicity of T(α) leads to
the following observation. In general, the level set S of a scalar valued function f(u) is
star shaped when there exists a center point, u0 ∈ IRn
, such that every point in S lies on
a unique ray emanating from u0. In particular, convex level sets are always star shaped.
Since T(γ(u)) is monotonically increasing along rays emanating from u∗, its level sets form
star shaped sets as illustrated in the following example.
Example: Consider the polygonal environment depicted in Figure 3.8(a). The safe time op-
timal path connecting pS and pT in this environment, α∗, crosses the Voronoi arcs e1 and e2.
The piecewise time optimal paths connecting pS and pT while crossing e1 and e2 are param-
eterized by their Voronoi crossing points p(u1) and p(u2), where (u1, u2) ∈ [0, 1] × [0, 1]. As
shown in Figure 3.8(b), the contours of T(γ(u1, u2)) in the (u1, u2) plane are star shaped
(and convex), with a center point at the optimal parameter values (u∗
1, u∗
2) corresponding to
α∗. ◦
Convex optimization algorithms require that the level sets of T(γ(u)) be convex. The
same algorithms can be applied when the level sets are star shaped with center point at the
optimal parameter values u∗. This observation is based on the relation T(γ(u))·(u−u∗) ≥ 0
76](https://image.slidesharecdn.com/0468cf6d-4b62-44c0-8a22-ec19452dbac9-150703153321-lva1-app6892/85/Manor_final-83-320.jpg)
![9 10 11 12 13 14
9
10
11
12
13
14
15
1u
2u
*
1u
*
2u
Sp
Tp
*
2u
*
1u
(a) (b)
iO
*
1e
2e
1u
2u
* *
1 2( , )u u
Figure 3.8: (a) The piecewise time optimal paths connecting pS and pT while crossing the
Voronoi arcs e1 and e2 are parametrized by (u1, u2). (b) The contours of T(γ(u1, u2)) are
star shaped (and convex) with center at (u∗
1, u∗
2).
satisfied by star shaped level sets, which allows construction of a barrier function (or a bound-
ing ellipsoid) in each iteration of the convex optimization process. Standard convex opti-
mization algorithms run in O(n3 log(R/ )) time, where R is the maximal pointwise distance
between the initial path αo and the exact time optimal path α∗, and is the desired solution
accuracy [51]. Note that the number of Voronoi crossing points, n, is upper bounded by
the number of obstacle features in the environment. Here, too, we used Newton-Raphson’s
method in the optimization of T(γ(u)) as next explained.
3.6.1 Path Optimization Using Newton’s Method
Recall that the time optimal path α∗ is known to have a continuously varying tangent
(Prop. 3.4.3). Thus, the computation of the optimal crossing points p1, ..., pn which lie on
their respective Voronoi arcs e1, . . . , en an be also reduced into a system of ¯n equations in ¯n
variables:
S(p1, ..., p¯n) =
Ang(α1(p1) − α2(p1))
...
Ang(α¯n(p¯n) − α¯n+1(p¯n))
= 0, (3.19)
where pi ∈ ei for i = 1...¯n and Ang(αi(pi) − αi+1(pi)) is angle between the tangents of the
two path segments meeting at pi. The expression for αi is derived by differentiating the time
77](https://image.slidesharecdn.com/0468cf6d-4b62-44c0-8a22-ec19452dbac9-150703153321-lva1-app6892/85/Manor_final-84-320.jpg)
![optimal path given in Section 3.3 for point and wall segment obstacles. For a point obstacle,
the time optimal path is parameterized by polar coordinates, α(θ) = (r(θ) cos θ, r(θ) sin θ)
for θ ∈ [θS, θT ], and the derivative dr/dθ at pi is given by,
dr
dθ
(pi) = ri
ri
2ac2
− 1, (3.20)
where ri is the distance measured from the point obstacle to the crossing point pi, and c
is determined by the path’s end conditions. For a wall segment obstacle, the time optimal
path is parameterized by cartesian coordinates, α(x) = (x, y(x)) for x ∈ [xS, xT ], and the
derivative dy(x)/dx at pi is given by,
dy
dx
(pi) =
1
2ac2piy
− 1, (3.21)
where pi = (pix, piy) is the crossing point and c is determined by the path’s end condi-
tions. Note that (3.19) requires that (3.20) and (3.21) be expressed in a common system of
coordinates.
Based on the expressions for the path derivatives, the optimal crossing points, p∗
1, ..., p∗
n,
can be computed using any numerical method, such as Newton’s method. At the i’th iteration
of Newton’s method, a new set of crossing points, pi = (pi
1, ..., pi
¯n), is computed. Let Ei =
maxj=1...¯n{|p∗
j − pi
j|} be the i’th error measure and denote by J the ¯n × ¯n Jacobian matrix
of S(p1, ..., p¯n). When the initial guess, p0
1, ..., p0
¯n, is sufficiently close to the optimal solution,
quadratic asymptotic convergence is guaranteed, |Ei+1| κ|Ei|2, where κ is a positive
constant defined by properties of S(p1, ..., p¯n) ([34], p.115). When J has a full rank and
(3.19) is known to have a unique solution, the method is robust with respect to the exact
position of the initial crossing points p0
1, ..., p0
¯n. Each iteration of Newton’s method requires
O(¯n3) steps, and in practice it suffices to run 5-10 iterations in order to generate a practically
accurate path. While lacking a global run time bound, it is easier to implement and rapidly
converges to the exact solution as indicated in the following example.
Example: Figure 3.9 depicts a disc robot navigating in a 10 × 10 meters room populated
by four point obstacles. The speed graph of the environment is depicted in Figure 3.9(a).
Next, let pS and pT be located at two speed graph nodes as indicated in Figure 3.9(a). The
78](https://image.slidesharecdn.com/0468cf6d-4b62-44c0-8a22-ec19452dbac9-150703153321-lva1-app6892/85/Manor_final-85-320.jpg)
![1 1.5 2 2.5 3 3.5 4 4.5 5 5.5 6
5.77
5.775
5.78
5.785
5.79
5.795
5.8
5.805
5.81
5.815
5.82
Iteration
Traveltime
1 2 3
travel time [sec]
Iteration
1 1.5 2 2.5 3 3.5 4 4.5 5
7.6
7.7
7.8
7.9
8
8.1
8.2
8.3
*
0
54
Figure 3.10: The Newton-Raphson’s travel time convergence during five iterations on the
paths of Figure 3.9.
number of vertices on the speed graph [23]. The total number of edges, e, is upper bounded
by v(v−1)/2. Since each vertex of the speed graph is located on an individual Voronoi edge,
the total number of vertices, v, equals the number of Voronoi edges in the environment, which
is linear in the number of the polygonal obstacles’ features, denoted f [61]. Hence, the k-best
time optimal paths on the speed graph can be computed in O(f2 +klog(k)) steps. Note that
when computing the k-best paths on the speed graph, two paths on the graph from the same
homotopy class may be selected. Therefore, it is necessary to eliminate paths from repeating
homotopy classes.
3.7 Numerical Simulations and Experimental Setup
This section describes simulations of the speed graph method in environments that resemble
an industrial warehouse and an office floor. The section concludes with a brief mention of
initial experiments, highlighting certain weaknesses of current mobile robot platforms.
Warehouse environment: Figure 3.11 depicts a warehouse containing eight storage racks,
O1, . . . , O8, surrounded by corridors. The warehouse measures 30 × 32 meters, while each
storage rack measures 5×10 meters with one meter clearance between neighboring racks. The
outer corridors are four meters wide, except the rightmost corridor which is seven meters
wide. A cylindrical mobile robot is serving a delivery counter located at the warehouse’s
lower right corner. The robot has a diameter of 80 cm, and its maximal deceleration has
80](https://image.slidesharecdn.com/0468cf6d-4b62-44c0-8a22-ec19452dbac9-150703153321-lva1-app6892/85/Manor_final-87-320.jpg)
![Path α1 α2 α3 α4 α5 α6
T(α) [sec] 27.9 33.8 43.6 47.7 54.3 57.4
Table 3.1: The safe time optimal travel time in the six best homotopy classes in the of-
fice floor environment.
homotopy class in terms of safe travel time.
A start point pS was selected at the lower left corner of the conference room, while a target
point pT was set at the lower chair in the neighboring office. The speed graph was computed
for this environment, and its minimum cost path initialized a computation of the safe time
optimal path α∗ depicted in Figure 3.13(a). Next, the six-best path homotopy classes in
terms of total travel time were computed along the speed graph, using a standard k-best
graph search algorithm [23]. For each of these path homotopy classes, the speed graph paths
initialized a computation of the safe time optimal path connecting pS to pT in the respective
path homotopy class. These paths are marked as α1, . . . , α6 in Figure 3.13(b), and their
travel times are listed in Table 3.1. The path homotopy class suggested by the speed graph
contains the time optimal path among all six homotopy classes, α1 = α∗, with total travel
time of 27.9 seconds. As a comparison, the next best path homotopy class happened to be
associated with the geometrically shortest path connecting pS to pT . The travel time along
the second best path, α2 = ¯α∗, is 33.8 seconds. The speed graph thus captured the time
optimal path homotopy class, as well as correctly ranked the k-best path homotopy classes.
Initial Experiments: An experimental validation of the speed graph method is challenging
for several reasons. First, the experiments require a truly fast mobile robot in order to
demonstrate the practical advantage of traveling along safe time optimal paths. However,
readily available platforms such as the iRobot Create can only move with maximal speeds of
about 1 m/sec [1]. Second, the speed graph stored in the robot’s on-board memory requires
accurate positioning, a notoriously hard problem in congested indoor environments. For
instance, the iRobot sensors include a front bumper that detects obstacle collision, and an
IR sensor that detects obstacles in the robot’s close vicinity [1]. However, the iRobot has no
global position sensor, and its position is estimated by odometric sensors mounted on the
robot’s wheels.
As a preliminary demonstration, we placed an iRobot platform at a docking station in
the ground floor of our office building. The robot was charged with the task of serving
83](https://image.slidesharecdn.com/0468cf6d-4b62-44c0-8a22-ec19452dbac9-150703153321-lva1-app6892/85/Manor_final-90-320.jpg)
![tion of the path smoothing process as a convex optimization problem. As demonstrated in
simulated environments, the speed graph suggests the path homotopy class containing the
globally time optimal path, as well as provides an initial path whose positional optimization
yields the safe time optimal path within the chosen path homotopy class.
Note that although not directly addressed by the calculus of variations formulation, the
turning radius constraint that prevents sliding events is satisfied by these paths. This could
be easily validated by computing the paths’ curvatures as suggested in Appendix D, then
comparing them with the minimal safe turning radius that prevents sliding (Eq. A.2).
The speed graph method is only a first step toward practical safe high speed mobile robot
navigation. Let us point out two extensions to this work currently under investigation. The
first extension concerns environments containing static as well as dynamic obstacles. For
instance, multiple mobile robots operating autonomously in industrial warehouses must be
able to move with high speed while maintaining braking safety constraints with respect to
all group members.
A second extension concerns the generalization of the circular braking safety zone into
a safety zone whose shape and size depends on the robot’s speed as well as heading. For
instance, when a mobile robot travels with high speed in a long narrow corridor, a non-
circular safety zone aligned with the corridor’s axis will allow the robot significantly higher
safe speeds than the conservative circular safety zone. A preliminary work on mobile robots
navigating under non-uniform braking safety constraints has recently appeared in [49].
85](https://image.slidesharecdn.com/0468cf6d-4b62-44c0-8a22-ec19452dbac9-150703153321-lva1-app6892/85/Manor_final-92-320.jpg)
![safety as a state dependant constraint, which in turn allows formulation of the safe time
optimal navigation problem using calculus of variations. The work presented here, analyzes
the properties of the travel time functional, T(α), then uses these properties to develop the
speed graph method which computes safe time optimal paths in environments populated by
polygonal obstacles. The speed graph consists of safe time optimal arcs connecting special
via points located on the Voronoi diagram of the environment. The speed graph is used to
efficiently select the most promising path homotopy class connecting the start and target
in the environment. Convexity properties of T(α) are then used to compute the safe time
optimal path within the candidate homotopy class as a convex optimization problem in
O(n3 log(1/ )) time, where is the desired solution accuracy.
Importantly, the two methods take place in the robot’s state space, thus treating the
geometric path planning and the speed profiling in a single framework. Table 4.1 compares
the two methods and indicates the advantages of each one of them.
Comparison Parameter VC Method Speed Graph Method
Output Produces a pseudo time opti-
mal path based on a connec-
tivity 3D network
Computes the time optimal
path based on a planar graph
Complexity running
time
O(n log n) O(n3 log(1/ ))
Safety constraints Guarantees safety in two steps Safety constraints are satis-
fied by a single computational
scheme
Mathematical foun-
dations
Morse theory and topology Calculus of variations and
convex optimization
Main advantage Low computational running
time
Computes the time optimal
path
Table 4.1: Comparison between the VC method and the Speed Graph method.
Next, we briefly discuss our current work which do not appear in this thesis but extends
its ideas to more real scenarios.
4.2 Current Work
Piloting along a high speed and curved path, as car racing drivers do, without the need to
maintain uniform braking safety clearance (see Figure 4.1 for our high speed inspiration
[12]), often results in a very high speed path which enforces an active speed dependant
88](https://image.slidesharecdn.com/0468cf6d-4b62-44c0-8a22-ec19452dbac9-150703153321-lva1-app6892/85/Manor_final-95-320.jpg)
![A second extension concerns the generalization of the circular braking safety zone into
a safety zone whose shape and size depends on the robot’s speed as well as heading. For
instance, when a mobile robot travels with high speed in a long narrow corridor, a non-
circular safety zone aligned with the corridor’s axis will allow the robot significantly higher
safe speeds than the conservative circular safety zone. A preliminary work on mobile robots
navigating under non-uniform braking safety constraints has recently appeared in [49].
Despite all this, note that the uniform braking safety which is considered in this thesis
is useful when the robot has to navigate in close proximity to adjacent obstacles and is
required to keep a sufficiently low speed to avoid collision with other moving agents or other
unexpected hazards. In such scenarios requiring uniform braking safety will ensure that the
robot can avoid these hazards subject to its sensing capabilities.
A third extension concerns the generalization of the planar disc robot to other types of
moving agents such as motor vessels piloting autonomously in dense commercial harbours.
These vessels are subjected to different dynamic constraints such as hydrodynamic resistance
and vessel’s heel that limits the boat’s handling through the water. In order to be safely
piloted autonomously these constraints should be taken into account when planning the high
speed boat’s path. An efficient on-line algorithm which regorosly ensure safety should be
implemented using the convexity tools developed by this thesis.
90](https://image.slidesharecdn.com/0468cf6d-4b62-44c0-8a22-ec19452dbac9-150703153321-lva1-app6892/85/Manor_final-97-320.jpg)
![Appendix A
The Turning Radius Constraint
This appendix develops an expression for the robot’s minimal turning radius, rmin(ν), as
a function of the robot’s parameters. We shall assume the common case of a four-wheeled
robot. However, the expression for rmin(ν) can be easily adapted to other mobile robots.
The three main constraints affecting a mobile robot turning radius are as follows. First, the
turning radius must respect the robot’s maximal steering angles. Second, the centrifugal
force affecting the robot must not incur radial sliding of the robot wheels. Third, the lateral
moment affecting the robot must be sufficiently low as to prevent tip-over during turning
maneuvers. The three constraints are next discussed.
We will use the following notations. The average steering angle of the robot’s front wheels
is denoted ω (Figure A.1(a)). This angle can vary in the range [−ωmax, ωmax]. The robot’s
wheels are mounted at the vertices of a rectangular frame having length l and width w (Fig-
ure A.1(a)). The robot’s center of mass is located at a height h above ground (Figure A.1(b)).
The robot’s turning radius, denoted r, is the current radius of curvature of the curve traced
by the robot’s center point. The robot’s inner turning radius, denoted rin, is the current
radius of curvature of the curve followed by the robot’s innermost wheel (Figure A.1(a)).
The kinematic turning radius constraint: Based on simple trigonometry, the average
steering angle ω is related to l, w, and rin by the expression:
tan ω =
l
w
2 + rin
.
92](https://image.slidesharecdn.com/0468cf6d-4b62-44c0-8a22-ec19452dbac9-150703153321-lva1-app6892/85/Manor_final-99-320.jpg)
![
inr
CMr
l
. . .c o m
mg
CFf
sf sf
tip
left turn
(b)(a)
h
w
w
Figure A.1: (a) Top view of a four-wheel mobile robot turning geometry. (b) Rear view of
a left turning mobile robot.
The center point turning radius is given by
r =
l2
4
+
w
2
+ rin
2
.
Replacing w/2 + rin with l/ tan ω gives the turning radius:
r =
l2
4
+
l
tan ω
2
ω ∈ [−ωmax, ωmax].
Since r is monotonically decreasing in |ω|, the minimal feasible turning radius, denoted rkin,
is attained at ω = ±ωmax. Thus r ≥ rkin, where
rkin =l
1
4
+
1
tan2(ωmax)
. (A.1)
The sliding constraint: Sliding occurs when the centrifugal force acting on the robot ex-
ceeds the radial friction force acting between the wheels and the ground. The net centrifugal
force, denoted fr, is given by
fr =
mν2
r
,
where m is the robot’s mass, ν is the robot’s linear velocity magnitude, and r is the robot’s
turning radius. The radial friction force acting between the wheels and the ground, denoted
fs, is given by
fs = µfN = µmg,
93](https://image.slidesharecdn.com/0468cf6d-4b62-44c0-8a22-ec19452dbac9-150703153321-lva1-app6892/85/Manor_final-100-320.jpg)
![Appendix B
The Time Optimal Path for a Disc
Robot
This appendix considers a disc robot of radius ρ traveling near a point obstacle located
at the origin of the (x, y) plane. Using polar coordinates, the path traced by the robot’s
center is parameterized by α(θ) = (r(θ) cos θ, r(θ) sin θ) for θ ∈ [θS, θT ], with the endpoint
conditions r(θS) = rS and r(θT ) = rT . The distance between the disc robot and the point
obstacle, denoted dr(θ), is given by dr(θ) = r(θ) − ρ along α. Note that dr(θ) must be
non-negative for all θ ∈ [θS, θT ] in order to prevent collision. Braking safety requires that
dr(θ) ≥ d(v(θ)). Substituting d(ν) = ν2/2amin and using the maximal allowed speed for
travel time minimization gives the constraint:
ν(r) = 2amin(r − ρ) r ≥ ρ. (B.1)
The travel time functional T(α) is thus given by
T(α) =
θT
θS
F r(θ), r (θ) dθ,
where
F(r, r ) =
r2 + r 2
2amin(r − ρ)
. (B.2)
96](https://image.slidesharecdn.com/0468cf6d-4b62-44c0-8a22-ec19452dbac9-150703153321-lva1-app6892/85/Manor_final-103-320.jpg)
![located on the boundary of a common Voronoi cell of a polygonal obstacle Oi (Figure C.1).
Each speed graph edge is the safe time optimal path connecting p1 and p2 in F. Suppose
m +1 proximity cells separate between p1 and p2 (three such cells separate p1 and p2 in
Figure C.1). The boundaries between adjacent proximity cells form line segments denoted lj
for j = 1, . . . , m. The computation will focus on the piecewise time optimal paths connecting
p1 and p2, consisting of time optimal path segments within each proximity cell and corner
points that can freely vary along the proximity cells’ boundary lines. The corner points
are parameterized by p(uj) : [0, 1] → lj for j = 1, . . . , m. As discussed in Section 3.5, the
safe travel time functional T(α) becomes a function of u1, . . . , um. To compute the time
optimal path between p1 and p2 (and hence the speed graph edge), we will use the following
monotonic property of the travel time functional T(α) .
Lemma 3.5.1. Let γ(u1, . . . , um) parameterize the piecewise time optimal path segment con-
necting the nodes p1 and p2 in F while crossing the proximity cell boundary lines l1, . . . , lm.
Let u∗ = (u∗
1, . . . , u∗
m) be the parameter values of the optimal crossing points. The functional
T(γ(u1, . . . , um)) is monotonically increasing along any ray emanating from u∗ in the set
(u1, . . . , um) ∈ [0, 1] × · · · × [0, 1].
The proof of Lemma 3.5.1 is similar to the proof of the Prop. 3.6.1 with minor changes
in notations. The path γ is replaced with α and the crossing points p(ui) for i = 1...m or n
move from the proximity cell boundary lines denoted lj onto the Voronoi arcs denoted ej.
Computation of the safe time optimal path: The speed graph method identifies a can-
didate path homotopy class connecting pS and pT in F, then determines which Voronoi arcs
must be crossed by the safe time optimal path in the chosen path homotopy class. Denote
by e1, . . . , en the latter Voronoi arcs (two such arcs, e1 and e2, appear in Figure C.2(a)). The
computation of the safe time optimal path will focus on the piecewise time optimal paths that
cross the Voronoi arcs e1, . . . , en at points that can freely vary along these arcs. The Voronoi
crossing points are parameterized by p(ui) : [0, 1] → ej for i = 1, . . . , n (Figure C.2(a)).
As discussed in Section 3.6, the safe travel time functional T(α) becomes a function of the
parameters u1, . . . , un. The computation of the safe time optimal path will use the following
monotonicity property of T(α).
Proposition 3.6.1. Let α(u1, . . . , un) parameterize the piecewise time optimal paths con-
100](https://image.slidesharecdn.com/0468cf6d-4b62-44c0-8a22-ec19452dbac9-150703153321-lva1-app6892/85/Manor_final-107-320.jpg)
![1( )p u
*
1( )p u
2( )p u
Sp
Tp
Voronoi diagram
*
t
Sp
Tp
1e
2e
*
2( )p u
1e
2e
tangent to 1e
tangent to 2e
1( )
sv
(a) (b)
( )t s
iO iO
bisector
bisector
1- ( )
sv
2( )
sv
2- ( )
sv
Figure C.2: (a) The safe time optimal path α∗ and a variant path αt. (b) The sector spanned
by −v(s−
i ) and v(s+
i ) locally contains the Voronoi arc tangent line at p(ui) for i = 1, 2.
necting pS and pT in F while crossing the Voronoi arcs e1, . . . , en. Let u∗ = (u∗
1, . . . , u∗
n) be
the parameter values of the optimal Voronoi crossing points. The functional T(α(u1, . . . , un))
is monotonically increasing along any ray emanating from u∗ in the set (u1, . . . , un) ∈
[0, 1] × · · · × [0, 1].
Proof: The piecewise time optimal paths connecting pS and pT in F will be parameterized
by a fixed division of the unit interval into sub-intervals [0, s1], [s1, s2], . . . , [sn, 1], such that
the Voronoi crossing points of these paths will correspond to s = s1, . . . , sn. Let α∗ denote the
safe time optimal path connecting pS and pT in F while crossing the Voronoi arcs e1, . . . , en.
A Voronoi path variation of α∗ is a differentiable mapping H(s, t) : [0, 1] × [0, 1] → F, such
that H(s, 0) = α∗(s) for s ∈ [0, 1], with fixed endpoints H(0, t) = pS and H(1, t) = pT for
t ∈ [0, 1]. Each path in this variation, denoted αt, corresponds to a particular value of t
(Figure C.2(a)). As t increases from t = 0 in the unit interval, the Voronoi crossing points
of αt vary along the Voronoi arcs e1, . . . , en. When H(s, t) represents a ray emanating from
u∗, the Voronoi crossing points of αt move away from those of α∗ along the Voronoi arcs as
t increases in the unit interval.
When the travel time functional T(α) is evaluated along the paths of H(s, t) it becomes
a function of t, T(t) =
1
0 F (αt(s), αt(s)) ds. All paths of H(s, t) cross the Voronoi arcs at
s = s1, ..., sn, and T(t) can be equivalently written as the sum:
T(t) =
n
i=0
si+1
si
F αt(s), αt(s) ds,
101](https://image.slidesharecdn.com/0468cf6d-4b62-44c0-8a22-ec19452dbac9-150703153321-lva1-app6892/85/Manor_final-108-320.jpg)
![Further substitution of dy/dx and d2y/dx2 in (D.2) according to the optimal path’s
formula, then comparing with the safe turning radius, would suggest that for every y(x) ≤
1/(2aminc2) the turning radius constraint is kept and the robot can safely follow this optimal
path segment without the risk of sliding. The constant amin = µg is the robot’s maximal
deceleration and c is a parameter determined by the path’s end conditions.
Next, observe the expression for dy/dx for this path segment:
dy(x)
dx
=
1
2aminc2y(x)
− 1. (D.3)
Equation D.3 implies that this optimal path segment is only valid when y(x) ≤ 1/(2aminc2)
and hence we can conclude that ρc ≥ rslide. Next, consider the optimal path segment located
near a single point obstacle.
A single point obstacle: Consider a time optimal path segment located near a single point
obstacle. This optimal path segment was developed in Section 3.3 and is given by a parabolic
curve formulated in polar coordinates (3.12). However, to show that this path segment
maintains the safe turning radius constraint we shale formulate it in cartesian coordinates
as follows:
y(x) =
x2 + y2
0
2y0
for x ∈ [xS, xT ],
where y0 is the minimal distance between the obstacle and the imaginary wall segment that
creates the parabolic optimal curve. Note that the x axis merges with that imaginary wall.
The safe speed along this optimal path segment is given by,
ν(x) =
µg(x2 + y2
0)
y0
,
and therefore the turning radius that will evoke sliding while navigating with this speed is:
rslide(ν(x)) =
ν2(x)
µg
=
x2 + y2
0
y0
.
Requiring that the path’s curvature ρc, derived according to (D.2), would be equal or
105](https://image.slidesharecdn.com/0468cf6d-4b62-44c0-8a22-ec19452dbac9-150703153321-lva1-app6892/85/Manor_final-112-320.jpg)
![Bibliography
[1] The iRobot Create owners guide. www.irobot.com.
[2] GUARDIUM-UGV. G-NIUS Unmanned Ground Systems, accesssed Jan. 2012.
http://g-nius.co.il.
[3] GUSS, a ground unmanned support surrogate. The Naval Surface Warfare Center and
TORC Robotics, accesssed Jan. 2012. http://www.torcrobotics.com/ground-unmanned-
support-surrogate.
[4] Traffic jam assistance. Volvo Car Group,
accesssed May. 2013. https://www.media.volvocars.com/global/enhanced/en-
gb/media/preview.aspx?mediaid=46386.
[5] A.P. Aguiar and J.P. Hespanha. Trajectory-tracking and path-following of underactu-
ated autonomous vehicles with parametric modeling uncertainty. IEEE Transactions
on Automatic Control, 52:1362–1379, 2007.
[6] K.J. Astr¨om and R.M. Murray. Feedback systems: an introduction for scientists and
engineers. Princeton university press, 2010.
[7] F. Aurenhammer and R. Klein. Voronoi diagrams. In Handbook of Computational
Geometry, chapter 5, pages 201–290. Elsevier Science, 2000.
[8] F. Aurenhammer, R. Klein, and D.T. Lee. Voronoi Diagrams and Delaunay Triangula-
tions. World Scientific, 2013.
[9] J.E. Bobrow, S. Dubowsky, and J. Gibson. Time-optimal control of robotic manipulators
along specified paths. The International Journal of Robotics Research, 4:3–17, 1985.
107](https://image.slidesharecdn.com/0468cf6d-4b62-44c0-8a22-ec19452dbac9-150703153321-lva1-app6892/85/Manor_final-114-320.jpg)
![[10] J. Borenstein and Y. Koren. The vector field histogram - fast obstacle avoidance for
mobile robots. IEEE Transactions on Robotics and Automation, 7:278–288, 1991.
[11] C.B. Boyer and U.C. Merzbach. A History of Mathematics. New York: Wiley, 1991.
[12] F. Braghin, F. Cheli, S. Melzi, and E. Sabbioni. Race driver model. Computers
Structures, 86:1503–1516, 2008.
[13] O. Brock and O. Khatib. High-speed navigation using the global dynamic window
approach. In IEEE International Conference on Robotics and Automation, 341–346,
1999.
[14] A.E. Bryson. Applied optimal control: optimization, estimation and control. CRC Press,
1975.
[15] F. Bullo and R.M. Murray. Tracking for fully actuated mechanical systems: A geometric
framework. Automatica, 35:17–34, 1999.
[16] H. Chitsaz and S.M. LaValle. Time-optimal paths for a dubins airplane. In 46th IEEE
Conference on Decision and Control, 2379–2384, 2007.
[17] H. Choset. Coverage for robotics a survey of recent results. Annals of Mathematics
and Artificial Intelligence, 31:113–126, 2001.
[18] H. Choset, K.M. Lynch, S. Hutchinson, G. Kantor, W. Burgard, L.E. Kavraki, and
S. Thrun. Principles of Robot Motion. MIT Press, Cambridge MA, 2005.
[19] T.H. Chung, G.A. Hollinger, and V. Isler. Search and pursuit-evasion in mobile robotics,
a survey. Autonomous Robots, 31:299–316, 2011.
[20] F.H. Clarke. Optimization and Nonsmooth Analysis. SIAM Publication, 1990.
[21] B. Donald, P. Xavier, J. Canny, and J. Reif. Kinodynamic motion planning. Journal of
The ACM, 40:1048–1066, 1993.
[22] L.E. Dubins. On curves of minimal length with a constraint on average curvature,
and with prescribed initial and terminal positions and tangents. American Journal of
Mathematics, 497–516, 1957.
108](https://image.slidesharecdn.com/0468cf6d-4b62-44c0-8a22-ec19452dbac9-150703153321-lva1-app6892/85/Manor_final-115-320.jpg)
![[23] D. Eppstein. Finding the k shortest paths. SIAM Journal on Computing, 28:652–673,
1998.
[24] P. Fiorini and Z. Shiller. Motion planning in dynamic environments using velocity
obstacles. The International Journal of Robotics Research, 17:760–772, 1998.
[25] S. Fleury, P. Soueres, J.P. Laumond, and R. Chatila. Primitives for smoothing mobile
robot trajectories. IEEE Transactions on Robotics and Automation, 11:441–448, 1995.
[26] D. Fox, W. Burgard, and S. Thrun. The dynamic window approach to collision avoid-
ance. IEEE Robotics Automation Magazine, 4:23–33, 1997.
[27] T. Fraichard. A short paper about motion safety. In IEEE International Conference on
Robotics and Automation, 1140–1145, 2007.
[28] S.S. Ge and Y.J. Cui. Dynamic motion planning for mobile robots using potential field
method. Autonomous Robots, 13:207–222, 2002.
[29] I. M. Gelfand and S.V. Formin. Calculus of Variations. Prentice-Hall, 1963.
[30] M. Goresky and R. MacPherson. Stratified Morse Theory. Springer Verlag, New York,
1980,.
[31] B. H´eriss´e and R. Pepy. Shortest paths for the dubins’ vehicle in heterogeneous en-
vironments. In 52nd IEEE Annual Conference on Decision and Control, 4504–4509,
2013.
[32] D. Hsu, R. Kindel, J.C. Latombe, and S. Rock. Randomized kinodynamic motion plan-
ning with moving obstacles. The International Journal of Robotics Research, 21:233–255,
2002.
[33] K. Iagnemma, S. Shimoda, and Z. Shiller. Near-optimal navigation of high speed mobile
robots on uneven terrain. In IEEE/RSJ Int. Conference on Intelligent Robots and
Systems (IROS), 4098–4103, 2008.
[34] E. Isaacson and H.B. Bishop. Analysis of numerical methods. Dover, 1994.
[35] K. Kant and S.W. Zucker. Toward efficient trajectory planning: The path-velocity
decomposition. The International Journal of Robotics Research, 5:72–89, 1986.
109](https://image.slidesharecdn.com/0468cf6d-4b62-44c0-8a22-ec19452dbac9-150703153321-lva1-app6892/85/Manor_final-116-320.jpg)
![[36] S. Karaman and E. Frazzoli. Sampling-based algorithms for optimal motion planning.
International Journal of Robotics Research, 30:846–894, 2011.
[37] R. Kindel, D. Hsu, J.C. Latombe, and S. Rock. Kinodynamic motion planning amidst
moving obstacles. In IEEE International Conference on Robotics and Automation, 537–
543, 2000.
[38] Y. Koren and J. Borenstein. Potential field methods and their inherent limitations for
mobile robot navigation. In IEEE International Conference on Robotics and Automa-
tion, 2:1398–1404, 1991.
[39] F. Large, D. Vasquez, T. Fraichard, and C. Laugier. Avoiding cars and pedestrians
using v-obstacles and motion prediction. In The IEEE Intelligent Vehicle Symp., 2004.
[40] J.C. Latombe. Robot Motion Planning. Kluwer Academic Publishers, 1991.
[41] S.M. Lavalle. Rapidly-exploring random trees: A new tool for path planning. Technical
report, Dept. of Computer Science, Iowa State University, 1998.
[42] S.M. LaValle and J.J. Kuffner. Randomized kinodynamic planning. In IEEE Interna-
tional Conference on Robotics and Automation, 1:473–479, 1999.
[43] S.M. LaValle and J.J. Kuffner. Randomized kinodynamic planning. The International
Journal of Robotics Research, 20:378–400, 2001.
[44] S.M. LaValle and J.J. Kuffner. Rapidly-exploring random trees: progress and prospects.
In Algorithmic and Computational Robotics: New Directions, 293–308. Wellesley, MA,
2001.
[45] M. Lepetic, G. Klancar, I. Skrjanc, D. Matko, and B. Potocnik. Time optimal path
planning considering acceleration limits. Robotics and Autonomous Systems, 45:199–
210, 2003.
[46] T. Lozano-Perez. Spatial planning: A configuration space approach. IEEE Transactions
on Computers, C-32:108–120, 1983.
[47] G. Manor and E. Rimon. High-speed navigation of a uniformly braking mobile robot us-
ing position-velocity configuration space. In IEEE International Conference on Robotics
and Automation, 193–199, 2012.
110](https://image.slidesharecdn.com/0468cf6d-4b62-44c0-8a22-ec19452dbac9-150703153321-lva1-app6892/85/Manor_final-117-320.jpg)
![[48] G. Manor and E. Rimon. Vc-method: high-speed navigation of a uniformly braking
mobile robot using position-velocity configuration space. Autonomous Robots, 34:295–
309, 2013.
[49] G. Manor and E. Rimon. The speed graph method: Time optimal navigation among ob-
stacles subject to safe braking constraint. In IEEE International Conference on Robotics
and Automation, 1155–1160, 2014.
[50] J. Markoff. Google cars drive themselves, in traffic. The New York Times, October 9th
2010.
[51] Y. E. Nesterov and A. S. Nemirovsky. Interior Point Polynomial Methods in Convex
Programming: Theory and Applications. Springer Verlag, New York, 1992.
[52] V. Nieuwstadt, J. Michael, and R.M. Murray. Real time trajectory generation for dif-
ferentially flat systems. California Institute of Technology, 1997.
[53] L. Nirenberg. Variational and topological methods in nonlinear problems. Bulletin of
the AMS, 4:267–302, 1981.
[54] C. O’D´unlaing and C. K. Yap. A retraction method for planning the motion of a disc.
Journal of Algorithms, 6:104–111, 1985.
[55] M.G. Park, J.H. Jeon, and M.C. Lee. Obstacle avoidance for mobile robots using arti-
ficial potential field approach with simulated annealing. In IEEE International Sympo-
sium on Industrial Electronics, 3:1530–1535, 2001.
[56] R. Parthasarathi and T. Fraichard. An inevitable collision state-checker for a car-like
vehicle. In IEEE Intenrational Conference on Robotics and Automation, 3068–3073,
2007.
[57] S. Petti, T. Fraichard, and I. Rocquencourt. Safe motion planning in dynamic envi-
ronments. In IEEE/RSJ International Conference on Intelligent Robots and Systems
(IROS), 3726–3731, 2005.
[58] J. Reeds and L. Shepp. Optimal paths for a car that goes both forwards and backwards.
Pacific Journal of Mathematics, 145:367–393, 1990.
111](https://image.slidesharecdn.com/0468cf6d-4b62-44c0-8a22-ec19452dbac9-150703153321-lva1-app6892/85/Manor_final-118-320.jpg)
![[59] E. Rimon. Exact Robot Navigation Using Artificial Potential Functions. PhD thesis,
Yale University, New Haven CT, Dec 1990.
[60] E. Rimon and D.E. Koditschek. Exact robot navigation using artificial potential func-
tions. IEEE Transactions on Robotics and Automation, 8:501–518, 1992.
[61] J.R. Sack and J. Urrutia, editors. Handbook of Computational Geometry. Elsevier, 1999.
Chapter 5.
[62] M. I. Shamos. Computational Geometry. PhD thesis, Yale University, 1978. Section
6.3.
[63] I. Sherr and M. Ramsey. Toyota and audi move closer to driverless cars, The Wall Street
Journal, Jan. 3 2013.
[64] Z. Shiller and Y. R. Gwo. Dynamic motion planning of autonomous vehicles. IEEE
Transactions on Robotics and Automation, 7:241–249, 1991.
[65] Z. Shiller and H.H. Lu. Computation of path constrained time optimal motions with
dynamic singularities. Journal of dynamic systems, measurement, and control, 114:34–
40, 1992.
[66] Z. Shiller, S. Sharma, I. Stern, and A. Stern. Online obstacle avoidance at high speeds.
The International Journal of Robotics Research, 32:1030–1047, 2013.
[67] S. Shimoda, Y. Kuroda, and K. Iagnemma. Potential field navigation of high speed
unmanned ground vehicles on uneven terrain. In IEEE International Conference on
Robotics and Automation, 2828–2833, 2005.
[68] R. H. Smith. Analyzing Friction in the Design of Rubber Products and Their Paired
Surfaces. CRC Press, 2008.
[69] J. Snape, J. van den Berg, and S. J. Guy. The hybrid reciprocal velocity obstacle. IEEE
Transactions on Robotics and Automation, 27:696–706, 2011.
[70] K.C. Teixeira, M. Becker, and G.A.P. Caurin. Automatic routing of forklift robots in
warehouse applications. In ABCM Symposium in Mechatronics, 4:335–344, 2010.
112](https://image.slidesharecdn.com/0468cf6d-4b62-44c0-8a22-ec19452dbac9-150703153321-lva1-app6892/85/Manor_final-119-320.jpg)
![[71] I. Ulrich and J. Borenstein. Vfh+: Reliable obstacle avoidance for fast mobile robots.
In International Conference on Robotics and Automation, 2:1572–1577, 1998.
[72] E. Velenis and P. Tsiotras. Optimal velocity profile generation for given acceleration
limits: theoretical analysis. In The American Control Conference, 2:1478–1483, 2005.
[73] J. Villagra, V. Milanes, J. Perez, and J. Godoy. Smooth path and speed planning for
an automated public transport vehicle. Robotics and Autonomous Systems, 2012.
[74] W. Weigo, C. Huitang, and W. Peng-Yung. Time optimal path planning for a wheeled
mobile robot. Journal of Robotic Systems, 17:585–591, 2000.
[75] R. Wein, J. van den Berg, and D. Halperin. Planning high-quality paths and corridors
amidst obstacles. International Journal of Robotics Research, 27:1213–1231, 2008.
[76] E. Welzl. Constructing the visibility graph for n-line segments in o(n2) time. Information
Processing Letters, 20:167–171, 1985.
[77] D.K. Wilde. Computing clothoid segments for trajectory generation. In IEEE/RSJ
International Conference on Intelligent Robots and Systems (IROS), 2440–2445, 2009.
[78] D. Wilkie, J.V. den Berg, and D. Manocha. Generalized velocity obstacles. In IEEE/RSJ
International Conference on Intelligent Robots and Systems (IROS), 2009.
[79] P.R. Wurman, R. D’Andrea, and M. Mountz. Coordinating hundreds of cooperative,
autonomous vehicles in warehouses. AI Magazine, 29:9–19, 2008.
[80] J.M. Yang and J.H. Kim. Sliding mode control for trajectory tracking of nonholonomic
wheeled mobile robots. IEEE Transactions on Robotics and Automation, 1999.
[81] J. Youssef. The Mountain Pass Theorem, Variants, Generalizations and Some Applica-
tions. Cambridge University Press, 2003.
[82] M. Athans and P.L. Falb. Optimal Control. An Introduction to the Theory and Its
Applications. Dover Publications, 2007.
[83] G. Leitmann. The calculus of variations and optimal control. Springer, 1981.
[84] J.Z. Ben-Asher. Optimal control theory with aerospace applications. AIAA Educational
Series, 2010.
113](https://image.slidesharecdn.com/0468cf6d-4b62-44c0-8a22-ec19452dbac9-150703153321-lva1-app6892/85/Manor_final-120-320.jpg)
Manor_final
- 1. Autonomous Mobile Robot Navigation with Velocity Constraints Gil Manor
- 2. Autonomous Mobile Robot Navigation with Velocity Constraints Research thesis In Partial Fulfillment of the Requirements for the Degree of Doctor of Philosophy Gil Manor Submitted to the Senate of the Technion - Israel Institute of Technology Tevet, 5775 Haifa January 2015
- 3. The Research Thesis Was Done Under the Supervision of Prof. Elon Rimon in the Faculty of Mechanical Engineering ACKNOWLEDGMENT I would like to express my sincere gratitude to Prof. Elon Rimon for his support and dedicated guidance and assistance throughout this research. The Generous Financial of The Technion and The Halevy Fellowship Scholarship is Gratefully Acknowledged
- 4. List of Publications Referred Papers in Professional Journals • Gil Manor and Elon Rimon. VC-method: high-speed navigation of a uniformly braking mobile robot using position-velocity configuration space. Autonomous Robots (2013) 34:295-309. • Gil Manor and Elon Rimon, The Speed Graph Method: Time Optimal Naviga- tion Among Obstacles Subject to Safe Braking Constraint, Submitted to IEEE Transactions on Robotics. Nov. 2014. Referred Papers in Conference Proceedings (* - Presenter) • *Gil Manor and Elon Rimon, High-Speed Navigation of a Uniformly Braking Mo- bile Robot Using Position-Velocity Configuration Space, International Conference of Robotic and Automation, St. Paul MN, May 2012. • *Gil Manor and Elon Rimon, The Speed Graph Method: Time Optimal Naviga- tion Among Obstacles Subject to Safe Braking Constraint, International Confer- ence of Robotic and Automation, Hong Kong, June 2014. Contributed Papers in Conferences and Workshops (* - Presenter) • Gil Manor, Global Time Optimal Path Planning Using the Brachistochrone Curve, Technion, Nov. 2012. • *Gil Manor and Elon Rimon, The Speed Graph Method: Time Optimal Naviga- tion Among Obstacles Subject to Safe Braking Constraint, ICR 2013, Tel Aviv, Nov. 2013.
- 5. Contents Abstract 1 List of Figures 3 List of Tables 6 Nomenclature 7 1 Introduction and Literature Review 11 1.1 Literature Review 12 1.1.1 Configuration Space 12 1.1.2 Two-steps Methods 13 1.1.3 Reactive Motion Planners 14 1.1.4 Discretized Search Spaces 16 1.1.5 Exact Optimal Paths Using Calculus of Variations 17 1.1.6 Optimal Control Based Methods 18 1.1.7 Potential Field Methods 18 1.2 Objectives and Significance of Work 19 1.3 Thesis Outline 20 2 High-Speed Navigation of a Uniformly Braking Mobile Robot Us- ing Position-Velocity Configuration Space 22 2.1 Introduction 23 2.2 Problem Description and Preliminaries 24 2.3 Position Velocity Configuration Space 27 2.4 Cellular Decomposition of Position-Velocity Configuration Space 29
- 6. 2.4.1 The Critical Points of the Height Function 29 2.4.2 The Partition of F v Into 3D Cells 33 2.5 The Cells Adjacency Graph 36 2.6 Practical Path Realization 38 2.7 Simulations 41 2.7.1 Influence of Obstacles Clearance on VC-Method Paths 41 2.7.2 Comparison of the VC-Method with the Dynamic Window Ap- proach 43 2.8 Experiments 45 2.9 Conclusion 47 3 The Speed Graph Method: Time Optimal Navigation Among Ob- stacles Subject to Safe Braking Constraint 49 3.1 Introduction 50 3.2 Problem Description and Preliminaries 51 3.3 The Safe Time Optimal Path Near a Single Obstacle Feature 53 3.3.1 The Travel Time Functional 53 3.3.2 The Safe Time Optimal Path Near a Wall Segment 54 3.3.3 The Safe Time Optimal Path Near a Point Obstacle 57 3.4 Basic Properties of the Safe Time Optimal Path in Polygonal Environ- ments 60 3.4.1 The Safe Time Optimal Variational Problem in Polygonal Envi- ronments 60 3.4.2 Basic Properties of the Safe Travel Time Functional 61 3.5 The Speed Graph 69 3.6 Speed Graph Based Computation of the Safe Time Optimal Path in Polygonal Environments 74 3.6.1 Path Optimization Using Newton’s Method 77 3.7 Numerical Simulations and Experimental Setup 80 3.8 Conclusion 84
- 7. 4 Conclusion 86 4.1 Summary 87 4.2 Current Work 88 4.3 Future Extension 89 Appendices 91 A The Turning Radius Constraint 92 B The Time Optimal Path for a Disc Robot 96 C Computation of the Speed Graph Edges and the Safe Time Optimal Path as Convex Optimization Problems 99 D The Turning Radius Constraint Along an Optimal Path 104 Bibliography 113 Hebrew Abstract I
- 8. Abstract Navigation is an important aspect in mobile robotics. Recently, we see a trend in robotic systems leaving laboratories and clean rooms and moving to real world envi- ronments. At this point, it is critical to ensure the safety of the robotic system as well as the environment in which it travels. One of the most common approaches to this problem is modeling a configuration space of permitted positions and orientations of the robot, then searching for a free collision path in this space. The method is applicable both off-line, pre knowing all the surrounding, and also on-line, while re- trieving the data with contact and distance sensors. Despite all this, the problem of navigating a mobile robot between obstacles while taking into account the dynamic characteristics of the robot while satisfying some quality measure of the path, remains with no certain unique solution. One quality measure for paths is their total travel time or in other words, the time optimal path between two end points. For such a quality measure, high speeds of motion are required to complete the task as quick as possible. Traveling with high speed imposes two main safety concerns that should be taken into account when planning the path. (1) A speed dependant safe braking constraint; (2) A turning radius constraint that prevents sliding affects or tipping over. These safety constraints involve the robot’s speed, denoted ν, as well as the coefficient of friction between the robot’s wheels and the ground, denoted µ. Common approaches for solving the high speed navigation problem first compute a collision free path for the robot, then parameterize the speed along it such that velocity constraints are met. In this research we synthesize the speed dependant safety constraints and the geometric path selection into a single framework. This ensures that the path taken by the robot, prematurely satisfies these safety constraints and therefore allows to compute the globally time optimal path. Two subsequent approaches are suggested. The first approach, suggests modeling an augmented configuration space for the 1
- 9. robot with the parameters x, y and ν. The speed dependant safe braking constraint is modeled in this space and represented by towering columns whose interior coordinates are states of the robot which lead to an imminent collision with the surrounding obstacles. The Morse Theory is then used to identify special events in this space where the free space for navigation losses its connectivity. These special events determine a cellular decomposition which in turn induces an adjacency graph. A standard graph search algorithm on this adjacency graph yields an approximated time optimal path in a highly efficient way in terms of computational time. The second approach uses calculus of variations and applies convexity properties of these paths to compute the globally time optimal path in the environment. First, the calculus of variations is used to compute the time optimal path near a single point or wall obstacles subject to the safe braking constraint. These optimal paths are then composed into a “speed graph” whose edges are time optimal path segments connecting each individual pair of nodes. A time optimal path candidate is then selected using a standard graph search algorithm. Next, convexity properties of these paths are used to smooth the optimal candidate and to derive the globally time optimal path in the environment. For both methods, the computed paths satisfy the turning radius constraint such that the robot does not slide or tip over. The results are illustrated with examples and described as readily implementable procedures. 2
- 10. List of Figures 1.1 (a) An arrangement of polygonal obstacles in the plane. (b) Each obstacle induces a c-obstacle in the robot’s c-space. 13 1.2 The velocity obstacle voB induced by a moving obstacle ˆB and a moving robot ˆA. The entities’ velocities are vA and vB respectively. 15 2.1 The straight line braking path ends at the maximal Euclidean distance from the robot’s initial position. 24 2.2 (a) The c-obstacle of a disc robot. (b) The vc-obstacle slice when the robot travels with velocity magnitude ν. 27 2.3 The vc-obstacles resemble columns whose cross section expands monotoni- cally along the ν coordinate. 29 2.4 (a) A Type I critical point is a saddle point of ϕ in Fv. (b) The vc-obstacles slices in the vicinity of the saddle point. 32 2.5 A Type II critical point is a local maximum of ϕ in Fv. 33 2.6 The (x, y) projection of a stack of 3D cells. 34 2.7 A stack of 3D cells is generically surrounded by three vc-obstacle and three vertical partitions. 35 2.8 The (x, y) projection of the 3D cells associated with a non-generic obstacle arrangement. 35 2.9 The adjacency graph of the cellular decomposition induced by three polygo- nal obstacles and an outer wall. 37 2.10 (a) The optimal path lies in vertical strips. (b) Path unfolding and vertical edges realization. 38 3
- 11. 2.11 A switching curve (in red) for the double integrator problem along which the point mass reaches the origin with zero speed. 40 2.12 (a) The optimal path passing through a node. (b) The smoothed path follows the minimum safe turning radius arc. 40 2.13 A disc robot navigating with high speed in an office environment. 41 2.14 The speed of the robot for δ = 3.4. 42 2.15 The ratio t(vc-method)/t(lmin) of the vc-method as a function of δ. 43 2.16 The vc-method and DWA paths in (a)-(b) a rectangular room containing an obstacle cluster, and (c) a urban-like environment. 44 2.17 The velocity profiles of the paths illustrated in Figures 2.16(a)-(b). 45 2.18 The experiment setup of three polygonal obstacles within a rectangular outer boundary. 46 2.19 The velocity profiles of the three programmed paths. 46 3.1 (a) A circle rolling on the wall segment generates the cycloid curve. (b) The safe time optimal path near a wall segment. 56 3.2 (a) Two symmetrically shaped time optimal paths from pS to pT . (b) The safe time optimal path is a parabolic arc equidistant from the point obstacle and the line l. 59 3.3 (a) A hypothetical time optimal path touching a convex obstacle vertex. (b) The path can be moved away from the vertex, with a new segment of length L < 2 . 63 3.4 (a) The Voronoi diagram, and (b) The speed graph of an environment popu- lated by two polygonal obstacles (Voronoi arcs are solid curves, speed graph edges are dashed curves). 69 3.5 (a) Two speed graph nodes, p1 and p2, are separated by three proximity cells whose boundary lines are parameterized by (u1, u2). (b)T(γ(u1, u2)) is star shaped, moreover its contours are convex in the (u1, u2) plane. 71 3.6 The Voronoi saturation problem. 73 4
- 12. 3.7 (a) The local smoothing directions of αo and αo point toward each other, and (b) require a splitting of pi into two crossing points that will slide on the neighboring Voronoi arcs ei1 and ei2. 75 3.8 (a) The piecewise time optimal paths connecting pS and pT while crossing the Voronoi arcs e1 and e2 are parametrized by (u1, u2). (b) The contours of T(γ(u1, u2)) are star shaped (and convex) with center at (u∗ 1, u∗ 2). 77 3.9 (a) The speed graph of the given environment, indicated with dashed curves. (b) The initial speed graph path αo, and (c) the safe time optimal path α∗ connecting pS and pT in F. 79 3.10 The Newton-Raphson’s travel time convergence during five iterations on the paths of Figure 3.9. 80 3.11 (a) The speed graph of the warehouse environment. (b) The safe time optimal path α∗ selected by the speed graph, and the safe time optimal path ¯α∗ associated with the geometrically shortest path. 81 3.12 The speed profile along the paths α∗ and ¯α∗ in the warehouse environment. 81 3.13 The six best safe time optimal paths in the office floor environment. 82 4.1 A Formula One race car has to travel at high speed during turns without sliding. 89 A.1 (a) Top view of a four-wheel mobile robot turning geometry. (b) Rear view of a left turning mobile robot. 93 B.1 The safe time optimal path determined by the polygonal approximation, ¯α∗, and the exact path α∗ associated with the disc obstacle. 97 C.1 The safe time optimal path connecting the speed graph nodes p1 and p2 crosses two proximity cell boundary lines, l1 and l2, at the corner points p(u1) and p(u2). 99 C.2 (a) The safe time optimal path α∗ and a variant path αt. (b) The sector spanned by −v(s− i ) and v(s+ i ) locally contains the Voronoi arc tangent line at p(ui) for i = 1, 2. 101 5
- 13. List of Tables 2.1 Running time in seconds and length in meters of the three simulations. 44 2.2 Experiment Results 47 3.1 The safe time optimal travel time in the six best homotopy classes in the office floor environment. 83 4.1 Comparison between the VC method and the Speed Graph method. 88 6
- 14. Nomenclature (r, θ) Polar coordinates used for paths near point and disc obstacles (x, y) Cartesian coordinates in the plane α Path connecting two endpoints in IR2 α The tangent of α α∗ , β∗ Local time minimal paths αo The most promising path selected by the speed graph dst(α, Oi) The minimal distance between the path α and obstacle Oi dst(q, Oi) The minimal Euclidean distance between point q and obstacle Oi A desired solution accuracy η A piecewise smooth function F The obstacle-free portion of the environment ˆv The robot’s instantaneous heading int(COi) The interior of COi int(COv i ) The interior of COv i H The hessian of a scalar function of 2 variables F The free configuration space of the robot 7
- 15. µ The static coefficient of friction with the ground at the contacts ν The robot’s linear speed (velocity magnitude) ω The robot’s front wheels average steering angle IR The real numbers IR2 The two dimensional infinite plane IR3 The three dimensional infinite space ρ The radius of a disc robot ρc The radius of curvature of a path segment ϕ Morse function v The normalized tangent of α ηi The normal to bdy(COv i ) at p T(α) The energy functional of T(α) ξ The angle of a point on a rolling circle that creates the Cycloid amax The robot’s maximal acceleration amin The robot’s minimal acceleration Ang(α1(pi) − α2(pi)) The polar angle between two path tangents α1, α2 at node pi bdy(COv i ) The surface of COv i C(1) The division of paths with continuously varying tangent COi A configuration obstacle induced by obstacle Oi COv i A velocity configuration obstacle induced by obstacle Oi d(ν) The robot’s minimal braking distance 8
- 16. d1(α1, α2) The metric between two paths α1, α2 Ef The net work applied by the friction force ei Voronoi arc i Ek The kinetic energy of the robot F The integrant of T(α) fN The net normal force at the contacts Fv The free position-velocity configuration space g The gravitational acceleration Gv The adjacency graph H A horizontal plane in IR3 h The robot’s c.o.g. hight l The robot’s wheels base lj Proximity cell boundary line j m The robot’s mass n Total number of obstacle features in a polygonal environment O0 The outer boundary of environment populated by obstacles Oi An internal obstacle enumerated i P A vertical plane in IR3 p A point in the velocity configuration space p(ui) A corner point i of path α pi A graph’s vertex i 9
- 17. pS Start position pT Goal position q The robot’s center position in IR2 R The robot R(q) The set of points occupied by a robot R centered at q rmin(ν) The robot’s minimal turning radius s The robot’s path distance parameter T(α) The total travel time along path α ui The length parameter of corner point i v The robot’s linear velocity w The robot’s width 10
- 18. Chapter 1 Introduction and Literature Review As autonomous mobile robots are deployed in ever more demanding tasks, they strive to complete their tasks while minimizing overall travel time. This requirement is achieved by maximizing the robot’s speed while maintaining velocity dependant safety constraints throughout the navigation process. Examples are autonomous mobile robots operating in sentry duty and surveillance [2, 3], where high speed is required to complete the task without being detected by potential intruders [19]. Other examples are self-driving cars [50] and traffic assistance systems [4] that will soon allow vehi- cles to be piloted autonomously at speeds up to 50 km/h [63]. High speed systems also include mobile robots carrying cargo in large warehouses [70, 79]. In all of these applications collision safety along high speed paths depends on velocity constraints, thus requiring a consideration of the robot’s full position-velocity state in order to synthesize such paths. There are two main safety concerns during execution of fast mobile robot tasks in planar environment. The first is to ensure that the robot will always be able to reach a full stop without colliding with any of the stationary obstacles or moving agents [27, 57]. The second is to ensure that the robot will not slide or tip-over while taking sharp turns at high velocity. These two limitations on robot’s speed are induced by the robot’s geometry and the power delivered by its actuators as well as by the friction between the wheels and the ground. 11
- 19. According to the classical path planning approach, one first determines a collision free path using the robot’s configuration space, then selects a time parametrization that attempts to minimize total travel time subject to velocity-dependent safety con- straints [33, 35, 64]. The more general approach of planning a path in a space that in- cludes the robots position as well as velocity is a relatively unexplored area in robotics. In this work, the entire process of path planning takes place in the robot’s state space, thus treating the geometric path planning and the speed profiling in a single frame- work. 1.1 Literature Review Next, we provide a survey of relevant works in the literature. The survey is divided into parts according to the different approaches taken in this field. 1.1.1 Configuration Space The classical approach for path planning considers planning a collision free path in a configuration space [40, 46]. Given an arrangement of obstacles and the geometry of a single rigid robot with no kinematic constraints (see Figure 1.1(a) for example), the configuration space is computed. As depicted in Figure 1.1(b), for every orientation of the robot, each obstacle induces a c-obstacle. The c-obstacle includes all the points in the plane which bring the robot to penetrate the physical obstacle. Note that the c-space depends on the placement of the robot’s coordinates system. When a 2D environment of obstacles is considered while assuming a planar robot that can freely translate and rotate, the induced c-space includes towering columns in IR3 . Each col- umn, represents forbidden places and orientations that will result an imminent collision between the robot and the physical obstacle. Generally, the c-space’s size depends on the degrees of freedom of the robot as well as on the environment’s complexity. The c-space method allows to compute a collision free path connecting the start 12
- 20. (a) (b) physical environement c-space c-obstacle robot’s reference point Figure 1.1: (a) An arrangement of polygonal obstacles in the plane. (b) Each obstacle induces a c-obstacle in the robot’s c-space. to target using various methods. These include exact methods such as cellular decom- position where the free c-space is decomposed into simple regions whose connectivity induces a undirected graph denoted a connectivity graph [17]. Other methods suggest constructing a road map for the environment such as the Voronoi diagram or visibility graph [76], then search for a collision free path on this graph. Although very useful in known environment with unconstrained motion, adding dynamic constraints, which is the main topic of this thesis, increases the size of the c- space and hence makes it harder to implement most common exact methods. Moreover, the addition of non-holonomic constraints on the robot’s motion adds non-integrable equations involving the configuration parameters and their derivatives which limits the possible motions of the robot but does not reduce the size of the c-space. 1.1.2 Two-steps Methods The next approach is a two-steps method that first computes pure geometric collision free path, then generates a velocity profile in such way that total travel time along the path is minimized while meeting velocity and higher derivative safety constraints. One of the original works in this subdivision was introduced by Bobrow et al. in [9]. In their work, the speed parameterizations of the joints of a serial robot were determined using 13
- 21. a linear controller while following a desired path. These speed profiles are selected such that actuators’ constraints are met. Their work was later extended by [65] to include the dynamics of a mobile robot. Other works followed starting with lepetic et al. [45], that proposed a method for high speed path planning while considering acceleration limits induced by maximal ground friction forces. Their path is generated by a spline curve computed over a set of “control via points”, whose positional optimization gives an approximate time-minimal path. Velenis and Tsiotras [72], propose a method to determine the robot’s speed in order to minimize travel time along a pre-specified collision free path. The velocity profile respects the mobile robot’s acceleration limits. Villagra et al. [73], first compute collision free paths with continuous curvatures and curvatures’ derivatives, then apply a speed profiler such that time minimal trajectories are obtained along the curvature- based paths. Their time minimal trajectories satisfy additional comfort constraints such as jerk limitations. Wu et al. [74], first compute a geometric path by taking into account kinematic constraints such as minimal turning radius. Then, a velocity profiler is applied to maintain prescribed velocity constraints. All these methods plan the robot’s trajectory in two separate steps. First a purely geometric collision free path is computed, then the robot’s speed is selected along the path such that velocity safety concerns are met. However, when truly time optimal paths are desired under velocity dependent safety constraints, the geometric path taken by the robot and the speed profile along the path must be synthesized within a single position-velocity space, which is the topic of this research. 1.1.3 Reactive Motion Planners An important class of high speed motion planners operate locally in discrete time steps. Whereby at each time step the robot selects a local high-speed maneuver that avoids nearby obstacles. The velocity obstacles method models the robot’s safe velocities as 14
- 22. v ‚ƒWXiPRSUvHar€xSX‘Rhzˆr‘€xY`PRab„`SUP€xWXabfŠ€`tbvd§QTSXSUWX„`WUQRaŒd§QRabv@D¤D4(HF1«ˆr‘ 910h‡PR„2„…tbQ‚£saŸWXa¥V6WUfR‚bY`v Q ‘™†tbv ‰—¨‚“ ™!!pB™W£q“¨U—¡ £¢WU„©€`tbv§ahfbvHœbabv§fŒPR„ F £¢QICD¤D4(HF1¥¤ 91 { $ B ^ VB VO B V A VB A ^ VGWUfT‚bY…v Q Fy™†tbv2iTvHSUQqd§W’€—‘(QTˆb„p€ePRdHSUv¦ ¦¢ 10 Figure 1.2: The velocity obstacle voB induced by a moving obstacle ˆB and a moving robot ˆA. The entities’ velocities are vA and vB respectively. an allowed velocity cone [24]. As depicted in Figure 1.2, each velocity cone determines a local avoidance maneuver, and a series of such maneuvers is selected until the robot reaches the target. The velocity obstacles method has been extended to local time optimal maneuvers, and to motion in the presence of other moving agents such as cars and pedestrians [39, 69, 78]. The dynamic window approach plans the robot’s path within a finite time horizon whose size is determined by the robot’s current state [26]. At each time step, the robot’s trajectory is chosen by maximizing a weighted sum of the robot’s distance from the obstacles, the robot’s speed, and the current direction to the target. Some dynamic window implementations explicitly account for safe braking constraints [13, 26]. Another approach is the method of inevitable collision states [27, 56, 57]. This method encodes the surrounding obstacles as well as the robot’s braking capabilities into first-order differential equations. Numerical integration of these equations gives the inevitable collision states that can be incorporated into any fast mobile robot navigation scheme. Additionally, let us mention the vector field histogram approach [10, 71]. Using the robot’s sensors, this method builds a histogram based on the distances to the obstacles in discrete set of angles. The method then searches for gaps in this histogram that represent collision-free headings of motion from the robot’s current position. Although 15
- 23. efficient for on-line directional control, this method does not consider explicitly safety criteria or other high level requirements such as total travel time. Finally, let us discuss the work presented in [66] which considers static environments populated by disc obstacles. Instead of computing at each time instant a local maneu- ver which must avoid the obstacles, each obstacle is considered at a time. This reduces the computational running time to linear in the number of obstacles. About each in- dividual obstacle, an optimal bypassing maneuver is computed for a non-holonomic point robot while assuming symmetrically bounded actuators and double integrator dynamics. A global path is computed by concatenating a series of such paths. How- ever, this method does not consider safety limitations which is the main topic of this research. This family of reactive motion planners is clearly useful in dynamically evolving environments. In such environments the robot cannot posses full knowledge of the obstacles and therefore cannot plan its complete path before travel. Therefore the robot is forced to use a greedy strategy in which it computes its best and fasted path with respect to its known surroundings. 1.1.4 Discretized Search Spaces Other motion planners search for a high speed and safe path in the robot’s discretized state space. First suggested by the kinodynamics method, which discretizes the robot’s state space into a regular grid and searches the obstacle-free portion of the grid for a minimum travel time path subject to velocity dependent safety constraints [21, 37, 42]. The kinodynamics approach requires a huge grid in complex environment [32, 43]. The method’s running time is proportional to the grid size and can be very high in complex environments. Karvaki [18] and LaValle and Kuffner [41, 44] introduced the RRT method, or rapidly-exploring random trees, computed in the robot’s discretized state space. At each time step until the target is reached, a randomly chosen point is connected to the 16
- 24. current tree with a feasible maneuver that obeys predefined velocity constraints and minimizes a cost function such as path length or travel time. While the RRT method was an important breakthrough, there are cases where the algorithm converges to a non optimal path. To over come this, Karman and Frazzoli, [36], introduced the RRT*. Beside linking a randomized chosen point to the current tree with an optimal edge that satisfies the velocity constraints, in each stage, the RRT* also updates the connections between the nodes in the current tree. The connections between the nodes are updated in such way that reduces the total cost of the path between the start to the current node. The RRT* is proven of being complete and converges to the optimal path as number of iterations reaches infinity. However, its rate of convergence can be excessively slow in congested environment. Also note that this class of motion planers usually assumes fully known environments. 1.1.5 Exact Optimal Paths Using Calculus of Variations Using calculus of variations, Wein et al. [75] compute collision free paths whose geo- metric shape is determined by a user specified “path quality” parameter δ. A value of δ = 0 corresponds to minimal length paths that trace the visibility graph, while δ = ∞ corresponds to maximal clearance paths that trace the Voronoi diagram of the environment. Based on this path quality measure, they describe a scheme that discretizes the environment’s Voronoi diagram into -length segments, then searches the adjacency graph of these segments for optimal paths in O((Λ2 / 2 )n5/2 log n) time, where Λ is the total length of the environment’s Voronoi diagram, and n is the num- ber of obstacle features in the environment. However, [75] does not explicitly consider velocity dependent safety constraints or travel time minimization. 17
- 25. 1.1.6 Optimal Control Based Methods The use of optimal control for time optimal path computation is well cited in the literature. One of the earliest works in this field includes Dubins’s work which was published in 1957 [22]. In his work, Dubins considered a non-holonomic motion of a car like vehicle navigating between two end points in an obstacles-free environment while assuming constant speed. He then proved that the time optimal path (with constant speed!) between these two end points is constructed by path primitives consisting of constant radius arcs and linear path segments. It is important to note that the initial and terminal states of the car like vehicle are determined by its position as well as its orientation. In [58], the car was also allowed with backward motion which created a non smooth path with cusps. Many works were inspired by Dubins’s work. In [31], for e.g., the authors compute path primitives which minimize overall travel distance on the plane in heterogeneous environments. In such environments the robot’s permitted speed is determined by its position. Other works create the path using smooth path primitives such as clothoid which allows to create smooth connectivity between adjacent segments [25, 77]. Com- puting time optimal path primitives using optimal control theory was also suggested in [16]. In this work, path primitives were computed for Dubins airplanes. For such airplanes, the state space is 4D rather than 3D as in the planar Dubins car. However, non of these works explicitly account the speed dependant constraints involving braking safety as well as the turning radius of the robot while navigating with high speed between adjacent obstacles. Although not fully presented in this thesis, this approach is currently being taken by us in current work as briefly discussed in Section 4.2. 1.1.7 Potential Field Methods Last, consider works were potential field methods, or PFMs, are used to navigate the robot. For such methods each obstacle exerts a repulsive force on the robot while the target imposes an attracting force. A summation of these forces propels the robot between the obstacles to the target, for e.g. [55, 67, 28]. Although quiet efficient for global path planning and also 18
- 26. as a local planner, PFMs suffer from two main drawbacks as discussed by Koren et al in [38]. (1) PFMs may lead the robot to a local minimum which will prevent the robot from reaching its target. This drawback is well investigated in the literature and many solutions were suggested, for e.g. [60, 59]. (2) PFMs may bring the robot to lose its stability when implemented online in narrow corridors or near a long wall obstacle. This loss of stability is characterized by rapidly growing oscillations, left and right, that bring the robot to lose its grip with the ground and collide with adjacent obstacles. Koren et al. point out this drawback of PFMs and suggest a stability criteria that has to be taken into account when implementing such paths using PFMs. Although this thesis utilizes ideas also used by PFM methods, the implementation draw- back, discussed by Koren et al. is not in the scope of this research. In this research we focus mainly on path and trajectory planning rather then trajectory tracking. These two problems are complementary but can be isolated and dealt independently from each other. Few examples for works that concern the tracking problem can be found in [6, 52, 15, 5, 80]. Despite all this, we find it important to mention Koren’s work which enlighten the navigation problem from a different, rather important, point of view. 1.2 Objectives and Significance of Work As presented in Section 1.1, the problem of finding a collision free path connecting two points in occupied environments has no particular optimal solution. Moreover, when safety con- straints such as braking distance and speed dependant turning radius are imposed, dynamic constraints on robot’s motion are involved which makes the path planning a more complex task. As discussed, when dynamic constraints are involved, common numerical approaches suggest discretizing the search space into a regular grid then implementing a numerical solver which in some cases does converges to the optimal path (with respect to some quality measure) in an infinite time. This thesis considers a relatively simplified problem in which the robot’s model is a planar disc navigating in the plane among stationary obstacles. The robot is subjected to 19
- 27. braking safety as well as turning radius constraint. The thesis synthesizes these dynamic constraints into the geometrical path planning and allows to rigorously ensure safety as well as optimality in analytical fundamental verified methods. Our first approach taken for computing a safe high speed path is supported by the Stratified Morse Theory (Chapter 2). Then a new approach is taken using the Theory of Calculus of Variations (Chapter 3). Convexity properties of these paths allow to ensure convergence to the time optimal path in an accuracy algorithm which is highly efficient for path planning. The tools developed in this research will allow extension to more complex scenarios where dynamic constraints are involved and efficient computation process is required. 1.3 Thesis Outline The thesis consists of two main parts excluding this introductory chapter and the concluding one. The first part of the thesis (Chapter 2) consists of a simplified model where the robot’s speed dependant safe braking constraint is considered uniformly in all directions. A pseudo time optimal path is computed, one that not only avoids the surrounding obstacles by also satisfies the safe braking constraint. This simplified model allows to model the safe braking constraint in a 3D configurations space, denoted vc-space, whose axes are the robot’s position in IR2 and it’s velocity magnitude or speed. A method for modeling the safe braking distance is presented, then a cellular decomposition of the free portion of the vc-space is suggested. Based on this cellular decomposition, an adjacency graph is constructed and a pseudo time optimal path is found. This model enables us to find the pseudo-optimal, collision free path for the robot while preserving the braking distance constraint. A post processing algorithm ensures that the safe turning radius constraint that prevents sliding and tipping over is satisfied. The second part of the thesis (Chapter 3) consists of finding the time optimal path subject to the uniform safe braking constraint, using the speed graph method. Using calculus of variations, the speed graph method computes the global time optimal path of a mobile robot navigating in an environment populated by stationary obstacles and subjected to a 20
- 28. uniform safe braking constraint. The classical Brachistochrone problem describes the time optimal path between two points in an obstacle free environment subjected to a constant force field. By encoding the braking safety constraint as a force field surrounding each obstacle, the method generalizes the Brachistochrone problem into the time optimal navigation problem in an environment populated by stationary obstacles. The time optimal safe path between any start and target is first computed on a simplified problem, where the mobile robot navigates in the vicinity of a single point or a wall segment obstacle. Then, the analysis is extended to environment containing outer walls and multiple internal polygonal obstacles. First, a Speed Graph for the environment in constructed, which consists of time optimal edges that connect critical via points. The speed graph is then used to efficiently compute the homotopy class of paths which most likely contains the global time optimal path1. In the second stage, the exact time optimal path is computed within the chosen homotopy class based on convexity properties of these paths. The concluding chapter of this thesis, summarizes the results of this work and discusses future extension beyond the scope of this research. Auxiliary material is also added in the appendices. 1 Two continuous paths α : [a, b] → IRn and β : [a, b] → IRn connecting start and target denoted pS and pT , belong to the same homotopy class if there is a continuous mapping, F : [a, b] × [0, 1] → IRn , such that F(t, 0) = α(t), F(t, 1) = β(t) for t ∈ [a, b] and F(a, s) = pS, F(b, s) = pT for s ∈ [0, 1]. 21
- 29. Chapter 2 High-Speed Navigation of a Uniformly Braking Mobile Robot Using Position-Velocity Configuration Space 22
- 30. 2.1 Introduction This chapter considers planar environments populated by stationary obstacles whose location is known to the robot. The assumption of stationary obstacles is a conservative approach, suitable for environments where emergency events occur along obstacle edges or just beyond corners obstructed by obstacles. For instance, an autonomous vehicle moving in an urban environment may detect another vehicle suddenly emerging from its parking space. By maintaining suitable safe velocity, the moving vehicle can brake and prevent collision with the emerging vehicle. More generally, any environment in which emergency events can be detected at a distance exceeding the robot’s braking distance can be modeled according to the method proposed in this chapter and also published in [48, 47]. As presented in the abstract, there are two main safety concerns during execution of fast mobile robot tasks. The first is to ensure that the robot will always be able to reach a full stop without colliding with any of the stationary obstacles or moving agents [27, 57]. The second is to ensure that the robot will not slide or tip-over while taking sharp turns at high velocity. In this chapter we model the uniform braking constraint as vc-obstacles in position- velocity configuration space. The vc-obstacles’ cross section increases monotonically along the velocity coordinate. We characterize the critical points where two vc-obstacles meet for the first time and locally disconnect the free position-velocity configuration space. In the physical environment, these are critical events where the robot’s velocity becomes too large to support safe passage between neighboring obstacles. Next, we describe a cellular decomposition of the free position-velocity space which is based on these critical points. An adjacency graph is constructed for the cellular decomposition, such that its edges project onto the Voronoi diagram of the planar environment. A specialized search algorithm called the vc-method finds an approximate time-minimal path along the adjacency graph. Finally, the path’s vertices are smoothed to ensure safe turning at the prescribed path velocity. The resulting path leads the robot from start to target with maximum velocity while guaranteeing safe braking and turning along the path. The chapter is structured as follows. Section 2.2 describes the main velocity depen- dent safety constraints governing high-speed mobile robot motion. Section 2.3 introduces the position-velocity space and the vc-obstacles induced by the safe braking constraint in 23
- 31. v r 0v 0v v d(v) x y 0v v 0v 0v v wiggly path stops at a shorter distance from start Figure 2.1: The straight line braking path ends at the maximal Euclidean distance from the robot’s initial position. this space. Section 2.4 defines a cellular decomposition of the free position-velocity space. Section 2.5 constructs an adjacency graph for the cellular decomposition. A search along this graph yields a pseudo time optimal path that maintains safe braking distance from the obstacles. Practical path realization issues such as path turning safety are considered in Section 2.6. Simulations and experimental results of the method are discussed in Sections 2.7 and 2.8 as well as a brief conclusion in the concluding section of this chapter. Axillary material is also added in the Appendix A. 2.2 Problem Description and Preliminaries This section describes the velocity dependent safety constraints governing fast mobile robot motion. The robot, denoted R, is assumed to be a disc that can freely move in the (x, y) plane (Figure 2.1). The robot’s position and linear velocity are denoted by q ∈ IR2 and v ∈ IR2 . The robot is assumed to move with fixed orientation, so that its configuration space, or c-space, is two dimensional. The main velocity dependent safety constraints are the need to maintain safe braking distance, and to prevent sliding as well as tipover during sharp turns. First consider the safe braking constraint. When the robot executes fast motion between obstacles, it must be able to brake and reach a full stop without hitting any obstacle during the braking process. This requirement ensures that sudden events, such as a pedestrian emerging between parked vehicles into the road, can be safely avoided by a fast moving mobile robot. The length of the 24
- 32. shortest braking path is dictated by the robot’s maximal deceleration, a system dependent parameter which is part of the following definition. Definition 1 (Linear Acceleration Constraint). The robot’s linear acceleration is con- strained by system dependent parameters amin and amax, which bound the robot’s acceleration by amin ˙v · ˆv amax, where ˆv is the robot’s linear velocity direction and amin 0 to represent deceleration. The robot’s shortest braking path is defined as follows. Definition 2 (Shortest Braking Path). The robot’s shortest braking path is any feasible path along which the robot applies the maximal deceleration amin. The distance from the robot’s initial position to the point where it reaches a full stop is not constant. As the robot follows a wiggly but feasible path, this distance becomes shorter as depicted in Figure 2.1. The robot’s minimal braking distance, denoted d, is the length of any of its shortest braking paths along which the robot decelerate with amin. Stretching these braking paths into a linear path of length d will also give a shortest braking path, but the one with the largest distance measured between the robot’s initial position and the final braking point. For safety, the robot must keep at least d away from any of the obstacles. The minimal braking distance can be computed using energy balance as follows [27]. The mobile robot is modeled as a cylindrical rigid body moving with fixed orientation. Its kinetic energy is given by Ek = 1 2 mν2 , (2.1) where m is the robot’s mass and ν = ||v|| is the robot’s linear velocity magnitude. Let α(s), s ∈ [0, 1] be a braking trajectory which brings the robot to a full stop along any of the shortest braking paths. The net work applied by the static friction force, which acts between the wheels and the ground in order to bring the robot to a complete stop is given by Ef = µ α(s) fN ds = µfN d = µmgd, (2.2) where µ is the static coefficient of friction between the wheels and the ground, g is the 25
- 33. gravitational acceleration, and fN =mg is the net normal force at the contacts. The minimal braking path, d, occurs when the frictional reaction forces between the wheels and the ground are aligned with the friction-cone edge opposing the direction of motion. Note that during this braking phase, the wheels are rolling without sliding at the contacts. A complete stop of the mobile robot requires that all its initial kinetic energy be absorbed by the braking force, Ek = Ef . (2.3) Substituting for Ek and Ef gives 1 2mν2 = µmgd. The minimal braking distance is therefore d(ν) = 1 2µg ν2 = kν2 , (2.4) where k = 1/2µg is a constant. Note that d(ν) does not depend on the robot’s mass. This assumption is valid only under an ideal rigid body model. In practice the robot’s wheels are rubber coated and experience substantial deformations at the contacts. In these cases the friction coefficient µ (and hence the braking distance d) is strongly influenced by the robot’s mass m [68][chap. 3]. Also note that the actual frictional work is accomplished by the braking pads of the robot and not by the static friction between the wheels and the ground. However if we assume that the braking pads can deliver a larger braking force than the friction force at the contacts then the previous development sustains. The uniform braking constraint: Uniform braking safety is ensured when the robot is kept at least d(ν) away from the nearest obstacle. This conservative safety criterion is illustrated in Figure 2.1. All paths that bring the robot to a full stop while applying the maximal deceleration amin have the same length d. When the closest obstacle is located at least d away from the robot’s initial position, the robot can reach a full stop without hitting any obstacle while following any maximal deceleration path. The second major velocity dependent constraint is the robot’s minimal turning radius. While turning, centrifugal forces and lateral moments act on the robot’s body. A practical means to prevent sliding or tipping-over is to lower bound the robot’s turning radius at any given velocity, as specified in the following definition. 26
- 34. v iCO R ( )d v x R y iO iCO iO (a) (b) Figure 2.2: (a) The c-obstacle of a disc robot. (b) The vc-obstacle slice when the robot travels with velocity magnitude ν. Definition 3 (Turning Radius Constraint). The robot’s minimal turning radius is speci- fied by a system dependent function rmin(ν), where ν is the robot’s current velocity magnitude. An expression for the robot’s minimal turning radius, rmin(ν), is derived in Appendix A. The high-speed navigation method proposed in this chapter generates a pseudo time optimal path that maintains safe braking distance from the obstacles. The path is then locally smoothed to ensure safe turning radius at the prescribed velocity along the path. 2.3 Position Velocity Configuration Space This section defines the position-velocity configuration space and the “obstacles” that encode the safe braking constraint in this space. Consider the mobile robot to be a disc, R, moving in the (x, y) plane. Let R(q) be the set of points occupied by the disc robot when its center is located at q=(x, y). The obstacles occupy stationary sets denoted O1, ..., Ok. Each obstacle Oi induces a c-obstacle, denoted COi, given by COi = {q∈IR2 : R(q) ∩ Oi = ∅} i = 1 . . . k. (2.5) Each c-obstacle is obtained by uniformly expanding the physical obstacle by the robot’s radius, as depicted in Figure 2.2(a). The free configuration space is the complement of the c-obstacle interiors, F = IR2 − ∪ i=1...k int(COi), (2.6) 27
- 35. where int(COi) denotes the interior of COi. Position-velocity configuration space generalizes the notion of configuration space by adding the robot’s velocity magnitude, ν, as an additional coordinate. Definition 4. The position velocity configuration space of a disc robot R is the three- dimensional space IR3 with coordinates p = (x, y, ν), where q = (x, y) is the robot’s position and ν is the robot’s linear velocity magnitude. The safe braking constraint is next defined as forbidden regions in position-velocity con- figuration space. Let dst(q, Oi) be the minimal Euclidean distance of a point q ∈ IR2 from the obstacle Oi. Definition 5. Given a robot R and an obstacle Oi, the vc-obstacle induced by Oi, denoted COv i , is given by COv i = (q, ν) ∈ IR3 : dst (q, Oi) ≤ d(ν) , (2.7) where d(ν) is the minimal braking distance for a robot traveling with velocity magnitude ν. The free position-velocity configuration space, denoted Fv , is the complement of the vc- obstacle interiors, Fv = IR3 − ∪ i=1..k int(COv i ), (2.8) where int(COv i ) denotes the interior of COv i . At any point p ∈ Fv, the robot initially positioned at q and traveling with velocity magnitude ν can reach a full stop, without hitting any of the obstacles O1, ..., Ok. Each vc-obstacle forms a vertical column in position-velocity configuration space. The ν = 0 slice of this column coincides with the c-obstacle COi. As ν increases, the column’s cross section expands, since the braking distance increases quadratically with ν. Figure 2.2(b) depicts a fixed-ν slice of a vc-obstacle, while Figures 2.4 and 2.5 sketch the full vc-obstacles. The rate by which each vc-obstacle cross section expands is quadratic in ν, due to the relation d(ν) = kν2. Example: The vc-obstacles associated with three polygonal obstacles are shown in Fig- ure 2.3. Their bottom cross sections are the c-space obstacles, drawn in the (x, y) plane which corresponds to ν =0. As ν increases, the vc-obstacles expand outwards while the free 28
- 36. x y Figure 2.3: The vc-obstacles resemble columns whose cross section expands monotonically along the ν coordinate. space Fv shrinks inward. The graph depicted in the (x, y) plane is the Voronoi diagram of the c-space obstacles and an outer boundary which is not shown. The Voronoi diagram will play an important role in Section 2.5. ◦ 2.4 Cellular Decomposition of Position-Velocity Con- figuration Space This section describes a cellular decomposition of the free position-velocity configuration space, Fv, such that each cell is associated with a range of velocities that can be safely executed at a particular locality of the environment. Section 2.4.1 introduces the position- velocity height function, ϕ, and analyzes its critical points. Section 2.4.2 partitions Fv into 3D cells based on the critical points of ϕ. 2.4.1 The Critical Points of the Height Function The free position-velocity configuration space is a stratified set [30], consisting of the following manifolds. The interior of Fv is a three-dimensional manifold. The portions of the vc- obstacle surfaces on the boundary of Fv form two-dimensional manifolds. The intersection curves of vc-obstacle surfaces form one-dimensional manifolds. The isolated points where three vc-obstacle surfaces intersect form zero-dimensional manifolds. 29
- 37. The cellular decomposition of Fv will be determined by the critical points of the function ϕ : Fv → IR , which measures the “height” of points p = (x, y, ν) along the ν coordinate in position-velocity configuration space, ϕ(x, y, ν) = ν. (2.9) The critical points of ϕ in Fv are the union of the critical points of the restriction of ϕ to the individual manifolds comprising Fv [30]. A point p ∈ Fv is a critical point of ϕ when ϕ(p) is collinear with the generalized normal to the particular manifold containing the point p. The generalized normal of the manifolds comprising Fv can be characterized as follows [20]. In the interior three-dimensional manifold, the generalized normal is the zero vector. At a point on a vc-obstacle surface, it is the usual surface normal pointing into Fv. At a point on the intersection curve of two vc-obstacle surfaces, it is the convex combination of the two surface normals meeting at this point. Last, every zero-dimensional manifold is automatically a critical point of ϕ. The following theorem asserts that ϕ has two types of critical points. Theorem 1. The critical points of ϕ in Fv are located either on the intersection curve of two vc-obstacle surfaces, or at the intersection point of three vc-obstacle surfaces. Proof: First consider the interior of Fv. Since ϕ(p) = (0, 0, 1) is a non-zero vector, ϕ cannot have any critical point in the interior of Fv. Next consider the vc-obstacle surfaces, denoted bdy(COv i ) for i = 1 . . . k. Denote by ηi = (ηx, ηy, ην) the normal to bdy(COv i ) at p. If p is a critical point of ϕ, ϕ must be collinear with ηi: 0 0 1 × ηx ηy ηv = −ηy ηx 0 = 0 0 0 ⇒ η= 0 0 ηv (2.10) Thus, a necessary condition for p to be a critical point is that (ηx, ηy) = (0, 0) at this point. The boundary of COv i is the zero level-set of the scalar valued function: ψ(x, y, ν) = dst((x, y), Oi) − kν2 = 0. Therefore ψ is collinear with the vc-obstacle normal ηi, and up 30
- 38. to a multiplicative factor: ηi(x, y, ν) = ψ(x, y, ν) = dst (q, Oi) −2kν . The function dst(q, Oi) measures the minimal Euclidean distance between q=(x, y) and Oi. This function is known to have a unit magnitude gradient, pointing from the neatest point on Oi’s boundary to q [20][Proposition 2.5.4]1. Since dst(q, Oi) is a non-zero vector, ϕ cannot have critical points on bdy(COv i ) for i = 1 . . . k. Next consider a point p on the intersection curve of adjacent vc-obstacle surfaces. The generalized normal at p ∈ bdy(COv i )∩bdy(COv j ) is the convex combination of the vc-obstacle normals, ηi and ηj, η ∈ λ1ηi + λ2ηj 0 ≤ λ1, λ2 ≤ 1, λ1+λ2 =1. A critical point p must therefore satisfy the condition: ϕ(p) = 0 0 1 = λ1ηi + λ2ηj for some λ1, λ2 ≥ 0. (2.11) It follows from (2.11) that ϕ has a critical point at p when the tangent to the intersection curve at p is purely horizontal in (x, y, ν)-space. Such points occur at the lowest points along the intersection curves. Finally, every isolated point where three vc-obstacle surfaces intersect is automatically a critical point of ϕ. The critical points of ϕ on the intersection curve of vc-obstacle pairs will be called Type I points, while the critical points of ϕ at the intersection points of vc-obstacle triplets will be called Type II points. The two types of critical points are either saddles or local maxima, as stated in the following proposition. Proposition 2.4.1. The function ϕ:Fv →IR has non-smooth saddles at the Type I critical points, and non-smooth local maxima at the Type II critical points. 1 One must use the generalized normal of dst(q, Oi) when q has multiple nearest points on bdy(Oi). 31
- 39. P v jCOv iCO x y pv F Intersection curve injn pv v pv v pv v (a) (b) Figure 2.4: (a) A Type I critical point is a saddle point of ϕ in Fv. (b) The vc-obstacles slices in the vicinity of the saddle point. Proof Sketch: Let p be a Type I critical point of ϕ. Then p ∈ bdy(COv i ) ∩ bdy(COv j ), such that the tangent to the curve bdy(COv i ) ∩ bdy(COv j ) is purely horizontal at p. Since the vc-obstacles form columns whose cross-section expands monotonically along the ν axis, the point p must be a local minimum of ϕ along the intersection curve. Let Vp be the plane orthogonal to the curve bdy(COv i ) ∩ bdy(COv j ) at p. Let Up be a small three-dimensional ball centered at p. The free space in Vp ∩ Up looks like an inverted V shape, with p located at its upper vertex (see Figure 2.4(a)). It follows that ϕ has a local maximum within the set Vp ∩ Up. Since ϕ has a local minimum and maximum along orthogonal directions at p, it has a non-smooth saddle at this point. When p is a Type II critical point of ϕ, three vc-obstacles intersect at this point. The cross-section of each vc-obstacle expands monotonically along the ν axis. Hence at any fixed- ν slice above p, the three vc-obstacles strictly penetrate each other. Since Fv is bounded from above by the three vc-obstacles at p, the point p is a non-smooth local maximum of ϕ. A Type I critical point p is thus a saddle of ϕ in Fv. Based on stratified Morse theory [30], a locally connected fixed-ν slice of Fv just below p splits at the saddle and becomes two locally disconnected sets in the slices just above p. Example: Figure 2.4(a) shows a saddle point p where two vc-obstacles meet for the first time along the ν axis. Note that ϕ = (0, 0, 1) is orthogonal to the intersection curve 32
- 40. P v jCOv iCO x y v v kCO jn kn in Figure 2.5: A Type II critical point is a local maximum of ϕ in Fv. bdy(COv i ) ∩ bdy(COv j ) at p. Figure 2.4(b) depicts three fixed-ν slices: below p, through p, and above p. ◦ The next example illustrates the free space Fv at a Type II critical point. Example: Figure 2.5 illustrates a local maximum point p, where three vc-obstacles meet for the first time along the ν axis. The point p is a zero-dimensional manifold and hence it is a critical point of ϕ. ◦ 2.4.2 The Partition of F v Into 3D Cells The free position-velocity configuration space will be partitioned by vertical planes called vertical partitions, and by horizontal planes called horizontal partitions. Each of these par- titions will pass through a critical point of ϕ. The vertical partitions are defined as follows. Definition 6. Let p ∈ Fv be a saddle point of ϕ. Let P be the plane which passes through p and spanned by the vc-obstacle normals at p. The vertical partition at p is the portion of P ∩ Fv containing the point p. Every vertical partition is bounded from below by the (x, y, 0) plane, from above by the saddle point p, and from the “sides” by the vc-obstacles. The lower edges of the vertical partitions decompose the (x, y) plane into two-dimensional regions, as shown in Figure 2.6. To see that the vertical partitions are indeed vertically aligned, note that the tangent to the intersection curve of two vc-obstacles is purely horizontal at p. At a critical point of ϕ, the 33
- 41. (x,y) projection of 3D cells vertical partitions bottom edges saddle points projections kO jO iO Voronoi node Figure 2.6: The (x, y) projection of a stack of 3D cells. latter tangent is orthogonal to the vc-obstacle normals at p. Hence the plane spanned by the vc-obstacle normals at p is vertically aligned in (x, y, ν) space. The horizontal partitions are next defined. Definition 7. Let p ∈ Fv be a saddle or a local maximum point of ϕ. Let H be a plane parallel to the (x, y) plane passing through p. The horizontal partition at p is the portion of H ∩ Fv containing the point p. Each horizontal partition lies in a fixed-ν slice. When a horizontal partition passes through a local maximum point, it consists of this single point. When the partition passes through a saddle point, it forms a two-dimensional planar set bounded by nearby vc-obstacles. The union of the horizontal and vertical partitions induces a cellular decomposition of the free position-velocity space into 3D cells. Definition 8. The cellular decomposition of the free position-velocity space, Fv, consists of 3D cells bounded by the horizontal and vertical partitions passing through the critical points of ϕ and the vc-obstacles. The cellular decomposition of Fv consists of “stacks” of 3D cells separated by three vertical partitions (in the generic case) and vc-obstacles. The cells within each stack are separated by horizontal partitions, as illustrated in the following example. Example: Figure 2.7 depicts three vc-obstacles together with their horizontal and vertical partitions. The portion of Fv between the vc-obstacles forms a stack of four 3D cells, separated by horizontal partitions passing through three saddles and one local maximum of ϕ. ◦ A stack of 3D cells can have a more complex structure in non-generic situations. For 34
- 42. saddle points vertical partitions horizontal partitions 3D cell maximum point 3D cell 3D cell 3D cell x y v Figure 2.7: A stack of 3D cells is generically surrounded by three vc-obstacle and three vertical partitions. instance, when four identical obstacles are symmetrically arranged about a common center point, their stack of 3D cells is surrounded by four vc-obstacles and four vertical parti- tions. However, almost any local perturbation of the obstacle positions will give the generic structure described above. (x,y) projection of 3D cells vertical patitions’ bottom edges saddle points projections jO Voronoi node kO iO lO Figure 2.8: The (x, y) projection of the 3D cells associated with a non-generic obstacle arrangement. Example: Figure 2.8 depicts four symmetrically arranged obstacles in the plane. The corresponding 3D cells are surrounded by four vc-obstacles and four vertical partitions whose bottom edges can be seen in Figure 2.8. The portion of Fv between the vc-obstacles and the vertical partitions consists of five rather than four 3D cells, and the cells’ local maximum is the intersection point of four rather than three vc-obstacles. Note that almost any local perturbation of the obstacle positions would give the generic structure described above. ◦ 35
- 43. 2.5 The Cells Adjacency Graph The vc-method constructs an adjacency graph, denoted Gv, for the cellular decomposition of the free position-velocity space. Each vertex of Gv represents one 3D cell in the cellular decomposition. The vertex, pi = (xi, yi, νi), is selected at the cell’s center as follows. Its (xi, yi) coordinates are located at the point which maximizes the minimal distance from the surrounding c-obstacles in the (x, y) plane (it is a node on the Voronoi diagram). Note that the vertices in a stack of cells project to the same point in the (x, y) plane. The νi coordinate is selected at the midpoint between the ν-values of the cell’s bottom and top horizontal partitions (although it is possible to select any ν-value inside the cell). The edges of Gv are of two types: (i) Type A edge connects two vertices whose cells share a common vertical partition. The (x, y) projection of the edge coincides with the Voronoi edge connecting the (x, y) projections of the respective cell vertices. (ii) Type B edge connects two vertices whose cells share a common horizontal partition. The edge forms a vertical line whose (x, y) projection coincides with the Voronoi node underlying the cells’ vertices. When the robot traces a Type A edge it moves along a Voronoi edge with a continuously varying velocity. Tracing of a Type B edge requires that the robot change its velocity without any positional change. While not physically realizable, the vertical edges will guide the search for a pseudo time optimal chain of cells in the cellular decomposition of Fv. Example: Figure 2.9 depicts three c-obstacles surrounded by a rectangular wall in the (x, y) plane. The vc-obstacles shown in the figure induce a cellular decomposition of Fv, and the cells adjacency graph is depicted in the figure. ◦ Path Search on Gv: The vc-method assigns positive weights to the Type A edges and zero weights to the Type B edges (as the latter edges do not correspond to any positional change of the robot). The weight of a Type A edge reflects the minimum travel time along its underlying Voronoi edge. Let a Type A edge connect two adjacency graph vertices, pi = (qi, νi) and pj = (qj, νj), and pass through a saddle point pij = (qij, νij). The points lying on the upper boundary of Fv above pi and pj are local maximum points. Denote by 36
- 44. adjacency graph Voronoi edges x y v Figure 2.9: The adjacency graph of the cellular decomposition induced by three polygonal obstacles and an outer wall. νmax(pi) and νmax(pj) the speeds at the two local maxima. The minimum travel time along the underlying Voronoi edge consists of two motion modes. Starting at qi with maximal speed νmax(pi), the robot traces the Voronoi edge with maximal deceleration amin, until reaching the (x, y) projection of the saddle point pij. The robot next traces the remaining portion of the Voronoi edge with maximal acceleration amax, until reaching the endpoint qj with maximal speed νmax(pj). The Type A edge cost is given by Type A edge cost = νmax(pi) − νij |amin| + νmax(pj) − νij amax where νij is the lowest speed at the saddle point pij =(qij, νij). Any shortest path search algorithm on Gv gives a pseudo time optimal chain of 3D cells connecting the start cell to the target cell. Note that the chain of cells does not necessarily contains the geometrically shortest path, but rather an approximate minimum-time path that guides the robot safely from start to target under a uniform braking distance constraint. Computational complexity of the vc-method: The adjacency graph vertices project to Voronoi nodes in the (x, y) plane, while the Type A edges project to Voronoi edges in the (x, y) plane. In general, the Voronoi diagram of a polygonal environment containing n vertices has O(n) nodes and edges [7, 62]. Each vertical stack in the cellular decomposition of Fv consists of four 3D cells. Hence there are O(n) vertices in Gv. Each 3D cell is 37
- 45. start (a) 61 2 3 4 5 targetstart (b) s saddle saddle target x y amax acceleration amin deceleration 7 8 Figure 2.10: (a) The optimal path lies in vertical strips. (b) Path unfolding and vertical edges realization. horizontally connected to at most three neighboring cells, and is vertically connected to at most two cells. Hence there are O(n) edges in Gv. Standard shortest path search algorithms run in O(E log E) steps, where E is the total number of graph edges. Since Gv has size O(n), the vc-method constructs the adjacency graph in O(n) time and searches the graph in O(n log n) time. While the method is highly efficient, it only provides an approximate minimum-time path subject to a uniform braking distance constraint. 2.6 Practical Path Realization There are two practical path realization concerns. First, the optimal path on Gv contains purely vertical Type B edges that must be modified into physically realizable arcs. Second, the Type A edges project to Voronoi edges in the (x, y) plane. The Voronoi edges meet non-smoothly at the nodes, so they must be smoothed at the nodes according to the robot’s minimal turning radius. These two practical considerations are next discussed. Path realization: Let the Type A edges along the optimal path be momentarily represented as horizontal edges in (x, y, ν) space, as illustrated in Figure 2.10(a). The vertical strips along the optimal path can be unfolded into a common vertical plane, as shown in Figure 2.10(b). Note that the horizontal coordinate within this plane is the distance traversed along the path’s horizontal edges, denoted s in the figure. For any physical robot, it is reasonable to assume that its maximal acceleration does not exceeds its maximal deceleration, amax ≤ |amin|. Let us assume here that amax =|amin| (the path realization procedure can be adapted to the case where amax |amin|). A time optimal realization is obtained by shifting the 38
- 46. adjacency-graph path vertically upward in (x, y, ν) space, until it lies on the upper boundary of Fv. The shifted path vertices lie at local maximum points. Each arc of the shifted path traces a vc-obstacle downward with minimal slope amin until reaching a saddle point, then traces a vc-obstacle upward with maximal slope amax until reaching the next local maximum point (Figure 2.10(b)). The path realization procedure is summarized for start and target located on vc-obstacle boundaries. 1. Accelerate from the start position with amax to the first local maximum point along the path (arc 1 in Figure 2.10(b)). 2. Decelerate from a local maximum point with amin to the next saddle point along the path (arcs 2, 4, and 6 in Figure 2.10(b)). 3. Accelerate from the saddle point with amax to the next local maximum point along the path (arcs 3, 5, and 7 in Figure 2.10(b)). 4. Decelerate from the last local maximum point with amin along the path, until reaching the target with zero velocity (arc 8 in Figure 2.10(b)). The path realization procedure achieves minimum travel time among all possible re- alizations of the adjacency graph paths. The acceleration profile along the realized path is basically a Bang-Bang parametrization: it always selects the maximal deceleration and maximal acceleration, amin or amax, while maintaining a safe braking distance from the obstacles. The problem of minimizing the travel time along a specific given path is also known in the literature as the time optimal control problem of a double integrator. The objective in this problem is to bring a point mass moving with initial speed vS at xS to rest at the origin in minimum time using bounded acceleration. This can be easily extended to any terminal speed vT . The dynamics of this system is given by ˙x1(t) = x2 ˙x2(t) = u, with |u| 1, where x1(t) is the object’s position and x2(t) is its speed. A solution for this 39
- 47. 1x 2x Figure 2.11: A switching curve (in red) for the double integrator problem along which the point mass reaches the origin with zero speed. kB jB iB kB jB iBminr x y (a) (b) 1e 2e 1e 2e Figure 2.12: (a) The optimal path passing through a node. (b) The smoothed path follows the minimum safe turning radius arc. problem is derived by a switching curve (see Figure 2.11) in the x1−x2 plane which determines the exact time instant when u switches between its extreme values. See [14](Section 3.9) for full details. Path smoothing at the nodes: The Type A edges project to Voronoi edges that meet non-smoothly at discrete nodes in the (x, y) plane. But the robot must respect a velocity dependent lower bound on its turning radius, rmin(ν), specified in the appendix. When the optimal path in Gv passes through a vertex pi = (qi, νi), the robot changes its direction of motion along a circular arc of radius rmin(νi) which is tangent to the Voronoi edges meeting at qi. By following this circular arc with velocity magnitude which varies linearly between its values at the arc’s endpoints, the robot executes a smooth edge transition while satisfying the minimum turning radius constraint. The path smoothing procedure is illustrated in the following example. Example: Figure 2.12(a) depicts three c-obstacles in the (x, y) plane, together with three 40
- 48. 0 50 100 150 200 250 300 0 50 100 150 200 250 300 target start Voronoi edges vc-method lmin uW lW Figure 2.13: A disc robot navigating with high speed in an office environment. Voronoi edges meeting at a node. The (x, y) projection of the adjacency graph path follows the edges e1 and e2 indicated in the figure. The path must pass through the non-smooth node at a speed ν = νi prescribed by the (x, y, ν) path. Figure 2.12(b) schematically shows the circle having the minimal turning radius rmin(νi) (specified in the appendix). The locally smoothed path replaces the node and portions of e1 and e2 with a circular arc of radius rmin(νi) tangent to the path’s Voronoi edges at the node. The robot executes a smooth transition from e1 to e2 by following the circular arc with with speed that varies linearly between its values at the arc’s endpoints. ◦ 2.7 Simulations This section describes two simulation studies. The first study considers the effect of the inter- obstacle clearance on the vc-method paths. The second simulation compares the vc-method with the classical dynamic window approach. 2.7.1 Influence of Obstacles Clearance on VC-Method Paths Consider the simplified office environment depicted in Figure 2.13. The left space is an empty room, while the right space contains a large table of variable length. The Voronoi diagram 41
- 49. (a) s0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1 0 5 10 15 20 25 30 minl vc-method v Figure 2.14: The speed of the robot for δ = 3.4. which guides the construction of the adjacency graph is also depicted in the figure. The upper passage is assigned a variable width Wu, while the lower passage is kept at a fixed width, Wl, which equals 1.5 times the robot’s diameter. The ratio between Wu and Wl is defined as δ = Wu/Wl, where δ ≥ 1. Using the start and target indicated in Figure 2.13, the geometrically shortest path from start to target on the Voronoi diagram passes through the lower passage. In this particular environment, as long as 1 ≤ δ ≤ 3 the vc-method travels with low speeds through the lower passage. When δ 3, the geometrically longer path can be safely traced at a higher speed and hence with a shorter travel time. Figure 2.14 depicts the velocity profiles of the two paths for δ = 3.4. The vertical axis is the robot’s speed, ν, while the horizontal axis is the robot’s normalized position , s the path. Each velocity profile includes segments where the robot accelerates or decelerates with its maximal capabilities and segments where the robot travels with constant speed. The graph in Figure 2.15 plots the ratio between the vc-method travel time, t(vc-method), and the geometrically shortest path safe travel time on the Voronoi diagram, t(lmin), as a function of δ. Note that t(lmin) remains constant for all values of δ. When δ is small, the robot cannot travel rapidly through the top passage due to the braking distance constraint. Therefore it selects the geometrically shortest path through the bottom passage. As δ in- creases, the robot can safely travel with high speed through the top passage, thus achieving a sorter travel time compared with t(lmin). While the travel time improvement is modest, congested obstacle scenarios would give substantial travel time savings. 42
- 50. lower passage paths upper passage paths t(vc-method)/t(lmin) 3 Figure 2.15: The ratio t(vc-method)/t(lmin) of the vc-method as a function of δ. 2.7.2 Comparison of the VC-Method with the Dynamic Win- dow Approach The vc-method is next compared with the dynamic window approach [13, 26]. The dynamic window approach, or DWA, chooses the path’s velocity within a user specified time-window. Within each time-window the robot moves along a circular arc of radius r. The selection of r and the speed ν along this arc is based on maximization of an objective function that consists of three components: 1. Target heading: a function head(r, ν) measuring the inverse deviation of the robot heading from the target direction. 2. Obstacle clearance: a function dst(r, ν) measuring the minimal distance to the sur- rounding obstacles. 3. Forward velocity: a function vel(r, ν) specifying the robot’s normalized speed during tracing of the current circular arc. The objective function, denoted G(r, ν), is the weighted sum of the three elements: G(r, ν) = α · head(r, ν) + β · dst(r, ν) + γ · vel(r, ν) where α, β, and γ are user-specified weights. In each time-window, G(r, ν) is maximized over all turning radii r and speeds ν that ensure collision-free trajectories as well as braking 43
- 51. (b) goal start (a) start targettarget start vc-method DWA Voronoi edges Voronoi edges vc-method DWA target start DWA Voronoi edges (c) vc-method Figure 2.16: The vc-method and DWA paths in (a)-(b) a rectangular room containing an obstacle cluster, and (c) a urban-like environment. VC-method time DWA time VC-method length DWA length Example (i) 24 39 4.85 4.37 Example (ii) 31 52 6.25 2.63 Example (iii) 24 46 5.04 4.38 Table 2.1: Running time in seconds and length in meters of the three simulations. safety. In our simulations, the dynamic window approach is implemented with unit weights, α=β =γ =1, and time-window of one second. In order to study the difference between the vc-method and the dynamic window ap- proach, two scenarios are considered. The first scenario, depicted in Figure 2.16(a)-(b), consists of an obstacle cluster in a rectangular room. In Example (i), the start and target lie at opposite sides of the obstacle cluster (Figure 2.16(a)). The DWA path in this example passes between the obstacles through low-clearance passages. While the vc-method path is geometrically longer, it passes through wide-clearance passages that allow higher speeds subject to the uniform braking distance constraint. The robot’s speed profile is illustrated in Figure 2.17(a). As indicated in Table 2.1, the vc-method travel time is almost one half of the DWA travel time. In Example (ii), the target lies at the upper left corner of the room, where it is directly visible from the start (Figure 2.16(b)). The DWA path moves through the narrow passage along the geometrically shortest path to the target. The vc-method path circumnavigates the obstacle cluster along a significantly longer path. However, this path allows the robot to maximize its velocity while respecting the braking distance constraint (Figure 2.17(b)). The vc-method travel time is again almost one half of the DWA travel time (Table 2.1). 44
- 52. 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1 0 5 10 15 20 25 30 35 (b) 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1 0 5 10 15 20 25 30 35 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1 0 5 10 15 20 25 30 35 (a) DWA DWA vc-method vc-method minl v s s s v 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1 0 5 10 15 20 25 30 vc-method v vc-method (a) Figure 2.17: The velocity profiles of the paths illustrated in Figures 2.16(a)-(b). The second scenario, depicted in Figure 2.16(c), resembles urban city blocks surrounded by a rectangular boundary. The start and target are located at opposite corners of the envi- ronment. The vc-method path follows Voronoi edges that maintain wide clearance from the obstacles. This path permits high speeds subject to the uniform braking distance constraint. The DWA path passes through a low-clearance passage between adjacent city blocks. While this is a shorter path, the narrow passage forces the robot to slow down, resulting in a path whose travel time is almost twice longer than the vc-method travel time (Table 2.1). The simulations indicate that when the motivation is to reduce travel time rather than path length, wide clearance paths support higher speeds (and hence reduced travel time) under the uniform braking constraint. By methodically searching the free cells of position- velocity configuration space, significant travel time improvements can be obtained relative to more traditional high-speed navigation methods. 2.8 Experiments The vc-method was implemented on an Irobot Create 4400 platform. The robot’s mini- mal deceleration was set to amin = −100 mm/sec2 , the maximal acceleration was set to amax = 100 mm/sec2 , and the minimal turning radius was set to rmin = 300 mm. These scaled values reflect those of autonomous high-speed platforms (e.g. [3, 2]). Figure 2.18 de- picts the experimental setup. The environment contains three rectangular obstacles located within a rectangular outer boundary. The Voronoi diagram which underlies the adjacency graph construction is marked on the floor. The environment resembles the obstacle-cluster environment described in Section 2.7. 45
- 53. 1 2 3 Figure 2.18: The experiment setup of three polygonal obstacles within a rectangular outer boundary. 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1 0 5 10 15 20 25 30 35 (b) 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1 0 5 10 15 20 25 30 35 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1 0 5 10 15 20 25 30 DWA vc-method vc-method DWA vc-method vc-method minl v s ( / sec)v mm (sec)T s v 0 5 10 15 20 25 30 35 40 45 0 50 100 150 200 250 300 1 2 3 Figure 2.19: The velocity profiles of the three programmed paths. 46
- 54. The robot was programmed to trace three trajectories along the Voronoi diagram edges (Figure 2.18) and to implement different velocity profiles. The trajectories begin and end at the same start and target points, but each has a different velocity profile dictated by the uniform braking distance constraint. The experiment compares the geometrically shortest path along the Voronoi diagram (trajectory 1), an intermediate length path (trajectory 2), and the vc-method path (trajectory 3). The experimental path length, travel time, and average speed of the three trajectories are listed in Table 2.8. The velocity profiles of the three programmed paths are depicted in Figure 2.19. The geometrically shortest path (trajectory 1) takes the longest time. The low-clearance passages along this path force low average speed of 0.08 m/sec and travel time of 45 seconds. The intermediate length path (trajectory 2) moves with average speed of 0.14 m/sec and travel time of 32 seconds. The vc-method path (trajectory 3) moves through wide clearance passages with average speed of 0.26 m/sec and the shortest travel time of 26 seconds. Path Length (m) Duration (sec) Avg. speed (m/sec) Trajectory 1 3.5 45 0.08 Trajectory 2 4.5 32 0.14 Trajectory 3 6.7 26 0.26 Table 2.2: Experiment Results The experiment corroborates the vc-method usefulness for high-speed safe navigation in congested environments. Controlling the Irobot position and speed proved to be difficult in the experiments, due to its position sensor inaccuracy. This inaccuracy limited the robot ability to precisely trace the Voronoi edges and implement the desired velocity profile. It is interesting to note that on-line implementation of the method (which requires obstacle detection sensors not available on the Irobot) will be able to compensate for such positional inaccuracies based on local sensor readings. 2.9 Conclusion Maintaining safe braking distance from the obstacles is a major concern during high-speed mobile robot navigation. This chapter introduces position-velocity configuration space, where the safe braking constraint is modeled as vc-obstacle. Based on the critical points of the 47
- 55. robot’s velocity magnitude function, ϕ(x, y, ν) = ν, the chapter described a cellular decom- position of position-velocity configuration space into free cells. Each cell is associated with a particular range of velocities that can be safely followed by the robot. An adjacency graph constructed for the cellular decomposition allows a search for a pseudo time optimal path from start to target which avoids the vc-obstacles. The path follows Voronoi edges which are smoothed to ensure safe turning at the prescribed velocity magnitudes. The resulting high-speed path leads the robot from start to target while maintaining safe braking distance from the obstacles. The vc-method treats the two velocity-dependent safety concerns, safe braking distance and safe turning radius, at different stages of the planning process. The safe braking distance is modeled by the vc-obstacles in position-velocity configuration space. The adjacency graph path avoids the vc-obstacles and thus ensures the uniform braking distance constraint. The (x, y) projection of the adjacency graph path traces Voronoi edges that meet non-smoothly at discrete nodes. The turning radius safety constraint is enforces as a post-processing stage, by locally smoothing the path at the Voronoi nodes. This division of the two velocity-dependent safety concerns seems to offer practical advantages in terms of computational efficiency and ease of implementation. Based on simulations and experiments, the vc-method offers substantial improvement of total travel time required for safe mobile robot navigation. The method computes an approx- imate minimum-time safe path in O(n log n) time, where n is the total number of obstacle vertices in the case of a polygonal environment. While the vc-method is highly efficient, it does not provide an upper bound on the quality of approximation with respect to the exact time-optimal safe path. Next, we propose the speed graph method which computes the time optimal path of a mobile robot subjected to similar dynamic constraints as discussed here. Although it first seems like an NP-hard problem, we will show that the time optimal path computation can be reduced into a low polynomial complexity. 48
- 56. Chapter 3 The Speed Graph Method: Time Optimal Navigation Among Obstacles Subject to Safe Braking Constraint 49
- 57. 3.1 Introduction This chapter considers the time optimal paths of a mobile robot navigating in a polygonal en- vironment subject to uniform braking safety constraints. Similarly to Chapter 2, stationary obstacles are assumed as well as uniform braking safety. This enables to formulate to encode the braking safety as a state (position and velocity) dependant constraint, which in turn allows formulation of the safe time optimal navigation problem using calculus of variations. The time optimal path minimizes a travel time functional, denoted T(α), defined over all collision free paths α connecting the start and target. The work presented here, analyzes the properties of T(α), then uses these properties to develop the speed graph method which com- putes safe time optimal paths in environments populated by polygonal obstacles. The speed graph consists of safe time optimal arcs connecting special via points located on the Voronoi diagram of the environment. The speed graph is used to efficiently select the most promising path homotopy class connecting the start and target in the environment1. Convexity prop- erties of T(α) are then used to compute the safe time optimal path within the candidate homotopy class as a convex optimization problem in O(n3 log(1/ )) time, where is the de- sired solution accuracy. Importantly, the entire process takes place in the robot’s state space, thus treating the geometric path planning and the speed profiling in a single framework. The chapter is structured as follows. Section 3.2 formulates the safe time optimal navi- gation problem under uniform braking safety constraints. Section 3.3 describes analytic so- lutions for the safe time optimal path near a point obstacle and a wall segment. Section 3.4 formulates the safe time optimal navigation problem in polygonal environments, then de- scribes basic properties of the safe travel time functional in such environments. Section 3.5 describes the speed graph whose construction uses the point obstacle and wall segment time optimal solutions. Section 3.6 describes a convex optimization scheme for computing the exact time optimal path in each path homotopy class suggested by the speed graph. Sec- tion 3.7 discusses simulations and preliminary experiments. The concluding section discusses extensions of the safe time optimal navigation problem to more demanding scenarios. Two appendices contain auxiliary material. 1 The notion of path homotopy is reviewed later in the chapter. 50
- 58. 3.2 Problem Description and Preliminaries This chapter considers the following problem, first formulated by Fraichard [27]. A mobile robot modeled as point or a disc has to navigate with high speed in a planar environment pop- ulated by static polygonal obstacles. The robot is assumed to move freely in IR2 and its state, denoted (q, ν) ∈ IR2 × IR+ , consists of its position, q = (x, y), and its speed ν = ||v|| = || ˙q||. While the robot navigates with high speed in the given environment, emergency events may occur at any time along obstacle boundaries. For instance, a vehicle driving autonomously along an urban road must limit its speed to a level allowing the vehicle to brake and reach a full stop when another car suddenly emerges from its parking spot, or a pedestrian suddenly emerges between two parked vehicles. To ensure motion safety, the robot must brake and reach a full stop without hitting any surrounding obstacles whenever such an event is detected by the robot. Braking safety is ensured when the robot maintains the following safe braking distance constraint. Note that some notations where already defined in Section 2.2, however for ease of reading we present them again. Definition 9. The robot’s safe braking distance, denoted d, is the length of the shortest kinematically feasible braking path leading from the robot’s current state (q, ν) to a full stop at ν = 0. The shortest braking path may be any kinematically feasible path along which the robot applies maximal deceleration (a system dependent parameter), eventually reaching a full stop without hitting any obstacle. By maintaining a safe braking distance with respect to the surrounding obstacles, the robot can handle emergency events that typically occur along obstacle boundaries. Note that mobile robots are deployed in environments containing static as well as dynamic obstacles. The current work assumes static obstacles, with the intention of incorporating dynamic obstacles in future work. To derive an expression for the safe braking distance as a function of the robot’s current speed, first consider the robot’s maximal deceleration, denoted amin. Braking on the verge of sliding determines the robot’s maximal deceleration. The net frictional reaction force acting at the robot’s wheels during maximal deceleration is given by µmg, where µ is the coefficient of friction at the ground contacts, m is the robot’s mass, and g is the gravitational 51
- 59. acceleration. The robot’s dynamics during maximal deceleration is given by the equation mamin = µmg, and this equation gives the maximal deceleration: amin = µg. (3.1) While amin does not depend on the robot’s mass, the coefficient of friction µ usually depends on m due to local deformations at the robot’s wheels [68][p. 15]. The safe braking distance is computed using energy balance. When the robot moves along a kinematically feasible braking path, the frictional reaction forces at the contacts are aligned with the friction cone edge opposing the direction of motion (note that the wheels are rolling without sliding during this braking phase). A complete stop of the robot requires that all its initial kinetic energy be absorbed by the braking force. This energy balance leads to the expression (see e.g. [27]), d(ν) = 1 2amin ν2 (3.2) where amin is the robot’s maximal deceleration, and ν is the robot’s initial speed. The robot is required to maintain a uniform braking safety distance from all surrounding obstacles, as stated in the following definition. Definition 10. When the mobile robot travels with speed ν, uniform braking safety is ensured when the robot is kept at least d(ν) away from the nearest obstacle at every state (q, ν). Uniform braking safety is clearly a conservative safety criterion for autonomous robots. To satisfy this requirement, the robot must maintain a circular safety zone in order to allow safe deceleration to a full stop without hitting any obstacle. Since d(ν) is monotonically increasing in ν, the circular safety zone increases in size when the robot navigates with higher speeds, thus limiting the robot’s maximal speed at every point in the environment. In particular, when the start or target are located at an obstacle boundary point, the robot must start or reach the respective point with zero velocity. Note that the actual frictional work is accomplished by the braking pads of the robot and not by the static friction between the wheels and the ground. However if we assume that the 52
- 60. braking pads can deliver a larger braking force than the friction force at the contacts then the previous development sustains. 3.3 The Safe Time Optimal Path Near a Single Ob- stacle Feature This section formulates the safe time optimal path variational problem, then describes an- alytic solutions for the safe time optimal path near a single obstacle feature which can be a wall segment or a point obstacle. These solutions will later be used to compute the safe time optimal path in polygonal environment. 3.3.1 The Travel Time Functional The robot’s path can be any piecewise smooth curve α(s) : [0, 1] → IR2 with endpoints pS = α(0) and pT = α(1). When the geometric parameter s is parameterized by time, s(t), the robot’s speed along α is given by ν(s(t)) = d dt α(s(t)) = α (s) ds(t) dt where α (s) = d ds α(s). Solving for dt gives: dt = α (s) ν(s) ds, (3.3) Integrating both sides of (3.3) gives the travel time functional: T(α) = 1 0 α (s) ν(s) ds, (3.4) and its minimization over all safe paths α defines the following variational problem. Definition 11. The travel time variational problem seeks to minimize T(α) over all piecewise smooth paths α(s) : [0, 1] → IR2 connecting pS and pT , where the speed ν along α satisfies the uniform braking constraint d(ν) = ν2/2amin. 53
- 61. Note that T(α) is a path dependent function. The collection of piecewise smooth paths in IR2 forms a metric space under the d1 metric: d1(α1, α2) = max s∈[0,1] { α1(s)−α2(s) }+ max s∈[0,1] { α1(s)−α2(s) }, where α1 and α2 are two piecewise smooth paths connecting the same end points. Moreover, T(α) is a continuous function of α with respect to this metric [29]. The inte- grant of T(α) in (3.4), denoted F, is given by F α(s), α (s), s = α (s) ν(s) . Based on calculus of variations [29], when F is differentiable with respect to its arguments, any extremal path of T(α) over all piecewise smooth paths connecting pS and pT must satisfy the Euler-Lagrange equation: ∂F ∂α − ∂ ∂s ∂F ∂α = 0, (3.5) where α and α are to be treated as formal variables. The safe time optimal path near a wall segment and a point obstacle is next computed. 3.3.2 The Safe Time Optimal Path Near a Wall Segment First consider the case of a point robot traveling near a wall segment. This case is a direct extension of the classical Brachistochrone problem. The Brachistochrone problem considers a mass particle sliding under the influence of gravity on a wire, and computes the wire’s geometric shape that would give minimum travel time between two fixed endpoints [11]. In our case the robot’s maximal deceleration, amin, replaces the gravitational acceleration as follows. Consider a wall segment aligned with the x axis as shown in Figure 3.1(a). Denote by pS = (xS, yS) and pT = (xT , yT ) the start and target, which are assumed to lie in the upper 54
- 62. half of the infinite strip spanned by the wall segment. To encode the safe braking constraint, note that the robot’s minimal distance from the wall is given by its y coordinate. Along a safe time optimal path, the robot must keep a distance of at least d(ν) away from the wall segment: y ≥ d(ν). Substituting d(ν) = ν2/2amin while requiring the robot’s speed to be maximized for travel time minimization gives: ν(y) = 2amin · y y ≥ 0. (3.6) In the case of a wall segment, let us use the horizontal x coordinate as the path param- eter s. The robot’s path is thus parameterized by α(x) = (x, y(x)) for x ∈ [xS, xT ]. In this parametrization α (x) dx = 1 + (y (x))2dx, and the safe travel time functional (3.4) is given by T(α) = xT xS F y(x), y (x) dx where F y, y = 1 + y 2 2amin · y y ≥ 0, (3.7) with the end conditions y(xS) = yS and y(xT ) = yT . Any extremal path of T(α) must satisfy the Euler-Lagrange equation (3.5). Substitut- ing for F(y, y ) in this equation, while considering the fact that ∂G(y , y, x)/∂x = 0, then integrating with respect to x, gives the implicit scalar differential equation: 1 = c2 1 + y (x) 2 y(x) (3.8) where c is yet to be determined. Note that the robot’s maximal deceleration, amin, does not appear in (3.8). Thus, while the extremal path’s total travel time depends on amin, 55
- 63. 0 5 10 15 20 25 30 35 0 5 10 15 20 25 30 y x 5 10 15 20 25 30 35 40 0 5 10 15 20 25 30 y x wall segment (b)(a) time optimal path is a cycloid Sp Tp rolling circle wall segment Figure 3.1: (a) A circle rolling on the wall segment generates the cycloid curve. (b) The safe time optimal path near a wall segment. its geometric shape is invariant with respect to amin. The extremal path solving (3.8) is a cycloid, α(ξ) = (x(ξ), y(ξ)), where ξ represents the angle that a point fixed on a rolling circle spans with respect to the vertical direction (Figure 3.1(a)). The formula for α(ξ) is specified in the following lemma. Lemma 3.3.1. The safe extremal path of a point robot moving in a planar environment near a wall segment aligned with the x axis is the cycloid curve: α(ξ) = x0 0 + 1 2c2 ξ − sin ξ 1 − cos ξ ξ ∈ [ξS, ξT ] (3.9) where ξS, ξT , x0, and c are determined by the path’s endpoints pS and pT . Moreover, α(ξ) is a local minimum of T(α). While a formal proof of the lemma is omitted (see e.g. [29]), the Cycloid curve is known of being the time optimal path connecting two points under a constant gravity field. Since the problem formulated in this section is identical to the Brachistochrone problem, this Cycloid curve is time minimal and unique. Note that the time optimal path is uniquely determined by its endpoints pS and pT . Also note that the time optimal path reaches the x axis along the normal direction to this axis (Figure 3.1(a)). Moreover, in addition to keeping a safe braking distance, this path segment also maintains the safe turning radius constraint that prevents sliding events (see Appendix D). Example: The time optimal path of a point robot traveling near a wall segment from pS to 56
- 64. pT is shown Figure 3.1(b). The path is a cycloid segment that bends with convex curvature away from the wall segment. ◦ To complete the wall segment case, consider a scenario where the robot is a disc of radius ρ. The time optimal path of the robot’s center is given by the formula: α(ξ) = x0 ρ + 1 2c2 ξ − sin ξ 1 − cos ξ ξ ∈ [ξS, ξT ] where ξS, ξT , x0, and c are determined by the endpoints pS and pT . 3.3.3 The Safe Time Optimal Path Near a Point Obstacle Next consider the case of a point robot traveling near a point obstacle located at the origin of the (x, y) plane. Using polar coordinates centered at the origin, (r, θ), the robot’s position is given by (x, y) = (r cos θ, r sin θ). The robot’s distance from the point obstacle is given by its r coordinate. Braking safety requires that the robot keep a distance of at least d(ν) away from the point obstacle: r ≥ d(ν). Substituting d(ν) = ν2/2amin while maximizing the robot’s speed for travel time minimiza- tion gives: ν(r) = √ 2amin · r r ≥ 0. Let the robot’s path be parameterized by θ, so that α(θ) = (r(θ) cos θ, r(θ) sin θ) for θ ∈ [θS, θT ]. Under this parametrization α (θ) dθ = r2(θ) + (r (θ))2dθ, and the travel time functional is given by T(α) = θT θS F r(θ), r (θ) dθ 57
- 65. where F(r, r ) = r2 + r 2 2amin · r r ≥ 0, (3.10) with the end conditions r(θS) = rS and r(θT ) = rT . Substituting for F(r, r ) in the Euler- Lagrange equation (3.5), then integrating with respect to θ, gives the implicit scalar differ- ential equation: c2 r2 (θ) + (r (θ))2 = r3 (θ) (3.11) where c is yet to be determined. Much like the wall segment case, the travel time T(α) depends on amin, but the geometric shape of the extremal path solving (3.11) is invariant with respect to amin. The solution of (3.11) is a parabolic curve specified in the following lemma. Lemma 3.3.2. The safe extremal path of a point robot moving in a planar environment near a point obstacle located at the origin is the parabolic curve: α(θ) = r(θ) cos θ, r(θ) sin θ θ ∈ [θS, θT ] where r(θ) is given by r(θ) = 2c2 1+sin(θ−θ0) cos2(θ − θ0) θ ∈ [θS, θT ] (3.12) such that θS, θT , c, and θ0 are determined by the path’s endpoints pS and pT . Moreover, α(θ) is a local minimum of T(α). While a formal proof of the lemma is omitted, let us note that ∂2F(r, r )/∂2r 0 and that F(r, r ) is differentiable with respect to r and r . This condition implies that the extremal path is a local minimum of T(α) [29][Theorem 5.5]. In addition, since this problem is known by having a global minimum (Lemma. 3.4.2), the path α(θ) which is the unique extremal path must be that global and unique minimum. The safe time optimal path is uniquely determined by pS and pT , except when the path’s endpoints lie on opposite sides of the point obstacle. This special case is illustrated in the following example. Note that this path segment also maintains the safe turning radius 58
- 66. point obstacle point obstacle ( )r 0s 1s Sp Tp x y ( )r Sp Tp y x (a) (b) l ( )r Sr Tr Figure 3.2: (a) Two symmetrically shaped time optimal paths from pS to pT . (b) The safe time optimal path is a parabolic arc equidistant from the point obstacle and the line l. constraint that prevents sliding events (see Appendix D). Example: Consider a point robot traveling near a point obstacle located at the origin. When pS and pT are located on opposite sides of the obstacle as depicted in Figure 3.2(a), there are two symmetric safe time optimal paths leading from pS and pT . Except for this special case, all other locations of pS and pT in IR2 are connected by a unique time optimal path. Note that the safe time optimal paths depicted in Figure 3.2(a) are convex arcs that bend away from the point obstacle. ◦ The safe time optimal path can be graphically constructed as follows. Let (rS, θS) and (rT , θT ) be the polar coordinates of pS and pT with respect to the point obstacle. Construct the line l tangent to the circles centered at pS and pT with radii rS and rT , as shown in Figure 3.2(b). The safe time optimal path is the parabolic arc equidistant from the point obstacle and l. To complete the point obstacle case, the safe time optimal path of a disc robot traveling near a point obstacle is considered in Appendix B. 59
- 67. 3.4 Basic Properties of the Safe Time Optimal Path in Polygonal Environments This section formulates the safe time optimal variational problem in multiple obstacle envi- ronments2, then discusses basic properties of the safe travel time functional in these envi- ronments. These properties will guide the speed graph construction and the optimal path computation. 3.4.1 The Safe Time Optimal Variational Problem in Polyg- onal Environments When a mobile robot is subjected to a uniform braking safety constraint, its maximal allowed speed is governed by its distance to the nearest obstacles. Consider a polygonal environment consisting of an outer boundary, O0, populated by internal obstacles O1, . . . , Ok. Let F denote the obstacle-free portion of the environment together with the obstacles’ boundaries. The robot’s path can be any piecewise smooth curve, α(s) : [0, 1] → F, such that α(0) = pS and α(1) = pT . Braking safety requires that as the robot moves along α(s), it must keep a distance of at least d(ν) away from the nearest obstacles, min i=0...k {dst (α(s), Oi)} ≥ d(ν(s)) s ∈ [0, 1], (3.13) where dst(α(s), Oi) is the minimal distance between the robot positioned at α(s) and the obstacle Oi. Travel time minimization requires maximal speed. Substituting the robot’s maximal allowed speed, d(ν) = ν2/2amin, into (3.13) gives the maximal safe speed: ν(s) = 2amin· min i=0...k {dst (α(s), Oi)} s ∈ [0, 1]. The safe travel time functional to be minimized is thus given by T(α) = 1 0 F α(s), α (s) ds, 2 The term variational problem refers to optimization of a path functional over a set of admissible paths. 60
- 68. where F(α, α ) is given by F α, α = α 2amin·mini=0...k{dst(α, Oi)} . (3.14) The minimization of T(α) over all piecewise smooth paths connecting pS with pT in F defines the safe time optimal variational problem. Note that F(α , α) is continuously differentiable with respect to the variables α and α, except at special points which are equidistant from two or more obstacle features in the environment. Next we define the energy functional associated with T(α). Definition 12. The energy functional associated with T(α) is the functional T(α) having the squared integrant, T(α) = 1 0 ¯F α (s), α(s) ds, where ¯F α, α = α 2 2amin·mini=0...k{dst(α, Oi)} . (3.15) Although the integrants of T(α) and T(α) are almost identical, there is no correspondence between their extremal paths. This would have enabled us to project the convexity properties of T(α) on T(α). Consequently, an alternate approach had been taken to prove monotonicity of T(α) in the paths α as described in Appendix C. 3.4.2 Basic Properties of the Safe Travel Time Functional This section describes basic properties of the safe time optimal paths in F. Let us first review the notion of homotopic paths. Definition 13. Two continuous paths with endpoints pS and pT , α(s) : [0, 1] → F and β(s) : [0, 1] → F, belong to the same homotopy class if there exists a continuous mapping, f(s, t) : [0, 1] × [0, 1] → F, such that f(s, 0) = α(s), f(s, 1) = β(s) for s ∈ [0, 1], with f(0, t) = pS, f(1, t) = pT for t ∈ [0, 1]. The following example illustrates the notion of path homotopy classes. 61
- 69. Example: Let the point obstacle depicted in Figure 3.2(a) represent a disc obstacle of small radius. The two paths depicted in this figure belong to distinct homotopy classes, since one path cannot be continuously deformed into the other without crossing the obstacle. ◦ Two paths belonging to the same path homotopy class can be thought of as “points” connected by a continuous curve (the path homotopy), in the metric space of piecewise smooth paths in IR2 . Under this interpretation every path homotopy class forms a connected set in the metric space. Before proceeding on with the properties of the time optimal path, a fundamental argument about the continuity of T(α) is presented and summarized by the next Lemma. Lemma 3.4.1 (Continuity of T(α)). The travel time functional, T(α), is continuous with respect to paths connecting pS and pT in the metric space induced by the d1 metric. Proof. The travel time functional T(α) is given by: T(α) = 1 0 F α, α ds, where, F α, α = ||α (s)|| 2amin · mini=0...k{dst (α(s), Oi)} , for s ∈ [0, 1]. Note that F is a ratio between two functions which are continuous in α and α. Hence it is a continuous function in the pair (α, α ) at points where the denominator does not vanish. Therefore, for every 0 there exists a δ( ) 0 such that for any two paths α and β that satisfy: d1 (α(s), β(s)) = max s∈[0,1] {||α(s) − β(s)||} + max s∈[0,1] {||α (s) − β (s)||} δ, the integrant F satisfies: F α, α − F β, β , (3.16) for every s ∈ [0, 1]. Next, examine the travel time difference for two neighboring paths, 62
- 70. -0.04 -0.03 -0.02 -0.01 0 0.01 0.02 0.03 0.04 -0.03 -0.02 -0.01 0 0.01 0.02 0.03 -30 -20 -10 0 10 20 30 -30 -20 -10 0 10 20 30 2L zoom ( )d TpSp (a) (b) iO iO Figure 3.3: (a) A hypothetical time optimal path touching a convex obstacle vertex. (b) The path can be moved away from the vertex, with a new segment of length L 2 . T(α) and T(β), in the same homotopy class, T(α) − T(β) = 1 0 F(α, α )ds − 1 0 F(β, β )ds = 1 0 F (α, α ) − F(β, β ) ds ≤ 1 0 F(α, α ) − F(β, β ) ds. Based on (3.16) we get: T(α) − T(β) ≤ 1 0 F(α, α ) − F(β, β ) ds ≤ 1 0 ds = , which shows that for every 0 there exists a δ( ) 0, such that if d1(α, β) δ then |T(α) − T(β)| . This proves the continuity of T. The following lemma establishes the existence of a safe time optimal path in every path homotopy class connecting pS and pT in F. Proposition 3.4.2 (Optimal Path Existence). Let F be a bounded polygonal environ- ment populated by obstacles with non-empty interiors. The safe travel time functional T(α) attains a global minimum in each path homotopy class connecting pS and pT in F. The corresponding time optimal path lies in the interior of F, except at its endpoints that can lie on obstacle boundaries. It will be later shown that T(α) attains a unique minimum in each path homotopy class of F. Proof sketch: An argument explaining why T(α) attains a global minimum in each path homotopy class of F is based on the continuity of T(α) (Lemma 3.4.1) as follows. Let M be the union of all voronoi arcs, and all proximity cell boundary lines spanned within a distinct 63
- 71. path homotopy class of F. Denote by pi the crossing points between the path α with the segments ci ∈ M, for i = 1...k, where k is the number of segments crossed by α. Note that k ≤ m where m = size(M) is upper bounded in every path homotopy class. Next, let us parameterize the position of every crossing point pi along its respective segment ci with the parameter ui ∈ [0, 1]. When ui = 0, the crossing point pi is positioned on one end of the segment ci. As ui grows, the crossing point pi is shifted continuously towards the other end of ci. Let α be the concatenation of time optimal path segments connected at the crossing points. Then for every specific selection of the parameters u1, ..., uk the complete path is uniquely determined and the travel time along it can be computed. The travel time T(α) can now be written in terms of the parameters u1, ..., uk, as T(α(u1, ..., uk)), which is continuous over the compact domain u1 ×...×uk ∈ [0, 1]×, ..., [0, 1], and thus possesses a global minimum in this domain. Let us accept this global minimality as a fact, and proceed to show that every local minimum of T(α) lies in the interior of F. Braking safety requires that the robot must travel with zero speed along any obstacles’ boundary segment, thus giving T(α) = ∞ along any boundary segment. It follows that a local minimum path of T(α) can only touch isolated obstacle boundary points otherwise T(α) = ∞. If such an isolated point lies in the interior of a polygon edge or at a concave polygon vertex, the path can be locally shortened and moved away from the obstacle, resulting in a reduced total travel time along the modified path. A local minimum path of T(α) can therefore touch only isolated convex obstacle vertices. Suppose the path touches a convex obstacle vertex as depicted in Figure 3.3(a). The path locally consists of two cycloids that reach the vertex along the respective edge normals. Such a path can be moved away from the vertex as depicted in Figure 3.3(b), resulting in a shorter path that maintains larger clearance from the obstacle. The modified path has a reduced total travel time, implying that any local minimum path of T(α) must lie in the interior of F. Calculus of variations is next used to establish the C(1) smoothness of the safe time optimal path. Proposition 3.4.3 (Optimal Path Smoothness). Every safe time optimal path connect- ing pS and pT in F is a C(1) curve having a continuous tangent. 64
- 72. Proof: Every obstacle feature in the environment (a polygon vertex or an edge) defines a proximity cell, consisting of all points in F which are closest to the given obstacle feature. The proximity cells partition the free space F, and every path connecting pS and pT in F consists of segments that lie inside individual proximity cells. Let α∗ be a safe time optimal path connecting pS and pT in F. This path lies in the interior of F according to Prop 3.4.2. The path satisfies the Euler-Lagrange equation in each proximity cell, and hence forms a C(1) curve within each cell. Let two segments of α∗ meet at a corner point, p0 = α∗(s0), located on the common boundary of two proximity cells in F. The term ∂F(α, α )/∂α appearing in the Euler-Lagrange equation is a continuous function of the path parameter s along any extremal path of T(α) [29][Section 15].3 Using the expression for F(α, α ) specified in (3.15), the derivative ∂F(α, α )/∂α is given by ∂F(α, α ) ∂α = 1 L(α) · α (s) α (s), where L(α) = 2amin · mini=0...k {dst(α, Oi)}. Continuity with respect to s at p0 gives: 1 L(α∗((s− 0 ))) · α ∗(s− 0 ) α ∗ (s− 0 ) = 1 L(α∗((s+ 0 ))) · α ∗(s+ 0 ) α ∗ (s+ 0 ). (3.17) Since L is a continuous function of s, L(α∗(s− 0 )) = L(α∗(s+ 0 )), eq. (3.17) implies that α ∗(s− 0 ) = α ∗(s+ 0 ), and α∗ is therefore a C(1) curve. The next lemma establishes the minimality of the extremal paths of T(α). Lemma 3.4.4 (Optimal Path Minimality). Every extremal path of the safe travel time functional T(α) which lies in the interior of F is a local minimum of T(α). Proof Sketch: We will use the environment’s x coordinate as the robot’s path parameter s. The robot’s path is thus parameterized by α(x) = (x, y(x)) for x ∈ [xS, xT ]. Consider the travel time functional T(α), T(α) = xT xS F x, y(x), y (x) dx. 3 This property is known as Erdman’s corner condition in calculus of variations. 65
- 73. where F = 1 + y 2(x) 2amax · mini=1...k{dst (α(x), Oi)} dx. Sufficient conditions for a path α(x) = (x, y(x)) to be a local minimum of T(α) are: 1. The path α is an extremal of T(α). 2. Along the path α, the second derivative ∂2F ∂α 2 (α, α , s) = ∂2F ∂y 2 (y, y , x) 0. 3. The interval [xS, xT ] contains no points conjugate to xS. See ([29](chapter 5)) for more details. The first condition stated above is clearly satisfied since every extremal path under consideration must be a solution of the Euler-Lagrange equation given by, ∂F ∂α − d ds ∂F ∂α = 0, or in cartesian coordinates as follows: ∂F ∂y − d dx ∂F ∂y = 0. Moreover, each extremal is C(1) since it satisfies the Erdmann corner condition as described by Prop. 3.4.3. Next, consider the functional F = 1+y 2(x) 2amax·mini=1...k{dst(α(x),Oi)} dx. Its second derivative with respect to y is given by, Fy y = 1 2amax · mini=1...k{dst (α(x), Oi)} · 1 (1 + y 2)3/2 0. Since Fy y 0, the second condition stated above is met not only along an extremal path but also along every other path. The condition is also known as the strengthened Legen- dre Condition. Note that necessary conditions only require semi positiveness of Fy y and therefore, here the strengthened form is met. Note that generally, the integrant F(y, y ) must be continuously differentiable with re- spect to y(x) and y (x). In our case, F is continuously differentiable with respect to y but only piece-wise differentiable with respect to y. At points where two obstacles are equidistant F loses its differentiability due the minimum function in its denominator. However, these points coincide with the corner points of the path and hence in every interval between two 66
- 74. subsequent corner points the integrant F is continuously differentiable. Last, we will omit a formal proof of the third condition, also known as the Jacobi condition and assume that there are no points conjugate to xS along an extremal path. This is a common assumption in the literature and in many applications, the absence of conjugate points is validated by substituting the computed extremal solution in the Jacobi equation (For e.g. [82](p. 361),[83](p.58),[29](p.112),[84](p.161)). This allows us to derive the minimality of α. Note that although sufficient conditions for a minimum are not fully satisfied, neces- sary conditions are met. Results appearing in the continuation of this thesis support this minimality (For e.g. see Sec. 3.6.1). The following theorem establishes the uniqueness of the safe time optimal path in each path homotopy class connecting pS with pT in F. Theorem 2 (Optimal Path Uniqueness). The safe travel time functional, T(α), pos- sesses a unique time optimal path in each path homotopy class connecting pS with pT in F. Proof sketch: According to Prop 3.4.2, T(α) attains a global minimum in every path homotopy class connecting pS with pT in F. Suppose one of these path homotopy classes contains two local minimum paths, α∗ and β∗ . Since α∗ and β∗ belong to the same path homotopy class, the two paths can be joined by a path homotopy, f(s, t) : [0, 1] × [0, 1] → F, such that f(s, 0) = α∗(s) and f(s, 1) = β∗ (s) for s ∈ [0, 1]. The path homotopy f(s, t) can be interpreted as a continuous path connecting α∗ with β∗ in the metric space of piecewise smooth paths in IR2 . The functional T(α) is a continuous function of α. According to the mountain pass theo- rem [53, 81], the collection of all continuous paths (each being a path homotopy) connecting α∗ and β∗ contains a distinguished path along which the maximal value attained by T(α) is minimized.4 Moreover, the maximal value is attained at an extremal path, γ, which is necessarily a saddle point of T(α). The path γ must lie in the interior of F based on the following argument. Braking safety implies that γ can only touch isolated obstacle boundary 4 The mountain pass theorem requires that T(α) be defined on a closed and bounded set of paths in the d1 metric space [53]. This condition is met on the set of piecewise optimal paths connecting pS with pT in F. 67
- 75. points. Suppose γ touches an obstacle vertex. Consider a path homotopy between α∗ and β∗ obtained by locally pulling the path homotopy containing γ away from the vertex as depicted in Figure 3.3. The travel time along the modified path homotopy is reduced, and the maximal value attained by T(α) along the modified path homotopy is lower than T(γ). Therefore γ must lie in the interior of F. This leads to a contradiction, since every extremal path of T(α) in the interior of F must be a local minimum according to Lemma 3.4.4. It follows that T(α) attains a unique local minimum in each path homotopy class connecting pS with pT in F. The last Prop. considers cases where the time optimal path coincides with a Voronoi arc. Proposition 3.4.5 (Voronoi edge optimality). Given start and target positioned along an individual straight or parabolic Voronoi arc e, the time optimal path α∗ coincides with the segment of e bounded by pS and pT . Proof Sketch: First, assume that e is a Straight Voronoi arc. When pS and pT belong to e, the shortest path connecting them is the portion of e bounded by pS and pT . Therefore this portion of e must be the time optimal path connecting pS and pT since it maximizes the robot’s clearance and speed and additionally ensures minimal travel distance. Next consider a parabolic Voronoi arc spanned between a wall segment and a point obstacle. Since this type of Voronoi arc is known of having a parabolic shape [7], its formula can be readily parameterized by polar coordinates with the point obstacle being at the origin as follows: r(θ) = y0 1 + sin(θ − θ0) cos2(θ − θ0) , (3.18) where y0 is the distance between the wall segment and the point obstacle, θ0 is the polar angle in which the parabolic arc acquires its minimal clearance from the obstacles and θ ∈ [θS, θT ]. Note that (3.18) is identical to the time optimal path near a point obstacle (3.12) with 2c2 = y0 and hence it is optimal with respect to this obstacle feature. Consider small perturbations of the path away from e towards the wall segment. Such small perturbations will increase the total length of the path. Moreover since e is equidis- tant from the wall segment and the point obstacle, any small perturbation of the path will 68
- 76. 1o (b) speed graph edges speed graph nodes Sp Tp 1e 2e 3e 4e 2o1o (a) 2o Figure 3.4: (a) The Voronoi diagram, and (b) The speed graph of an environment populated by two polygonal obstacles (Voronoi arcs are solid curves, speed graph edges are dashed curves). necessary reduce the permitted safe speed. Therefore e is optimal. 3.5 The Speed Graph This section describes the speed graph, which will allow selection of the path homotopy class that most likely contains the global time optimal path connecting pS with pT in F. The speed graph construction will use the following Voronoi diagram of F [8, 54]. Definition 14. The Voronoi diagram of a polygonal environment F is the collection of points equidistant from at least two non-adjacent obstacle features. The equidistant points form a network of Voronoi arcs that meet at Voronoi nodes. The Voronoi diagram of F forms a connected network of curves which retains the path homotopic structure of F. As shown in Figure 3.4(a), the Voronoi arcs reach every concave corner in F and consequently partition the free space into distinct Voronoi cells. For instance, the environment depicted in Figure 3.4(a) is partitioned into six Voronoi cells: two cells surround the internal obstacles O1 and O2, and four cells are associated with the outer walls. The Voronoi diagram of a polygonal environment with n obstacle features has O(n) Voronoi arcs and can be computed in O(n log n) time [8]. This work assumes the Voronoi diagram of F has been computed and is available for the following speed graph construction. The speed graph consists of safe time optimal arcs connecting points located on the Voronoi diagram as next described. 69
- 77. Definition 15. The speed graph of F is the weighted graph having the following structure: 1. The speed graph nodes are points on the Voronoi arcs where the obstacles’ equidis- tance function attains its minimal value along the respective Voronoi arc (possibly at its endpoint). 2. The speed graph edges are safe time optimal paths connecting every pair of nodes that lie on the boundary of a common Voronoi cell of F. 3. The cost of each edge is the value of the safe travel time functional, T(α), along this edge. There are O(n) Voronoi arcs in a polygonal environment having n obstacle features [8], and each Voronoi arc contains a single speed graph node. The speed graph thus consists of O(n) nodes and O(n2) edges. Example: The environment depicted in Figure 3.4(b) is populated by two polygonal ob- stacles O1 and O2. The figure shows the speed graph nodes, as well as the principle layout of the speed graph edges whose construction is discussed below. Note that each Voronoi arc contains a single speed graph node, and that every pair of adjacent Voronoi arcs is con- nected by a speed graph edge. Since the Voronoi diagram retains the path homotopy classes of F, the speed graph contains all the path homotopy classes of F. For instance, the path homotopy class connecting pS to pT while passing between O1 and O2 in Figure 3.4(b) is represented by the speed graph path along its edges e1, e2, e3, and e4. ◦ Construction of Speed Graph Edges: Consider two speed graph nodes, p1 and p2, located on the boundary of a Voronoi cell associated with a polygonal obstacle Oi. As illustrated in Figure 3.5(a), the edges and vertices of Oi define proximity cells, each consisting of the points in F closest to the given obstacle feature. The two speed graph nodes are separated by a finite number, say m + 1, of proximity cells. For instance, the nodes p1 and p2 in Figure 3.5(a) are separated by three proximity cells associated with a vertex and two incident edges of Oi. The boundary between adjacent proximity cells forms a line segment denoted lj for j = 1, . . . , m. The computation will focus on the piecewise time optimal paths connecting p1 and p2, consisting of time optimal path segments within each proximity cell and corner points that can freely vary along the proximity cells’ boundary lines. The corner points are parameterized 70
- 78. 10 10.5 11 11.5 12 12.5 13 13.5 14 10 10.5 11 11.5 12 12.5 13 13.5 14 1u 2u * 1u * 2u 1p * 2u * 1u (a) (b) 2p proximity cell boundary lines speed graph edge * * 1 2( , )u u iO 1u 2u initial path Figure 3.5: (a) Two speed graph nodes, p1 and p2, are separated by three proximity cells whose boundary lines are parameterized by (u1, u2). (b)T(γ(u1, u2)) is star shaped, moreover its contours are convex in the (u1, u2) plane. by p(uj) : [0, 1] → lj for j = 1, . . . , m. For instance, Figure 3.5(a) depicts two corner points p(u1) and p(u2). Since each of the time optimal path segments is uniquely determined by its two endpoints, the safe travel time functional T(α) becomes a function of u1, . . . , um. To compute the time optimal path between p1 and p2 (and hence the speed graph edge), we will use the following Monotonic property of the travel time functional T(α). The proof of the following Lemma appears in Appendix C. Lemma 3.5.1. Let γ(u1, . . . , um) parameterize the piecewise time optimal path segment con- necting the nodes p1 and p2 in F while crossing the proximity cell boundary lines l1, . . . , lm. Let u∗ = (u∗ 1, . . . , u∗ m) be the parameter values of the optimal crossing points. The functional T(γ(u1, . . . , um)) is monotonically increasing along any ray emanating from u∗ in the set (u1, . . . , um) ∈ [0, 1] × · · · × [0, 1]. The monotonicity of T(γ) leads to the following observation. In general, the level set S of a scalar valued function f(u) is star shaped when there exists a center point, u0 ∈ IRn , such that every point in S lies on a unique ray emanating from u0. In particular, convex level sets are always star shaped. Since T(γ(u)) is monotonically increasing along rays emanating from u∗, its level sets form star shaped sets as illustrated in the following example. Example: Consider the speed graph nodes p1 and p2 depicted in Figure 3.5(a). The two nodes are separated by three proximity cells. The proximity cell boundary lines are param- eterized by (u1, u2) ∈ [0, 1] × [0, 1], and the convex contours of T(γ(u1, u2)) are depicted in Figure 3.5(b). Starting with an initial path α0, convex optimization of T(γ(u1, u2)) yields 71
- 79. the optimal parameter values (u∗ 1, u∗ 2) corresponding to the speed graph edge connecting p1 and p2. ◦ Based on Lemma 3.5.1, every speed graph edge can be computed as a convex optimiza- tion of T(γ(u1, . . . , um)) on the set (u1, . . . , um) ∈ [0, 1] × · · · × [0, 1]. Convex optimization algorithms such as the ellipsoid algorithm run in O(m2 log(1/ )) steps [51], where m is the number of corner points and is the desired solution accuracy. Convex optimization requires evaluation of the gradient of T(γ(u1, . . . , um)), a computation that takes O(m) operations and increases the total run time to O(m3 log(1/ )). It is worth mentioning that our examples were prepared with the more practical Newton-Raphson’s method. While lacking a global run time bound, it is easier to implement and rapidly converges to an accurate solution; see Section 3.6.1 for implementation details. The Voronoi saturation problem: In Section 3.3, analytic solutions for time optimal paths near a wall segment obstacle and a single point obstacle were derived. In a real environment which includes multiple internal obstacles, the time optimal path under the influence of one obstacle may emerge out of its Voronoi cell and penetrate into a neighboring Voronoi cell, as illustrated in the next example. Motivational example: Figure 3.6 depicts a rectangular environment containing three point obstacles, O1, O2, O3. The points pS and pT are positioned within the same Voronoi cell. The Voronoi edges passing between the obstacles are also depicted in the figure. Three possible paths are sketched. The path γ is a time optimal path computed as if only obstacle O1 is present. Note that γ crosses the Voronoi edge at points a and b and penetrates the neighboring Voronoi cell. The path β includes three segments. The first and last segments, are the portions of γ connecting pS with a and pT with b. The middle segment of β, coincides with the portion of the Voronoi diagram between points a and b. The path α is the true time optimal path connecting pS with pT in this environment. Note that although γ was computed analytically, it is not time optimal since path β reduces γ’s total travel time. ◦ In order to find the true time optimal path, a saturation method is proposed: (i) Compute the time optimal path as if only obstacle O1 exists (Path γ). (ii) If the computed path does not exceed from the Voronoi cell of O1, it is the time optimal 72
- 80. voronoi arcs oa ob a b 1p 2p 3o 2o 1o speed graph node speed graph node Figure 3.6: The Voronoi saturation problem. path connecting the two points. If it does, compute the points a and b where the path γ crosses the Voronoi edge. (iii) The time optimal path between a and b is the portion of the Voronoi edge between these points (Prop. 3.4.5). Thus, connect points a and b by tracing the Voronoi edge. This produces a new path, β, such that T(β) T(γ). (iv) The path β does not belong to C(1). Therefore, move the points a and b to ao and bo until the complete path has a continuously varying tangent at these points. As stated below (Lemma 3.5.2), this path, denoted α, is the time optimal path connecting points pS and pT . Note that when the original path, γ, penetrates into two neighboring cells, a very similar technique can be applied. Lemma 3.5.2 (Path saturation). A path α from pS to pT which includes several path segments is the time optimal path if it satisfies the following conditions: 1. Each individual path segment is time optimal between its two endpoints. 2. At each join point of two path segments, the path has a continuously varying tangent α . Proof. Any piecewise optimal path α, connecting pS with pT , is an extremal path if it has smooth corners satisfying the Weierstrass Erdmann corner condition. According to Theorem 2 only one extremum exists in each homotopy class and hence, α must be that extremum. Based on Lemma. 3.4.4, this extremal path is also the global time minimal path. 73
- 81. 3.6 Speed Graph Based Computation of the Safe Time Optimal Path in Polygonal Environments This section describes a speed-graph based scheme for computing the safe time optimal path bet- ween two points pS and pT in F. As a preliminary step, pS and pT are connected with safe time optimal arcs to the speed graph nodes lying on the boundary of their Voronoi cells. The latter arcs can be computed with the convex optimization scheme used to compute the speed graph edges in Section 3.5. The minimum cost path from pS and pT is next computed along the augmented speed graph using any standard graph search algorithm. As the speed graph contains O(n2) edges, the minimum cost path can be computed in O(n2 log n) time, where n is number of obstacle features in the environment. The resulting minimum cost path, denoted αo, consists of safe time optimal arcs (the augmented speed graph edges), meeting non-smoothly at the speed graph nodes. The path αo indicates the most promising path homotopy class in terms of safe travel time from pS and pS in F. It will next serve as the initial path for computing the exact time optimal path in the chosen path homotopy class. The computation will focus on the piecewise time optimal paths connecting pS and pT , consisting of safe time optimal path segments in each Voronoi cell, and corner points that can freely slide along the Voronoi arcs of F. Our objective would be to slide the corner points of αo along the Voronoi arcs, until the entire path becomes a C(1) curve. In calculus of variations [29][Section 15], any piecewise optimal path having continuous tangents at its corner points is automatically an extremal path of the respective functional. Hence the resulting C(1) curve, denoted α∗, is an extremal path of T(α). Based on Theorem 2, α∗ is the safe time optimal path connecting pS and pT in the path homotopy class of αo in F. The path smoothing scheme first identifies which Voronoi arcs of F, termed the optimal Voronoi arcs, are crossed by α∗. Let p1, . . . , pm denote the Voronoi crossing points of αo. By inspecting the acute angle spanned by the tangents of αo at these points, one can determine in O(m) steps the identity of the optimal Voronoi arcs. This property is formally justified in Appendix C (see proof of Proposition 3.6.1), and is the basis for the following procedure. 74
- 82. smoothing direction smoothing direction optimal voronoi arc voronoi edge new crossing points smoothing direction (a) (b) Sp Tp ip o * * o 'o ie 1ie 2ie 'o Tp Sp ip 'i p 'i p Figure 3.7: (a) The local smoothing directions of αo and αo point toward each other, and (b) require a splitting of pi into two crossing points that will slide on the neighboring Voronoi arcs ei1 and ei2. Optimal Voronoi arcs procedure: Input: Voronoi crossing points of the initial path α0, pi for i = 1, . . . , m. Repeat for each Voronoi crossing point: 1. Set the local smoothing direction of αo at pi along the acute angle spanned by the tan- gents to the segments of αo at pi (Figure 3.7(a)). 2. Determine the Voronoi node pi towards which pi should move according to the local smoothing direction. 3. Compute the piecewise time optimal path, αo, passing through pi (instead of pi) while all other Voronoi crossing points are held fixed. 4. Set the local smoothing direction of αo along the acute angle spanned by its tan- gents at pi. 5. Compare the local smoothing directions of αo and αo: 5.1. If the smoothing directions at pi and pi point toward each other, mark the Voronoi arc containing these points as optimal (Figure 3.7(a)). 5.2. Otherwise, mark the two neighboring Voronoi arcs emanating from pi as optimal. Replace pi with two crossing points located on the neighboring Voronoi arcs at pi (Figure 3.7(b)).5 End of repeat loop. 5 When the new crossing points slide along the neighboring Voronoi arcs, the time optimal segment bounded by these points bends away from its obstacle feature, thus generating acute angles that point back to pi. Hence there will be no subsequent splitting of the two Voronoi crossing points. 75
- 83. Let e1, . . . , en for n ≥ m denote the optimal Voronoi arcs identified by the above proce- dure. For instance, two such optimal Voronoi arcs, e1 and e2, are depicted in Figure 3.8(a). The computation of α∗ will focus on the piecewise time optimal paths connecting pS with pT while crossing the Voronoi arcs e1, . . . , en at points that freely slide along these arcs. The Voronoi crossing points are parameterized by p(ui) : [0, 1] → ej for i = 1, . . . , n (Fig- ure 3.8(a)). Since each of the piecewise time optimal paths connecting pS and pT in F is uniquely determined by its Voronoi crossing points p(ui) for i = 1, . . . , n, the safe travel time functional T(α) becomes a function of u1, . . . , un. The computation of the safe time optimal path will use the following monotonicity property of T(α). Proposition 3.6.1. Let α(u1, . . . , un) parameterize the piecewise time optimal paths con- necting pS and pT in F while crossing the optimal Voronoi arcs e1, . . . , en. Let u∗ = (u∗ 1, . . . , u∗ n) be the parameter values of the optimal Voronoi crossing points. The functional T(α(u1, . . . , un)) is monotonically increasing along any ray emanating from u∗ in the set (u1, . . . , un) ∈ [0, 1] × · · · × [0, 1]. The proof of Proposition 3.6.1 appears in Appendix C. The monotonicity of T(α) leads to the following observation. In general, the level set S of a scalar valued function f(u) is star shaped when there exists a center point, u0 ∈ IRn , such that every point in S lies on a unique ray emanating from u0. In particular, convex level sets are always star shaped. Since T(γ(u)) is monotonically increasing along rays emanating from u∗, its level sets form star shaped sets as illustrated in the following example. Example: Consider the polygonal environment depicted in Figure 3.8(a). The safe time op- timal path connecting pS and pT in this environment, α∗, crosses the Voronoi arcs e1 and e2. The piecewise time optimal paths connecting pS and pT while crossing e1 and e2 are param- eterized by their Voronoi crossing points p(u1) and p(u2), where (u1, u2) ∈ [0, 1] × [0, 1]. As shown in Figure 3.8(b), the contours of T(γ(u1, u2)) in the (u1, u2) plane are star shaped (and convex), with a center point at the optimal parameter values (u∗ 1, u∗ 2) corresponding to α∗. ◦ Convex optimization algorithms require that the level sets of T(γ(u)) be convex. The same algorithms can be applied when the level sets are star shaped with center point at the optimal parameter values u∗. This observation is based on the relation T(γ(u))·(u−u∗) ≥ 0 76
- 84. 9 10 11 12 13 14 9 10 11 12 13 14 15 1u 2u * 1u * 2u Sp Tp * 2u * 1u (a) (b) iO * 1e 2e 1u 2u * * 1 2( , )u u Figure 3.8: (a) The piecewise time optimal paths connecting pS and pT while crossing the Voronoi arcs e1 and e2 are parametrized by (u1, u2). (b) The contours of T(γ(u1, u2)) are star shaped (and convex) with center at (u∗ 1, u∗ 2). satisfied by star shaped level sets, which allows construction of a barrier function (or a bound- ing ellipsoid) in each iteration of the convex optimization process. Standard convex opti- mization algorithms run in O(n3 log(R/ )) time, where R is the maximal pointwise distance between the initial path αo and the exact time optimal path α∗, and is the desired solution accuracy [51]. Note that the number of Voronoi crossing points, n, is upper bounded by the number of obstacle features in the environment. Here, too, we used Newton-Raphson’s method in the optimization of T(γ(u)) as next explained. 3.6.1 Path Optimization Using Newton’s Method Recall that the time optimal path α∗ is known to have a continuously varying tangent (Prop. 3.4.3). Thus, the computation of the optimal crossing points p1, ..., pn which lie on their respective Voronoi arcs e1, . . . , en an be also reduced into a system of ¯n equations in ¯n variables: S(p1, ..., p¯n) = Ang(α1(p1) − α2(p1)) ... Ang(α¯n(p¯n) − α¯n+1(p¯n)) = 0, (3.19) where pi ∈ ei for i = 1...¯n and Ang(αi(pi) − αi+1(pi)) is angle between the tangents of the two path segments meeting at pi. The expression for αi is derived by differentiating the time 77
- 85. optimal path given in Section 3.3 for point and wall segment obstacles. For a point obstacle, the time optimal path is parameterized by polar coordinates, α(θ) = (r(θ) cos θ, r(θ) sin θ) for θ ∈ [θS, θT ], and the derivative dr/dθ at pi is given by, dr dθ (pi) = ri ri 2ac2 − 1, (3.20) where ri is the distance measured from the point obstacle to the crossing point pi, and c is determined by the path’s end conditions. For a wall segment obstacle, the time optimal path is parameterized by cartesian coordinates, α(x) = (x, y(x)) for x ∈ [xS, xT ], and the derivative dy(x)/dx at pi is given by, dy dx (pi) = 1 2ac2piy − 1, (3.21) where pi = (pix, piy) is the crossing point and c is determined by the path’s end condi- tions. Note that (3.19) requires that (3.20) and (3.21) be expressed in a common system of coordinates. Based on the expressions for the path derivatives, the optimal crossing points, p∗ 1, ..., p∗ n, can be computed using any numerical method, such as Newton’s method. At the i’th iteration of Newton’s method, a new set of crossing points, pi = (pi 1, ..., pi ¯n), is computed. Let Ei = maxj=1...¯n{|p∗ j − pi j|} be the i’th error measure and denote by J the ¯n × ¯n Jacobian matrix of S(p1, ..., p¯n). When the initial guess, p0 1, ..., p0 ¯n, is sufficiently close to the optimal solution, quadratic asymptotic convergence is guaranteed, |Ei+1| κ|Ei|2, where κ is a positive constant defined by properties of S(p1, ..., p¯n) ([34], p.115). When J has a full rank and (3.19) is known to have a unique solution, the method is robust with respect to the exact position of the initial crossing points p0 1, ..., p0 ¯n. Each iteration of Newton’s method requires O(¯n3) steps, and in practice it suffices to run 5-10 iterations in order to generate a practically accurate path. While lacking a global run time bound, it is easier to implement and rapidly converges to the exact solution as indicated in the following example. Example: Figure 3.9 depicts a disc robot navigating in a 10 × 10 meters room populated by four point obstacles. The speed graph of the environment is depicted in Figure 3.9(a). Next, let pS and pT be located at two speed graph nodes as indicated in Figure 3.9(a). The 78
- 86. 0 20 40 60 80 100 0 10 20 30 40 50 60 70 80 90 100 0 20 40 60 80 100 0 10 20 30 40 50 60 70 80 90 100 1O 2O 3O 4O (a) Sp Tp 1O 2O 3O 4O (b) Sp Tp 1O 2O 3O 4O * (c) o 1e 2e 3e * 1u * 2u * 3u robot robot Sp Tp 1u 2u 3u Figure 3.9: (a) The speed graph of the given environment, indicated with dashed curves. (b) The initial speed graph path αo, and (c) the safe time optimal path α∗ connecting pS and pT in F. minimum cost path from pS to pT on the speed graph, αo, is indicated in Figure 3.9(b). Note that αo is non-smooth at its Voronoi crossing points. These points are next used to identify which Voronoi arcs are crossed by the exact solution α∗. These arcs are marked as e1, e2, and e3 in Figure 3.9(b). Using (u1, u2, u3) to parameterize the optimal Voronoi arcs, Newton- Raphson’s method was used to compute the exact safe time optimal path connecting pS and pT in the chosen path homotopy class. Using maximal deceleration of amin = 1 m/sec2 for the robot, the travel time along the initial speed graph path was T(αo) = 7.8 seconds, while the travel time along the safe time optimal path was reduced to T(α∗) = 7.6 seconds. As depicted in Figure 3.10, Newton-Raphson’s method converged to α∗ after only five iterations, with travel time fluctuations of less than 0.00001 seconds in subsequent iterations. The safe time optimal path connecting pS with pT in F is the C(1) curve α∗ depicted in Figure 3.9(c). More elaborate simulations of the speed graph method are next described. A remark on the k-homotopy classes: A planar environment populated by k internal obstacles may contain up to 2k homotopy classes of non self-intersecting paths6. While the speed graph usually suggests the most promising homotopy class in terms of total travel time subject to safe braking constraint, a standard k-best algorithm can compute the k best paths on the speed graph in order to suggest the k-best homotopy classes in terms of total travel time. Computing the k best paths along the speed graph can be done in O(e + vlog(v) + klog(k)) steps, where e is the total number of edges and v is the total 6 The homotopy classes can be easily counted using the Voronoi diagram: each Voronoi node has a degree of three, hence at any node the path can be split into two paths creating two new homotopy classes of paths. 79
- 87. 1 1.5 2 2.5 3 3.5 4 4.5 5 5.5 6 5.77 5.775 5.78 5.785 5.79 5.795 5.8 5.805 5.81 5.815 5.82 Iteration Traveltime 1 2 3 travel time [sec] Iteration 1 1.5 2 2.5 3 3.5 4 4.5 5 7.6 7.7 7.8 7.9 8 8.1 8.2 8.3 * 0 54 Figure 3.10: The Newton-Raphson’s travel time convergence during five iterations on the paths of Figure 3.9. number of vertices on the speed graph [23]. The total number of edges, e, is upper bounded by v(v−1)/2. Since each vertex of the speed graph is located on an individual Voronoi edge, the total number of vertices, v, equals the number of Voronoi edges in the environment, which is linear in the number of the polygonal obstacles’ features, denoted f [61]. Hence, the k-best time optimal paths on the speed graph can be computed in O(f2 +klog(k)) steps. Note that when computing the k-best paths on the speed graph, two paths on the graph from the same homotopy class may be selected. Therefore, it is necessary to eliminate paths from repeating homotopy classes. 3.7 Numerical Simulations and Experimental Setup This section describes simulations of the speed graph method in environments that resemble an industrial warehouse and an office floor. The section concludes with a brief mention of initial experiments, highlighting certain weaknesses of current mobile robot platforms. Warehouse environment: Figure 3.11 depicts a warehouse containing eight storage racks, O1, . . . , O8, surrounded by corridors. The warehouse measures 30 × 32 meters, while each storage rack measures 5×10 meters with one meter clearance between neighboring racks. The outer corridors are four meters wide, except the rightmost corridor which is seven meters wide. A cylindrical mobile robot is serving a delivery counter located at the warehouse’s lower right corner. The robot has a diameter of 80 cm, and its maximal deceleration has 80
- 88. 0 50 100 150 200 250 300 350 0 50 100 150 200 250 300 (b) 0 50 100 150 200 250 300 350 0 50 100 150 200 250 300 (a) 2o Sp Tp * * 1o 3o 4o 5o 6o 7o 8o 2o1o 3o 4o 5o 6o 7o 8o Sp Tp delivery counter delivery counter 0 Figure 3.11: (a) The speed graph of the warehouse environment. (b) The safe time optimal path α∗ selected by the speed graph, and the safe time optimal path ¯α∗ associated with the geometrically shortest path. 0 10 20 30 40 50 60 70 0 0.5 1 1.5 2 2.5 3 /secm v sec t * * 42.931.2 0 Figure 3.12: The speed profile along the paths α∗ and ¯α∗ in the warehouse environment. been set to amin = 1 m/sec2, which is typical for autonomous robots operating in warehouse environments. To synthesize the speed graph for this environment, we located the speed graph nodes on the Voronoi diagram of the environment, then computed the safe time optimal arcs connecting neighboring nodes. The speed graph of the warehouse environment is depicted in Figure 3.11(a). A start point pS was next selected at the delivery counter, while a target point pT was selected at the lower corner of the storage rack O1. The minimum cost path connecting pS to pT along the speed graph, αo, is depicted in Figure 3.11(a). Note that αo passes through the outer corridors, as these corridors allow significantly higher speeds than the narrow inner corridors. The path αo initialized the computation of the safe time optimal path within the path homotopy class suggested by the speed graph. This computation generated the path 81
- 89. (a) (b) Sp * Tp 0 50 100 150 200 250 300 350 400 450 500 0 50 100 150 200 250 300 350 400 Sp Tp* 2 = 3 4 5 6 * 1= Figure 3.13: The six best safe time optimal paths in the office floor environment. α∗ depicted in Figure 3.11(b), which has a total travel time of 31.2 seconds. To highlight the speed graph usefulness in suggesting the time optimal path homotopy class, we also computed the safe time optimal path in the homotopy class of the geometrically shortest path connecting pS to pT in the warehouse environment. The latter path homotopy class starts at pS, passes between O4 and O6, then between O3 and O4, and finally reaches pT while passing between O1 and O3. Again, we first computed the speed graph path connecting pS to pT in the selected path homotopy class, which then initialized the computation of the safe time optimal path connecting pS to pT in the chosen path homotopy class. This computation generated the path ¯α∗ depicted in Figure 3.11(b). While ¯α∗ is shorter than α∗, its total travel time of 42.9 seconds is 40% longer than T(α∗). Figure 3.12 depicts the speed profile along the two paths, indicating the higher (but still safe) speeds taken by the robot along the outer corridors of the environment. Office Floor Environment: Figure 3.13 depicts an office floor populated by obstacles resembling pieces of furniture as well as inner walls. The office floor measures 30×40 meters and contains 25 internal obstacles, with a total of 60 vertices in its polygonal representation. While the speed graph of this environment is not shown, it contains 94 nodes (one node per Voronoi arc of the environment), and 422 edges. This number of edges is much lower than the theoretical upper bound of O(n2) edges mentioned in Section 3.5. The cylindrical robot size is 20 cm, and its maximal deceleration was set to amin = 1 m/sec2. The speed graph was next used to rank several path homotopy classes connecting that start and target points, with the objective of highlighting the speed graph’s usefulness in identifying the best path 82
- 90. Path α1 α2 α3 α4 α5 α6 T(α) [sec] 27.9 33.8 43.6 47.7 54.3 57.4 Table 3.1: The safe time optimal travel time in the six best homotopy classes in the of- fice floor environment. homotopy class in terms of safe travel time. A start point pS was selected at the lower left corner of the conference room, while a target point pT was set at the lower chair in the neighboring office. The speed graph was computed for this environment, and its minimum cost path initialized a computation of the safe time optimal path α∗ depicted in Figure 3.13(a). Next, the six-best path homotopy classes in terms of total travel time were computed along the speed graph, using a standard k-best graph search algorithm [23]. For each of these path homotopy classes, the speed graph paths initialized a computation of the safe time optimal path connecting pS to pT in the respective path homotopy class. These paths are marked as α1, . . . , α6 in Figure 3.13(b), and their travel times are listed in Table 3.1. The path homotopy class suggested by the speed graph contains the time optimal path among all six homotopy classes, α1 = α∗, with total travel time of 27.9 seconds. As a comparison, the next best path homotopy class happened to be associated with the geometrically shortest path connecting pS to pT . The travel time along the second best path, α2 = ¯α∗, is 33.8 seconds. The speed graph thus captured the time optimal path homotopy class, as well as correctly ranked the k-best path homotopy classes. Initial Experiments: An experimental validation of the speed graph method is challenging for several reasons. First, the experiments require a truly fast mobile robot in order to demonstrate the practical advantage of traveling along safe time optimal paths. However, readily available platforms such as the iRobot Create can only move with maximal speeds of about 1 m/sec [1]. Second, the speed graph stored in the robot’s on-board memory requires accurate positioning, a notoriously hard problem in congested indoor environments. For instance, the iRobot sensors include a front bumper that detects obstacle collision, and an IR sensor that detects obstacles in the robot’s close vicinity [1]. However, the iRobot has no global position sensor, and its position is estimated by odometric sensors mounted on the robot’s wheels. As a preliminary demonstration, we placed an iRobot platform at a docking station in the ground floor of our office building. The robot was charged with the task of serving 83
- 91. fire extinguishers to various spots located as far as 50 meters from the docking station. The robot received as input the floor’s speed graph, and delivered the fire extinguishers to simulated fire breakups along safe time optimal paths computed along the speed graph of the environment. Measurements taken during these experiments show that the iRobot was able to reach sites located 50 meters away from its start point within 60 seconds once a fire alarm was activated. However, the iRobot platform is not designed for such high speed navigation tasks, and its poor localization system had difficulties in accurately following the time optimal paths. Nevertheless, the experiments highlight the potential usefulness of high speed navigation for applications such as first responder robots. 3.8 Conclusion In this chapter we introduced the speed graph method which synthesizes time optimal paths for a mobile robot navigating among polygonal obstacles subject to uniform braking safety constraints. The safe braking constraint was encoded as a speed dependant safety zone surrounding the robot’s current position. To find the safe time optimal path, the safe travel time functional T(α) was formulated. Basic properties of T(α) guided the construction of the speed graph for the environment. The speed graph nodes are located on the Voronoi diagram of the environment, while its edges are safe time optimal paths connecting neighboring speed graph nodes. Individual speed graph edges lie within particular Voronoi cells of the environment. The chapter established that T(α) is a convex function over the piecewise time optimal paths within any given Voronoi cell, thus leading to a computation of the speed graph edges based on standard convex optimization techniques. Once the speed graph is constructed for a given environment, the minimum cost path connecting the start and target along the speed graph suggests the most promising path ho- motopy class connecting the start and target in the environment. However, the speed graph path is only piecewise time optimal, and the task of smoothing this initial path into a C(1) curve connecting the start and target was next discussed. The initial path corner points located at the speed graph nodes were allowed to slide along their respective Voronoi arcs. The thesis then established that T(α) is a monotonic function with respect to rays emanating from the parameters definition the C(1) time optimal curve. This result lead to a formula- 84
- 92. tion of the path smoothing process as a convex optimization problem. As demonstrated in simulated environments, the speed graph suggests the path homotopy class containing the globally time optimal path, as well as provides an initial path whose positional optimization yields the safe time optimal path within the chosen path homotopy class. Note that although not directly addressed by the calculus of variations formulation, the turning radius constraint that prevents sliding events is satisfied by these paths. This could be easily validated by computing the paths’ curvatures as suggested in Appendix D, then comparing them with the minimal safe turning radius that prevents sliding (Eq. A.2). The speed graph method is only a first step toward practical safe high speed mobile robot navigation. Let us point out two extensions to this work currently under investigation. The first extension concerns environments containing static as well as dynamic obstacles. For instance, multiple mobile robots operating autonomously in industrial warehouses must be able to move with high speed while maintaining braking safety constraints with respect to all group members. A second extension concerns the generalization of the circular braking safety zone into a safety zone whose shape and size depends on the robot’s speed as well as heading. For instance, when a mobile robot travels with high speed in a long narrow corridor, a non- circular safety zone aligned with the corridor’s axis will allow the robot significantly higher safe speeds than the conservative circular safety zone. A preliminary work on mobile robots navigating under non-uniform braking safety constraints has recently appeared in [49]. 85
- 94. 4.1 Summary Navigating with high speed while ensuring safety for the mobile robot as well as to its surroundings is becoming more demanding in today’s robotics. As mobile robots are traveling with increasing speed their dynamics characteristics must be taken into account and be synthesized into the path construction. In this thesis we considered two main safety constraints involving high speed navigation: (1) A speed dependant uniform safe braking constraint; (2) A turning radius constraint that prevents sliding affects and tipping over. These constraints cover two different groups of safety constraints. The first group considers speed, environmentally dependant, safety constraints such as the safe braking constraint which depends on the robot’s clearance from the obstacles. The second group considers speed, path dependant, safety constraints such as speed limitations due to tight curvature of the path. Additionally, the paths analyzed in this thesis satisfy a quality measure - the total travel time. This quality measure requires to maximize the robot’s speed to complete the task as fast as possible. Two approaches were taken to rigorously ensure the safety of the robot throughout its navigation process. These approaches are purely analytic and suggest highly efficient methods for computing the approximated time optimal path and then the globally time optimal between obstacles subject to safety constraints. These two methods synthesize the safety constraints into the path planning process rather than first computing a geo- metrically collision free path and then parameterizing the speed along it such that velocity constraints are met. The first method discussed, denoted vc-method, models the uniform braking constraint as vc-obstacles in position-velocity configuration space. We have implemented the Morse Theory to characterize critical events where two vc-obstacles meet for the first time and locally disconnect the free position-velocity configuration space. A cellular decomposition of the free position-velocity space is described and is based on these critical events. Based on the cellular decomposition, an adjacency graph is constructed, then a special search algorithm retracts an approximated time-minimal path along this graph in O(n log n). Finally, the path’s vertices are smoothed to ensure safe turning at the prescribed path velocity. The second method presented, denoted speed graph method, encodes the braking 87
- 95. safety as a state dependant constraint, which in turn allows formulation of the safe time optimal navigation problem using calculus of variations. The work presented here, analyzes the properties of the travel time functional, T(α), then uses these properties to develop the speed graph method which computes safe time optimal paths in environments populated by polygonal obstacles. The speed graph consists of safe time optimal arcs connecting special via points located on the Voronoi diagram of the environment. The speed graph is used to efficiently select the most promising path homotopy class connecting the start and target in the environment. Convexity properties of T(α) are then used to compute the safe time optimal path within the candidate homotopy class as a convex optimization problem in O(n3 log(1/ )) time, where is the desired solution accuracy. Importantly, the two methods take place in the robot’s state space, thus treating the geometric path planning and the speed profiling in a single framework. Table 4.1 compares the two methods and indicates the advantages of each one of them. Comparison Parameter VC Method Speed Graph Method Output Produces a pseudo time opti- mal path based on a connec- tivity 3D network Computes the time optimal path based on a planar graph Complexity running time O(n log n) O(n3 log(1/ )) Safety constraints Guarantees safety in two steps Safety constraints are satis- fied by a single computational scheme Mathematical foun- dations Morse theory and topology Calculus of variations and convex optimization Main advantage Low computational running time Computes the time optimal path Table 4.1: Comparison between the VC method and the Speed Graph method. Next, we briefly discuss our current work which do not appear in this thesis but extends its ideas to more real scenarios. 4.2 Current Work Piloting along a high speed and curved path, as car racing drivers do, without the need to maintain uniform braking safety clearance (see Figure 4.1 for our high speed inspiration [12]), often results in a very high speed path which enforces an active speed dependant 88
- 96. Figure 4.1: A Formula One race car has to travel at high speed during turns without sliding. turning radius constraint to prevent sliding affects. As discussed in this thesis, the minimal turning radius that prevents side-sliding affects equals the squared speed, ν2, divided by µg, thus requiring very large turning radii and high clearance zones when navigating with high speeds. Moreover, in any practical system, the robot’s maximal acceleration is bounded by a fixed value which depends on robot’s actuators and physical parameters such as robot’s mass. Navigating along such a high speed path while trying to minimize the travel time (as a race driver) creates a tradeoff between the path’s curvature and its length. As the robot navigates with higher speeds, the required safe radius of curvature increases thus creating a total longer path for the car to follow. In this extension for our research, we analyze the time optimal navigation problem and wish to compute an explicit solution which takes into account the constraints on robot’s speed and turning radius while minimizing overall travel time. We analyze this problem using the theory of time optimal control and impose Pontryagin’s minimal principle to derive time optimal path primitives which are then composed into a global time optimal path. The suggested computational scheme will allow to extract the global time optimal path and to be implemented by off-line as well as on-line planners for high speed motion. 4.3 Future Extension The thesis raises several issues that should be investigated in future research. Let us point out three extensions to this work. The first extension concerns environments containing static as well as dynamic obstacles. For instance, multiple mobile robots operating autonomously in industrial warehouses must be able to move with high speed while maintaining braking safety constraints with respect to all group members. 89
- 97. A second extension concerns the generalization of the circular braking safety zone into a safety zone whose shape and size depends on the robot’s speed as well as heading. For instance, when a mobile robot travels with high speed in a long narrow corridor, a non- circular safety zone aligned with the corridor’s axis will allow the robot significantly higher safe speeds than the conservative circular safety zone. A preliminary work on mobile robots navigating under non-uniform braking safety constraints has recently appeared in [49]. Despite all this, note that the uniform braking safety which is considered in this thesis is useful when the robot has to navigate in close proximity to adjacent obstacles and is required to keep a sufficiently low speed to avoid collision with other moving agents or other unexpected hazards. In such scenarios requiring uniform braking safety will ensure that the robot can avoid these hazards subject to its sensing capabilities. A third extension concerns the generalization of the planar disc robot to other types of moving agents such as motor vessels piloting autonomously in dense commercial harbours. These vessels are subjected to different dynamic constraints such as hydrodynamic resistance and vessel’s heel that limits the boat’s handling through the water. In order to be safely piloted autonomously these constraints should be taken into account when planning the high speed boat’s path. An efficient on-line algorithm which regorosly ensure safety should be implemented using the convexity tools developed by this thesis. 90
- 98. Appendices 91
- 99. Appendix A The Turning Radius Constraint This appendix develops an expression for the robot’s minimal turning radius, rmin(ν), as a function of the robot’s parameters. We shall assume the common case of a four-wheeled robot. However, the expression for rmin(ν) can be easily adapted to other mobile robots. The three main constraints affecting a mobile robot turning radius are as follows. First, the turning radius must respect the robot’s maximal steering angles. Second, the centrifugal force affecting the robot must not incur radial sliding of the robot wheels. Third, the lateral moment affecting the robot must be sufficiently low as to prevent tip-over during turning maneuvers. The three constraints are next discussed. We will use the following notations. The average steering angle of the robot’s front wheels is denoted ω (Figure A.1(a)). This angle can vary in the range [−ωmax, ωmax]. The robot’s wheels are mounted at the vertices of a rectangular frame having length l and width w (Fig- ure A.1(a)). The robot’s center of mass is located at a height h above ground (Figure A.1(b)). The robot’s turning radius, denoted r, is the current radius of curvature of the curve traced by the robot’s center point. The robot’s inner turning radius, denoted rin, is the current radius of curvature of the curve followed by the robot’s innermost wheel (Figure A.1(a)). The kinematic turning radius constraint: Based on simple trigonometry, the average steering angle ω is related to l, w, and rin by the expression: tan ω = l w 2 + rin . 92
- 100. inr CMr l . . .c o m mg CFf sf sf tip left turn (b)(a) h w w Figure A.1: (a) Top view of a four-wheel mobile robot turning geometry. (b) Rear view of a left turning mobile robot. The center point turning radius is given by r = l2 4 + w 2 + rin 2 . Replacing w/2 + rin with l/ tan ω gives the turning radius: r = l2 4 + l tan ω 2 ω ∈ [−ωmax, ωmax]. Since r is monotonically decreasing in |ω|, the minimal feasible turning radius, denoted rkin, is attained at ω = ±ωmax. Thus r ≥ rkin, where rkin =l 1 4 + 1 tan2(ωmax) . (A.1) The sliding constraint: Sliding occurs when the centrifugal force acting on the robot ex- ceeds the radial friction force acting between the wheels and the ground. The net centrifugal force, denoted fr, is given by fr = mν2 r , where m is the robot’s mass, ν is the robot’s linear velocity magnitude, and r is the robot’s turning radius. The radial friction force acting between the wheels and the ground, denoted fs, is given by fs = µfN = µmg, 93
- 101. where µ is the friction coefficient between the wheels and the ground, and fN = mg is the gravitational force magnitude acting downward on the robot. Sliding is prevented as long as fr ≤ fs. Substituting for fr and fs, the minimal turning radius that prevents sliding, denoted rslide, is given by rslide(ν) = ν2 µg , (A.2) where ν is the robot’s linear velocity magnitude. The tipping-over constraint: Denote by τtip the net moment affecting the robot about a horizontal axis passing through its outer wheels contacts with the ground. When τtip is positive, the robot will tip-over and loose ground contact at its inner wheels (Figure A.1(b)). The lateral moment generated on the robot by the centrifugal force, denoted τr, is given by τr = h · fr = mh r ν2 , where h is the robot’s center of mass height above ground. The stabilizing moment generated on the robot by the gravitational force is given by τg = −mgw/2. Tipping over is prevented when τtip =τr + τg is negative semi-definite, τtip = τr + τg = mh r ν2 − mg w 2 ≤ 0. Tipping-over is prevented when r ≥ rtip, where rtip is given by rtip(ν) = 2h gw ν2 . (A.3) The robot’s minimal turning radius, denoted rmin, must satisfy all three turning radius constraints. Based on formulas A.1, A.2, and A.3, the minimal turning radius is specified as follows. Lemma A.1. The minimal turning radius of a four-wheel mobile robot moving with linear speed ν is given by rmin(ν) = max {rkin, rslide(ν), rtip(ν)} , where rkin = l 1/4 + 1/ tan2 (ωmax) is the minimal kinematically feasible turning radius, 94
- 102. rslide(ν)=ν2/µg is the minimal sliding turning radius, and rtip(ν)=(2h/gw)ν2 is the mini- mal tip-over turning radius. 95
- 103. Appendix B The Time Optimal Path for a Disc Robot This appendix considers a disc robot of radius ρ traveling near a point obstacle located at the origin of the (x, y) plane. Using polar coordinates, the path traced by the robot’s center is parameterized by α(θ) = (r(θ) cos θ, r(θ) sin θ) for θ ∈ [θS, θT ], with the endpoint conditions r(θS) = rS and r(θT ) = rT . The distance between the disc robot and the point obstacle, denoted dr(θ), is given by dr(θ) = r(θ) − ρ along α. Note that dr(θ) must be non-negative for all θ ∈ [θS, θT ] in order to prevent collision. Braking safety requires that dr(θ) ≥ d(v(θ)). Substituting d(ν) = ν2/2amin and using the maximal allowed speed for travel time minimization gives the constraint: ν(r) = 2amin(r − ρ) r ≥ ρ. (B.1) The travel time functional T(α) is thus given by T(α) = θT θS F r(θ), r (θ) dθ, where F(r, r ) = r2 + r 2 2amin(r − ρ) . (B.2) 96
- 104. Substituting for F(r , r) in the Euler-Lagrange equation, then integrating once with re- spect to θ, gives the implicit scalar differential equation: r4 (θ) = c2 r(θ) − ρ r2 (θ) + (r (θ))2 , (B.3) where c is yet to be determined. The analytic solution of (B.3) is the sum of two elliptic in- tegrals. Rather then use elliptic integrals, a practical approximation for the analytic solution of (B.3) is next described. -20 -15 -10 -5 0 5 10 15 20 -25 -20 -15 -10 -5 0 5 10 15 20 25 polygonal approximation Sp Tp * * 1u 2u 3u 4u 1( )p u 2( )p u 3( )p u 4( )p u disc obstacle Figure B.1: The safe time optimal path determined by the polygonal approximation, ¯α∗, and the exact path α∗ associated with the disc obstacle. To compute an approximate solution path, expand the point obstacle into a disc obstacle of radius ρ, while simultaneously shrinking the robot to a point. Next replace the portion of the disc obstacle between θS and θT by polygonal segments tangent to the disc obstacle as depicted in Figure B.1. The computation of the safe time optimal path in the vicinity of the polygonal obstacle is a convex optimization problem according to Lemma 3.5.1. The convex optimization scheme focuses on the piecewise time optimal paths connecting pS to pT , consisting of time optimal path segments within each proximity cell determined by the obstacle features (i.e. the points closest to a given obstacle vertex or edge), and corner points that freely vary along proximity cells boundary lines. For instance, in Figure B.1 there are five proximity cells with corner points p(u1), . . . , p(u4) that vary along four proximity cells boundary lines. Each of the time optimal path segments is uniquely determined by its two endpoints, and the safe travel time functional thus T(α) becomes a convex function of the corner point 97
- 105. parameters. For instance, convex optimization functional T(α(u1, . . . , u4)) gives the safe time optimal path ¯α∗ in Figure B.1. Note that ¯α∗ closely approximates the exact time optimal path α∗, which was computed with trial and error of the precise initial conditions at pS that lead to pT . Moreover, the quality of approximation improves as the number of segments used in the polygonal approximation increases. 98
- 106. Appendix C Computation of the Speed Graph Edges and the Safe Time Optimal Path as Convex Optimization Problems This appendix describes a convex optimization scheme. The scheme is first used to compute the speed graph edges considered in Section 3.5. Nest, the optimization scheme is used to compute the safe time optimal path as discussed in Section 3.6. proximity cell boundary line Voronoi edge 1( )p u 2( )p u speed graph edge 1p 2p iO 1l proximity cell boundary line 2l Figure C.1: The safe time optimal path connecting the speed graph nodes p1 and p2 crosses two proximity cell boundary lines, l1 and l2, at the corner points p(u1) and p(u2). Computation of the speed graph edges: The following discussion appears in Section 3.5 and is summarized here for ease of reference. Consider two speed graph nodes, p1 and p2, 99
- 107. located on the boundary of a common Voronoi cell of a polygonal obstacle Oi (Figure C.1). Each speed graph edge is the safe time optimal path connecting p1 and p2 in F. Suppose m +1 proximity cells separate between p1 and p2 (three such cells separate p1 and p2 in Figure C.1). The boundaries between adjacent proximity cells form line segments denoted lj for j = 1, . . . , m. The computation will focus on the piecewise time optimal paths connecting p1 and p2, consisting of time optimal path segments within each proximity cell and corner points that can freely vary along the proximity cells’ boundary lines. The corner points are parameterized by p(uj) : [0, 1] → lj for j = 1, . . . , m. As discussed in Section 3.5, the safe travel time functional T(α) becomes a function of u1, . . . , um. To compute the time optimal path between p1 and p2 (and hence the speed graph edge), we will use the following monotonic property of the travel time functional T(α) . Lemma 3.5.1. Let γ(u1, . . . , um) parameterize the piecewise time optimal path segment con- necting the nodes p1 and p2 in F while crossing the proximity cell boundary lines l1, . . . , lm. Let u∗ = (u∗ 1, . . . , u∗ m) be the parameter values of the optimal crossing points. The functional T(γ(u1, . . . , um)) is monotonically increasing along any ray emanating from u∗ in the set (u1, . . . , um) ∈ [0, 1] × · · · × [0, 1]. The proof of Lemma 3.5.1 is similar to the proof of the Prop. 3.6.1 with minor changes in notations. The path γ is replaced with α and the crossing points p(ui) for i = 1...m or n move from the proximity cell boundary lines denoted lj onto the Voronoi arcs denoted ej. Computation of the safe time optimal path: The speed graph method identifies a can- didate path homotopy class connecting pS and pT in F, then determines which Voronoi arcs must be crossed by the safe time optimal path in the chosen path homotopy class. Denote by e1, . . . , en the latter Voronoi arcs (two such arcs, e1 and e2, appear in Figure C.2(a)). The computation of the safe time optimal path will focus on the piecewise time optimal paths that cross the Voronoi arcs e1, . . . , en at points that can freely vary along these arcs. The Voronoi crossing points are parameterized by p(ui) : [0, 1] → ej for i = 1, . . . , n (Figure C.2(a)). As discussed in Section 3.6, the safe travel time functional T(α) becomes a function of the parameters u1, . . . , un. The computation of the safe time optimal path will use the following monotonicity property of T(α). Proposition 3.6.1. Let α(u1, . . . , un) parameterize the piecewise time optimal paths con- 100
- 108. 1( )p u * 1( )p u 2( )p u Sp Tp Voronoi diagram * t Sp Tp 1e 2e * 2( )p u 1e 2e tangent to 1e tangent to 2e 1( ) sv (a) (b) ( )t s iO iO bisector bisector 1- ( ) sv 2( ) sv 2- ( ) sv Figure C.2: (a) The safe time optimal path α∗ and a variant path αt. (b) The sector spanned by −v(s− i ) and v(s+ i ) locally contains the Voronoi arc tangent line at p(ui) for i = 1, 2. necting pS and pT in F while crossing the Voronoi arcs e1, . . . , en. Let u∗ = (u∗ 1, . . . , u∗ n) be the parameter values of the optimal Voronoi crossing points. The functional T(α(u1, . . . , un)) is monotonically increasing along any ray emanating from u∗ in the set (u1, . . . , un) ∈ [0, 1] × · · · × [0, 1]. Proof: The piecewise time optimal paths connecting pS and pT in F will be parameterized by a fixed division of the unit interval into sub-intervals [0, s1], [s1, s2], . . . , [sn, 1], such that the Voronoi crossing points of these paths will correspond to s = s1, . . . , sn. Let α∗ denote the safe time optimal path connecting pS and pT in F while crossing the Voronoi arcs e1, . . . , en. A Voronoi path variation of α∗ is a differentiable mapping H(s, t) : [0, 1] × [0, 1] → F, such that H(s, 0) = α∗(s) for s ∈ [0, 1], with fixed endpoints H(0, t) = pS and H(1, t) = pT for t ∈ [0, 1]. Each path in this variation, denoted αt, corresponds to a particular value of t (Figure C.2(a)). As t increases from t = 0 in the unit interval, the Voronoi crossing points of αt vary along the Voronoi arcs e1, . . . , en. When H(s, t) represents a ray emanating from u∗, the Voronoi crossing points of αt move away from those of α∗ along the Voronoi arcs as t increases in the unit interval. When the travel time functional T(α) is evaluated along the paths of H(s, t) it becomes a function of t, T(t) = 1 0 F (αt(s), αt(s)) ds. All paths of H(s, t) cross the Voronoi arcs at s = s1, ..., sn, and T(t) can be equivalently written as the sum: T(t) = n i=0 si+1 si F αt(s), αt(s) ds, 101
- 109. where s0 = 0 corresponds to pS and sn+1 = 1 corresponds to pT . Using Leibnitz rule, the derivative of T(t) with respect to t is given by d dt T(t) = n i=0 si+1 si ∂F(α,α ) ∂α − d ds ∂F(α,α ) ∂α · ∂H(s,t) ∂t ds+ n i=1 ∂F(α,α ) ∂α s− i − ∂F(α,α ) ∂α s+ i · ∂H(si,t) ∂t (C.1) where we used the fact that H(s, t) is a fixed endpoints variation. Each of the variant paths, αt(s), consists of time optimal path segments satisfying the Euler-Lagrange equation (3.5). Hence all the integrals in (C.1) vanish, and the derivative of T(t) with respect to t becomes: d dt T(t) = n i=1 ∂F(α, α ) ∂α s− i − ∂F(α, α ) ∂α s+ i · ∂H(si, t) ∂t . (C.2) According to (3.15), F(α, α ) = α (s) /ν(s), where ν(s) = (2amin·mini=0...k{dst(α(s), Oi)})1/2 is the robot’s maximal speed along α(s). The expression for ∂F(α, α )/∂α is thus given by ∂ ∂α F(α, α ) = 1 ν(s) 1 α (s) α (s) = 1 ν(s) v(s), where v = α / α is the normalized tangent of α. Substituting for ∂F(α, α )/∂α in (C.2) and using the subscript t to denote terms associated with αt gives: d dt T(t) = n i=1 1 νt(si) vt(s− i ) − vt(s+ i ) · ∂ ∂t H(si, t). (C.3) The vector vt(s− i )−vt(s+ i ) is the bisector of the sector spanned by −vt(s− i ) and vt(s+ i ) at αt(si) (Figure C.2(b)). The vector ∂H(si, t)/∂t is tangent to the Voronoi arc ei at αt(si), since the Voronoi crossing point αt(si) varies along ei as a function of t. The vectors −vt(s− i ) and vt(s+ i ) are tangent to the segments of αt that reach the crossing point αt(si) from opposite sides of ei (Figure C.2(b)). Hence the tangent line to ei lies within the sector spanned by −vt(s− i ) and vt(s+ i ) at αt(si). It follows that the i’th summand in (C.3) is the product of two vectors lying in a common sector. Since one of the two vectors is the bisector, the i’th summand in (C.3) can vanish only when vt(s− i ) − vt(s+ i ) = 0. In this case the time optimal path segments meeting at αt(si) form a single C(1) curve connecting the neighboring Voronoi 102
- 110. crossing points αt(si−1) and αt(si+1). The Voronoi crossing points are located at (u∗ 1, . . . , u∗ n) at t = 0, then move away from their optimal locations along e1, . . . , en as t increases in the unit interval. Hence T(t) is increasing at t = 0. Suppose T(t) ceases to increase at t = t1. In this case there must exists 0 t0 t1 at which one of the summands in (C.3) becomes zero and then negative for t t0. This particular summand of index i0 satisfies the condition vt0 (s− i0 ) = vt0 (s+ i0 ) at the Voronoi crossing point αt0 (si0 ). The time optimal path segments meeting at αt0 (si0 ) form a single extremal path of T(α) connecting the neighboring Voronoi crossing points. By assumption T(α) attains a local maximum along a local variation of this path which varies only the ei0 crossing point while keeping the neighboring Voronoi crossing points fixed. However, any extremal path of T(α) must be a local minimum according to Lemma 3.4.4. This leads to a contradiction, and T(t) therefore remain monotonically increasing for all 0 ≤ t ≤ 1. 103
- 111. Appendix D The Turning Radius Constraint Along an Optimal Path As discussed in Appendix A, when the robot navigates along a high speed path, it may be subjected to high centrifugal forces that may bring the robot to lose its grip and slide sideways. Therefore it is necessary to ensure that the radius of curvature of the time optimal path segments that where derived in Section 3.3 do not violate the turning radius constraint given by: r(ν) ≥ rslide(ν) = ν2 µg . (D.1) Note that the development of (D.1) appeared in Appendix A and hence is omitted here. A single wall segment obstacle: Consider a time optimal path segment located near a single wall segment. This optimal path segment was developed in Section 3.3 and is given by the Cycloid curve (3.9). An expression for the radius of curvature, written in cartesian coordinates is given by: ρc = 1 + (dy/dx)2 3/2 d2y/dx2 . (D.2) where y(x) is the y coordinate of the optimal path segment α(x) = (x, y(x)). 104
- 112. Further substitution of dy/dx and d2y/dx2 in (D.2) according to the optimal path’s formula, then comparing with the safe turning radius, would suggest that for every y(x) ≤ 1/(2aminc2) the turning radius constraint is kept and the robot can safely follow this optimal path segment without the risk of sliding. The constant amin = µg is the robot’s maximal deceleration and c is a parameter determined by the path’s end conditions. Next, observe the expression for dy/dx for this path segment: dy(x) dx = 1 2aminc2y(x) − 1. (D.3) Equation D.3 implies that this optimal path segment is only valid when y(x) ≤ 1/(2aminc2) and hence we can conclude that ρc ≥ rslide. Next, consider the optimal path segment located near a single point obstacle. A single point obstacle: Consider a time optimal path segment located near a single point obstacle. This optimal path segment was developed in Section 3.3 and is given by a parabolic curve formulated in polar coordinates (3.12). However, to show that this path segment maintains the safe turning radius constraint we shale formulate it in cartesian coordinates as follows: y(x) = x2 + y2 0 2y0 for x ∈ [xS, xT ], where y0 is the minimal distance between the obstacle and the imaginary wall segment that creates the parabolic optimal curve. Note that the x axis merges with that imaginary wall. The safe speed along this optimal path segment is given by, ν(x) = µg(x2 + y2 0) y0 , and therefore the turning radius that will evoke sliding while navigating with this speed is: rslide(ν(x)) = ν2(x) µg = x2 + y2 0 y0 . Requiring that the path’s curvature ρc, derived according to (D.2), would be equal or 105
- 113. larger than rslide for every x gives: ρc = y0 · 1 + x2 y2 0 3/2 y0 · 1 + x2 y2 0 = rslide, which is always true and hence this time optimal path segment satisfies the turning radius constraint thus preventing sliding events. 106
- 114. Bibliography [1] The iRobot Create owners guide. www.irobot.com. [2] GUARDIUM-UGV. G-NIUS Unmanned Ground Systems, accesssed Jan. 2012. http://g-nius.co.il. [3] GUSS, a ground unmanned support surrogate. The Naval Surface Warfare Center and TORC Robotics, accesssed Jan. 2012. http://www.torcrobotics.com/ground-unmanned- support-surrogate. [4] Traffic jam assistance. Volvo Car Group, accesssed May. 2013. https://www.media.volvocars.com/global/enhanced/en- gb/media/preview.aspx?mediaid=46386. [5] A.P. Aguiar and J.P. Hespanha. Trajectory-tracking and path-following of underactu- ated autonomous vehicles with parametric modeling uncertainty. IEEE Transactions on Automatic Control, 52:1362–1379, 2007. [6] K.J. Astr¨om and R.M. Murray. Feedback systems: an introduction for scientists and engineers. Princeton university press, 2010. [7] F. Aurenhammer and R. Klein. Voronoi diagrams. In Handbook of Computational Geometry, chapter 5, pages 201–290. Elsevier Science, 2000. [8] F. Aurenhammer, R. Klein, and D.T. Lee. Voronoi Diagrams and Delaunay Triangula- tions. World Scientific, 2013. [9] J.E. Bobrow, S. Dubowsky, and J. Gibson. Time-optimal control of robotic manipulators along specified paths. The International Journal of Robotics Research, 4:3–17, 1985. 107
- 115. [10] J. Borenstein and Y. Koren. The vector field histogram - fast obstacle avoidance for mobile robots. IEEE Transactions on Robotics and Automation, 7:278–288, 1991. [11] C.B. Boyer and U.C. Merzbach. A History of Mathematics. New York: Wiley, 1991. [12] F. Braghin, F. Cheli, S. Melzi, and E. Sabbioni. Race driver model. Computers Structures, 86:1503–1516, 2008. [13] O. Brock and O. Khatib. High-speed navigation using the global dynamic window approach. In IEEE International Conference on Robotics and Automation, 341–346, 1999. [14] A.E. Bryson. Applied optimal control: optimization, estimation and control. CRC Press, 1975. [15] F. Bullo and R.M. Murray. Tracking for fully actuated mechanical systems: A geometric framework. Automatica, 35:17–34, 1999. [16] H. Chitsaz and S.M. LaValle. Time-optimal paths for a dubins airplane. In 46th IEEE Conference on Decision and Control, 2379–2384, 2007. [17] H. Choset. Coverage for robotics a survey of recent results. Annals of Mathematics and Artificial Intelligence, 31:113–126, 2001. [18] H. Choset, K.M. Lynch, S. Hutchinson, G. Kantor, W. Burgard, L.E. Kavraki, and S. Thrun. Principles of Robot Motion. MIT Press, Cambridge MA, 2005. [19] T.H. Chung, G.A. Hollinger, and V. Isler. Search and pursuit-evasion in mobile robotics, a survey. Autonomous Robots, 31:299–316, 2011. [20] F.H. Clarke. Optimization and Nonsmooth Analysis. SIAM Publication, 1990. [21] B. Donald, P. Xavier, J. Canny, and J. Reif. Kinodynamic motion planning. Journal of The ACM, 40:1048–1066, 1993. [22] L.E. Dubins. On curves of minimal length with a constraint on average curvature, and with prescribed initial and terminal positions and tangents. American Journal of Mathematics, 497–516, 1957. 108
- 116. [23] D. Eppstein. Finding the k shortest paths. SIAM Journal on Computing, 28:652–673, 1998. [24] P. Fiorini and Z. Shiller. Motion planning in dynamic environments using velocity obstacles. The International Journal of Robotics Research, 17:760–772, 1998. [25] S. Fleury, P. Soueres, J.P. Laumond, and R. Chatila. Primitives for smoothing mobile robot trajectories. IEEE Transactions on Robotics and Automation, 11:441–448, 1995. [26] D. Fox, W. Burgard, and S. Thrun. The dynamic window approach to collision avoid- ance. IEEE Robotics Automation Magazine, 4:23–33, 1997. [27] T. Fraichard. A short paper about motion safety. In IEEE International Conference on Robotics and Automation, 1140–1145, 2007. [28] S.S. Ge and Y.J. Cui. Dynamic motion planning for mobile robots using potential field method. Autonomous Robots, 13:207–222, 2002. [29] I. M. Gelfand and S.V. Formin. Calculus of Variations. Prentice-Hall, 1963. [30] M. Goresky and R. MacPherson. Stratified Morse Theory. Springer Verlag, New York, 1980,. [31] B. H´eriss´e and R. Pepy. Shortest paths for the dubins’ vehicle in heterogeneous en- vironments. In 52nd IEEE Annual Conference on Decision and Control, 4504–4509, 2013. [32] D. Hsu, R. Kindel, J.C. Latombe, and S. Rock. Randomized kinodynamic motion plan- ning with moving obstacles. The International Journal of Robotics Research, 21:233–255, 2002. [33] K. Iagnemma, S. Shimoda, and Z. Shiller. Near-optimal navigation of high speed mobile robots on uneven terrain. In IEEE/RSJ Int. Conference on Intelligent Robots and Systems (IROS), 4098–4103, 2008. [34] E. Isaacson and H.B. Bishop. Analysis of numerical methods. Dover, 1994. [35] K. Kant and S.W. Zucker. Toward efficient trajectory planning: The path-velocity decomposition. The International Journal of Robotics Research, 5:72–89, 1986. 109
- 117. [36] S. Karaman and E. Frazzoli. Sampling-based algorithms for optimal motion planning. International Journal of Robotics Research, 30:846–894, 2011. [37] R. Kindel, D. Hsu, J.C. Latombe, and S. Rock. Kinodynamic motion planning amidst moving obstacles. In IEEE International Conference on Robotics and Automation, 537– 543, 2000. [38] Y. Koren and J. Borenstein. Potential field methods and their inherent limitations for mobile robot navigation. In IEEE International Conference on Robotics and Automa- tion, 2:1398–1404, 1991. [39] F. Large, D. Vasquez, T. Fraichard, and C. Laugier. Avoiding cars and pedestrians using v-obstacles and motion prediction. In The IEEE Intelligent Vehicle Symp., 2004. [40] J.C. Latombe. Robot Motion Planning. Kluwer Academic Publishers, 1991. [41] S.M. Lavalle. Rapidly-exploring random trees: A new tool for path planning. Technical report, Dept. of Computer Science, Iowa State University, 1998. [42] S.M. LaValle and J.J. Kuffner. Randomized kinodynamic planning. In IEEE Interna- tional Conference on Robotics and Automation, 1:473–479, 1999. [43] S.M. LaValle and J.J. Kuffner. Randomized kinodynamic planning. The International Journal of Robotics Research, 20:378–400, 2001. [44] S.M. LaValle and J.J. Kuffner. Rapidly-exploring random trees: progress and prospects. In Algorithmic and Computational Robotics: New Directions, 293–308. Wellesley, MA, 2001. [45] M. Lepetic, G. Klancar, I. Skrjanc, D. Matko, and B. Potocnik. Time optimal path planning considering acceleration limits. Robotics and Autonomous Systems, 45:199– 210, 2003. [46] T. Lozano-Perez. Spatial planning: A configuration space approach. IEEE Transactions on Computers, C-32:108–120, 1983. [47] G. Manor and E. Rimon. High-speed navigation of a uniformly braking mobile robot us- ing position-velocity configuration space. In IEEE International Conference on Robotics and Automation, 193–199, 2012. 110
- 118. [48] G. Manor and E. Rimon. Vc-method: high-speed navigation of a uniformly braking mobile robot using position-velocity configuration space. Autonomous Robots, 34:295– 309, 2013. [49] G. Manor and E. Rimon. The speed graph method: Time optimal navigation among ob- stacles subject to safe braking constraint. In IEEE International Conference on Robotics and Automation, 1155–1160, 2014. [50] J. Markoff. Google cars drive themselves, in traffic. The New York Times, October 9th 2010. [51] Y. E. Nesterov and A. S. Nemirovsky. Interior Point Polynomial Methods in Convex Programming: Theory and Applications. Springer Verlag, New York, 1992. [52] V. Nieuwstadt, J. Michael, and R.M. Murray. Real time trajectory generation for dif- ferentially flat systems. California Institute of Technology, 1997. [53] L. Nirenberg. Variational and topological methods in nonlinear problems. Bulletin of the AMS, 4:267–302, 1981. [54] C. O’D´unlaing and C. K. Yap. A retraction method for planning the motion of a disc. Journal of Algorithms, 6:104–111, 1985. [55] M.G. Park, J.H. Jeon, and M.C. Lee. Obstacle avoidance for mobile robots using arti- ficial potential field approach with simulated annealing. In IEEE International Sympo- sium on Industrial Electronics, 3:1530–1535, 2001. [56] R. Parthasarathi and T. Fraichard. An inevitable collision state-checker for a car-like vehicle. In IEEE Intenrational Conference on Robotics and Automation, 3068–3073, 2007. [57] S. Petti, T. Fraichard, and I. Rocquencourt. Safe motion planning in dynamic envi- ronments. In IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 3726–3731, 2005. [58] J. Reeds and L. Shepp. Optimal paths for a car that goes both forwards and backwards. Pacific Journal of Mathematics, 145:367–393, 1990. 111
- 119. [59] E. Rimon. Exact Robot Navigation Using Artificial Potential Functions. PhD thesis, Yale University, New Haven CT, Dec 1990. [60] E. Rimon and D.E. Koditschek. Exact robot navigation using artificial potential func- tions. IEEE Transactions on Robotics and Automation, 8:501–518, 1992. [61] J.R. Sack and J. Urrutia, editors. Handbook of Computational Geometry. Elsevier, 1999. Chapter 5. [62] M. I. Shamos. Computational Geometry. PhD thesis, Yale University, 1978. Section 6.3. [63] I. Sherr and M. Ramsey. Toyota and audi move closer to driverless cars, The Wall Street Journal, Jan. 3 2013. [64] Z. Shiller and Y. R. Gwo. Dynamic motion planning of autonomous vehicles. IEEE Transactions on Robotics and Automation, 7:241–249, 1991. [65] Z. Shiller and H.H. Lu. Computation of path constrained time optimal motions with dynamic singularities. Journal of dynamic systems, measurement, and control, 114:34– 40, 1992. [66] Z. Shiller, S. Sharma, I. Stern, and A. Stern. Online obstacle avoidance at high speeds. The International Journal of Robotics Research, 32:1030–1047, 2013. [67] S. Shimoda, Y. Kuroda, and K. Iagnemma. Potential field navigation of high speed unmanned ground vehicles on uneven terrain. In IEEE International Conference on Robotics and Automation, 2828–2833, 2005. [68] R. H. Smith. Analyzing Friction in the Design of Rubber Products and Their Paired Surfaces. CRC Press, 2008. [69] J. Snape, J. van den Berg, and S. J. Guy. The hybrid reciprocal velocity obstacle. IEEE Transactions on Robotics and Automation, 27:696–706, 2011. [70] K.C. Teixeira, M. Becker, and G.A.P. Caurin. Automatic routing of forklift robots in warehouse applications. In ABCM Symposium in Mechatronics, 4:335–344, 2010. 112
- 120. [71] I. Ulrich and J. Borenstein. Vfh+: Reliable obstacle avoidance for fast mobile robots. In International Conference on Robotics and Automation, 2:1572–1577, 1998. [72] E. Velenis and P. Tsiotras. Optimal velocity profile generation for given acceleration limits: theoretical analysis. In The American Control Conference, 2:1478–1483, 2005. [73] J. Villagra, V. Milanes, J. Perez, and J. Godoy. Smooth path and speed planning for an automated public transport vehicle. Robotics and Autonomous Systems, 2012. [74] W. Weigo, C. Huitang, and W. Peng-Yung. Time optimal path planning for a wheeled mobile robot. Journal of Robotic Systems, 17:585–591, 2000. [75] R. Wein, J. van den Berg, and D. Halperin. Planning high-quality paths and corridors amidst obstacles. International Journal of Robotics Research, 27:1213–1231, 2008. [76] E. Welzl. Constructing the visibility graph for n-line segments in o(n2) time. Information Processing Letters, 20:167–171, 1985. [77] D.K. Wilde. Computing clothoid segments for trajectory generation. In IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2440–2445, 2009. [78] D. Wilkie, J.V. den Berg, and D. Manocha. Generalized velocity obstacles. In IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2009. [79] P.R. Wurman, R. D’Andrea, and M. Mountz. Coordinating hundreds of cooperative, autonomous vehicles in warehouses. AI Magazine, 29:9–19, 2008. [80] J.M. Yang and J.H. Kim. Sliding mode control for trajectory tracking of nonholonomic wheeled mobile robots. IEEE Transactions on Robotics and Automation, 1999. [81] J. Youssef. The Mountain Pass Theorem, Variants, Generalizations and Some Applica- tions. Cambridge University Press, 2003. [82] M. Athans and P.L. Falb. Optimal Control. An Introduction to the Theory and Its Applications. Dover Publications, 2007. [83] G. Leitmann. The calculus of variations and optimal control. Springer, 1981. [84] J.Z. Ben-Asher. Optimal control theory with aerospace applications. AIAA Educational Series, 2010. 113
- 121. מאפשרים ולכןקבלת אףאופטימלי מסלול.נומרי איננו המסלולים בניית תהליך מכך יתרה נע הוא בה ולסביבה לעצמו הרובוט בטיחות את ריגורוזי באופן ומבטיח. גי שתימוצגות שות.הראשונה בגישה,אילוהבטיחו ציממודלים המהירות תלויי ת ומיקום מהירות תלוי קונפיגורציה במרחבשלהרובוט.מכן לאחר,מחולק החופשי המרחב אלו תאים בין קישוריות וגרף חופשיים לתאיםנבנה.מסלולים מניב זה גרף על חיפוש מיותר יעיל חישובי באופן מקורבים אופטימליים.מראש מקיימים אלו שמסלולים לציין ישאת הבטיחות אילוצי.בהשניה גישה,משתמשיםראשיתואריצ בחשבוןיחישוב לצורך ות בדידים למכשולים בסמוך האופטימליים המסלולים.על המבוסס חיפוש גרף נבנה בהמשך אלו מסלולים.הגרף על איכותי מסלול מציאת לאחר,הבטיחות אילוצי את המקיים כזה, של קמירות תכונות מנצלת השיטההגלובלי והמסלול אלו במסלולים התנועה זמן פונקציונאל מחושב האופטימלי.נעזר זה חישובבאלגוריתםקמורות פונקציות עבור האליפסואידשהינו זה מסוג בחישובים ביותר יעיל.המתק המסלולהחופשי מסלול הינו בלמהתנגשויות,מקיים הבטיחות אילוצי אתמראשהתנועה זמן את וממזער.עבהגישות שתי ור,מלווים ההסברים המ כשגרות ומתוארים רבות בדוגמאותומיידי למימוש כנות. III
- 122. מידי גבוה מסה מרכז מיקום עקב הרובוט של התהפכותו את שתמנע כזו מהירות.אילוצי הרובוט במהירות תלויים אלו בטיחות,ובמקד הרובוט בגיאומטרייתצמיגי בין החיכוך ם מיקום הכולל הרובוט של המלא המצב למרחב התייחסות דורשים ולכן לקרקע הרובוט ומהירות. הניווט בעייה לפתרון שיטותגבוהה במהירותתוךהדינמיים לאילוצים התיחסות דרישות כורח המוכתביםהבטיחות,עיקריות קבוצות לשלוש נחלקות.הראשונה בקבוצה המסלול בנייתנחלקתלשנישלבים.בשלבלרובוט גיאומטרי תנועה מסלול מוגדר הראשון מסויימת מידה אמת ומקיים מהתנגשויות חופשי אשרהכולל המסלול אורך כגון.בשלבהשני התנועה תכנון של,מוכתבתהמס לאורך הרובוט מהירותהנתון לול,לא הבטיחות שאילוצי כך יופרו.זו תכנון שיטת,מהיר מסלול קבלת אומנם מאפשרתנמוכה חישובית בעלות,וכן, כ מסלולהדינאמיים הבטיחות אילוצי את מפר שלא זה,מסלולים קבלת מאפשרת איננה אך ו היות אופטימלייםאלודורשיםהמהירות וקביעת הגיאומטרי התכנון של סינתזהיחדיו. ידועות מכשולים סביבות עבור מסלול לתכנון נומריות שיטות כוללת השנייה הקבוצה. מסלול חיפוש שמאפשר כזה באופן דסקרזיציה עובר המסלולים חיפוש מרחב אלו בשיטות וכזה מהתנגשויות החופשיההבטיחות אילוצי את מקיים.למסלול לרוב מתכנסות אלו שיטות ה זמן כאשר האופטימלימסלולים של מתמטיות תכונות מנצלות אינן אך מוגבל איננו חישוב החישוב זמן והפתחת הדיוק שיפור לטובת אלו.בעיקר ויעילה שנפוצה השלישית הקבוצה אמת בזמן מסלולים בחישוב,עבור המהירות ופרופיל התנועה מסלול את מחשבתזמן צעד מוגדר,מק בסביבה רק המסלול לתכנון מתייחסים בוהרובוט של ומית.זמן צעד בכל דסקרטי,מחושביםהמהירות ופרופיל המסלוללאורכומסויימת מידה שאמת כזה באופן הביטחות אילוצי על שמירה כדי תוך ממוקסמת.לכל התייחסות מאפשרות אינן אלו שיטות יעיל באופן בעת בו המכשולים. זה במחקרהמסלול תכנון ואת הדינאמיים הבטיחות אילוצי את מסתנזים אנו ומהירות מסלול את המשפיעים המרכיבים כל את בחשבון הלוקח אחד לשלד הגיאומטרי הרובוט.תנועה מסלולי לקבל מאפשר זה שילוב,מרא הבטיחות אילוצי את המקיימים כאלוש II
- 123. תקציר ניווטרובוטיםאוטונומייםהרובוטיקה בתחום מאוד חשוב היבט הינו.עדים אנו לאחרונה לנוע ועוברות הסטריליות מהמעבדות יוצאות אוטונומיות רובוטיות מערכות בה למגמה ולפעולו חיצוניות בסביבותממשיות.זו בנקודה,המערכת בטיחות את להבטיח חיוני מערכת אותה בתוכה הסביבה בטיחות עם יחד הרובוטיתרובוטיתנעה.השיטות אחת זו בעייה לפתרון המקובלות,לרובוט קונפיגורציות מרחב למדל הינה.כל את כולל זה מרחב לפגיעה גורמת איננה הרובוט של המצאותו בהן והאורינטציות הצירים ראשית מיקומי הסמוכים במכשולים.חיפוש עץ כגון נתונים מבנה של בנייה מתבצעת זה מרחב בניית לאחר אומימדי תלת גרף.הרובוט בין מהתנגשויות החופשי מסלול עבור חיפש מתבצע לבסוף האחרים והמכשולים.וגם מלכתחילה ידועה המכשולים סביבת בו במצב גם ישימה זו שיטה חיישנים ידי על תנועה כדי תוך נרכש המכשולים על המידע בו אמת בזמן בניווט.כל למרות זאת,פתרון הוצע לא כה עדהרובוט דינאמיקת כאשר מכשולים בין הניווט לבעיית מובהק בחשבון נלקחת.מכך יתרה,או התנועה זמן מזעור כדוגמאת איכותי מסלול לקבלת דרישה מהמכשולים המרווח מיקסום,הדינאמיים האילוצים קיום עם יחד,חדשה ניווט בעיית מגדירה ומורכבת. מהתנגשויות החופשיים למסלולים אחת מידה אמת,התייחסות תוך כמובן הרובוט לדינאמיקת,הרובוט של הכולל התנועה זמן היא,אחרות במילים או,המסלול נתונות קצה נקודות שתי בין זמן מבחינת האופטימלי.מידה אמת לכזו,תנועה מהירות הרובוט של גבוההמהר שיותר כמה המשימה את להשלים כדי וזאת הכרחית.רובוט תנועת בטיחות אילוצי לשני התייחסות מאלצת גבוה במהירותדינאמייםלקחת יש אשר עקריים המסלול תכנון בתהליך בחשבון.בלימה מרחק את שמירה הינו הראשון האילוץ,כזה מבל לעצור ולהספיק פתע לאירועי להגיב לרובוט שיאפשרבמכשולים לפגוע י.השני האילוץ פנייה בעת הרובוט לתנועת נוגע.ישרות שאינן קשתות גבי על נע הרובוט כאשר,לנוע עליו הקרקע עם מספיק בלתי חיכוך עקב לצדדים החלקה שתמנע כזו במהירות,לחילופין או I
- 124. רימון אילון פרופסור בהנחיית נעשה המחקר מכונות להנדסת מהפקולטה ע לקרן מודה אניוג לארווין וכן הלוי ש'ואן ג'במהלך הנדיבה הכספית התמיכה על ייקובס התשלמותי
- 125. מהירות אילוצי עם אוטונומיים רכבים ניווט מחקר על חיבור התואר לקבלת הדרישות של חלקי מילוי לשם לפילוסופיה דוקטור מנור גיל הטכניון לסנט הוגש–לישראל טכנולוגי מכון חיפה,טבתתשעה,ינואר2015
- 126. אוטונומיים רכבים ניווט מהירות אילוצי עם מנור גיל