An indirect numerical method for a timeoptimal stateconstrained control problem in a steady twodimensional fluid flow ^{†}^{†}thanks: The support from the Russian Foundation for Basic Research during the projects 163160005, 182903061, and the support of FCT R&D Unit SYSTEC – POCI010145FEDER006933/SYSTEC funded by ERDF Compete2020 Fct/mec PT2020 extension to 2018, Project STRIDE NORTE010145FEDER000033 funded by ERDF NORTE2020, and FCT Project MAGIC POCI010145FEDER032485 funded by ERDF Compete2020 POCI are acknowledged.
Abstract
This article concerns the problem of computing solutions to optimal control problems with state constraints and whose trajectory is also affected by known flow vector fields. This general mathematical framework is particularly pertinent to the requirements underlying the control of Autonomous Underwater Vehicles in realistic scenarii. The key contribution consists in devising a computational indirect method that becomes effective in the numerical computation of extremals to optimal control problems with state constraints by using multipliers of the Maximum Principle in the form of Gamkrelidze whose measure component is ensured to be continuous. The specific problem of timeoptimal control of an Autonomous Underwater Vehicle in a bounded space set, subject to the effect of a flow field and with bounded actuation is used to illustrate the proposed approach. The corresponding numerical results are presented and discussed.
I Introduction
This article addresses the challenges of using computational indirect methods based on the application of the Pontryagin Maximum Principle (PMP) to solve stateconstrained optimal control problems arising in the optimal motion planning of Autonomous Underwater Vehicles (AUVs) subject to state and control constraints as well as the effect of known fluid vector fields. State constraints are specified by the boundary of the free space in which the AUV is allowed to navigate, while the known, possibly time varying, vector flow fields, such as currents, moving fronts, among others, are generated by the diverse underwater phenomena.
In order to facilitate the exposition, we consider a simplified twodimensional AUV model, that is, the immersion depth is constant, and, moreover, that the steering angle can be changed instantaneously. The later means that the AUV velocity may exhibit discontinuities which can occur at any time due to the control action. This simplified model does not entail any loss of generality of the proposed approach, but also it is obviously still relevant from the point of view of applications. Note, that this approximation is reasonably close to reality, if the global transition time from a given initial point to a final point is several orders of magnitude larger than the time spent on the abrupt change of the position of the vehicle rudder.
The novelty of the approach used here consists in using numerical algorithms based on an indirect method that uses the PMP to efficiently solve stateconstrained optimal control problems. It is well known that a conventional scheme using this idea would encounter enormous computational difficulties in the application of a shooting method to solve the twopoint boundary value problem that arises fom the application of the PMP due to the generally highly irregular measure Lagrange multiplier associated with the state constraints.
How do we overcome this apparently, in general, unsurmountable difficulty? The key observation consists in the fact that, for the considered AUV model and constraints setup, the regularity condition with respect to the state constraints proposed by R.V. Gamkrelidze in [1] (check also the classic monograph [2], Chapter 6) is a priori satisfied for any feasible arc. This allows us to assert that the measure Lagrange multiplier from the PMP is continuous, cf. [3]. In turn, the regularity condition yields an explicit formula for this multiplier, and, by virtue of its continuity, the junction points in which the optimal arc meets the boundary of the state constraint set, can be found. Thus, largely due to this property, and by using a variation of the standard shooting method (see [4] for an overview) for numerical solution of the twopoint boundary value problem arising from the application of the PMP, a novel algorithm solving the timeoptimal problem is constructed.
There is a vast array of publications on the theory of stateconstrained optimal control problems. So far, many important theoretical questions have been investigated. The continuity of the measuremultiplier has also been examined, for example, in [5, 6, 7, 8, 9]. Other contributions made on the general development of this theory can be found, for example, in [10, 11, 12, 13, 14, 15, 16, 19, 20, 21, 22, 23, 24, 25, 18, 17]. Issues on numerical solutions to stateconstrained problems have been studied, for example, in [26, 27, 28, 29, 30, 31]. These selective lists of contribution are, obviously, far from to be exhaustive.
Our article is organized as follows. In Section II, the problem formulation for the twodimensional AUVmotion optimal planning is presented. Section III is aimed to discuss the PMP and its application to a particular case of AUVmodel problem in which the feasible control set is given by the unit disk in . In Section IV the numerical results are presented. Section V concludes the article with a short summary.
Ii Problem Statement and the Concept of Regularity
Consider an AUV moving in an underwater milieu subject to state constraints and to the influence of fluid flow vector field. The velocity resulting from the force exerted by the fluid flow, represented by the vector at each point , affect the motion of the vehicle, that is, the way the AUV is propelled.
The problem formulation is described as follows:

