Markov-Dubins Path via Optimal Control Theory

TL;DR

This paper revisits the Markov-Dubins path problem using optimal control theory, revealing new types of solutions, including abnormal controls, and proposes a numerical method for path computation.

Contribution

It provides a new analysis of the Markov-Dubins problem with optimal control, identifying abnormal solutions and establishing their validity, along with a numerical approach.

Findings

Abnormal control solutions exist and are valid.

Feasible paths satisfy the Pontryagin maximum principle.

Numerical method effectively computes Markov-Dubins paths.

Abstract

Markov-Dubins path is the shortest planar curve joining two points with prescribed tangents, with a specified bound on its curvature. Its structure, as proved by Dubins in 1957 nearly 70 years after Markov posed the problem of finding it, is elegantly simple: a selection of at most three arcs are concatenated, each of which is either a circular arc of maximum (prescribed) curvature or a straight line. The Markov-Dubins problem and its variants have since been extensively studied in practical and theoretical settings. A reformulation of the Markov-Dubins problem as an optimal control problem was subsequently studied by various researchers using the Pontryagin maximum principle and additional techniques, to reproduce Dubins' result. In the present paper, we study the same reformulation, and apply the maximum principle, with new insights, to derive Dubins' result again. We prove that abnormal control solutions, which were ruled out in previous studies, do exist. We characterize them as a concatenation of at most two circular arcs and show that they are also solutions of the normal problem. More importantly, we prove that any feasible path of the types mentioned above is a stationary solution, i.e., that it satisfies the Pontryagin maximum principle. We propose a numerical method for computing Markov-Dubins path. We illustrate the theory and the numerical approach by three qualitatively different examples.

Equations

$$\kappa (t)=\Vert \ddot{z}(t)\Vert ,\text{mbox}with\Vert \dot{z}(t)\Vert =1,$$

$${\int }_0^{t_f}\Vert \dot{z}(t)\Vert dt=t_f,$$

