The speed graph method: pseudo time optimal navigation among obstacles subject to uniform braking safety constraints

Abstract

This paper considers the synthesis of pseudo time optimal paths for a mobile robot navigating among obstacles subject to uniform braking safety constraints. The classical Brachistochrone problem studies the time optimal path of a particle moving in an obstacle free environment subject to a constant force field. By encoding the mobile robot’s braking safety constraint as a force field surrounding each obstacle, the paper generalizes the Brachistochrone problem into safe time optimal navigation of a mobile robot in environments populated by polygonal obstacles. Convexity of the safe travel time functional, a path dependent function, allows efficient construction of a speed graph for the environment. The speed graph consists of safe time optimal arcs computed as convex optimization problems in $O(n^3 \log (1/\epsilon ))$ total time, where n is the number of obstacle features in the environment and $\epsilon$ is the desired solution accuracy. Once the speed graph is constructed for a given environment, pseudo time optimal paths between any start and target robot positions can be computed along the speed graph in $O(n^2\log n)$ time. The results are illustrated with examples and described as a readily implementable procedure.

1 Introduction

As autonomous mobile robots are deployed in ever more demanding tasks, they strive to safely complete their tasks while minimizing overall travel time. This requirement is achieved by maximizing the robot’s speed while maintaining velocity dependent safety constraints throughout the navigation process. Examples are autonomous mobile robots operating in sentry duty and surveillance,¹ $^{,}$ ² where high speed is required to complete the task without being detected by potential intruders (Chung et al. 2011). Other examples are self-driving cars (Markoff 2010) and traffic assistance systems³ that will soon allow vehicles to be piloted autonomously at speeds up to 50 km/h (Sherr and Ramse 2013). High speed systems also include mobile robots carrying cargo in large warehouses (Teixeira et al. 2010; Guizzo 2008; Wurman et al. 2008). In all of these applications collision safety is modeled by velocity dependent constraints, thus requiring a consideration of the robot’s full position-velocity state space in order to synthesize such paths.

This paper considers the computation of safe time optimal paths for a mobile robot navigating with high speed in a polygonal environment. While navigating in the environment, the robot must limit its speed according to a uniform braking safety constraint with respect to the surrounding obstacles. When encoded as a state (position and speed) dependent constraint, the safe time optimal navigation problem can be formulated using calculus of variations. The safe time optimal path minimizes a travel time functional, $T(\alpha )$, defined over all collision free paths, $\alpha$, connecting the start and target. The paper analyzes the properties of $T(\alpha )$, and based on these properties develops the speed graph method. The method constructs a speed graph, consisting of safe time optimal arcs connecting discrete nodes located on the Voronoi diagram of the environment. The speed graph preserves the path homotopy classes of the environment,⁴ and is used to efficiently compute time optimal paths in the most promising path homotopy classes connecting the start and target in the environment. 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.

In the robotics literature, early motion planners first computed collision free paths without any velocity considerations, then selected a speed profile which minimizes travel time subject to velocity dependent safety constraints along the chosen path (Kant and Zucker 1986; Shiller and Gwo 1991). The next important development are the time optimal motion planners that operate locally in discrete time steps, whereby at each time step the mobile robot selects a local time optimal maneuver that avoids nearby obstacles. A notable example is the velocity obstacles method (Fiorini and Shiller 1998; Shiller et al. 2013), which has been extended to motion in the presence of other moving agents such as cars and pedestrians (Large et al. 2004; Snape et al. 2011). Another example is the dynamic window approach, which plans the robot’s path within a finite time horizon whose size is determined by the robot’s current state (Fox et al. 1997). Some dynamic window implementations explicitly account for safe braking constraints (Brock and Khatib 1999; Fox et al. 1997). All of these local planners are useful in dynamically evolving environments. This paper considers a static environment, where motion along the global time optimal path provides the best overall system performance.

An important global motion planning approach searches the robot’s discretized state space for safe time optimal paths. Karvaki (Choset et al. 2005) and LaValle and Kuffner (Lavalle 1998) 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 current tree with a feasible maneuver that obeys predefined velocity dependent constraints and minimizes a cost function such as path length or travel time. Karaman and Frazzoli (2011) introduced the RRT* method that additionally updates the connections between the nodes in the current search tree, in a way that ensures convergence to the global time optimal path as the number of iterations reaches infinity. This paper offers a more analytic approach for computing time optimal paths subject to velocity dependent safety constraints.

Wein, Berg, and Halperin describe a  scheme that computes collision free paths whose geometric shape is determined by a user specified “path quality” parameter $\delta$ (Wein et al. 2008). A value of $\delta =0$ corresponds to minimal length paths, while $\delta =\infty$ corresponds to maximal clearance paths. By discretizing the environment’s Voronoi diagram into $\epsilon$-length segments, the segments’ adjacency graph is searched for $\delta$-optimal paths. While (Wein et al. 2008) does not explicitly consider velocity dependent safety constraints or minimum travel time, $\delta = 1/2$ matches the safe travel time functional considered in this paper.

Our work on safe time optimal navigation started with the vc method (Manor and Rimon 2013b). The method decomposes the robot’s position-speed configuration space into free cells, whose adjacency graph allows computation of pseudo time optimal paths in the environment. However, the paths generated by the vc method trace the Voronoi diagram of the environment. Such paths maximize the robot’s clearance from the obstacles, but the true safe time optimal paths typically move closer to the obstacles with reduced total travel time. The current paper proposes a completely different approach in order to find these paths. First, the exact time optimal paths are computed in the  Voronoi cells surrounding each obstacle in the environment. The paper shows how to compute these paths as convex optimization problems in $O(n^3 \log ( 1/\epsilon ))$ total time, where n is the number of obstacle vertices, and $\epsilon$ is the desired solution accuracy. Next, a speed graph is constructed for the environment based on the time optimal path segments. The speed graph preserves the path homotopy classes of the environment, and allows efficient computation of safe time optimal paths in the most promising path homotopy classes of the environment.

The paper is structured as follows. Section 2 formulates the safe time optimal navigation problem under uniform braking safety constraints. Section 3 describes analytic solutions for the safe time optimal path near a point obstacle and a wall segment. These solutions will become building blocks for the safe time optimal paths in the Voronoi cells surrounding the obstacles in the environment. Section 4 formulates the safe time optimal navigation problem in polygonal environments, then describes basic properties of the safe travel time functional in these environments. Section 5 describes a convex optimization scheme for computing the speed graph of a given environment. Section 6 discusses examples of the speed graph’s paths in simulated environments as well as preliminary experiments. The concluding section discusses extensions of the safe time optimal navigation problem to more demanding scenarios. A preliminary version of this paper appeared in Icra 2014 (Manor and Rimon 2014).

2 Problem description and preliminaries

This paper considers the following problem, first formulated by Fraichard (2007). A mobile robot modeled as a point or a disc has to navigate with high speed in a planar environment populated by static polygonal obstacles, using ideal position and local obstacle detection sensors.⁵ The robot is assumed to move freely in $\mathbb {R}^2$ and its state, denoted $(p,\nu ) \in \mathbb {R}^2 \times \mathbb {R}^+$, consists of position, $p=(x,y)$, and speed $\nu =||\dot{p}||$. While the robot navigates with high speed in the given environment, emergency events may occur at any time along obstacle boundaries.⁶ To ensure motion safety, the robot must brake and reach a full stop without hitting any obstacle whenever such an event is detected by the robot. Braking safety is ensured when the robot maintains the following distance from the surrounding obstacles.

Definition 1

The robot’s safe braking distance, d, is the length of the shortest kinematically feasible braking path leading from the robot’s current state $(p,\nu )$ to a full stop at $\nu = 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 from the surrounding obstacles, the robot can handle emergency events that typically occur along obstacle boundaries. Note that mobile robots are typically deployed in environments containing static as well as dynamic obstacles. The current paper 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 speed, consider the robot’s maximal deceleration, denoted $a_{max}$. 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 $\upmu$mg, where $\upmu$ is the coefficient of friction at the ground contacts, m is the robot’s mass, and g is the gravitational acceleration. The robot’s dynamics during maximal deceleration is given by the equation $m a_{max} =\upmu$mg. This equation determines the robot’s maximal deceleration, $a_{max} = \upmu$g.

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 ground 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. Fraichard 2007):

