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

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}$, 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.

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 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.

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:

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,

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$: 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: 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$: 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.

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