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
, which is defined as the inverse of the maximum curvature of the path,
. 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
,
and
, with angles
and
is the
and piecewise
path of minimum length
such that the absolute value of the curvature of the path
, at almost every point, is not greater than
; 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
subject to
,
Each extremal is composed of at most three internal arcs. This sequence of curves can be written as words:
,
,
, 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]
and
C. Thus, there are 15 possible combinations:
,
,
.
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
using interpolation with Dubins paths [
12,
13]. The initial and final angles,
and
, are assigned, while the other intermediate angles
, for
, 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
with orientation
, a middle point
, and a final point
with orientation
, find the shortest concatenation of two Dubins paths that begin in
, pass through
, and reach
while respecting the constraints of having the maximum absolute curvature not exceeding
; see
Figure 1. The problem is clearly one-dimensional, with the angle
at
being the only unknown of the problem. Once
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
, 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
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
[
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
, 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,
subproblems must be solved, each containing a local minimum of the original problem. In their analysis, the authors reduce the complexity to
, 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
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
, to a worse case improvement of a factor of 5 for points that are closer than
. 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
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 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
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
; 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
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
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 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
, and
,
with a unitary maximum curvature of
. Let
d be the Euclidean distance between the initial and final points scaled by the maximum curvature:
Then, the aforementioned transform is the following, (
3): the translation is given by
and is followed by a rotation around
by an angle
given by
, followed by a scaling of
, or compactly,
Let
and
be the initial and final angles in the transformed coordinate system. The new initial point is thus the origin with angle
, and the final point is on the positive
-axis at distance
d, (
2), from the origin and angle
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
:
where
ℓ is the curvilinear abscissa,
and
are the coordinates, and
is the orientation. Let us define
and
, corresponding to the initial point. Notice that, when the integer variable
s is equal to 0, the function represents a line segment, whereas, when
, the curve is an arc of a unitary circle turning left, and, when
, 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
,
, and
for the signs of the curvature of the three segments coming from (
4), we can write the equations for the three segments:
The proposed transformation simplifies the equations, eliminating
,
,
,
,
, and
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
and
that will be used in the following sections, to specify that the expression depends on trigonometric combinations of
and
:
where
denotes a set of the trigonometric functions of the angles
and
. We remark that
and
are functions of
, but we drop the explicit dependence on
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.
3.1. Case CSC
This case happens when
and, depending on the sign of
and
, gives origin to the curves LSL, RSR, LSR, and RSL. Then, system (
5) simplifies to
and by imposing the continuity conditions to (
8) on the angle of the line segment
, we have
We combine these two Equation in (
9), multiplying by
and
, respectively, and we obtain an explicit expression for
and an implicit expression for
:
This case can be further separated into two subcases, the first, , with the two circular arcs turning in the same direction (LSL and RSR), and the other, , with circular arcs in the opposite direction (LSR and RSL). They have two different solutions, as detailed below.
3.1.1. The Subcase : LSL and RSR
As stated in the previous section, this solution type falls into the
case, where
and
. Hence, we can rewrite Equation (
10) in terms of the parameters
using notation (
6) as follows:
From the first equation, we can solve for
, and the second is already solved for
, as a function of
. Thus, we have
We choose the only
producing a non-negative
. Any
not respecting this constraint is discarded. The length
is obtained from (
11), knowing the solution in (
12), and from the condition
, we can derive the other lengths
and
, which are as follows:
3.1.2. The Subcase : LSR and RSL
This solution type falls into the
case, where
,
, and
. It is possible to rewrite Equation (
10) in terms of the parameters
using notation (
6) as follows:
We can substitute the expression for
to obtain a polynomial equation as a function of the auxiliary variable
X.
Therefore, using (
15) in (
14), multiplying by
, and using
, we can obtain a quadratic equation in
X as follows:
Hence, once the polynomial equation is solved. We can substitute the expression for
in (
14), derive the lengths
, and retrieve
and
with the following:
If
, the polynomial in (
16) is not well posed and ill conditioned. Therefore, we can change the coordinates to
to reshape the polynomial, yielding the following expression:
From the condition
and (
14), (
15), or (
18), we can derive the lengths
,
, and
, recalling also (
17), as follows:
3.1.3. Range of the Solution
The solution exists only for angles coming from real roots of polynomial (
16). The discriminant
of that polynomial is
In introducing new variables
Z and
W, defined by the relations
and
, it is possible to convert the trigonometric expressions into polynomials. Substituting each of these relations into the discriminant in (
20), we obtain two polynomials:
where
. The real roots of
represent the border of the range interval where
can run, to obtain a solution for the
problem for a fixed
. The same holds for
for the interval range of
.
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 and .
3.2. Case
The
case, happening when
, comprises two paths: LRL and RLR. In this situation, we can impose
and simplify (
5) to obtain
Equation (
22) can be rewritten in compact form as
We can state Equation (
23) in terms of the parameters
using notation (
6) as
From (
24) and squaring, we obtain
and by adding the two equalities (
25) (and dividing by 2), we obtain
where the last step follows inserting the sixth equation of (
5), that is,
, and noting that cosine is even. Thus,
can be obtained from (
26) as follows:
The solution exists when the argument of arccos is in the interval
. Therefore, the
solved in (
27) must be comprised between
and 1, yielding the following inequality:
The previous expression (
28) can be multiplied by 4 and moved
to the left and right side:
We can obtain the other relations from the trigonometric relations furnished by (
24). From
and
and expanding the trigonometric functional, we obtain a linear system in
and
, and from
and
and expanding the trigonometric functions again, we obtain a linear system in
and
as follows:
where
and
. In conclusion, we can summarize the results for the
case as follows:
If
, then build
solve the linear system (
30) and compute
Discard the solutions that do not satisfy .
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
and
, and substituting them in the inequality (
29), we obtain four quadratic polynomials:
and
The real roots of the polynomials return the border of the interval range where can run, to yield a solution for the problem with fixed. The same holds for for the interval range of .
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.
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:
where
,
, and
are the three points defining the 3PDP path,
and
are the initial and final angles of the path, and finally,
is the angle of the middle point and the only unknown variable. Therefore, we will consider the total length
as a function of
only and also consider its derivative,
. Hence, (
46) can be rewritten as
The derivatives are computed thanks to the results of the previous sections and, given (
47), we have the following:
Recalling that for a single segment
and
,
,
, and
, we obtain
However,
and
do not depend on
and can be considered constants. Therefore, the derivatives of the lengths with respect to
are computed using (
48) and (
49):
However, we cannot simply find the root of the previous equation because the lengths are piecewise continuous functions of . Therefore, we need to use a numerical method to find the optimal angle 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 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 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 .
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
is fixed, the corresponding length of the 3PDP is readily obtained by applying the classic Dubins solution two times, first from
,
to
,
and a second time from
,
to
,
. 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
and a discretization of one degree. It is described in Algorithm 2.
Algorithm 2 Discretization-Based Method (DBM [29]) |
- 1:
function Dubins3pDBM(, , , , , , N) - 2:
; ; ; - 3:
while do - 4:
S ← TwoDubins; - 5:
if then - 6:
; ; (* select this solution *) - 7:
end if - 8:
; - 9:
end while - 10:
return C; (* the structure of the selected solution *) - 11:
end function
- 1:
function TwoDubins(, , , , , , ) - 2:
S.A ← Dubins; - 3:
S.B ← Dubins; - 4:
; (* store total length *) - 5:
; ; ; - 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
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(, , , , , , , , , ) - 2:
; - 3:
; - 4:
; - 5:
while 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 and 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 and and return. - 13:
- 14:
; - 15:
; - 16:
if then - 17:
if then - 18:
; - 19:
else - 20:
; - 21:
end if - 22:
else - 23:
if then - 24:
; - 25:
else - 26:
; - 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
. 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
. 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(, , , , , , , , , ) - 2:
; - 3:
; - 4:
; - 5:
while 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 and 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 and and return. - 13:
- 14:
; - 15:
if then - 16:
; - 17:
if then - 18:
; ; - 19:
else - 20:
; ; ; - 21:
end if - 22:
else - 23:
; - 24:
if then - 25:
; - 26:
if then - 27:
; ; - 28:
else - 29:
; ; ; - 30:
end if - 31:
else - 32:
; ; - 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(, , , , , , ) - 2:
Compute the candidate discontinuity points - 3:
sample points (at least 2 close to with distance less or equal to ) - 4:
for all do - 5:
left and right sample points of - 6:
if and then - 7:
C←Refine - 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
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,
Table 3,
Table 4 and
Table 5.
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 and , respectively.
In the first sampling, we took the middle point uniformly distributed in
. The initial and final angles were in the interval
, and the maximum curvature was kept constant at 1. The same test was conducted in [
26,
28,
31]. In summary,
The second test sampled the middle point randomly inside the region
. As before, the initial and final angles were randomly sampled in the interval
, and we randomly generated the maximum curvature inside the interval
. In brief,
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 and 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 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
; 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
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:
The DBM yields the following solution:
This is a suboptimal solution, as highlighted in [
28]. Their work claims to obtain a solution that is
% better than that of the DBM. Our approach provides solutions with the following values, reported in
Table 6 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 (
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 to 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 to 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.