$$\mbox{(P)}\left\{\begin{array}[]{rl}\min&\ t_{f}\\[5.69054pt] \mbox{s.t.}&\ z(0)=p_{0},\ z(t_{f})=p_{f},\\[5.69054pt] &\ \dot{z}(0)=v_{0},\ \dot{z}(t_{f})=v_{f},\\[5.69054pt] &\ \|\ddot{z}(t)\|\leq a\,,\ \ \|\dot{z}(t)\|=1\,,\mbox{ for a.e. }t\in[0,t_{f}]\,,\end{array}\right.$$

$$\Vert \ddot{z}{\Vert }^2={\ddot{x}}^2+{\ddot{y}}^2={\dot{\theta }}^2.$$

$$\mbox{(Pc)}\left\{\begin{array}[]{rll}\min&\ \displaystyle t_{f}=\int_{0}^{t_{f}}(1)\,dt&\\[11.38109pt] \mbox{s.t.}&\ \dot{x}(t)=\cos\theta(t)\,,&x(0)=x_{0}\,,\ x(t_{f})=x_{f}\,,\\[5.69054pt] &\ \dot{y}(t)=\sin\theta(t)\,,&y(0)=y_{0}\,,\ y(t_{f})=y_{f}\,,\\[5.69054pt] &\ \dot{\theta}(t)=u(t)\,,&\theta(0)=\theta_{0}\,,\ \theta(t_{f})=\theta_{f}\,,\\[5.69054pt] &&|u(t)|\leq a\,,\mbox{ for a.e. }t\in[0,t_{f}]\,.\end{array}\right.$$

$$H(x,y,\theta ,{\lambda }_0,{\lambda }_1,{\lambda }_2,{\lambda }_3,u):={\lambda }_0+{\lambda }_1\cos \theta +{\lambda }_2\sin \theta +{\lambda }_{3}u,$$

$$H[t]:=H(x(t),y(t),\theta (t),{\lambda }_0,{\lambda }_1(t),{\lambda }_2(t),{\lambda }_3(t),u(t)).$$

$${\dot{\lambda }}_1(t)=-H_x[t]=0,$$

$${\dot{\lambda }}_2(t)=-H_y[t]=0,$$

$${\dot{\lambda }}_3(t)=-H_{\theta }[t]={\lambda }_1(t)\sin \theta (t)-{\lambda }_2(t)\cos \theta (t),$$

$$\rho :=\sqrt{{\overline{\lambda }}_1^2+{\overline{\lambda }}_2^2},\tan \varphi :=\frac{{\overline{\lambda }}_2}{{\overline{\lambda }}_1}.$$

$$H[t]={\lambda }_0+\rho \cos (\theta (t)-\varphi )+{\lambda }_3(t)u(t)$$

$${\dot{\lambda }}_3(t)=\rho \sin (\theta (t)-\varphi ).$$

$$u(t)\in \underset{\Vert v\Vert \le a}{\mathrm{argmin}}H(x(t),y(t),\theta (t),{\lambda }_0,{\lambda }_1(t),{\lambda }_2(t),{\lambda }_3(t),v),$$

$$H[t]=0.$$

$$u(t)\in \underset{\Vert v\Vert \le a}{\mathrm{argmin}}{\lambda }_3(t)v$$

$$u(t)=\left\{\begin{array}[]{ll}\ \ a\,,&\mbox{if}\ \lambda_{3}(t)<0\,,\\[8.53581pt] -a\,,&\mbox{if}\ \lambda_{3}(t)>0\,,\\[8.53581pt] \mbox{undetermined}\,,&\mbox{if}\ \lambda_{3}(t)=0\,.\end{array}\right.$$

$${\lambda }_3(t)u(t)+\rho \cos (\theta (t)-\varphi )+{\lambda }_0=0.$$

$${\dot{\lambda }}_3^2(t)+{\left(a|{\lambda }_3(t)|-{\lambda }_0\right)}^2={\rho }^2.$$

$${\dot{\lambda }}_3^2(t)={\rho }^2{\sin }^2(\theta (t)-\varphi )={\rho }^2-{\rho }^2{\cos }^2(\theta (t)-\varphi ).$$

$$\rho \cos (\theta (t)-\varphi )=a|{\lambda }_3(t)|-{\lambda }_0.$$

$${\left({\lambda }_3\pm \frac{{\lambda }_0}{a}\right)}^2+\frac{{\dot{\lambda }}_3^2}{a^2}=\frac{{\rho }^2}{a^2},$$

$$|{\lambda }_3(t)|=\frac{1}{a}\left[\rho \cos (\theta (t)-\varphi )+{\lambda }_0\right],\text{mbox}fora.e.t\in [{\zeta }_3,{\zeta }_4]\subset [0,t_f].$$

$$a^2{\lambda }_3^2+{\dot{\lambda }}_3^2={\rho }^2.$$

$$\zeta =\gamma +\pi ,$$

$$\xi =\gamma -2\delta +2(\beta +c/\sin \delta ),$$

$$\xi (\delta )=\gamma +\pi -4\delta +\frac{4(1-\cos \delta )}{\sin \delta },\text{mbox}for\delta \in (0,\pi /2].$$

$$\eta (\delta )=4\left(\delta -\frac{\sin \delta }{1+\cos \delta }\right),\text{mbox}for\delta \in [0,\pi /2].$$

$${\eta }^{\prime }(\delta )=4\left(1-\frac{1}{1+\cos \delta }\right),\text{mbox}for\delta \in [0,\pi /2],$$

$$\tau ={\tau }_0+2(\pi +\gamma )+{\tau }_3.$$

$$s_{c_0}=(0,0),s_{c_1}=(2,0),s_{c_2}=2(1+e^{-i\gamma }),s_{c_3}=2(2+e^{-i\gamma }),$$

$$s_1=2+e^{-i\gamma },s_2=3+2e^{-i\gamma },$$

Full text

Key words. Markov-Dubins path, Bounded curvature, Optimal control, Singular control, Bang–bang control, Abnormal optimal control problem.

AMS subject classifications. Primary 49J15, 49K15  Secondary 65K10, 90C30

1 Introduction

The problem of finding the shortest planar path of bounded curvature between two prescribed end-points and tangents in the plane was posed in 1889 by Andrey Andreyevich Markov [29], in the context of railway design (also see [27]). Back then, Markov studied only some of the specific instances of this problem. Nearly 70 years later (in 1957), Lester Eli Dubins [15] published a general solution to the problem for the first time, using geometric arguments. Dubins path, as coined by many authors in the literature but which we refer to here as Markov-Dubins path in recognition of Markov’s earlier contribution in [29], is the shortest $C^{1}$ and piecewise-$C^{2}$ curve that is a concatenation of circular subarcs and a straight line. Suppose that a circular arc is represented by $C$ and a straight line segment by $S$. Dubins’ elegant solution asserts that the sequence of concatenated arcs in such a shortest path can be of type $CSC$, $CCC$, or a subset thereof.

Markov-Dubins path and its variants have since been extensively studied for optimal path planning of uninhabited aerial vehicles (UAVs) [18, 19, 42] and robots [39]. They have also been used for tunnelling in underground mines, where it is paramount to minimize the cost of excavating and operating a tunnel [11, 12]. Markov-Dubins path and its generalizations have been an active area of research for decades now [1, 3, 6, 21, 36, 37, 38, 17, 13, 31, 34, 9, 10, 33, 35].

A straightforward exemplification of Markov-Dubins path is the shortest path of a car (modelled as a point mass) which goes only forwards at unit speed under the same constraints described above. In 1990, Reeds and Shepp [33] considered a car which goes not only forwards but also backwards, as an extension of the Markov-Dubins problem. This extension clearly allows reversals of the path, which constitute cusps. Therefore, the shortest curve for the Reeds-Shepp car is no longer necessarily $C^{1}$, and the results are somewhat richer. In 1991, Boissonnat, Cérézo and Leblond [9] (also see [10]) used optimal control theory, as well as perturbation analysis of the solution trajectories, to derive the same results as those obtained by Reeds and Shepp. Sussmann and Tang [38] independently did the same in 1991, by using geometric optimal control theory and control synthesis.

In both [9, Lemma 5] and [38, Lemma 2], the optimal control problem associated with the Reeds-Shepp car is proved to be normal, i.e., the multiplier of the objective functional is positive (non-zero). In [9], however, normality is carried over to the case of the forward-moving car as a special case of the Reeds-Shepp car, and the results that are derived for the Reeds-Shepp car are reduced to those of Dubins. Under the assumption that the optimal control problem is normal, the optimal curve types one gets indeed overlap with those of the result of Dubins; however, it turns out that the problem may very well be abnormal, i.e., the multiplier of the objective functional may become zero. Abnormality of the Markov-Dubins problem is observed in [38, Remark 11]; however, abnormal solutions have not been studied explicitly up to now. Therefore, there is need to characterize abnormal solutions adequately for completeness of the optimal control approach.

In the present paper, we formulate the Markov-Dubins problem as a time-optimal control problem as in [9] and [38] and apply the Pontryagin maximum principle. We prove that there exist abnormal, as well as normal, solutions to the Markov-Dubins problem, and we characterize these. We carry out perturbation analysis of the solution trajectories, for the normal and abnormal cases, so as to get the results reported by Dubins.

First, as in [9] and [38], we show that the optimal control trajectories are comprised of bang and singular arcs such that a bang arc is associated with a circular arc $C$ and a singular arc with a straight line segment $S$. Then we derive a differential equation for the switching function and construct the phase portraits of this differential equation for the normal and abnormal cases (see Figures 1 and 2). These phase portraits exhibit markedly different phase plane trajectories from one another, which help with the characterization of the possible types of solutions.

From a simple lemma (Lemma 1), as well as the abnormal phase portrait (Figure 2), it becomes evident that an abnormal solution curve cannot contain a singular subarc. Next, by using perturbation analysis, we show that an abnormal optimal solution curve must be either of type $C$ or $CC$ (Lemma 7). By using a similar perturbation analysis, we also show that a normal optimal solution curve cannot be of type $CCCC$ (Lemma 8). The latter analysis is akin to that in [9]; however, the tools and details of our working are different. A combination of all these results reproduce the earlier result given by Dubins (Theorem 1). In other words, by using optimal control theory and perturbation analysis, we provide a (full) alternative proof of Dubins’ result.

In another new result (Theorem 2), we state that any feasible path, i.e., any path satisfying the constraints of the optimal control formulation of the Markov-Dubins problem, which is of type $CSC$, $CCC$, or a subset thereof, is a stationary solution, i.e., it satisfies the Pontryagin maximum principle. Stationarity of feasible solutions has computational implications as pointed in the next paragraph. The result in Corollary 1 illustrates that abnormal Markov-Dubins path is not rare at all. Corollary 2, on the other hand, states that any abnormal path is a normal path, but not vice versa.

Since the solution structure for the optimal control problem is a concatenation of bang and singular arcs, we parameterize the problem with respect to the unknown terminal time and the switching times at which the solution curve switches from one subarc to the other. We propose a model for switching-time computation in a similar fashion to those in [25, 30], which converts the Markov-Dubins problem, which is an infinite-dimensional optimization problem, to an equivalent finite-dimensional one. The transformed problem can then be solved using standard optimization methods and software. It is well-known that most iterative methods for optimization would in general converge to a stationary solution, but not necessarily to a locally optimal one, let alone a globally optimal one. So, by using standard optimization software, one would hope to get at best, by virtue of Theorem 2, one of the usually many feasible solutions of the Markov-Dubins problem.

Three example Markov-Dubins problems are studied. In the first two, we construct all of the (normal) stationary solutions. For the Markov-Dubins path of Example 1, we illustrate the construction of the switching function and the phase plane trajectories. In Example 2, we draw six of all seven stationary curves. Example 3 illustrates four stationary curves, including the optimal one, the Markov-Dubins path, which is abnormal.

A realistic generalization of the Markov-Dubins problem is the requirement that the path passes through a number of prescribed intermediate points, giving rise to an interpolation problem. Although this generalization is beyond the scope of the present paper, the material is presented in such a way that it can be conveniently/simply extended to study what we call the Markov-Dubins interpolation problem. The particular numerical approach we propose in this paper constitutes a crucial building block of a numerical method for this extension.

The paper is organized as follows. In Section 2, we describe the Markov-Dubins problem and its reformulation as a time-optimal control problem. In Section 3, we provide the preliminary results, including the abnormal and normal solutions, leading to Dubins’ theorem. The stationarity results are presented in Section 4. In Section 5, we describe the numerical approach and present the numerical examples and experiments. Section 6 concludes the paper, with a discussion and short descriptions of further work.

2 Markov-Dubins Problem

Markov-Dubins path is the shortest $C^{1}$ and piecewise-$C^{2}$ curve $z:[0,t_{f}]\longrightarrow{\rm{I\ \kern-5.39993ptR}}^{2}$ between two prescribed oriented points $p_{0}$ and $p_{f}$ at [math] and $t_{f}$, respectively, in the plane, where the slopes at $p_{0}$ and $p_{f}$ are also prescribed, such that the curvature of the path $z(t)$ at almost every (a.e.) point is not greater than $a>0$. Note that the parameter $t_{f}$ is unknown, and so it is also to be determined.

Recall that the curvature $\kappa(t)$ of a $C^{2}$ curve, parameterized with respect to its length, is defined as the length of its acceleration vector; in other words,

[TABLE]

where $\dot{z}=dz/dt$, $\ddot{z}=d^{2}z/dt^{2}$, and $\|\cdot\|$ is the Euclidean norm. So, it is required that $\kappa(t)\leq a$, for a.e. $t\in[0,t_{f}]$. In practical terms, if we consider, for example, the motion of a vehicle in the plane, $1/\kappa(t)$ is nothing but the (instantaneous) turning radius. So, in the Markov-Dubins problem, the turning radius is constrained to be at least $1/a$. Finding the shortest curve requires minimization of the arc-length functional

[TABLE]

where we have used $\|\dot{z}(t)\|=1$, for a.e. $t\in[0,t_{f}]$. Note that this reconfirms parameterization of the curve with respect to its length. The Markov-Dubins problem can then be expressed as

[TABLE]

where $\|v_{0}\|=\|v_{f}\|=1$. Problem (P) can equivalently be cast as an optimal control problem as follows. Let $z(t):=(x(t),y(t))\in{\rm{I\ \kern-5.39993ptR}}^{2}$, with $\dot{x}(t):=\cos\theta(t)$ and $\dot{y}(t):=\sin\theta(t)$, where $\theta(t)$ is the angle the velocity vector $\dot{z}(t)$ of the curve $z(t)$ makes with the horizontal. These definitions verify that $\|\dot{z}(t)\|=1$. Moreover,

[TABLE]

Therefore, $|\dot{\theta}(t)|$ is nothing but the curvature. In fact, $\dot{\theta}(t)$ itself, which can be positive or negative, is referred to as the signed curvature. For example, consider a vehicle travelling along a circular path. If $\dot{\theta}(t)>0$ then the vehicle travels in the counter-clockwise direction, i.e., it turns left, and if $\dot{\theta}(t)<0$ then the vehicle travels in the clockwise direction, i.e., it turns right.

Let $u(t):=\dot{\theta}(t)$. Suppose that the directions at the points $p_{0}$ and $p_{f}$ are denoted by the angles $\theta_{0}$ and $\theta_{f}$, respectively. Problem (P) can then be re-written as a time-optimal (or minimum-time) control problem, where $x$, $y$ and $\theta$ are the state variables and $u$ the control variable:

[TABLE]

3 Markov-Dubins Curves

In this section, we apply the Pontryagin maximum principle to Problem (Pc) and ultimately reproduce the result obtained by Dubins [15].

3.1 Bang–bang and singular arcs

Define the Hamiltonian function as in [32] for Problem (Pc) as:

[TABLE]

where $\lambda_{0}$ is a scalar (multiplier) parameter and $\lambda_{i}:[0,t_{f}]\to{\rm{I\ \kern-5.39993ptR}}$, $i=1,2,3$, are the adjoint (or costate) variables. Let

[TABLE]

The adjoint variables are required to satisfy

[TABLE]

where $H_{x}=\partial H/\partial x$, etc. By these definitions, the state and adjoint variables verify a Hamiltonian system in that, in addition to (2)–(4), one has $\dot{x}(t)=H_{\lambda_{1}}[t]$, $\dot{y}(t)=H_{\lambda_{2}}[t]$ and $\dot{\theta}(t)=H_{\lambda_{3}}[t]$. Note that (2)–(3) imply that $\lambda_{1}(t)=\overline{\lambda}_{1}$ and $\lambda_{2}(t)=\overline{\lambda}_{2}$ for all $t\in[0,t_{f}]$, where $\overline{\lambda}_{1}$ and $\overline{\lambda}_{2}$ are constants.

Define new constants

[TABLE]

Then (1) and (4) can respectively be re-written as

[TABLE]

and

[TABLE]

Next we state the Pontryagin maximum principle [32, Theorem 1] for our setting as follows. Suppose that $x,y,\theta\in W^{1,\infty}(0,t_{f};{\rm{I\ \kern-5.39993ptR}})$, $u\in L^{\infty}(0,t_{f};{\rm{I\ \kern-5.39993ptR}})$, and $t_{f}\in[0,M)$, where $M$ is large enough so that $t_{f}<M-\varepsilon$ with $\varepsilon>0$, solve Problem (Pc). Then there exist a number $\lambda_{0}\geq 0$ and functions $\lambda_{i}\in W^{1,\infty}(0,t_{f};{\rm{I\ \kern-5.39993ptR}})$, $i=1,2,3$, such that $\lambda(t):=(\lambda_{0},\lambda_{1}(t),\lambda_{2}(t),\lambda_{3}(t))\neq\bf 0$, for every $t\in[0,t_{f}]$, and, in addition to the state differential equations and other constraints given in Problem (Pc) and the adjoint differential equations (2)–(3) and (7), the following conditions hold:

[TABLE]

Using the definition in (1), (8) can more simply be written as

[TABLE]

which yields the optimal control as

[TABLE]

Furthermore, (6) and (9) give

[TABLE]

The control $u(t)$ to be chosen for the case when $\lambda_{3}(t)=0$ for a.e. $t\in[\zeta_{1},\zeta_{2}]\subset[0,t_{f}]$ is referred to as singular control, because (10) does not yield any further information. On the other hand, when $\lambda_{3}(t)\neq 0$ for a.e. $t\in[0,t_{f}]$, i.e., it is possible to have $\lambda_{3}(t)=0$ only for isolated values of $t$, the control $u(t)$ is said to be nonsingular. It should be noted that, if $\lambda_{3}(\tau)=0$ only at an isolated point $\tau$, the optimal control at this isolated point can be chosen as $u(\tau)=-a$ or $u(\tau)=a$, conveniently. If the control $u(t)$ is nonsingular, it will take on either the value $-a$ or $a$, the bounds on the control variable. In this case, the control $u(t)$ is referred to as bang–bang. Since the sign of $\lambda_{3}(t)$ determines the value of the optimal control $u(t)$, $\lambda_{3}$ is referred to as the switching function.

Lemma 1** (Normality of Singular Control)**

Suppose that the optimal control $u(t)$ for Problem (Pc) is singular over an interval $[\zeta_{1},\zeta_{2}]\subset[0,t_{f}]$. Then $\rho=\lambda_{0}>0$.

Proof. Suppose that the optimal control $u(t)$ is singular for a.e. $t\in[\zeta_{1},\zeta_{2}]\subset[0,t_{f}]$. Then $\lambda_{3}(t)=\dot{\lambda}_{3}(t)=0$ for a.e. $t\in[\zeta_{1},\zeta_{2}]$. From (7), $\sin(\theta(t)-\phi)=0$, which implies that $\cos(\theta(t)-\phi)=1$ or $-1$. Then, substituting $\lambda_{3}(t)=0$ and $\cos(\theta(t)-\phi)=-1$ into (12), one gets $\rho=\lambda_{0}\geq 0$. It should be noted that $\cos(\theta(t)-\phi)=1$ yields $\rho=-\lambda_{0}\geq 0$, or $\lambda_{0}\leq 0$, which is not allowed by the maximum principle, unless $\lambda_{0}=0$. Suppose that $\rho=\lambda_{0}=0$. Then, since $\lambda_{3}(t)=0$, the adjoint variable vector $\lambda(t)={\bf 0}$, which contradicts the maximum principle. Therefore $\lambda_{0}>0$. $\Box$

Remark 1

The problems that yield $\lambda_{0}=0$ are referred to as abnormal in the optimal control theory literature, for which the necessary conditions in (8)–(9) are independent of the objective functional $t_{f}$ and therefore insufficiently informative. The problems that yield $\lambda_{0}>0$ are referred to as normal. Lemma 1 above asserts that if the optimal path contains a singular arc, then Problem (Pc) is normal. $\Box$

Lemma 2** (Singularity and Straight Line Segments)**

Suppose that the optimal control $u(t)$ for Problem (Pc) is singular over an interval $[\zeta_{1},\zeta_{2}]\subset[0,t_{f}]$. Then $\theta(t)$ is constant, i.e., $u(t)=0$.

Proof. Suppose that the optimal control $u(t)$ is singular, i.e., $\lambda_{3}(t)=0$, for a.e. $t\in[\zeta_{1},\zeta_{2}]\subset[0,t_{f}]$. Recall by Lemma 1 that $\rho>0$. The rest of the proof can be given in two alternative ways:

(i) For a.e. $t\in[\zeta_{1},\zeta_{2}]$: since $\lambda_{3}(t)=0$, one also has that $\dot{\lambda}_{3}(t)=0$; in other words, from (7), $\sin(\theta(t)-\phi)=0$, which implies that $\theta(t)$ is constant, i.e., $\dot{\theta}(t)=u(t)=0$.

(ii) Substituting $\rho=\lambda_{0}>0$ from Lemma 1 and $\lambda_{3}(t)=0$ into (12), one gets $\cos(\theta(t)-\phi)=-1$, which implies that $\theta(t)$ is constant, i.e., $\dot{\theta}(t)=u(t)=0$, for a.e. $t\in[\zeta_{1},\zeta_{2}]$. $\Box$

Remark 2

From (11) and Lemma 2, one can simply write $u(t)=-a\,\operatorname*{sgn}(\lambda_{3}(t))$, a.e. $t\in[0,t_{f}]$. $\Box$

Lemma 3

The adjoint variable $\lambda_{3}$ for Problem (Pc) solves the differential equation

[TABLE]

Proof. From (7),

[TABLE]

Using $u(t)=-a\,\operatorname*{sgn}(\lambda_{3}(t))$ in (12), one gets

[TABLE]

Substituting this into the right-hand side of (14) and rearranging give (13). $\Box$

In the rest of the paper, we will at times not show dependence of variables on $t$ for clarity of presentation.

Remark 3** (Normal Phase Portrait)**

Note that the differential equation (13) is given in terms of the phase variables $\lambda_{3}$ and $\dot{\lambda}_{3}$, and can be put into the form

[TABLE]

where the “$+$” sign in the first square term stands for the case when $\lambda_{3}<0$ and the “$-$” sign for $\lambda_{3}>0$. Equation (15) clearly tells us that, for $\lambda_{0}>0$, the trajectories in the phase plane for $\lambda_{3}$ (the \lambda_{3}$$\dot{\lambda}_{3}-plane) will be pieces or concatenations of pieces of concentric ellipses centred at $\lambda_{3}=-\lambda_{0}/a$ (for $u(t)=a$) and $\lambda_{3}=\lambda_{0}/a$ (for $u(t)=-a$), as shown in Figure 1. Based on the phase plane diagram, also referred to as the phase portrait, of $\lambda_{3}$ depicted in Figure 1, the following observations are made.

• (i)

When $\rho>\lambda_{0}>0$ the trajectories are concatenations of (pieces of) ellipses, examples of which are shown by (dark blue) solid curves in Figure 1. The ellipses are concatenated at the switching points $(0,\sqrt{\rho^{2}-\lambda_{0}^{2}})$ and $(0,-\sqrt{\rho^{2}-\lambda_{0}^{2}})$, where the value of the bang–bang control $u(t)$ switches from $a$ to $-a$ or from $-a$ to $a$, respectively. The concatenated ellipses cross the $\lambda_{3}$-axis at two points, $(\lambda_{0}+\rho)/a$ and $-(\lambda_{0}+\rho)/a$. One can promptly deduce from the diagram that if the bang–bang control has two switchings, the second arc must have a length strictly greater than $\pi/a$. The diagram, however, does not tell as to how many switchings optimal control must have.

• (ii)

Recall, by Lemma 1, that $\rho=\lambda_{0}>0$ for singular control. The case when only a part of the trajectory is singular, referred to as a bang–singular trajectory, is represented by the two unique (red) dashed elliptic curves in Figure 1. Note that singular control takes place only at the origin $(0,0)$ of the phase plane. At any other point, the control trajectory is of bang–bang type.

• (iii)

For the case when $0<\rho<\lambda_{0}$, example elliptic trajectories are shown with (black) dotted curves in Figure 1. The trajectories cross the $\lambda_{3}$-axis at four distinct points $(\lambda_{0}\pm\rho)/a$ and $-(\lambda_{0}\pm\rho)/a$; however, they no longer intercept the $\dot{\lambda}_{3}$-axis; therefore they represent bang–bang control with no switchings, i.e., either $u(t)=a$ for all $t\in[0,t_{f}]$ or $u(t)=-a$ for all $t\in[0,t_{f}]$. $\Box$

Lemma 4

Suppose that optimal control $u(t)$ for Problem (Pc) is nonsingular over an interval $[\zeta_{3},\zeta_{4}]\subset[0,t_{f}]$. Then

[TABLE]

Proof. Substitution of  $u(t)=-a\,\operatorname*{sgn}(\lambda_{3}(t))$  into (12) and rearranging yield the required expression. $\Box$

Lemma 5** (Nonsingular Curves)**

*Consider Problem (Pc) and the necessary conditions of optimality for it.

1. (a)

If $\rho=0$, then $\lambda_{0}>0$ and either $u(t)=a$ or $u(t)=-a$, for all $t\in[0,t_{f}]$. 2. (b)

If $\rho>0$ and $\rho\neq\lambda_{0}$, then $\lambda_{0}\geq 0$ and $u(t)$ is of bang–bang type.

Proof. (a) Suppose that $\rho=0$. Then, from (12) and Remark 2, $-\lambda_{0}=\lambda_{3}(t)\,u(t)=-a\,|\lambda_{3}(t)|\leq 0$, i.e., $\lambda_{0}\geq 0$. Suppose further that $\lambda_{0}=0$. Then we have $\lambda_{3}(t)\,u(t)=0$, which means that either $\lambda_{3}(t)=0$ or $u(t)=0$, for a.e. $t\in[0,t_{f}]$. If $\lambda_{3}(t)=0$, then the vector of adjoint variables $\lambda(t)={\bf 0}$, for a.e. $t\in[0,t_{f}]$, which is not permitted by the Pontryagin maximum principle. Therefore $\lambda_{3}(t)\neq 0$. Then, by (11), $u(t)\neq 0$, either. Therefore $\lambda_{0}\neq 0$, namely that $\lambda_{0}>0$. Now that $|\lambda_{3}(t)|=\lambda_{0}/a$, $\lambda_{3}(t)$ is a nonzero constant, i.e., $u(t)$ is either $a$ or $-a$, for all $t\in[0,t_{f}]$.

(b) Suppose that $\rho>0$ and $\rho\neq\lambda_{0}$. We need to examine the normal and abnormal cases, separately.

(i) The normal case, $\lambda_{0}>0$ : We have that $0<\rho\neq\lambda_{0}>0$. The contrapositive of Lemma 1 states that if $0<\rho\neq\lambda_{0}>0$ or $\rho=\lambda=0$ then optimal control is bang–bang, which furnishes the proof for this case.

(ii) The abnormal case, $\lambda_{0}=0$ : Equation (13) reduces to

[TABLE]

If $\lambda_{3}=0$, then, by Equation (17), $\dot{\lambda}_{3}=\pm\rho\neq 0$, implying that singular optimal control is not possible. Therefore, the optimal control is of bang–bang type. $\Box$

Remark 4

The constant optimal control in Lemma 5(a), $u(t)=a$ or $u(t)=-a$, and the associated $|\lambda_{3}(t)|=\lambda_{0}/a$, can be viewed graphically as the limiting case when $\rho\to 0$ in Figure 1. $\Box$

Remark 5** (Abnormal Phase Portrait)**

Lemma 5(b) implies that the optimal control formulation of the Markov-Dubins problem (Pc) can be abnormal, i.e., $\lambda_{0}=0$ is possible. The phase portrait of the dynamical system in (17), which is depicted in Figure 2 for various values of $\rho$, is now comprised of concentric ellipses centred at $(0,0)$, crossing the $\dot{\lambda}_{3}$-axis at $\rho$ and $-\rho$, where switchings between $u(t)=a$ and $u(t)=-a$ may occur, giving rise to bang–bang control.

The phase portrait also implies that the length of any bang-arc is at most $\pi/a$. If the bang–bang control has two switchings then the length of the second arc is exactly $\pi/a$. The portrait does not actually tell as to what the maximum number of switchings could be. We will answer this question later in Lemma 7. $\Box$

We will refer to a solution to Problem (Pc) with $\lambda_{0}=0$ as an abnormal optimal solution, and a solution with $\lambda_{0}>0$ a normal optimal solution.

By Remarks 3–5, an optimal path, normal or abnormal, will in general be a concatenation of straight lines (i.e., singular arcs, where $u(t)=0$) and circular arcs (i.e., nonsingular arcs, where $u(t)=a$ or $-a$). In a solution trajectory, we will denote a straight line segment by an $S$ and a circular arc segment of curvature $a$ (or, turning radius $1/a$) by a $C$, resulting in descriptions of optimal paths to be of type, for example, $CSCC\cdots$, $SCS\cdots$, etc.

Lemma 6** (Straight Line Segment)**

If an optimal path for Problem (Pc) contains a straight line segment $S$, then it is of type $CSC$, $CS$, $SC$ or $S$.

Proof. It suffices to show that an optimal path cannot contain a curve segment of any of the types $SCS$, $SCC$, and $CCS$. Suppose that an optimal path contains the line segment $S$. Then, the switching function $\lambda_{3}$ of an optimal path containing $S$ will follow the (red) dashed trajectory in the phase portrait in Figure 1, since it is the only trajectory which passes through the origin, where $\lambda_{3}=\dot{\lambda}_{3}=0$. Suppose that the optimal path contains a curve of type $SCS$. Then one traces one of the two dashed ellipses passing through the origin in the phase portrait, and comes back to the origin, meaning that one whole ellipse has been traced. In other words, the circular segment $C$ completes one full cycle, which obviously is not optimal since a full circular arc is in itself redundant. Therefore, the optimal path cannot contain a curve of type $SCS$. Through similar arguments (again using the non-optimality of a full circular segment), one can easily conclude that a curve of type $SCC$ or type $CCS$ in the optimal path is not optimal either, and so an optimal path cannot contain them. $\Box$

3.2 Abnormal optimal solution

By Lemmas 1 and 5 (also see Remark 5), only a bang–bang optimal solution can be abnormal. We characterize these solutions in Lemma 7 below. In what follows, we refer to an abnormal (bang–bang) solution to Problem (Pc) as an abnormal optimal path.

Lemma 7** (Abnormal Markov-Dubins Curves)**

An abnormal optimal path for Problem (Pc) is either of type $CC$ or $C$, with respective lengths of at most $2\pi/a$ and $\pi/a$.

Proof. Suppose that one has an abnormal solution to Problem (Pc). Then $\lambda_{0}=0$ and, by Lemma 1, optimal control is of bang–bang type. Suppose further that optimal control has two switchings, i.e., the optimal path in the $xy$-plane is a concatenation of three circular arcs. For simplicity, let $a=1$. Without loss of generality, suppose that the optimal bang–bang control takes on the values, $1$, $-1$ and $1$, sequentially. Recall from Remark 5 that the second bang-arc is a semi-circle and so is of length $\pi$.

Then a general configuration of the arcs will be like the one shown in Figure 3, where the initial and terminal points $z_{0}$ and $z_{f}$ and the directions at these points are as indicated. It should be noted that the given configuration is general enough, as the only other configuration which is different from the one shown is the mirror image of the diagram about the line joining the centres of the circles, corresponding to the bang-bang control sequence of $-1$, $1$ and $-1$, instead. In Figure 3, the angle $\delta$ is chosen small enough so that the (blue) dashed curve, which goes through the switching points at $(1,0)$ and $(3,0)$, is a part of the three concatenated circular arcs from $z_{0}$ to $z_{f}$. Note that $z_{0}$ and $z_{f}$ can be placed only in the lower halves of the respective circles that they belong to, because, again by Remark 5, the length of any bang-arc can be at most $\pi$. We will show that the trajectory shown by the (blue) dashed curve is not the shortest path, in that the “perturbed” trajectory, which follows the (red) solid curve depicted in Figure 3, is shorter.

The length of the dashed curve from $z_{0}$ to $z_{f}$ is simply given by

[TABLE]

where $\gamma$ is the sum of the lengths of the circular arcs from $z_{0}$ to $(1,0)$ and from $(3,0)$ to $z_{f}$.

The solid curve in the perturbed trajectory is composed by concatenating two straight line segments and a circular arc of radius $1$, in a symmetric fashion, as shown. It is clear from the geometry that the length of each of the two straight line segments is $c\,/\sin\delta$ and the length of the circular segment is $2\,\beta$, making the length of the solid curve $2\,(\beta+c\,/\sin\delta)$. The length of the perturbed curve can then be written as

[TABLE]

where $\beta=\pi/2-\delta$ and $c=(4-2\,\cos\delta-2\,\sin\beta)/2$, from the given geometry. Substituting the expressions for $c$ and $\beta$, using $\sin\beta=\cos\delta$, and rearranging further give the length of the perturbed curve as a function of $\delta$ as

[TABLE]

Let $\eta(\delta):=\zeta-\xi(\delta)$. Substitutions and trigonometric manipulations yield

[TABLE]

Note that as $\delta\to 0^{+}$, $\eta(\delta)=\eta(0)=0$, and so $\xi(0)=\zeta$. This is in line with the geometric observation in Figure 3 that as $\delta\to 0^{+}$ the solid curve approaches the dashed curve on the upper half of the middle circle. Note also that

[TABLE]

with $\lim_{\delta\to 0^{+}}\eta^{\prime}(\delta)=\eta^{\prime}(0)=2$, and that $\eta^{\prime}(\delta)=-\xi^{\prime}(\delta)>0$ for all $\delta\in[0,\pi/2)$. These imply, along with $\xi(0)=\zeta$, that $\xi(\delta)<\zeta$, for all $\delta\in(0,\pi/2)$, furnishing the fact that an abnormal optimal path cannot be of type $CCC$. This leaves the types $C$ and $CC$ as the only candidates.

An abnormal optimal path of type $C$ will necessarily have a length not greater than $\pi$, as otherwise, by Remark 5, one has to switch to another circular arc. By the same argument, an abnormal optimal path of type $CC$ will necessarily have a length not greater than $2\pi$. $\Box$

3.3 Normal nonsingular optimal solution

In Lemma 8 below, we state that any path of type CCCC is not optimal for Problem (Pc). It is clear by Lemma 7 that, in the abnormal case, i.e., when $\lambda_{0}=0$, Lemma 8 holds immediately. For the proof of Lemma 8 when $\lambda_{0}>0$ (the normal case), the diagram in Figure 4, where a general configuration for a path of type CCCC and its perturbation are shown, will be used. In the figure, we have set $a=1$ for simplicity. Recall from the phase plane diagram in Figure 1 and Remark 3(i) that if the bang–bang control has two switchings, then the second arc will have a length strictly greater than $\pi$, say $\pi+\gamma$, with $\gamma>0$. A path of type CCCC has three switchings, so the second and third arcs must both have the length $\pi+\gamma$.

Consider the path of type CCCC from $z_{0}$ to $z_{f}$, along the (blue) dashed curve, so the path satisfies the necessary optimality condition given graphically in Figure 1. Let $\tau_{0}$ be the length of the circular arc from $z_{0}$ to $s_{0}$, $\tau_{1}$ the length from $s_{0}$ to $s_{1}$, $\tau_{2}$ the length from $s_{1}$ to $s_{2}$ and $\tau_{3}$ the length from $s_{2}$ to $z_{f}$. Then the length $\tau$ of the path from $z_{0}$ to $z_{f}$ (shown by the dashed curve) with switchings from one circular subarc to another at $s_{0}$, $s_{1}$ and $s_{2}$ is simply given by $\tau=\tau_{0}+\tau_{2}+\tau_{3}+\tau_{3}$, or

[TABLE]

It can be clearly seen in Figure 4 that most labelled points can be obtained from, or can be expressed in terms of, another labelled point by using rotation. For clarity and convenience in manipulations, we will express points in the complex plane and employ complex rotation. For example, $r_{0}$ can be obtained by rotating $s_{0}$ by angle $\delta$ in the clockwise direction about the centre $s_{c_{0}}=(0,0)$ of the leftmost unit circle; in other words, $r_{0}=e^{-i\delta}=\cos\delta-i\,\sin\delta$, which is the complex representation of the planar point $r_{0}=(\cos\delta,-\sin\delta)$. Similarly, $s_{1}$ is obtained by rotating the point $(3,0)$ by angle $\gamma$ about the centre $s_{c_{1}}$ of a unit circle in the clockwise direction, i.e., that $s_{1}=2+e^{-i\gamma}$ or equivalently $s_{1}=(2+\cos\gamma,-\sin\gamma)$. By using the geometry, each labelled point in Figure 4 can then be expressed as follows.

[TABLE]

[TABLE]

[TABLE]

Note that $r_{1}=(r_{c_{1}}+r_{c_{2}})/2$. Then

[TABLE]

Lemma 8** (Non-optimality of $CCCC$ Type)**

Any path of type CCCC is not optimal for Problem (Pc).

Proof. As discussed before, a general configuration for a path of type CCCC is depicted in Figure 4. Recall that the coordinates of the labelled points in Figure 4 have been given (in the complex plane) in (19)–(22).

We will show that the path of type CCCC between the oriented points $z_{0}$ and $z_{f}$ in Figure 4 is not optimal. Note that the point $z_{0}$ can be taken to be anywhere on the lower semi-circle where it currently sits. The perturbation $\delta>0$ is arbitrarily small so that $r_{0}$ is between $z_{0}$ and $s_{0}$. Similarly, the point $z_{f}$ can be taken to be anywhere on the upper semi-circle where it currently resides.

The length $\eta$ of the perturbed path can now be written as a function of $\delta$ as follows.

[TABLE]

where $\xi_{1}$ is the length of the circular path from $r_{0}$ to $r_{1}$, $\xi_{2}$ the length of the circular path from $r_{1}$ to $r_{2}$, and $\beta$ the length of the circular path from $r_{2}$ to $s_{2}$, each of which are functions of $\delta$. Note that

[TABLE]

and so, using (18),

[TABLE]

Then it suffices to show that $\eta^{\prime}(0)<0$. If, however, $\eta^{\prime}(0)=0$, as it will turn out to be the case, it will consequently suffice to show that $\eta^{\prime\prime}(0)<0$. From (23), one has that

[TABLE]

In what follows, we will not always show dependence of $\xi_{1}$, $\xi_{2}$ and $\beta$ on $\delta$, for clarity. It is straightforward to write, from Figure 4, that

[TABLE]

After substitutions and simplifying manipulations, (26)–(28) yield the following equations, respectively.

[TABLE]

When $\delta=0$ (the unperturbed case), Equation (29) is verified by $\cos(\beta)=1$, i.e., $\beta=0$, Equation (30) reduces to $e^{-i\xi_{1}(0)}=-e^{-i\gamma}=e^{-i(\pi+\gamma)}$, i.e., $\xi_{1}(0)=\pi+\gamma$, and Equation (31) to $e^{-i\xi_{2}(0)}=-e^{-i\gamma}=e^{-i(\pi+\gamma)}$, i.e., $\xi_{2}(0)=\pi+\gamma$, which altogether reconfirm (24).

In order to evaluate $\eta^{\prime}(0)$ in (25), we need $\xi_{1}^{\prime}(0)$, $\xi_{2}^{\prime}(0)$ and $\beta^{\prime}(0)$, which can be obtained by differentiating Equations (29)–(31) and substituting the known quantities for the unperturbed case. Differentiating (29) with respect to $\delta$, one gets

[TABLE]

Then substitution of $\delta=\beta=0$ (the unperturbed case) into (32) and the fact that $\sin\gamma\neq 0$ yield

[TABLE]

Differentiation of Equation (30) with respect to $\delta$ gives

[TABLE]

Substitution of $\delta=\beta=0$, $\beta^{\prime}(0)=1$ and $\xi_{1}(0)=\pi+\gamma$, for the unperturbed case, into Equation (34) and manipulations result in

[TABLE]

Similarly, differentiation of Equation (31) with respect to $\delta$ results in

[TABLE]

and the substitution of $\delta=\beta=0$, $\beta^{\prime}(0)=1$ and $\xi_{2}(0)=\pi+\gamma$, and manipulations give

[TABLE]

Now, substituting (33), (35) and (37) into (25), one gets

[TABLE]

As pointed out immediately after (24) above, this necessitates to check if

[TABLE]

to furnish the proof. Now, differentiate the equation in (32) to get

[TABLE]

Substituting into (39) the (unperturbed) quantities $\delta=\beta=0$ and $\beta^{\prime}(0)=1$, and manipulating further, one gets

[TABLE]

Recall that $\gamma>0$, so (40) is well-defined. Next, differentiate (34) to get

[TABLE]

Substituting into (41) $\delta=\beta=0$, $\beta^{\prime}(0)=1$, $\beta^{\prime\prime}(0)=-2\,\cot\gamma$, $\xi_{1}(0)=\pi+\gamma$ and $\xi_{1}^{\prime}(0)=-1$, and manipulating further, one gets

[TABLE]

which yields

[TABLE]

Similarly, differentiation of (36) gives

[TABLE]

Substitution of $\beta^{\prime}(0)=1$, $\beta^{\prime\prime}(0)=-2\,\cot\gamma$, $\xi_{2}(0)=\pi+\gamma$ and $\xi_{2}^{\prime}(0)=1$ into (43), and some lengthy manipulations, result in

[TABLE]

Substitution of (40), (42) and (44) into (38) yields

[TABLE]

for any $\gamma\in(0,\pi/2]$. Therefore, given the fact that $\eta(0)=\tau$ and $\eta^{\prime}(0)=0$, we conclude that $\eta$ is decreasing at $\delta=0$ and, by continuity of $\eta$, the perturbed length $\eta(\delta)<\tau$ for small enough $\delta>0$. This completes the proof. $\Box$

3.4 Dubins’ result

The preceding results (Lemmas 5–8 and the phase plane diagram in Figure 1) obtained by using Pontryagin maximum principle and perturbation of trajectories can now be used to prove Dubins’ result in [15].

Theorem 1** (Markov-Dubins Curves – Dubins [15])**

Any solution of Problem (P), that is, any $C^{1}$ and piecewise-$C^{2}$ shortest path of bounded curvature in the plane between two prescribed endpoints, where the slopes of the path are also prescribed, is of type $CSC$, or of type $CCC$, or a subset thereof. Moreover, if the shortest path is of type $CCC$, then the second circular arc is of length greater than $\pi/a$.

Proof. If the solution is abnormal, i.e., $\lambda_{0}=0$, then by Lemma 7 the shortest path is of type $C$ or $CC$, which in either case is a subset of $CCC$. Suppose that the solution is normal, i.e., $\lambda_{0}\neq 0$. Then the shortest path is either of type

1. (i)

$CSC$, $CS$, $SC$ or $S$, if it contains a straight line segment, by Lemma 6, or 2. (ii)

$CCC$, $CC$ or $C$, by Lemmas 5 and 8.

The last statement of the theorem is proved by using the phase portrait in Figure 1, with $\rho>\lambda_{0}>0$. If the shortest path is of type $CCC$, then three pieces of ellipses in Figure 1 are concatenated, with the second ellipse sweeping an angle greater than $\pi$, completing the proof. $\Box$

4 Stationarity of feasible curves of type $CSC$ or $CCC$

Dubins’ result in Theorem 1 states that if a curve is optimal for Problem (P), or equivalently for Problem (Pc), then that curve is of type $CSC$ or $CCC$, or a subset thereof. It is clear that a solution curve of Problem (Pc) verifies the Pontryagin maximum principle, which furnish necessary conditions of optimality. However, neither [15] nor [9, 38] provide any further explanation as to whether or not any other feasible curve for Problem (Pc), i.e., any other curve that satisfies the constraints of Problem (Pc), which are of the types listed above, would also verify the Pontryagin maximum principle. These feasible curves presumably have a length larger than that of a solution curve of Problem (Pc). In Theorem 2 below, we state that these kinds of feasible solutions indeed verify the necessary conditions of optimality, i.e., they are stationary, or critical, solutions of Problem (P).

In the proof of Theorem 2, we assume that a feasible solution has been provided, in that the times $t_{1}$ and $t_{2}$ at which switchings from one subarc to another occur (note that in general $0\leq t_{1}\leq t_{2}\leq t_{f}$), as well as the terminal time $t_{f}$, which is the length of the curve, are known. The signed curvatures ($u(t)=a$ or $-a$) of the $C$ subarcs of the feasible curve are also given, and so $\theta_{1}:=\theta(t_{1})$ and $\theta_{2}:=\theta(t_{2})$ are also known/easily calculable. Recall from the original problem description that $\theta(0)=\theta_{0}$.

Theorem 2** (Stationarity of Feasible Curves)**

*Any feasible path for Problem (Pc), i.e., any path satisfying the constraints of Problem (Pc), which is of type $CSC$ or $CCC$, or a subset thereof, verifies the Pontryagin maximum principle.

Proof. It suffices to show that, for any feasible curve of type $CSC$ or $CCC$, or a subset thereof, there exist adjoint variables $\lambda_{0}\geq 0$, $\lambda_{1}$, $\lambda_{2}$ and $\lambda_{3}$ such that (12) holds. Recall that the constants $\rho$ and $\phi$ in (12) are defined in (5) in terms of the constant values of $\lambda_{1}$ and $\lambda_{2}$. So the task boils down to finding pertaining $\rho$, $\phi$ and $\lambda_{3}$ for every single possible feasible curve of the given types.

First, suppose that Problem (Pc) is normal, i.e., $\lambda_{0}>0$, and without loss of generality, set $\lambda_{0}=1$. We examine feasible curves of the two basic types, $CSC$ or $CCC$, and their subsets thereof, for the normal case, one by one.

• (a)

Consider a feasible curve of type $CSC$, or of one of the types $CS$, $SC$, and $S$. Recall that along the subarc $S$, $\lambda_{3}(t)=0$, and so $\rho=\lambda_{0}=1$ by Lemma 1. Hence, Equation (12) reduces to $\cos(\theta(t)-\phi)=-1$, with $\theta(t)=\theta_{1}$ constant, which implies that $\phi=\theta_{1}\pm\pi$. Set $\phi=\theta_{1}-\pi$. Then, for a feasible curve of type $CSC$, the adjoint variable, or the switching function, $\lambda_{3}(t)$ can be constructed uniquely, in terms of the feasible solution parameters $t_{1}$, $t_{2}$, $t_{f}$ and $\theta_{1}$, as

[TABLE]

We note that the first equation in (45) can be re-written, by using $u(t)=-a\,\operatorname*{sgn}(\lambda_{3}(t))$, as

[TABLE]

which agrees with (16). The expression in (45) holds for any feasible curve of type $CS$ (where $t_{2}=t_{f}$), type $SC$ (where $t_{1}=0$) or type $S$ (where $t_{1}=0$ and $t_{2}=t_{f}$), too.

• (b)

Consider feasible curves of each of the types $CCC$, $CC$ and $C$, one by one, below.

• (i)

Type $CCC$: This type requires two switchings, so $0<t_{1}<t_{2}<t_{f}$ and that $\lambda_{3}(t_{1})=0$ and $\lambda_{3}(t_{2})=0$, with which Equation (12) yields two equations in the two unknowns $\rho$ and $\phi$; namely, $\rho\,\cos(\theta_{1}-\phi)+1=0$ and $\rho\,\cos(\theta_{2}-\phi)+1=0$, where $\rho>1$ by Remark 3. These two equations result in $\cos(\theta_{1}-\phi)=\cos(\theta_{2}-\phi)$. By Figure 1 and the second statement of Theorem 1, $\theta_{2}-\phi=-(\theta_{1}-\phi)$. Then simple algebraic manipulations provide a unique solution for the constants $\rho$ and $\phi$ as:

[TABLE]

Here, one can easily verify that $\rho>1$, indeed, as follows: since $|\theta_{1}-\theta_{2}|>\pi$ by the second statement of Theorem 1, $-1<\cos\left((\theta_{1}-\theta_{2})/2\right)<0$ and so $-\sec\left((\theta_{1}-\theta_{2})/2\right)>1$. Finally, a unique solution for $\lambda_{3}(t)$, in terms of the feasible solution parameters $t_{1}$, $t_{2}$, $t_{f}$, $\theta_{1}$ and $\theta_{2}$, can be written down for this case as

[TABLE]

With $u(t)=-a\,\operatorname*{sgn}(\lambda_{3}(t))$, Equation (46) becomes

[TABLE]

which agrees with (16).

• (ii)

Type $CC$: This type requires only one switching, so, without loss of generality, let $0<t_{1}<t_{2}=t_{f}$. Then $\lambda_{3}(t_{1})=0$ and Equation (12) result in $\rho\,\cos(\theta_{1}-\phi)+1=0$, where $\rho>1$ by Remark 3. This single equation in two unknowns results in infinitely many solutions for $\rho>1$ and $\phi$. For simplicity, take $\rho=2$ and so $\phi=\theta_{1}\pm 2\pi/3$, or simply take $\phi=\theta_{1}-2\pi/3$. Then $\lambda_{3}(t)$ is given as

[TABLE]

For verification purposes, it can be easily seen that

[TABLE]

which agrees with (16).

• (iii)

Type $C$: This type requires no switchings, so by Remark 3, any $0<\rho<1$ and any $-\pi\leq\phi\leq\pi$ would do. Take $\rho=1/2$ and $\phi=0$, for the sake of simplicity, which yield

[TABLE]

From (48), one gets $|\lambda_{3}(t)|=\left[\cos\theta(t)+1\right]/(2a)$, which agrees with (16).

Next, suppose that Problem (Pc) is abnormal, i.e., $\lambda_{0}=0$. Then, by Lemma 7, the only possible types are $C$ and $CC$, which we consider below.

• (a)

Type $CC$: In this case, by (17) and Figure 2, $|\theta_{0}-\theta_{1}|\leq\pi$ and $|\theta_{1}-\theta_{f}|\leq\pi$. This type has only one switching, so without loss of generality, let $0\neq t_{1}\neq t_{2}=t_{f}$. Then $\lambda_{3}(t_{1})=0$ and Equation (12) give $\rho\,\cos(\theta_{1}-\phi)=0$, where $\rho>0$ by Remark 5, and so $\cos(\theta_{1}-\phi)=0$, which yields $\phi=\theta_{1}\pm\pi/2$. For simplicity, take $\rho=1$ and $\phi=\theta_{1}-\pi/2$. Now, one can write

[TABLE]

From (49), one gets $|\lambda_{3}(t)|=\cos(\theta(t)-\theta_{1}+\pi/2)/a$, which agrees with (16).

• (b)

Type $C$: In this case, by (17) and the phase plane diagram in Figure 2, $|\theta_{0}-\theta_{1}|\leq\pi$. This type has no switchings, so $\rho>0$ can be chosen arbitrarily and $\phi$ can be determined based on the value of $\rho$. For simplicity take $\rho=1$. Then

[TABLE]

[TABLE]

which agrees with (16) if $\theta_{0}-\phi=-\pi/2$, whenever $u(t)\equiv a$, or if $\theta_{0}-\phi=\pi/2$, whenever $u(t)\equiv-a$. Therefore a valid construction of $\lambda_{3}(t)$ is as follows. For a.e. $t\in[0,t_{f}]$,

[TABLE]

This completes the proof. $\Box$

Remark 6

The proof of Theorem 2 provides closed form expressions for the adjoint variable $\lambda_{3}$, which is also the switching function, once a feasible curve of certain types, of the Markov-Dubins problem, is given. Expression (45) is provided for feasible curves of types $CSC$, $CS$, $SC$ and $S$. In the case when Problem (Pc) is normal, Expression (46) is derived for type $CCC$, (47) for type $CC$ and (48) for type $C$. In the case when Problem (Pc) is abnormal, Expression (49) is provided for type $CC$ and (50) for type $C$. It is interesting to note that, in the case when $|\theta_{0}-\theta_{1}|\leq\pi$ and $|\theta_{1}-\theta_{f}|\leq\pi$, either of the expressions (47) (with $\lambda_{0}>0$) and (49) (with $\lambda_{0}=0$) can be used, meaning that normal and abnormal adjoint variable solutions exist concurrently. The same comment holds for the expressions (48) (with $\lambda_{0}>0$) and (50)(with $\lambda_{0}=0$), in the case when $|\theta_{0}-\theta_{1}|\leq\pi$. It should, however, be noted that, if $|\theta_{0}-\theta_{1}|>\pi$ or $|\theta_{1}-\theta_{f}|>\pi$, then the solution cannot be abnormal. These comments justify the following two corollaries. $\Box$

Corollary 1** (Equivalence of Adjoint Variables in Normal and Abnormal Cases)**

• (a)

Suppose that a feasible solution of type $C$ of Problem (Pc) is given. If $|\theta_{0}-\theta_{1}|\leq\pi$, then the adjoint variable $\lambda_{3}$ exists with any $\lambda_{0}\geq 0$. In particular, one has (48) with $\lambda_{0}=1$, and (50) with $\lambda_{0}=0$.

• (b)

Suppose that a feasible solution of type $CC$ of Problem (Pc) is given. If $|\theta_{0}-\theta_{1}|\leq\pi$ and $|\theta_{1}-\theta_{f}|\leq\pi$, then the adjoint variable $\lambda_{3}$ exists with any $\lambda_{0}\geq 0$. In particular, one has (47) with $\lambda_{0}=1$, and (49) with $\lambda_{0}=0$.

Thus we can state:

Corollary 2** (Normality of Abnormal Curves)**

If Problem (Pc) has an abnormal solution, then that abnormal solution (or path) is also a normal solution. The converse, however, does not hold.

5 A Numerical Method for Markov-Dubins Curves

Theorem 1 facilitates the employment of an efficient numerical scheme for finding a Markov-Dubins curve, or path, as it characterizes all of the types of solutions, where each subarc solution can be written analytically, and so the infinite-dimensional optimization problem (P) is reduced to a combinatorial decision making with a relatively small number of choices. Theorem 1 asserts that a Markov-Dubins path will be of one of the types $CSC$ and $CCC$, or a subset of these types, giving rise to seven possibilities, as listed in (i) and (ii) in the proof of Theorem 1. Recall that a $C$ subarc may happen to be a left-turn circular arc, denoted $L$, where $\dot{\theta}(t)>0$, or a right-turn circular arc, denoted $R$, where $\dot{\theta}(t)<0$. This increases the number of possibilities: By using the signed curvature, Markov-Dubins path can be characterized as one of particular types listed in the set

[TABLE]

or a subset of one of the types in the list, altogether yielding 15 possibilities. All of these possible types, including, for example, the types $LR$, $RL$, $RS$, $L$, and so on, can be checked algebraically to find all feasible paths and select the shortest of them—see, for example, [35], for a classification and identification of these path types. We propose an alternative numerical approach to identifying the type of the path and computing the duration of each component, or subarc, of the path. Ultimately, the approach we present here should serve as a building block for a numerical approach for constructing Markov-Dubins interpolating curves, in which case, the much larger number of possible types of curves is prohibitive for an algebraic approach.

The technique we propose involves parameterization of the shortest path problem with respect to the terminal and switching times, i.e., the times at which the path switches from one subarc to another, say, from $L$ to $S$. This approach is markedly different from previous approaches to computing the Markov-Dubins path in the literature. Efficient computation of switching times, referred to as switching time optimization, for optimal control problems have been extensively studied in the past—see [25, 30, 24, 40] and the references therein. In comparison with these general studies, we do not have to integrate numerically the ODEs involved. We can write down a concatenation of the analytic solutions for the state and control variables along each subarc in terms of the switching times—this avoids the task of discretizing the problem and thus makes computations fast—see [22, 7] for examples of discretization of general optimal control problems.

Let $L_{\xi_{1}}$ denote a left-turn arc with length $\xi_{1}$, $R_{\xi_{2}}$ a right-turn arc with length $\xi_{2}$ and $S_{\xi_{3}}$ a straight line with length $\xi_{3}$. Similarly, let $L_{\xi_{4}}$ denote a left-turn arc with length $a\xi_{4}$ and $R_{\xi_{5}}$ a right-turn arc with length $\xi_{5}$. Then the types of solution arcs described in Theorem 1 or the more particular types of solution arcs given in (51) can all be represented by the string

[TABLE]

For example, the type $RLR$ is given with $\xi_{1}=\xi_{3}=0$ and $\xi_{2},\xi_{4},\xi_{5}>0$. On the other hand, the type $LR$ is obtained either with $\xi_{3}=\xi_{4}=\xi_{5}=0$ and $\xi_{1},\xi_{2}>0$ or with $\xi_{1}=\xi_{2}=\xi_{3}=0$ and $\xi_{4},\xi_{5}>0$.

Let the initial time $t_{0}:=0$ and the terminal time $t_{5}:=t_{f}$. Also define the switching times $t_{j}$, $j=1,\ldots 4$, such that

[TABLE]

The lengths $\xi_{j}$ may also be referred to as arc durations. Note that along an arc of type $L$ or $R$ the optimal control is bang–bang; in particular, $u(t)\equiv a$ along an arc of type $L$ and $u(t)\equiv-a$ along an arc of type $R$. Along an arc of type $S$, on the other hand, optimal control is singular; in particular, $u(t)\equiv 0$. Then the ODEs in Problem (Pc) can be solved as follows. For $t_{j-1}\leq t<t_{j}$,

[TABLE]

where

[TABLE]

Note that the control variable $u(t)$ is a piecewise constant function, which takes here the sequence of values $\{a,-a,0,a,-a\}$. After evaluating the state variables in (53)–(59) at the switching times and carrying out algebraic manipulations, one can equivalently rewrite Problem (Pc) as follows.

[TABLE]

where

[TABLE]

Substitution of $\theta_{1}$, $\theta_{2}$, $\theta_{4}$ and $\theta_{5}$ in (61) into Problem (Ps) yields a finite-dimensional nonlinear optimization problem in just five variables, $\xi_{j}$, $j=1,\ldots,5$.

Remark 7

With the constraints $\sin\theta_{f}=\sin\theta_{5}$ and $\cos\theta_{f}=\cos\theta_{5}$, we make sure that we satisfy the slope condition at the terminal point. Otherwise, the constraint $\theta_{f}=\theta_{5}$ is stronger, and imposing it might result in missing some of the feasible solutions—for example, the optimal solution itself given in Figure 6(a) for the problem in Example 2. $\Box$

Remark 8

Problem (Ps) can be solved by standard optimization methods and software, for example, Algencan [4, 8], which implements augmented Lagrangian techniques, or Ipopt [41], which implements an interior point method, or SNOPT [20], which implements a sequential quadratic programming algorithm. For general nonconvex optimization problems like Problem (Ps), what one can hope for, by using these software, is to get (at best) a locally optimal solution.

Recall that, by Theorem 2, there will be as many stationary solutions as the number of feasible solutions of the listed types. Suppose that a solution of Problem (Pc) is found by software, with locally optimal length $t_{f}=L_{0}$. Since this solution is likely not to be a global solution, Problem (Ps) can be modified by adding the constraint $\sum_{j=1}^{5}\,\xi_{j}\leq L_{0}+\varepsilon$, with some small $\varepsilon>0$, and then Problem (Ps) numerically solved again, which might provide a curve with a length $L_{1}<L_{0}$. Suppose that a shorter curve with length $L_{1}$ is found. Then the same procedure can be repeated in, say, Step $i+1$, $i=1,2,\ldots$, by adding the constraint $\sum_{j=1}^{5}\,\xi_{j}\leq L_{i}+\varepsilon$, until it is no longer possible to find a curve with a length shorter than $L_{i}$.

At this point, one might rightfully argue that, since the number of variables in Problem (Ps) is only five, rather than using the “global procedure” described in the previous paragraph, one can employ some general global optimization method and associated software which might serve as a remedy in finding a shortest curve solution. However, as we pointed earlier, Problem (Ps) is set up here as a prototype/building block for the Markov-Dubins interpolating curves, in which case, the companion of Problem (Ps) will in general have a significantly larger number of variables, prohibiting employment of a global solver. $\Box$

5.1 Numerical experiments

In this section, we present numerical experiments by solving Problem (Ps) under various sets of data and use the globalization strategy described in Remark 8, to construct Markov-Dubins curves and other stationary curves. For solving Problem (Ps), we use Ipopt, version 3.12, a popular optimization software based on an interior point method [41]. AMPL [16] is used as an optimization modelling language, which employs Ipopt as a solver. All Ipopt tolerances are set at $10^{-15}$, with MA77 chosen as the linear solver.

Example 1

Figure 5(a) depicts a CSC type, or more specifically an LSR type, Markov Dubins path of maximum curvature of 3 units (or, minimum turning radius of 1/3 units) between the initial and terminal points $(0,0)$ and $(1,1)$, obtained by solving Problem (Ps), a number of times by adding a constraint, which imposes an upper bound on the length of the curve. The initial and terminal orientation angles are specified as $-60^{\circ}$ and $-30^{\circ}$, respectively.

The solution to Problem (Ps), by following the procedure in the second paragraph of Remark 8, results in a Markov-Dubins curve of type $L_{\xi_{1}}S_{\xi_{3}}R_{\xi_{5}}$ (with ${\xi_{2}}={\xi_{4}}=0$), where

[TABLE]

Re-define the switching times, for simplicity, as $t_{1}=\xi_{1}$, $t_{2}=\xi_{1}+\xi_{3}$ and $t_{f}=\xi_{1}+\xi_{3}+\xi_{5}$, which are different from the definition given in (52) but more intuitive for our practical purposes. Then

[TABLE]

The graph of the switching function, $\lambda_{3}(t)$, in Figure 5(b), as well as the phase plane trajectory in Figure 5(c), have been drawn simply by using the expression (45). These graphs are included here for illustration purposes; otherwise, we already know by Theorem 2 that any numerical solution to Problem (Ps) of certain types satisfies the Pontryagin maximum principle.

In Figure 5(d), we provide a stationary solution of Problem (Ps), which is of type $R_{\xi_{2}}S_{\xi_{3}}L_{\xi_{4}}$ (with ${\xi_{1}}={\xi_{5}}=0$), where

[TABLE]

and so, with $t_{1}=\xi_{2}$, $t_{2}=\xi_{2}+\xi_{3}$ and $t_{f}=\xi_{2}+\xi_{3}+\xi_{4}$,

[TABLE]

A comparison of the lengths $t_{f}$ of the two curves tells us that the stationary curve provided in Figure 5(d) is more than twice longer than the Markov-Dubins curve in Figure 5(a), although it does not appear so since Figure 5(d) is presented on a different scale. It should be noted that, in addition to the stationary solution curve in Figure 5(d), there are two further stationary solution curves, which are of type $RSR$ with $t_{f}=3.34456289$, and type $LSL$ with $t_{f}=3.69362874$, respectively, either of which can be found by solving Problem (Ps). Each of these solutions is normal, i.e., $\lambda_{0}\neq 0$, by the results proved before.

Example 2

The only difference of the problem in the present example from the one in Example 1 is that as the terminal point we take $(0.4,0.4)$ instead of $(1,1)$. Figure 6 depicts six of the seven stationary solutions to Problem (Pc), obtained by solving Problem (Ps), the first one of which, shown in Figure 6(a), is the Markov-Dubins curve. The seventh stationary solution, which is not shown in the figure, is of type $RSL$ with $t_{f}=4.54008162$. As can be seen, there are two stationary solutions each of types $RLR$ (Figure6(b) and (e)) and $LRL$ (Figure6(d) and (f)). There does not seem to be a stationary (or feasible) solution of type $LSR$ or of any other type which concatenates less than three subarcs.

As in Example 1, the adjoint variable, or the switching function, $\lambda_{3}(t)$, can be easily computed, for each of the solutions in Figure 6(a)–(f). For the solutions in Figure 6(a)–(b) and (e), the formula in (45), and for the solutions in Figure 6(c)–(d), the formula in (46) can be used. Note that by the results obtained before, each of these solutions is normal, i.e., $\lambda_{0}\neq 0$.

Example 3 (An Abnormal Case)

Here, we provide an example to the solution of an abnormal problem. Suppose that we want to find a Markov Dubins path of maximum curvature of 1 unit (or, minimum turning radius of 1 unit) between the initial and terminal points $(0,0)$ and $(4,4)$, where the initial and terminal orientation angles are both $-90^{\circ}$.

Figure 7 depicts all of the stationary solution curves for the given problem. The one shown in (a) is a Markov-Dubins curve, for which $\lambda_{0}=0$, and so the curve is abnormal: Note that the trajectory in Figure 7 corresponds to exactly one cycle along an ellipse in Figure 2 and that switching occurs when $\theta_{1}=\pi/2$. This is a situation that can also happen with $\lambda_{0}\neq 0$, by Corollary 2, and this can be verified by the phase portrait in Figure 1. The adjoint variable, or the switching function, $\lambda_{3}(t)$, for the abnormal Markov-Dubins curve here can be found by using the formula in (49). By Corollary 2, the abnormal Markov-Dubins curve here is also normal, and the associated switching function, $\lambda_{3}(t)$ can be obtained by using the formula in (47).

The adjoint variable $\lambda_{3}(t)$ for the stationary solutions depicted in Figure 6(b)–(d), on the other hand, can be found by using the formula in (45). It should be noted that these solutions in Figure 6(b)–(d) are only normal, for which $\lambda_{0}\neq 0$.

6 Conclusion and Further Work

We have presented a study of the Markov-Dubins problem by employing optimal control theory, and reproduced Dubins’ result (Theorem 1) which classifies the types of the shortest curves of bounded-curvature between two oriented points. We have shown that the optimal control formulation of the problem can be abnormal, as well as normal (Lemmas 5 and 7 and Corollaries 1 and 2). We characterized these solutions. We have also shown that feasible solutions of the types an optimal solution is required to be of are stationary (Theorem 2). We presented a numerical method based on switching time parameterization, and applied it to two example problems to illustrate the method as well as the results on the normal Markov-Dubins paths. With an additional example, we illustrated the presence of an abnormal Markov-Dubins path.

The results, as well as the numerical method, presented in this study constitute building blocks for a study of a generalization to Markov-Dubins interpolating curves, where the curve is required to pass through a number of additional intermediate points. Work in this direction is in progress, incorporating additional techniques from optimal control theory available in [5, 14, 26].

Theorem 2 proves stationarity of feasible curves of certain types, but does not say anything about the local optimality of these curves. The proof of Theorem 2, on the other hand, provides explicit expressions for the switching function in both the normal and abnormal cases. These expressions could possibly be used in verifying the second-order sufficient conditions of optimality, which can be found in [30], for curves of type $CCC$, i.e., for bang–bang type control. To the author’s knowledge, the only result on the local optimality of a stationary curve of the Markov-Dubins problem is provided in [2] for a curve of type $CS$, as a case study.

Another interesting extension of the results and the numerical method in the current paper would be a study of the Markov-Dubins problem in the three-dimensional space. Sussmann presents in [36] a study of the three-dimensional Markov-Dubins problem, by using geometric optimal control theory, and finds that the optimal path can include a helicoidal arc.

In practical situations, for example in the flight trajectory planning of a UAV, constraints are often imposed because of the terrain, flight traffic and no-go areas. Therefore, yet another interesting extension of the current study would be to situations where constraints on the space are added, giving rise to state-constrained optimal control problems, which are significantly more challenging.

Acknowledgment

The author would like to thank Helmut Maurer for pointing to Reference [2], after a talk the author had presented on the topic.