Submitted to IEEE Transactions on Robotics
Generalized Multi-Speed Dubins Motion Model ^†^†thanks: This work was supported by US Office of Naval Research under Award Number N000141613032. Any opinions or findings herein are those of the authors and do not necessarily reflect the views of the sponsoring agencies. ^†^†thanks: ${}^{\dagger}$ Dept. of Electrical and Computer Engineering, University of Connecticut, Storrs, CT 06269, USA. ^†^†thanks: ${}^{\ddagger}$ Naval Undersea Warfare Center, Newport, RI 02841, USA

James P. Wilson

{}^{\dagger}

Shalabh Gupta

{}^{\dagger}

Thomas A. Wettergren

{}^{\ddagger}

Abstract

The paper develops a novel motion model, called Generalized Multi-Speed Dubins Motion Model (GMDM), which extends the Dubins model by considering multiple speeds. While the Dubins model produces time-optimal paths under a constant-speed constraint, these paths could be suboptimal if this constraint is relaxed to include multiple speeds. This is because a constant speed results in a large minimum turning radius, thus producing paths with longer maneuvers and larger travel times. In contrast, multi-speed relaxation allows for slower speed sharp turns, thus producing more direct paths with shorter maneuvers and smaller travel times. Furthermore, the inability of the Dubins model to reduce speed could result in fast maneuvers near obstacles, thus producing paths with high collision risks.

In this regard, GMDM provides the motion planners the ability to jointly optimize time and risk by allowing the change of speed along the path. GMDM is built upon the six Dubins path types considering the change of speed on path segments. It is theoretically established that GMDM provides full reachability of the configuration space for any speed selections. Furthermore, it is shown that the Dubins model is a specific case of GMDM for constant speeds. The solutions of GMDM are analytical and suitable for real-time applications. The performance of GMDM in terms of solution quality (i.e., time/time-risk cost) and computation time is comparatively evaluated against the existing motion models in obstacle-free as well as obstacle-rich environments via extensive Monte Carlo simulations. The results show that in obstacle-free environments, GMDM produces near time-optimal paths with significantly lower travel times than the Dubins model while having similar computation times. In obstacle-rich environments, GMDM produces time-risk optimized paths with substantially lower collision risks.

Index Terms:

Motion planning, Kinodynamic constraints, Dubins vehicles, Multi-speed vehicles, Time-risk cost

I Introduction

The paper develops a motion model, called Generalized Multi-Speed Dubins Motion Model (GMDM), for multi-speed vehicles. GMDM provides a higher fidelity than the Dubins model by allowing the change of speed along the path. The capability to reduce speed from its max value enables GMDM to 1) create sharp turns, thus resulting in overall shorter travel times than the Dubins paths, and 2) reduce collision risks around obstacles, thus resulting in safe maneuvering. It is theoretically established that GMDM achieves full reachability of the configuration space between any start and goal poses for any arbitrary set of speeds. It is also shown that GMDM reduces to the Dubins model for constant speeds. Furthermore, GMDM provides computationally efficient analytical solutions, thus making it suitable for real-time applications.

I-A Literature Review

A fundamental problem in path planning is to find the minimum time path from a start pose to a goal pose while considering kinodynamic constraints of the vehicle. Dubins [1][2] showed that in absence of obstacles, the shortest path for a curvature-constrained constant-speed vehicle between a pair of poses must be one of the following six canonical path types: LSL, RSR, LSR, RSL, LRL and RLR, where L(R) refers to a left (right) turn with the maximum curvature, and S indicates a straight line segment. Reeds-Shepp curves [3] extended Dubins curves by considering backward velocity. These models have analytical solutions that are easy to compute and implement. Further research considered field-of-view constraints [4], environmental disturbances [5, 6, 7, 8], multiple vehicles [9, 10], obstacle-avoidance [11], the moving-target interception roblem [12][13][14], the traveling salesman problem [15, 16, 17, 18, 19], the orienteering problem [20, 21], and the coverage problem [22, 23]. These problems, however, do not consider multiple speeds which are essential for time-optimal risk-aware motion planning [24].

Recently, Wolek, et al. [25] developed a motion model for time-optimal planning in obstacle-free environments. Their solution is based on two speeds, i.e., the min and max speeds, which are sufficient for time-optimality in obstacle-free environments. However, the solutions of this model are not closed-form and require nonlinear solvers. Kucerov, et al. [26, 27] proposed multiple speeds and turning radii to find time-optimal paths for aircraft. However, Kucerov’s model does not consider the LRL and RLR path types and requires higher computation times than the Dubins model without providing performance guarantees. Finally, there exist other kinodynamic models that consider acceleration [28] and curvature [29] constraints, but the high-dimensional nature of these problems make them infeasible for practical implementation [30].

Other motion models enforce curvature continuity (e.g., smooth transition from an L to an R segment) by considering a constraint on its derivative. Fermat’s spiral has been used to ensure that transitions between Dubins segments are curvature-continuous [31]. Fraichard and Scheuer [32] extended Reeds-Shepp curves by using Euler’s spiral, where the curvature changes linearly with respect to arc-length. Bruyninckx and Reynaerts [33] enforced a continuously differentiable path by finding fifth-order Pythagorean hodographs that satisfy the continuity constraints. Qu et al. [34] used piecewise-constant polynomials to construct complex paths that are twice-differentiable. Faigl and Vana [35] used Bézier curves to generate smooth paths to travel through a set of way points.

I-B Motivation

The constant speed constraint in Dubins model severely restricts the vehicle’s maneuverability. In obstacle-free environments, the Dubins model produces suboptimal paths that are longer with larger travel times due to its inability to create sharp turns by reducing the speed. Fig. 1a shows an example where the GMDM path turns at the minimum speed to rapidly orient the vehicle towards the goal, thus producing a path which is both shorter and quicker than the Dubins path.

The Wolek’s motion model [25] is an improvement over the Dubins model as it produces time-optimal paths in obstacle-free environments. This is achieved by utilizing extremal (i.e., maximum and minimum) speeds, which are sufficient for time-optimality in obstacle-free environments. However, unlike Dubins, Wolek’s solutions require numerical solvers for nonlinear equations, thus limiting their use in many real-time applications. On the other hand, GMDM provides analytical solutions that can be computed in real-time while approaching the time-optimal solutions of the Wolek’s model.

In obstacle-rich environments, risk evaluation becomes critical in motion planning [36, 37, 38, 39, 40, 41]. Both time and risk costs are considered by the T ${}^{\star}$ algorithm [24] for time-optimal risk-aware planning. However, the solution quality of a high-level motion planner (e.g., RRT ${}^{\star}$ and T ${}^{\star}$ ) depends on the underlying motion model used to connect any two way points. Thus, a motion planner using the Dubins model lacks the capability to reduce risk and often produces long and risky paths. Moreover, due to constant speed, the Dubins paths lack flexible maneuvering around obstacles. Fig. 1b shows an example where the Dubins path goes through a long narrow corridor which is risky, while the GMDM path is shorter, quicker and safer by virtue of changing the speed.

Similarly, a motion planner using the Wolek’s model produces sub-optimal results in obstacle-rich environments because the two extremal speeds are not sufficient to minimize the time-risk cost. For instance, the straight line (S) segments in the Wolek’s model always have the max speed for time-optimality; however, these segments should adopt lower speeds to reduce risk when approaching an obstacle. Similarly, the turn segments should adopt different slower speeds to tightly wrap around the obstacles of different geometries.

As such, the desired motion model should enable the selection of appropriate speeds for each path segment in order to produce time-risk-optimal paths that are both fast and safe. The speed selection should consider both travel time and risk based on vehicle’s orientation and distance from the obstacle. The model should preferably have analytical solutions for real-time computation. Finally, the model should be easy to understand and implement. To the best of our knowledge, no existing model in literature possesses all of these attributes.

In this regard, this paper develops a motion model, GMDM, which is a generalization of the Dubins model that incorporates multiple speeds such that any path segment can select an appropriate speed. The model was first introduced in [42], where preliminary results showed that the model provides smoother and shorter paths than those obtained with existing motion models, thus providing reduced travel times and risks.

I-C Contributions

The main contributions of this paper are as follows:

•
Model development
- –
  
  A fundamental extension of the Dubins motion model to GMDM considering multiple speeds for each path segment (i.e., L, S, and R) that enables both time and risk analysis in motion planning.
•
Model solution
- –
  
  Derivation of analytical solutions of the forward and inverse problems of the GMDM paths, which allow for real-time implementation.
- –
  
  Theoretical guarantee of full reachability for any pair of start and goal poses for any selection of speeds.
- –
  
  Analytical result to show that the Dubins model is a specific case of GMDM for constant speeds.
•
Model validation
- –
  
  Comparative evaluation of GMDM against Dubins and Wolek’s models in terms of solution quality (i.e., time/time-risk cost) and computation time in both obstacle-free and obstacle-rich environments.
- –
  
  Numerical results to show that in obstacle-free environments, the solution quality of GMDM approaches the time-optimal solutions of the Wolek’s model while enabling significantly faster computation.

I-D Organization

The rest of the paper is organized as follows. Section II presents the details, solution and properties of GMDM. Section III presents the reachability analysis of GMDM with theoretical proofs. Section IV analyzes the performance of this model in obstacle-free and obstacle-rich environments, respectively. Finally, Section V concludes the paper with recommendations for future work.

II GMDM

This section presents the analytical details of GMDM. The Dubins model consists of a set of six constant speed path types: LSL, RSR, LSR, RSL, LRL and RLR. These can be solved for times spent on each path segment to obtain the minimum-time path between any two given poses in obstacle free environments. GMDM generalizes the Dubins model by relaxing the constant speed constraint. Specifically, GMDM allows the motion planner to select the speeds for L, S, and R segments of the Dubins path types. Note that the different speeds for L and R segments lead to different turning radii, thus enhancing path maneuverability. On the other hand, reducing the speed on any segment near obtacles reduces the collision risk, thus enhancing path safety. This allows the motion planner to minimize the time and time-risk costs in obstacle-free and obstacle-rich environments, respectively.

II-A Vehicle Description

Consider a vehicle whose motion is described as

$\displaystyle\dot{x}(t)$	$\displaystyle=v(t)\cos{\theta(t)}$	(1a)
$\displaystyle\dot{y}(t)$	$\displaystyle=v(t)\sin{\theta(t)}$	(1b)
$\displaystyle\dot{\theta}(t)$	$\displaystyle=\omega(t),$	(1c)

where $\textbf{p}(t)\triangleq(x(t),y(t),\theta(t))\in\textup{SE}(2)$ is the vehicle pose at time $t$ ; $v(t)\in$ $\mathbb{V}=$ $[v_{min},v_{max}]$ is its speed in m/s at time $t$ , where $v_{min},v_{max}\in\mathbb{R}^{+}$ ; and $\omega(t)\in$ $\Omega=$ $[-\omega_{max},\omega_{max}]$ is its angular speed (i.e., turning rate) in rad/s at time $t$ , where $\omega_{max}\in\mathbb{R}^{+}$ and $+/-$ denote the left/right turn. The curvature of the vehicle is defined as $\kappa(t)=|\omega(t)|/v(t)$ , where $0\leq\kappa(t)\leq\omega_{max}/v_{min}$ . The turning radius is $r(t)=1/\kappa(t)$ , where $\kappa(t)=0$ means forward movement on a straight line.

