Optimal motion strategies with logic-based constraints for ocean vehicles

FACULDADE DE ENGENHARIA DA UNIVERSIDADE DO PORTO Optimal motion strategies with logic-based constraints for ocean vehicles Miguel Campos Pinto Coelho de Aguiar Mestrado Integrado em Engenharia Eletrotécnica e de Computadores Supervisor: João Borges de Sousa Co-supervisor: Jorge Estrela da Silva July 17, 2019 c Miguel Aguiar, 2019 Abstract Optimal trajectory generation for ocean vehicles has attracted considerable attention from both the research community and the industry. In the industry, the motivation is the reduction of travel times and fuel costs, and the focus is on long distance routes. Research on ship routing algorithms has shown that the fuel savings attained by such algorithms can be strongly affected by the ocean currents. In the research community, the focus is on trajectory generation for unmanned and autonomous marine craft in military and/or scientific applications. Here the mission times and distances are typically much shorter, but ocean current vary on smaller time scales and their magnitude can be as high as twice the vehicle’s maximum speed. As such there is a pressing need for mission planning algorithms that are able to incorporate data from high temporal-spatial resolution ocean models. As mission requirements become more complex, planning methods must also be able to take into account spatial and temporal constraints which arise in scenarios such as multi-stage operations in areas with tidal driven currents. We propose a method for trajectory generation for unmanned marine vehicles based on dynamic programming. The application of dynamic programming techniques converts an optimal control problem to the problem of solving a Hamilton-Jacobi-Bellman equation, which is a nonlinear partial differential equation. Data about ocean flows, produced by High Frequency radar or ocean models, is easily integrated in this framework. Our parallel implementation of a numerical method for solving Hamilton-Jacobi-Bellman equations allows us to obtain the solution in a few minutes for real-life sized problems. Once the equation is solved, optimal trajectories can be calculated from any deployment point in the operational area. Since dynamic programming can be applied to dynamical systems with both discrete and continuous states, the method is extensible to problems involving logic-based constraints. We present an efficient dynamic programming solution for trajectory generation in multi-stage missions. The problem is reduced to solving a sequence of partial differential equations, each of which can be solved by our numerical solver. The method is validated through real-life mission scenarios using data from ocean models of the Tejo and Sado estuaries in Portugal. i ii Acknowledgments I must begin by thanking my advisor, Professor João Borges de Sousa. His no-nonsense guidance and untiring encouragement were crucial for the success of this work, as were the many hours of interesting discussions. The contributions of Jorge Estrela da Silva and Renato Mendes were also very important. I thank them sincerely for their constant availability, patience and interest in this work. I also wish to gratefully acknowledge the contribution of our collaborators at NMEC-CESAM, Américo Ribeiro and João Miguel Dias, who graciously provided the data from their ocean models of the Sado and Tejo estuaries used in this work. I was lucky to be able to work on this thesis at LSTS, where I found a great work team and environment, which helped make the daily grind a lot more tolerable. Special thanks to José Pinto, Maria Costa and Paulo Dias for their help with using the LSTS toolchain. LSTS also supported my participation in a number of eye-opening conferences and meetings, where I was able to get feedback on this work. My participation at the extremely interesting interdisciplinary Symposium on Oceanographic Data Analytics was also made possible thanks to the help of João Fortuna. I made a lot of great friends these past five years, and I’ll treasure our days at FEUP, as well as the nights in downtown Porto. I’ve learned a lot from all of them, and I know they’ll continue to make me proud to know them in years to come. Finally, I’d like to thank my family, and in particular my parents, who taught me the joys of learning. Miguel Aguiar iii iv The whole purpose of mountain-climbing to me isn’t just to show off you can get to the top, it’s getting out to this wild country. Jack Kerouac (in The Dharma Bums) v vi Contents 1 Introduction 1 1.1 Motivationandcontext ............................... 1 1.2 Problemdescription................................. 2 1.3 Approach and contributions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2 1.4 Publications..................................... 4 1.5 DocumentStructure................................. 5 2 Background 7 2.1 Modelingoceancurrents .............................. 7 2.1.1 HydrodynamicModels........................... 7 2.1.2 Lagrangian Coherent Structures . . . . . . . . . . . . . . . . . . . . . . 8 2.2 Hybridsystemmodels ............................... 10 2.3 DynamicProgramming............................... 11 2.3.1 Introduction................................. 11 2.3.2 Application of the principle of optimality . . . . . . . . . . . . . . . . . 16 2.3.3 Issues with the application of the principle of optimality . . . . . . . . . 21 2.3.4 Numerical methods for solving Hamilton-Jacobi equations . . . . . . . . 22 2.4 The LSTS software toolchain . . . . . . . . . . . . . . . . . . . . . . . . . . . . 29 3 Problem Description 33 3.1 Thebaseproblem.................................. 33 3.2 Planning with logic-based constraints . . . . . . . . . . . . . . . . . . . . . . . 35 4 Related Work 39 5 Approach 43 5.1 Motionmodel.................................... 43 5.2 Thebaseproblem.................................. 44 5.2.1 Incorporating obstacles and constraints . . . . . . . . . . . . . . . . . . 46 5.3 Planning with logic-based constraints . . . . . . . . . . . . . . . . . . . . . . . 47 5.4 Selection of the numerical algorithm . . . . . . . . . . . . . . . . . . . . . . . . 53 5.5 Implementation of a numerical solver based on the fast sweeping method . . . . 54 5.5.1 Preliminary design decisions . . . . . . . . . . . . . . . . . . . . . . . . 55 5.5.2 Interfaceandusage............................. 56 5.5.3 Loading and accessing gridded data . . . . . . . . . . . . . . . . . . . . 59 5.5.4 Single-threaded implementation . . . . . . . . . . . . . . . . . . . . . . 60 5.5.5 First parallel implementation . . . . . . . . . . . . . . . . . . . . . . . . 62 5.5.6 Second parallel implementation . . . . . . . . . . . . . . . . . . . . . . 63 vii xiv LIST OF TABLES Notation Rthe set of real numbers R≥0the set of nonnegative real numbers Rnthe vector space of n-tuples of real numbers p p p= (p1,...,pn) |p p p|Euclidean norm of a vector p p p∈Rn τdeployment time of the vehicle x x xhorizontal position of the vehicle, an element of R2 ˙ x x xvelocity of the vehicle, an element of R2 rmaximum propulsion speed of the vehicle u u ucontrol function, taking values in R2 v v vocean flow velocity, an element of R2 Ωtarget set, a subset of R2 q,gcomponents of the cost function Jcost function, mapping a trajectory to a positive real number Vvalue function xv Chapter 1 Introduction 1.1 Motivation and context Over the past two decades, advances in navigation, control and communications have brought unmanned and autonomous marine vehicles (U/AUVs) to the forefront of ocean exploration [7]. From a surveillance and defense point of view, several kinds of missions, such as mine countermeasures, near-land and harbor monitoring, monitoring of undersea infrastructure such as communication cables, or anti-submarine warfare, greatly benefit from the use of unmanned ocean vehicles. Autonomous vehicles facilitate operations in areas which are unreachable by surface vessels, and can also reduce or eliminate the human risk factor in the mission [10]. From a scientific and industrial point of view, autonomous underwater vehicles have been successfully used in surveys such as seafloor mapping and monitoring and geochemical water column measurements, and have enabled data collection at resolutions that are not achievable with traditional ship-based surveys and in previously inaccessible areas, e.g. beneath ice sheets [70]. At the Underwater Systems and Technology Laboratory (LSTS), researchers in computer science, electrical engineering, mechanical engineering and oceanography have been working on the development and deployment of networked vehicle systems for marine applications, with emphasis on AUV operations [18,19]. In this context, persistent operations are an important goal, and trajectory generation can play an important role in reducing energy consumption, for instance. In the context of ship routing algorithms, simulations have shown that ocean currents can have a major influence in the fuel savings achieved by route optimization algorithms [35]. These studies also concluded that the magnitude of the fuel savings depends strongly on specific ocean current patterns, implying that rigorous model-based analyses can have a significant edge over heuristic methods [40]. Typical unmanned vehicle missions involve much smaller distances and time frames than those involved in ship routing problems, and as such it is expected that the impact of the ocean flow can be even more significant, as small scale variations in the current velocity become relevant. Besides their effect on energy consumption, ocean currents can have substantial impact on mission feasibility: in some operating environments such as fjords or estuaries, ocean vehicles can face 1 2Introduction currents whose magnitudes exceed their maximum speed, and even in areas where the ocean current has relatively low amplitude, it can still be relevant for missions with weak propulsion vehicles such as gliders. 1.2 Problem description In this work we focus on two particular problems of trajectory generation for unmanned underwater vehicles. The first, simpler problem, which we call the ‘base problem’, consists in generating a trajectory from a given deployment position to a target position or target region, while minimizing some cost function. As an example of a practical implementation, consider a vehicle which is deployed from a ship and must travel to some predefined region where it will perform a survey. It only makes sense to use forecasts of the ocean current to plan the trajectory from the ship to the target survey region so as to take maximum advantage of the ocean current velocity. The second problem we consider is that of generating trajectories for a multi-stage mission with logic constraints. A wide range of single-vehicle missions of practical interest can be expressed as a sequence of tasks to be completed by the vehicle in a predetermined sequence, where a trajectory must be designed for each step. For instance, one could consider a seafloor mapping mission where a vehicle is deployed from a harbor, has to reach a given survey area, execute the survey and return to the harbor. 1.3 Approach and contributions The problem of generating robot trajectories is well studied for robots in passive environments, with a number of well-established methods which can be categorized roughly into discrete methods, sampling-based methods and combinatorial methods [33]. In their most basic incarnations, sampling-based and combinatorial methods assume that the kinematics are unconstrained, and thus begin by planning a geometric path through the configuration space of the robot, typically with obstacle avoidance in mind. This high-level path is then adjusted to satisfy any differential constraints and used to derive control references for the dynamics controllers. This places the emphasis on planning the unrestricted configuration space path, which may be appropriate when the differential constraints are not too harsh (e.g., bounded velocity and curvature), but not for underwater vehicles navigating in strong dynamic current fields. Additionally, these methods focus on generating feasible paths, not including in their formulation any notion of optimality. In our case, it seems appropriate to use some form of optimization to select the trajectories which can take the most advantage of the ocean currents. Discrete methods, on the other hand, are by their nature amenable to optimization-based formulations. However, there is the issue of discretizing the robot motion, which becomes more difficult due to the presence of the ocean currents. It is also well known that there are inherent problems in approximating continuous problems in a discrete state space: in some cases, the discrete solution does not approach the optimal solution even as the discretization step goes to zero [51]. 1.3 Approach and contributions 3 Control-theoretic approaches seem to be the most appropriate for addressing continuousstate problems where the differential constraints play a significant role [33]. The use of controltheoretic models such as differential equations and inclusions has a distinct advantage: there are well established formulations for dynamic optimization problems involving such models, known as optimal control problems. Besides being optimization-based formulations, optimal control formulations have several benefits. First, the problem is expressed using the ‘natural’ continuous state space and dynamics of the robot, so matters of discretization or approximation are decoupled from the formulation. Second, these formulations allow for the inclusion of multiple types of constraints, both on the geometric path of the robot and on the values of the control inputs. Third, the solution is given as the sequence of values taken by the control inputs, so this eliminates having to solve additional problems to map the state trajectory to control inputs. Finally, there is a well-established body of literature on both the theoretical aspects of the formulations, such as existence and characterization of solutions, and on the numerical computation of solutions. Historically, there have been two main approaches to the solution of optimal control problems: variational methods and dynamic programming [56]. Variational methods are a generalization of the calculus of variations and the method of Lagrange multipliers, and their application typically gives necessary conditions for local optimality. In dynamic programming, the problem is embedded in a family of optimal control problems parameterized by one of the problem parameters, e.g. an initial or final condition, and the result is a sufficient condition for global optimality in the form of a partial differential equation known as the Hamilton-Jacobi-Bellman equation (HJBE). An important characteristic of dynamic programming methods is that an optimal feedback law can be recovered from the solution of the partial differential equation, since the solutions to all the problems in the family in which the original problem was embedded are obtained at once. Direct use of optimal control techniques is often neglected in robotics due to computational considerations, and they are often used only to support heuristic methods [33]. In fact, both approaches are prohibitively expensive in general for complex robot models: numerical methods relying on the variational approach typically involve the solution of nonlinear equations, twopoint boundary value problems or nonconvex optimization problems, while dynamic programming methods require the solution of a nonlinear partial differential equation, which is expensive even in low-dimensional state spaces ( Rn with n>4 ). However, the most relevant effects of the ocean currents on the motion of ocean vehicles are captured by kinematic models, and it is expectable that a kinematic trajectory which takes into account the differential constraints should give rise to feasible velocity references for the lower-level control loops. Hence, low dimensional kinematic models should be enough, and in that case the direct application of optimal control techniques becomes feasible. Even though vehicles may be equipped with sensors able to obtain local measurements of the ocean current velocity, its effect on the vehicle’s motion is a global issue, and decisions cannot be made at a local level: what can seem like a good decision based on local information may lead the vehicle to areas with unfavorable currents. Additionally, unless the mission time frame is very small, the time-varying nature of the ocean currents cannot be ignored. This aggravates the issue, 4Introduction as even if one is accounting for the variation of the ocean currents over the whole operational area, it all depends on the time at which the vehicle goes through any particular region. Thus, a complete picture of the ocean flow on the operational area over the mission time frame is necessary, and this can only be fulfilled by using forecasts from ocean models. This means that the downside of dynamic programming, which is the computationally expensive offline step of solving the HJBE, is irrelevant. Our approach uses dynamic programming to solve the two problems outlined above. Dynamic programming is easily applied to the first class of problems. Besides its use in trajectory generation, the solution of the HJBE is useful for mission planning, as it can be used to compare deployment times and positions. The second type of problem that we consider can be naturally expressed in the framework of hybrid systems, which are systems whose dynamics contain both discrete and continuous components. Since dynamic programming also applies to this class of dynamical systems, the dynamic programming approach is extensible to multi-stage problems with logic-based constraints. In this framework the problem is reduced to solving a sequence of partial differential equations, one for each stage. In what concerns the base problem, compared to existing approaches in the literature our method simultaneously obtains globally optimal trajectories, allows for comparison of deployment times and positions, and allows for general cost functions depending on the vehicle’s position. Our approach to planning with logic-based constraints extends previous work in robotic path planning [4]. Over the past two decades, a wide variety of numerical methods for solving Hamilton-JacobiBellman equations arising from a variety of optimal control problems have been developed. Some attention has also been given to the development of parallelizable versions of these methods. This enables the solution of large and high-dimensional problems in high performance computing platforms, which typically have compute nodes with multiple many-core processors, and on modern multi-core desktop and laptop computers. We implemented a multithreaded version of such a numerical method in C ++ , which we adapted specifically to our class of problems, in order to efficiently solve the partial differential equations arising from the two considered classes of problems. To our knowledge this is the only publicly available parallel implementation of a numerical solver for general Hamilton-Jacobi equations. 1.4 Publications The following conference articles were published as a result of the work developed in this thesis: • Miguel Aguiar et al. “Trajectory Optimization for Underwater Vehicles in Time-Varying Ocean Flows”. In: 2018 IEEE/OES Autonomous Underwater Vehicle Workshop (AUV). IEEE, Nov. 2018. DOI:10.1109/auv.2018.8729777 1.5 Document Structure 5 • M. Aguiar, J. Estrela da Silva, and J. Borges de Sousa. “Trajectory optimization for marine vehicles: models and numerical methods”. In: SYMCOMP2019 - 4th International Conference on Numerical and Symbolic Computation. Porto, 2019 Additionally, the work was presented at the following conferences: • Symposium on Oceanographic Data Analytics – Poster presentation. NTNU, Trondheim, Norway, November 2018. • Portuguese Meeting on Oceanography 2019 – Oral presentation. Peniche, Portugal, May 2019. • Portuguese Meeting on Optimal Control 2019 – Oral presentation. FEUP, Porto, Portugal, June 2019. 1.5 Document Structure The rest of the document is structured as follows. Chapter 2contains background material on ocean modeling, hybrid systems and dynamic programming, as well as an overview of the LSTS software toolchain, which is used for software-in-the-loop simulations. Chapter 3describes the two classes of problems considered in the thesis, formulating them as optimization problems. A review of recent work in trajectory generation for ocean vehicles is done in Chapter 4. In Chapter 5, the theoretical underpinnings of the approach are described, as well as the implementation of the multithreaded numerical solver used to solve the HJBE. Numerical examples using data from real ocean models are presented in Chapter 6. Finally, Chapter 7presents the conclusions and future research directions. 6Introduction Chapter 2 Background 2.1 Modeling ocean currents 2.1.1 Hydrodynamic Models Although hydrodynamic modeling in itself is not the purpose of the dissertation, the proposed approach requires the availability of an ocean current forecast over the operational area and time window. Consequently, some familiarity with the output of these models is necessary. Hydrodynamic models integrate the Navier-Stokes equations on a rectangular or curvilinear grid over the region of interest. We are interested in the quantities u(x,y,z,t),v(x,y,z,t),w(x,y,z,t), the velocities in the x , y and z directions, respectively. The horizontal position (x,y) is typically given in spherical coordinates (latitude and longitude), and velocity values are specified at the center of each grid cell. The z coordinate can be specified either as a Cartesian coordinate or in the σ -coordinate system, which consists of several layers bounded by a plane following the free surface and another plane following the bottom topography. For two dimensional models, the horizontal velocities are typically depth-averaged [49]. These models can integrate tidal forcing, sea surface elevation boundary conditions, surface boundary conditions imposed from weather forecasts, heat transport and freshwater inputs. For examples and details on model calibration and validation, see e.g. [2,49,50,60]. In what concerns velocity modeling errors, [49] reports root mean square errors mostly between 0.1 and 0.3 m/s in each velocity component, although in some areas the error is between 0.4 and 0.5 m/s (for typical velocity amplitudes of 1 m/s). 7 14 Background first case, it will contain the sequence EG, which means that its cost is greater than or equal to 5 . If it passes through H, it will contain the sequence HG, so its cost will be at least 1 . Thus, we can label H in blue. Since we now know H’s cost-to-go, we can estimate F’s cost-to-go as the sum of the link cost to H and the cost-to-go from H (Figure 2.6). A B C D 5E 3F 0G 1H 1 2 2 2 2 3 1 1 1 5 2 1 1 Figure 2.6: Using H’s cost-to-go, F’s cost-to-go can be upper-bounded By the same argument we used for H, we can see that F’s cost-to-go must be equal to this estimate, since the only other path is through E and such a path would imply a higher cost. We continue by estimating the cost-to-go of D and C. We can also re-estimate E’s cost-to-go. This new estimate must be correct, since all paths from E to G contain either EF or EG. A B 5C 4D 4E 3F 0G 1H 1 2 2 2 2 3 1 1 1 5 2 1 1 Figure 2.7: The procedure continues, expanding outward from the target node Note that any path starting from of the nodes colored in white or grey in Figure 2.7 must go through either E or F. In particular, this implies that A cannot have a cost-to-go which is less than 3 , and for this reason node D’s estimate must be correct. In addition, once we have D’s cost-to-go, we see that C’s cost-to-go estimate must also be correct, and then we can update nodes A and B. Node A can go through B or D. A path through B must go through C, and this would result in a cost of at least 5 . Hence A’s cost-to-go estimate is correct. Finally, B’s cost-to-go estimate is correct, since it can only go through C. The result is represented in Figure 2.9. 2.3 Dynamic Programming 15 6A 7B 5C 4D 4E 3F 0G 1H 1 2 2 2 2 3 1 1 1 5 2 1 1 Figure 2.8: The procedure continues, expanding outward from the target node 6A 7B 5C 4D 4E 3F 0G 1H 1 2 2 2 2 3 1 1 1 5 2 1 1 Figure 2.9: The cost-to-go from each node 16 Background It is now quite easy to calculate the optimal path between A and G, by choosing at each instant a neighbor with least cost-to-go. The result is ADFHG, which has a cost of 6 , equal to A’s cost-to-go. From this single calculation we are also able to obtain optimal paths from any node to G. This procedure can be generalized to arbitrary directed graphs with positive edge weights, resulting in Dijkstra’s algorithm [14]. If the edge weights were to change at each step, the procedure would no longer be applicable, since the cost-to-go from each node would depend on the time of arrival at the node. In that case one solution is to consider a bigger graph where each node represents a possible arrival time at each of the nodes of the graph represented in Figure 2.3. 2.3.2 Application of the principle of optimality We now proceed to give examples of typical applications of dynamic programming to control problems. Only a brief overview of the simplest examples is considered, and the focus is on intuitive arguments. Mathematical details can be found in the literature [6,8,21,56]. 2.3.2.1 Discrete time systems Consider the discrete time dynamical system given by the transition equation x x xt+1=ϕ ϕ ϕ(x x xt,u u ut)(2.3) where t is an integer. The state variable x x xt takes values in some set X . The decision or control variable u u utis restricted to some set U. A standard problem in control is the infinite horizon regulator problem, where the objective is to find a sequence of control values u u uτ,τ≥0 which minimize the cost function J(ξ ξ ξ,u u u) = ∞ ∑ t=0 g(x x xt(ξ ξ ξ, , ,u u u),u u ut) where g:X×U→R≥0 . Note that in the above expression u u u represents the whole control sequence {u u uτ}τ≥0 and x x xt(ξ ξ ξ,u u u) is the value of the system state at time t when the initial condition is x x x0(ξ ξ ξ,u u u) = ξ ξ ξand the control sequence is u u u. Following the example of the previous section, we begin by defining the cost-to-go from each state: ϑ(x x x) = inf u u u∈UJ(x x x,u u u), where U is the set of sequences u u u taking values in U . The function ϑ:X→[0,+∞] is also known in this context as the value function. Suppose u u u?is an optimal control sequence from the state x x x, i.e. J(x x x,u u u?) = ϑ(x x x). 2.3 Dynamic Programming 17 By the principle of optimality, the portion of the control starting at t=τ≥0 , is an optimal control from the initial condition x x xτ(x x x,u u u?), i.e. Jx x xτ(x x x,u u u?),{u u u? t}t≥τ=ϑ(x x xτ(x x x,u u u?)). In particular this is true for τ=1: Jϕ ϕ ϕ(x x x,u u u? 0),{u u u? t}t≥1=ϑ(ϕ ϕ ϕ(x x x,u u u? 0)) so that ϑ(x x x) = g(x x x,u u u? 0)+ϑ(ϕ ϕ ϕ(x x x,u u u? 0)).(2.4) Consider any w w w0∈U . Let w w w? be an optimal sequence from ϕ ϕ ϕ(x x x,w w w0) and let u u u be defined by u u u0=w w w0and u u uτ=w w w? τ−1for τ≥1. Then ϑ(x x x)≤J(x x x,u u u) =g(x x x,w w w0)+Jϕ ϕ ϕ(x x x,w w w0),{w w w? τ}τ≥1 =g(x x x,w w w0)+ϑ(ϕ ϕ ϕ(x x x,w w w0)) Together with (2.4), this implies that ϑ(x x x) = min η η η∈U{g(x x x,η η η)+ϑ(ϕ ϕ ϕ(x x x,η η η))}.(2.5) This formula is known as the Bellman equation, and it provides a way of calculating the cost-to-go from each state. Note that u u u? 0 is a minimizer in (2.5) , so that once ϑ has been found one can also recover the optimal control sequence. The classical discrete time Linear Quadratic Regulator problem can be solved using this method [27]. 2.3.2.2 Continuous time systems Consider the dynamical system given by the system of ordinary differential equations ˙ x x x(t) = f f f(x x x(t),u u u(t)) (2.6) where t∈R,x x x∈Rnis the state variable and u u u∈U⊂Rmis the control variable. Assume that f f f satisfies a standard set of assumptions [6] which guarantee that for each measurable function u u u:R≥0→Rm and initial state x x x0∈Rn there exists a unique absolutely continuous solution of (2.6) defined for all t∈R≥0 , which we denote by ξ ξ ξ(·;x x x0,u u u):R≥0→Rn , that is, we have ξ ξ ξ(0;x x x0,u u u) = x x x0 d dtt=τ ξ ξ ξ(t;x x x0,u u u) = f f f(ξ ξ ξ(τ;x x x0,u u u),u u u(τ)) 18 Background Consider the problem of controlling the state x x x to some closed set Ω⊂Rn . We begin by defining T(x x x,u u u) = inf{t≥0|ξ ξ ξ(t;x x x,u u u)∈Ω}, i.e., Tis the time of first arrival at Ω. Then we can define a cost function on the trajectories: J(x x x,u u u) = q(ξ ξ ξ(T;x x x,u u u))+ZT 0 g(ξ ξ ξ(t;x x x,u u u),u u u(t))dt where q:∂Ω→R≥0 and g:(Rn\Ω)×U→R>0 are the arrival cost and running cost, respectively. A particularly interesting case is q≡0 and g≡1 , in which case the cost function is the time taken to reach Ω, since J(x x x,u u u) = T(x x x,u u u). We can now formulate the problem as that of minimizing J . As before, the cost-to-go from x x x is defined as ϑ(x x x) = inf u u u∈UJ(x x x,u u u) where Uis the set of measurable functions u u u:R≥0→U. Consider a state x x xand a corresponding optimal control u u u, that is ϑ(x x x) = J(x x x,u u u). For any 0 ≤s≤T, let u u usdenote a shift of u u uby sunits of time, i.e. u u us(t) = u u u(t+s).(2.7) Then ϑ(x x x) = Zs 0 g(ξ ξ ξ(t;x x x,u u u),u u u(t))dt+q(ξ ξ ξ(T;x x x,u u u))+ZT s g(ξ ξ ξ(t;x x x,u u u),u u u(t))dt =Zs 0 g(ξ ξ ξ(t;x x x,u u u),u u u(t))dt+q(ξ ξ ξ(T−s;ξ ξ ξ(s;x x x,u u u),u u us)) +ZT−s 0 g(ξ ξ ξ(t+s;x x x,u u u),u u u(t+s))dt =Zs 0 g(ξ ξ ξ(t;x x x,u u u),u u u(t))dt+q(ξ ξ ξ(T−s;ξ ξ ξ(s;x x x,u u u),u u us)) +ZT−s 0 g(ξ ξ ξ(t;ξ ξ ξ(s;x x x,u u u),u u us),u u us(t))dt =Zs 0 g(ξ ξ ξ(t;x x x,u u u),u u u(t))dt+J(ξ ξ ξ(s;x x x,u u u),u u us) =Zs 0 g(ξ ξ ξ(t;x x x,u u u),u u u(t))dt+ϑ(ξ ξ ξ(s;x x x,u u u)) The last equality follows from the principle of optimality, as u u usmust be optimal for ξ ξ ξ(s;x x x,u u u). Now let u u u1:[0,s]→U be any measurable function and let u u u2∈U be an optimal control for 2.3 Dynamic Programming 19 ξ ξ ξ(s;x x x,u u u1). Then, letting u u ube the concatenation of u u u1and u u u2, by a similar computation we find ϑ(x x x)≤J(x x x,u u u) =Zs 0 g(ξ ξ ξ(t;x x x,u u u1),u u u1(t))dt+J(ξ ξ ξ(s;x x x;u u u1),u u u2) =Zs 0 g(ξ ξ ξ(t;x x x,u u u1),u u u1(t))dt+ϑ(ξ ξ ξ(s;x x x;u u u1)) So that ϑ(x x x) = inf u u u1Zs 0 g(ξ ξ ξ(t;x x x,u u u1),u u u1(t))dt+ϑ(ξ ξ ξ(s;x x x;u u u1)),(2.8) a counterpart to the discrete-time Bellman equation we found before. Assuming ϑ is differentiable at x x x , we can obtain a local version of this equation. First, rewrite this as 0=inf u u u1 {g(x x x,u u u1(0))s+o(s)+ϑ(ξ ξ ξ(s;x x x;u u u1))−ϑ(x x x)} dividing by sand taking a limit s→0, this becomes 0=inf w w w{g(x x x,w w w)+∇ϑ(x x x)·f f f(x x x,w w w)}(2.9) This gives a partial differential equation (PDE) for the value function (known as the HamiltonJacobi-Bellman PDE, with the boundary condition ϑ(x x x) = q(x x x),x x x∈∂Ω The issue of differentiability of ϑ is a delicate one, and so are the questions of existence of uniqueness of a solution to the boundary value problem. The theory of viscosity solutions was developed to tackle these problems, and the results show that for a wide variety of problems and under quite general conditions the value function is the unique viscosity solution of the boundary value problem [6]. For the special case of a minimum-time problem, under some controllability assumptions it can also be shown that ϑ is locally Lipschitz, which implies it is differentiable almost everywhere [6]. If we can compute a map κ:Rn→Uwhich satisfies κ(x x x)∈argmin w w w{g(x x x,w w w)+∇ϑ(x x x)·f f f(x x x,w w w)} then κis a feedback law which drives the system to Ωfrom any state. Other types of problems can be considered, such as infinite horizon regulator-type problems or finite-horizon problems (in which case the value function has a time dependence). For problems with time-dependent dynamics, i.e., problems involving dynamical systems of the form ˙ x x x(t) = f f f(t,x x x(t),u u u(t)), 20 Background the dynamic programming principle cannot be applied directly. This is due to the fact that the costto-go at x x x depends on the time at which the trajectory passes through x x x . As long as f f f is sufficiently regular in t , we can view t as a state variable. Setting z z z= (x x x,τ) and g g g(z z z,u u u) = (f f f(τ,x x x,u u u),1) we then have a dynamical system ˙ z z z(t) = g g g(z z z(t),u u u(t)), which is of the same form as (2.6), so dynamic programming may be applied to this model. A different kind of model may be adopted for continuous-time dynamical systems, using the concept of a differential inclusion. If we consider the set-valued function Fdefined by F(x x x) = f(x x x,U), then (2.6) can be written as ˙ x x x(t)∈F(x x x(t)), i.e., the selection of a control value is interpreted as the selection of a velocity at each point of a trajectory. If the running cost gdoes not depend on u u u, the PDE (2.9) can then be written as −g(x x x) = inf v v v∈F(x x x) ∇ϑ(x x x)·v v v 2.3.2.3 Hybrid systems Let us now consider how dynamic programming can be applied to models of the form described in Section 2.2. The control problem under consideration is again the minimum-cost-to-target problem, with the addition of a switching cost. Here we can consider a target Ω defined in Z×Rn . Each projection of the target, i.e., the sets Ωζ:={x x x∈Rn|(ζ,x x x)∈Ω} should be closed. Then, defining as before the time of arrival T=inf{t≥0|(z(t),ξ ξ ξ(t)) ∈Ω} the cost associated to a trajectory with ξ ξ ξ(0) = x x xand z(0) = ζis defined as J(x x x,ζ,u u u,σ) = q(z(T),ξ ξ ξ(T))+ZT 0 g(z(t),ξ ξ ξ(t),u u u(t))dt + N ∑ k=1 c(ξ ξ ξ(tk),z(tk),ξ ξ ξ(t− k),z(t− k),σ(t− k)) where the switching cost c is assumed to satisfy c(·,ζi,·,ζj,·)>0 if i6=j , and {t1,...,tN} are the times at which the discrete state switches. The positive switching cost ensures only a finite number of switching times exist. 2.3 Dynamic Programming 21 For simplicity, consider the case where nζ=nσ=2 and the discrete dynamics are simply ζ(t) = σ(t−) so that no jumps in the continuous state can occur and c(ξ ξ ξ(t),z(t),ξ ξ ξ(t−),z(t−),σ(t−)) = c(ξ ξ ξ(t),z(t),z(t−)). The value function must also include the discrete state. Hence, we define ϑi,i=1,2 as the cost-to-go from x x x if the discrete state is initially ζi . The application of the principle of optimality from the state (x x x,ζ1)then leads to ϑ1(x x x)≤min(inf u u uZs 0 g(ζ1,ξ ξ ξ(t;x x x,u u u),u u u(t))dt+ϑ1(ξ ξ ξ(s;x x x,u u u)), c(ξ ξ ξ(s;x x x,u u u),ζ2,ζ1)+ϑ2(ξ ξ ξ(s;x x x,u u u))) since the possibilities are remaining in the first discrete state over [0,s] , switching at time s or switching before time s , in which case the cost must be smaller than the cost of the first or second possibilities. Taking a limit as s→0 yields 0=min(inf v v v{g1(x x x,v v v)+∇ϑ1(x x x)·f f f1(x x x,v v v)},c(x x x,ζ2,ζ1)+ϑ2(x x x)−ϑ1(x x x))(2.10) Similar equations hold for ϑ2. A problem of this type is considered in [53], where a person must reach some point in minimum time, either by walking or skating. Walking or skating correspond in that case to the two discrete states, and there is a time penalty for putting on or taking off the skates, which corresponds to the switching cost. 2.3.3 Issues with the application of the principle of optimality In some problems it can be nontrivial to apply the principle of optimality directly. For instance, in a problem with fuel constraints, i.e., a constraint of the form Z∞ 0 |u u u(t)|dt≤α, Equation (2.8) is ‘incomplete’ since it might be the case that the concatenation of the two controls does not satisfy the fuel constraint even if the individual controls do. Thus, for the application of the principle of optimality the set of admissible controls should satisfy some conditions, namely [6]: 22 Background 1. If u u uis an admissible control, so is u u usfor all s>0, defined by u u us(t) = u u u(t+s) 2. If u u u1 and u u u2 are admissible controls, so is u u u defined as the concatenation of u u u1 up to time τ>0 with u u u2, i.e. u u u(s) =    u u u1(t)0≤t≤τ u u u2(t−τ)τ<t Additionally, there are conditions on the cost functional γ, namely γ(x x x(ti[.]tf)) = β(x x x(ti[.]t∗),α) when α=γ(x x x(t∗[.]tf)) , and β is continuous and non-decreasing in α . Under these conditions γ is called a positional functional [31]. The principle optimality is applicable to cost functionals which are positional. 2.3.4 Numerical methods for solving Hamilton-Jacobi equations A static Hamilton-Jacobi equation is a first-order nonlinear PDE of the type H(x x x,∇ϑ(x x x)) = 0,x x x∈Γ ϑ(x x x) = q(x x x),x x x∈∂Γ(2.11) where Γ⊂Rn is open, H:Γ×Rn→R and ϑ:Γ∪∂Γ→R is the unknown function. These equations arise from several applications including geometric optics, computer vision, computational geometry, geophysics and optimal control. Note that (2.9) is itself a static Hamilton-Jacobi PDE, if we set H(x x x,∇ϑ(x x x)) :=inf v v v∈U{g(x x x,v v v)+∇ϑ(x x x)·f f f(x x x,v v v)} It is well known that even if H and q are smooth, a smooth solution of (2.11) need not exist, and one must adopt the notion of a viscosity solution [6]. For a given problem there can be an infinite number of nonsmooth ‘weak’ solutions, but only one matches the physical interpretation of the problem, and the viscosity solution is defined so that it matches this ‘physically correct’ solution [51]. In most numerical methods, the first step to the numerical solution of this type of equation is to discretize the domain Γ∪∂Γ into a finite set of points. The viscosity solution of (2.11) is then approximated over that set of points. These numerical methods can be classified as Eulerian, Lagrangian and Semi-Lagrangian. In Eulerian methods, the set of points is a fixed grid and the equation is discretized using finite differences. In Lagrangian methods, on the other hand, the set of points moves according to the solution (in the case of a Hamilton-Jacobi PDE originating from optimal control problem, each point would follow an optimal trajectory). Semi-Lagrangian methods 2.3 Dynamic Programming 23 are a hybrid, in that there is also a fixed grid, but the solution is calculated by moving each gridpoint as in a Lagrangian method for a small timestep. Here we discuss Eulerian and Semi-Lagrangian methods which are suitable for solving Hamilton-Jacobi equations arising from the application of dynamic programming to optimal control problems. The grid used in Eulerian and Lagrangian methods can be regular or unstructured. A regular (also known as Cartesian or rectangular) grid is a set of regularly spaced points x x xi1,i2,...,in discretizing a rectangular domain D which contains Γ∪∂Γ . An unstructured grid can be any set of points, with the advantage that it can be adapted to the geometry of Γ∪∂Γ . In both cases, the grid has a graph-like structure, so that each gridpoint has a set of neighbors. The set of points which do not have a neighbor in some direction is the computational boundary. Although most of the methods described below have extensions to unstructured grids, we focus on regular grids. In that case, every point not in the computational boundary has two neighbors for each dimension, and the computational boundary consists of points on the boundary of the rectangular domain D. The simplest case of (2.11) can be seen as the Hamilton-Jacobi-Bellman PDE of a particularly simple instance of the minimum-cost-to-target problem we considered in Section 2.3.2.2. If the dynamics are given by the equation ˙ x x x(t) = u u u(t),u u u(t)∈Bγ, where Bγ is a closed ball with radius γ centered at the origin, then the PDE for the value function is γ|∇ϑ(x x x)|=g(x x x)(2.12) which is the well-known Eikonal equation from geometric optics (here |p p p| denotes the standard Euclidean norm of p p p∈Rn ). If an approximation of ϑ is known, the optimal control value at each point can be computed from the formula u u u(t) = −∇ϑ(x x x(t)) |∇ϑ(x x x(t))| Tsitsiklis [64] described an efficient Semi-Lagrangian method for the approximation of ϑ based on direct discretization of this optimal control problem. At each grid point, the optimal path passing through that grid point is approximated by a straight line until it exits the simplex defined by the point under consideration and its neighbors. The cost-to-go at the point is then approximated by the cost of the straight line path plus the cost-to-go at the intersection of the path and the simplex, which is linearly interpolated from the values of the cost-to-go at neighboring nodes. Hence, if the problem is to be solved on a grid of side h , letting ∆ be the unit simplex in Rn , A={−1,1}n and e e eibe the i-th coordinate vector in Rn,ϑis approximated using the expression ϑ(x x x)≈min α∈A,θ∈∆(g(x x x)h γ|θ|+ n ∑ i=1 θiϑ(x x x+hαie e ei))(2.13) 30 Background Figure 2.11: LSTS toolchain components DUNE is a platformand architecture-independent runtime environment written in C ++ , providing onboard software components for sensing, control, navigation, communication and vehicle supervision. The runtime environment comprises a set of independent building blocks which are called ‘DUNE tasks’. Each task has a distinct purpose and runs in a separate thread of execution. Examples of DUNE tasks are sensor drivers, controllers or loggers. Tasks communicate by publishing and subscribing to IMC message types which are forwarded by a global message bus [16,46]. The set of tasks which is active in any particular instance of DUNE is defined in a runtime configuration file. For instance, a configuration file can be defined for a particular vehicle, enabling tasks which are relevant for the vehicle’s hardware and payload. This configuration file can also be used to define configuration parameters, so that tasks can be written in a vehicle-independent fashion, and adapted to each vehicle at runtime. Sets of tasks can also be enabled and disabled according to different operating profiles. Available profiles are for instance Never (the task is never enabled), Always (the task is always enabled), Simulation (the task is enabled when there is no hardware in the loop), HIL (for hardware-in-the-loop simulation), and Hardware (for running DUNE with the vehicle’s onboard computer connected to the actual vehicle hardware). [45,46]. These different components support a layered control architecture where a mission is defined as a set of plans, each of which is a sequence of maneuvers. A maneuver is a high-level control objective such as a waypoint the vehicle should reach, a loiter command or a trajectory to be tracked. In DUNE, maneuver controllers then translate these objectives into navigation and guidance commands, which are used as actuation references by low-level controllers [45,46]. The different layers interact via IMC messages, so that plans and maneuvers can be generated either onboard by the vehicle or through an operator console such as the one provided by Neptus, with no change in the lower layers [45]. As described in Estrela da Silva et al. [16], DUNE contains an underwater vehicle simulation engine which effectively replaces a vehicle’s sensors and actuators without affecting the rest of the toolchain ecosystem, since the simulator communicates with the remaining components via IMC messages. Thus, the toolchain can be used for realistic software-in-the-loop simulation and testing. 2.4 The LSTS software toolchain 31 (a) A Neptus console (b) Ripples Also included in the LSTS toolchain are Ripples, a cloud-based centralized hub for data disseminations and situation awareness, ACCU, a command and control Android application, and GLUED, a minimal Linux distribution for embedded systems. The toolchain is currently used in over 15 countries, both by academic research groups and in the industry. 32 Background Chapter 3 Problem Description 3.1 The base problem The simplest version of the type of problems considered in this work can be stated as follows: Given the initial position of a vehicle, the deployment time and a forecast of the ocean currents in the operational area, what is the best trajectory which takes the vehicle to some target position? Of course, one has to specify the meaning of ‘best’ trajectory, and this is done using a cost function, which allows us to compare or rank trajectories. The most common objectives would be to minimize the traveling time or energy. However, one may also be interested in generating risk-averse trajectories, for instance. There is an immediate generalization of this problem which is important in practice. Namely, the deployment position and time should be allowed to vary. Consider for instance the case of deployment of an unmanned vehicle from other marine craft, such as a ship or a manned submarine, where practical circumstances may force changes in the deployment position. Even if one can accommodate these changes, it is only logical to want to choose the deployment position in order to obtain the best possible performance. The situation is even more dramatic when it comes to the deployment time: in practice one may have only a window of time for the deployment, and in some scenarios of practical interest (such as deployments in zones with tidal-forced ocean currents) the deployment time can make the difference between feasibility and infeasibility of the mission objectives. Thus, an updated version of the problem statement reads as follows: Given a possible range of initial positions of a vehicle, a range of deployment times, and a forecast of the ocean currents in the operational area, what is the best trajectory which takes the vehicle to some target position, for each possible initial position and time? At this point we can introduce some notation to express this rigorously. In what follows we consider planar motions only, although it is directly applicable to trajectories in three-dimensions. We denote by x x x(t)∈R2 the horizontal position of the vehicle at some time t∈R . Consider first that the deployment position and time are fixed and equal to ξ ξ ξ0 and ti , respectively, and let the target position be ξ ξ ξT . A trajectory is then a function x x x:[ti,tf]→R2 mapping time instants to vehicle 33 34 Problem Description Ω x x x(t) v v v(t,x x x) Figure 3.1: A trajectory reaching the target set Ωunder the influence of the ocean current v v v. positions, which naturally should be at least continuous. We should consider only those trajectories satisfying x x x(ti) = ξ ξ ξ0and x x x(tf) = ξ ξ ξT. Trajectories are compared using an integral cost function: J(x x x) = Ztf ti g(x x x(t))dt(3.1) where gis a piecewise continuous positive real-valued function which gives the cost per unit time of travelling through a given point. The simplest example would be to take g identically equal to one, in which case J(x x x) = Ztf ti dt=tf−ti, which equals the time taken by the vehicle to reach ξ ξ ξT . If g is for instance a risk map which assigns a risk level to each position, the optimal trajectories will be the trajectories which minimize the cumulative risk of travelling from the deployment position to the deployment time. The ‘best’ trajectory x x x? can then be defined as a minimizer of this cost function over all trajectories x x x: x x x?∈argminJ(x x x)x x x:[ti,tf]→R2,x x x(ti) = ξ ξ ξ0,x x x(tf) = ξ ξ ξT.(3.2) Note that tfis unspecified in this formulation. If we consider that the initial time and position can range over some set, the formulation is the same, but the optimal trajectory then depends on the initial time and position: x x xt,ξ ξ ξ ?∈argminJ(x x x)x x x:[t,tf]→R2,x x x(t) = ξ ξ ξ,x x x(tf) = ξ ξ ξT. A basic extension of this problem is to consider a target set Ω⊂R2 rather than a single target position, as illustrated in Figure 3.1 ( v v v is the forecasted ocean current). In that case it makes sense to consider a cost function of the form J(x x x) = q(x x x(tf)) + Ztf ti g(x x x(t))dt 3.2 Planning with logic-based constraints 35 where q is a nonnegative piecewise continuous real-valued function assigning to each point of Ω the cost of ending the mission at that point. In this case, the optimal trajectory x x xt,ξ ξ ξ ?is defined as x x xt,ξ ξ ξ ?∈argminJ(x x x)x x x:[t,tf]→R2,x x x(t) = ξ ξ ξ,x x x(tf)∈Ω. As an example, consider Ω=nξ ξ ξ1 T,ξ ξ ξ2 To , i.e., we have a two possible points where the vehicle can end the mission. These can represent two different harbors where the vehicle can be recovered, for instance. If we would rather have the vehicle stop at ξ ξ ξ1 T than at ξ ξ ξ2 T , we can set q(ξ ξ ξ1 T)<q(ξ ξ ξ2 T) to promote trajectories ending at ξ ξ ξ1 T , but still allow the vehicle to stop at ξ ξ ξ2 T if the integral component of the cost dominates. 3.2 Planning with logic-based constraints Going beyond single-stage problems, we want to be able to plan a whole mission given a high-level specification of the objectives and constraints. As an example, consider the following mission specification for an ocean vehicle in an estuarine environment: 1. The vehicle is deployed at sea from a predefined location ξ ξ ξstart ; the deployment time is unspecified. 2. After it is deployed the vehicle should go towards a prespecified region Ωinside the river. 3. After arriving at Ω, the vehicle should remain inside it for at least 60 minutes. 4. After the 60 minutes have passed, the vehicle should begin the exfiltration at some unspecified time and go toward some safe region Γoutside the river. Here too the trajectories will be optimized according to a cost function: J(x x xin,x x xout) = γstart(τstart)+Zτarrival τstart g(x x xin(t))dt+γexit(τexit)+Zτend τexit g(x x xout(t))dt+γend(τend,x x xout(τend)) where x x xin :[τstart,τarrival]→R2 denotes the vehicle trajectory from its starting position to the target region inside the river and x x xout :[τexit,τend]→R2 denotes the vehicle trajectory during the exfiltration. The cost functions γstart and γexit can be used to specify preferences and constraints on the start and exit times, respectively, while γend allows us to do this on the end time and the arrival position, similarly to the function qin the base problem. The optimal solution to the problem is then defined as (x x xin ?,x x xout ?)∈argminJ(x x xin,x x xout)x x xin :[τstart,τarrival]→R2,x x xout :[τexit,τend]→R2,τexit −τarrival ≥60, x x xin(τstart) = ξ ξ ξstart,x x xin(τarrival)∈Ω,x x xout(τexit)∈Ω,x x xout(τend)∈Γ 36 Problem Description (assuming that the times are given in minutes). Note that this has a similar form to the minimization in the first problem, only involving two trajectories. The times τstart,τarrival,τexit,τend are not fixed, and are chosen as part of the minimization. The problem of optimizing an entire mission can be arbitrarily complex, given a sufficiently complex mission specification. In this work we focus on a class of problems which generalize the given example, which are n -stage single-vehicle missions. At the start of stage i,i=0,...,n−1 we want the vehicle to be in the target set Ωi , and between stages i and i+1 the vehicle follows a trajectory x x xi:[τi,τi+1]→R2 . The last trajectory x x xn−1 consists of the single point x x xn−1(τn−1) . Define a cost function for each stage recursively as J0(x x x0) = γ0(τ0,x x x0(τ0)) Ji+1(x x xi+1,x x xi,...,x x x0) = Ji(x x xi,...,x x x0)+Zτi+1 τi gi(x x xi(t))dt+γi+1(τi+1,x x xi+1(τi+1)). The optimal mission plan is then a selection of trajectories (x x x0 ?,...,x x xn−1 ?) which minimizes the cost function of the last stage: (x x x0 ?,...,x x xn−1 ?)∈argminJn−1(x x xn−1,...,x x x0)x x x0(τ0)∈Ω0, x x x0(τ1) = x x x1(τ1)∈Ω1,...,x x xn−2(τn−1) = x x xn−1(τn−1)∈Ωn−1. Note that the trajectories should be consistent, i.e. x x xi(τi+1) = x x xi+1(τi+1) . As in the example problem, the τiare not fixed. Stage 0 ˙c(t) = g0(x x x0(t)) Stage 1 ˙c(t) = g1(x x x1(t)) ... Stage n−1 c(τ0):=γ0(τ0,x x x0(τ0)) c(τ1):=c(τ− 1)+γ1(τ− 1,x x x1(τ− 1)) c(τ2):=c(τ− 2)+γ2(τ− 2,x x x2(τ− 2)) c(τn−1):=c(τ− n−1)+γn−1(τ− n−1,x x xn−1(τ− n−1)) Figure 3.2: Representation of the general n-step problem as a hybrid automaton The problem is represented as a hybrid automaton in Figure 3.2, where c(t) is the temporal evolution of the cost. 3.2 Planning with logic-based constraints 37 Our initial example fits in this general formulation by setting Ω0={ξ ξ ξstart},γ0(ξ ξ ξstart,τ0) = γstart(τ0),x x x0=x x xin,g0=g Ω1=Ω,γ1=0,g1=0 Ω2=Ω,γ2(τexit,x x x(τexit)) = γexit(τexit),x x x2=x x xout,g2=g Ω3=Γ,γ3=γend. Note that there was no cost imposed on the trajectory of the vehicle during the second stage, only that the vehicle stay inside Ω . It is assumed that the vehicle can hold its position while it is inside Ω, so that x x xout(τexit) = x x xin(τarrival). 38 Problem Description Chapter 4 Related Work In this chapter we provide an overview of the literature on trajectory optimization for ocean vehicles which is most relevant for this work. In particular, we focus on methods for route optimization using ocean current data. For this reason, we do not discuss methods which also have an optimization component but which have radically different objectives, such as path planning for adaptive sampling. Inanc et al. [26] describe a method for trajectory optimization based on an optimal control formulation. A holonomic motion model is adopted, and the cost function is a linear combination of the travelling time and the expended energy. An optimal control solver which parametrizes the optimal trajectories using B-spline functions is used to convert the optimal control problem to a nonlinear optimization problem. This method is susceptible to local minima as the resulting optimization problem is nonconvex. Simulations using ocean current data from high frequency radar stations show that there is a link between the optimal trajectories and Lagrangian Coherent Structures in the flow. Namely, an optimal trajectory departing from a point in the LCS to a point in the LCS stays in the LCS. The authors suggest that this may be used as a heuristic for trajectory generation. The time-varying nature of the ocean currents is accounted for using a receding horizon approach, which can lead to suboptimal solutions. Hence, this work is extended by Zhang et al. [71], by considering a fully time-varying model of the ocean currents. Zhang et al. also consider a dynamic model of an AUV with turning rate constraints, but do not compare the results with the original holonomic model. The link between LCS and the optimal trajectories is shown to hold in this case. Petres et al. [44] discuss an extension of fast marching method for underwater path planning which incorporates elements of the A* algorithm. The original motivation is obstacle avoidance using underwater imagery, from which an obstacle map is derived. The ocean currents, which are assumed to be static, as well as the obstacles are included in the cost function. An ordered upwind method to solve the resulting equation is derived. The authors also show that the radius of curvature of the optimal paths is lower bounded. Soulignac et al. [57,58] point out that the resulting paths can be unfeasible in the presence of strong currents (i.e., currents with speed exceeding the vehicle’s maximum speed), and propose a different wavefront expansion algorithm based on a 39 46 Approach Since for each x x xΩ∈Ωone has J(x x xΩ,τ,u u u) = q(x x xΩ), Vmust satisfy the boundary conditions V(τ,x x x) = q(x x x),x x x∈Ω. If the target set is specified directly as a subset Ω0 of R3 , the partial differential equation is unaffected, but the boundary condition is then V(τ,x x x) = q(x x x),(x x x,τ)∈Ω0. Assuming we have computed V, we can use it to compute an optimal feedback law: u u u(τ,x x x) = −rVx x x(τ,x x x) |Vx x x(τ,x x x)|(5.6) Then we have two ways of using this feedback law. The first is to simply plug it in the motion model, so that we get an autonomous nonlinear differential equation: ˙ x x x(τ) = v v v(τ,x x x(τ))−rVx x x(τ,x x x(τ)) |Vx x x(τ,x x x(τ))|(5.7) We can solve this differential equation with any initial condition x x x(τ0) = x x x0 to obtain an optimal trajectory with that initial condition. Such a trajectory can then be used as a reference for the lower level level control loops in the vehicle’s onboard navigation and control software. Alternatively, the feedback law (5.6) can be used directly as a heading controller. Since we are concerned with trajectory generation, our focus is on the first option. Note that the components of the cost q and g can depend on τ with no changes to the resulting equations; for simplicity we considered them to be time-invariant. 5.2.1 Incorporating obstacles and constraints When operating in areas such as estuarine regions, the vehicle will have to be able to navigate around the geography and bathymetry of the region to prevent it from running aground. This must be included directly in the formulation because the optimal unobstructed path can be quite different from the optimal path which takes into account the geography and bathymetry constraints. In addition, it may be desirable to set other constraints such as no-go zones and to-avoid zones. These constraints can be added in one of two ways, as hard constraints or soft constraints. Hard constraints are adequate for the geography constraints or no-go zones. If those points are represented by a set Kh, we impose an additional boundary condition: V(τ,x x x) = M,x x x∈Kh, 5.3 Planning with logic-based constraints 47 where M is large in the sense that any trajectory having a cost larger than M is considered infeasible in practice. This is equivalent to adding the points in Kh to the target set with a large value of q . For instance, if the cost function is the time taken to reach the target, M can be any number larger than the length of the mission time window. This way, any trajectory which contains a point in Kh will have a cost of at least M , and the target is considered unreachable from the corresponding initial condition. Soft constraints are adequate for to-avoid zones. If we want to add a soft constraint for the set Ks, we can simply add a multiple of the indicator function of Ks 1Ks(x x x) =    1,x x x∈Ks 0,x x x/∈Ks to the cost component g . Trajectories crossing Ks will then pay an additional cost proportional to the amount of time spent inside Ks. We observe that this problem is not amenable to the application of exact penalization methods such as those described in Clarke et al. [11]. 5.3 Planning with logic-based constraints In this section we extend the previous result to the generic n-step problem described in Section 3.2. We begin by relaxing the constraints x x xi(τi)∈Ωi . These can be included in the functions γi , by setting γito a high value outside of the set Ωi. Given two points x x xi,x x xi+1∈R2 and two time points ti,ti+1 , either there is no trajectory x x x(·) of the vehicle satisfying x x x(ti) = x x xi,x x x(ti+1) = x x xi+1 or there is a single such trajectory which minimizes the integral cost Zti+1 ti gi(x x x(t))dt. Consider an optimal solution to the problem, (x x x0,...,x x xn−1) . Then x x xi must be the trajectory connecting x x xi(τi) and x x xi+1(τi+1) which minimizes the integral cost; for if not then the cost Jn−1 can be reduced by replacing x x xi with the optimal trajectory connecting those two points, and this would imply that the solution is not optimal after all. Hence we can reduce the problem to that of planning the initial positions of the vehicle at each stage, ξ ξ ξi=x x xi(τi) , and the associated times τi . Once the ξ ξ ξi and τi have been found, the solution to the original problem can be recovered by solving n−1 optimal control problems for the trajectories connecting those positions. This is similar to the class of problems considered in Alton and Mitchell [4], and here we adapt their approach. We begin by defining δi(τ0,ξ ξ ξ0,τ,ξ ξ ξ) = infZτ0 τ gi(x x x(t))dtx x x(τ) = ξ ξ ξ,x x x(τ0) = ξ ξ ξ0. 48 Approach Then the cost functions for each stage can be expressed in terms of the τiand ξ ξ ξi: J0(τ0,ξ ξ ξ0) = γ0(τ0,ξ ξ ξ0) Ji+1(τi+1,ξ ξ ξi+1,...,τ0,ξ ξ ξ0) = Ji(τi,ξ ξ ξi,...,τ0,ξ ξ ξ0)+δi(τi+1,ξ ξ ξi+1,τi,ξ ξ ξi)+γi+1(τi+1,ξ ξ ξi+1) We can then define the functions Vi,Wias Vi(τ,ξ ξ ξ) = min (τi−1,ξ ξ ξi−1,...,τ0,ξ ξ ξ0)Ji(τ,ξ ξ ξ,...,τ0,ξ ξ ξ0),i=0,...,n−1 Wi(τ,ξ ξ ξ) = min (τi,ξ ξ ξi,...,τ0,ξ ξ ξ0){Ji(τi,ξ ξ ξi,...,τ0,ξ ξ ξ0)+δi(τ,ξ ξ ξ,τi,ξ ξ ξi)},i=0,...,n−2. The value Vi(τ,ξ ξ ξ) is the optimal cost up to stage i if τi=τ and x x xi(τi) = ξ ξ ξ , i.e., if the vehicle starts stage i of the mission at time τ and at position ξ ξ ξ . The value of Wi(τ,ξ ξ ξ) is the optimal cost of performing stages 0,...,i−1 and then travelling to ξ ξ ξ. Obviously we have V0=γ0, and so W0(τ,ξ ξ ξ) = min τ0,ξ ξ ξ0γ0(τ0,ξ ξ ξ0)+δ0(τ,ξ ξ ξ,τ0,ξ ξ ξ0)=W0(τ,ξ ξ ξ) = min τ0,ξ ξ ξ0V0(τ0,ξ ξ ξ0)+δ0(τ,ξ ξ ξ,τ0,ξ ξ ξ0). This relation actually holds for all i: Wi(τ,ξ ξ ξ) = min (τi,ξ ξ ξi,...,τ0,ξ ξ ξ0){Ji(τi,ξ ξ ξi,...,τ0,ξ ξ ξ0)+δi(τ,ξ ξ ξ,τi,ξ ξ ξi)} =min (τi,ξ ξ ξi)(min (τi−1,ξ ξ ξi−1,...)Ji(τi,ξ ξ ξi,...)+δi(τ,ξ ξ ξ,τi,ξ ξ ξi)) =min (τi,ξ ξ ξi){Vi(τi,ξ ξ ξi)+δi(τ,ξ ξ ξ,τi,ξ ξ ξi)}. There is also a simple formula linking Vi+1and Wi: Vi+1(τ,ξ ξ ξ) = min (τi,ξ ξ ξi,...)Ji+1(τ,ξ ξ ξ,...) =min (τi,ξ ξ ξi,...){Ji(τi,ξ ξ ξi,...)+δi(τ,ξ ξ ξ,τi,ξ ξ ξi)}+γi+1(τ,ξ ξ ξ) =Wi(τ,ξ ξ ξ)+γi+1(τ,ξ ξ ξ). Assuming for the moment that we can compute δi , using these relations we can compute all the Vi and Wi (since V0 is known). We now show that the optimal solution can be calculated using Vi and Wi . Let (τn−1,ξ ξ ξn−1,...,τ0,ξ ξ ξ0) be an optimal solution. Suppose that Vn−1(τn−1,ξ ξ ξn−1)>Vn−1(τ,ξ ξ ξ) for some (τ,ξ ξ ξ). Then Vn−1(τ,ξ ξ ξ)≥Jn−1(τ,ξ ξ ξ,τn−2,ξ ξ ξn−2,...,τ0,ξ ξ ξ0) ≥Jn−1(τn−1,ξ ξ ξn−1,τn−2,ξ ξ ξn−2,...,τ0,ξ ξ ξ0) 5.3 Planning with logic-based constraints 49 implying that Vn−1(τn−1,ξ ξ ξn−1)>Jn−1(τn−1,ξ ξ ξn−1,τn−2,ξ ξ ξn−2,...,τ0,ξ ξ ξ0)≥Jn−1(τn−1,ξ ξ ξn−1,τ0 n−2,ξ ξ ξ0 n−2,...,τ0 0,ξ ξ ξ0 0) for any (τ0 n−2,ξ ξ ξ0 n−2,...,τ0 0,ξ ξ ξ0 0), which contradicts the definition of Vn−1. Hence (τn−1,ξ ξ ξn−1)∈arg min (τ,ξ ξ ξ)Vn−1(τ,ξ ξ ξ).(5.8) Now suppose that Vn−2(τn−2,ξ ξ ξn−2)+δn−2(τn−1,ξ ξ ξn−1,τn−2,ξ ξ ξn−2)>Vn−2(τ,ξ ξ ξ)+δn−2(τn−1,ξ ξ ξn−1,τ,ξ ξ ξ). Then Vn−2(τn−2,ξ ξ ξn−2)+δn−2(τn−1,ξ ξ ξn−1,τn−2,ξ ξ ξn−2)+γn−1(τn−1,ξ ξ ξn−1)> Vn−2(τ,ξ ξ ξ)+δn−2(τn−1,ξ ξ ξn−1,τ,ξ ξ ξ)+γn−1(τn−1,ξ ξ ξn−1) min (τ0 n−3,ξ ξ ξ0 n−3,...,τ0 0,ξ ξ ξ0 0) Jn−1(τn−1,ξ ξ ξn−1,τn−2,ξ ξ ξn−2,τ0 n−3,ξ ξ ξ0 n−3,...,τ0 0,ξ ξ ξ0 0)> min (τ0 n−3,ξ ξ ξ0 n−3,...,τ0 0,ξ ξ ξ0 0) Jn−1(τn−1,ξ ξ ξn−1,τ,ξ ξ ξ,τ0 n−3,ξ ξ ξ0 n−3,...,τ0 0,ξ ξ ξ0 0) Jn−1(τn−1,ξ ξ ξn−1,τn−2,ξ ξ ξn−2,τn−3,ξ ξ ξn−3,...,τ0,ξ ξ ξ0)> min (τ0 n−3,ξ ξ ξ0 n−3,...,τ0 0,ξ ξ ξ0 0) Jn−1(τn−1,ξ ξ ξn−1,τ,ξ ξ ξ,τ0 n−3,ξ ξ ξ0 n−3,...,τ0 0,ξ ξ ξ0 0). The last inequality contradicts the fact that the (τi,ξ ξ ξi)constitute an optimal solution. Hence (τn−2,ξ ξ ξn−2)∈arg min (τ,ξ ξ ξ)Vn−2(τ,ξ ξ ξ)+δn−2(τn−1,ξ ξ ξn−1,τ,ξ ξ ξ). By the same argument, we conclude that (τi,ξ ξ ξi)∈arg min (τ,ξ ξ ξ)Vi(τ,ξ ξ ξ)+δi(τi+1,ξ ξ ξi+1,τ,ξ ξ ξ).(5.9) for all i. Thus, any optimal solution satisfies (5.8) and (5.9) . To show that the converse also holds, 50 Approach consider an arbitrary tuple (τn−1,ξ ξ ξn−1,...,τ0,ξ ξ ξ0). Then Jn−1(τn−1,ξ ξ ξn−1,...,τ0,ξ ξ ξ0) = γn−1(τn−1,ξ ξ ξn−1)+ n−2 ∑ i=0 γi(τi,ξ ξ ξi)+δi(τi+1,ξ ξ ξi+1,τi,ξ ξ ξi) ≥min τ0 0,ξ ξ ξ0 0(γn−1(τn−1,ξ ξ ξn−1)+ n−2 ∑ i=0 γi(τi,ξ ξ ξi)+δi(τi+1,ξ ξ ξi+1,τi,ξ ξ ξi)) =γn−1(τn−1,ξ ξ ξn−1)+W0(τ1,ξ ξ ξ1)+ n−2 ∑ i=1 γi(τi,ξ ξ ξi)+δi(τi+1,ξ ξ ξi+1,τi,ξ ξ ξi) =γn−1(τn−1,ξ ξ ξn−1)+V1(τ1,ξ ξ ξ1)+δ1(τ2,ξ ξ ξ2,τ1,ξ ξ ξ1)+ n−2 ∑ i=2 γi(τi,ξ ξ ξi)+δi(τi+1,ξ ξ ξi+1,τi,ξ ξ ξi) ≥γn−1(τn−1,ξ ξ ξn−1)+W1(τ2,ξ ξ ξ2)+ n−2 ∑ i=2 γi(τi,ξ ξ ξi)+δi(τi+1,ξ ξ ξi+1,τi,ξ ξ ξi) =γn−1(τn−1,ξ ξ ξn−1)+V2(τ2,ξ ξ ξ2)+δ2(τ3,ξ ξ ξ3,τ2,ξ ξ ξ2)+ n−2 ∑ i=3 γi(τi,ξ ξ ξi)+δi(τi+1,ξ ξ ξi+1,τi,ξ ξ ξi) ≥... ≥Vn−1(τn−1,ξ ξ ξn−1) ≥min τ,ξ ξ ξVn−1(τ,ξ ξ ξ). If (τn−1,ξ ξ ξn−1,...,τ0,ξ ξ ξ0) satisfies (5.9) and (5.8) , then each of the inequalities is an equality, so Jn−1is minimized. Hence, the optimal solutions can be recovered from Wi and Vi . Thus, all that is left is a way to efficiently compute δi . Using the motion model, we can derive a partial differential equation which allows us to effectively compute Wi , Vi and δi simultaneously. As in the previous problem, we consider that the vehicle’s trajectories are solutions of the differential equation ˙ x x x(t) = u u u(t) + v v v(t,x x x(t)) with u u u∈Br . Let U[a,b] be the set of measurable control functions u u u:[a,b]→Br . For each u u u∈U[τ,τ0] denote by x x x(t;τ,ξ ξ ξ,u u u) the value at time t of the solution of the differential equation which satisfies x x x(τ) = ξ ξ ξ, where t∈[τ,τ0]. We can write δias δi(τ0,ξ ξ ξ0,τ,ξ ξ ξ) = infZτ τ gi(x x x(t;τ,ξ ξ ξ,u u u))dtx x x(τ0;t,ξ ξ ξ,u u u) = ξ ξ ξ0,u u u∈U[τ,τ0], so that Wican be expressed as Wi(τ,ξ ξ ξ) = min (τ0,ξ ξ ξ0)Vi(τ0,ξ ξ ξ0)+δi(τ,ξ ξ ξ,τ0,ξ ξ ξ0)=inf τ0,u u u∈U[τ0,τ]Vi(τ0,x x x(τ0;τ,ξ ξ ξ,u u u))+Zτ0 τ gi(x x x(t;τ,ξ ξ ξ,u u u))dt. The function Wi can hence be seen as a value function for an optimal starting problem: defining the 5.3 Planning with logic-based constraints 51 cost function Hi(τ,ξ ξ ξ,τ0,u u u) = Vi(τ0,x x x(τ0;τ,ξ ξ ξ,u u u))+Zτ0 τ gi(x x x(t;τ,ξ ξ ξ,u u u))dt, we have Wi(τ,ξ ξ ξ) = inf τ0,u u u∈U[τ0,τ]Hi(τ,ξ ξ ξ,τ0,u u u) This is a type of optimal control problem where, besides the usual optimization of the control, the initial time is also optimized. We adapt the proofs in Bardi and Capuzzo-Dolcetta [6], who developed a dynamic programming approach for a time-invariant optimal stopping problem. We begin by noting that Wi≤Vi, since Wi(τ,ξ ξ ξ)≤Hi(τ,ξ ξ ξ,τ,0 0 0) = Vi(τ,ξ ξ ξ) Now fix some ξ ξ ξ , τ , s≤τ and u u u∈U[s,τ] . Let ε>0 . By definition there exist τε and u u uε∈U[τε,s]such that Hi(s,x x x(s;τ,ξ ξ ξ,u u u),τε,u u uε)≤Wi(s,x x x(s;τ,ξ ξ ξ,u u u))+ε. Letting ¯ u u ube the concatenation of u u uεand u u u, Wi(t,ξ ξ ξ)≤Hi(τ,ξ ξ ξ,τε,¯ u u u) =Vi(τε,x x x(τε;s,x x x(s;τ,ξ ξ ξ,u u u),u u uε))+Zs τεgi(x x x(t;s,x x x(s;τ,ξ ξ ξ,u u u),u u uε))dt+Zτ s gi(x x x(t;τ,ξ ξ ξ,u u u))dt =Hi(s,x x x(s;τ,ξ ξ ξ,u u u),τε,u u uε)+Zτ s gi(x x x(t;τ,ξ ξ ξ,u u u))dt ≤ε+Wi(s,x x x(s;τ,ξ ξ ξ,u u u))+Zτ s gi(x x x(t;τ,ξ ξ ξ,u u u))dt Since εis arbitrary, we conclude that Wi(τ,ξ ξ ξ)≤Zτ s gi(x x x(t;τ,ξ ξ ξ,u u u))dt+Wi(s,x x x(s;τ,ξ ξ ξ,u u u)) (5.10) for any s≤τ and u u u∈U[s,τ] . Note that this is a weak form of the dynamic programming principle, and all we have to show now is that equality holds when u u u is optimal. If Wi(τ,ξ ξ ξ) = Vi(τ,ξ ξ ξ) , then the optimal starting time is τand the optimal control is the empty function. Hence, we consider τ and ξ ξ ξsuch that Wi(τ,ξ ξ ξ)<Vi(τ,ξ ξ ξ). Let {(τn,u u un)}∞ n=1be a sequence such that τn≤τand lim n→∞Hi(τ,ξ ξ ξ,τn,u u un) = Wi(τ,ξ ξ ξ). Bardi and Capuzzo-Dolcetta [6] show that if Vi is bounded and uniformly continuous, then so is Wi . Hence we assume that V0 (which we are free to choose) is bounded and uniformly continuous, so that all the Vi and Wi are too. Therefore, by uniform continuity we can find a function ω:R≥0→R≥0 52 Approach such that ω(0) = lim t→0ω(t) = 0 Vi(τ0,ξ ξ ξ0)−Vi(τ0,ξ ξ ξ0)≤ω(τ0−τ0+ξ ξ ξ0−ξ ξ ξ0). Then, setting µn=Hi(τ,ξ ξ ξ,τn,u u un)−Wi(τ,ξ ξ ξ), one has Wi(τ,ξ ξ ξ)+ µn=Vi(τn,x x x(τn;τ,ξ ξ ξ,u u un))+Zτ τn gi(x x x(t;τ,ξ ξ ξ,u u un))dt ≥Vi(τn,x x x(τn;τ,ξ ξ ξ,u u un)) ≥Vi(t,ξ ξ ξ)−ω(|τ−τn|+|ξ ξ ξ−x x x(τn;τ,ξ ξ ξ,u u un)|). If some subsequence of τn tends to τ , then taking a limit along that subsequence in each side of the above inequality yields Wi(τ,ξ ξ ξ)≥Vi(τ,ξ ξ ξ) , contradicting our initial assumption. Hence we can find some τ0<τsuch that τn≤τ0for all nsufficiently large. For s∈[τ0,τ], Hi(τ,ξ ξ ξ,τn,u u un) = Hi(s,x x x(s;τ,ξ ξ ξ,u u un),τn,u u un)+Zτ s gi(x x x(t;τ,ξ ξ ξ,u u un))dt ≥Wi(s,x x x(s;τ,ξ ξ ξ,u u un))+Zτ s gi(x x x(t;τ,ξ ξ ξ,u u un))dt ≥inf u u u∈U[s,τ]Wi(s,x x x(s;τ,ξ ξ ξ,u u u))+Zτ s gi(x x x(t;τ,ξ ξ ξ,u u u))dt taking a limit as n→∞and using (5.10), we conclude Wi(τ,ξ ξ ξ) = inf u u u∈U[s,τ]Wi(s,x x x(s;τ,ξ ξ ξ,u u u))+Zτ s gi(x x x(t;τ,ξ ξ ξ,u u u))dt. We can obtain a PDE form of this dynamic programming principle via the usual procedure. We have for h≥0 0≤inf u u u∈U[τ−h,τ]Wi(τ−h,x x x(τ−h;τ,ξ ξ ξ,u u u))−Wi(τ,ξ ξ ξ)+Zτ τ−h gi(x x x(t;τ,ξ ξ ξ,u u u))dt with equality if Wi<Vi. Dividing by hand taking a limit as h→0, 0≤min u u u∈Brgi(ξ ξ ξ)−∇ξ ξ ξWi(τ,ξ ξ ξ)·(u u u+v v v(τ,ξ ξ ξ))−∂Wi ∂τ (τ,ξ ξ ξ). Therefore we have 0≥Wi(τ,ξ ξ ξ)−Vi(τ,ξ ξ ξ) 0≥max u u u∈Br∇ξ ξ ξWi(τ,ξ ξ ξ)·(u u u+v v v(τ,ξ ξ ξ))+ ∂Wi ∂τ (τ,ξ ξ ξ)−gi(ξ ξ ξ) 5.4 Selection of the numerical algorithm 53 and at least one of these is equal to zero. This can be summarized as 0=maxWi(τ,ξ ξ ξ)−Vi(τ,ξ ξ ξ),max u u u∈Br∇ξ ξ ξWi(τ,ξ ξ ξ)·(u u u+v v v(τ,ξ ξ ξ))+ ∂Wi ∂τ (τ,ξ ξ ξ)−gi(ξ ξ ξ). The maximizer is u u u=r∇ξ ξ ξWi(τ,ξ ξ ξ) |∇ξ ξ ξWi(τ,ξ ξ ξ)|, so this becomes 0=maxWi(τ,ξ ξ ξ)−Vi(τ,ξ ξ ξ),∇ξ ξ ξWi(τ,ξ ξ ξ)·v v v(τ,ξ ξ ξ)+r∇ξ ξ ξWi(τ,ξ ξ ξ)+∂Wi ∂τ (τ,ξ ξ ξ)−gi(ξ ξ ξ). (5.11) This type of equation is known as a variational inequality. The PDE term, however, is almost exactly what we encountered previously. Note also the similarity to (2.10) , which was to be expected due to the interpretation of the n -stage problem as a standard optimal control problem for a hybrid system. Using these results, we have the following algorithm for computing the optimal trajectories: 1. Calculate W0,V1,W1,...,Vn−1 in that order by solving (5.11) and using the relation Vi+1= Wi+γi+1. 2. Find the optimal ending time and position from (τn−1,ξ ξ ξn−1)∈arg min (τ,ξ ξ ξ)Vn−1(τ,ξ ξ ξ) 3. For i=n−2,...,0, integrate the differential equation ˙ x x xi(τ) = r∇ξ ξ ξWi(τ,x x xi(τ)) ∇ξ ξ ξWi(τ,x x xi(τ))+v v v(τ,x x xi(τ)) (5.12) backwards in time with terminal condition x x xi(τi+1) = ξ ξ ξi+1 until Wi(τ,x x xi(τ)) = Vi(τ,x x xi(τ)) , at which point set τi=τ,ξ ξ ξi=x x xi(τ). 5.4 Selection of the numerical algorithm As we typically can not hope to solve (5.5) analytically, both because of the complexity of the equation itself and the fact that v v v is given in practice by numerical data and not by an analytical expression, we must turn to the numerical methods described in Section 2.3.4. Equation (5.5) is a static Hamilton-Jacobi equation, and its Hamiltonian is H(z z z,p p p) = rqp2 2+p2 3−(p1+p2v1(z z z)+ p3v2(z z z)) where z z z= (τ,x x x) and p p p= (p1,p2,p3) = (Vτ,Vx x x) . Given the type of equation, we are limited to the following options: •Ordered upwind methods [54] •Level set methods [41] 54 Approach •Semi-Lagrangian methods [17] •Fast sweeping methods [28] Ordered upwind methods require that H be homogeneous in p p p , so that there is a function F such that H(z z z,p p p) = |p p p|Fz z z,p p p |p p p| and this is satisfied in our case with F=H . However, there is an additional requirement on F , namely that that F be bounded below and above by positive numbers. In our case, if p p p= (1,0,0) then F=−1 , so this is clearly not satisfied. This is related to the fact that, in terms of the optimal control problem associated to a given Hamilton-Jacobi equation, ordered upwind methods require local controllability in all directions [54], and we clearly don’t have controllability in the time direction. The use of level set methods is based on converting between (5.5) and the related equation φs+r|φx x x|−(φτ+φx x x·v v v) g(x x x)=0 via a change of variables, where φ=φ(s,τ,x x x) . Level set methods allow us to solve this equation for φ and then find V from φ . Thus we have to solve an equation on a space with an extra dimension. The extra dimension weighs considerably on both memory requirements and the computational cost, and we would like to use a more efficient and scalable method. Semi-Lagrangian methods are also not the most efficient choice, since these methods explicitly find the optimal control at each point by solving the minimization in (2.14) . Since we have an explicit form of the Hamilton-Jacobi-Bellman equation in this case (i.e., the minimization in (2.9) is explicitly solved), this minimization step is unnecessary, and we can solve the HJBE directly. Hence, the logical choice seems to be the class of fast sweeping methods. Since we have an explicit form of the Hamiltonian, we can use the Lax-Friedrichs discretization of the Hamiltonian described in [28]. 5.5 Implementation of a numerical solver based on the fast sweeping method To the best of our knowledge, there is at the time of writing no publicly available implementation of fast sweeping methods for general Hamilton-Jacobi equations. Hence it was necessary to implement a fast sweeping method solver from scratch. Our goal is to have an implementation which is able to quickly solve standard-sized problems and which will scale to problems in more than two dimensions or large two-dimensional problems. Increasing numbers of desktop computers and laptops now ship with multi-core processors [68], and high-performance computing (HPC) platform nodes typically have at least 16 processing units. It makes sense to leverage this computing power to achieve the stated goal through a parallel implementation of the fast sweeping method. 5.5 Implementation of a numerical solver based on the fast sweeping method 55 5.5.1 Preliminary design decisions The development environment and target platform is x86_64 Linux. Nonetheless, portability was kept in mind while making the design decisions that follow. The logical choice for the programming language of the implementation was C ++ , due to a multitude of reasons [22,61]: •Zero-cost abstractions; •C-like efficiency with a stronger type system which makes writing correct programs easier; •Support for generic, object-oriented and concurrent programming paradigms; • Interfaces for most or all of the widely used C and FORTRAN scientific computing libraries are available, so adding new functionalities or integrating new libraries will not force a change of programming language. It is also important to carefully choose the version of the C ++ standard to be used in development. On the one hand, a recent standard will provide more language and library features, but on the other it requires the availability of a recent compiler release. We want to be able to build and run the solver on FEUP’s HPC platform Grid FEUP, which has the GNU Compiler Collection (GCC) version 7.3.1 installed, so we opted for the ISO C ++ 2014 standard, which is fully supported by GCC since version 5 1. Since we want to solve at least three-dimensional problems, data input/output via text files is not practical, and a proper file format for data storage must be used. We chose the Hierarchical Data Format version 5 (HDF5) file format for the following reasons: • The HDF5 C library is either preinstalled or easily downloadable in most (if not all) Linux distributions; •There are multiple C++ interfaces available; •Most data processing software supports reading and writing HDF5 format files; • Filesystem-like interface which allows storing multiple related datasets in a single binary file, with metadata stored alongside the data; •Native support for multidimensional datasets. For reading and writing HDF5 files in C ++ , the h5cpp library was chosen due to its simple modern C ++ interface. Besides the HDF5 C library, the h5cpp library depends only on the Boost C++ libraries, which typically come preinstalled in any Linux distribution, so the integration cost is minimal. The CMake build system generator is used to simplify the build process and promote portability. The source code repository is managed using the git version control system and hosted on GitHub. 1See https://gcc.gnu.org/projects/cxx-status.html 62 Approach The solution is computed over the square [−1,1]3 . The parameters for the fast sweeping solver are M=2.0, ε=1×10−16,σi(x x x) = 1. The results for various grid sizes are summarized in Table 5.1. The computations were performed on a laptop running Linux with an Intel(R) CoreTM i7-8550U CPU @ 1.80GHz. The absolute error decreases only slightly worse than linearly as the resolution h=1 N−1 decreases, where N is the number of gridpoints per dimension, consistent with the results reported in Kao et al. [28]. 5.5.5 First parallel implementation The first attempt at a parallel implementation is based on the first method described in [73]. As described in Section 2.3.4.1, the method consists simply of performing the different sweeps simultaneously. Thus, we have 2d threads of execution, and must keep 2d+1 different copies of the solution in memory: 2d copies to be able to perform each sweep in different data, and an extra copy to store the old value of the solution, so that we can keep track of the convergence progress. S S S S B B B B M M M M Figure 5.2: Task structure in the main loop for d=2 The task structure is represented in Figure 5.2 for d=2 (For d6=2 the only difference is the number of tasks in each step). The black nodes represent steps where tasks are spawned, white nodes represent tasks, arrows represent the task flow and the ‘walls’ represent steps where all preceding tasks must finish before proceeding. The nodes labelled ‘S’ denote a sweep, those labelled ‘B’ denote the boundary update and those labelled ‘M’ denote the ‘merge’ or synchronization step where the different solutions are combined and the `∞ norm of the difference between the values before and after the iteration is calculated. In order to parallelize the merge step, we divide the gridpoints among the worker threads and each of them calculates the solution value on its part of the grid as well as the `∞ difference between iterations on that part of the grid. After all threads have finished the merge step, the maximum difference among all parts of the grid is found, which is the `∞ difference between iterations on the whole grid. During the sweep and boundary updates, each thread is working only on its own array of solution values, so no locking or synchronization is necessary. In the merge step, all threads have access to all the solution arrays, but each thread only reads from/writes to a segment of the array which no other thread is accessing, so there is no risk of data races here either. 5.5 Implementation of a numerical solver based on the fast sweeping method 63 Hence, the only possible parallel overhead is due to thread creation. In order to avoid creating a thread each time a task is spawned, we use a thread pool. This is a data structure which starts a given number of threads at program startup. Tasks can then be pushed to a task queue and when a thread is idle it will pop a task from the queue and execute it [68]. We use an implementation based on the open source thread pool library available at https://github.com/progschj/ThreadPool . This implementation uses the concurrency facilities available in the C ++ standard library, so that it is platform-independent and introduces no additional dependencies. Since at program startup we already have one thread of execution (the main thread), we only need to start 2d−1 additional threads for the thread pool. In the main loop, the main thread enqueues the first 2d−1 sweep tasks in the thread pool queue and executes the last sweeping direction itself. After it is done executing its sweep, the main thread waits for all other threads to finish the sweep and boundary update. The same method is used for the merge step, where the main thread splits the grid into 2d sections, queues the first 2d−1 sections on the thread pool and executes the merge task on the last section before waiting on the remaining threads. 5.5.5.1 Validation and benchmarks The implementation is validated using the same example as before (three-dimensional eikonal equation), using the same platform and settings. The laptop used has four cores with 2 processing units each, so that 8 threads threads can be executed in parallel, which is just what is needed for a three-dimensional problem. The results are shown in Table 5.2. The speedup is calculated as the wall clock time for the single-threaded implementation divided by the wall clock time for the parallel implementation. The parallelization does not seem to bring any advantage. This is because a larger number of iterations are necessary for convergence when the sweeps are done in parallel, which cancels out the reduction in the time taken to compute a single iteration. Gridpoints per dimension Wall clock time (s) Speedup `∞Absolute error 51 10.07 0.64 0.28797977078281090 101 105.85 0.61 0.15862420696771684 121 197.90 0.61 0.13533268851117053 151 452.57 0.56 0.11132716009398247 171 748.37 0.82 0.09973797954314612 201 1313.37 1.07 0.08643654921007737 221 1860.80 1.01 0.07946110092067005 251 2762.36 1.06 0.07096529729804213 Table 5.2: Computation time and absolute error for various grid sizes – first parallel implementation 5.5.6 Second parallel implementation Due to the shortcomings of the method used for the first parallel implementation, which were confirmed in the benchmarks, we opted to implement the hyperplane stepping method [13], which 64 Approach promises near-optimal parallel speedup. Recall that the gist of the method is to partition the grid into sets of points (‘levels’) Lm such that the points in Lm can be updated simultaneously. As we are targeting a multi-CPU architecture where the number of threads will be small compared to the number of points in Lm , we split the points in Lm into as many groups as there are threads, and each thread updates its group of points concurrently. The number of threads is not determined by any of the problem parameters and can be set freely. In our implementation, it can be set at compile time by the user via a constexpr variable. Alternatively, if the user does not provide the desired number of threads, we use the std::thread::hardware_concurrency function from the C ++ standard library which lets us determine the number of parallel threads supported by the hardware in a platform independent way. If std::thread::hardware_concurrency is is unable to determine the number of parallel threads it will return zero, in which case we use two threads. U U U U M L0 U U U U M L1 ... ... U U U U M LM−1 Figure 5.3: Task structure during sweeping with four threads Figure 5.3 represents the task structure during sweeping with four threads. The ‘U’ tasks consist of updating a group of points in a level. As in the single-threaded implementation, the `∞ norm difference of the difference before and after the sweep is calculated as the points are being updated. After a ‘U’ task has finished, we have the norm of the difference for the corresponding group of points. The purpose of the ‘M’ tasks is to keep track of the maximum norm of the difference among the groups and the previous levels that have been calculated, so that after the last ‘M’ task has finished we have the norm of the difference on the whole grid. The total number of levels is M=1+∑d k=1(Nk−1). Similarly to the previous implementation, the ‘U’ tasks are assigned to threads using a thread pool, and the main thread performs a ‘U’ task itself after it has scheduled all the other tasks. If d=2, generating the sets Lmis simple. Starting from the point x x xi,jwith i=min{m,N1−1},j=m−i we generate the next point in Lmby moving along the diagonal: i←i−1,j←j+1 5.5 Implementation of a numerical solver based on the fast sweeping method 65 until either i=0 or j=N2−1 . For d>2 we can do it recursively on the number of dimensions. Let L0 m be the levels for the grid with dimensions (N2,...,Nd) . Then we can iterate over the points x x xi1,...,id∈Lmusing the following algorithm: 1. For m0=m,m−1,...,0: (a) i1←min{m0,N1−1},n←m0−i1,S←L0 n (b) While S6=/0: i. Remove a point x x xj2,...,jdfrom S ii. For k=2,...,d,ik←jk As in [13], instead of generating the levels in different orders for the different sweeping directions, we can generate them for a single order and rotate the axes for the different sweeping orders, i.e., if we are sweeping in the order given by s , we apply the following transformation to each point in Lm: 1. For k=1,...,d: (a) If s& 2k−16=0, ik←Nk−1−ik In addition, instead of generating the sets Lm in every iteration, we can generate and store them after the solver has been set up. This implies storing ∏d k=1Nk points, so it might not be an option for large or high-dimensional problems, depending on the available memory. 5.5.6.1 Validation and benchmarks We validate the implementation using the same example, settings and platform as in the previous two implementations. Eight parallel threads were used in the computations. The results are shown in Table 5.3. The implementation is clearly more efficient and nearoptimal speedup is achieved for the larger problems. Note that the absolute errors are exactly the same as in the single-threaded implementation tests, which is evidence that the points are updated in the same sequence (so that the number of iterations is the same). Gridpoints per dimension Wall clock time (s) Speedup `∞Absolute error 51 3.19 2.01 0.28797977078281200 101 19.02 3.40 0.15862420696771995 121 32.84 3.69 0.13533268851117297 151 65.25 3.91 0.11132716009398536 171 101.90 6.01 0.09973797954314878 201 182.85 7.69 0.08643654921008004 221 245.44 7.67 0.07946110092067471 251 386.06 7.57 0.07096529729804590 Table 5.3: Computation time and absolute error for various grid sizes – second parallel implementation 66 Approach 5.6 Configuration of the solver for optimal planning problems 5.6.1 Base problem In order to solve the equation numerically, the first step is to choose an hyperrectangle on which the solution is calculated, i.e., a product of intervals D= [τmin,τmax]×[xmin 1,xmax 1]×[xmin 2,xmax 2]. This computational domain Dshould contain the three-dimensional target set Ω0. Note that we will only be able to compute the value of V(τ,x x x) for (τ,x x x) such that τ+ T(x x x,τ,u u u?)<τmax , where u u u? is the optimal control, since if this inequality does not hold at a gridpoint, the optimal trajectory from that point is not contained in the computational boundary. We then discretize this domain with a given resolution in each dimension. It makes sense to discretize x1 and x2 with the same resolution, because there is no preferred direction of motion. Hence we choose resolutions δτ and δx x x for time and position, respectively. The number of gridpoints in each dimension is chosen so that the resolution is equal to or finer than the specified resolutions: Nτ=1+ceilτmax −τmin δτ Nx1=1+ceilxmax 1−xmin 1 δx x x Nx2=1+ceilxmax 2−xmin 2 δx x x ( ceil(x) denotes the ceiling of x , i.e., the smallest integer larger than x ). The effective resolution in each dimension will then be hτ=τmax −τmin Nτ−1≤δτ hxi=xmax i−xmin i Nxi−1≤δx x x,i=1,2. and the gridpoints are defined as τi=τmin +i·hτ,i=0,...,Nτ−1 xj 1=xmin 1+j·hx1,j=0,...,Nx1−1 xk 2=xmin 2+k·hx2,k=0,...,Nx2−1. The output of a numerical solver will be the approximate values of the solution at the gridpoints: Vi,j,k=V(τi,xj 1,xk 2). To minimize roundoff errors and ensure that all operations are done with as much precision as 5.6 Configuration of the solver for optimal planning problems 67 possible, it is wise to normalize the problem data so that D is as close to a unit cube as possible and the grid cell widths hτ,hx1,hx2are within an order of magnitude of each other. The target set is specified simply as a binary mask on the gridpoints, which determines which gridpoints are considered to belong to Ω0 . The terminal cost q specifies the values of the solution on those gridpoints: Vi,j,k=q(xj 1,xk 2) where xj 1,xk 2 are such that the gridpoint (τi,xj 1,xk 2) is in the discretized target set. The running cost function g and the values of the ocean flow velocity v v v= (v1,v2) also only need to be specified at the gridpoints. The fast sweeping method needs two additional user inputs, namely the initial value M of the numerical solution Vi,j,k at points not in the target set and the tolerance ε . The initial value M should be an upper bound of the value function over the computational region. For our problem, this is easy to estimate: Vi,j,k≤(τmax −τmin)gmax +qmax where gmax is an upper bound on g over the computational region, and qmax is an upper bound on q on the target set. As for the tolerance ε , we could simply take it to be equal to the machine epsilon (approximately 2.22 ×10−16 for 64-bit double precision). However, the error due to the discretization will almost always be orders of magnitude larger than this value, as shown in the solver benchmarks. Hence it is probably wiser to set ε according to the problem data, that is, choosing it such that a difference of ε in the value of the value function at a gridpoint does not change the meaning of that value. For instance, if we are solving a minimum time problem, then a difference of one second in the optimal cost from some point is immaterial if the optimal cost from that point is in the order of several hours. The values of the solution along the time axis follow a causality property. Namely, the value of the solution at earlier times depends on the value of the solution at later times: V(τ0,x x x0) = Zτ τ0 g(x x x(t))dt+V(τ,x x x(τ)) where x x x(t) is an optimal trajectory, and the values of the solution at the terminal times are determined by q . This means that the values at the computational boundary τ=τmax should not be updated using formula (2.20) , since it uses the values at points with τ<τmax to determine the value at the points with τ=τmax. In fact, the correct approach is to not updated the points with τ=τmax at all, since one of two things will happen: 1. The point is in the target set, and then its correct value is already determined by q; 2. The point is not in the target set, in which case the optimal trajectory starting at the point would necessarily exit the computational region, so the optimal cost from that point can not be determined. In this case, the target should be considered unreachable from that point, and 68 Approach the value of the value function at that point is equal to +∞ . For our purposes, the upper bound M is the same as infinity, and that is the initial value of the value function at the gridpoints not in the target set; therefore the correct approach is to not update these points. 5.6.2 Planning with logic-based constraints In terms of the selection of the computational domain and its discretization, as well as the selection of the tolerance parameter ε , the procedure is exactly the same as in the base problem. The main difference lies in the boundary conditions. The variational equation (5.11) does not specify any boundary conditions, but since we have that Wi≤Vi , the numerical approximation of Wi should be initialized with the values of Viin every gridpoint. We can discretize (5.11) as 0=maxnWi1,i2,i3 i−Vi1,i2,i3 i,Wi1,i2,i3 i−update(Wi)o where update(Wi) is the Lax-Friedrichs update formula (2.19) adapted to the PDE which appears in the second argument of the max in (5.11). This is the same as Wi1,i2,i3 i=minnVi1,i2,i3 i,update(Wi)o. Since Wi is initialized as Vi and the fast sweeping method never allows the solution to become larger than the initial value, the first part of the min is unnecessary. Therefore, the update function for this variational inequality is exactly the same as the one for Hamilton-Jacobi PDEs, and the solver does not need to be adapted in any way to solve this equation. There is again a causality property which should be observed, however, in this case values of the value function at later times depend on the values at earlier times, i.e., the direction of the causality in this case is forward in time. This is because there is an initial cost given by Vi , instead of a terminal cost as in the base problem. Hence, the τ=τmin boundary should not be updated in this case. 5.7 Calculating optimal trajectories from the value function After an approximation of the value function has been computed by numerically solving the HJBE, we can calculate an optimal trajectory with initial condition x x x(τ0) = x x x0 for any (τ0,x x x0)∈D by integrating the ordinary differential equation (5.7) with this initial condition. This can be done using any standard ODE integration method such as Euler’s method or fourth order Runge-Kutta. The spatial gradient Vx x x can be calculated in each gridpoint using a finite-difference formula, but we must be able to calculate the right-hand side of the equation at any point of Din order to use such methods. This is done by interpolating the gradient Vx x x to the point of interest. If v v v is also known only at the gridpoints then it must also be interpolated when solving (5.7) . The same method is used for numerically integrating (5.12). 5.7 Calculating optimal trajectories from the value function 69 It is important to note that despite the fact that we only have knowledge of the value function at a discrete set of points, we can generate trajectories from any deployment position and time inside the computational region. Additionally, the trajectories are not a discrete sequence of gridpoints but continuous curves which can be sampled with arbitrarily small timesteps (within the limits of floating point computations, obviously). 70 Approach Chapter 6 Numerical examples 6.1 Test cases The method will be validated using three test cases which will illustrate its effectiveness and usability in real-life operations scenarios. These will be performed using two datasets from high resolution ocean models of the Sado and Tejo estuaries in Portugal. The ocean current data is courtesy of Américo S. Ribeiro, João Miguel Dias and Renato Mendes (NMEC-CESAM, Physics Department, University of Aveiro, Portugal). Following is a description of each of the tests. Entering and exiting the Sado estuary – We consider two mission scenarios in the Sado estuary: 1. The vehicle is deployed outside the estuary; the objective is to enter the estuary and reach a given location in minimum time; 2. The vehicle starts from a point located inside the estuary and must reach a target area outside the estuary in minimum time. Each of the two scenarios will be tested using 24 different tidal periods extracted from the data. This allows us to get an estimate of the computational time of the method for a real life problem, as well as to see how different patterns in the ocean current affect the solution. The Sado estuary is an area with tidal-driven currents with large velocities, reaching and sometimes exceeding 2 meters per second. The geography of the region also makes it an interesting test site, as the vehicle has to go through a narrow channel to enter or exit the inner part of the estuary, and there are a lot of low-depth ‘islands’ which the vehicle must avoid. Currents with high spatial variability – The second test will involve an ocean flow in the Sado and Tejo estuaries which displays rapid variations in its direction with respect to the spatial variable. 71 78 Numerical examples the upper bound of M=12 hours (since the cost is the time to target) and the tolerance was set at ε=1×10−4hours, which is less than a second. The computations were performed on a laptop running Linux with a four-core Intel (R) Core TM i7-8550U CPU @ 1.80GHz with 8 parallel threads. The solver code was compiled using the GCC C ++ compiler version 9.1.0 with level 3 optimizations enabled. The computation times reported in Table 6.2 measure only the time taken to solve the HJBE, i.e., the time taken to load and write data to disk is not included. Figure 6.5 shows the values of the optimal cost as a function of the deployment position (i.e., the values of V(τ,·) ) for six fixed values of τ , the deployment time, for the first test case. The target set is shown in blue. Figure 6.5a shows the contours for τ=τmin . The highest cost values for this deployment time are about 6 hours, so at the latest the vehicle arrives at the target at high tide, before the currents have reversed direction. This means that the optimal trajectory from each deployment position will face favorable currents, which explains the homogeneity of the cost values across the operational area (modulo the distance to the target). In Figure 6.5b, we can see that at τ=τmin +2 hours some points which are furthest away from the target are unable to reach it within the mission time frame. Notice that although the points on the bottom-left of the operational area are closer to the target than those on the bottom-right, the points on the right side have similar lower cost values, which is due to the currents having a more favorable direction on the right side on the operational area. Four hours after low tide, the current magnitude is starting to decrease, and the vehicle has only two hours to reach the target before facing opposing currents. This is reflected in Figure 6.5c, which shows that the target set is unreachable from most points when starting at that deployment time. Figure 6.5d shows that after 6 hours only the points closest to the target are able to reach it, with the cost increasing rapidly as the distance to the target increases on the left side. In Figures 6.5e and 6.5f we see that the target is only reachable within the mission time frame from points on its right side due to the strong outflow currents. The optimal trajectories are calculated using the method described in 5.7. Here we integrate the ODE using Euler’s method with a timestep of one second, and the values of the gradient and the ocean flow in points not on the grid are calculated by linear interpolation. The gradient of the value function is calculated using a second order finite difference formula. Four trajectories generated using the value function corresponding to the first tide period in Table 6.2 are shown in Figure 6.6. The deployment position and the target position are indicated by the blue circle and the red star, respectively. The titles in the figures indicate the deployment time. The ocean current velocity is superimposed on the trajectory every 15 minutes. Note that once the vehicle has entered the channel, the ocean current is nearly tangent to its trajectory. This can be verified in Figure 6.7, where the flow magnitude along the trajectory and the projection of the flow velocity on the vehicle velocity (the cosine of the angle between the two velocity vectors) are shown. As mentioned in Section 5.1, the motion model does not take into account the vehicle’s limited turning rate. We can verify whether the trajectories satisfy this constraint by calculating their 6.3 Entering and exiting the Sado estuary 79 (a) (b) (c) (d) (e) (f) Figure 6.5: Values of V(τ,·)for six different values of τ, for the first tide period in Table 6.2 80 Numerical examples (a) (b) (c) (d) Figure 6.6: Four optimal trajectories calculated using the result of test #1 (a) (b) (c) (d) Figure 6.7: Ocean current magnitude and projection of the ocean current velocity on the vehicle’s velocity for the trajectories in Figure 6.6 6.3 Entering and exiting the Sado estuary 81 minimum radius of curvature using the standard formula Rmin =min t |˙ x x x|3/2 ˙x1¨x2−˙x2¨x1 For the trajectories shown in Figure 6.6 the minimum radius of curvature is approximately 306 m, 285 m, 235 m and 197 m (clockwise from the top left), which is well above the minimum for most marine vehicles. For instance, the LAUV class AUVs can track trajectories with radius of curvature above 5 meters when the forward velocity is 1 meter per second [63]. Although we can not measure the error in the value function, as the exact solution is not known, we can get an idea of its accuracy by comparing the value of the value function at the initial condition of the trajectories with the cost of the calculated trajectory, which is easily calculated numerically. In this case the cost is the time taken to reach the target, so it is enough to check the number of time steps in the integration. For the trajectories shown in Figure 6.6, the value of V(τ0,x x x(τ0)) is 3.82, 3.69, 3.42 and 3.46 hours (clockwise from top left), while the actual time to target for each trajectory is 3.54, 3.42, 3.20 and 3.25 hours. Considering the second set of values to be the true values of the optimal cost, the relative errors are 7.91%, 7.89%, 6.88% and 6.46%. Note that the numerical approximation to the value function is an upper bound of the actual value of the cost, which is not guaranteed by the method, but was to be expected since the value function is initialized with a value which is an upper bound to the true cost. This error is due both to the finite discretization of the computational region and the finite number of iterations. However, note that an error in the value function is not qualitatively relevant for the optimal trajectories unless it changes the direction of the gradient. As the currents are time-dependent, the optimal trajectories starting from a fixed deployment position at different initial times can be quite different. Figure 6.8 shows trajectories corresponding to deployment times 30 minutes apart over an interval of 3 hours for test cases #1 and #8 in Table 6.2. In Figure 6.8b we observe that the trajectories corresponding to the two last initial times are significantly different from the trajectories departing at earlier times, and in fact the two groups of trajectories approach the island at 38.448870 o N, 8.961995 o W from different sides. This indicates that the trajectories are indeed globally optimal. If an iterative trajectory generation methods based on improving an initial guess was used, the solution would inevitably approach the obstacle from the same side as the initial guess, which can result in a solution that is only a local minimum. Since the trajectories are time-optimal, one way to measure the impact of the ocean currents on the trajectory is by comparing the distance travelled by the vehicle along the optimal trajectory with the distance the vehicle would travel in the same amount of time if there were no ocean currents, i.e. g=s rT −1 (6.1) where s is the arc length of the trajectory, T is the time taken to reach the target and r is as always the speed of the vehicle. This is the total gain in velocity from the ocean currents. The total 82 Numerical examples (a) Test #1 (b) Test #8 Figure 6.8: Effect of varying the deployment time on the optimal trajectories 6.3 Entering and exiting the Sado estuary 83 arclength s is easily calculated: if the optimal trajectory is x x x and the result of Euler’s method is the set of points x x x0,...,x x xn, then s=ZT τ|˙ x x x(t)|dt≈ n−1 ∑ i=0 |x x xi+1−x x xi|.(6.2) For the trajectories shown in Figure 6.6, the values of the velocity gain g are approximately 44.3%, 51.2%, 55.0% and 60.5% (clockwise from the top left). Besides its role in trajectory generation, the value function contains information which can be useful for mission planning. For instance, we can calculate the optimal deployment time for each possible deployment position by finding the value of τ which minimizes V(τ,x x x) for fixed x x x . This is shown in Figure 6.9 for an approximately 11.7 km by 5.7 km area in the lower latitudes of the operational area, for some of the test runs in Table 6.2. For each point in the area the color indicates the optimal deployment time in hours relative to τmin, the first time instant for which the value function is computed, which is indicated in the title. Note that the plots are qualitatively different, which shows that details in the ocean flow play a major role, and the optimal cost is not solely determined by the geometry of the operational area. In fact, note the discontinuity in Figures 6.9b and 6.9f. This is not a numerical artifact, but reflects the fact that the optimal trajectories departing from these points at the optimal deployment time are qualitatively quite different, as shown in Figure 6.10. The large areas with optimal deployment time equal to τmin in Figures 6.9a,6.9d and 6.9e (dark blue) suggest that deployment times earlier than τmin could lead to smaller optimal cost. Hence this can also be used to adjust the computational boundary after an initial guess based on the tide tables. We can plot the velocity gain g corresponding to the trajectory which departs from each point in the considered area at the optimal deployment time, as in Figure 6.11. Once again the maps corresponding to different tests are qualitatively different, and some of the patterns visible in Figure 6.9 are apparent. There are more discontinuities, since the velocity gain is strongly influenced by the geometry of the trajectories, and here too the discontinuities separate regions where the trajectories are qualitatively different. This can be verified in Figure 6.12, where pairs optimal trajectories starting from points which are close but on different sides of the discontinuities in Figure 6.11e are shown. Note that in Figures 6.11b and 6.11f, in the left side of the considered area, there are some points where the gain value is missing. This is because the optimal trajectory from these points is not contained in the operational area, so it cannot be calculated. This unavoidable if the flow is strong and pointing towards the outside of the computational region, and should be taken into account. One way of roughly predicting this phenomena is to check points near the computational boundary where the ocean current speed is larger than the vehicle’s speed, and in any case the areas considered for deployment should not be too close to the computational boundary. 84 Numerical examples (a) Test #1 (b) Test #2 (c) Test #3 (d) Test #13 (e) Test #15 (f) Test #21 Figure 6.9: Maps of the optimal deployment time in a subset of the deployment area 6.3 Entering and exiting the Sado estuary 85 Figure 6.10: Two optimal trajectories corresponding to two different sides of the discontinuity in Figure 6.9f 86 Numerical examples (a) Test #1 (b) Test #2 (c) Test #3 (d) Test #13 (e) Test #15 (f) Test #21 Figure 6.11: Maps of the velocity gain in a subset of the operational area 6.3 Entering and exiting the Sado estuary 87 Figure 6.12: Trajectories starting on different sides of the discontinuities in Figure 6.11e are geometrically distinct 94 Numerical examples (a) Trajectories departing at τmin (b) Trajectories departing at τmin +1 hour (c) Trajectories departing at τmin +2 hours (d) Trajectories departing at τmin +3 hours Figure 6.18: Optimal trajectories departing from four different points on the southwest of the operational area (a) Trajectories departing at τmin (b) Trajectories departing at τmin +0.5 hours (c) Trajectories departing at τmin +1 hour (d) Trajectories departing at τmin +1.5 hours Figure 6.19: Optimal trajectories departing from 100 uniformly distributed points on the southwest of the operational area. 6.4 Currents with high spatial variability 95 Figure 6.18 shows that the trajectories indeed take relatively sharp turns when crossing the interfaces between regions in the flow with distinct behavior. However, the radius of curvature of each of the trajectories shown in Figure 6.18 is lower bounded by 600 meters. Figure 6.19 shows trajectories departing from 100 randomly selected points from a uniform distribution (in UTM coordinates) over a rectangular 5 km by 15 km selection of the operational area. The minimum radius of curvature among all the trajectories is 360 meters, and only 7 of the 400 trajectories have a minimum radius of curvature smaller than 600 meters. Hence, the results confirm the hypothesis that for ‘physical’ models of the ocean currents the optimal trajectories are feasible, at least as long as the cost function does not induce sharp turns in the trajectories. 96 Numerical examples 6.5 Software-in-the-loop simulations In this section we report results of simulations of the generated trajectories with the LSTS toolchain. The purpose of these simulations is twofold. First, we want to confirm that an ocean vehicle can satisfactorily track the trajectories generated by the method. Second, we want to understand how the method can be integrated in existing frameworks for unmanned vehicle missions, such as the LSTS toolchain. In Neptus, a curved trajectory can be defined using the FollowPath or the FollowTrajectory maneuvers. The FollowPath maneuver specifies the trajectories as a sequence of North, East and Down direction displacements given in meters, relative to an initial point specified in WGS84 coordinates. The FollowTrajectory maneuver is similar, except it allows the specification of an arrival time at each point relative to the time at which the maneuver starts being executed. While executing a FollowTrajectory maneuver, the vehicle varies its speed in order to reach each point as close to its associated time as possible. While our method generates trajectories and not just geometric paths, we want the vehicle to travel at constant speed, so we use the FollowPath maneuver. The curved trajectories generated by the method must be sampled in order to represent them as FollowPath maneuvers. Sampling uniformly in time, however, would lead to a large number of points to represent a trajectory. Since each of the points in a FollowPath maneuver is treated as a waypoint, there is a heading transient every time a point is reached, so a large number of points leads to bad tracking performance. The solution is to have few sample points when the trajectory is nearly a straight line and more sampling points when the trajectory has high curvature. This is done using a method similar to that described in Pagani and Scott [43]. We define a new parameter q as q(t) = αs(t) s(T)+(1−α)Rτ t0k(τ)dτ RT t0k(τ)dτ(6.3) where s is the arclength parameter, k is the positive curvature and α is an adjustable parameter. Then we sample the trajectory x x x(t) uniformly along this parameter, i.e. the sample points are x x x(t0),x x x(t0+q−1(h)),x x x(t0+q−1(2h)),...,x x x(T) , where h=1 N−1 for a given number N of sample points. The α parameter controls the weight between sampling at a fixed distance between sampling points or sampling only according to the variation in the curvature. Since DUNE’s simulation engine only allows setting a fixed value for the ocean current, we implemented an ocean current simulator and integrated in the simulation engine. A new simulation task was implemented which consumes the estimated position of the vehicle, reads current data from an HDF5 format file, interpolates it to the current position of the vehicle and dispatches it in an IMC EstimatedStreamVelocity message. This message is in turn consumed by the vehicle simulation engine, which uses the value of the ocean current velocity to update the vehicle’s position. In keeping with the design principles described in Section 2.4, the vehicle simulator is independent of the source of the ocean current velocity values, so that in other scenarios other sources of data (e.g., remote sensing) can be used without changing the simulator source code. 6.5 Software-in-the-loop simulations 97 Figure 6.20: The third test trajectory in the Neptus operator console We simulate three trajectories from the first test case described in Section 6.3.2 using the DUNE configuration file for LSTS’s LAUV Noptilus 1 AUV. Figure 6.20 shows the vehicle tracking the third test trajectory in the Neptus operator console. The results are shown in Figures 6.21 to 6.23. Note that the tracking errors are low relative to the scale of the trajectories and the spatial resolution of the computational grid (50 m). To further improve the results, one could take advantage of the feedback form of the solution, and the trajectory could be replanned online by the vehicle, either periodically in a receding-horizon type scheme, or when a certain tracking error threshold is exceeded. 98 Numerical examples (a) Simulated and planned position (b) Simulated and planned ocean current velocity components along the trajectory (c) Tracking error and Euclidean norm of the difference between the planned and simulated ocean current velocities. Figure 6.21: Results of the first software-in-the-loop simulation 6.5 Software-in-the-loop simulations 99 (a) Simulated and planned position (b) Simulated and planned ocean current velocity components along the trajectory (c) Tracking error and Euclidean norm of the difference between the planned and simulated ocean current velocities. Figure 6.22: Results of the second software-in-the-loop simulation 100 Numerical examples (a) Simulated and planned position (b) Simulated and planned ocean current velocity components along the trajectory (c) Tracking error and Euclidean norm of the difference between the planned and simulated ocean current velocities. Figure 6.23: Results of the third software-in-the-loop simulation Chapter 7 Conclusions and Future Work In that flash I realized, it’s impossible to fall off mountains, you fool! Jack Kerouac (in The Dharma Bums) 7.1 Summary We presented a dynamic programming-based approach to solver singleand multi-stage optimal trajectory generation problems for unmanned ocean vehicles subject to forcing by currents. In the simplest case of generating a single trajectory from the deployment position to a target region, the approach reduces the problem to the solution of a nonlinear first-order partial differential, the Hamilton-Jacobi-Bellman equation, the solution of which is the value function associated to the optimal control problem. Using our multithreaded C++ implementation of a fast sweeping method for Hamilton-Jacobi equations, we are able to solve real problems in a few minutes. After the value function has been calculated, globally optimal trajectories can be generated in real-time from any deployment position and time by integrating a first-order differential equation. We extended the approach to multi-stage single-vehicle missions with logic-based constraints, reducing the problem to the solution of a sequence of partial differential equations. Each of these partial differential equations is similar to the one encountered in the single-stage problem, so we can obtain the solution to multi-stage problems using our numerical solver. Using data from ocean flow models of the Sado estuary in Portugal, we confirmed the usability of the approach in real operational scenarios. The data is easily integrated in the framework, as are constraints arising from the geography and bathymetry of the operational region. These results show that the ocean currents can have a large impact on the optimal trajectories, and in some cases the average speed of the vehicle along the optimal trajectory is increased by more than 50 %. We also demonstrated how the value function can itself be useful in the process of mission planning, as it gives an estimate of the optimal cost over the operational area and mission time frame As such, it can be used for checking mission feasibility from a given deployment position 101 102 Conclusions and Future Work and time, planning the deployment position given a deployment time and vice-versa, or comparing possible deployment positions and times. Complex flow patterns and constraints are translated into quantitative visualizations such as those shown in Figures 6.5 and 6.14 and Figures 6.9 and 6.11, which are more easily interpreted by human operators. Although the motion model used in this work does not take into account the turning rate constraints of most ocean vehicles, the numerical examples suggest that the generated trajectories are nonetheless feasible for those vehicles. We further confirmed this through a test using a simulation of extreme conditions of the ocean flow in the Tejo estuary, where the mission objectives required the vehicle’s trajectory to cross the interface between two regions where the flow had radically different directions, and the results again showed that the generated trajectories are feasible for ocean vehicles. These results provide numerical evidence for the hypothesis that the cost function rather than the ocean currents is the most important factor affecting the curvature of the optimal trajectories. Finally, we showed that the approach can be integrated easily in state-of-the-art software frameworks for autonomous vehicle operations such as the LSTS toolchain. Using a simple sampling method we obtained satisfactory tracking performance, showing that our kinematic motion model is sufficient for global trajectory optimization purposes. 7.2 Future research directions The present work can be extended in several ways. Although here we limited ourselves to planar problems, the techniques can be directly extended to trajectory optimization in three-dimensional environments, the only difference being the additional computational burden. Since the numerical solver showed good performance in the two-dimensional case, we expect that it should be usable as-is for 3D environments. Given that the parallel implementation scales well with the number of processors, the use of grid computing facilities would make it simple to speed up computational times in that case, if needed. Our numerical examples were limited to coastal environments. However, the approach is not tied to any particular kind of area, nor is it limited in terms of the size of the operational area or the time frame. Thus it is directly applicable to large-scale missions. The only bottleneck is the computational time. As we remarked above, the results indicate that the current solver implementation should scale to larger problems. In any case, there are several improvements which could be used to further reduce the computation time. As remarked by Detrixhe et al. [13], the hyperplane stepping method is amenable to GPU-based implementations. Recent work [12] also showed how the method can be used in conjunction with a domain-decomposition approach in a heterogeneous computing context. Another possible improvement would be the use of unstructured grids adapted to the ocean flow to concentrate the resolution in regions where the value function can be expected to be more heterogeneous. Given the literature linking optimal trajectories to Lagrangian Coherent Structures [26,47,71], such structures could be used to directly generate a grid from the ocean flow velocity. 7.2 Future research directions 103 In what concerns the multi-stage problem, one possible direction is the use of distributed computing to simultaneously calculate the value functions associated to each stage. Extensions to multiple vehicle operations such as coordinated rendezvous could also be considered in the framework of Alton and Mitchell [4]. The method can be integrated in the LSTS toolchain in several ways, building on the sampling approach shown here. Trajectories can be generated onboard by the vehicle, integrating with real-time obstacle avoidance algorithms. Integration with Ripples could also be considered, by deploying the solver on a server or directly on a web page (via WebAssembly compilation). Mission objectives could then be defined and updated during operations, and the corresponding value functions calculated and disseminated to the vehicle or vehicles. Finally, we plan to evaluate and test the method in a real deployment to take place in the Sado river. This will be done in the context of the 10th edition of the Rapid Environmental Picture MUS exercise jointly organized by LSTS-Porto University, the Portuguese Navy and the Centre for Maritime Research and Experimentation. In this 10th edition the exercise will take place under the auspices of the Marine Unmanned Systems (MUS) initiative from NATO. 110 BIBLIOGRAPHY [68] Anthony Williams. C++ Concurrency in Action. Manning, Feb. 1, 2019. 568 pp. ISBN: 1617294691. [69] Jonas Witt and Matthew Dunbabin. “Go with the flow: Optimal AUV path planning in coastal environments”. In: Proceedings of the ACRA 2008 (Jan. 2008). [70] Russell B. Wynn et al. “Autonomous Underwater Vehicles (AUVs): Their past, present and future contributions to the advancement of marine geoscience”. In: Marine Geology 352 (June 2014), pp. 451–468. DOI:10.1016/j.margeo.2014.03.012. [71] Weizhong Zhang et al. “Optimal trajectory generation for a glider in time-varying 2D ocean flows B-spline model”. In: 2008 IEEE International Conference on Robotics and Automation. IEEE, 2008. DOI:10.1109/robot.2008.4543348. [72] Hongkai Zhao. “A fast sweeping method for Eikonal equations”. In: Mathematics of Computation 74.250 (May 2004), pp. 603–628. DOI:10.1090/s0025-5718-04-01678-3. [73] Hongkai Zhao. “Parallel implementations of the fast sweeping method”. In: J. Comp. Math. 25 (2007), pp. 421–429.