$$\begin{aligned} d(\nu )=\frac{1}{2 a_{max}}\nu ^{2} \end{aligned}$$

(1)

where $a_{max}$ is the robot’s maximal deceleration and $\nu$ 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 2

When the mobile robot travels with speed $\nu$, uniform braking safety is ensured when the robot is kept at least $d(\nu )$ away from the nearest obstacle at every state $(p,\nu )$.

Uniform braking safety is clearly a conservative safety criterion. To satisfy this requirement, the robot must maintain a circular safety zone of radius $d(\nu )$ as to allow safe deceleration to a full stop without hitting any obstacle. Since $d(\nu )$ is monotonically increasing in $\nu$, 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.

Maximal speed and other safety concerns: While upper speed limits are not explicitly imposed on the mobile robot, such limits are indirectly enforced by the uniform braking safety constraint in typical indoor environments. For instance, consider the office floor environment depicted in Fig. 10 (Sect. 6). Assuming a cylindrical mobile robot of size 20 cm moving with maximal deceleration of $a_{max} = 1\, \mathrm{{{m}/{s}}}^2$, the uniform braking safety constraint imposes an upper speed limit of $\nu _{max} = 3$ m/s at all points of this environment. A mobile robot traveling with high speed must satisfy additional safety concerns, most notably the prevention of sideways sliding and tipover. The safe time optimal path generated under the uniform braking safety constraint has a sufficiently flat curvature which prevents any sideways sliding at the allowed speeds along this path, see Lemma B.2. Tipover can be prevented by careful design of the mobile robot’s mass distribution.

3 The safe time optimal path near a single obstacle feature

This section describes analytic 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 form building blocks for the safe time optimal path in the proximity cell surrounding each polygonal obstacle in the environment.⁷

3.1 The safe travel time functional

The robot path can be any piecewise smooth curve, $\alpha (s) : [0,1] \rightarrow \mathbb {R}^2$, with endpoints $p_{S}=\alpha (0)$ and $p_{T}= \alpha (1)$. When the path’s geometric parameter s is parameterized by time, s(t), the robot’s speed along $\alpha$ is given by