II-B Motion Primitives

Let $\textbf{u}\triangleq(v,\omega)\in\mathbb{U}$ , where $\mathbb{U}=\mathbb{V}\times\Omega$ , be a constant input to the vehicle with pose $\textbf{p}(t)$ applied for a certain time duration $\tau\in\{\mathbb{R}^{+}\cup{0}\}$ . Let $\textup{M}:\textup{SE}(2)\times\mathbb{U}\times\{\mathbb{R}^{+}\cup{0}\}% \rightarrow\textup{SE}(2)$ be the motion primitive that describes the evolution of the pose $\textbf{p}(t)$ subject to the input $\textbf{u}\in\mathbb{U}$ . Thus, $\textbf{p}(t+\tau)=\textup{M}_{\textbf{u},\tau}(\textbf{p}(t))$ . The motion primitive M is of two types as follows:

\textup{M}_{\textbf{u},\tau}(\textbf{p}(t))=\begin{cases}\textup{C}_{\textbf{u% },\tau}(\textbf{p}(t))&\omega\neq 0\\ \textup{S}_{v,\tau}(\textbf{p}(t))&\omega=0,\end{cases}

(2)

where $\textup{C}_{\textbf{u},\tau}(\cdot)$ and $\textup{S}_{v,\tau}(\cdot)$ denote the turning ( $\omega\neq 0$ ) and straight line ( $\omega=0$ ) motions, respectively. The turning motion $\textup{C}_{\textbf{u},\tau}(\cdot)$ is again of two types: left turn $\textup{L}_{\textbf{u},\tau}(\cdot)$ ( $\omega>0$ ) and right turn $\textup{R}_{\textbf{u},\tau}(\cdot)$ ( $\omega<0$ ). Fig. 2 shows the motion primitives for straight, left, and right turn maneuvers.

•

For turning motion: $\textbf{p}(t+\tau)=\textup{C}_{\textbf{u},\tau}(\textbf{p}(t))$ s.t.

$\displaystyle x(t+\tau)$	$\displaystyle=x(t)-\frac{v}{\omega}\Big{(}\sin{\theta(t)}-\sin{\big{(}\theta(t% )+\omega\tau\big{)}}\Big{)},$	(3a)
$\displaystyle y(t+\tau)$	$\displaystyle=y(t)+\frac{v}{\omega}\Big{(}\cos{\theta(t)}-\cos{\big{(}\theta(t% )+\omega\tau\big{)}}\Big{)},$	(3b)
$\displaystyle\theta(t+\tau)$	$\displaystyle=\theta(t)+\omega\tau.$	(3c)

•

For straight line motion: $\textbf{p}(t+\tau)=\textup{S}_{v,\tau}(\textbf{p}(t))$ s.t.

$\displaystyle x(t+\tau)$	$\displaystyle=x(t)+v\tau\cos{\theta(t)},$	(4a)
$\displaystyle y(t+\tau)$	$\displaystyle=y(t)+v\tau\sin{\theta(t)},$	(4b)
$\displaystyle\theta(t+\tau)$	$\displaystyle=\theta(t).$	(4c)

II-C Motion Model

Based on the motion primitives of (• ‣ II-B) and (• ‣ II-B), we now describe GMDM. Let $\textbf{p}_{0}=(x_{0},y_{0},\theta_{0}$ ) and $\textbf{p}_{f}=(x_{f},y_{f},\theta_{f})$ denote the start and goal poses, respectively. The objective is to find the GMDM path between these poses. Similar to Dubins, a GMDM path connecting $\textbf{p}_{0}$ and $\textbf{p}_{f}$ consists of three path segments labeled by $i=1,2,3$ . Let the start and end poses of segment $i$ be denoted by $\textbf{p}_{i-1}=(x_{i-1},y_{i-1},\theta_{i-1})$ and $\textbf{p}_{i}=(x_{i},y_{i},\theta_{i})$ , respectively. Note that $\textbf{p}_{f}=\textbf{p}_{3}$ . Let $\textbf{u}_{i}=(v_{i},\omega_{i})\in\mathbb{U}$ be the input to vehicle on segment $i$ , applied for a time duration $\tau_{i}\in\{\mathbb{R}^{+}\cup{0}\}$ . Then, $\textbf{p}_{i-1}$ evolves on segment $i$ to $\textbf{p}_{i}$ s.t. $\textbf{p}_{i}=\textup{M}_{\textbf{u}_{i},\tau_{i}}(\textbf{p}_{i-1})$ . Thus, each segment $i$ follows the motion primitive of either a turn C or a straight line S.

This leads to two fundamental classes of GMDM path types: CSC (i.e., LSL, LSR, RSL and RSR) and CCC (i.e., LRL and RLR). For notation simplicity, we avoid the subscripts on the motion primitives and use them only when needed. A CSC path first turns (either left or right), then goes straight, and finally turns (either left or right) before reaching the final pose. Similarly, a CCC path makes three turning maneuvers before reaching the final pose, where $\operatorname{sign}\omega_{1}\neq\operatorname{sign}\omega_{2}$ and $\operatorname{sign}\omega_{2}\neq\operatorname{sign}\omega_{3}$ (i.e., consecutive turning motions must be in different directions). Note: the other complex path types (e.g., paths with more than three segments) are not included in GMDM and are beyond the scope of this work.

Furthermore, while GMDM is built on the six Dubins path types, it allows a different speed on each path segment. Thus, the total number of GMDM path type configurations depend on the number speeds allowed for each path segment. We show later that GMDM provides full reachability of the configuration space for any speed selections. Figure 3 shows the GMDM path types with a different speed on each segment.

II-D Model Analysis

For GMDM analysis we discuss the forward and inverse problems. First, we define the following parameters.

	$\displaystyle\begin{split}r_{i}\triangleq\frac{v_{i}}{\omega_{i}},\ i=1,2,3,% \end{split}$			(5a)
	$\displaystyle\begin{split}r_{ij}\triangleq r_{i}-r_{j},\ i,j=1,2,3,\end{split}$			(5b)
	$\displaystyle\begin{split}\delta_{i}\triangleq v_{i}\tau_{i},\ i=1,2,3,\end{split}$			(5c)
	$\displaystyle\begin{split}\phi_{i}\triangleq\omega_{i}\tau_{i},\ i=1,2,3,\end{split}$			(5d)
	$\displaystyle\begin{split}\theta_{ij}\triangleq\bmod(\theta_{i}-\theta_{j},2% \pi\operatorname{sign}{(\omega_{i})}),\ i,j=0,1,2,3,\end{split}$			(5e)

where $\bmod(a,m)\triangleq a-m\lfloor\frac{a}{m}\rfloor\ \forall a,m\in\mathbb{R}$ [43]. Note that $|r_{i}|$ , $|\phi_{i}|$ and $\delta_{i}$ , $i=1,2,3$ , represent the turning radius, rotation and length of a path segment $i$ , respectively. The rotations satisfy the constraint $|\phi_{i}|<2\pi$ .

II-D1 The forward problem analysis

Definition II.1 (Forward problem).

The forward problem aims to find the final pose $\textbf{p}_{f}$ of a GMDM path given the control inputs $\textbf{u}_{i}$ and time durations $\tau_{i}$ of its segments $i=1,2,3$ .

The final pose of a GMDM path is obtained by applying the motion primitive for each segment consecutively as follows

\textbf{p}_{f}=\textup{M}_{u_{3},\tau_{3}}(\textup{M}_{u_{2},\tau_{2}}(\textup% {M}_{u_{1},\tau_{1}}(\textbf{p}_{0}))).

(6)

Proposition II.1 (CSC forward).

Given $\textbf{p}_{0}$ and $(\textbf{u}_{i},\tau_{i})$ , $i=1,2,3$ , the final pose $\textbf{p}_{f}$ for a CSC path is given as

	$\displaystyle\begin{split}x_{f}={}&x_{0}-r_{1}\sin\theta_{0}-r_{31}\sin(\theta% _{0}+\phi_{1})+\\ &\delta_{2}\cos(\theta_{0}+\phi_{1})+r_{3}\sin(\theta_{0}+\phi_{1}+\phi_{3}),% \end{split}$			(7a)
	$\displaystyle\begin{split}y_{f}={}&y_{0}+r_{1}\cos\theta_{0}+r_{31}\cos(\theta% _{0}+\phi_{1})+\\ &\delta_{2}\sin(\theta_{0}+\phi_{1})-r_{3}\cos(\theta_{0}+\phi_{1}+\phi_{3}),% \end{split}$			(7b)
	$\displaystyle\begin{split}\theta_{f}={}&\bmod(\theta_{0}+\phi_{1}+\phi_{3},2% \pi).\end{split}$			(7c)

Proof.

See Appendix -A1. ∎

Proposition II.2 (CCC forward).

Given $\textbf{p}_{0}$ and $(\textbf{u}_{i},\tau_{i})$ , $i=1,2,3$ , the final pose $\textbf{p}_{f}$ for a CCC path is given as

	$\displaystyle\begin{split}x_{f}={}&x_{0}-r_{1}\sin\theta_{0}+r_{12}\sin(\theta% _{0}+\phi_{1})+\\ &r_{23}\sin(\theta_{0}+\phi_{1}+\phi_{2})+r_{3}\sin{(\theta_{0}+\phi_{1}+\phi_% {2}+\phi_{3})},\end{split}$			(8a)
	$\displaystyle\begin{split}y_{f}={}&y_{0}+r_{1}\cos\theta_{0}-r_{12}\cos(\theta% _{0}+\phi_{1})-\\ &r_{23}\cos(\theta_{0}+\phi_{1}+\phi_{2})-r_{3}\cos{(\theta_{0}+\phi_{1}+\phi_% {2}+\phi_{3})},\end{split}$			(8b)
	$\displaystyle\begin{split}\theta_{f}={}&\bmod(\theta_{0}+\phi_{1}+\phi_{2}+% \phi_{3},2\pi).\end{split}$			(8c)

Proof.

See Appendix -A2. ∎

II-D2 The inverse problem analysis

Definition II.2 (Inverse problem).

The inverse problem aims to find the time durations $\tau_{i}$ of GMDM path segments given the start and goal poses ( $\textbf{p}_{0}$ , $\textbf{p}_{f}$ ) and the control inputs $\textbf{u}_{i}$ , $i=1,2,3$ . The total travel time of the path is given as $\mathscr{T}=\sum_{i}\tau_{i}$ .

Let us define the following parameters:

	$\displaystyle\begin{split}a\triangleq x_{f}-x_{0}+r_{1}\sin\theta_{0}-r_{3}% \sin\theta_{f},\end{split}$			(9a)
	$\displaystyle\begin{split}b\triangleq y_{f}-y_{0}-r_{1}\cos\theta_{0}+r_{3}% \cos\theta_{f},\end{split}$			(9b)