The waterway is defined on the plane by given affine state constraints.

The AUV motion is determined by a linear control system that encompasses the vector field and the control actuation .

The control actuation takes on values in a closed bounded set .

The initial and final positions of the AUV are given, respectively, by the points and .

The task is to find the minimum time trajectory joining the points and .
. More precisely:
Minimize  (1)  
subject to  
Here, is the state variable, is the control variable. A measurable function , where is a given compact (the socalled feasible control set), is termed control. The point is the starting point, while is the terminal point, and is a smooth map which defines a steady fluid flow varying in space. The terminal time is free and is supposed to be minimized.
Next, let us formulate the regularity concept w.r.t. the state constraints for Problem (1) (cf. [9]). Let us define
Clearly, . Then, the regularity condition from [9] is as follows.
Assumption R) Assume that for all and , such that , and , there exist a vector , and a vector such that and .
Here, is the contingent tangent cone and is the dual of the limiting normal cone. For these definitions, see [32]. It is clear that, in the case of convex , Assumption R) always fails to hold wherever there exists a corner point of for which for some such that .
Nonetheless, Assumption R) represents a kind of a priori verification condition imposed on the data of the problem. It is easy to single out various classes of problems which satisfy this condition. One of such class of problems is demonstrated in the next section. Note that, if Assumption R) is satisfied, then any feasible arc is regular in the sense proposed in [1], and the measure Lagrange multiplier from the PMP is continuous. Next, we proceed to the PMP formulation.
Iii Maximum Principle
Consider the extended HamiltonPontryagin function
where , and .
We assume that Assumption R) holds. Then, for an optimal process , the PMP ensures the existence of Lagrange multipliers composed by a number , an absolutely continuous adjoint arc , and a scalar function , such that the following conditions are satisfied:

Adjoint equation
for a.a. ;

Maximum condition
for a.a. ;

Conservation law

is constant on the time intervals where
increasing on the time intervals where , and decreasing on the time intervals where . Moreover, is continuous on ;