$$\begin{aligned} \nu \big ((\alpha (s{(t)})\big )= \Vert \frac{d}{dt} \alpha (s{(t)}) \Vert = \Vert \alpha '(s) \Vert \frac{ds(t)}{dt} \end{aligned}$$

where $\alpha '(s)= \tfrac{d}{ds} \alpha (s)$. Solving for dt gives:

$$\begin{aligned} dt=\frac{\Vert \alpha '(s) \Vert }{\nu (s)}ds. \end{aligned}$$

(2)

Integrating both sides of (2) gives the travel time functional:

$$\begin{aligned} T(\alpha ) = \int _{0}^{1} \frac{\Vert \alpha '(s) \Vert }{\nu (s)} ds. \end{aligned}$$

(3)

Note that $T(\alpha )$ is a path dependent function. The collection of piecewise smooth paths in $\mathbb {R}^2$ forms a metric space under the $d_1$ metric:

$$\begin{aligned} d_1(\alpha _1,\alpha _2)= & {} \max _{s\in [0,1]}\left\{ \Vert \alpha _1(s)-\alpha _2(s) \Vert \right\} \\&+ \max _{s\in [0,1]} \left\{ \Vert \alpha '_1(s)-\alpha '_2(s) \Vert \right\} . \end{aligned}$$

Moreover, $T(\alpha )$ is a continuous function of $\alpha$ with respect to this metric (Gelfand and Formin 1963). The integrand of $T(\alpha )$ in (3), is given by

$$\begin{aligned} F\big (\alpha (s),\alpha '(s),s\big )=\frac{\Vert \alpha '(s) \Vert }{\nu (s)}. \end{aligned}$$

Based on calculus of variations (Gelfand and Formin 1963), when F is differentiable with respect to its arguments, any extremal path of $T(\alpha )$ over all piecewise smooth paths connecting $p_{S}$ and $p_{T}$ must satisfy the Euler–Lagrange equation along the path’s smooth segments:

$$\begin{aligned} \frac{\partial F}{\partial \alpha } - \frac{\partial }{\partial s}\Big (\frac{\partial F}{\partial \alpha '}\Big )=0, \end{aligned}$$

(4)

where $\alpha$ and $\alpha '$ are to be treated as formal variables. The safe time optimal path near an obstacle edge and an obstacle vertex is next computed.

Fig. 1

a A circle rolling on the wall segment generates the cycloid curve. b The safe time optimal path near a wall segment

3.2 The safe time optimal path near an obstacle edge

The case of a point robot traveling near an obstacle edge, or a wall segment, is a direct extension of the classical Brachistochrone problem.⁸ In our case the robot’s maximal deceleration, $a_{max}$, replaces the gravitational acceleration as follows. Consider a wall segment aligned with the x axis as shown in Fig. 1a. Denote by $p_{S}=(x_{S},y_{S})$ and $p_{T}=(x_{T},y_{T})$ the start and target, which are assumed to lie in the upper half of the infinite strip spanned by the wall segment. 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(\nu )$ from the wall segment: $y \ge d(\nu )$. Substituting $d(\nu )={\nu ^2}/{2 a_{max}}$ while requiring maximal robot speed for travel time minimization gives:

$$\begin{aligned} \nu (y)= \sqrt{2 a_{max} \cdot y} \quad y \ge 0. \end{aligned}$$

(5)

Let the wall segment’s x coordinate serve as the path parameter, s, so that the robot path is parameterized by $\alpha (x)= (x,y(x))$ for $x \in [x_{S},x_{T}]$. The term $\Vert \alpha '(s) \Vert ds$ becomes ${\Vert \alpha '(x) \Vert dx =\sqrt{1+(y' (x))^2}dx}$, and the safe travel time functional (3) is given by $T(\alpha ) = \int _{x_{S}}^{x_{T}} F\big (y(x),y'(x)\big )dx$, where

$$\begin{aligned} F\big (y,y'\big ) = \sqrt{\frac{1+y'^2}{2 a_{max}\cdot y}} \qquad y \ge 0, \end{aligned}$$

(6)

with the end conditions $y(x_{S})=y_{S}$ and $y(x_{T})=y_{T}$. Any extremal path of $T(\alpha )$ must satisfy the Euler–Lagrange equation. Substituting for $F(y,y')$ in  (4), then integrating with respect to x, gives the implicit differential equation:

$$\begin{aligned} 1= c^2 \big (1+\big (y'(x)\big )^2\big )y(x) \end{aligned}$$

(7)

where c is yet to be determined. Note that the geometric shape of the extremal path solving (7) is invariant with respect to the robot’s maximal deceleration $a_{max}$. The extremal path solving (7) is a cycloid, ${\alpha (\varphi )=(x(\varphi ), y(\varphi ))}$, where $\varphi$ is the angle that a point fixed on a rolling circle spans with respect to the vertical direction (Fig. 1a).

Lemma 3.1

(Gelfand and Formin 1963) 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:

$$\begin{aligned} \alpha (\varphi ) = \begin{pmatrix} x_0 \\ \\ 0 \end{pmatrix} + \frac{1}{2 c^2} \begin{pmatrix} \varphi -\sin \varphi \\ \\ 1-\cos \varphi \end{pmatrix} \qquad \, \, \varphi \in [\varphi _{S},\varphi _{T}] \end{aligned}$$

(8)

where $\varphi _{S}$, $\varphi _{T}$, $x_0$, and c are determined by the endpoints $p_{S}$ and $p_{T}$.⁹ Moreover, $\alpha (\varphi )$ is a local minimum of $T(\alpha )$.

The time optimal path of a point robot traveling near a wall segment from $p_{S}$ to $p_{T}$ is shown Fig. 1b. The path is a cycloid that bends away from the wall segment, and reaches the x axis along the normal direction to this axis (Fig. 1a). To complete the wall segment case, consider the case where the robot is a disc of radius $\rho$. The time optimal path of the robot’s center point is given by the formula (Manor and Rimon 2013a):

$$\begin{aligned} \alpha (\varphi )= \begin{pmatrix} x_0 \\ \\ \rho \end{pmatrix} + \frac{1}{2 c^2} \begin{pmatrix} \varphi -\sin \varphi \\ \\ 1-\cos \varphi \end{pmatrix} \qquad \, \, \varphi \in [\varphi _{S},\varphi _{T}] \end{aligned}$$

where $\varphi _{S}$, $\varphi _{T}$, $x_0$, and c are determined by the endpoints $p_{S}$ and $p_{T}$.

3.3 The safe time optimal path near an obstacle vertex

Next consider a point robot traveling near an obstacle vertex, represented by a point obstacle located at the origin of the (x, y) plane. Using polar coordinates centered at the origin, ${(r,\theta )}$, the robot’s position is given by $(x,y)= (r \cos \theta ,r \sin \theta )$. 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(\nu )$ from the point obstacle: $r \ge d(\nu )$. Substituting $d(\nu )={\nu ^2}/{2 a_{max}}$ while maximizing the robot’s speed for travel time minimization gives:

$$\begin{aligned} \nu (r) = \sqrt{2a_{max} \cdot r} \qquad r\ge 0. \end{aligned}$$

Let the robot path be parametrized by the angle $\theta$, so that $\alpha (\theta )= (r(\theta ) \cos \theta , r(\theta ) \sin \theta )$ for $\theta \in [\theta _{S},\theta _{T}]$. Under this parametrization $\Vert \alpha '(\theta ) \Vert d\theta = \sqrt{r^2(\theta ) + (r'(\theta ))^2}d\theta$, and the travel time functional is given by $T(\alpha )= \int _{\theta _{S}}^{\theta _{T}} F\big ( r(\theta ), r'(\theta ) \big )d\theta$, where

$$\begin{aligned} F(r,r')=\sqrt{\frac{r^2+r'^2}{2a_{max} \cdot r}} \qquad r\ge 0, \end{aligned}$$

(9)

with the end conditions $r(\theta _{S})=r_{S}$ and $r(\theta _{T})=r_{T}$. Substituting for $F(r,r')$ in the Euler–Lagrange equation, then integrating with respect to $\theta$, gives the implicit differential equation:

$$\begin{aligned} c^2\big (r^2(\theta )+(r'(\theta ))^2\big )=r^3(\theta ) \end{aligned}$$

(10)

where c is yet to be determined. Much like the wall segment case, the geometric shape of the extremal path solving (10) is invariant with respect to $a_{max}$. The solution of (10) is a parabolic curve specified in the following lemma.

Lemma 3.2

(Manor 2014) The safe extremal path of a point robot moving in a planar environment near a point obstacle is the parabolic curve: $\alpha (\theta )= \big (r(\theta ) \cos \theta , r(\theta ) \sin \theta \big )$, where $r(\theta )$ is given by

$$\begin{aligned} r(\theta )= 2 c^2 \frac{1}{1 - \sin (\theta -\theta _0)} \qquad \theta \in [\theta _{S},\theta _{T}] \end{aligned}$$

(11)

such that $\theta _{S}$, $\theta _{T}$, c, and $\theta _0$ are determined by the endpoints $p_{S}$ and $p_{T}$. Moreover, $\alpha (\theta )$ is a local minimum of $T(\alpha )$.

The safe time optimal path can be graphically constructed as follows. Let $(r_{S},\theta _{S})$ and $(r_{T},\theta _{T})$ be the polar coordinates of $p_{S}$ and $p_{T}$. Construct the line l tangent to the circles centered at $p_{S}$ and $p_{T}$ with radii $r_{S}$ and $r_{T}$, as shown in Fig. 2a. The safe time optimal path is the parabolic arc equidistant from the point obstacle and l. The robot’s speed along the safe time optimal path is plotted in Fig. 2b, with the minimal speed attained along the portion of the path which is closest to the obstacle.

Fig. 2

a The safe time optimal path is a parabolic arc (see text). b The robot’s speed along the time optimal path, with minimum speed $\nu _0$ at the point closest to the point obstacle.q

To complete the point obstacle case, the safe time optimal path of a disc robot traveling near a point obstacle is considered in Appendix 1.

4 Basic properties of the safe time optimal path in polygonal environments

This section formulates the safe time optimal variational problem in polygonal environments, then discusses basic properties of the safe travel time functional, $T(\alpha )$, in these environments. These properties will guide the speed graph construction and the time optimal path computation.

4.1 The safe time optimal variational problem

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, $O_0$, populated by internal obstacles $O_1,\ldots ,O_k$. Let $\mathcal {F}$ denote the obstacle-free portion of the environment together with the obstacles’ boundaries. The robot’s path can be any piecewise smooth curve, $\alpha (s): [0,1]\rightarrow \mathcal {F}$, such that $\alpha (0)=p_{S}$ and $\alpha (1)=p_{T}$. Braking safety requires that the robot keep a distance of at least $d(\nu )$ from the nearest obstacle:

$$\begin{aligned} \min _{i=0 \ldots k}\{ \mathsf{dst}\left( \alpha (s),O_i\right) \} \ge d(\nu {(s)}) \qquad s \in [0,1] \end{aligned}$$

(12)

where $\mathsf{dst}(\alpha (s),O_i)$ is the minimal distance between the robot positioned at $\alpha (s)$ and the obstacle $O_i$. Travel time minimization requires maximal speed. Substituting the robot’s maximal allowed speed, $d(\nu )={\nu ^2}/{2 a_{max}}$, into (12) gives the maximal safe speed:

$$\begin{aligned} \nu (s) = \sqrt{ 2a_{max} \cdot \min _{i=0\ldots k}\{ \mathsf{dst}\left( \alpha (s),O_i\right) \} } \qquad s \in [0,1]. \end{aligned}$$

When $\nu (s)$ is substituted into $T(\alpha )$ in (3), the safe travel time functional to be minimized becomes:

$$\begin{aligned} T(\alpha )= \int _{0}^{1} F\big (\alpha (s),\alpha '(s)\big )ds \end{aligned}$$

where $F(\alpha ,\alpha ')$ is given by

$$\begin{aligned} F\left( \alpha ,\alpha '\right) =\frac{\Vert \alpha ' \Vert }{\sqrt{2a_{max} \cdot \min _{i=0\ldots k}\{ \mathsf{dst}(\alpha ,O_i) \}}}. \end{aligned}$$

(13)

The minimization of $T(\alpha )$ over all piecewise smooth paths connecting $p_{S}$ with $p_{T}$ in $\mathcal {F}$ defines the safe time optimal variational problem.

4.2 Basic properties of the safe travel time functional

This section describes basic properties of the safe travel time functional, $T(\alpha )$, starting with the notion of homotopic paths.

Definition 3

Two continuous paths with endpoints $p_{S}$ and $p_{T}$, $\alpha (s):[0,1]\rightarrow \mathcal {F}$ and $\beta (s):[0,1]\rightarrow \mathcal {F}$, belong to the same path homotopy class if there exists a continuous mapping, $f(s,t):[0,1]\times [0,1] \rightarrow \mathcal {F}$, such that $f(s,0)=\alpha (s), f(s, 1)=\beta (s)$ for $s \in [0,1]$, with $f(0,t) = p_{S},f(1,t) = p_{T}$ for $t \in [0,1]$.

For example, let the point obstacle depicted in Fig. 2b represent a disc obstacle of small radius. The two paths depicted in Fig. 2b belong to distinct homotopy classes, since one path cannot be continuously deformed into the other without crossing the obstacle. When two paths belong to the same path homotopy class, they can be thought of as “points” connected by a continuous curve (the path homotopy), in the metric space of piecewise smooth paths in $\mathbb {R}^2$. Under this interpretation, every path homotopy class forms a connected set in the metric space of piecewise smooth paths in $\mathbb {R}^2$. The following lemma establishes the existence of a safe time optimal path in every path homotopy class in $\mathcal {F}$.

Lemma 4.1

(Optimal Path Existence) The safe travel time functional, $T(\alpha )$, attains a global minimum in each path homotopy class connecting $p_{S}$ and $p_{T}$ in $\mathcal {F}$. Moreover, every local minimum path of $T(\alpha )$ lies in the interior of $\mathcal {F}$, except at its endpoints that can lie on obstacle boundaries.

Fig. 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_{\epsilon } < 2\epsilon$

Fig. 4

a The proximity cells of an obstacle $O_i$. b Rotation of the time optimal path segment tangent at the  endpoint $p_i$

Proof sketch:

The functional $T(\alpha )$ is a continuous function of $\alpha$. This property ensures that $T(\alpha )$ attains a global minimum in each path homotopy class of $\mathcal {F}$ (see Manor and Rimon 2013a[Section 4]). Let us proceed to show that every local minimum path of $T(\alpha )$ lies in the interior of $\mathcal {F}$. Braking safety requires that the robot travel with zero speed along any obstacles boundary segment, thus giving $T(\alpha )=\infty$ along any boundary segment. It follows that a local minimum path of $T(\alpha )$ can only touch isolated obstacle boundary points. 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(\alpha )$ can therefore touch only isolated convex obstacle vertices. Suppose the path touches a convex obstacle vertex as depicted in Fig. 3a. 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 Fig. 3b, 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(\alpha )$ must lie in the interior of $\mathcal {F}$, except possibly at its endpoints. $\square$

The next two properties of $T(\alpha )$ will use the following notion of proximity cells induced by the geometric features of each obstacle in the environment.

Definition 4

The geometric features of an obstacle $O_i$ (polygon edges and vertices) define proximity cells, each consisting of the points in $\mathcal {F}$ closest to the given obstacle feature.

The proximity cells of an obstacle $O_i$ are illustrated in Fig. 4a. Note that adjacent proximity cells of $O_i$ are separated by line segments, each emanating from a vertex of $O_i$ along the respective edge normal. Calculus of variations is next used to establish the $C^{(1)}$ smoothness of the safe time optimal paths.

Proposition 4.2

(Optimal Path Smoothness) Every local minimum path of the safe travel time functional, $T(\alpha )$, is a ${{\mathbf {C}}}^{(1)}$ curve having a continuous tangent.

Proof

The proximity cells partition the free space $\mathcal {F}$, and every path connecting $p_{S}$ and $p_{T}$ in $\mathcal {F}$ consists of segments that lie inside individual proximity cells. Let $\alpha _{opt}$ be a local minimum of $T(\alpha )$, connecting $p_{S}$ and $p_{T}$ in $\mathcal {F}$. The path $\alpha _{opt}$ lies in the interior of $\mathcal {F}$ according to Lemma 4.1. Since $\alpha _{opt}$ satisfies the Euler–Lagrange equation in each proximity cell of $\mathcal {F}$, it forms a $C^{(1)}$ curve in each cell. Let two segments of $\alpha _{opt}$ meet at a corner point, $p_0 = \alpha _{opt}(s_0)$, located on the common boundary of two proximity cells. The term $\partial F(\alpha ,\alpha ')/\partial \alpha '$ appearing in the Euler–Lagrange equation is a continuous function of the path parameter, s, along any extremal path of $T(\alpha )$ (Gelfand and Formin 1963)[Section 15]. Using the expression for $F(\alpha ,\alpha ')$ specified in (13), the term $\partial F(\alpha ,\alpha ')/\partial \alpha '$ is given by

$$\begin{aligned} \frac{\partial F(\alpha ,\alpha ')}{\partial \alpha '} = \frac{1}{ L(\alpha (s)) \cdot \Vert \alpha '(s) \Vert } \alpha '(s) \end{aligned}$$

where $L(\alpha )= \sqrt{2 a_{max} \cdot \min _{i=0 \ldots k}{\{\mathsf{dst}(\alpha ,O_i)\}}}$. Since ${L(\alpha (s))}$ is a continuous function of s,¹⁰ continuity of $\partial F(\alpha ,\alpha ')/\partial \alpha '$ with respect to s implies that $\alpha _{opt}'(s_0^-) =\alpha _{opt}'(s_0^+)$. Hence $\alpha _{opt}$ is a $C^{(1)}$ curve. $\square$

The next theorem establishes the minimality of the extremal paths of $T(\alpha )$.

Theorem 1

(Optimal Path Minimality) Every extremal path of the safe travel time functional, $T(\alpha )$, which lies in the interior of $\mathcal {F}$ is a local minimum of $T(\alpha )$.

Proof sketch:

Let $\alpha ^*$ be an extremal path of $T(\alpha )$, connecting $p_{S}$ and $p_{T}$ in the interior of $\mathcal {F}$. If $\alpha ^*$ lies in a single proximity cell, it forms a local minimum of $T(\alpha )$ according to Lemmas 3.1–3.2. Let us therefore assume that $\alpha ^*$ passes through $m+1 \ge 2$ proximity cells, while crossing the proximity cell boundary lines, denoted $l_1,\ldots ,l_m$ (Fig. 4a). We will show that $\alpha ^*$ is a local minimum of $T(\alpha )$ with respect to piecewise time optimal paths, $\alpha (s): [0,1]\rightarrow \mathcal {F}$ such that $\alpha (0)=p_{S}$ and $\alpha (1)=p_{T}$, consisting of time optimal segments in each proximity cell. These paths will be parametrized by a fixed division of the unit interval into sub-intervals $[0,s_1],[s_1,s_2],\ldots ,[s_m,1]$, such that $s_1,\ldots , s_m$ correspond to the crossing points of $\alpha$ with the proximity cell boundary lines $l_1,\ldots ,l_m$. A proximity cell path variation is a continuous mapping $H(s,t): [0,1]\times [0,1]\rightarrow \mathcal {F}$, such that $H(s,0)= \alpha ^*(s)$ for $s\in [0,1]$, with fixed endpoints $H(0,t)=p_{S}$ and $H(1,t)=p_{T}$ for $t\in [0,1]$. Each path of this variation is denoted $\alpha _t(s)$.

When $T(\alpha )$ is evaluated along the paths $\alpha _t(s)$, it becomes a function of the parameter t, $T(t) = \int _{0}^{1} F\left( \alpha _t(s),\alpha _t'(s)\right) ds$. These paths cross the proximity cell boundary lines at $s=s_1,\ldots ,s_m$. Hence T(t) can be equivalently written as the sum:

$$\begin{aligned} T(t) = \sum _{i=0}^{m} \int _{s_i}^{s_{i+1}} F\left( \alpha _t(s),\alpha '_t(s)\right) ds. \end{aligned}$$

Using Leibnitz rule, it is shown in (Manor and Rimon 2013a) that the derivative of T(t) with respect to t is:

$$\begin{aligned} \frac{d }{dt} T(t) = \sum _{i=1}^m \frac{1}{\nu _t(s_i)} \left( {\hat{\varvec{v}}}_t(s_i^-) - {\hat{\varvec{v}}}_t(s_i^+)\right) \cdot \frac{\partial }{\partial t}H(s_i,t). \end{aligned}$$

where ${\hat{\varvec{v}}} = \alpha _t'/\Vert \alpha _t' \Vert$ is the unit magnitude tangent vector of $\alpha _t$. Note that $\tfrac{d }{dt} T(0)=0$, since $\alpha ^*$ is a $C^{(1)}$ curve according to Proposition 4.2, hence ${\hat{\varvec{v}}}_t(s_i^-) = {\hat{\varvec{v}}}_t(s_i^+)$ for $i= 1\ldots m$ at $t= 0$.

Let us next determine the sign of the second derivative of T(t) at $t=0$. Consider the two segments of $\alpha ^*$ meeting at a common endpoint $p_i$ on the line $l_i$ (Fig. 4b). Let the segments’ other endpoints, $p_{i-1}$ and $p_{i+1}$, be held fixed. As $p_i$ varies along $l_i$, the travel time along the time optimal segments having fixed endpoints at $p_{i-1}$ and $p_{i+1}$ form two unimodal functions. Hence, the tangent to each time optimal segment at $p_i$ is collinear with $l_i$ when $p_i$ is located at the obstacle’s vertex, rotates monotonically until it becomes orthogonal to $l_i$, then continues to rotate monotonically until becoming collinear again with $l_i$ as $p_i$ moves arbitrarily far from ${\mathcal {O}}$ along $l_i$ (Fig. 4b). Based on this geometric insight, the change in the tangents of the time optimal path segment at $p_i$ satisfies the relation (see Manor and Rimon 2013a):

$$\begin{aligned} \frac{d}{dt} \Big ( ({\hat{\varvec{v}}}_t(s_i^-) - {\hat{\varvec{v}}}_t(s_i^+) )\cdot \frac{\partial }{\partial t} H(s_i,t)\Big ) > 0 \qquad t\in [0,1]. \end{aligned}$$

Hence, the second derivative of T(t) at $t=0$ satisfies:

$$\begin{aligned} \frac{d^2}{dt^2} T(0) {=} \sum _{j=1}^m \frac{1}{\nu (s_i)} \frac{d}{dt} \Big ( \big ({\hat{\varvec{v}}}(s_i^-) - {\hat{\varvec{v}}}(s_i^+)\big )\cdot \frac{\partial }{\partial t}H(s_i,0) \Big ) > 0 \end{aligned}$$

where we used the fact that ${\hat{\varvec{v}}}(s_i^-) = {\hat{\varvec{v}}}(s_i^+)$ at $t=0$. Since $\tfrac{d }{dt} T(0)=0$ and $\tfrac{d ^2}{dt^2} T(0)>0$, T(t) has a local minimum at $t=0$, which corresponds to the extremal path $\alpha ^*$. $\square$

The last property concerns the uniqueness of the safe time optimal path in each path homotopy class in $\mathcal {F}$.

Theorem 2

(Optimal Path Uniqueness) The safe travel time functional, $T(\alpha )$, possesses a unique safe time optimal path in each path homotopy class connecting $p_{S}$ with $p_{T}$ in $\mathcal {F}$.

Fig. 5

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)

Proof sketch:

According to Lemma 4.1, $T(\alpha )$ attains a global minimum in every path homotopy class connecting $p_{S}$ with $p_{T}$ in $\mathcal {F}$. Suppose one of these path homotopy classes contains two local minimum paths of $T(\alpha )$, $\alpha ^*$ and $\beta ^*$. Since $\alpha ^*$ and $\beta ^*$ belong to the same path homotopy class, the two paths can be joined by a path homotopy, $f(s,t):[0,1]\times [0,1] \rightarrow \mathcal {F}$, such that $f(s,0)=\alpha ^*(s)$ and $f(s,1)=\beta ^*(s)$ for $s \in [0,1]$. The path homotopy, f(s, t), can be interpreted as a continuous path connecting $\alpha ^*$ with $\beta ^*$ in the metric space of piecewise smooth paths in $\mathbb {R}^2$.

The functional $T(\alpha )$ is a continuous function of $\alpha$. According to the mountain pass theorem (Nirenberg 1981), the collection of all continuous paths (each being a path homotopy) connecting $\alpha ^*$ and $\beta ^*$ contains a distinguished path along which the maximal value attained by $T(\alpha )$ is minimized. Moreover, the maximal value is attained at an extremal path, $\gamma$, which is necessarily a saddle point of $T(\alpha )$. If $\gamma$ touches an obstacle vertex, a new path homotopy between $\alpha ^*$ and $\beta ^*$ can be obtained by locally pulling the path homotopy containing $\gamma$ away from the vertex as depicted in Fig. 3. The travel time along the modified path homotopy is reduced, hence the maximal value attained by $T(\alpha )$ along the modified path homotopy is lower than $T(\gamma )$. The path $\gamma$ must therefore lie in the interior of $\mathcal {F}$. This leads to a contradiction, since every extremal path of $T(\alpha )$ in the interior of $\mathcal {F}$ is a local minimum of $T(\alpha )$ according to Theorem 1. Hence $T(\alpha )$ attains a unique minimum in each path homotopy class connecting $p_{S}$ with $p_{T}$ in $\mathcal {F}$. $\square$

5 The speed graph of polygonal environments

This section describes the speed graph of a given polygonal environment, which will allow efficient computation of pseudo time optimal paths connecting $p_{S}$ with $p_{T}$ through the most promising path homotopy classes of the environment. The speed graph construction will use the following Voronoi diagram of $\mathcal {F}$.

Definition 5

The Voronoi diagram of a polygonal environment $\mathcal {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 forms a connected network of curves which retains the path homotopic structure of $\mathcal {F}$. As shown in Fig. 5a, the Voronoi arcs reach every concave corner in $\mathcal {F}$, and consequently partition the free space into distinct Voronoi cells. For instance, the environment depicted in Fig. 5a is partitioned into six Voronoi cells. A definition of the speed graph follows.

Definition 6

The speed graph of $\mathcal {F}$ has the following structure. The speed graph nodes are points on the Voronoi arcs where the obstacles’ equidistance function attains its minimal value along the respective Voronoi arc (possibly at its endpoint). 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 $\mathcal {F}$. The cost of each edge is the value of the safe travel time functional, $T(\alpha )$, along this edge.

A polygonal environment with n obstacle features contains O(n) Voronoi arcs. Each Voronoi arc contains a single speed graph node. Hence the speed graph consists of O(n) nodes and $O(n^2)$ edges. Figure 5b depicts an environment populated by two polygonal obstacles, $O_1$ and $O_2$. 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 every pair of adjacent Voronoi arcs is connected by a speed graph edge. Since the Voronoi diagram retains the path homotopy classes of $\mathcal {F}$, the speed graph contains all the path homotopy classes of $\mathcal {F}$. For instance, the path homotopy class connecting $p_{S}$ to $p_{T}$ while passing between $O_1$ and $O_2$ in Fig. 5b is represented by the speed graph path along its edges $e_1$, $e_2$, $e_3$, and $e_4$.

Fig. 6

a Adjacent speed graph nodes, $p_1$ and $p_2$, separated by three proximity cells whose boundary lines are parametrized by $(u_1,u_2)$. b The convex contours of ${T(\gamma (u_1,u_2))}$ in the $(u_1,u_2)$ plane

Every speed graph edge connects nodes located on the boundary of a common Voronoi cell associated with an obstacle $O_i$. As illustrated in Fig. 6a, the geometric features of $O_i$ define proximity cells (see Definition 4). Suppose two adjacent speed graph nodes, $p_1$ and $p_2$, are separated by $m+1$ proximity cells, with the boundaries between adjacent proximity cells denoted $l_j$ for  $j=1\ldots m$. The computation of the time optimal path between $p_1$ and $p_2$ (and hence the speed graph edge) focuses on the piecewise time optimal paths connecting $p_1$ and $p_2$. These paths consist of time optimal segments in each proximity cell, such that the segments’ endpoints, termed crosssing points, vary along the proximity cells’ boundary lines. The crosssing points are parametrized by $p(u_j):[0,1]\rightarrow l_j$ for $j=1\ldots m$. For instance, Fig. 6a depicts two crosssing points, $p(u_1)$ and $p(u_2)$. Each of the time optimal path segments is associated with a particular obstacle feature, and is therefore uniquely determined by its endpoints. The safe travel time functional, $T(\alpha )$, thus becomes a function of the parameters $u_1,\ldots ,u_m$. Computation of the speed graph edge connecting $p_1$ and $p_2$ is next performed in the set $(u_1,\ldots ,u_m) \in [0,1]\times \cdots \times [0,1]$, based on the following monotonicity property of $T(\alpha )$.

Proposition 5.1

(Manor 2014) Let $\alpha (u_1,\ldots ,u_m)$ parametrize the piecewise time optimal paths connecting $p_{S}$ and $p_{T}$ in $\mathcal {F}$, while crossing the proximity cell boundary lines $l_1,\ldots ,l_m$. Let $\varvec{u}^*=(u^*_1,\ldots ,u^*_m)$ be the parameter values of the optimal crossing points. The functional ${T(\alpha (u_1,\ldots ,u_m))}$ is monotonically increasing along any ray emanating from $\varvec{u}^*$ in the set $(u_1,\ldots ,u_m) \in [0,1]\times \cdots \times [0,1]$.

Since ${T(\alpha (u_1,\ldots ,u_m))}$ is  monotonically increasing along rays emanating from $(u^*_1,\ldots ,u^*_m)$, its level sets form star shaped sets in the set $[0,1]\times \cdots \times [0,1]$.¹¹ This property is illustrated in the following example.

Example

Figure 6 plots the contours of ${T(\alpha (u_1,u_2))}$, for the piecewise time optimal paths passing through $p(u_1)$ and $p(u_2)$. The contours form convex sets (and hence star shaped sets) in the $(u_1,u_2)$ plane, with center point at the optimal parameter values $(u^*_1,u^*_2)$. $\square$

Convex optimization algorithms require that the level sets of ${T(\alpha (u_1,\ldots ,u_m))}$ be convex. However, the same algorithms can be applied when the level sets are star shaped with a common center point at the optimal parameter values $\varvec{u}^*$. This observation is based on the relation $\nabla T(\alpha (\varvec{u})) \cdot (\varvec{u}-\varvec{u}^*) \ge 0$ satisfied by star shaped level sets, which allows construction of a barrier function (or a bounding ellipsoid) in each iteration of the convex optimization process. Standard convex optimization algorithms run in $O(m^3 \log (R/\epsilon ))$ time (Nesterov and Nemirovsky 1992), where m is the number of crossing points, R is the travel time difference between the initial path and the exact solution, and $\epsilon$ is the desired solution accuracy. Note that $\log (R/\epsilon )$ grows very slowly with the solution accuracy. When this computation is repeated over all Voronoi cells of the environment, the total number of crossing points is upper bounded by the number of obstacle features, n. The convex optimization therefore runs in $O(n^3 \log (R/\epsilon ))$ total time.

Path smoothing at the speed graph nodes: The path computed along the speed graph consists of time optimal path segments that meet non-smoothly at the speed graph nodes. At any such node, $p_i$, the robot must execute a non-smooth transition at a specified speed, $\nu _i$. However, the robot’s turning radius at $p_i$ must respect a velocity dependent lower bound, $r_{min}(\nu _i)$, described in Appendix 2. In order to satisfy this lower bound, the non-smooth transition at $p_i$ is replaced by a circular arc of radius $r_{min}(\nu _i)$, tangent to the time optimal path segments meeting at $p_i$. By following the circular arc with speed which varies linearly between its values at the circular 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.

Fig. 7

Smoothing of the time optimal path at the speed graph nodes

Fig. 8

a The speed graph of a synthetic environment (dashed curves). b The speed graph’s time optimal path, $\alpha _0$, and the smooth path $\alpha ^*$

Fig. 9

a The speed graph of the warehouse environment. b The time optimal path $\alpha ^*$ computed along the speed graph, and the time optimal path $\bar{\alpha }^*$ associated with the homotopy class of the geometrically shortest path. c The robot’s speed along the time optimal paths $\alpha ^*$ and $\bar{\alpha }^*$ in two path homotopy classes of the warehouse environment

Example

Figure 7 depicts three obstacles in $\mathbb {R}^2$, together with the Voronoi diagram and the speed graph associated with these obstacles. The safe time optimal path segments connecting $p_{S}$ with $p_{T}$ meet non-smoothly at the speed graph nodes $p_1$ and $p_2$. The robot’s speed specified at these nodes determines a minimal turning radius, $r_{min}(\nu _i)$ for $i=1,2$. By following the circular arcs having these radii at $p_1$ and $p_2$ as schematically indicated in Fig. 7, the robot executes a smooth transition between the speed graph edges meeting at  $p_1$ and $p_2$.

The computation of pseudo time optimal paths on the speed graph is next illustrated with examples.

6 Simulation examples and experimental setup

This section describes three simulation examples of the speed graph method. The first example is a highly simplified environment which allows careful inspection of the speed graph construction.

Synthetic environment: Figure 8 depicts a disc robot navigating in a room populated by four point obstacles $O_1,\ldots ,O_4$. As depicted in Fig. 8a, the speed graph nodes are selected on the Voronoi diagram of the environment, then the speed graph edges are computed within each Voronoi cell of the environment. For $p_{S}$ and $p_{T}$ located at the indicated nodes, the time optimal path connecting these points along the speed graph, $\alpha _0$, is depicted in Fig. 8b. The local smoothing of $\alpha _0$ gives the path $\alpha ^*$ depicted in Fig. 8b. Using maximal deceleration of $a_{max}= 1~\mathrm {m/sec^2}$ for the robot, and $10 \times 10$ m for the room dimensions, the travel time along the pseudo time optimal path $\alpha ^*$ is $T(\alpha ^*)=7.8$ s. For comparison, the travel time along the exact time optimal path, computed numerically with GPOPS,¹² is 7.6 s. $\square$

The next two examples resemble an industrial warehouse and an office floor environment.

Fig. 10

a The time optimal path $\alpha ^*$ computed along the speed graph. b The six best safe time optimal paths in the office floor environment. c The robot’s speed along the time optimal path ${\alpha }^*$

Warehouse environment: Figure 9 depicts a warehouse environment containing eight storage racks, $O_1,\ldots ,O_8$, surrounded by corridors. The warehouse measures $30 \times 32$ m, each storage rack measures $4 \times 8$ m, with 1 m clearance between neighboring racks. The outer corridors are four meters wide, except for the rightmost corridor which is 6 m wide. A cylindrical mobile robot is serving a delivery counter located at the lower right corner. The robot has a diameter of 80 cm, and its maximal deceleration was set to $a_{max} = 1~\mathrm {m/sec^2}$, which is typical for industrial mobile robots. The speed graph of the warehouse environment is depicted in Fig. 9a. A start point, $p_{S}$, was next selected at the delivery counter, while a target point, $p_{T}$, was selected at the lower corner of the storage rack $O_1$. The time optimal path connecting $p_{S}$ to $p_{T}$ along the speed graph edges, $\alpha _0$, is depicted in Fig. 9a. Note that $\alpha _0$ passes through the outer corridors, as these corridors allow significantly higher speeds than the narrow inner corridors. The local smoothing of $\alpha _0$ at the speed graph nodes generated the path $\alpha ^*$ depicted in Fig. 9b, with $T(\alpha ^*)=31$ s.

To highlight the speed graph’s usefulness in suggesting the most promising path homotopy class of the environment, we computed the safe time optimal path in the homotopy class of the geometrically shortest path connecting $p_{S}$ to $p_{T}$ in the warehouse environment. The homotopy class containing the geometrically shortest path starts at $p_{S}$, passes between $O_4$ and $O_6$, then between $O_3$ and $O_4$, and finally reaches $p_{T}$ while passing between $O_1$ and $O_3$. Again, we first computed the speed graph path connecting $p_{S}$ to $p_{T}$ in the selected path homotopy class, which was then locally smoothed at the nodes into the path $\bar{\alpha }^*$ depicted in Fig. 9b. While $\bar{\alpha }^*$ is shorter than $\alpha ^*$, its total travel time of $T(\bar{\alpha }^*)=43$ s is 40 % longer than $T(\alpha ^*)$. Figure 9(c) depicts the robot’s speed along the two paths, indicating the higher (but still safe) speeds taken by the robot along the outer corridors of the environment. $\square$

Office floor environment: Figure 10 depicts an office floor populated by obstacles resembling pieces of furniture as well as inner walls. The office floor measures $30 \times 40$ m and contains 24 internal obstacles, with a total of 60 vertices in its polygonal representation. While the speed graph of this environment is not shown, it contains 92 nodes (one node per Voronoi arc of the environment), and 428 edges. Note that the number of edges is much lower than the theoretical upper bound of $O(n^2)$ edges mentioned in Sect. 5. The cylindrical robot size is 20 cm, and its maximal deceleration was set to $a_{max} = 1~\mathrm {m/sec^2}$.

A start point, $p_{S}$, was selected at the lower left corner of the “conference room,” while a target point, $p_{T}$, was set in a neighboring office. The speed graph was computed for this environment, and the time optimal path computed along the speed graph, $\alpha ^*$, is depicted in Fig. 10a. Next, the six best path homotopy classes in terms of safe travel time were computed along the speed graph, using a standard k-best graph search algorithm. For each of these homotopy classes, the respective speed graph time optimal paths, $\alpha _1,\ldots ,\alpha _6$, are depicted in Fig. 10b. The travel time along these paths is $T(\alpha _1)=27.9$, $T(\alpha _2)=33.8$, $T(\alpha _3)=43.6$, $T(\alpha _4)=47.7$, $T(\alpha _5)=54.3$, and $T(\alpha _6)=57.4$ s. The best path homotopy class according to the speed graph contains the best time optimal path among all six homotopy classes, $\alpha _1=\alpha ^*$. The next best path homotopy class according to the speed graph happened to be associated with the geometrically shortest path connecting $p_{S}$ to $p_{T}$, $\alpha _2=\bar{\alpha }^*$. The speed graph thus captured the best path homotopy class in this environment, as well as correctly ranked the environment’s six best path homotopy classes. Figure 10c depicts the robot’s speed along the time optimal path $\alpha ^*$. $\square$

Initial experiments: A full experimental validation of the speed graph method is challenging for several practical reasons. First, the experiments require a truly fast mobile robot platform in order to demonstrate the practical advantage of high-speed travel along safe time optimal paths. However, readily available platforms such as iRobot Create move with maximal speed of about $1~\mathsf {m/sec}$,¹³ which is significantly lower than the theoretically safe upper speed for typical indoor environments. 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 [iRobot]. However, the iRobot lacks any global position sensor, and its position is estimated by odometric sensors mounted on the robot’s wheels.

Experiments under preparation will demonstrate the speed graph method on a customized Thunder Tiger platform, which moves with maximal speed of $10~\mathrm {m/s}$ and $a_{max} = 3~\mathrm {m/s^2}$. As a preliminary demonstration, we placed an iRobot at a docking station in the ground floor of our office building. The robot was charged with the task of serving fire extinguishers to various spots located as far as 50 m from its docking station. The robot received as input the floor’s speed graph, and delivered fire extinguishers to simulated fire breakups along safe time optimal paths computed along the speed graph of the environment. Measurements taken during these experiments showed that the iRobot was able to reach sites located 50 m away from its start point within 60 s once a fire alarm was activated (Manor and Rimon 2013a)[Section 8]. However, the iRobot is not designed for high speed navigation tasks, and its poor localization system has difficulties in accurately following the time optimal paths. Nevertheless, the experiments highlight the potential usefulness of high speed navigation for time critical applications such as emergency first response robotic platforms.

7 Conclusion

The speed graph method synthesizes pseudo time optimal paths for mobile robots navigating among polygonal obstacles subject to uniform braking safety constraints. The safe braking constraint is encoded as a speed dependent circular safety zone surrounding the robot’s current position. To find a safe time optimal path, the travel time functional, $T(\alpha )$, was formulated. Basic properties of $T(\alpha )$ guided the construction of the speed graph for the environment. The speed graph nodes are located on the environment’s Voronoi diagram, while its edges are safe time optimal arcs connecting neighboring speed graph nodes within the Voronoi cells of the environment. Monotonicity of $T(\alpha )$ over the piecewise time optimal paths in each Voronoi cell of the environment lead to an efficient convex optimization scheme for computing the speed graph edges. Once the speed graph has been constructed for a given environment, it can be used to efficiently compute time optimal paths connecting the start and target within the most promising path homotopy classes in the environment.

As demonstrated in simulation examples, the paths generated by the speed graph method specify the geometric shape of the safe time optimal path as well as the robot’s speed profile along this path. This feature provides a significant progress with respect to classical configuration space path planning approaches, which only generate collision avoiding paths without any consideration of the robot’s speed during the path planning process. Moreover, the geometric shape of the safe time optimal path is invariant with respect to the robot’s maximal deceleration, $a_{max}$. Hence, while the actual travel time along the safe time optimal path is a system dependent parameter, the path’s geometric shape depends only on the location of the start and target in the environment.

Two important extensions of the speed graph method currently under investigation are as follows. The first extension concerns environments containing static as well as dynamic obstacles. For instance, mobile robots operating autonomously in industrial warehouses may encounter tens and even hundreds of other mobile robots sharing the same environment. These robots must eventually be able to move with high speed while maintaining braking safety constraints with respect to all group members as well as the surrounding obstacles. A second extension concerns the generalization of the circular braking safety zone into a non-circular 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 would allow the robot significantly higher safe speeds than the conservative circular safety zone. This challenging extension will require planning in the mobile robot’s full four-dimensional position and velocity space, and we hope to report initial results in the near future.

Appendices

Appendix 1: Time optimal path for a disc robot

Consider a disc robot of radius $\rho$ traveling near a point obstacle located at the origin of the (x, y) plane. The path traced by the robot’s center point is parameterized by polar coordinates, $\alpha (\theta )=(r(\theta )\cos \theta , r(\theta )\sin \theta )$ for $\theta \in [\theta _{S},\theta _{T}]$, with the endpoint conditions $r(\theta _{S})=r_{S}$ and $r(\theta _{T})=r_{T}$. The distance between the disc robot and the point obstacle is given by $r(\theta )-\rho$. Braking safety requires that $r(\theta )-\rho \ge d(\nu )$. Substituting $d(\nu )={\nu ^2}/{2 a_{max}}$ while maximizing the speed for travel time minimization gives the constraint: ${\nu (r) =\sqrt{2 a_{max} (r-\rho )}}$ for $r\ge \rho$. The safe travel time functional, $T(\alpha )=\int _{\theta _{S}}^{\theta _{T}} F\big (r(\theta ),r'(\theta )\big )d\theta$, has the integrant: ${F(r,r') = \sqrt{(r^2+r'^2)/2a_{max} (r-\rho )}}$ for $r\ge \rho$. Substituting for $F(r',r)$ in the Euler–Lagrange equation, then integrating with respect to $\theta$, gives the implicit differential equation:

$$\begin{aligned} c^2 \big ( r(\theta )-\rho \big )\big ( r^2(\theta ) + (r'(\theta ))^2 \big ) = r^4(\theta ) \end{aligned}$$

(14)

where c is yet to be determined. The analytic solution of (14) is a sum of two elliptic integrals (see Manor and Rimon 2013a). Rather then use elliptic integrals, a practical conservative approximation for the analytic solution of (14) is next described. First expand the point obstacle into a disc c-obstacle of radius $\rho$, while shrinking the robot to a point. Next, replace the portion of the c-obstacle between $\theta _{S}$ and $\theta _{T}$ by polygonal segments tangent to the c-obstacle as depicted in Fig. 11. The computation of the safe time optimal path in the polygon’s vicinity forms a convex optimization problem (Manor and Rimon 2013a).

Fig. 11

The safe time optimal path determined by the polygonal approximation, $\bar{\alpha }^*$, and the exact safe time optimal path, $\alpha ^*$, near a disc obstacle

Example

The c-obstacle depicted in Fig. 11 is approximated by three segments. The segments and their connecting vertices induce five proximity cells, with variable corner points $p(u_1),\ldots ,p(u_4)$. Convex optimization of the travel time functional ${T(\alpha (u_1,\ldots ,u_4))}$ gives the safe time optimal path $\bar{\alpha }^*$ depicted in Fig. 11. Note that $\bar{\alpha }^*$ closely approximates the exact time optimal path, $\alpha ^*$, with the quality of approximation improving as the number of polygonal segments increases.

Appendix 2: The mobile robot turning radius constraint

This appendix develops an expression for the robot’s minimal turning radius, $r_{min}(\nu )$, as a function of the robot’s speed $\nu$. We shall assume the common case of a four-wheeled mobile robot. However, the expression for $r_{min}(\nu )$ can be easily adapted to other mobile robots. The three main constraints affecting the mobile robot’s 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.

Fig. 12

a Top view of a four-wheel mobile robot turning geometry. b Rear view of a left turning mobile robot

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 $r_{in}$, is the current radius of curvature of the curve traced by the robot’s innermost wheel (Fig. 12a).

The kinematic turning radius constraint: Let $\gamma$ denote the robot’s front wheels average steering angle (Fig. 12a). This angle can vary within mechanical limits in the range $[-\gamma _{max},\gamma _{max}]$. The robot’s wheels are mounted at the vertices of a rectangular frame having length L and width W (Fig. 12a). Based on simple trigonometry, the average steering angle $\gamma$ is given by the expression: $\tan \gamma = L / ( \tfrac{W}{2}+ r_{in})$. The center point turning radius is given by $r = \sqrt{\L ^2/4 + \left( W/2 + r_{in} \right) ^{2}}$. Replacing $\tfrac{W}{2} + r_{in}$ by $L / \tan \gamma$ gives the turning radius:

$$\begin{aligned} r = \sqrt{\frac{L^2}{4} + \left( \frac{L}{\tan \gamma } \right) ^{2}} \qquad \gamma \in [-\gamma _{max}, \gamma _{max}]. \end{aligned}$$

Since r is monotonically decreasing in $\left| \gamma \right|$, the minimal feasible turning radius, denoted $r_{kin}$, is attained at $\gamma = \pm \gamma _{max}$. Thus $r \ge r_{kin}$, where ${r_{kin} = L \sqrt{1/4 + 1/\tan ^2(\gamma _{max})}}$.

The sliding constraint: Sliding occurs when the centrifugal force acting on the robot, $f_r$, exceeds the radial friction force acting at the ground contacts. The net centrifugal force is given by $f_r = m \nu ^{2}/r$, where m is the robot’s mass, $\nu$ is the robot’s speed, and r is the robot’s turning radius. The radial friction force acting between the wheels and the ground, denoted $f_s$, is given by $f_s =\mu f_N = \mu mg$, where $\mu$ is the coefficient of friction between the wheels and the ground, and $f_N = mg$ is the gravitational force acting downward on the robot. Sliding is prevented as long as $f_r \le f_s$. Substituting for $f_r$ and $f_s$, the minimal turning radius that prevents sliding, denoted $r_{slide}$, is given by $r \ge r_{slide}$, where $r_{slide}(\nu ) = \nu ^2/\mu g$.

The tip-over constraint: Let $\tau _{tip}$ denote the net moment affecting the robot about a horizontal axis passing through its outer wheels’ contacts with the ground. When $\tau _{tip}<0$, the robot will tip-over and loose ground contact at its inner wheels (Fig. 12b). The lateral moment generated on the robot by the centrifugal force $f_r$, denoted $\tau _r$, is given by $\tau _r = H \cdot f_r = -\tfrac{m H}{r} \nu ^2$, where H is the robot’s center of mass height above ground (Fig. 12b). The stabilizing moment generated on the robot by the gravitational force, denoted $\tau _g$, is given by $\tau _g = mg W/2$. Tip-over is prevented when $\tau _{tip} = \tau _r + \tau _g$ is positive semi-definite:

$$\begin{aligned} \tau _{tip}= \tau _g + \tau _r = mg\frac{W}{2} - \frac{m H}{r}\nu ^2 \ge 0. \end{aligned}$$

Hence, tip-over is prevented when $r \ge r_{tip}$, where $r_{tip}$ is given by $r_{tip}(\nu ) = (2 H/g W) \nu ^{2}$.

The robot’s minimal turning radius must satisfy all three constraints as summarized in the following lemma.

Lemma 7.1

The minimal turning radius of a four-wheel mobile robot moving with speed $\nu$, $r_{min}(\nu )$, is given by

$$\begin{aligned} r_{min}(\nu ) = \max \{ r_{kin}, r_{slide}(\nu ), r_{tip}(\nu ) \} \end{aligned}$$

where $r_{kin}$ is the minimal kinematically feasible turning radius, $r_{slide}(\nu )$ is the minimal sliding turning radius, and $r_{tip}(\nu )$ is the minimal tip-over turning radius.

Let us finally establish that the time optimal path which maintains a safe braking distance from the obstacles has a sufficiently flat curvature which prevents any sideways sliding.

Lemma 7.2

Let $\alpha (s):[0,1]\rightarrow \mathcal {F}$ be the time optimal path connecting $p_{S}$ to $p_{T}$ under the uniform braking safety constraint. The path’s radius of curvature is lower bounded by the minimal sliding turning radius, $r(s)\ge r_{slide}(\nu (s))$ for $s\in [0,1]$.

Proof

Let us use the robot’s x coordinate as the geometric parameter s, so that the robo’s time optimal path is given by $\alpha (x)= (x,y(x))$ for $x \in [x_{S},x_{T}]$. The path’s radius of curvature is given by

$$\begin{aligned} r(x) = \frac{\big (1+ (y'(x))^2\big )^{3/2}}{ \left| y''(x) \right| }. \end{aligned}$$

(15)

First consider the time optimal path near a wall segment, which satisfies the differential equation (7): $c^2 (1+(y'(x))^2) y(x) = 1$. Differentiation of both sides of this equation with respect to x gives: $2y(x)y''(x) + 1+(y'(x))^2=0$. Substituting for $y'(x)$ and $y''(x)$ in (15) gives: $r(x) = \tfrac{2}{c} \sqrt{y(x)}$. The robot’s speed along the time optimal path satisfies the equation $\nu = \sqrt{2 a_{max} y}$. Since $a_{max}= \mu g$, the minimal sliding turning radius along the time optimal path is given by $r_{slide}(\nu ) = \nu ^2 / \mu g = 2 y(x)$. Hence, the inequality $r(x) \ge r_{slide}(\nu (x))$ holds true when $\frac{2}{c}\sqrt{y(x)} \ge 2 y(x)$, or equivalently when $y(x)\le 1/c^2$. Since $1/2c^2$ represents the diameter of the rolling circle which generates the cycloid curve, $y(x) \le 1/2c^2 < 1/c^2$, which gives the inequality $r(x) \ge r_{slide}(\nu (x))$ for $x \in [x_{S},x_{T}]$.

Next consider the time optimal path near an obstacle vertex. The time optimal path forms a parabolic curve, whose parametrization in terms of the robot’s x coordinate is given by

$$\begin{aligned} y(x) =\frac{x^2 + y_0^2}{2y_0} \qquad x \in [x_{S},x_{T}] \end{aligned}$$

where $y_0$ is the minimal distance between the parabolic curve and the x axis aligned with the line l (Fig. 2). Based on (15), the path’s radius of curvature is given by $r(x)= y_0 \cdot \left( 1+\tfrac{x^2}{y_0^2}\right) ^{3/2}$. The parabolic curve is equidistant from the obstacle vertex and the line l. Hence, the robot’s speed along this time optimal path is given by $\nu (x) = \sqrt{2a_{max} y(x)}= \sqrt{a_{max} (x^2 + y_0^2)/y_0}$. Since $r_{slide}(\nu ) = \nu ^2 / \mu g$ and $a_{max}=\mu g$, the inequality $r(x)\ge r_{slide}(\nu (x))$ holds true since $y_0 \cdot (1+\tfrac{x^2}{y_0^2} )^{3/2} \ge y_0 \cdot (1 + \tfrac{x^2}{y_0^2})$, which again confirms the inequality $r(x) \ge r_{slide}(\nu (x))$ for $x \in [x_{S},x_{T}]$. $\square$