which are known for the inverse problem.

Proposition II.3 (CSC inverse).

Given $\textbf{p}_{0}$ , $\textbf{p}_{f}$ and $\textbf{u}_{i}$ , the time durations $\tau_{i}$ , $i=1,2,3$ , of CSC path segments are given as

\tau_{1}=\frac{\theta_{10}}{\omega_{1}},\tau_{2}=\frac{\delta_{2}}{v_{2}},\ % \textrm{and}\ \tau_{3}=\frac{\theta_{31}}{\omega_{3}},

(10)

where

\theta_{1}=\arcsin\Big{(}\frac{-r_{31}}{\sqrt{a^{2}+b^{2}}}\Big{)}-% \operatorname{atan2}(-b,a),

(11)

and

\delta_{2}=\sqrt{a^{2}+b^{2}-r^{2}_{31}}.

(12)

Proof.

See Appendix -B1. ∎

Proposition II.4 (CCC inverse).

Given $\textbf{p}_{0}$ , $\textbf{p}_{f}$ and $\textbf{u}_{i}$ , the time durations $\tau_{i}$ , $i=1,2,3$ , of CCC path segments are given as

\tau_{1}=\frac{\theta_{10}}{\omega_{1}},\tau_{2}=\frac{\theta_{21}}{\omega_{2}% },\tau_{3}=\frac{\theta_{32}}{\omega_{3}},

(13)

where

\theta_{1}=\pi-\arcsin\Big{(}\frac{a^{2}+b^{2}+r_{12}^{2}-r_{23}^{2}}{2r_{12}% \sqrt{a^{2}+b^{2}}}\Big{)}-\operatorname{atan2}(-b,a)

(14)

and

\theta_{2}=\pi-\arcsin\Big{(}\frac{a^{2}+b^{2}+r_{23}^{2}-r_{12}^{2}}{2r_{23}% \sqrt{a^{2}+b^{2}}}\Big{)}-\operatorname{atan2}(-b,a).

(15)

Proof.

See Appendix -B2. ∎

Corollary II.1.

The GMDM path types reduce to the Dubins set [2] when

1.

$|r_{1}|=|r_{3}|$ for CSC.
2.

$|r_{1}|=|r_{2}|=|r_{3}|$ for CCC.

Proof.

The Dubins set [2] follows by plugging conditions 1 and 2 in (II.1) and (II.2), respectively. ∎

This section introduced GMDM and provided the solutions for its forward and inverse problems. The optimal controls $\textbf{u}_{i}^{*}$ for a GMDM path are selected by a higher-level planner. For practical implementation, the optimal controls are determined as follows. For a given start and goal pose, first a discrete set of controls are used to generate different GMDM path type configurations. For example, consider $v_{i}\in\{v_{min},v_{max}\}$ and $|\omega_{i}|\in\{\omega_{max},0\}$ , then the six GMDM path types generate a total of $6\times 2^{3}=48$ different configurations. Then, the inverse solution for each of these configurations is determined, and its path quality (i.e., time/time-risk [24] cost) is evaluated. Due to the closed-form nature of the solutions, they can be computed quickly in real time. Finally, the control that provides the best path quality is selected to yield the GMDM path. Section IV shows several examples for different applications.

III Reachability Analysis

This section presents the reachability analysis of GMDM.

Definition III.1 (Reachability).

A pose $\textbf{p}_{f}$ is said to be reachable if there exists a GMDM path from $\textbf{p}_{0}$ to $\textbf{p}_{f}$ .

Let $\mathcal{R}_{j}\subseteq\text{SE(2)}$ denote the reachable set of a GMDM path type $j$ , $j$ = CSC or CCC, that includes all final poses $\textbf{p}_{f}$ that are reachable from the start pose $\textbf{p}_{0}$ . Let

	$\displaystyle c\triangleq x_{0}-r_{1}\sin\theta_{0}+r_{3}\sin\theta_{f},$		(16a)
	$\displaystyle d\triangleq y_{0}+r_{1}\cos\theta_{0}-r_{3}\cos\theta_{f}.$		(16b)

Theorem 1 (CSC Reachability).

The reachable set $\mathcal{R}_{CSC}$ of GMDM satisfies the following

\begin{split}(x_{f}-c)^{2}+(y_{f}-d)^{2}\geq&r_{13}^{2}.\end{split}

(17)

Proof.

See Appendix -C1. ∎

Fig. 4a visualizes the reachable set $\mathcal{R}_{CSC}$ derived from (17). The top row of Fig. 4a shows $\mathcal{R}_{CSC}$ in SE(2) space with four reachability plots corresponding to LSL, RSR, LSR and RSL path types. As seen in each of these plots, the reachable region of a path type lies outside a tube including its boundary while the region inside this tube is unreachable. The bottom three rows of Fig. 4a show the cross sections of $x_{f}$ - $y_{f}$ planes for different $\theta_{f}$ . As seen from each of these cross-sections, the reachable region of a path type lies outside an open circle with center $(c,d)$ and radius $|r_{13}|$ .

Remark III.1.

The control inputs $\textbf{u}_{i}$ , $i=1,2,3$ , affect the radii $|r_{i}|$ , which in turn affect the center and radius of the circular region in (17). Hence, the reachability of a CSC path type can be improved by choosing from a variety of controls.

Corollary III.1.

The Dubins LSL and RSR path types each provide full reachability of the SE(2) space.

Proof.

For the Dubins LSL and RSR path types, $r_{1}=r_{3}$ , thus $r_{31}=0$ . From (17) we get $(x_{f}-c)^{2}+(y_{f}-d)^{2}\geq 0$ , which is true for all $p_{f}=(x_{f},y_{f},\theta_{f})\in SE(2)$ . ∎

Theorem 2 (CCC Reachability).

The reachable set $\mathcal{R}_{CCC}$ of GMDM satisfies the following:

\begin{split}r_{13}^{2}\leq&\big{(}x_{f}-c\big{)}^{2}+\big{(}y_{f}-d\big{)}^{2% }\leq\big{(}r_{12}-r_{23}\big{)}^{2}\end{split}

(18)

Proof.

See Appendix -C2. ∎

Fig. 4b visualizes the reachable set $\mathcal{R}_{CCC}$ derived from (18). The top row of Fig. 4b shows $\mathcal{R}_{CCC}$ in SE(2) space with two reachability plots corresponding to LRL and RLR path types. As seen in each of these plots, the reachable region for a path type lies inside an annular region within a tube with inner and outer boundaries included, while everything else is unreachable. The bottom three rows of Fig. 4b show the cross sections of $x_{f}$ - $y_{f}$ planes for different $\theta_{f}$ . As seen from each of these cross-sections, the reachable region of a path type lies inside an annulus of a closed circle with center $(c,d)$ , inner radius $|r_{31}|$ and outer radius $|r_{12}-r_{23}|$ .

Remark III.2.

The control inputs $\textbf{u}_{i}$ , $i=1,2,3$ , affect the radii $|r_{i}|$ , which in turn affect the center and radii of the annulus region in (18). Hence, the reachability of a CCC path type can be improved by choosing from a variety of controls.

Corollary III.2.

The Dubins $LRL$ and $RLR$ paths each provide reachability inside a circle with center $(c,d)$ and radius $4|r_{1}|$ .

Proof.

For the Dubins LRL and RLR path types, $r_{1}=-r_{2}=r_{3}$ , thus $r_{31}=0$ and $r_{12}-r_{23}=r_{1}-r_{2}-r_{2}+r_{3}=4r_{1}$ . From (18) we get $\big{(}x_{f}-c\big{)}^{2}+\big{(}y_{f}-d\big{)}^{2}\leq(4r_{1})^{2}$ . ∎

The reachable sets for the GMDM CSC and CCC path types in Theorems 1 and 2 above were numerically verified by solving a large number of goal poses. From the results of Theorems 1 and 2, it is clear that any individual GMDM path type from either CSC or CCC class does not provide full reachability. However, the next theorem guarantees full reachability of GMDM.

Theorem 3 (Full Reachability).

GMDM achieves full reachability of the SE(2) space using LSL and RSR path types.

Proof.

See Appendix -C3. ∎

Fig. 5 visualizes and numerically validates Theorem 3. Fig. 5a shows the unreachable regions of LSL and RSR path types in the SE(2) space, while Fig. 5b shows these regions in the cross-sections of the $x_{f}-y_{f}$ plane for different $\theta_{f}$ . The plots are drawn for $\textbf{p}_{0}=(0,0,0)$ and $(r_{1},r_{3})=(0.1,1.0)$ . Clearly, the unreachable regions of LSL and RSR path types are disjoint, thus verifying that all poses are reachable when using at least these two path types. Numerical evaluations for other sets of turning radii show similar results.

Remark III.3.

The pair of $LSL$ and $RSR$ path types provides full reachability to GMDM. However, it can be easily shown that for any other pair, the unreachable regions of path types are not always disjoint, thus not providing full reachability.

IV Results and Discussion

This section presents the comparative evaluation results of GMDM with the baseline models for time-optimal and time-risk optimal planning in different scenarios. The performance is measured by the solution quality (i.e., time/time-risk cost), computation time and collision risk of the produced path.

The solutions of the Dubins model and GMDM are closed form and obtained analytically, whereas the solutions of the Wolek model required the IPOPT solver [44]. All motion models are coded in C++. The computations were done on an Intel Core-i7 $7700$ processor with 32GB of RAM on Ubuntu $16.04$ LTS. A curvature-constrained vehicle with $v_{\min}=0.3$ m/s, $v_{\max}=1.0$ m/s and $\omega_{\max}=1.0$ rad/s is considered, with the associated turning radii of $r_{\min}=0.3$ m and $r_{\max}=1.0$ m. The results are generated by extensive Monte Carlo simulations and a detailed discussion on the advantages of GMDM for time/time-risk optimal planning is presented.

IV-A Discussion of the Baseline Models and GMDM

The Dubins model is the first baseline model for comparison. It produces the time-optimal paths for constant-speed curvature-constrained vehicles. Let $\Gamma_{D}$ denote the set of Dubins path types: LSL, LSR, RSL, RSR, LRL, RLR. As discussed earlier, the Dubins model might produce suboptimal paths for multi-speed vehicles due to its inability to create sharp turns; however, it is the simplest model with closed form solution, thus suitable for onboard real-time implementation.

Table I: Sets of candidate path types.

