# A Non-Smooth Numerical Optimization Approach to the Three-Point Dubins Problem (3 PDP)
## 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}=\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_{i}}=(x_{i},y_{i})\) and \(\mathbf{P_{f}}=(x_{f},y_{f})\), with angles \(\theta_{i}\) and \(\theta_{f}\) is the \(C^{1}\) and piecewise \(C^{2}\) path of minimum length \(\gamma:[0,L]\rightarrow\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 [PERSON] in 1889 was solved only in 1957 by [PERSON] [9], who proved that there are six extremal solutions to the associated optimal control problem:  
\[\text{Minimize }L=\int_{0}^{L}1\,\mathrm{d}\ell\quad\text{subject to }|u(\ell)|\leq\kappa_{\text{max}}, \tag{1}\] \[x^{\prime}(\ell) =\,\cos(\theta(\ell)),\quad y^{\prime}(\ell) =\,\sin(\theta(\ell)),\quad\theta^{\prime}(\ell) =\,u(\ell),\] \[x(0) =\,x_{i},\qquad\qquad y(0) =\,y_{i},\qquad\qquad\theta(0) =\,\theta_{i},\] \[x(L) =\,x_{f},\qquad\qquad y(L) =\,y_{f},\qquad\qquad\theta(L) =\,\theta_{f}.\]  
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,SCC,CC\), and \(C\). Thus, there are 15 possible combinations: \(LSL,LSR,RSL,RSR,RLR,LRL,LS,RS,SL,SR,SL,RR,LL,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},\ldots,\mathbf{P}_{n}\) using interpolation with Dubins paths [12; 13]. The initial and final angles, \(\theta_{0}\) and \(\theta_{n}\), are assigned, while the other intermediate angles \(\theta_{j}\), for \(j=1,\ldots,n-1\), are free and thus are unknowns of the problem.