Nontriviality condition
Above, stands for the optimal time, thus, the optimal pair is considered over time interval .
Now, let us focus on a particular case of the control set . In this article, we consider the case given by
that is, by the unit disk in the plane. The main target is to explicit the necessary optimality conditions provided by the maximum principle and to determine the appropriate expressions for the optimal control and the multipliers for a given vector field . At the same time, it is also required to check whether Assumption R) holds.
After specifying the multipliers, it becomes possible to define an algorithm in order to compute the field of extremals by virtue of the PMP. The main challenge is that, in the presence of state constraints, the extra multiplier appears in the PMP, which varies on the subset of in which the state constraint becomes active, i.e. at all points in time for which the optimal arc reaches the boundary of the state constraint set. If the regularity condition expressed by Assumption R) does not hold for Problem (1), then may have jumps at such points of time.^{1}^{1}1See Example 1 in [33]. Moreover, in the absence of regularity, it is not clear how to express via the rest of multipliers, that is and . Both facts entail obvious numerical complexities in computing the multipliers. Nonetheless, Assumption R) and the continuity of ensured by this assumption, enable the numerical efficiency of the proposed algorithm, as it will be discussed in Section IV.
Based on this, consider several claims relevant to the ongoing analysis.
Claim 1. Assumption R) holds for Problem (1) in which provided that such that .
Indeed, on the boundary of the state constraint one has , and hence, by the assumption, it holds that . Then, since
Assumption R) obviously holds with with . The claim is therefore confirmed.
Observe that the vehicle actuators should be sufficiently powerful to enable it to cross the given column waterway milieu. This is obviously so, should the flow field verify the condition for all , which implies the corresponding condition of Claim 1. Moreover, this condition guarantees the existence of a feasible path from to , as the main fluid flow is supposed to be along the axis , in the direction from to . Therefore, the application of Filippov’s Theorem [22] yields the following claim.
Claim 2. Problem (1) with has a solution under the above nonrestrictive assumptions imposed on the vector field .
From the PMP, it follows
Claim 3. In the PMP for Problem (1), one has
(2) 
Indeed, if there exists some such that condition (2) is violated, then due to the Conservation law (c), we obtain that , which contradicts the Nontriviality condition (e), thus confirming the claim.
Due to Claim 2, a solution to Problem (1) with , exists, while due to Claim 1 the above PMP can be applied to it. The next step consists in deducing explicit formulas for and expressed via . These are needed to solve the fourdimensional boundary value problem arising from the application of the PMP. From the Maximum condition (b), and by virtue of (2), the optimal control is expressed uniquely via the multipliers (as the control set is strictly convex) and takes the following form (the dependence on time variable is omitted just to simplify the notation):
(3) 
At the boundary points of the state constraint set along the optimal trajectory, we have , or equivalently, . Then,
where we set . This implies (bearing in mind that )
(4) 
The derived formula holds at the boundary of the state constraints, that is, on the time intervals on which . Moreover, is increasing on the time intervals where , decreasing where , and constant in the interior of the state constraint set .
At the same time, can be chosen such that , (equivalently, ), and it is continuous on . Thus, the above formulas (3) and (4) provide an explicit expression for and via , and, thus, also for the boundary value problem to compute by the control dynamics of Problem (1) and the Adjoint equation (a). This boundary value problem is described and numerically solved in the next section.
Iv The algorithm and numerical results
Summarizing the results discussed above, the field of extremals is described by the following twopoint boundary value problem:
(5)  
together with (3) and (4). Here, is a given vector field satisfying the condition in Claim 1; points and are the given initial and final positions, respectively; travelling time, , is unknown. Note that multiplier given by relation (4) is continuous and according to the previous section can be defined in two equivalent ways either or .
Iva The algorithm
The implemented algorithm to solve Problem (5) numerically is a variation of the shooting method (see, e.g., [4] for details) and consists of two parts.
IvA1 Backward time integration
We perform backward time integration by the standard fourthorder RungeKutta method of the ordinary differential equations (ODEs) in (5) for the initial conditions in the form^{2}^{2}2By virtue of (2), it is clear to see that one could take .:
for increasing from 0 to with a step . Also, for while the trajectory stays in the interior of the state constraint set. For each trajectory the distance to the point is measured as well as the corresponding travelling time. Local minima in of the distance is found by bisection (i.e. repeating the procedure for a halved and between the two neighboring values of where the current estimate of the minimum is computed) such that distance between the trajectory and is not greater than – we name such trajectories to be inner extremals.
If a trajectory meets the boundary, then is computed on the boundary using (4). Bisection in is used to find trajectories minimizing the multiplier at the junction point such that it verifies . Along such trajectories is continuous and they are potentially parts of extremals involving a boundary segment – boundary extremals.
IvA2 Forward time integration
Next step is forward in time integration of the ODEs in (5) for the initial conditions in the form:
also for with the same step . Only trajectories meeting the boundary with are of interest. When such trajectories are computed by bisection in , the segment of the trajectory starting from the junction point is computed by integrating the governing equations along the boundary. Trajectories found at this step of the algorithm are shown in Figs.13 by blue lines.
If a trajectory following the boundary meets a point where a backward in time trajectory meets the boundary with , then the continuity of the multiplier at this time is checked via the following relation:
If it is continuous, then the forward in time trajectory (involving the corresponding boundary segment) and the backward in time trajectory together constitute an extremal.
IvB Numerical results
For numerical experiments we consider two sample flow velocity fields (Figs. 12) and (Fig.3) mimicking real river flows. The former is symmetric with vanishing component transversal to the boundary (permitting validation of the numerical method), while the latter is more realistic; in both cases, fluid flows are faster near the boundary.
For the flow , , , the field of extremals and the corresponding travelling times are shown in Fig. 1 displaying three inner and two boundary extremals. The flow and the position of the points and are symmetric about the vertical axis and, as a consequence, all the extremals are also symmetric about this axis. Since the flow is faster at the boundary, as one can expect, solution to the minimum time problem are the boundary extremals (with optimal traveling time ).
Field of extremals for the same flow, , but for a different position of the starting and terminal points, and , is shown in Fig. 2. Qualitatively, the extremals are of the same nature – three inner and two boundary extremals, but they are not symmetric anymore. The left boundary extremal, which is closer (in comparison to the other boundary extremal) to both points and , is the solution to the minimum time problem with . Interestingly, although the right boundary extremal is more distant from and than the inner extremals, it is less time consuming.
For the flow , the field of extremals (see Fig. 3) is constituted of one inner and one boundary extremals. One backward in time trajectory meeting the right boundary and one forward in time meeting the same boundary (both are shown in Fig. 3) do not meet each other and, hence, do not constitute an extremal. As for the field V above, the minimum time trajectory is the boundary extremal with .
V Conclusions
A simplified twodimensional AUVmotion model in an underwater milieu has been considered albeit under the state constraints. An indirect numerical method based on the application of the Pontryagin maximum principle has been proposed. Formulas for the measure Lagrange multiplier and the optimal control expressed via the costate function have been derived and the corresponding boundary value problem has been explicated. The numerical results have been presented and discussed.
Besides the presented numerical results concerning the unit disk as a feasible control set, other types of feasible sets have been considered such as square, ellipsoid, etc. For these models, other types of conditions on the vector field are assumed in order to fulfill the regularity requirements.
References
 [1] Gamkrelidze, R.V.: Optimumrate processes with bounded phase coordinates, Dokl. Akad. Nauk SSSR, 125, 475–478 (1959)
 [2] Pontryagin, L.S., Boltyanskii, V.G., Gamkrelidze, R.V., and Mishchenko, E.F.: The Mathematical Theory of Optimal Processes, Interscience, New York (1962)
 [3] Arutyunov, A.V., Karamzin, D.Yu. On Some Continuity Properties of the Measure Lagrange Multiplier from the Maximum Principle for State Constrained Problems. SIAM Journal on Control and Optimization, 53, 4 (2015)
 [4] Press, W.H.; Teukolsky, S.A.; Vetterling, W.T.; Flannery, B.P. Numerical Recipes: The Art of Scientific Computing, 3rd ed., Cambridge University Press, 2007.
 [5] Hager, W.W.: Lipschitz continuity for constrained processes, SIAM J. Control and Optim., 17 (1979)
 [6] Maurer, H.: Differential stability in optimal control problems. Applied Mathematics and Optimization. 51 (1979)
 [7] Afanas’ev, A.P., Dikusar, V.V., Milyutin, A.A., Chukanov, S.A.: Necessary condition in optimal control. Moscow: Nauka (1990) [in Russian]
 [8] Galbraith, G.N., Vinter, R.B.: Lipschitz continuity of optimal controls for state constrained problems, SIAM J. Control and Optim. 42, 5 (2003)
 [9] Karamzin, D.Yu., Pereira, F.L.: On a Few Questions Regarding the Study of StateConstrained Problems in Optimal Control J. Optim. Theory Appl., to appear (2019)
 [10] Warga, J.: Minimizing variational curves restricted to a preassigned set, Trans. Amer. Math. Soc., 112 (1964)
 [11] Dubovitskii, A.Ya., Milyutin, A.A.: Extremum problems in the presence of restrictions, Zh. Vychisl. Mat. Mat. Fiz., 5:3, 395–453 (1965); U.S.S.R. Comput. Math. Math. Phys., 5:3, 1–80 (1965)
 [12] Neustadt, L.W.: An abstract variational theory with applications to a broad class of optimization problems. II: Applications, SIAM J. Control, 5 (1967)
 [13] Halkin, H.: A satisfactory treatment of equality and operator constraints in the DubovitskiiMilyutin optimization formalism J. Optim. Theory Appl., 6, 2, (1970)
 [14] Ioffe, A.D., Tikhomirov, V.M.: Theory of Extremal Problems, NorthHolland, Amsterdam (1979)
 [15] Arutyunov, A.V., Tynyanskiy, N.T.: The maximum principle in a problem with phase constraints. Soviet Journal of Computer and System Sciences, 23 (1985)
 [16] Arutyunov, A.V.: Perturbations of extremal problems with constraints and necessary optimality conditions, Journal of Soviet Mathematics, 54, 6 (1991)
 [17] Bettiol, P., Khalil N., and Vinter, R.B. Normality of generalized EulerLagrange conditions for state constrained optimal control problems. J. Convex Anal., 23(1), 291–311 (2016).
 [18] Khalil, N. Optimality conditions for optimal control problems and applications. 2017. Thèse de doctorat. Université de Bretagne occidentaleBrest.
 [19] Vinter, R.B., Ferreira, M.M.A.: When is the maximum principle for state constrained problems nondegenerate? J. Math. Anal. and Appl., 187 (1994)
 [20] Arutyunov, A.V., Aseev, S.M.: Investigation of the degeneracy phenomenon of the maximum principle for optimal control problems with state constraints. SIAM J. Control Optim., 35:3 (1997)
 [21] Arutyunov, A.V.: Optimality conditions: Abnormal and Degenerate Problems. Mathematics and Its Application. Kluwer Academic Publisher (2000)
 [22] Vinter, R.B.: Optimal Control. Birkhauser, Boston (2000)
 [23] Milyutin, A.A.: Maximum Principle in a General Optimal Control Problem. Moscow, Fizmatlit (2001) [in Russian]
 [24] Arutyunov, A.V., Karamzin, D.Yu., Pereira, F.L.: The Maximum Principle for Optimal Control Problems with State Constraints by R.V. Gamkrelidze: Revisited. J. Optim. Theory Appl., 149 (2011)
 [25] Colombo, G., Henrion, R., Hoang, N.D., Mordukhovich, B.S. Discrete Approximations of a Controlled Sweeping Process (2015) SetValued and Variational Analysis, 23 (1), pp. 69–86.
 [26] Bryson, E.R., YuChi Ho. Applied Optimal Control. Teylor&Francis, 1969.
 [27] Betts, J.T., and Huffman, W.P. Pathconstrained trajectory optimization using sparse sequential quadratic programming, Journal of Guidance, Control, and Dynamics, Vol. 16, No. 1, pp. 59–68 (1993)
 [28] Buskens, C., Maurer, H. SQPmethods for solving optimal control problems with control and state constraints: adjoint variables, sensitivity analysis and realtime control. Journal of Computational and Applied Mathematics. V. 120, pp. 85–108 (2000)
 [29] Haberkorn, T., Trelat, E. Convergence results for smooth regularizations of hybrid nonlinear optimal control problems, SIAM Journal on Control and Optimization (2011)
 [30] Dang, T.P., Diveev, A.I., Sofronova, E.A. A Problem of Identification Control Synthesis for Mobile Robot by the Network Operator Method. Proceedings of the 11th IEEE Conference on Industrial Electronics and Applications (ICIEA), pp. 2413–2418 (2016)
 [31] Zeiaee, A., SoltaniZarrin, R., Fontes, F.A.C.C., Langari, R. Constrained directions method for stabilization of mobile robots with input and state constraints (2017) Proceedings of the American Control Conference, pp. 3706–3711.
 [32] Mordukhovich, B.S., Variational Analysis and Generalized Differentiation. Volume I. Basic Theory. SpringerVerlag, 2006, Grundlehren der Mathematischen Wissenschaften [Fundamental Principles of Mathematical Sciences], Berlin.
 [33] Zakharov, E.V., Karamzin, D.Y. On the study of conditions for the continuity of the Lagrange multiplier measure in problems with state constraints (2015) Differential Equations, 51 (3), pp. 399–405.