$\Gamma_{W}$ (Wolek, et al. [25])		$\Gamma_{G2^{\prime}}$ (GMDM) ¹
No.	Path Type	No.	Path Type
1	L ${}^{+}$ S ${}^{+}$ L ${}^{+}$	1	L ${}^{+}$ S ${}^{+}$ L ${}^{+}$
2	L ${}^{+}$ S ${}^{+}$ R ${}^{+}$	2	L ${}^{+}$ S ${}^{+}$ R ${}^{+}$
3	R ${}^{+}$ S ${}^{+}$ L ${}^{+}$	3	R ${}^{+}$ S ${}^{+}$ L ${}^{+}$
4	R ${}^{+}$ S ${}^{+}$ R ${}^{+}$	4	R ${}^{+}$ S ${}^{+}$ R ${}^{+}$
5	L ${}^{+}$ S ${}^{+}$ L ${}^{+}$ L ${}^{-}$	5	L ${}^{+}$ S ${}^{+}$ L ${}^{-}$
6	L ${}^{+}$ S ${}^{+}$ R ${}^{+}$ R ${}^{-}$	6	L ${}^{+}$ S ${}^{+}$ R ${}^{-}$
7	R ${}^{+}$ S ${}^{+}$ L ${}^{+}$ L ${}^{-}$	7	R ${}^{+}$ S ${}^{+}$ L ${}^{-}$
8	R ${}^{+}$ S ${}^{+}$ R ${}^{+}$ R ${}^{-}$	8	R ${}^{+}$ S ${}^{+}$ R ${}^{-}$
9	L ${}^{-}$ L ${}^{+}$ S ${}^{+}$ L ${}^{+}$	9	L ${}^{-}$ S ${}^{+}$ L ${}^{+}$
10	L ${}^{-}$ L ${}^{+}$ S ${}^{+}$ R ${}^{+}$	10	L ${}^{-}$ S ${}^{+}$ R ${}^{+}$
11	R ${}^{-}$ R ${}^{+}$ S ${}^{+}$ L ${}^{+}$	11	R ${}^{-}$ S ${}^{+}$ L ${}^{+}$
12	R ${}^{-}$ R ${}^{+}$ S ${}^{+}$ R ${}^{+}$	12	R ${}^{-}$ S ${}^{+}$ R ${}^{+}$
13	L ${}^{-}$ L ${}^{+}$ S ${}^{+}$ L ${}^{+}$ L ${}^{-}$	13	L ${}^{-}$ S ${}^{+}$ L ${}^{-}$
14	L ${}^{-}$ L ${}^{+}$ S ${}^{+}$ R ${}^{+}$ R ${}^{-}$	14	L ${}^{-}$ S ${}^{+}$ R ${}^{-}$
15	R ${}^{-}$ R ${}^{+}$ S ${}^{+}$ L ${}^{+}$ L ${}^{-}$	15	R ${}^{-}$ S ${}^{+}$ L ${}^{-}$
16	R ${}^{-}$ R ${}^{+}$ S ${}^{+}$ R ${}^{+}$ R ${}^{-}$	16	R ${}^{-}$ S ${}^{+}$ R ${}^{-}$
17	L ${}^{-}$ R ${}^{-}$ L ${}^{-}$	17	L ${}^{+}$ R ${}^{+}$ L ${}^{+}$
18	R ${}^{-}$ L ${}^{-}$ R ${}^{-}$	18	R ${}^{+}$ L ${}^{+}$ R ${}^{+}$
19	L ${}^{+}$ L ${}^{-}$ L ${}^{+}$ L ${}^{+}$	19	L ${}^{+}$ R ${}^{+}$ L ${}^{-}$
20	L ${}^{+}$ L ${}^{-}$ L ${}^{+}$ R ${}^{+}$	20	R ${}^{+}$ L ${}^{+}$ R ${}^{-}$
21	R ${}^{+}$ R ${}^{-}$ R ${}^{+}$ L ${}^{+}$	21	L ${}^{+}$ R ${}^{-}$ L ${}^{+}$
22	R ${}^{+}$ R ${}^{-}$ R ${}^{+}$ R ${}^{+}$	22	R ${}^{+}$ L ${}^{-}$ R ${}^{+}$
23	L ${}^{+}$ L ${}^{+}$ L ${}^{-}$ L ${}^{+}$	23	L ${}^{+}$ R ${}^{-}$ L ${}^{-}$
24	L ${}^{+}$ R ${}^{+}$ R ${}^{-}$ R ${}^{+}$	24	R ${}^{+}$ L ${}^{-}$ R ${}^{-}$
25	R ${}^{+}$ L ${}^{+}$ L ${}^{-}$ L ${}^{+}$	25	L ${}^{-}$ R ${}^{+}$ L ${}^{+}$
26	R ${}^{+}$ R ${}^{+}$ R ${}^{-}$ R ${}^{+}$	26	R ${}^{-}$ L ${}^{+}$ R ${}^{+}$
27	L ${}^{+}$ L ${}^{-}$ L ${}^{+}$ L ${}^{+}$ L ${}^{-}$	27	L ${}^{-}$ R ${}^{+}$ L ${}^{-}$
28	L ${}^{+}$ L ${}^{-}$ L ${}^{+}$ R ${}^{+}$ R ${}^{-}$	28	R ${}^{-}$ L ${}^{+}$ R ${}^{-}$
29	R ${}^{+}$ R ${}^{-}$ R ${}^{+}$ L ${}^{+}$ L ${}^{-}$	29	L ${}^{-}$ R ${}^{-}$ L ${}^{+}$
30	R ${}^{+}$ R ${}^{-}$ R ${}^{+}$ R ${}^{+}$ R ${}^{-}$	30	R ${}^{-}$ L ${}^{-}$ R ${}^{+}$
31	L ${}^{-}$ L ${}^{+}$ L ${}^{+}$ L ${}^{-}$ L ${}^{+}$	31	L ${}^{-}$ R ${}^{-}$ L ${}^{-}$
32	L ${}^{-}$ L ${}^{+}$ R ${}^{+}$ R ${}^{-}$ R ${}^{+}$	32	R ${}^{-}$ L ${}^{-}$ R ${}^{-}$
33	R ${}^{-}$ R ${}^{+}$ L ${}^{+}$ L ${}^{-}$ L ${}^{+}$
34	R ${}^{-}$ R ${}^{+}$ R ${}^{+}$ R ${}^{-}$ R ${}^{+}$

The Wolek model [25] expanded the Dubins model for time-optimal planning of multi-speed vehicles. However, this model uses only the extremal (i.e., min and max) speeds which are shown to be sufficient for time-optimal planning in obstacle-free environments. As such, this model might produce sub-optimal results for time/time-risk optimal planning in obstacle-rich environments. Specifically, in this model, the L and R segments are of two types: bang ( ${}^{+}$ ) and cornering ( ${}^{-}$ ), which correspond to the max and min speeds, respectively. The S segments are always at the max speed. Clearly, the speeds other than min and max might be necessary on the turn segments to wrap tightly around the obstacles and on the straight line segments to reduce risk. Furthermore, there is no restriction on the number of segments for a path type. For example, the path L ${}^{-}$ L ${}^{+}$ S ${}^{+}$ R ${}^{+}$ R ${}^{-}$ means that the vehicle first turns left at min speed, then turns left at max speed, then continues on a straight line at max speed, then turns right at max speed, and finally turns right at min speed before reaching the goal. This kind of complex maneuver might be difficult to follow by the vehicle. Wolek et al. [25] provided a sufficient set of $84$ candidate path types that is guaranteed to yield the time-optimal path to any goal pose in an obstacle-free environment. For simplicity, Wolek et al. [25] further provided a smaller set of $34$ candidate path types that is most likely to yield the time-optimal path. Let $\Gamma_{W}$ denote this set of most-likely candidate paths [25], as shown in Table I. Unlike Dubins, the solution of the Wolek model requires nonlinear optimization, thus making it impractical for onboard real-time implementation.

Finally, GMDM is an extension of the Dubins model that allows multi-speed configurations of the Dubins path types. The set of GMDM path types depends on the number of speed configurations that can be adapted as needed.

Definition IV.1.

Let GMDM-k denote the GMDM where the speed of each path segment belongs to a set $\mathbb{V}_{k}=\{v_{\ell}\in[v_{min},v_{max}]:\ell=1,...k\}$ of $k\in\mathbb{N}^{+}$ different speeds and the turning rate $|\omega_{i}|\in\{0,\omega_{max}\}$ .

Let $\Gamma_{Gk}$ denote the set of GMDM- $k$ path types. Then, the total number of configurations of the GMDM- $k$ path types is $|\Gamma_{Gk}|$ = $6k^{3}$ .

Remark IV.1.

The set $\Gamma_{D}$ of Dubins path types is contained in the set $\Gamma_{Gk}$ of GMDM- $k$ path types, $\forall$ $k\in\mathbb{N}^{+}$ , (i.e., $\Gamma_{D}\subseteq\Gamma_{Gk}$ ). The equality holds for $k=1$ (i.e., $\Gamma_{D}=\Gamma_{G1}$ ). Thus,

•

GMDM-1 is identical to the Dubins model.
•

The solution quality of GMDM paths in terms of time/time-risk costs is guaranteed to be better than or the same as the Dubins paths.

GMDM provides a closed form solution just like the Dubins model; thus, its computation is straightforward. Furthermore, the GMDM path types are simple with only three segments; thus, the time complexity of GMDM to obtain the solution of each individual path type is the same as that of Dubins. However, as compared to the Dubins model, GMDM has more path types depending on the number of possible speeds on each segment. Thus, the time complexity of GMDM typically falls between that of the Dubins and Wolek’s models; however, GMDM computation is still real-time and significantly faster than the Wolek’s model due to the closed-form nature of the solutions. While GMDM does not guarantee to produce time-optimal solutions in obstacle-free environments, the results later show that GMDM approaches the solution quality of the Wolek’s model (i.e., time-optimal solutions), and they are obtained in real-time with orders of magnitude faster computation. On the other hand, neither GMDM nor the baseline models guarantee time/time-risk optimal planning in obstacle-rich environments; however, the results later show that GMDM in fact produces significantly better solution quality (i.e., time-risk cost) than the baseline models with much faster computation time and simplicity of paths.

The Dubins model, the Wolek’s model and GMDM use the path types from $\Gamma_{D}$ , $\Gamma_{W}$ , and $\Gamma_{Gk}$ , respectively, to compute the different possible paths between any pair of poses. Then, each model selects the path with the least cost as the final result.

IV-B Time-Optimal Planning

First, we present the comparative results for time-optimal planning. According to the Wolek’s model [25], the time-optimal paths can be generated using the extremal speeds in obstacle-free environments. Thus, we consider a set $\Gamma_{G2^{\prime}}\subset\Gamma_{G2}$ of GMDM path types that are built from bang (i.e., max speed) and cornering (i.e., min speed) arcs and max speed straight line segments. Table I shows the set $\Gamma_{G2^{\prime}}$ that consists of $32$ GMDM path types.

IV-B1 Visualization of time-optimized paths

