A Non-Smooth Numerical Optimization Approach to the Three-Point Dubins Problem (3PDP)

Abstract

This paper introduces a novel non-smooth numerical optimization approach for solving the Three-Point Dubins Problem (3PDP). The 3PDP requires determining the shortest path of bounded curvature that connects given initial and final positions and orientations while traversing a specified waypoint. The inherent discontinuity of this problem precludes the use of conventional optimization algorithms. We propose two innovative methods specifically designed to address this challenge. These methods not only effectively solve the 3PDP but also offer significant computational efficiency improvements over existing state-of-the-art techniques. Our contributions include the formulation of these new algorithms, a detailed analysis of their theoretical foundations, and their implementation. Additionally, we provide a thorough comparison with current leading approaches, demonstrating the superior performance of our methods in terms of accuracy and computational speed. This work advances the field of path planning in robotics, providing practical solutions for applications requiring efficient and precise motion planning.

1. Introduction

One of the central problems in robotics is motion, or path planning, which accounts for computing a feasible path (possibly an optimal one, for some design criterion) for a robot moving from a starting configuration to a final one. A configuration can be just the position, but can also contain additional information like the orientation or the curvature. A feasible path can be required to pass along fixed or moving obstacles or to follow internal waypoints [1,2,3]. Other physical limitations that restrict the path’s shape are differential constraints, called nonholonomic, which characterize a vehicle’s trajectories. An important one is the requirement of bounded curvature, which asks that the path does not have excessive sharp turns because the robot can steer up to a minimum turning radius ${\rho }_{min}$, which is defined as the inverse of the maximum curvature of the path, ${\kappa }_{max}={\displaystyle \frac{1}{{\rho }_{min}}}$. As an optimality criterion, minimizing the path length is very common. Another common assumption is that the vehicle travels at a constant speed; thus, the shortest path is equivalent to the other important optimization functional, the minimum time. However, this can be extended and completed by combining motion primitives [4,5,6].

The mathematical model containing all these design requirements is the Markov–Dubins problem, which is defined formally as follows. A Markov–Dubins path (MDP) [7] between two assigned points in ${\mathbb{R}}^2$, ${\mathbf{P}}_{\mathbf{i}}=(x_i,y_i)$ and ${\mathbf{P}}_{\mathbf{f}}=(x_f,y_f)$, with angles ${\vartheta }_i$ and ${\vartheta }_f$ is the $C^1$ and piecewise $C^2$ path of minimum length $\gamma :[0,L]\to {\mathbb{R}}^2$ such that the absolute value of the curvature of the path $\gamma$, at almost every point, is not greater than ${\kappa }_{max}>0$; see [8]. The problem, originally formulated by Markov in 1889 was solved only in 1957 by Dubins [9], who proved that there are six extremal solutions to the associated optimal control problem:

Minimize ${\displaystyle L={\int }_0^{L}1\mathrm{d}\ell }$ subject to $\left|u(\ell )\right|\le {\kappa }_{max}$,

$$\begin{array}{ccccccccccc}\hfill x^{\prime }(\ell )& =& cos\left(\theta \right(\ell \left)\right),\hfill & & \hfill y^{\prime }(\ell )& =& sin\left(\theta \right(\ell \left)\right),\hfill & & \hfill {\theta }^{\prime }(\ell )& =& u(\ell ),\hfill \\ \hfill x\left(0\right)& =& x_i,\hfill & & \hfill y\left(0\right)& =& y_i,\hfill & & \hfill \theta \left(0\right)& =& {\vartheta }_i,\hfill \\ \hfill x\left(L\right)& =& x_f,\hfill & & \hfill y\left(L\right)& =& y_f,\hfill & & \hfill \theta \left(L\right)& =& {\vartheta }_f.\hfill \end{array}$$

(1)

Each extremal is composed of at most three internal arcs. This sequence of curves can be written as words: $LSR$, $RLR$, $LSL$, etc., where L, S, and R represent a left turn, straight segment, and right turn, respectively. A more compact notation is obtained by writing C for an arc of a circle and S for a straight line segment. Not all possible combinations of curves are optimal; it has been proved that the optimal words are only [8,10] $CSC,CS,SC,S,CCC,CC,$ and C. Thus, there are 15 possible combinations: $LSL,LSR,RSL,RSR,RLR,LRL$, $LS,RS,SL,SR,S$, $LR,RL,L,R$.

In practice, only the first six words are considered; the others can be obtained by setting some of their arcs to zero. However, there is no way to know in advance which of the six words is the shortest, and usually, the solution is found through trial-and-error (or brute force) enumeration of all six. A recent method to avoid this trial-and-error procedure employs Machine Learning to train an AI to determine the optimal solution without the need to enumerate all candidate solutions [11].

The Multipoint Markov–Dubins Problem (MPMDP) generalizes the classic problem to a sequence of assigned planar points ${\mathbf{P}}_0,\dots ,{\mathbf{P}}_n$ using interpolation with Dubins paths [12,13]. The initial and final angles, ${\vartheta }_0$ and ${\vartheta }_n$, are assigned, while the other intermediate angles ${\vartheta }_j$, for $j=1,\dots ,n-1$, are free and thus are unknowns of the problem. Even for a few points, the brute force method, i.e., trying all cases for each pair of consecutive points, becomes rapidly infeasible due to the exponential growth of possible test cases. Recent solutions to the multipoint problem are based on a Non-Linear Programming approach (NLP) [12,13] or Mixed-Integer Non-Linear Programming (MINLP) [12]. The most effective technique is actually the iDPP method based on the Iterative Dynamic Programming Principle [12] and is suitable for parallel computing architectures on CPUs as well as on GPUs [14].

A special case of the MPMDP requires finding an obstacle-free path through three points, which is a problem with name and importance: 3PDP—Three-Point Dubins Problem. Despite being a subcase of the solved MPMDP case, the 3PDP has received special attention in the last decade because of its application as a subroutine in a high-level planner for the Dubins Traveling Salesman Problem (DTSP) [15,16,17]. This subroutine is used to insert a new point in an existing path; therefore, the initial, the new middle, and the final point are specified, as well as the initial and final angles. Thus, the 3PDP reduces to the one-dimensional problem of finding the angle corresponding to the central point, so that the curvature constraint is satisfied, and the minimum length is preserved. To solve this non-smooth minimization problem, several methods can be employed [18,19,20].

The crucial point of interest in this special instance of the MPMDP is the computational time: since it is called repeatedly as a subroutine of the DTSP, the solutions based on brute force, Dynamic Programming, or NLP/MINLP are considered too inefficient because of the considerable overhead. Therefore, a solution to this problem has been sought, as detailed in the next section, aiming at computational efficiency.

1.1. Problem Formulation

The 3PDP is formulated as follows: given an initial point ${\mathbf{P}}_{\mathbf{i}}=(x_i,y_i)$ with orientation ${\vartheta }_i$, a middle point ${\mathbf{P}}_{\mathbf{m}}=(x_m,y_m)$, and a final point ${\mathbf{P}}_{\mathbf{f}}=(x_f,y_f)$ with orientation ${\vartheta }_f$, find the shortest concatenation of two Dubins paths that begin in ${\mathbf{P}}_{\mathbf{i}}$, pass through ${\mathbf{P}}_{\mathbf{m}}$, and reach ${\mathbf{P}}_{\mathbf{f}}$ while respecting the constraints of having the maximum absolute curvature not exceeding ${\kappa }_{max}$; see Figure 1. The problem is clearly one-dimensional, with the angle ${\vartheta }_m$ at ${\mathbf{P}}_{\mathbf{m}}$ being the only unknown of the problem. Once ${\vartheta }_m$ is determined, the solution becomes trivial, since it is just twice the application of the standard Dubins problem.

Despite being formally simple, the 3PDP is quite involved: the function of the total length to be minimized is, indeed, not continuous and thus non-smooth. Therefore, classic optimization methods cannot be applied, and different methods are required [19,21].

1.2. Paper Contribution and Layout

Our research presents an efficient and compact solution to the Three-Point Dubins Problem (3PDP), addressing the non-smooth minimization challenge. Our key contributions are as follows:

• Analytical Derivative Computation: We formulated a compact, closed-form solution for the Markov–Dubins problem. With this formulation, we significantly reduce computational overhead and improve the efficiency. This is instrumental to finding the analytical expressions for the derivatives of the Dubins path length with respect to the initial and final angles. These expressions allow for a more precise and faster computation of path lengths and gradients, enabling a better optimization performance. Additionally, we established bounds for the existence regions of each of the six possible solutions. With this analysis, we can isolate discontinuities and provide a more accurate and efficient solution to the 3PDP.
• Optimal Path Length and Derivative Analysis: We computed the length and derivatives of the optimal 3PDP path, precisely framing it as a non-smooth minimization problem.
• Introduction of Two Non-smooth Minimization Methods: We propose two specialized non-smooth minimization algorithms tailored for the 3PDP:

  –

  A pattern search algorithm, which can be further enhanced with Algorithm 748 [22].

  –

  A trichotomy algorithm, similarly augmentable with Algorithm 748.

  Algorithm 748 can be used to perform the root-finding of the first derivatives of the path length, improving the efficiency and accuracy of the solution up to machine precision.
• Implementation and Accessibility: We provided a comprehensive implementation of our algorithms in a C++ library with a Matlab interface, named Clothoids [23,24]. The library, along with clear descriptions and pseudocode of the algorithms, is freely available and open-source, facilitating further research and practical applications, bridging the gap between theoretical research and practitioners, ensuring that our work can be readily used and extended by others.

This paper is structured as follows: Section 2 gives insight into the 3PDP and presents state-of-the-art methods on the subject. Section 3 develops the solution to the standard Dubins problem and, in particular, the length of the solution as a function of the givens of the problem. Our novel derivation allows us to compute the derivatives of the lengths of the various cases, as shown in Section 4. In Section 5, we detail our solution of the 3PDP, comprising all cases. The corresponding algorithms are discussed in Section 6 and tested in Section 7. Finally, we present our conclusions in Section 8.

2. Background and State-of-the-Art Methods

Several results approximate the solution of the MPMDP under the hypothesis that the distance between consecutive points is at least $d^{★}:=4{\rho }_{min}=4/{\kappa }_{max}$, a condition that implies that only CSC paths will constitute the solution, so that cases CCC are not considered. Under this hypothesis, in defining the angles at the waypoints as the angles of the line segment connecting the adjacent points in the sequence, the approximation yields a path that is not more than 1.91 times longer than the optimal one. Without the $d>d^{★}$ distance assumption, there is a method to construct a path that is at most 5.03 times longer than the optimal, a factor which decreases to 2.58 if the distance between the points is more than $d^{★}$ [25,26].

Apart from the bounds, some exact results can be derived and computed in different ways, covering some or all the possible feasible paths.

The first exact result in the problem of visiting an ordered sequence of waypoints with a Dubins path is given in [25,27,28], under the assumption that the distance between consecutive points is at least $d^{★}$, thus considering the CSC cases only. It is shown that the problem becomes locally convex over an open region; however, this region is not even connected, and therefore, $2^{n-2}$ subproblems must be solved, each containing a local minimum of the original problem. In their analysis, the authors reduce the complexity to $2^k$, with k being the number of sharp turns of the path.

A different approach, based on inversive geometry [26], proposes an iterative method (IM) to solve the 3PDP with a $3\times 3$ non-linear system of equations to find the optimal angle at the middle point for paths CSC-CSC (or shorter words). This method that combines geometric arguments with the bisection algorithm is compared with the brute force method of discretizing the middle angle with 360 samples and testing all cases (called the discretization-based method—DBM [29,30]), showing a computational time improvement ranging from a factor of about 14 in the best case, for points that have a distance greater than $d^{★}$, to a worse case improvement of a factor of 5 for points that are closer than $d^{★}$. Regarding accuracy, the iterative method in [26] is generally more precise than the brute force DBM. However, it does not cover the cases where the path contains a CCC word.