To compare the paths produced by the three motion models, we consider different goal poses around the origin with the start pose $\textbf{p}_{0}$ = $(0,0,0)$ . Fig. 6 shows the paths produced by the three motion models to the goal poses and the corresponding travel times. As seen, the Dubins paths to the goals are indirect, i.e., long and curvy, due to the single-speed restriction. However, the Dubins model takes only $\sim 2\times 10^{-5}$ s to find the solution. The Wolek’s paths, on the other hand, are time-optimal with sharp turns and shortest travel times. However, the Wolek’s model has a higher average computation time of $\sim 6.53\times 10^{-1}$ s. Finally, the GMDM paths are much more direct than the Dubins paths and approach the solution quality of the Wolek’s paths. GMDM takes only $\sim 4\times 10^{-5}$ s to find the solution; thus, it is well-suited for real time applications. Taking an example, say the goal pose to the right of the start pose, the Dubins, Wolek’s and GMDM paths have the travel time costs of $\sim 6.70$ s, $\sim 3.62$ s and $\sim 4.09$ s, respectively. Thus, considering the travel and computation time costs, the GMDM paths provide much better solution quality as compared to the Dubins paths while requiring similar computation times. On the other hand, GMDM has a significant computational advantage over the Wolek’s solutions while achieving similar path quality. Furthermore, GMDM is easier to implement due to the closed-form solutions. Also, its paths with only three segments are simpler and more appealing as compared to the Wolek’s paths with more than three segments; thus, they are easier to follow by onboard controllers.

IV-B2 Traveling Salesman Problem

The above analysis provided insights into the types of paths produced by the three motion models with their travel and computation time costs. However, many real-time path planning problems often need to consider the combinations of different intermediate waypoints before reaching the final destination. One such problem is the Dubins traveling salesman problem (TSP) [15, 16, 17, 18] which is stated as follows: given a collection of $n$ points that must be visited, find (1) the sequence of points and (2) the heading at each point in the sequence that yields the time-optimal path for a curvature-constrained vehicle. It is clear that the overall path quality and computation time of this combinatorial optimization problem depends on the quality and computation time of the underlying motion model.

We consider the scenario shown in Fig. 7a. The starting point of the vehicle is at the origin with $\textbf{p}_{0}=(0,0,0)$ , which is marked with the number "0". There are three points of interest that must be visited after which the vehicle must return to the start pose. At each point, four headings { $0$ , $\pi/2$ , $\pi$ , $3\pi/2$ }rad are considered. The objective is to find the minimum time path that visits each of these points at a certain heading and finally returns back to the origin. To solve TSP using any underlying motion model, we have to first compute the paths and their time costs for every pose pair between different points of interest. A pose at a point of interest can connect to $8$ other poses. Thus, for a total of $12$ poses, there are $12\times 8=96$ pose pairs. Additionally, there are $12+12=24$ connections between the start pose and the other poses and vice versa. Thus, there are a total of $96+24=120$ pose pairs. For each motion model, once the paths and their time costs are computed for the above pose pairs, the optimal solution of TSP is found by searching for the sequence that has the least total travel time cost.

Fig. 7a shows the paths produced by each motion model, where the numbers $1$ , $2$ and $3$ indicate the order in which the points of interest are visited. The Dubins path takes long and clumsy routes to connect each of the points of interest, resulting in a total travel time cost of $\sim 18.30$ s. While the Dubins path is the longest, it is produced in the fastest time of $\sim 0.007$ s. The Wolek’s solution, on the other hand, provides a superior path quality, with direct and fast paths connecting the points of interest with a total travel time cost of $\sim 11.24$ s. However, it takes $\sim 60.83$ s to compute this path, which might not be acceptable for on-demand dynamic path planning problems. Finally, the quality of GMDM TSP solution is very close to the Wolek’s solution with a total travel time cost of $\sim 12.64$ s, but it is obtained significantly faster in $\sim 0.011$ s.

For further validation, we conducted a Monte Carlo study of the TSP. For this study, thirty scenarios were considered, with each consisting of five points of interest. For each scenario, these five points of interest were randomly distributed around the start pose within a $5m$ radial distance. Each point has four possible headings as before. Again, the objective is to find the minimum time path that travels through each of these points and returns to the origin. Fig. 7b shows the statistical comparison results by box plots of the travel and computation time costs for the above Monte Carlo simulations. The dot marks the median, the box shows the middle $50^{th}$ percentile and the horizontal lines show the min and max values. Fig. 7b shows that the GMDM’s solution quality (i.e., travel time cost) is superior to the Dubins solutions and approaches that of the Wolek’s time-optimal solutions. At the same time, Fig. 7b shows that GMDM is significantly faster, similar to Dubins, and took only $\sim 0.5$ s to get the TSP solution as compared to the Wolek’s model which required $\sim 230$ s to get the solution. Overall, the above results demonstrate that in obstacle-free environments: (1) GMDM provides superior path quality as compared to the Dubins model while approaching the quality of the time-optimal Wolek’s paths, and (2) GMDM has very low computational requirements similar to Dubins while being significantly faster than the Wolek’s model; thus it is suitable for real-time path planning and replanning applications.

IV-B3 Time-optimal planning in obstacle-rich environments

Next, we present the comparative results for time-optimal planning in obstacle-rich environments with no consideration of risk. Fig. 8a shows an obstacle-rich scenario with the start and goal poses. For global planning, we use RRT ${}^{*}$ [45] as the high level planner that uses the Dubins, Wolek, and GMDM as the underlying motion models to connect any pair of sample poses. As seen in Fig. 8a, the Dubins model produces a long path around the bottom obstacle due to its inability to make a sharp turn. The Wolek’s model produces a better path that goes around the top obstacle to reach the goal. This path has a shorter travel time since it turns rapidly at slow speed and go between the two obstacles. Finally, GMDM produces the best path that wraps around the top obstacle to quickly reach the goal in the fastest travel time. Note: the Wolek’s paths are time-optimal in obstacle-free environments; however, there is no performance guarantee in obstacle-rich environments.

Fig. 8b shows the convergence plot for each motion model. As seen, the Dubins plot converged the fastest, although the solution quality is low. On the other hand, the Wolek’s plot did not converge in the allocated time, thus yielding a mediocre solution quality. Finally, the GMDM plot converged orders of magnitude faster than the Wolek’s plot, while yielding the best solution quality as compared to both the baseline models. Figs. 8c and 8d show the results of Monte Carlo simulations on this scenario. Fig. 8c shows the travel time box plots of the three models. Clearly, GMDM produces the overall best travel times while the Dubins paths took the longest times. Fig. 8d shows the number of nodes created using RRT ${}^{*}$ . Clearly, GMDM allowed for the creation of a large number of nodes due to faster computation and thus yielding better solution quality. On the other hand, the Wolek’s model allowed the creation of much smaller number of nodes due to its high computation time, thus yielding slow convergence.

IV-C Time-risk optimal planning

Now, we present the comparative results for time-risk optimal planning in obstacle-rich environments.

IV-C1 Motivation for time-risk optimal planning

As discussed earlier, the joint time-risk optimal planning becomes critical in obstacle-rich environments to produce short but also safe paths. However, this requires flexibility in changing speed along the path to reduce risks near obstacles or through narrow passages yet providing reasonable travel times.

Fig. 9 visualizes and compares the time-optimal and time-risk optimal paths produced using the T ${}^{\star}$ planner [24] with GMDM as the underlying motion model. While the top row of Fig. 9 shows the speed-encoded time-optimal and time-risk optimal paths, the bottom row shows the corresponding risk-encoded paths. As seen in the top row, the time-optimal path runs only at the max-speed and yields a shorter travel time. On the other hand, the time-risk optimal path runs at three different speeds and yields a higher travel time. The time-risk optimal path chooses the min speed to make sharp turns and to reduce the collision risks. Thus, it runs at max speed when it is safe (i.e., when the approach time to an obstacle is high), min speeds during turning and in high-risk regions, and moderate speeds in other regions. As seen in the bottom row, the time-optimal path runs at high risk in most regions, while the time-risk optimal path runs at minimum risk and it is safe. The figure also shows that both the average and max risk of the time-risk optimal path are significantly reduced.

IV-C2 Comparative evaluation results

As mentioned before, GMDM produces significantly better paths in terms of time-risk cost due to its added flexibility on choosing multiple speeds. While the Dubins paths have limited maneuverability because of the single speed constraint, the Wolek’s paths also have limited capability in reducing the time-risk cost due to the use of only two speeds (i.e., min and max) and because the straight line segments are always at max speed, which increases the collision risk. In contrast, the GMDM paths enable appropriate speed selection to keep a balance between time and risk costs. In addition, the turning segments in GMDM can have multiple turning radii depending on the speeds, thus providing better maneuverability around obstacles.

Fig. 10 compares the time-risk optimal paths produced by GMDM-1 (the Dubins model), the Wolek’s model, GMDM-2, GMDM-3 and GMDM-4. These models are implemented in the T ${}^{\star}$ planner [24] for the construction of the time-risk optimal paths in a complex maze scenario. For the T ${}^{\star}$ framework, the map is divided into cells of size $1m\times 1m$ with 8-connectivity. Each cell has 8 possible orientations that are evenly spaced at intervals of $\pi/4$ . The computations in T ${}^{\star}$ are done using an a priori look-up table generation of all possible path types between any pair of neighboring poses on the grid. This speeds up the search time for path generation later. Details of time-risk cost calculation and path generation are found in [24] with a summary in Appendix -D.

The Dubins model used $v_{max}$ , the Wolek’s model used $v_{min}$ and $v_{max}$ , and GMDM-2, GMDM-3 and GMDM-4 used 2, 3 and 4 speeds, respectively, which are uniformly-spaced in $[v_{min},v_{max}]$ . The results in Figs. 10a and 10f show that the GMDM-1 (the Dubins model) path travels fastest; however, it has the highest time-risk cost due to high average and max risks on the path. Figs. 10b and 10f show that the Wolek’s path slows down at corners and reduces risk only slightly but is still very high due to the fast straight line segments, thus leading to an overall high time-risk cost. Figs. 10c and 10f show that the GMDM-2 path adapts two speeds throughout the entire path and hence able to reduce the risk significantly, thus leading to a much smaller time-risk cost. As seen, GMDM-2 mildly compromises the travel time for better safety. The computation time of GMDM-2, including the table generation and the search times, is smaller than the Wolek’s model. Finally, Figs. 10d-10f show that GMDM-3 and GMDM-4 adapt multiple speeds throughout the path to further reduce the risk and overall time-risk cost at the expense of slightly higher computation time. Thus, its clear that GMDM provides enhanced maneuverability by using multiple speeds while substantially reducing the risks, thereby providing a notably superior time-risk optimal path. Furthermore, the use of multiple speeds and thus turning radii gives smoother paths that can better adapt to the shape of the obstacles.

V Conclusions and Future Work

This paper developed a new motion model, called Generalized Multi-speed Dubins Motion Model (GMDM), that extends the Dubins model to incorporate multiple speeds for fast and safe maneuvering. GMDM allows each path segment to have any speed and turning rate. GMDM is mathematically proven to provide full reachability with closed form solutions. It is shown that GMDM solutions are suitable for real-time implementation. For constant speed, GMDM reduces to the original Dubins model. To the best of our knowledge, no existing model offers these capabilities. The effectiveness of GMDM was demonstrated for both time-optimal and time-risk optimal motion planning problems in various scenarios. For the time-optimal motion planning problem, GMDM generates paths with a solution quality approaching that of the optimal paths and gives a significant improvement over Dubins model. In obstacle-rich environments, GMDM provides shorter paths while achieving a substantially lower risk.

Future work includes using GMDM in advanced motion planners in real-world planning applications, e.g., dynamic environments [40], human-robot interaction [46] and dynamic target tracking [47]. GMDM could also be extended to higher-order motion models (e.g., considering acceleration).

-A Solution of the Forward Problem

-A1 Proof of Proposition II.1

Proof.

For a CSC path type, the forward equation is given as $\textbf{p}_{f}=C_{\textbf{u}_{3},\tau_{3}}(S_{v_{2},\tau_{2}}(C_{\textbf{u}_{1% },\tau_{1}}(\textbf{p}_{0})))$ , where

	$\displaystyle\begin{split}\textbf{p}_{1}=C_{\textbf{u}_{1},\tau_{1}}(\textbf{p% }_{0}),\end{split}$			(19a)
	$\displaystyle\begin{split}\textbf{p}_{2}=S_{v_{2},\tau_{2}}(\textbf{p}_{1}),% \end{split}$			(19b)
	$\displaystyle\begin{split}\textbf{p}_{f}=C_{\textbf{u}_{3},\tau_{3}}(\textbf{p% }_{2}).\end{split}$			(19c)

From (19a), (• ‣ II-B) and (II-D) we get

	$\displaystyle\begin{split}x_{1}={}&x_{0}-r_{1}\big{(}\sin{\theta_{0}}-\sin{(% \theta_{0}+\phi_{1})}\big{)},\end{split}$			(20a)
	$\displaystyle\begin{split}y_{1}={}&y_{0}+r_{1}\big{(}\cos{\theta_{0}}-\cos{(% \theta_{0}+\phi_{1})}\big{)},\end{split}$			(20b)
	$\displaystyle\begin{split}\theta_{1}={}&\bmod(\theta_{0}+\phi_{1},2\pi).\end{split}$			(20c)

From (19b), (-A1), (• ‣ II-B) and (II-D) we get

	$\displaystyle\begin{split}x_{2}={}&x_{0}-r_{1}\big{(}\sin{\theta_{0}}-\sin{(% \theta_{0}+\phi_{1})}\big{)}+\delta_{2}\cos{(\theta_{0}+\phi_{1})},\end{split}$			(21a)
	$\displaystyle\begin{split}y_{2}={}&y_{0}+r_{1}\big{(}\cos{\theta_{0}}-\cos{(% \theta_{0}+\phi_{1})}\big{)}+\delta_{2}\sin{(\theta_{0}+\phi_{1})},\end{split}$			(21b)
	$\displaystyle\begin{split}\theta_{2}={}&\bmod(\theta_{0}+\phi_{1},2\pi).\end{split}$			(21c)

From (19c), (-A1), (• ‣ II-B) and (II-D) we get

	$\displaystyle\begin{split}x_{f}={}&x_{0}-r_{1}\sin{\theta_{0}}-r_{31}\sin{(% \theta_{0}+\phi_{1})}+\\ {}&\delta_{2}\cos{(\theta_{0}+\phi_{1})}+r_{3}\sin{(\theta_{0}+\phi_{1}+\phi_{% 3})},\end{split}$			(22a)
	$\displaystyle\begin{split}y_{f}={}&y_{0}+r_{1}\cos\theta_{0}+r_{31}\cos(\theta% _{0}+\phi_{1})+\\ &\delta_{2}\sin(\theta_{0}+\phi_{1})-r_{3}\cos(\theta_{0}+\phi_{1}+\phi_{3}),% \end{split}$			(22b)
	$\displaystyle\begin{split}\theta_{f}={}&\bmod(\theta_{0}+\phi_{1}+\phi_{3},2% \pi).\end{split}$			(22c)

∎

-A2 Proof of Proposition II.2

Proof.

For a CCC path type the forward equation is given as $\textbf{p}_{f}=C_{\textbf{u}_{3},\tau_{3}}(C_{\textbf{u}_{2},\tau_{2}}(C_{% \textbf{u}_{1},\tau_{1}}(\textbf{p}_{0})))$ where

	$\displaystyle\begin{split}\textbf{p}_{1}=C_{\textbf{u}_{1},\tau_{1}}(\textbf{p% }_{0}),\end{split}$			(23a)
	$\displaystyle\begin{split}\textbf{p}_{2}=C_{\textbf{u}_{2},\tau_{2}}(\textbf{p% }_{1}),\end{split}$			(23b)
	$\displaystyle\begin{split}\textbf{p}_{f}=C_{\textbf{u}_{3},\tau_{3}}(\textbf{p% }_{2}).\end{split}$			(23c)

From (23a), (• ‣ II-B) and (II-D) we get

	$\displaystyle\begin{split}x_{1}={}&x_{0}-r_{1}\big{(}\sin{\theta_{0}}-\sin{(% \theta_{0}+\phi_{1})}\big{)},\end{split}$			(24a)
	$\displaystyle\begin{split}y_{1}={}&y_{0}+r_{1}\big{(}\cos{\theta_{0}}-\cos{(% \theta_{0}+\phi_{1})}\big{)},\end{split}$			(24b)
	$\displaystyle\begin{split}\theta_{1}={}&\bmod(\theta_{0}+\phi_{1},2\pi).\end{split}$			(24c)

From (23b), (-A2), (• ‣ II-B) and (II-D) we get

	$\displaystyle\begin{split}x_{2}={}&x_{0}-r_{1}\sin{\theta_{0}}+r_{12}\sin{(% \theta_{0}+\phi_{1})}+\\ &r_{2}\sin{(\theta_{0}+\phi_{1}+\phi_{2})},\end{split}$			(25a)
	$\displaystyle\begin{split}y_{2}={}&y_{0}+r_{1}\cos{\theta_{0}}-r_{12}\cos{(% \theta_{0}+\phi_{1})}-\\ &r_{2}\cos{(\theta_{0}+\phi_{1}+\phi_{2})},\end{split}$			(25b)
	$\displaystyle\begin{split}\theta_{2}={}&\bmod(\theta_{0}+\phi_{1}+\phi_{2},2% \pi).\end{split}$			(25c)

From (23c), (-A2), (• ‣ II-B) and (II-D) we get

	$\displaystyle\begin{split}x_{f}={}&x_{0}-r_{1}\sin{\theta_{0}}+r_{12}\sin{(% \theta_{0}+\phi_{1})}+\\ &r_{23}\sin{(\theta_{0}+\phi_{1}+\phi_{2})}+r_{3}\sin{(\theta_{0}+\phi_{1}+% \phi_{2}+\phi_{3})},\end{split}$			(26a)
	$\displaystyle\begin{split}y_{f}={}&y_{0}+r_{1}\cos{\theta_{0}}-r_{12}\cos{(% \theta_{0}+\phi_{1})}-\\ &r_{23}\cos{(\theta_{0}+\phi_{1}+\phi_{2})}-r_{3}\cos{(\theta_{0}+\phi_{1}+% \phi_{2}+\phi_{3})},\end{split}$			(26b)
	$\displaystyle\begin{split}\theta_{f}={}&\bmod(\theta_{0}+\phi_{1}+\phi_{2}+% \phi_{3},2\pi).\end{split}$			(26c)

∎

-B Solution of the Inverse Problem

-B1 Proof of Proposition II.3

Proof.

Using (II-D2) and (20c), we rewrite (7a) and (7b) as

	$\displaystyle-r_{31}\sin{\theta_{1}}+\delta_{2}\cos{\theta_{1}}={}$	$\displaystyle a,$		(27a)
	$\displaystyle\delta_{2}\sin{\theta_{1}}+r_{31}\cos{\theta_{1}}={}$	$\displaystyle b.$		(27b)

Taking the squares of (27a) and (27b), adding them and rearranging, we get

\delta_{2}=\sqrt{a^{2}+b^{2}-r^{2}_{31}}.

(28)

Next, (27a) $\cdot$ $\sin{\theta_{1}}$ - (27b) $\cdot$ $\cos{\theta_{1}}$ gives

a\sin{\theta_{1}}-b\cos{\theta_{1}}=-r_{31}.

(29)

Then applying the following trigonometric identity

A\sin{\alpha}+B\cos\alpha=\sqrt{A^{2}+B^{2}}\sin(\alpha+\operatorname{atan2}(B% ,A))

(30)

we get

\theta_{1}=\arcsin\Big{(}\frac{-r_{31}}{\sqrt{a^{2}+b^{2}}}\Big{)}-% \operatorname{atan2}(-b,a)

(31)

Since $\phi_{1}=\omega_{1}\tau_{1}=\theta_{10}$ , we can solve for $\tau_{1}$ using (31). Similarly, since $\delta_{2}=v_{2}\tau_{2}$ , we can solve for $\tau_{2}$ using (28). Finally, since $\phi_{3}=\omega_{3}\tau_{3}=\theta_{31}$ , we can solve for $\tau_{3}$ using (31). The solutions for $\tau_{1}$ , $\tau_{2}$ , and $\tau_{3}$ are given as

\tau_{1}=\frac{\theta_{10}}{\omega_{1}},\tau_{2}=\frac{\delta_{2}}{v_{2}},\ % \textrm{and}\ \tau_{3}=\frac{\theta_{31}}{\omega_{3}}.

(32)

∎

-B2 Proof of Proposition II.4

Proof.

Using (II-D2), (24c) and (25c), we rewrite (8a) and (8b) as

	$\displaystyle a-r_{12}\sin{\theta_{1}}={}$	$\displaystyle r_{23}\sin\theta_{2},$		(33a)
	$\displaystyle b+r_{12}\cos{\theta_{1}}={}$	$\displaystyle-r_{23}\cos\theta_{2}.$		(33b)

Taking the squares of (33a) and (33b), adding them and rearranging we get

a\sin\theta_{1}-b\cos\theta_{1}=(a^{2}+b^{2}+r_{12}^{2}-r_{23}^{2})/2r_{12}.

(34)

Using (30) and the fact that $|\phi_{2}|\in(\pi,2\pi)$ [48], we get

\theta_{1}=\pi-\arcsin\Big{(}\frac{a^{2}+b^{2}+r_{12}^{2}-r_{23}^{2}}{2r_{12}% \sqrt{a^{2}+b^{2}}}\Big{)}-\operatorname{atan2}(-b,a)

(35)

Next, we rewrite (33a) and (33b) as

	$\displaystyle a-r_{23}\sin\theta_{2}={}$	$\displaystyle r_{12}\sin{\theta_{1}}$		(36a)
	$\displaystyle b+r_{23}\cos\theta_{2}={}$	$\displaystyle-r_{12}\cos{\theta_{1}}$		(36b)

Taking the squares of (36a) and (36b), adding them and rearranging we get

a\sin\theta_{2}-b\cos\theta_{2}=(a^{2}+b^{2}+r_{23}^{2}-r_{12}^{2})/2r_{23}.

(37)

Again using (30) and the fact that $|\phi_{2}|\in(\pi,2\pi)$ [48], we get