An approach that covers all cases and is based on algebraic methods is the one proposed in [31], where the trigonometric relations that characterize the solution are transformed into polynomials of relatively high degree, from 4 to 20. In the paper, it is discussed that although, in principle, there are $6\times 6$ potential path types (4 CSCs and 2 CCCs for each pair of points), of these 36 cases, only 18 are optimal candidates; see

Table 1. The types of the 18 admissible paths; the subscript indicates the degree of the corresponding polynomial according to [31]. For instance, RSLRL₂₀ means that the path is of type CSCCC and that the resulting polynomial has degree 20.

CCCCC	CCCSC	CSCCC	CSCSC
RLRLR6	RLRSR8	RSRLR8	RSRSR4
LRLRL6	RLRSL20	LSRLR20	LSRSR8
	LRLSL8	RSLRL20	RSRSL8
	LRLSR8	LSLRL8	LSRSL8
			LSLSL4
			RSLSL8
			LSLSR8
			RSLSR8

.

The algorithm requires constructing and finding the (real) roots of these 18 polynomials and, for each admissible root, testing the length of the path. The angle corresponding to the shortest length is elected as the optimal angle at the midpoint. This method is called the polynomial-based method, PBM, and requires, in the worst case, the computation of $2\times 4+2\times 6+10\times 8+4\times 20=180$ pairs of Dubins paths, plus the time to find up to their 180 roots. Depending on the distance of the points, this method achieves a speed-up ranging from 25 to 45 times the speed of the DBM with 360 samples [31].

A recent method [28] is based on geometrical observations and solves the case for points having a distance larger than $d^{★}$; thus, the cases are CSC (but not CCC). It takes the name of the geometry-based method or GBM. This method is purely geometrical and bases the solution on the construction of a special ellipse. The solution is computed by building circles of radius ${\rho }_{min}$ with tangents on the initial and final points (four possible circles). Then, an ellipse is defined by its foci laying in the previously built circle centers. The ellipse must be tangent to the circle of radius ${\rho }_{min}$ centered in the middle point.

The optimal angle is given by the angle of the common tangent to the ellipse and the circle centered at the middle point, and a semi-analytical solution to the problem is provided. The computational improvement over the DBM is of a factor of 150, and it can find a shorter solution in about $92\%$ of the instances: if the path contains a CSC word, the solution will be exact and thus better than the DBM, but not when the path contains a CCC.

3. Derivation of Dubins Path with Trigonometry

This section is devoted to deriving the solution of the classic Dubins problem to obtain analytic relations for the optimal lengths so that the next section can compute their derivatives.

Without loss of generality, the original problem (1) can be transformed into a simpler one by translating, rotating, and scaling the original configuration such that $x_i=y_i=0$, and $x_f=d$, $y_f=0$ with a unitary maximum curvature of ${\kappa }_{max}=1$. Let d be the Euclidean distance between the initial and final points scaled by the maximum curvature:

$$d=\sqrt{{(x_f-x_i)}^2+{(y_f-y_i)}^2}/{\kappa }_{max}.$$

(2)

Then, the aforementioned transform is the following, (3): the translation is given by $-(x_i,y_i)$ and is followed by a rotation around $(0,0)$ by an angle $-\varphi$ given by $\varphi =\mathrm{atan}2(y_f-y_i,x_f-x_i)$, followed by a scaling of ${\kappa }_{max}^{-1}={\rho }_{min}$, or compactly,

$$\left[\begin{array}{c}x\\ y\end{array}\right]={\displaystyle \frac{1}{{\kappa }_{max}}}\left[\begin{array}{cc}cos\varphi & sin\varphi \\ -sin\varphi & cos\varphi \end{array}\right]\left[\begin{array}{c}x-x_i\\ y-y_i\end{array}\right].$$

(3)

Let $\alpha ={\vartheta }_i+\varphi$ and $\beta ={\vartheta }_f+\varphi$ be the initial and final angles in the transformed coordinate system. The new initial point is thus the origin with angle $\alpha$, and the final point is on the positive $x$-axis at distance d, (2), from the origin and angle $\beta$ with maximum unitary curvature.

The solution to a Dubins problem is a sequence of three curves (lines or arcs [32]) that can be written in the following form for $j\in 0,1,2$:

$$j-\mathrm{th}\mathrm{curve}:\left\{\begin{array}{ccc}\hfill x(\ell )& =& x_j+s(sin({\vartheta }_j+s\ell )-sin{\theta }_j)+(1-s^2)\ell cos{\theta }_j,\hfill \\ \hfill y(\ell )& =& y_j+s(cos{\theta }_j-cos({\vartheta }_j+s\ell ))+(1-s^2)\ell sin{\theta }_j,\hfill \\ \hfill \theta (\ell )& =& {\vartheta }_j+s\ell ,\hfill \end{array}\right.$$

(4)

where ℓ is the curvilinear abscissa, $x(\ell )$ and $y(\ell )$ are the coordinates, and $\theta (\ell )$ is the orientation. Let us define $x_0=y_0=0$ and ${\vartheta }_0=\alpha$, corresponding to the initial point. Notice that, when the integer variable s is equal to 0, the function represents a line segment, whereas, when $s=1$, the curve is an arc of a unitary circle turning left, and, when $s=-1$, the circle is turning right.

The Dubins problem is formulated with three internal segments (CSC or CCC) with known initial and final coordinates and headings in the transformed coordinate system. Hence, we can juxtapose three of such curves. Using the notation $s_1$, $s_2$, and $s_3$ for the signs of the curvature of the three segments coming from (4), we can write the equations for the three segments:

$$\begin{array}{ccc}\hfill x_1& =& 0+s_1(sin{\vartheta }_1-sin\alpha )\hfill \\ \hfill y_1& =& 0+s_1(cos\alpha -cos{\vartheta }_1)\hfill \\ \hfill {\vartheta }_1& =& \alpha +s_1{\ell }_1\hfill \\ \hfill x_2& =& x_1+s_2(sin{\vartheta }_2-sin{\vartheta }_1)+(1-s_2^2){\ell }_{2}cos{\vartheta }_1\hfill \\ \hfill y_2& =& y_1+s_2(cos{\vartheta }_1-cos{\vartheta }_2)+(1-s_2^2){\ell }_{2}sin{\vartheta }_1\hfill \\ \hfill {\vartheta }_2& =& {\vartheta }_1+s_2{\ell }_2\hfill \\ \hfill 0& =& x_2+s_3(sin\beta -sin{\vartheta }_2)\hfill \\ \hfill d& =& y_2+s_3(cos{\vartheta }_2-cos\beta )\hfill \\ \hfill \beta & =& {\vartheta }_2+s_3{\ell }_3.\hfill \end{array}$$

(5)

The proposed transformation simplifies the equations, eliminating $x_1$, $x_2$, $y_1$, $y_2$, ${\vartheta }_1$, and ${\vartheta }_2$ from the general system (5). Moreover, we can separate two families of solutions based on the presence or absence of the middle singular arc (straight line segment), that is, the CSC and CCC cases.

Before proceeding, it is also convenient to define some notation and shorthand for the trigonometric functions of the angles $\alpha$ and $\beta$ that will be used in the following sections, to specify that the expression depends on trigonometric combinations of $\alpha$ and $\beta$:

$$\left\{\begin{array}{ccc}\hfill {\mathcal{S}}^{-}& =& sin\alpha -sin\beta \hfill \\ \hfill {\mathcal{C}}^{-}& =& cos\alpha -cos\beta \hfill \\ \hfill {\mathcal{S}}^{+}& =& sin\alpha +sin\beta \hfill \\ \hfill {\mathcal{C}}^{+}& =& cos\alpha +cos\beta \hfill \end{array}{\mathcal{P}}_{\alpha ,\beta }=\{sin\alpha ,cos\alpha ,sin\beta ,cos\beta \}\right.$$

(6)

where ${\mathcal{P}}_{\alpha ,\beta }$ denotes a set of the trigonometric functions of the angles $\alpha$ and $\beta$. We remark that ${\mathcal{S}}^{\pm }$ and ${\mathcal{C}}^{\pm }$ are functions of ${\mathcal{P}}_{\alpha ,\beta }$, but we drop the explicit dependence on ${\mathcal{P}}_{\alpha ,\beta }$ to keep the notation light.

For all three cases, we want to compute the total length of the path, which is the sum of the lengths of the three segments.

$$\ell ={\ell }_1+{\ell }_2+{\ell }_3.$$

(7)

3.1. Case CSC

This case happens when $s_2=0$ and, depending on the sign of $s_1$ and $s_3$, gives origin to the curves LSL, RSR, LSR, and RSL. Then, system (5) simplifies to

$$\begin{array}{ccc}\hfill d& =& s_1(sin{\vartheta }_1-sin\alpha )+{\ell }_{2}cos{\vartheta }_1+s_3(sin\beta -sin{\vartheta }_2)\hfill \\ \hfill 0& =& s_1(cos\alpha -cos{\vartheta }_1)+{\ell }_{2}sin{\vartheta }_1+s_3(cos{\vartheta }_2-cos\beta ),\hfill \end{array}$$

(8)

and by imposing the continuity conditions to (8) on the angle of the line segment ${\vartheta }_1={\vartheta }_2={\vartheta }_S=\alpha +s_1{\ell }_1=\beta -s_3{\ell }_3$, we have

$$\begin{array}{ccc}\hfill d& =& (s_1-s_3)sin{\vartheta }_S-s_{1}sin\alpha +s_{3}sin\beta +{\ell }_{2}cos{\vartheta }_S\hfill \\ \hfill 0& =& (s_3-s_1)cos{\vartheta }_S+s_{1}cos\alpha -s_{3}cos\beta +{\ell }_{2}sin{\vartheta }_S.\hfill \end{array}$$

(9)

We combine these two Equation in (9), multiplying by $sin{\vartheta }_S$ and $cos{\vartheta }_S$, respectively, and we obtain an explicit expression for ${\ell }_2$ and an implicit expression for ${\vartheta }_S$:

$$\begin{array}{ccc}\hfill {\ell }_2& =& (d-s_{3}sin\beta +s_{1}sin\alpha )cos{\vartheta }_S+(s_{3}cos\beta -s_{1}cos\alpha )sin{\vartheta }_S\hfill \\ \hfill 0& =& (d-s_{3}sin\beta +s_{1}sin\alpha )sin{\vartheta }_S-(s_{3}cos\beta -s_{1}cos\alpha )cos{\vartheta }_S+s_3-s_1.\hfill \end{array}$$

(10)

This case can be further separated into two subcases, the first, $CS{C}^{+}$, with the two circular arcs turning in the same direction (LSL and RSR), and the other, $CS{C}^{-}$, with circular arcs in the opposite direction (LSR and RSL). They have two different solutions, as detailed below.

3.1.1. The Subcase $CS{C}^{+}$: LSL and RSR

As stated in the previous section, this solution type falls into the ${\mathrm{CSC}}^{+}$ case, where $s_2=0$ and $s_1=s_3=s$. Hence, we can rewrite Equation (10) in terms of the parameters ${\mathcal{P}}_{\alpha ,\beta }$ using notation (6) as follows:

$$\begin{array}{ccc}\hfill (d+s{\mathcal{S}}^{-})sin{\vartheta }_S+s{\mathcal{C}}^{-}cos{\vartheta }_S& =& 0\hfill \\ \hfill (d+s{\mathcal{S}}^{-})cos{\vartheta }_S-s{\mathcal{C}}^{-}sin{\vartheta }_S& =& {\ell }_2.\hfill \end{array}$$

(11)

From the first equation, we can solve for ${\vartheta }_S$, and the second is already solved for ${\ell }_2$, as a function of ${\vartheta }_S$. Thus, we have

$${\vartheta }_S^{{\mathrm{CSC}}^{+}}=\mathrm{atan}2\left(-{\mathcal{C}}^{-},d+s{\mathcal{S}}^{-}\right)mod\pi .$$

(12)

We choose the only ${\vartheta }_S$ producing a non-negative ${\ell }_2$. Any ${\vartheta }_S$ not respecting this constraint is discarded. The length ${\ell }_2$ is obtained from (11), knowing the solution in (12), and from the condition ${\vartheta }_1={\vartheta }_2={\vartheta }_S=\alpha +s{\ell }_1=\beta -s{\ell }_3$, we can derive the other lengths ${\ell }_1$ and ${\ell }_3$, which are as follows:

$$\begin{array}{ccc}\hfill {\ell }_1^{{\mathrm{CSC}}^{+}}& =& s({\vartheta }_S^{{\mathrm{CSC}}^{+}}-\alpha )mod2\pi \hfill \\ \hfill {\ell }_2^{{\mathrm{CSC}}^{+}}& =& (d+s{\mathcal{S}}^{-})cos{\vartheta }_S^{{\mathrm{CSC}}^{+}}-s{\mathcal{C}}^{-}sin{\vartheta }_S^{{\mathrm{CSC}}^{+}}\hfill \\ \hfill {\ell }_3^{{\mathrm{CSC}}^{+}}& =& s(\beta -{\vartheta }_S^{{\mathrm{CSC}}^{+}})mod2\pi .\hfill \end{array}$$

(13)

3.1.2. The Subcase $CS{C}^{-}$: LSR and RSL

This solution type falls into the $CS{C}^{-}$ case, where $s_2=0$, $s_1=s$, and $s_3=-s$. It is possible to rewrite Equation (10) in terms of the parameters ${\mathcal{P}}_{\alpha ,\beta }$ using notation (6) as follows:

$$\begin{array}{ccc}\hfill (d+s{\mathcal{S}}^{+})sin{\vartheta }_S+s{\mathcal{C}}^{+}cos{\vartheta }_S& =& 2s\hfill \\ \hfill (d+s{\mathcal{S}}^{+})cos{\vartheta }_S-s{\mathcal{C}}^{+}sin{\vartheta }_S& =& {\ell }_2.\hfill \end{array}$$

(14)

We can substitute the expression for ${\vartheta }_S$ to obtain a polynomial equation as a function of the auxiliary variable X.

$${\vartheta }_S=\pi +2arctan\left(X\right),cos{\vartheta }_S={\displaystyle \frac{X^2-1}{X^2+1}},sin{\vartheta }_S={\displaystyle \frac{-2X}{X^2+1}}.$$

(15)

Therefore, using (15) in (14), multiplying by $(X^2+1)$, and using $s^2=1$, we can obtain a quadratic equation in X as follows:

$$\begin{array}{c}0=(2-{\mathcal{C}}^{+})X^2+2(sd+{\mathcal{S}}^{+})X+(2+{\mathcal{C}}^{+}).\end{array}$$

(16)

Hence, once the polynomial equation is solved. We can substitute the expression for ${\vartheta }_S$ in (14), derive the lengths ${\ell }_2$, and retrieve ${\ell }_1$ and ${\ell }_3$ with the following: 

$${\vartheta }_S^{{\mathrm{CSC}}^{-}}=\mathrm{atan}2(-2X,X^2-1).$$

(17)

If $|2-{\mathcal{S}}^{+}|\ll 1$, the polynomial in (16) is not well posed and ill conditioned. Therefore, we can change the coordinates to $X=1/Y$ to reshape the polynomial, yielding the following expression:

$${\vartheta }_S=\pi +2arctan(1/Y),cos{\vartheta }_S={\displaystyle \frac{1-Y^2}{Y^2+1}},sin{\vartheta }_S={\displaystyle \frac{-2Y}{Y^2+1}}.$$

(18)

From the condition ${\vartheta }_1={\vartheta }_2={\vartheta }_S=\alpha +s_1{\ell }_1=\beta -s_3{\ell }_3$ and (14), (15), or (18), we can derive the lengths ${\ell }_1$, ${\ell }_2$, and ${\ell }_3$, recalling also (17), as follows: 

$$\begin{array}{ccc}\hfill {\ell }_1^{{\mathrm{CSC}}^{-}}& =& s({\vartheta }_S^{{\mathrm{CSC}}^{-}}-\alpha )mod\left(2\pi \right)\hfill \\ \hfill {\ell }_2^{{\mathrm{CSC}}^{-}}& =& (d+s{\mathcal{S}}^{+})cos{\vartheta }_S^{{\mathrm{CSC}}^{-}}-s{\mathcal{C}}^{+}sin{\vartheta }_S^{{\mathrm{CSC}}^{-}}\hfill \\ \hfill {\ell }_3^{{\mathrm{CSC}}^{-}}& =& s({\vartheta }_S^{{\mathrm{CSC}}^{-}}-\beta )mod\left(2\pi \right).\hfill \end{array}$$

(19)

3.1.3. Range of the Solution

The solution exists only for angles coming from real roots of polynomial (16). The discriminant ${\mathcal{D}}_{{\mathrm{CSC}}^{+}}$ of that polynomial is

$$\begin{array}{ccc}\hfill {\displaystyle \frac{1}{4}}{\mathcal{D}}_{{\mathrm{CSC}}^{+}}& =& {(sd+{\mathcal{S}}^{+})}^2-(2-{\mathcal{C}}^{+})(2+{\mathcal{C}}^{+})\hfill \\ \hfill & =& 2\left(sd(sin\alpha +sin\beta )+sin\alpha sin\beta +cos\alpha cos\beta \right)+d^2-2,\hfill \end{array}$$

(20)

In introducing new variables Z and W, defined by the relations $\alpha =2arctanZ$ and $\beta =2arctanW$, it is possible to convert the trigonometric expressions into polynomials. Substituting each of these relations into the discriminant in (20), we obtain two polynomials:

$$\begin{array}{ccc}\hfill P_{{\mathrm{CSC}}^{+}}(Z;\beta )& =& (sdsin\beta -cos\beta +t)Z^2+2(sd+sin\beta )Z+sdsin\beta +cos\beta +t,\hfill \\ \hfill P_{{\mathrm{CSC}}^{+}}(W;\alpha )& =& (sdsin\alpha -cos\alpha +t)W^2+2(sd+sin\alpha )W+sdsin\alpha +cos\alpha +t,\hfill \end{array}$$

(21)

where $t={\displaystyle \frac{d^2}{2}}-1$. The real roots of $P_{{\mathrm{CSC}}^{+}}(Z;\beta )$ represent the border of the range interval where $\alpha$ can run, to obtain a solution for the $CS{C}^{+}$ problem for a fixed $\beta$. The same holds for $P_{{\mathrm{CSC}}^{+}}(W;\alpha )$ for the interval range of $\beta$.

The range detection is fundamental in the solution to the 3PDP. In general, the length of the 3PDP is a discontinuous function of the middle angle (see Figure 2). Standard direct search [33,34] and non-smooth minimization algorithms [35,36] can fail to find the solution if evaluation points are located in a small discontinuity region (see Figure 3). Hence, our proposed approach, discussed in the later sections, employs the solution of polynomial (21) to isolate the discontinuity points and treat them separately in the minimization algorithm.

The range detection is fundamental to finding the points that are candidate to be points of discontinuity of the length of the Dubins path with respect to initial or final angles $\alpha$ and $\beta$.

3.2. Case $CCC$

The $\mathrm{CCC}$ case, happening when $s_2\ne 0$, comprises two paths: LRL and RLR. In this situation, we can impose $s_1=s_3=-s_2=s$ and simplify (5) to obtain

$$\begin{array}{ccc}\hfill sd& =& (sin{\vartheta }_1-sin\alpha )-(sin{\vartheta }_2-sin{\vartheta }_1)+(sin\beta -sin{\vartheta }_2)\hfill \\ \hfill 0& =& (cos\alpha -cos{\vartheta }_1)-(cos{\vartheta }_1-cos{\vartheta }_2)+(cos{\vartheta }_2-cos\beta ).\hfill \end{array}$$

(22)

Equation (22) can be rewritten in compact form as

$$\begin{array}{ccc}\hfill sd& =& 2(sin{\vartheta }_1-sin{\vartheta }_2)+sin\beta -sin\alpha \hfill \\ \hfill 0& =& 2(cos{\vartheta }_2-cos{\vartheta }_1)+cos\alpha -cos\beta \hfill \\ \hfill \beta & =& \alpha +s({\ell }_1-{\ell }_2+{\ell }_3).\hfill \end{array}$$

(23)

We can state Equation (23) in terms of the parameters ${\mathcal{P}}_{\alpha ,\beta }$ using notation (6) as

$$\begin{array}{ccc}\hfill sd& =& 2(sin{\vartheta }_1-sin{\vartheta }_2)-{\mathcal{S}}^{-}\hfill \\ \hfill 0& =& 2(cos{\vartheta }_2-cos{\vartheta }_1)+{\mathcal{C}}^{-}.\hfill \end{array}$$

(24)

From (24) and squaring, we obtain

$$\begin{array}{ccc}\hfill {\displaystyle \frac{1}{4}}{(sd+{\mathcal{S}}^{-})}^2& =& {(sin{\vartheta }_1-sin{\vartheta }_2)}^2=sin{\vartheta }_1^2+sin{\vartheta }_2^2-2sin{\vartheta }_{1}sin{\vartheta }_2\hfill \\ \hfill {\displaystyle \frac{1}{4}}{\left({\mathcal{C}}^{-}\right)}^2& =& {(cos{\vartheta }_2-cos{\vartheta }_1)}^2=cos{\vartheta }_1^2+cos{\vartheta }_2^2-2cos{\vartheta }_{1}sin{\vartheta }_2\hfill \end{array}$$

(25)

and by adding the two equalities (25) (and dividing by 2), we obtain

$$\begin{array}{ccc}\hfill {\displaystyle \frac{{(sd+{\mathcal{S}}^{-})}^2+{\left({\mathcal{C}}^{-}\right)}^2}{8}}& =& 1-sin{\vartheta }_{1}sin{\vartheta }_2-cos{\vartheta }_{1}sin{\vartheta }_2\hfill \\ \hfill & =& 1-cos({\vartheta }_2-{\vartheta }_1)=1-cos{\ell }_2,\hfill \end{array}$$

(26)

where the last step follows inserting the sixth equation of (5), that is, ${\vartheta }_2-{\vartheta }_1=s_2{\ell }_2$, and noting that cosine is even. Thus, ${\ell }_2$ can be obtained from (26) as follows:

$$\begin{array}{ccc}\hfill t^{\mathrm{CCC}}& =& {\displaystyle \frac{3+cos(\alpha -\beta )+sd(sin\beta -sin\alpha )}{4}}-{\displaystyle \frac{d^2}{8}},\hfill \\ \hfill cos{\ell }_2& =& 1-{\displaystyle \frac{{(sd+{\mathcal{S}}^{-})}^2+{\left({\mathcal{C}}^{-}\right)}^2}{8}}=t^{\mathrm{CCC}},\hfill \\ \hfill {\ell }_2& =& arccos\left(t^{\mathrm{CCC}}\right),\mathrm{or}{\ell }_2=2\pi -arccos\left(t^{\mathrm{CCC}}\right).\hfill \end{array}$$

(27)

The solution exists when the argument of arccos is in the interval $[-1,1]$. Therefore, the $t^{CCC}$ solved in (27) must be comprised between $-1$ and 1, yielding the following inequality:

$$-1\le {\displaystyle \frac{3+cos(\alpha -\beta )+sd(sin\beta -sin\alpha )}{4}}-{\displaystyle \frac{d^2}{8}}\le 1.$$

(28)

The previous expression (28) can be multiplied by 4 and moved $-d^2/2+3$ to the left and right side:

$${\displaystyle \frac{d^2}{2}}-7\le cos(\alpha -\beta )+sd(sin\beta -sin\alpha )\le {\displaystyle \frac{d^2}{2}}+1.$$

(29)

We can obtain the other relations from the trigonometric relations furnished by (24). From ${\vartheta }_1=\alpha +s{\ell }_1$ and ${\vartheta }_2=\alpha +s({\ell }_1-{\ell }_2)$ and expanding the trigonometric functional, we obtain a linear system in $cos{\ell }_1$ and $sin{\ell }_1$, and from ${\vartheta }_1=\beta +s({\ell }_2-{\ell }_3)$ and ${\vartheta }_2=\beta -s{\ell }_3$ and expanding the trigonometric functions again, we obtain a linear system in $cos{\ell }_3$ and $sin{\ell }_3$ as follows:

$$\begin{array}{ccc}\hfill \left[\begin{array}{cc}sin\alpha -sin\tilde{\alpha }& s(cos\alpha -cos\tilde{\alpha })\\ cos\alpha -cos\tilde{\alpha }& s(sin\tilde{\alpha }-sin\alpha )\end{array}\right]\left[\begin{array}{c}cos{\ell }_1\\ sin{\ell }_1\end{array}\right]& =& {\displaystyle \frac{1}{2}}\left[\begin{array}{c}sd+{\mathcal{S}}^{-}\\ {\mathcal{C}}^{-}\end{array}\right],\hfill \\ \hfill \left[\begin{array}{cc}sin\tilde{\beta }-sin\beta & s(cos\beta -cos\tilde{\beta })\\ cos\tilde{\beta }-cos\beta & s(sin\tilde{\beta }-sin\beta )\end{array}\right]\left[\begin{array}{c}cos{\ell }_3\\ sin{\ell }_3\end{array}\right]& =& {\displaystyle \frac{1}{2}}\left[\begin{array}{c}sd+{\mathcal{S}}^{-}\\ {\mathcal{C}}^{-}\end{array}\right],\hfill \end{array}$$

(30)

where $\tilde{\alpha }=\alpha -s{\ell }_2$ and $\tilde{\beta }=\beta +s{\ell }_2$. In conclusion, we can summarize the results for the $\mathrm{CCC}$ case as follows:

Evaluate

$$t^{\mathrm{CCC}}={\displaystyle \frac{3+cos(\alpha -\beta )-sd{\mathcal{S}}^{-}}{4}}-{\displaystyle \frac{d^2}{8}},$$

(31)

If $t^{\mathrm{CCC}}\in [-1,1]$, then build

$${\ell }_2=arccos\left(t^{\mathrm{CCC}}\right)\mathrm{and}{\ell }_2=2\pi -arccos\left(t^{\mathrm{CCC}}\right);$$

(32)

solve the linear system (30) and compute

$$\begin{array}{ccc}\hfill {\ell }_1& =& \mathrm{atan}2\left(sin{\ell }_1,cos{\ell }_1\right)\mathrm{mod}2\pi ,\hfill \\ \hfill {\ell }_3& =& \mathrm{atan}2\left(sin{\ell }_3,cos{\ell }_3\right)\mathrm{mod}2\pi .\hfill \end{array}$$

(33)

Discard the solutions that do not satisfy $\alpha +s({\ell }_1^{\mathrm{CCC}}-{\ell }_2^{\mathrm{CCC}}+{\ell }_3^{\mathrm{CCC}})=\beta \mathrm{mod}2\pi$.

Range of the Solution

The solution exists only when condition (29) is satisfied. Introducing new variables X and Y, in the same fashion as before, defined from the angles $\alpha =2arctanX$ and $\beta =2arctanY$, and substituting them in the inequality (29), we obtain four quadratic polynomials:

$$\begin{array}{ccc}\hfill p& =& sdsin\beta -{\displaystyle \frac{d^2}{2}},q=2(sin\beta -sd)\hfill \\ \hfill P_{\mathrm{CCC},1}(X;\beta )& =& (p-cos\beta -1)X^2+qX+p+cos\beta -1\hfill \\ \hfill P_{\mathrm{CCC},2}(X;\beta )& =& (p-cos\beta +7)X^2+qX+p+cos\beta +7\hfill \end{array}$$

(34)

and

$$\begin{array}{ccc}\hfill p& =& -sdsin\alpha -{\displaystyle \frac{d^2}{2}},q=2(sin\alpha +sd)\hfill \\ \hfill P_{\mathrm{CCC},3}(X;\alpha )& =& (p-cos\alpha -1)X^2+qX+p+cos\alpha -1\hfill \\ \hfill P_{\mathrm{CCC},4}(X;\alpha )& =& (p-cos\alpha +7)X^2+qX+p+cos\alpha +7.\hfill \end{array}$$

(35)

The real roots of the polynomials return the border of the interval range where $\alpha$ can run, to yield a solution for the $CCC$ problem with $\beta$ fixed. The same holds for $P_{\mathrm{CCC},2/3}(X;\alpha )$ for the interval range of $\beta$.

The following section uses the results derived so far to compute the derivatives of the optimal lengths so that an algorithm for the 3PDP can be constructed.

4. Derivatives of the Optimal Lengths

In this section, we present the partial derivatives of the optimal lengths with respect to the initial and final angles $\alpha$ and $\beta$.

4.1. Derivatives of the LSL and RSR Cases

For the ${\mathrm{CSC}}^{+}$ case given in (13), we can trivially derive some partial derivatives of the lengths with respect to $\alpha$ or $\beta$. For instance, ${\partial }_{\alpha }({\ell }_1+{\ell }_3)=-s$ and ${\partial }_{\beta }({\ell }_1+{\ell }_3)=s$. The derivatives of ${\ell }_2$ with respect to $\alpha$ and $\beta$ can be computed and simplified using (11) and collecting (12):

$$\begin{array}{ccc}\hfill {\partial }_{\alpha }{\ell }_2^{{\mathrm{CSC}}^{+}}& =& scos(\alpha -{\vartheta }_S^{{\mathrm{CSC}}^{+}})-\left(s{\mathcal{C}}^{-}cos{\vartheta }_S^{{\mathrm{CSC}}^{+}}+(sd+{\mathcal{S}}^{-})sin{\vartheta }_S\right){\partial }_{\alpha }{\vartheta }_S^{{\mathrm{CSC}}^{+}}\hfill \\ \hfill & =& scos(\alpha -(\alpha +s{\ell }_1^{{\mathrm{CSC}}^{+}}))\hfill \\ \hfill & =& scos{\ell }_1^{{\mathrm{CSC}}^{+}},\hfill \\ \hfill {\partial }_{\beta }{\ell }_2^{{\mathrm{CSC}}^{+}}& =& -scos(\beta -{\vartheta }_S^{{\mathrm{CSC}}^{+}})-s\left({\mathcal{C}}^{-}cos{\vartheta }_S^{{\mathrm{CSC}}^{+}}+(sd+{\mathcal{S}}^{-})sin{\vartheta }_S^{{\mathrm{CSC}}^{+}}\right){\partial }_{\beta }{\vartheta }_S^{{\mathrm{CSC}}^{+}}\hfill \\ \hfill & =& -scos(\beta -(\beta -s{\ell }_3^{{\mathrm{CSC}}^{+}}))\hfill \\ \hfill & =& -scos{\ell }_3^{{\mathrm{CSC}}^{+}}.\hfill \end{array}$$

(36)

Therefore, we can write the partial derivatives of the sum of the lengths using the result from (36):

$$\begin{array}{ccc}\hfill {\partial }_{\alpha }({\ell }_1^{{\mathrm{CSC}}^{+}}+{\ell }_2^{{\mathrm{CSC}}^{+}}+{\ell }_3^{{\mathrm{CSC}}^{+}})& =& s(cos{\ell }_1^{{\mathrm{CSC}}^{+}}-1),\hfill \\ \hfill {\partial }_{\beta }({\ell }_1^{{\mathrm{CSC}}^{+}}+{\ell }_2^{{\mathrm{CSC}}^{+}}+{\ell }_3^{{\mathrm{CSC}}^{+}})& =& s(1-cos{\ell }_3^{{\mathrm{CSC}}^{+}}).\hfill \end{array}$$

(37)

To remove the dependency of ${\ell }_1$ and ${\ell }_3$ from (37), we substitute their expression (13), so that we can write the partial derivatives of the sum of the lengths as follows (considering ℓ the total length, $\ell ={\ell }_1+{\ell }_2+{\ell }_3$):

$$\begin{array}{ccc}\hfill {\partial }_{\alpha }\left({\ell }^{{\mathrm{CSC}}^{+}}\right)& =& s(cos({\vartheta }_S^{{\mathrm{CSC}}^{+}}-\alpha )-1),\hfill \\ \hfill {\partial }_{\beta }\left({\ell }^{{\mathrm{CSC}}^{+}}\right)& =& s(1-cos(\beta -{\vartheta }_S^{{\mathrm{CSC}}^{+}})).\hfill \end{array}$$

(38)

The upper bound of the derivative in (37) and (38) and thus of the Lipschitz constant is 2.

4.2. Derivatives of the LSR and RSL Cases

We compute herein the partial derivatives of the optimal lengths for the ${\mathrm{CSC}}^{-}$ case given in (19) with respect to $\alpha$ or $\beta$:

$$\begin{array}{ccc}\hfill {\partial }_{\alpha }({\ell }_1^{{\mathrm{CSC}}^{-}}+{\ell }_3^{{\mathrm{CSC}}^{-}})& =& 2s{\partial }_{\alpha }{\vartheta }_S^{{\mathrm{CSC}}^{-}}-s,\hfill \\ \hfill {\partial }_{\beta }({\ell }_1^{{\mathrm{CSC}}^{-}}+{\ell }_3^{{\mathrm{CSC}}^{-}})& =& 2s{\partial }_{\beta }{\vartheta }_S^{{\mathrm{CSC}}^{-}}-s.\hfill \end{array}$$

(39)

Then, using (6) and (14), we have

$$\begin{array}{ccc}\hfill {\partial }_{\alpha }{\ell }_2^{{\mathrm{CSC}}^{-}}& =& {\partial }_{\alpha }\left((d+s{\mathcal{S}}^{+})cos{\vartheta }_S^{{\mathrm{CSC}}^{-}}-s{\mathcal{C}}^{+}sin{\vartheta }_S^{{\mathrm{CSC}}^{-}}\right)\hfill \\ \hfill & =& scos(\alpha -{\vartheta }_S^{{\mathrm{CSC}}^{-}})-\left((d+s{\mathcal{S}}^{+})sin{\vartheta }_S^{{\mathrm{CSC}}^{-}}+s{\mathcal{C}}^{+}cos{\vartheta }_S^{{\mathrm{CSC}}^{-}}\right){\partial }_{\alpha }{\vartheta }_S^{{\mathrm{CSC}}^{-}}\hfill \\ \hfill & =& scos{\ell }_1^{{\mathrm{CSC}}^{-}}-2s{\partial }_{\alpha }{\vartheta }_S^{{\mathrm{CSC}}^{-}},\hfill \\ \hfill {\partial }_{\beta }{\ell }_2^{{\mathrm{CSC}}^{-}}& =& {\partial }_{\beta }\left((d+s{\mathcal{S}}^{+})cos{\vartheta }_S^{{\mathrm{CSC}}^{-}}-s{\mathcal{C}}^{+}sin{\vartheta }_S^{{\mathrm{CSC}}^{-}}\right)\hfill \\ \hfill & =& scos(\beta -{\vartheta }_S^{{\mathrm{CSC}}^{-}})-\left((d+s{\mathcal{S}}^{+})sin{\vartheta }_S^{{\mathrm{CSC}}^{-}}+s{\mathcal{C}}^{+}cos{\vartheta }_S^{{\mathrm{CSC}}^{-}}\right){\partial }_{\beta }{\vartheta }_S^{{\mathrm{CSC}}^{-}}\hfill \\ \hfill & =& scos{\ell }_3^{{\mathrm{CSC}}^{-}}-2s{\partial }_{\alpha }{\vartheta }_S^{{\mathrm{CSC}}^{-}}.\hfill \end{array}$$

(40)

In this case, the partial derivatives of the sum of the lengths cancel out the contribution of the partial derivative of ${\vartheta }_S$, yielding a simplified expression derived from (39) and (40):

$$\begin{array}{ccc}\hfill {\partial }_{\alpha }({\ell }_1^{{\mathrm{CSC}}^{-}}+{\ell }_2^{{\mathrm{CSC}}^{-}}+{\ell }_3^{{\mathrm{CSC}}^{-}})& =& s(cos{\ell }_1^{{\mathrm{CSC}}^{-}}-1),\hfill \\ \hfill {\partial }_{\beta }({\ell }_1^{{\mathrm{CSC}}^{-}}+{\ell }_2^{{\mathrm{CSC}}^{-}}+{\ell }_3^{{\mathrm{CSC}}^{-}})& =& s(cos{\ell }_3^{{\mathrm{CSC}}^{-}}-1).\hfill \end{array}$$

(41)

Substituting the lengths into their expression from (19), we can write the partial derivatives of the sum of the lengths from (41) as

$$\begin{array}{ccc}\hfill {\partial }_{\alpha }\left({\ell }^{{\mathrm{CSC}}^{-}}\right)& =& s(cos({\vartheta }_S^{{\mathrm{CSC}}^{-}}-\alpha )-1),\hfill \\ \hfill {\partial }_{\beta }\left({\ell }^{{\mathrm{CSC}}^{-}}\right)& =& s(cos({\vartheta }_S^{{\mathrm{CSC}}^{-}}-\beta )-1).\hfill \end{array}$$

(42)

The upper bound of the derivative and thus of the Lipschitz constant is 2.

4.3. Derivatives of the LRL and RLR Cases

Using Equation (33) again, we have

$$2{\ell }_2^{\mathrm{CCC}}={\ell }_1^{\mathrm{CCC}}+{\ell }_2^{\mathrm{CCC}}+{\ell }_3^{\mathrm{CCC}}-s(\beta -\alpha )mod\left(2\pi \right),$$

(43)

and thus, deriving (43), we obtain

$$2{\partial }_{\alpha }{\ell }_2^{\mathrm{CCC}}={\partial }_{\alpha }{\ell }^{\mathrm{CCC}}+s,2{\partial }_{\beta }{\ell }_2^{\mathrm{CCC}}={\partial }_{\beta }{\ell }^{\mathrm{CCC}}-s.$$

(44)

Then, from (44) and in recalling Equation (33) and the expression of $t^{\mathrm{CCC}}$ from (27),

$$\begin{array}{ccc}\hfill {\partial }_{\alpha }\left({\ell }^{\mathrm{CCC}}\right)& =& 2{\partial }_{\alpha }\left({\ell }_2^{\mathrm{CCC}}\right)-s=\pm {\displaystyle \frac{sin(\alpha -\beta )+sdcos\alpha }{2\sqrt{1-{\left(t^{\mathrm{CCC}}\right)}^2}}}-s,\hfill \\ \hfill {\partial }_{\beta }\left({\ell }^{\mathrm{CCC}}\right)& =& 2{\partial }_{\alpha }\left({\ell }_2^{\mathrm{CCC}}\right)+s=s\mp {\displaystyle \frac{sin(\alpha -\beta )+sdcos\beta }{2\sqrt{1-{\left(t^{\mathrm{CCC}}\right)}^2}}}.\hfill \end{array}$$

(45)

The sign depends on whether the first or the second solution for ${\ell }_2$ is used.

The results in (38), (42) and (45) are compactly implemented in Algorithm 1, which solves the classic point-to-point Dubins problem and returns the solution and its derivatives. This is instrumental for building the algorithm that solves the 3PDP presented in the next section.

Algorithm 1 Dubin’s solver with derivatives

1: function Dubins(${\mathbf{P}}_{\mathbf{i}}$, ${\vartheta }_i$, ${\mathbf{P}}_{\mathbf{f}}$, ${\vartheta }_f$, ${\kappa }_{max}$)   2:     Input: ${\mathbf{P}}_{\mathbf{i}}=(x_i,y_i)$, ${\vartheta }_i$, ${\mathbf{P}}_{\mathbf{f}}=(x_f,y_f)$, ${\vartheta }_f$, ${\kappa }_{max}$;   3:     Transform to a standard problem with rotation, translation, and scaling   4:     $\varphi \leftarrow \mathrm{atan}2(y_f-y_i,x_f-x_i)$; $d\leftarrow \left|\right|{\mathbf{P}}_{\mathbf{i}}-{\mathbf{P}}_{\mathbf{f}}\left|\right|/{\kappa }_{max}$  (2); $\alpha \leftarrow {\vartheta }_i+\varphi$;$\beta \leftarrow {\vartheta }_f+\varphi$;   5:     ${\ell }_1,{\ell }_2,{\ell }_3,s_1,s_2,s_3,{\partial }_{\alpha }\ell ,{\partial }_{\beta }\ell \leftarrow$ DubinsCSCp (d, α, β)   6:     ${\ell }_1,{\ell }_2,{\ell }_3,s_1,s_2,s_3,{\partial }_{\alpha }\ell ,{\partial }_{\beta }\ell \leftarrow$ DubinsCSCp (d, α, β)   7:     ${\ell }_1,{\ell }_2,{\ell }_3,s_1,s_2,s_3,{\partial }_{\alpha }\ell ,{\partial }_{\beta }\ell \leftarrow$ DubinsCSCp (d, α, β)   8:     Select the solution with minimal ${\ell }_1+{\ell }_2+{\ell }_3$   9:     return ${\ell }_1/{\kappa }_{max},{\ell }_2/{\kappa }_{max},{\ell }_3/{\kappa }_{max},s_1,s_2,s_3,{\partial }_{\alpha }\ell /{\kappa }_{max},{\partial }_{\beta }\ell /{\kappa }_{max}$; 10: end function   1: function DubinsCSCp(d,$\alpha$,$\beta$)   2:     ${\mathcal{S}}^{-}\leftarrow sin\alpha -sin\beta$;   ${\mathcal{C}}^{-}\leftarrow cos\alpha -cos\beta$; (6)   3:     for $s\in \{-1,1\}$ do   4:         $\begin{array}{cc}{\vartheta }_S\leftarrow \mathrm{atan}2\left(-{\mathcal{C}}^{-},d+s{\mathcal{S}}^{-}\right)mod\pi \left(12\right)\hfill & \hfill \hfill \\ {\ell }_1\leftarrow s({\vartheta }_S-\alpha )mod2\pi \left(13\right)\hfill & \hfill \ell \leftarrow {\ell }_1+{\ell }_2+{\ell }_3\left(7\right)\hfill \\ {\ell }_2\leftarrow (d+s{\mathcal{S}}^{-})cos{\vartheta }_S-s{\mathcal{C}}^{-}sin{\vartheta }_S\left(13\right)\hfill & \hfill {\partial }_{\alpha }\ell \leftarrow s(cos({\vartheta }_S-\alpha )-1)\left(38\right)\hfill \\ {\ell }_3\leftarrow s(\beta -{\vartheta }_S)mod2\pi \left(13\right)\hfill & \hfill {\partial }_{\beta }\ell \leftarrow s(1-cos(\beta -{\vartheta }_S))\left(38\right)\hfill \end{array}$   5:     end for   6:     Select the solution with minimal ℓ and return ${\ell }_1,{\ell }_2,{\ell }_3,s,0,s,{\partial }_{\alpha }\ell ,{\partial }_{\beta }\ell$;   7: end function   1: function DubinsCSCm(d,$\alpha$,$\beta$)   2:     ${\mathcal{S}}^{+}\leftarrow sin\alpha +sin\beta$;   ${\mathcal{C}}^{+}\leftarrow cos\alpha +cos\beta$; (6)   3:     for $s\in \{-1,1\}$ do   4:         for X real root of $(2-{\mathcal{C}}^{+})X^2+2(sd+{\mathcal{S}}^{+})X+(2+{\mathcal{C}}^{+})=0$ do   5:            $\begin{array}{ll}{\vartheta }_S\leftarrow \mathrm{atan}2(-2X,X^2-1)\left(12\right)\hfill & \hfill \hfill \\ {\ell }_1\leftarrow s({\vartheta }_S-\alpha )mod\left(2\pi \right)\left(19\right)\hfill & \hfill \ell \leftarrow {\ell }_1+{\ell }_2+{\ell }_3\left(7\right)\hfill \\ {\ell }_2\leftarrow (d+s{\mathcal{S}}^{+})cos{\vartheta }_S-s{\mathcal{C}}^{+}sin{\vartheta }_S\left(19\right)\hfill & \hfill {\partial }_{\alpha }\ell \leftarrow s(cos({\vartheta }_S-\alpha )-1)\left(42\right)\hfill \\ {\ell }_3\leftarrow s({\vartheta }_S-\beta )mod\left(2\pi \right)\left(19\right)\hfill & \hfill {\partial }_{\beta }\ell \leftarrow s(cos({\vartheta }_S-\beta )-1)\left(42\right)\hfill \end{array}$   6:         end for   7:     end for   8:     Select the solution with minimal ℓ or set ${\ell }_1={\ell }_2={\ell }_3=\infty$ in case no solution exists.   9:     return ${\ell }_1,{\ell }_2,{\ell }_3,s,0,-s,{\partial }_{\alpha }\ell ,{\partial }_{\beta }\ell$; 10: end function   1: function DubinsCCC(d,$\alpha$,$\beta$)   2:     ${\mathcal{S}}^{-}\leftarrow sin\alpha -sin\beta$; (6)   3:     for $s\in \{-1,1\}$ do   4:         $t\leftarrow (3+cos(\alpha -\beta )-sd{\mathcal{S}}^{-})/4-d^2/8$   (31)   5:         if $t\in [-1,1]$ then   6:            for ${\ell }_2\in \{arccos\left(t\right),2\pi -arccos\left(t\right)\}$   (32) do   7:                solve linear system  (30) and compute   8:                ${\ell }_1\leftarrow \mathrm{atan}2\left(sin{\ell }_1,cos{\ell }_1\right)\mathrm{mod}2\pi$   (33);   9:                ${\ell }_3\leftarrow \mathrm{atan}2\left(sin{\ell }_3,cos{\ell }_3\right)\mathrm{mod}2\pi$   (33); 10:                ${\partial }_{\alpha }\ell \leftarrow \pm (sin(\alpha -\beta )+sdcos\alpha )/\left(2\sqrt{1-t^2}\right)-s$   (45) 11:                ${\partial }_{\beta }\ell \leftarrow s\mp (sin(\alpha -\beta )+sdcos\beta )/\left(2\sqrt{1-t^2}\right)$   (45) 12:            end for 13:         end if 14:         if not $\alpha +s({\ell }_1-{\ell }_2+{\ell }_3)=\beta \mathrm{mod}2\pi$   (34)  (35) then 15:            Discard the solution 16:         end if 17:     end for 18:     return ${\ell }_1,{\ell }_2,{\ell }_3,s,-s,s,{\partial }_{\alpha }\ell ,{\partial }_{\beta }\ell$; 19: end function

5. Three-Point Dubins Problem (3PDP)

In this section, we derive the formulation for the 3PDP. The resulting Dubins spline will be a concatenation of two Dubins paths. Hence, the total length of such a spline will be the sum of the lengths of the two elementary Dubins paths:

$${\ell }_{\mathrm{total}}({\mathbf{P}}_{\mathbf{i}},{\mathbf{P}}_{\mathbf{m}},{\mathbf{P}}_{\mathbf{f}},{\vartheta }_i,{\vartheta }_m,{\vartheta }_f)={\ell }_{\mathrm{dub}1}({\mathbf{P}}_{\mathbf{i}},{\mathbf{P}}_{\mathbf{m}},{\vartheta }_i,{\vartheta }_m)+{\ell }_{\mathrm{dub}2}({\mathbf{P}}_{\mathbf{m}},{\mathbf{P}}_{\mathbf{f}},{\vartheta }_m,{\vartheta }_f),$$

(46)

where ${\mathbf{P}}_{\mathbf{i}}=(x_i,y_i)$, ${\mathbf{P}}_{\mathbf{m}}=(x_m,y_m)$, and ${\mathbf{P}}_{\mathbf{f}}=(x_f,y_f)$ are the three points defining the 3PDP path, ${\vartheta }_i$ and ${\vartheta }_f$ are the initial and final angles of the path, and finally, ${\vartheta }_m$ is the angle of the middle point and the only unknown variable. Therefore, we will consider the total length ${\ell }_{\mathrm{total}}$ as a function of ${\vartheta }_m$ only and also consider its derivative, ${\ell }_{\mathrm{total}}^{\prime }$. Hence, (46) can be rewritten as

$$\begin{array}{ccc}\hfill {\ell }_{\mathrm{total}}\left({\vartheta }_m\right)& =& {\ell }_{\mathrm{dub}1}\left({\vartheta }_m\right)+{\ell }_{\mathrm{dub}2}\left({\vartheta }_m\right),\hfill \\ \hfill {\ell }_{\mathrm{total}}^{\prime }\left({\vartheta }_m\right)& =& {\ell }_{\mathrm{dub}1}^{\prime }\left({\vartheta }_m\right)+{\ell }_{\mathrm{dub}2}^{\prime }\left({\vartheta }_m\right).\hfill \end{array}$$

(47)

The derivatives are computed thanks to the results of the previous sections and, given (47), we have the following:

$$\begin{array}{ccccc}\hfill {\ell }_{\mathrm{dub}1}^{\prime }\left({\vartheta }_m\right)& =& {\partial }_{{\vartheta }_m}{\ell }_{\mathrm{dub}1}({\mathbf{P}}_{\mathbf{i}},{\mathbf{P}}_{\mathbf{m}},{\vartheta }_i,{\vartheta }_m)\hfill & =& {\partial }_{\beta }{\ell }_{\mathrm{dub}1}{\partial }_{{\vartheta }_m}{\beta }_1,\hfill \\ \hfill {\ell }_{\mathrm{dub}2}^{\prime }\left({\vartheta }_m\right)& =& {\partial }_{{\vartheta }_m}{\ell }_{\mathrm{dub}2}({\mathbf{P}}_{\mathbf{m}},{\mathbf{P}}_{\mathbf{f}},{\vartheta }_m,{\vartheta }_f)\hfill & =& {\partial }_{\alpha }{\ell }_{\mathrm{dub}2}{\partial }_{{\vartheta }_m}{\alpha }_2.\hfill \end{array}$$

(48)

Recalling that for a single segment ${\vartheta }_i=\alpha -\varphi$ and $x_f=d$, $y_f=0$, ${\vartheta }_f=\beta -\varphi$, and $\varphi =\mathrm{atan}2(y_f-y_i,x_f-x_i)$, we obtain

$$\begin{array}{}{\partial }_{{\vartheta }_m}{\beta }_1={\partial }_{{\vartheta }_m}({\vartheta }_m+{\varphi }_1)=1,\\ {\partial }_{{\vartheta }_m}{\alpha }_2={\partial }_{{\vartheta }_m}({\vartheta }_m+{\varphi }_2)=1\end{array}$$

(49)

However, ${\varphi }_1=\mathrm{atan}2(y_m-y_i,x_m-x_i)$ and ${\varphi }_2=\mathrm{atan}2(y_f-y_m,x_f-x_m)$ do not depend on ${\vartheta }_m$ and can be considered constants. Therefore, the derivatives of the lengths with respect to ${\vartheta }_m$ are computed using (48) and (49):

$${\ell }_{\mathrm{total}}^{\prime }\left({\vartheta }_m\right)={\partial }_{{\beta }_1}{\ell }_{\mathrm{dub}1}({\mathbf{P}}_{\mathbf{i}},{\mathbf{P}}_{\mathbf{m}},{\vartheta }_i,{\beta }_1-{\varphi }_1)+{\partial }_{{\alpha }_2}{\ell }_{\mathrm{dub}2}({\mathbf{P}}_{\mathbf{m}},{\mathbf{P}}_{\mathbf{f}},{\alpha }_2-{\varphi }_2,{\vartheta }_f).$$

However, we cannot simply find the root of the previous equation because the lengths are piecewise continuous functions of ${\vartheta }_m$. Therefore, we need to use a numerical method to find the optimal angle ${\vartheta }_m$ that minimizes the total length of the path.

Figure 2 illustrates the optimal configuration of the three-point Dubins connection. In the second subfigure, the total length is depicted as a function of the middle-point angle. This total length, which is the sum of the two individual lengths, can exhibit discontinuities. Depending on the problem’s boundary conditions, some configurations are feasible only for a limited range of internal angles. Additionally, the third subplot displays the behavior of the analytical derivative of the total length. Notably, there are jumps in the derivative even in regions where the total length appears to be smooth.

Therefore, finding the minimum length solution cannot rely on standard minimization or root-finding algorithms [19]. These traditional methods assume a certain level of smoothness and continuity in the target function and its derivatives. Therefore, we introduce a specialized section about algorithms designed to handle such irregularities effectively as in non-smooth optimization [18,19,20]. Furthermore, it is important to highlight that the minimum can occur at a jumping point where the derivative may differ from zero.

6. Algorithms

In this section, we present two algorithms employed to find the optimal angle ${\vartheta }_m$ that minimizes the total length of the three-point Dubins path. Both algorithms are based on a mixed approach that combines a pattern search method with a non-derivative root finder.

As highlighted in the previous section, the total length of the path can have discontinuities in the derivative. Therefore, the angle ${\vartheta }_m$ that minimizes the total length of the path cannot be found, in general, by simply computing the root of the derivative of the total length. This is true from a global perspective. However, if some regularity conditions are met in a specific region (interval), the root of the derivative can be used to find the optimal angle ${\vartheta }_m$.

6.1. Discretization-Based Method (DBM [29])

This algorithm is the baseline used in the literature to compare new methods. It is based on the observation that once the angle ${\vartheta }_m$ is fixed, the corresponding length of the 3PDP is readily obtained by applying the classic Dubins solution two times, first from ${\mathbf{P}}_{\mathbf{i}}$, ${\vartheta }_i$ to ${\mathbf{P}}_{\mathbf{m}}$, ${\vartheta }_m$ and a second time from ${\mathbf{P}}_{\mathbf{m}}$, ${\vartheta }_m$ to ${\mathbf{P}}_{\mathbf{f}}$, ${\vartheta }_f$. The method discretizes the round angle with N points and selects as the solution of the 3PDP the angle that returns the minimum length. This is not very accurate and not very fast. Nevertheless, it is a straightforward method used as a baseline with standard value $N=360$ and a discretization of one degree. It is described in Algorithm 2.

Algorithm 2 Discretization-Based Method (DBM [29])

1: function Dubins3pDBM(${\mathbf{P}}_{\mathbf{i}}$, ${\vartheta }_i$, ${\mathbf{P}}_{\mathbf{f}}$, ${\vartheta }_f$, ${\mathbf{P}}_{\mathbf{m}}$, ${\kappa }_{max}$, N)   2:     ${\vartheta }_m\leftarrow 0$;   $\Delta \vartheta \leftarrow 2\pi /N$;   ${\ell }_{min}\leftarrow \infty$;   3:     while ${\vartheta }_m<2\pi$ do   4:          S ← TwoDubins$({\mathbf{P}}_{\mathbf{i}},{\vartheta }_i,{\mathbf{P}}_{\mathbf{m}},{\vartheta }_m,{\mathbf{P}}_{\mathbf{f}},{\vartheta }_f,{\kappa }_{max})$;   5:         if $S.\ell <{\ell }_{min}$ then   6:            ${\ell }_{min}\leftarrow S.\ell$;   $C\leftarrow S$;      (* select this solution *)   7:         end if   8:         ${\vartheta }_m\leftarrow {\vartheta }_m+\Delta \vartheta$;   9:     end while 10:     return C;   (* the structure of the selected solution *) 11: end function   1: function TwoDubins(${\mathbf{P}}_{\mathbf{i}}$, ${\vartheta }_i$, ${\mathbf{P}}_{\mathbf{m}}$, ${\vartheta }_m$, ${\mathbf{P}}_{\mathbf{f}}$, ${\vartheta }_f$, ${\kappa }_{max}$)   2:     S.A ← Dubins$({\mathbf{P}}_{\mathbf{i}},{\vartheta }_i,{\mathbf{P}}_{\mathbf{m}},{\vartheta }_m,{\kappa }_{max})$;   3:     S.B ← Dubins$({\mathbf{P}}_{\mathbf{m}},{\vartheta }_m,{\mathbf{P}}_{\mathbf{f}},{\vartheta }_f,{\kappa }_{max})$;   4:     $S.\ell \leftarrow S.A.\ell +S.B.\ell$;      (* store total length *)   5:     $S.{\vartheta }_i\leftarrow {\vartheta }_i$;   $S.{\vartheta }_m\leftarrow {\vartheta }_m$;   $S.{\vartheta }_f\leftarrow {\vartheta }_f$;   6:     return S;      (* return a structure with the two curves *)   7: end function

6.2. Pattern Search with Root Finder

Algorithm 3 combines the potential of a pattern search method with a non-derivative root finder. The pattern search method is used iteratively to refine the interval of angles where the total length has a local minimum. The iterative search stops when the interval is small enough or whenever sufficient conditions to use the non-derivative root finder are met. The non-derivative root finder yields the optimal angle ${\vartheta }_m$ inside the regular interval retrieved by the pattern search method. The sufficient conditions are that the solution does not change type inside the interval. If the derivatives at the interval’s endpoints have opposite signs, then, by continuity, the minimum is inside the interval. In this case, a non-derivative root finder, which searches the zero of the derivative of the length, is used. The algorithm employed to find the root is a variant of the bisection with interpolation Algorithm 748 from [22].

Algorithm 3 Pattern Search Refinement (Based on Direct Search [37])

1: function Dubins3pPatternRefine(${\mathbf{P}}_{\mathbf{i}}$, ${\vartheta }_i$, ${\mathbf{P}}_{\mathbf{m}}$, ${\vartheta }_m$, ${\mathbf{P}}_{\mathbf{f}}$, ${\vartheta }_f$, ${\kappa }_{max}$, ${\vartheta }_m^L$, ${\vartheta }_m^C$, ${\vartheta }_m^R$)   2:     $L\leftarrow \mathrm{T}ᴡᴏ\mathrm{D}ᴜʙɪɴꜱ({\mathbf{P}}_{\mathbf{i}},{\vartheta }_i,{\mathbf{P}}_{\mathbf{m}},{\vartheta }_m^L,{\mathbf{P}}_{\mathbf{f}},{\vartheta }_f,{\kappa }_{max})$;   3:     $C\leftarrow \mathrm{T}ᴡᴏ\mathrm{D}ᴜʙɪɴꜱ({\mathbf{P}}_{\mathbf{i}},{\vartheta }_i,{\mathbf{P}}_{\mathbf{m}},{\vartheta }_m^C,{\mathbf{P}}_{\mathbf{f}},{\vartheta }_f,{\kappa }_{max})$;   4:     $R\leftarrow \mathrm{T}ᴡᴏ\mathrm{D}ᴜʙɪɴꜱ({\mathbf{P}}_{\mathbf{i}},{\vartheta }_i,{\mathbf{P}}_{\mathbf{m}},{\vartheta }_m^R,{\mathbf{P}}_{\mathbf{f}},{\vartheta }_f,{\kappa }_{max})$;   5:     while $R.{\vartheta }_m-L.{\vartheta }_m>\Delta {\vartheta }_{min}$ do (* refine solution *)   6:         OPTIONAL: If curve type for L, C is the same   7:         and the length derivative changes sign, use algo 748   8:         to refine the solution between $L.{\theta }_m$ and $C.{\theta }_m$ and return.   9:          10:         OPTIONAL: If curve type for C, R is the same 11:         and the length derivative changes sign, use algo 748 12:         to refine the solution between $C.{\theta }_m$ and $R.{\theta }_m$ and return. 13:          14:         $\widehat{L}\leftarrow \mathrm{T}ᴡᴏ\mathrm{D}ᴜʙɪɴꜱ({\mathbf{P}}_{\mathbf{i}},{\vartheta }_i,{\mathbf{P}}_{\mathbf{m}},(L.{\vartheta }_m+C.{\vartheta }_m)/2,{\mathbf{P}}_{\mathbf{f}},{\vartheta }_f,{\kappa }_{max})$; 15:         $\widehat{R}\leftarrow \mathrm{T}ᴡᴏ\mathrm{D}ᴜʙɪɴꜱ({\mathbf{P}}_{\mathbf{i}},{\vartheta }_i,{\mathbf{P}}_{\mathbf{m}},(R.{\vartheta }_m+C.{\vartheta }_m)/2,{\mathbf{P}}_{\mathbf{f}},{\vartheta }_f,{\kappa }_{max})$; 16:         if $\widehat{L}.\ell <\widehat{R}.\ell$ then 17:            if $\widehat{L}.\ell <C.\ell$ then 18:                $R\leftarrow C;$   $C\leftarrow \widehat{L}$; 19:            else 20:                $L\leftarrow \widehat{L};$   $R\leftarrow \widehat{R}$; 21:            end if 22:         else 23:            if $\widehat{R}.\ell <C.\ell$ then 24:                $L\leftarrow C;$   $C\leftarrow \widehat{R}$; 25:            else 26:                $L\leftarrow \widehat{L};$   $R\leftarrow \widehat{R}$; 27:            end if 28:         end if 29:     end while 30:     return C;   (* best solution found *) 31: end function

Algorithm 4 (Pattern Trichotomy) is similar to the previous one with a slight variation. The main drawback of the pattern search method is that it can find only one local minimum of the total length. Algorithm 4 overcomes, in part, this limitation using a pattern search method to find multiple local minima of the total length. The algorithm iteratively searches for the local minima of the total length and stores them in a list of candidate minima. Then, the algorithm explores the candidates in a neighborhood of decreasing size and stops the search in two cases. The first is when the interval is small enough. The second case is when sufficient conditions are met, where the fast bisection can be used on the total length derivative. In both cases, the solution found is stored if it is better than a previously computed one.

The previously described methods (Algorithms 3 and 4) are not always able to catch the correct minima if there are discontinuities or jumps smaller than the first sampling range $\Delta \vartheta$. Our proposed hybrid method (Algorithm 5) overcomes this problem by isolating first the discontinuity points, and then adding at least two more points (before and after the jump). Furthermore, additional points are sampled according to the interval range $\Delta \vartheta$. The discontinuity isolation is performed by computing the existence range of the solution computed in (21), (34) and (35). The algorithm loops over all the sampled points, comparing the left and right samples. If the length at the central point is less or equal to the left and right lengths, we refine the solution. The algorithm returns the best solution found. The refinement phase is conducted by either the Dubins3pPatternRefine or Dubins3pTrichotomyRefine functions described in Algorithms 3 and 4.

Algorithm 4 Trichotomy Refine (Based on Trichotomy Method [36])

1: function Dubins3pTrichotomyRefine(${\mathbf{P}}_{\mathbf{i}}$, ${\vartheta }_i$, ${\mathbf{P}}_{\mathbf{m}}$, ${\vartheta }_m$, ${\mathbf{P}}_{\mathbf{f}}$, ${\vartheta }_f$, ${\kappa }_{max}$, ${\vartheta }_m^L$, ${\vartheta }_m^C$, ${\vartheta }_m^R$ )   2:     $L\leftarrow \mathrm{T}ᴡᴏ\mathrm{D}ᴜʙɪɴꜱ({\mathbf{P}}_{\mathbf{i}},{\vartheta }_i,{\mathbf{P}}_{\mathbf{m}},{\vartheta }_m^L,{\mathbf{P}}_{\mathbf{f}},{\vartheta }_f,{\kappa }_{max})$;   3:     $C\leftarrow \mathrm{T}ᴡᴏ\mathrm{D}ᴜʙɪɴꜱ({\mathbf{P}}_{\mathbf{i}},{\vartheta }_i,{\mathbf{P}}_{\mathbf{m}},{\vartheta }_m^C,{\mathbf{P}}_{\mathbf{f}},{\vartheta }_f,{\kappa }_{max})$;   4:     $R\leftarrow \mathrm{T}ᴡᴏ\mathrm{D}ᴜʙɪɴꜱ({\mathbf{P}}_{\mathbf{i}},{\vartheta }_i,{\mathbf{P}}_{\mathbf{m}},{\vartheta }_m^R,{\mathbf{P}}_{\mathbf{f}},{\vartheta }_f,{\kappa }_{max})$;   5:     while $R.{\vartheta }_m-L.{\vartheta }_m>\Delta {\vartheta }_{min}$ do (* refine solution *)   6:         OPTIONAL: If curve type for L, C is the same   7:         and the length derivative change signs, use algo 748   8:         to refine the solution between $L.{\theta }_m$ and $C.{\theta }_m$ and return.   9:          10:         OPTIONAL: If curve type for C, R is the same 11:         and the length derivative changes sign, use algo 748 12:         to refine the solution between $C.{\theta }_m$ and $R.{\theta }_m$ and return. 13:          14:         $\widehat{L}\leftarrow \mathrm{T}ᴡᴏ\mathrm{D}ᴜʙɪɴꜱ({\mathbf{P}}_{\mathbf{i}},{\vartheta }_i,{\mathbf{P}}_{\mathbf{m}},(L.{\vartheta }_m+2C.{\vartheta }_m)/3,{\mathbf{P}}_{\mathbf{f}},{\vartheta }_f,{\kappa }_{max})$; 15:         if $\widehat{L}.\ell \le C.\ell$ then 16:            $\tilde{L}\leftarrow \mathrm{T}ᴡᴏ\mathrm{D}ᴜʙɪɴꜱ({\mathbf{P}}_{\mathbf{i}},{\vartheta }_i,{\mathbf{P}}_{\mathbf{m}},(2L.{\vartheta }_m+C.{\vartheta }_m)/3,{\mathbf{P}}_{\mathbf{f}},{\vartheta }_f,{\kappa }_{max})$; 17:            if $\tilde{L}.\ell \le \widehat{L}.\ell$ then 18:                $C\leftarrow \tilde{L}$;   $R\leftarrow \widehat{L}$; 19:            else 20:                $L\leftarrow \tilde{L}$;   $R\leftarrow C$;   $C\leftarrow \widehat{L}$; 21:            end if 22:         else 23:            $\widehat{R}\leftarrow \mathrm{T}ᴡᴏ\mathrm{D}ᴜʙɪɴꜱ({\mathbf{P}}_{\mathbf{i}},{\vartheta }_i,{\mathbf{P}}_{\mathbf{m}},(R.{\vartheta }_m+2C.{\vartheta }_m)/3,{\mathbf{P}}_{\mathbf{f}},{\vartheta }_f,{\kappa }_{max})$; 24:            if $\widehat{R}.\ell \le C.\ell$ then 25:                $\tilde{R}\leftarrow \mathrm{T}ᴡᴏ\mathrm{D}ᴜʙɪɴꜱ({\mathbf{P}}_{\mathbf{i}},{\vartheta }_i,{\mathbf{P}}_{\mathbf{m}},(2R.{\vartheta }_m+C.{\vartheta }_m)/3,{\mathbf{P}}_{\mathbf{f}},{\vartheta }_f,{\kappa }_{max})$; 26:                if $\tilde{R}.\ell \le \widehat{R}.\ell$ then 27:                    $L\leftarrow \widehat{R}$;   $C\leftarrow \tilde{R}$; 28:                else 29:                    $L\leftarrow C$;   $C\leftarrow \widehat{R}$;   $R\leftarrow \tilde{R}$; 30:                end if 31:            else 32:                $L\leftarrow \widehat{L}$;   $R\leftarrow \widehat{R}$; 33:            end if 34:         end if 35:     end while 36:     return C;   (* best solution found *) 37: end function

Algorithm 5 Hybrid Search

1: function Dubins3pHybridSearch(${\mathbf{P}}_{\mathbf{i}}$, ${\vartheta }_i$, ${\mathbf{P}}_{\mathbf{f}}$, ${\vartheta }_f$, ${\mathbf{P}}_{\mathbf{m}}$, ${\kappa }_{max}$, $\Delta \vartheta$)   2:     ${\mathbf{\vartheta }}_{\mathbf{d}}\leftarrow$ Compute the candidate discontinuity points   3:     ${\mathbf{\vartheta }}_{\mathbf{s}}\leftarrow$ sample points (at least 2 close to ${\mathbf{\vartheta }}_{\mathbf{d}}$ with distance less or equal to $\Delta \vartheta$)   4:     for all ${\vartheta }_m\in {\mathbf{\vartheta }}_{\mathbf{c}}$ do   5:         ${\vartheta }_m^L,{\vartheta }_m^R\leftarrow$ left and right sample points of ${\vartheta }_m$   6:         if $\ell \left({\vartheta }_m\right)\le \ell \left({\vartheta }_m^L\right)$ and $\ell \left({\vartheta }_m\right)\le \ell \left({\vartheta }_m^R\right)$ then   7:            C←Refine$({\mathbf{P}}_{\mathbf{i}},{\vartheta }_i,{\mathbf{P}}_{\mathbf{m}},{\vartheta }_m,{\mathbf{P}}_{\mathbf{f}},{\vartheta }_f,{\kappa }_{max},{\vartheta }_m^L,{\vartheta }_m,{\vartheta }_m^R)$   8:         end if   9:     end for 10:     return C;   (* the structure of the best computed solution *) 11: end function where Refine can be Dubins3pPatternRefine or Dubins3pTrichotomyRefine

7. Numerical Tests and Results

In this section, we briefly present the numerical results and performance achievements. The tests were conducted on a commercial laptop with an Apple M2 Max processor. The test was performed on a sampled pool of $10,000$ random three-point Dubins problems, which is the standard benchmark test for the 3PDP found in the literature. The results are summarized in

Table 2. The evaluation of the proposed algorithms with the benchmark test with mean $\mu$ and standard deviation $\sigma$ of iterations and computational times for the sampling in (50).

Algorithm	Iter Number		Time (ms)		N. Dubins Calls
	$\mathit{\mu }$	$\mathit{\sigma }$	$\mathit{\mu }$	$\mathit{\sigma }$
Pattern search	85.54	10.66	0.174	0.036	∼171
Pattern search + 748	46.27	10.72	0.122	0.031	∼92
Pattern Trichotomy	73.69	7.97	0.150	0.026	∼147
Pattern Trichotomy + 748	54.99	8.74	0.115	0.025	∼109
DBM – Algorithm 2	360	0.0	54.846	3.210	720
iDPP [12]	128	0.0	19.455	0.740	256

,

Table 3. Comparison between number of iterations and time with respect to brute force DBM for sampling in (50). (n/a means not available.)

Algorithm	Iter Number Ratio	Time Ratio
Pattern search	4.27	325.66
Pattern search + 748	6.57	471.78
Pattern Trichotomy	4.94	374.73
Pattern Trichotomy + 748	6.67	493.50
iDPP [12]	2.81	2.82
IM [26]	n/a	5
PBM [31]	n/a	[25, 45]
GBM [28]	n/a	150

,

Table 4. The evaluation of the proposed algorithms with the benchmark test with mean $\mu$ and standard deviation $\sigma$ of iterations and computational times for the sampling in (51).

Algorithm	Iter Number		Time (ms)		N. Dubins Calls
	$\mathit{\mu }$	$\mathit{\sigma }$	$\mathit{\mu }$	$\mathit{\sigma }$
Pattern search	88.35	20.57	0.217	0.047	∼176
Pattern search + 748	77.70	23.55	0.187	0.031	∼155
Pattern Trichotomy	74.78	13.34	0.179	0.032	∼149
Pattern Trichotomy + 748	68.54	14.81	0.163	0.034	∼137
DBM – Algorithm 2	360	0.0	55.577	3.357	720
iDPP [12]	128	0.0	19.720	0.845	256

and

Table 5. Comparison between number of iterations and time with respect to brute force DBM for sampling in (51). (n/a means not available.)

Algorithm	Iter Number Ratio	Time Ratio
Pattern search	4.27	265.66
Pattern search + 748	5.02	315.19
Pattern Trichotomy	4.95	318.01
Pattern Trichotomy + 748	5.47	354.27
iDPP [12]	2.81	2.82
IM [26]	n/a	5
PBM [31]	n/a	[25, 45]
GBM [28]	n/a	150

.

We employed two different ranges of sampling, the first coming from the literature and representing a standard benchmark case, whereas the second was more representative of the solution-type distribution. Without loss of generality, we kept the initial and final points fixed at $(-1,0)$ and $(1,0)$, respectively.

In the first sampling, we took the middle point uniformly distributed in $[-10,10]\times [-10,10]$. The initial and final angles were in the interval $[-\pi ,\pi ]$, and the maximum curvature was kept constant at 1. The same test was conducted in [26,28,31]. In summary,

$$(x_m,y_m)\in [-10,10]\times [-10,10],{\theta }_i,{\theta }_f\in [-\pi ,\pi ],\mathrm{and}{\kappa }_{max}=1,$$

(50)

The second test sampled the middle point randomly inside the region $[-2,2]\times [-2,2]$. As before, the initial and final angles were randomly sampled in the interval $[-\pi ,\pi ]$, and we randomly generated the maximum curvature inside the interval $[0.1,1.5]$. In brief,

$$(x_m,y_m)\in [-2,2]\times [-2,2],{\theta }_i,{\theta }_f\in [-\pi ,\pi ],\mathrm{and}{\kappa }_{max}\in [0.1,1.5].$$

(51)

We proposed the second test because, when the middle point is distant from the initial and the final points, we lose two potential solutions of the 3PDP. When the distance is greater than the maximum curvature inverse, we can only observe 4 of the 18 possible solutions, which are the ones with the straight line in the middle.

In Table 2 and Table 4, we present the mean and standard deviation of both the number of iterations and computational time for the sampling in (50) and (51), respectively. The last columns of the tables present the number of calls to the Dubins function, on average. It is important to highlight that the number of iterations is linked to the number of Dubins calls (twice). The information is redundant, but most of the available state-of-the-art publications report the number of Dubins calls. The computational time is measured in milliseconds. It is important to stress that the computational time depends on the implementation.

Table 3 and Table 5 present a comparison between different approaches scaling the results with the DBM benchmark. The ratio is computed as the number of iterations and time of the algorithm divided by the number of iterations and time of the DBM algorithm. The results are presented for the sampling in (50) and (51), respectively.

The four tables (Table 2, Table 3, Table 4 and Table 5) showcase the strong performance of our algorithm when the middle point is distant, on average. In fact, in this occurrence, both the length and the derivative do not jump; hence, non-smooth root-finding algorithms can be used to obtain the minimum length curve.

The results show that the proposed algorithms outperform the benchmark algorithms in terms of the number of iterations and time. The pattern search with the root finder and the pattern trichotomy algorithms are more efficient than the brute force (DBM) and refinement (iDDP) algorithms by a factor ranging between $4.27$ and $6.67$ times depending on the proposed sampling. Moreover, it should be noted that these methods do not achieve the same precision: indeed, the DBM evaluates the problem with a 1-degree precision, while the other methods have a precision of ${10}^{-2}$ degrees. However, the threshold precision can be set to a custom value in our proposed methods.

The Pattern Trichotomy algorithm is the most efficient one. It requires the fewest iterations and the least amount of time. The pattern search algorithm is slightly less efficient than the Pattern Trichotomy algorithm. However, it is still more efficient than the DBM, IM, PBM, and iDDP algorithms. The pattern search algorithm combined with the root finder (algorithm 748) requires fewer iterations on average. However, the trichotomy algorithm implementation allows a speed-up in the computational time. Hence, the pattern trichotomy algorithm equipped with a root finder is the fastest and most efficient. Furthermore, when root finding is employed, both pattern search with algorithm 748 and Pattern Trichotomy with algorithm 748 yield the same exact result up to machine precision.

Looking at the comparison in Table 5, we can see that the pattern search and Pattern Trichotomy algorithms require approximately 4 times fewer iterations than the DBM algorithm, and 2.8 times with respect to the iDPP. However, if we compare the time required by the algorithms, the pattern search exhibits a factor of 270, and the Pattern Trichotomy, a factor of 320. Other speed-up factors (compared to the brute force DBM) declared in the literature are about 3 for the iDPP [12], which is a general method capable of solving the MPMDP with n points, adapted for the case $n=3$; about 5 for the inversive geometry iterative method (IM) [26]; between 25 and 45 for the polynomial-based method (PBM) [31]; and about 150 for the geometry-based method (GBM) [28], (which solves exactly the case of points at distance $d>d^{★}$ only, and gives a good approximation otherwise). Therefore, we can conclude that our methods are performant.

Figure 3 illustrates the optimal configuration of the three-point Dubins connection in a challenging configuration. In the second subfigure, the total length showcases strong discontinuities resulting in a tiny interval of existence of the retrieved minimum. Furthermore, the derivative displays many noncontinuous points, hence demonstrating the non-applicability of derivative methods, at least globally.

We compared the example proposed in [28,31], where they tested their implementation versus the DBM. The test example has the following definition:

$${\mathbf{P}}_{\mathbf{i}}=(0,0),{\vartheta }_i={\displaystyle \frac{\pi }{3}},{\mathbf{P}}_{\mathbf{m}}=(10,5),{\mathbf{P}}_{\mathbf{f}}=(15,20),{\vartheta }_f={\displaystyle \frac{\pi }{6}}.$$

(52)

The DBM yields the following solution:

$${\ell }_{\mathrm{total}}=27.11279494,{\theta }_M=0.8575935377.$$

(53)

This is a suboptimal solution, as highlighted in [28]. Their work claims to obtain a solution that is $1.08\times {10}^{-6}$ % better than that of the DBM. Our approach provides solutions with the following values, reported in

Table 6. Comparison of result with the best result (pattern search + 748 and Pattern Trichotomy + 748).

Algorithm	Length	${\mathit{\theta }}_{\mathit{M}}$
Pattern search + 748	27.1127934	0.8556738609
Pattern Trichotomy + 748	+0.0	+0.0
Pattern search	$+1.552\times {10}^{-11}$	$+3.711\times {10}^{-5}$
Pattern Trichotomy	$+8.985\times {10}^{-13}$	$+8.929\times {10}^{-6}$
DBM – Algorithm 2	$+5.672\times {10}^{-8}$	$+2.243\times {10}^{-3}$
iDPP [12]	$+0.0$	$+1.510\times {10}^{-8}$

and Figure 4, representing our best solution. The DBM performs 360 iterations and will need to perform 100 times more to achieve the same precision as in [28]. Moreover, the pattern search and Pattern Trichotomy algorithms stop the iteration when a custom tolerance is reached ($1/100$ degree). On the other hand, when combined with Algorithm 748, our approach retrieves the solution to machine precision, yielding the true numerical minimum.

8. Conclusions

We presented two novel algorithms for solving the 3PDP, a non-smooth constrained minimization problem and an optimal control problem. Our methods leverage the pattern search technique, integrating analytic derivatives within continuous-length function intervals to enhance computational efficiency. This improvement is crucial for the 3PDP and may also be used for the Dubins Traveling Salesman Problem. In these examples, high performance is essential due to the routine’s frequent invocation.

Extensive numerical tests were conducted on a standard benchmark pool of 10,000 random Three-Point Dubins Problems. Two different sampling ranges were employed to reflect standard benchmarks and more representative scenarios with a middle point close to the initial and final points. The results demonstrate that our proposed algorithms significantly outperform existing methods in terms of both iteration count and computational time.

For the first sampling range, our pattern search and Pattern Trichotomy algorithms achieved an iteration count reduction factor of approximately $4.27$ to $6.674$ times compared to the DBM algorithm, and a time reduction factor ranging from ∼325 to ∼493 times. For the second sampling range, the iteration count reduction factors were similar with a range of $4.27$ to $5.47$ times, with the time reduction factors being 265 to 354 times compared to the DBM algorithm. Additionally, when combined with Algorithm 748, both the pattern search and Pattern Trichotomy methods consistently yielded results with machine precision, demonstrating their robustness and accuracy.

These performance improvements are notable when compared to other state-of-the-art methods, such as the polynomial-based method (PBM) in [31], which achieves a 25–45 times speed-up over the DBM, and the geometry-based method (GBM) in [28], which offers a speed-up of approximately 150 times. Our methods achieve a computational speed-up factor exceeding 300 times compared to the DBM and less than half the time declared in [28].

Overall, our methods represent a significant advancement in solving the 3PDP efficiently, making them highly suitable for applications where computational performance is critical.

9. Future Works

Our research is ongoing, and this work is part of a broader project. We are working on the generalization of the 3PDP to the Multipoint Markov–Dubins problem (MPMDP). The goal is to provide a general and fast method to solve these Dubins-related challenging problems encountered in robotics. Our research aims to find a solution that does not rely on finite meshing exploration.

Moreover, we are working on domain-specific applications for the 3PDP. We are including our implementation into an autonomous parking framework [38]. The solution to the 3PDP is used to efficiently compute an exploration trajectory with obstacle collision checking. Furthermore, we are extending the framework developed in [4] to include low-speed maneuvers with the 3PDP. The goal is to employ a piecewise constant curvature profile for valet maneuvers.