\theta_{2}=\pi-\arcsin\Big{(}\frac{a^{2}+b^{2}+r_{23}^{2}-r_{12}^{2}}{2r_{23}% \sqrt{a^{2}+b^{2}}}\Big{)}-\operatorname{atan2}(-b,a)

(38)

Since $\phi_{1}=\omega_{1}\tau_{1}=\theta_{10}$ , we can solve for $\tau_{1}$ using (35). Similarly, since $\phi_{2}=\omega_{2}\tau_{2}=\theta_{21}$ , we can solve for $\tau_{2}$ using (35) and (38). Finally, since $\phi_{3}=\omega_{3}\tau_{3}=\theta_{32}$ , we can solve for $\tau_{3}$ using (38). The solutions for $\tau_{1}$ , $\tau_{2}$ , and $\tau_{3}$ are given as

\tau_{1}=\frac{\theta_{10}}{\omega_{1}},\tau_{2}=\frac{\theta_{21}}{\omega_{2}% },\tau_{3}=\frac{\theta_{32}}{\omega_{3}}.

(39)

∎

-C Reachability Proofs

-C1 Proof of Theorem 1

Proof.

From the CSC solution in (10), it is clear that the solutions for $\tau_{i}$ exist as long as the solutions for $\theta_{1}$ and $\delta_{2}$ exist. Note that $a$ , $b$ , and $r_{31}$ are real by their definitions. The solution for $\theta_{1}$ exists if the domain of the $\arcsin$ term in (11) is between -1 and 1, i.e.,

-1\leq\frac{-r_{31}}{\sqrt{a^{2}+b^{2}}}\leq 1

(40)

This implies that $a^{2}+b^{2}\geq r_{31}^{2}$ . This condition also makes sure that $\delta_{2}$ is real in (12). By substituting $a=x_{f}-c$ and $b=y_{f}-d$ in the above condition we get $(x_{f}-c)^{2}+(y_{f}-d)^{2}\geq r_{31}^{2}$ . ∎

-C2 Proof of Theorem 2

Proof.

From the CCC solution in (13), it is clear that the solutions for $\tau_{i}$ exist as long as the solutions for $\theta_{1}$ and $\theta_{2}$ exist. Note that $a$ , $b$ , $r_{12}$ , and $r_{23}$ are real by their definitions. The solutions for $\theta_{1}$ and $\theta_{2}$ exist if the domains of the $\arcsin$ terms in (14) and (15) are between -1 and 1, i.e.,

	$\displaystyle-1\leq\frac{a^{2}+b^{2}+r_{12}^{2}-r_{23}^{2}}{2r_{12}\sqrt{a^{2}% +b^{2}}}\leq 1$		(41a)
	$\displaystyle-1\leq\frac{a^{2}+b^{2}-r_{12}^{2}+r_{23}^{2}}{2r_{23}\sqrt{a^{2}% +b^{2}}}\leq 1$		(41b)

The above inequalities (41a) and (41b) imply that

	$\displaystyle\left(\frac{a^{2}+b^{2}+r_{12}^{2}-r_{23}^{2}}{2r_{12}\sqrt{a^{2}% +b^{2}}}\right)^{2}\leq 1$		(42a)
	$\displaystyle\left(\frac{a^{2}+b^{2}-r_{12}^{2}+r_{23}^{2}}{2r_{23}\sqrt{a^{2}% +b^{2}}}\right)^{2}\leq 1$		(42b)

	$\displaystyle\left({a^{2}+b^{2}+r_{12}^{2}-r_{23}^{2}}\right)^{2}\leq 4r_{12}^% {2}(a^{2}+b^{2})$		(43a)
	$\displaystyle\left({a^{2}+b^{2}-r_{12}^{2}+r_{23}^{2}}\right)^{2}\leq 4r_{23}^% {2}(a^{2}+b^{2})$		(43b)

Putting $q=a^{2}+b^{2}$ we get

	$\displaystyle\left(q+(r_{12}^{2}-r_{23}^{2})\right)^{2}\leq 4r_{12}^{2}q$		(44a)
	$\displaystyle\left(q-(r_{12}^{2}-r_{23}^{2})\right)^{2}\leq 4r_{23}^{2}q$		(44b)

Rearranging we get

	$\displaystyle q^{2}+(r_{12}^{2}-r_{23}^{2})^{2}+2q(r_{12}^{2}-r_{23}^{2})-4r_{% 12}^{2}q\leq 0$		(45a)
	$\displaystyle q^{2}+(r_{12}^{2}-r_{23}^{2})^{2}-2q(r_{12}^{2}-r_{23}^{2})-4r_{% 23}^{2}q\leq 0$		(45b)

Adding we get

q^{2}-2(r_{12}^{2}+r_{23}^{2})q+(r_{12}^{2}-r_{23}^{2})^{2}\leq 0

(46)

Factorizing we get

(q-(r_{12}+r_{23})^{2})(q-(r_{12}-r_{23})^{2})\leq 0

(47)

For LRL path type, $r_{1}>0$ , $r_{2}<0$ and $r_{3}>0$ . Thus, $r_{12}\geq 0$ and $r_{23}\leq 0$ . For RLR path type $r_{1}<0$ , $r_{2}>0$ and $r_{3}<0$ . Thus, $r_{12}\leq 0$ and $r_{23}\geq 0$ . For both these path types

(r_{12}+r_{23})^{2}\leq(r_{12}-r_{23})^{2}

(48)

Therefore, from (47) and (48) we get

(r_{12}+r_{23})^{2}\leq q\leq(r_{12}-r_{23})^{2}

(49)

Simplifying and substituting for $q$ we get,

r_{13}^{2}\leq(x_{f}-c)^{2}+(y_{f}-d)^{2}\leq(r_{12}-r_{23})^{2}

(50)

∎

-C3 Proof of Theorem 3

Proof.

Consider the LSL and RSR path types. It is sufficient to show that the unreachable regions of these two path types are disjoint. This implies that the union of their reachable regions cover the entire SE(2) space. Based on the CSC reachability condition in Theorem 1, the unreachable regions of LSL and RSR path types for any final heading $\theta_{f}$ are described by open circles. Let $q^{LSL}\triangleq(c^{LSL},d^{LSL})$ and $q^{RSR}\triangleq(c^{RSR},d^{RSR})$ be the centres, and $|r_{31}^{LSL}|$ and $|r_{31}^{RSR}|$ be the radii of the unreachable circular regions of LSL and RSR path types, respectively. We need to show that the distance between the centers of these circles is larger than or equal to the sum of their radii. Note that for any given control, $r_{1}^{R}=-r_{1}^{L}<0$ and $r_{3}^{R}=-r_{3}^{L}<0$ .

The sum of radii of these circles is

|r^{LSL}_{13}|+|r^{RSR}_{13}|=|r^{L}_{1}-r^{L}_{3}|+|r^{R}_{1}-r^{R}_{3}|=2|r^% {L}_{1}-r^{L}_{3}|=2|r^{LSL}_{13}|.

(51)

The distance between the centres of these circles is given as

dist(q^{LSL},q^{RSR})=\sqrt{(c^{LSL}-c^{RSR})^{2}+(d^{LSL}-d^{RSR})^{2}}

(52)

Then using (III) we get,

\begin{split}dist(q^{LSL},q^{RSR})&=2\sqrt{(r^{L}_{1})^{2}+(r^{L}_{3})^{2}-2r^% {L}_{1}r^{L}_{3}\cos(\theta_{0}-\theta_{f})}\\ &\geq 2\sqrt{(r_{1}^{L})^{2}+(r_{3}^{L})^{2}-2(r_{1}^{L})(r_{3}^{L})}\\ &=2|r_{1}^{L}-r_{3}^{L}|\\ &=2|r_{13}^{LSL}|\end{split}

(53)

Thus from (51) and (53) the unreachable open circular regions of LSL and RSR path types are non-overlap**. ∎

-D Review of T ${}^{\star}$ for Time-Risk Path Planning

T ${}^{\star}$ [24] finds approximate time-risk optimal paths for curvature constrained vehicles in the presence of obstacles. The inputs to T ${}^{\star}$ are the motion models which generate the candidate paths between any two intermediate poses.

Then the time-risk cost is computed for all the candidate paths. The risk cost is computed by estimating the collision time of the vehicle with any obstacles tangent to its current direction. Let $\gamma$ be a feasible path parameterized by $s$ . Let $d_{c}(s)$ be the distance to the nearest obstacle in the direction of the pose at $\gamma(s)$ . Let $v(s)$ be the speed of the vehicle at $s$ . Thus, $t_{c}(s)\triangleq d_{c}(s)/v(s)$ is the expected collision time for the vehicle to hit the obstacle. The risk cost at $s$ is defined as:

\mathscr{R}(s)=\begin{cases}1+\frac{t^{\star}}{t_{c}(s)}\log{\left(\frac{t^{% \star}}{t_{c}(s)}\right)}&\text{if }t_{c}(s)\leq t^{\star}\\ 1&\text{otherwise}\end{cases}

(54)

where $t^{\star}=3$ s is the risk-free collision threshold (i.e., the time for the vehicle to stop, maneuver around, or regain its control). The time cost is computed from the lengths and velocities of path segments.

Then, a joint cost function is formulated considering both risk and time, which is defined as:

J(\gamma)=\int_{\gamma}\big{(}\mathscr{R}(s)\big{)}^{\lambda}\cdot\frac{1}{v(s% )}ds

(55)

where $\lambda=2$ is a user-defined risk-weight. Finally, the A* algorithm searches in a high-dimensional discrete configuration space considering vehicle poses to obtain the overall time-risk-optimal path. We refer the reader to [24] for further details.

References

[1] L. 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, vol. 79, no. 3, pp. 497–516, 1957.
[2] A. M. Shkel and V. Lumelsky, “Classification of the Dubins set,” Robotics and Autonomous Systems, vol. 34, no. 4, pp. 179 – 202, 2001.
[3] J. Reeds and L. Shepp, “Optimal paths for a car that goes both forwards and backwards,” Pacific Journal of Mathematics, vol. 145, no. 2, pp. 367–393, 1990.
[4] P. Salaris, D. Fontanelli, L. Pallottino, and A. Bicchi, “Shortest paths for a robot with nonholonomic and field-of-view constraints,” IEEE Transactions on Robotics, vol. 26, no. 2, pp. 269–281, 2010.
[5] K. Mittal, J. Song, S. Gupta, and T. A. Wettergren, “Rapid path planning for Dubins vehicles under environmental currents,” Robotics and Autonomous Systems, vol. 134, p. 103646, 2020.
[6] T. G. McGee and J. K. Hedrick, “Optimal path planning with a kinematic airplane model,” Journal of Guidance, Control, and Dynamics, vol. 30, no. 2, pp. 629–633, 2007.
[7] I. S. Dolinskaya and A. Maggiar, “Time-optimal trajectories with bounded curvature in anisotropic media,” The International Journal of Robotics Research, vol. 31, no. 14, pp. 1761–1793, 2012.
[8] A. Wolek and C. Woolsey, “Feasible Dubins paths in presence of unknown, unsteady velocity disturbances,” Journal of Guidance, Control, and Dynamics, vol. 38, no. 4, pp. 782–787, 2015.
[9] S. G. Loizou and K. J. Kyriakopoulos, “Navigation of multiple kinematically constrained robots,” IEEE Transactions on Robotics, vol. 24, no. 1, pp. 221–231, 2008.
[10] J. Song and S. Gupta, “CARE: Cooperative autonomy for resilience and efficiency of robot teams for complete coverage in unknown environments under robot failures,” Autonomous Robots, vol. 44, no. 3, pp. 647–671, 2020.
[11] P. Agarwal, T. Biedl, S. Lazard, S. Robbins, S. Suri, and S. Whitesides, “Curvature-constrained shortest paths in a convex polygon,” SIAM Journal on Computing, vol. 31, no. 6, pp. 1814–1851, 2002.
[12] E. Bakolas and P. Tsiotras, “Optimal synthesis of the Zermelo–Markov–Dubins problem in a constant drift field,” Journal of Optimization Theory and Applications, vol. 156, no. 2, pp. 469–492, 2013.
[13] Y. Meyer, P. Isaiah, and T. Shima, “On Dubins paths to intercept a moving target,” Automatica, vol. 53, pp. 256–263, 2015.
[14] Y. Ding, B. Xin, and J. Chen, “Curvature-constrained path elongation with expected length for dubins vehicle,” Automatica, vol. 108, p. 108495, 2019.
[15] K. Savla, E. Frazzoli, and F. Bullo, “Traveling salesperson problems for the Dubins vehicle,” IEEE Transactions on Automatic Control, vol. 53, no. 6, pp. 1378–1391, 2008.
[16] F. Bullo, E. Frazzoli, M. Pavone, K. Savla, and S. L. Smith, “Dynamic vehicle routing for robotic systems,” Proceedings of the IEEE, vol. 99, no. 9, pp. 1482–1504, 2011.
[17] J. Ny, E. Feron, and E. Frazzoli, “On the Dubins traveling salesman problem,” IEEE Transactions on Automatic Control, vol. 57, no. 1, pp. 265–270, 2012.
[18] I. Cohen, C. Epstein, and T. Shima, “On the discretized Dubins traveling salesman problem,” IISE Transactions, vol. 49, no. 2, pp. 238–254, 2017.
[19] P. Váňa, J. Sláma, and J. Faigl, “Surveillance planning with safe emergency landing guarantee for fixed-wing aircraft,” Robotics and Autonomous Systems, vol. 133, p. 103644, 2020.
[20] R. Pěnička, J. Faigl, P. Váňa, and M. Saska, “Dubins orienteering problem,” IEEE Robotics and Automation Letters, vol. 2, no. 2, pp. 1210–1217, 2017.
[21] N. Tsiogkas and D. M. Lane, “DCOP: Dubins correlated orienteering problem optimizing sensing missions of a nonholonomic vehicle under budget constraints,” IEEE Robotics and Automation Letters, vol. 3, no. 4, pp. 2926–2933, 2018.
[22] Z. Shen, J. P. Wilson, and S. Gupta, “An online coverage path planning algorithm for curvature-constrained AUVs,” in Proc. OCEANS’19 MTS/IEEE, SEATTLE, WA, USA, Oct. 2019, pp. 1–5.
[23] J. Song and S. Gupta, “ $\epsilon^{\star}$ : An online coverage path planning algorithm,” IEEE Trans. Robot., vol. 34, pp. 526–533, 2018.
[24] J. Song, S. Gupta, and T. A. Wettergren, “T ${}^{\star}$ : Time-optimal risk-aware motion planning for curvature-constrained vehicles,” IEEE Robotics and Automation Letters, vol. 4, no. 1, pp. 33–40, Jan 2019.
[25] A. Wolek, E. Cliff, and C. Woolsey, “Time-optimal path planning for a kinematic car with variable speed,” Journal of Guidance, Control, and Dynamics, vol. 39, no. 10, pp. 2374–2390, 2016.
[26] K. Kučerová, P. Váň, and J. Faigl, “On finding time-efficient trajectories for fixed-wing aircraft using Dubins paths with multiple radii,” in Proceedings of the 35th Annual ACM Symposium on Applied Computing. New York, NY, USA: Association for Computing Machinery, 2020, pp. 829–831.
[27] K. Kučerová, P. Váňa, and J. Faigl, “Variable-speed traveling salesman problem for vehicles with curvature constrained trajectories,” in 2021 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2021, pp. 4714–4719.
[28] B. Donald, P. Xavier, J. Canny, and J. Reif, “Kinodynamic motion planning,” J. ACM, vol. 40, no. 5, pp. 1048–1066, Nov. 1993. [Online]. Available: https://doi.org/10.1145/174147.174150
[29] J.-D. Boissonnat and S. Lazard, “A polynomial-time algorithm for computing shortest paths of bounded curvature amidst moderate obstacles,” International Journal of Computational Geometry & Applications, vol. 13, no. 03, pp. 189–229, 2003.
[30] M. Elbanhawi and M. Simic, “Sampling-based robot motion planning: A review,” IEEE Access, vol. 2, pp. 56–77, 2014.
[31] A. Lekkas, A. R. Dahl, M. Breivik, and T. I. Fossen, “Continuous-curvature path generation using Fermat’s spiral,” Modeling, Identification and Control, vol. 34, no. 4, pp. 183–198, 2013.
[32] T. Fraichard and A. Scheuer, “From Reeds and Shepp’s to continuous-curvature paths,” IEEE Transactions on Robotics, vol. 20, no. 6, pp. 1025–1035, Dec 2004.
[33] H. Bruyninckx and D. Reynaerts, “Path planning for mobile and hyper-redundant robots using Pythagorean hodograph curves,” in Proceedings of 8th International Conference on Advanced Robotics., Jul 1997, pp. 595–600.
[34] Zhihua Qu, **g Wang, and C. E. Plaisted, “A new analytical solution to mobile robot trajectory generation in the presence of moving obstacles,” IEEE Transactions on Robotics, vol. 20, no. 6, pp. 978–993, Dec 2004.
[35] J. Faigl and P. Váňa, “Surveillance planning with Bézier curves,” IEEE Robotics and Automation Letters, vol. 3, no. 2, pp. 750–757, 2018.
[36] A. A. Pereira, J. Binney, G. A. Hollinger, and G. S. Sukhatme, “Risk-aware path planning for autonomous underwater vehicles using predictive ocean models,” Journal of Field Robotics, vol. 30, no. 5, pp. 741–762, 2013.
[37] J. D. Hernández, M. Moll, E. Vidal, M. Carreras, and L. E. Kavraki, “Planning feasible and safe paths online for autonomous underwater vehicles in unknown environments,” in IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Oct 2016, pp. 1313–1320.
[38] D. Liu, M. Cong, and Y. Du, “Episodic memory-based robotic planning under uncertainty,” IEEE Transactions on Industrial Electronics, vol. 64, no. 2, pp. 1762–1772, Feb 2017.
[39] A. Marchidan and E. Bakolas, “A local reactive steering law for 2D collision avoidance with curvature constraints and constant speed,” Robotics and Autonomous Systems, vol. 155, p. 104156, 2022.
[40] Z. Shen, J. P. Wilson, S. Gupta, and R. Harvey, “SMART: Self-morphing adaptive replanning tree,” IEEE Robotics and Automation Letters, vol. 8, no. 11, pp. 7312–7319, 2023.
[41] Z. Shen, J. Song, K. Mittal, and S. Gupta, “CT-CPP: Coverage path planning for 3D terrain reconstruction using dynamic coverage trees,” IEEE Robotics and Automation Letters, vol. 7, no. 1, pp. 135–142, 2022.
[42] J. P. Wilson, K. Mittal, and S. Gupta, “Novel motion models for time-optimal risk-aware motion planning for variable-speed auvs,” in OCEANS 2019 MTS/IEEE SEATTLE, 2019, pp. 1–5.
[43] D. E. Knuth, The art of computer programming. Pearson Education, 1997, vol. 3.
[44] A. Wächter and L. T. Biegler, “On the implementation of an interior-point filter line-search algorithm for large-scale nonlinear programming,” Mathematical Programming, vol. 106, no. 1, pp. 25–57, Mar 2006.
[45] S. Karaman and E. Frazzoli, “Sampling-based algorithms for optimal motion planning,” The International Journal of Robotics Research, vol. 30, no. 7, pp. 846–894, 2011.
[46] J. Yang, J. P. Wilson, and S. Gupta, “DARE: Diver action recognition encoder for underwater human–robot interaction,” IEEE Access, vol. 11, pp. 76 926–76 940, 2023.
[47] J. Z. Hare, J. Song, S. Gupta, and T. A. Wettergren, “POSE.R: Prediction-based opportunistic sensing for resilient and efficient sensor networks,” ACM Trans. Sen. Netw., vol. 17, no. 1, nov 2020.
[48] S. LaValle, Planning algorithms. Cambridge University Press, 2006.

Abstract

Index Terms:

I Introduction

I-A Literature Review

I-B Motivation

I-C Contributions

I-D Organization

II GMDM

II-A Vehicle Description

II-B Motion Primitives

II-C Motion Model

II-D Model Analysis

II-D1 The forward problem analysis

Definition II.1 (Forward problem).

Proposition II.1 (CSC forward).

Proof.

Proposition II.2 (CCC forward).

Proof.

II-D2 The inverse problem analysis

Definition II.2 (Inverse problem).

Proposition II.3 (CSC inverse).

Proof.

Proposition II.4 (CCC inverse).

Proof.

Corollary II.1.

Proof.

III Reachability Analysis

Definition III.1 (Reachability).

Theorem 1 (CSC Reachability).

Proof.

Remark III.1.

Corollary III.1.

Proof.

Theorem 2 (CCC Reachability).

Proof.

Remark III.2.

Corollary III.2.

Proof.

Theorem 3 (Full Reachability).

Proof.

Remark III.3.

IV Results and Discussion

IV-A Discussion of the Baseline Models and GMDM

Definition IV.1.

Remark IV.1.

IV-B Time-Optimal Planning

IV-B1 Visualization of time-optimized paths

IV-B2 Traveling Salesman Problem

IV-B3 Time-optimal planning in obstacle-rich environments

IV-C Time-risk optimal planning

IV-C1 Motivation for time-risk optimal planning

IV-C2 Comparative evaluation results

V Conclusions and Future Work

-A Solution of the Forward Problem

-A1 Proof of Proposition II.1

Proof.

-A2 Proof of Proposition II.2

Proof.

-B Solution of the Inverse Problem

-B1 Proof of Proposition II.3

Proof.

-B2 Proof of Proposition II.4

Proof.

-C Reachability Proofs

-C1 Proof of Theorem 1

Proof.

-C2 Proof of Theorem 2

Proof.

-C3 Proof of Theorem 3

Proof.

-D Review of T⋆normal-⋆{}^{\star}start_FLOATSUPERSCRIPT ⋆ end_FLOATSUPERSCRIPT for Time-Risk Path Planning

References

-D Review of T ${}^{\star}$ for Time-Risk Path Planning