Online motion planning for unexplored underwater environments using autonomous underwater vehicles

We present an approach to endow an autonomous underwater vehicle with the capabilities to move through unexplored environments. To do so, we propose a computational framework for planning feasible and safe paths. The framework allows the vehicle to incrementally build a map of the surroundings, while simultaneously (re)planning a feasible path to a specified goal. To accomplish this, the framework considers motion constraints to plan feasible 3D paths, that is, those that meet the vehicle’s motion capabilities. It also incorporates a risk function to avoid navigating close to nearby obstacles. Furthermore, the framework makes use of two strategies to ensure meeting online computation limitations. The first one is to reuse the last best known solution to eliminate time‐consuming pruning routines. The second one is to opportunistically check the states’ risk of collision. To evaluate the proposed approach, we use the Sparus II performing autonomous missions in different real‐world scenarios. These experiments consist of simulated and in‐water trials for different tasks. The conducted tasks include the exploration of challenging scenarios such as artificial marine structures, natural marine structures, and confined natural environments. All these applications allow us to extensively prove the efficacy of the presented approach, not only for constant‐depth missions (2D), but, more important, for situations in which the vehicle must vary its depth (3D).


| INTRODUCTION
Since its beginning in the late 1950s, the capabilities and applications of autonomous underwater vehicles (AUVs) have continuously evolved. Their most common applications include imaging and inspecting different kinds of structures such as in-water ship hulls (Hover et al., 2012;Hurtós, Ribas, Cufí, Petillot, & Salvi, 2015), natural structures on the sea floor (Galceran et al., 2014), as well as collecting oceanographic information such as biological (Grasmueck et al., 2006), chemical (Li, Farrell, Pang, & Arrieta, 2006), and even archeological data (Bingham et al., 2010).
Most of the aforementioned applications require a priori information of the area or structure to be inspected, either to navigate at a safe and conservative altitude (Bingham et al., 2010;Grasmueck et al., 2006) or to precalculate a survey path that may be corrected or reshaped online (Galceran et al., 2014;Hover et al., 2012). There exist other applications that require a vehicle to navigate in close proximity to obtain more detailed information or to explore confined natural environments (e.g., underwater caves; Mallios, Ridao, Ribas, Carreras, & Camilli, 2015). In these scenarios, a priori information of the surroundings might not be available, AUVs must operate in unexplored and cluttered environments, and therefore they are more exposed to collisions. To safely navigate in such environments, the AUV must be able not only to plan 3D paths that are feasible according to its motion capabilities, but also to calculate or modify such paths as new information of the surroundings becomes available.
conducting such type of tasks in an underwater environment differs in certain aspects, such as the presence of external disturbances (currents), low-range visibility, and limited navigation accuracy.
Dealing with such constraints requires a path planner with online capabilities to help overcome the lack of environment information and the global position inaccuracy, especially when navigating in close proximity to nearby obstacles.
The framework establishes an online mapping and motion planning architecture that leads an AUV to navigate while simultaneously building a representation of the vehicle's surroundings. The framework is composed of three main modules: (a) a mapping module that uses Octomaps (Hornung, Wurm, Bennewitz, Stachniss, & Burgard, 2013) to represent the environment, (b) a planning module that calculates online collision-free, feasible, and safe paths, and (c) a mission handler that coordinates the planner and the AUV controllers.
The framework considers the vehicle's motion constraints and avoids generating unfeasible paths; it also uses a path optimization objective that combines collision risk and path length, thus ensuring the vehicle's safety; and finally, it reuses the last best known solution as a starting point for an anytime tree-based path planning method.
Despite these positive results, the above-mentioned planning framework had some limitations. First, the framework had been only demonstrated to be effective for 2D missions. Furthermore, trials had been limited to scenarios with artificial and symmetric underwater structures, which did not prove the framework's efficacy when navigating through more challenging environments, such as natural formations. Finally, the controller used to reach the goal position by following a series of discretized waypoints (WPs), rather than the original resulting path. With the previous limitations in mind, this paper presents an extended and improved version of our framework that attempts to overcome these limitations.
While preserving the main characteristics from our previous work (Hernández et al., 2015(Hernández et al., , 2016c, the main contributions of this paper are as follows: (a) The state space extension to support 3D missions by combining the 2D motion ( ψ x y , , ) and the vertical motion (z), both constrained according to the vehicle's motion capabilities. While preliminary results were already presented in Hernández et al. (2017), this paper includes additional tests and a more detailed description of the extended formulation and its implementation. (b) A new mission handler capable of interchanging information between the planning framework and a path tracking controller. This improved functional module eases the integration of the proposed approach with different vehicles. (c) Finally, a comprehensive evaluation of the extended planning framework using the Sparus II . This evaluation includes simulated and in-water trials in different real-world scenarios.
The remainder of this paper is organized as follows: Section 2 introduces previous research related to path planning for AUVs, especially for those vehicles that operate under under both motion and online computation constraints. Section 3 describes the problem of planning 2D paths under motion constraints, it then presents two alternatives to solve this problem, and finally it introduces an extended formulation for calculating 3D feasible and safe AUV paths.
Section 4 explains in detail the framework introduced in Hernández et al. (2015), (2016c), while making the necessary extensions for 3D motions. In Section 5, different real-world scenarios are used in simulation and in-water trials to validate the proposed approach.
Finally, concluding remarks and directions for further research are given in Section 6.

| RELATED WORK
AUVs are commonly divided into two main subcategories: Buoyancydriven and propeller-driven. The former category corresponds to the widely known underwater gliders, or AUVs that are capable of modifying its buoyancy to generate sinusoidal-like vertical motion that, together with its wings, also permits horizontal (forward and lateral) motion. Gliders' propulsion technique results in low energy consumption, but they are also less maneuverable. The propellerdriven AUVs, on the other hand, correspond to those that use propellers or thrusters to generate 3D motions. The work presented in this paper is focused on this latter kind of vehicle.
An important aspect to consider when comparing the motion planning approaches for propeller-driven AUVs is their application.
Based on this, the different contributions can be classified into two main categories. The first group gathers those applications that require coverage path planning techniques, which are commonly applied to guide AUVs in survey tasks. The most common examples within this group include coverage missions used for creating detailed bathymetric maps of the seabed (Fang & Anstee, 2010;Galceran & Carreras, 2012Galceran, Nagappa, Carreras, Ridao, & Palomer, 2013), detecting potential targets such as underwater mines (Stack & Smith, 2003;Williams, 2010) and inspecting artificial structures such as in-water ship hulls Hollinger, Englot, Hover, Mitra, & Sukhatme, 2013;Hover et al., 2012), as well as natural marine formations (Galceran et al., 2014).
The second group of propeller-driven AUV applications, on the other hand, includes those that are focused on safely and efficiently guiding the vehicle from one initial position to a specified goal. For doing so, different strategies have been applied to underwater vehicles. In fact, these start-to-goal methods are commonly used as low-level motion planners required for the aforementioned coverage applications. The work presented throughout this paper seeks to make contributions to this group of start-to-goal applications. It is therefore important to identify the main requirements among the different approaches used.
In some cases, the vehicle operates in open sea areas, where it does not usually have to deal with obstacles, narrow passages, or high-relief environments. However, this is not the case for the new potential applications presented in the Introduction.
A second requirement, especially critical in such new applications, is the capability of generating paths that meet the vehicle's motion constraints. The terms path planning and motion planning are often used interchangeably (LaValle, 2006;Sucan, 2011). Throughout this paper, we will use the term motion planning, to emphasize the motion constraints involved. There are different methods that deal with the motion constraints imposed by AUVs. A first group includes those approaches that use sensor-based methods either to navigate through unknown underwater environments (Ying et al., 2000), or to follow the terrain shape of a given bathymetric map (Houts et al., 2012). In both cases, the vehicle is assumed to be equipped with a forward-looking sonar to reactively avoid collisions. A local planner computes maneuvers that satisfy the AUV kinematics or dynamics.
An important characteristic of this kind of approach is the lack of global knowledge of the environment (i.e., a map is not incrementally built), which can cause the vehicle to get trapped in complex scenarios.
A similar reactive approach establishes a set of inequality constraints that describes the convex obstacles contained in the configuration space (C-Space; Petillot, Ruiz, & Lane, 2001). The initial configuration is treated as the starting point of a nonlinear search, where the goal configuration is assumed to be a unique global minimum of the objective function. The start-to-goal query is then solved as an optimization problem, in which a local planner takes into account the vehicle's constraints. This strategy was one of the first online obstacle avoidance approaches for underwater vehicles; it used a real-world data set of acoustic images obtained by a remotely operated vehicle (ROV) equipped with a multibeam forward-looking sonar. Its validity was demonstrated by guiding a simulated ROV.
However, this study still lacked the capability to simultaneously map (detection) and plan online.
A formulation that represents obstacles as convex regions has likewise been used either to represent obstacles detected online, which trigger collision avoidance maneuvers (Qu & Yuan, 2009), or to approximate the terrain shape that must be followed by the vehicle (Murthy & Rock, 2010). In both cases, the low-level controller attempts to generate feasible (doable) trajectories by using the AUV kinematic equations and spline-based interpolation techniques, respectively. However, the main drawback of these approaches is the difficulty in creating a convex representation of complex obstacles.
Another common strategy used in some of the aforementioned methods is trying to get as close as possible to a path that can be followed by an AUV. Petres et al. (2007), for instance, proposed a fast marching-based approach to find collision-free paths, which are smoothed by a cost function that contains kinematic and curvature constraints. Likewise, another example based on GAs finds a valid route to the goal by using basis spline (B-spline) curves, thus seeking to generate more feasible trajectories for AUVs (Cheng, Fallahi, Leung, & Tse, 2010).
Unlike the previous sensor-based approaches, another group of path planning methods for motion-constrained AUVs uses gridbased approaches. Sequeira and Ribeiro, for example, presented a two-layer framework that is composed of a high-level planner (HLP) and a low-level planner (LLP; Sequeira & Ribeiro, 1994).
The HLP creates a visibility graph using the information of the known obstacles, sea currents, and specified WPs of the mission.
Furthermore, the energy required to move between the graph nodes corresponds to the edge weights. The global and optimal motion plan to the goal is then found by Dijkstra's algorithm.
Finally, to calculate the vehicle's maneuvers between the different solution segments, the LLP uses an artificial potential field. This way the total artificial force includes a 3D double integrator that takes into account the AUV motion constraints.
There are other similar two-layer approaches, where the global path planning problem is tackled with a grid-based method, and the local motion planning deals with the AUV constraints (Arinaga, Nakajima, Okabe, Ono, & Kanayama, 1996). The main disadvantage of this line of works is that the grid-based layer generally requires a priori information of the environment, for example, a navigation map.
A more recent group of contributions includes those that use sampling-based planning algorithms. The most common approach builds a tree of collision-free configurations, which are obtained by integrating the differential equation that describes the AUV dynamic behavior (Caldwell et al., 2010;Heo & Chung, 2013;Tan et al., 2004).
This strategy and its use with a torpedo-shaped AUV will be explained in more detail in Section 3.1.1.
Finally, there is another concept for generating feasible paths.
It consists in utilizing Dubins curves (Dubins, 1957), which establishes a set of six maneuvers (RSR, RSL, LSR, LSL, RLR, and LRL, where R states for right, L for left, and S for straight) to i j  . When no obstacles are involved, these curves correspond to the shortest trajectory between q i and q j for a car-like vehicle that moves under certain motion constraints. They have previously been shown useful of trajectory generation (Cao, Cao, Zeng, & Lian, 2016;Wehbe, Shammas, Zeaiter, & Asmar, 2014) and will be used in this study as well.

| Online mapping and motion planning
As mentioned before, another requirement for coping with the restrictions imposed by the new and potential applications for AUVs is to endow the vehicles with the capability of navigating through unknown or unexplored environments. For doing so, it is necessary that the vehicle can (re)plan its own motions while exploring the surroundings. However, little research has addressed this problem of mapping and planning safe routes simultaneously and online for AUVs. Maki et al. (2007) proposed an online motion planning method that uses landmarks to guide an AUV. Nonetheless, their approach does not permit replanning and, furthermore, results were obtained in a controlled environment (i.e., in a water tank). The approach presented in this paper has also been evaluated in challenging realworld scenarios.
Finally, Table 1  allow it to spin around. However, it is still not capable of conducting lateral motion. Therefore, the vehicle is subject to a motion constraint along its y-axis.
Differential motion constraints can be expressed as a set of differential equations in the general form = q f q u ( , ), where q and q are the system state and its first derivative, respectively, and u is the control input. Having this in mind, and considering the aforementioned constraints when navigating at a constant depth, we represent a nonholonomic and torpedo-shaped AUV as a simple car-like vehicle, with a first-order motion model defined as follows: x y [ , , ] T corresponds to the system state that includes its 2D position and orientation with respect to an inertial reference frame, and ψ = q x ẏ [˙,˙,˙] T is the first-time derivative that depends on the state itself and the control inputs, that is, linear/surge speed (v) and turning rate (w).

| Variable-depth motions
Although there is an important number of AUV applications in which the vehicle navigates at a constant altitude or depth, there are other scenarios in which the vehicle is required to conduct 3D motions (see Sections 1 and 2). For 3D motions, six different variables specify the vehicle's position and orientation, which means that the C-Space is = SE (3) (see Figure 1). Such AUV motions can be formulated with kinematic and dynamic models. The former ones describe the geometry of motion by relating the system positions and velocities.
Dynamic models additionally take into account the forces and torques that generate the motions.
Dynamic models, which normally consider all the six variables, have been used in some AUV motion-planning applications (Ying et al., 2000;Wehbe et al., 2014). However, their high computational cost makes them an inappropriate approach for the scenarios proposed in this paper, which require online (re)planning. The associated overhead is especially significant when using a global planning method, where all configurations 1 are generated by evolving the system, that is, performing numerical integration of the equations of motion. A kinematic model provides a less accurate approximation for the vehicle's motion constraints, but enables more computation time to be dedicated to finding better collision-free paths.
There exist different kinematic models that could be used for describing 3D motions. An AUV such as the Sparus II has some characteristics that allow simplifying such a model. First of all, let us assume that the vehicle is capable of maintaining a near-zero roll angle (ϕ ≈ 0). Second, since the AUV has an independent propulsion force for its vertical motion, it is also possible to assume that the vehicle can follow a near-zero pitch vehicle motion (θ ≈ 0). This latter assumption allows us to decouple the motion in the horizontal plane from the heave (vertical) motion. Therefore, the 3D kinematic model can be described as: to the surge speed (v), the heave speed (d), and the rate of turn (w).
This motion separation is not new for underwater vehicles, since it has also been done for dynamic models formulations (Mišković, 2010). Section 5 will present the evaluation of this approach with the Sparus II AUV. Finally, although both approximations, that is, nearzero roll and near-zero pitch motion, are valid for online motion planning purposes. It is important to note that the vehicle's controller uses a model that fully considers the vehicle's dynamics (Fossen, 2011;Wadoo & Kachroo, 2010).
Once the constraints have been established, the next step is to define a strategy to generate feasible motions that meet such constraints. The following sections present two different approaches, which use sampling-based algorithms to calculate such kind of motions.

| Motion planning using differential equations
Sampling-based algorithms, especially tree-based algorithms like expansive-spaces tree (Hsu, Latombe, & Motwani, 1999)  All configurations refer not only to those that are part of the final solution path, but also those that create a tree of alternative paths evaluated during each planning cycle. In the tests presented in this paper, the planning cycle period is between 0.5 and 0.8 s, in which a tree of hundreds of configurations is generated.
makes an RRT-based algorithm an appropriate approach for the intended AUV applications.
A standard RRT algorithm builds a single tree that is rooted at an initial state (configuration) 2 , which is incrementally expanded towards uniformly random sampled states (see Algorithm 1; LaValle & Kuffner, 2001). To extend the tree towards q rand while meeting the vehicle's motion constraints, new collision-free motions (states) are obtained by integrating Equation ((1)) or ( (2)), as shown in Algorithm 2. This procedure first requires finding the state q near , that is the nearest to q rand (line 2; according to some distance metric over configurations).
Once the nearest motion has been found in the tree, the next step is to calculate the control input = u vw [ , ] near_to_rand that has to be applied to change the vehicle's state from q near towards q rand (line 3).
With an RRT algorithm under geometric constraints the tree is iteratively expanded a geometric distance ϵ, in the case of an RRT algorithm under motion constraints the tree is expanded by applying an input u for a period of time Δt (line 4).
When motion constraints are present, there are different approaches that permit calculating u near_to_rand . Two alternatives, presented by LaValle and Kuffner (2001), are either to randomly sample the control space, or to test a number of possible inputs and then select the one that generates the state q new that is closest to q rand . Figure 2 depicts an example of an RRT algorithm that solves a start-togoal query for an AUV. The vehicle is assumed to be navigating at constant depth under motion constraints described by Equation (1), and the control input u near_to_rand has been randomly sampled.

| Motion planning using dubins curves
There are several examples where a purely geometric path is first computed, and then it is transformed into a trajectory that is appropriate for the considered vehicle. For example, Yang and Sukkarieh (2008) used an RRT algorithm to find a route of collisionfree WPs, which is interpolated with a cubic Bézier spiral. This seeks to convert the initial route into a smooth and feasible path for an UAV. As another example, Kuwata et al. (2009) proposed to expand an RRT by considering not only the vehicle's dynamics, but also its controller behavior.
Nonetheless, all those approaches, including the one presented in the previous section, have a major drawback: They do not guarantee any kind of optimality. This issue could be critical in underwater applications, which may require optimizing different criteria such as visibility (for gathering information), vehicle's autonomy, or even the safety associated with a path when navigating in close proximity to nearby obstacles.
One option is to use the asymptotically optimal RRT (RRT*), which is a variant that guarantees convergence to a global optimum with respect to a given cost function  There exists another RRT variant called RRT-Connect that grows two trees, one from the start state and another one from the goal state. However, this is not possible when dealing with motion constraints, since merging both trees at a common configuration may become computationally intractable (Kuffner & LaValle, 2000). under motion constraints, having such a function implies calculating the required input to dynamically evolve the system from a given state to a desired one. However, defining this function requires solving a two-point boundary value problem, which is in general a very difficult problem.
For the purposes of this paper, as an alternative to defining a steering function, we adopt the Dubins vehicle model (Dubins, 1957).
Dubins geometrically demonstrated that, for a system that is only capable of traveling forward and with a constraint on the curvature of the path, the shortest path to connect any two configurations (states) and LRL. For a Dubins vehicle, at least one of these characterizes the optimal (shortest) trajectory between two states (see Figure 3).

| Dubins curves for constant-depth motions
For a torpedo-shaped AUV, Equation ((1)) describes an AUV that navigates at constant depth and with a constant surge speed v, where the maximum turning rate w max establishes the minimum turning radius r min for a Dubins vehicle. This alternative formulation for the AUV motion constraints can be used with incremental search methods such as the RRT variants. In such a case, the control input u near_to_rand , required to evolve the system from q near to q rand (Algorithm 1, line 3), can be replaced with Dubins curves.
Furthermore, it is important to note that Dubins curves also work as a steering function, thus permitting to generate near-optimal paths with an RRT* algorithm. Figure 4 depicts an example of how this approach is used for reconnecting configurations near to q new , and improving their associated cost. In this example the cost is assumed to be the path length. Further details about Dubins curves are found in references (Dubins, 1957;Shkel & Lumelsky, 2001).

| Dubins curves for variable-depth motions
Different authors have used Equation ((2)) to generate 3D motions for torpedo-shaped and propeller-driven AUVs. For instance, F I G U R E 2 Tree expansion of an RRT algorithm that considers the differential constraints described by Equation ((1) algorithm (Heo & Chung, 2013). The tree expansion is done by directly integrating Equation ( (2)), similarly as it was explained in Section 3.2. Although A* is a valid method to solve this problem, its main limitation is the need to discretize the four-dimensional C-Space ( ψ x y z , , , ). This discretization resolution is especially critical for the exploration of unknown environments, in which a particular C-Space discretization may work for some scenarios, but might fail in others, for exampe, when navigating through variable-width environments.
An alternative to find better 3D solution paths is to use the RRT* algorithm. The previous section explained the use of Dubins curves as a steering function for 2D paths. The concept is now extended to 3D motions. As was mentioned before, a valid approximation is to decouple the motion in the horizontal plane from the heave (vertical) motion. This allows to directly deal with the former component as stated in Section 3.1.1, while treating the latter component as an additional constraint over an extended C-Space, = × SE (2) . In this formulation, the path between two states for the 3D decoupled kinematics is the one that connects their components in the horizontal plane using Dubins curves, and their vertical components with a linear interpolation. The vertical motion must have a gradient no greater that the maximum ascending/descending AUV speed, thus meeting the vehicle's motion capabilities. The distance metric we use combines the lengths of the horizontal and the vertical paths as: where the first term corresponds to the length of the Dubins path between q i and q j in the horizontal plane, and the second term corresponds to the distance in the heave (vertical) motion: All this together, that is, the mathematical formulation, the use of the Dubins curves over a 4D space, and the corresponding metric over configurations, establish a computationally efficient approach to calculate 3D motions for torpedo-shaped AUVs under motion constraints. Note that in the 3D case, we no longer have an analytical solution for the shortest path between two poses. However, under the assumption that the AUV moves with a constant surge speed, the RRT* algorithm will, with the distance metric defined above, converge on the shortest path that satisfies the descent speed constraint.
Finally, to fully understand this approach, the next section presents solutions for different start-to-goal queries that have been obtained using both geometric and motion constraints. Moreover, it also demonstrates the advantages of using Dubins curves instead of using a standard RRT algorithm by integrating Equation (1) or (2).

| Simulation results of planning under geometric and motion constraints
This section presents simulation results of the Sparus II conducting 3D start-to-goal missions in different scenarios. The solution paths have been calculated under both geometric and differential (motion) constraints. In all cases, the vehicle navigated in a pre-explored environment, that is, the obstacles and their locations were known.
We present cases that emphasize the importance of taking the motion constraints into consideration when using nonholonomic AUVs.

| Conducting 3D missions under geometric constraints
Let us define a start-to-goal query where To validate the proposed motion planning framework, we present simulations of the Sparus II AUV conducting autonomous missions in a virtual breakwater structure scenario. This simulated environment corresponds to a real-world structure that is located in the external and open area of a harbor (see Figure 5). Figure 6 depicts the virtual breakwater structure over which a start-to-goal query solution and goal 1 1 1 , which means that the orientation was not taken into account. The 3D solution path was calculated using an RRT* algorithm under geometric constraints, and it was followed by the simulated Sparus II. It can be observed that, although the change of depth did not require a descending speed greater than the vehicle's capabilities (i.e., < d d max ), the vehicle followed a trajectory that deviated from the planned path due to the fact that the planner did not consider the vehicle's motion constraints.

| Conducting 2D missions under motion constraints
We evaluated the two alternatives described above to incorporate Equation ((1)) into the motion planner: Numerical integration and the use of Dubins curves. To compare these approaches, both were used to solve two different start-to-goal queries with a constant depth. For the first alternative, Figure 7a,b depict a simulated Sparus II AUV following solution paths that were calculated by an RRT algorithm that mathematically integrates Equation (1). The RRT algorithm does not provide any guarantee for optimality.
The second alternative uses the RRT* algorithm and the Dubins curves, which work as a steering function equivalent to Equation (1). there are also some risky states; these were generated because the planner tried to provide short paths, which were close to nearby obstacles. This specific issue about the safety of the resulting path is addressed in detail in Section 4.

| Conducting 3D missions under motion constraints
Let us assume that the vehicle has to solve a task for which a start-togoal query is defined as Under such constraints, the motion planner must find a solution that combines turning and descending maneuvers to reach the final position and orientation. This problem can be solved with the extended Dubins curves approach that has been explained in previous sections. One possible solution, where the vehicle circumnavigates while descending to reach the desired depth and orientation, can be observed in Figure 8. Furthermore, this solution is near-optimal under the assumption of a constant surge speed. Figure 9 presents another test over the virtual breakwater structure. Similarly as it occurred in the simulation presented in Figure 6, the start-to-goal query requires the vehicle to completely change its 3D position, but this time it also specifies the desired AUV heading. This time the vehicle's trajectory during execution coincides with the planned path, even when conducting turning maneuvers.
There are, however, some aspects that have to be addressed to make this approach really useful for the proposed applications. The first of them is that the intended usage scenarios involve generally unexplored spaces, which means that a map is not available. A second aspect is the safety of the vehicle, which can be compromised when navigating in close proximity to the obstacles. These and other aspects, as well as the different strategies proposed to cope with them, will be discussed in detail in the next section.

| FRAMEWORK FOR ONLINE MOTION PLANNING IN UNEXPLORED ENVIRONMENTS
New and potential AUV applications that were presented in Section 1 establish most of the requirements and objectives for the research developed in this paper. One of those requirements is the capability of planning feasible paths that take into account the vehicle's motion constraints. Another requirement, and probably the most relevant, is the necessity of incrementally building a map of the surroundings, while simultaneously (re)planning the collision-free path to the goal.
This characteristic would allow the vehicle to navigate through unexplored environments, as well as to overcome part of the navigation inaccuracy.
By using the foundations provided in Section 3 for planning AUV feasible paths, this section now presents a comprehensive planning framework to solve start-to-goal queries online for an AUV that operates in unexplored environments. The framework is composed of three functional modules. (a) A mapping module that builds an occupancy map of the environment using on-board perception sensors.
(b) A planning module that generates safe (collision-free) and feasible paths online. (c) A mission handler that works as a high-level coordinator that exchanges information with the other two modules and the AUV's controllers. Figure 10 depicts the proposed framework. Since the F I G U R E 9 Virtual breakwater structure composed of two obstacles that create a narrow passage. The solution path (in green) is calculated by an RRT* that uses the extended Dubins curves approach. A simulated Sparus II AUV describes a trajectory (in light blue), which does not differ significantly from the one calculated, even when conducting turning maneuvers.

| Mission handler
The mission handler is in charge of controlling and coordinating the other modules (mapping and planning). It also verifies whether the AUV is prepared to start solving and conducting a task. To do so, this module communicates with other functional modules in the vehicle to verify that navigation data is correctly being generated and that the vehicle's low-level controllers are not conducting any safety maneuvers. After completing this initial checking stage, the mission handler sends a starting signal to the mapping module, and generates the star-to-goal query for the planning module.
In our previous works (Hernández et al., 2015(Hernández et al., , 2016c, the mission handler received a discretized version of the solution path in a format of a set of WPs, which were sent, one by one, to the low-level controller. However, this format required establishing a discretization value, which in case of being to too high could generate gaps in between that could be difficult to handle in case a replanning maneuver was triggered, or in case of being too low could affect the smooth and continuous execution of the path. This latter situation was mainly due to communication delays when requesting and receiving a new WP.
The new mission handler uses a different approach. This approach requires the planning module to provide the original path, that is, without discretizing it, which is then sent to a path-following controller. The iterative mapping-planning process now consists in projecting the vehicle's current configuration to the previous path.
Such a projection establishes the initial configuration for every planning cycle. This avoids having initial states that are not close enough to any WP of a discretized path. Furthermore, it makes the planning framework itself more independent of the controller, eliminating the need to tune a discretization value. Finally, in case of a replanning maneuver, the new mission handler only has to provide a new valid path which must override the previous one, and in case of not finding a solution, it can make the vehicle stop.

| Module for (re)planning paths online
The planning module is in charge of calculating a collision-free and feasible path for the AUV. For doing so, this module receives a query that is specified with a start configuration (q start ) and a goal configuration (q goal ), and other parameters, such as the available computing time and the minimum valid distance to the goal.

| Planning safe paths using risk functions
Including motion constraints in the path planner may not be sufficient to avoid collisions with nearby obstacles. This is especially true when the vehicle is exposed to external perturbations, which do not permit the AUV to accurately follow the calculated path. Figure   7c,d, for instance, show feasible paths, but it leads the vehicle close to nearby obstacles, as the solution is optimized with respect to its length.
One alternative to cope with this kind of risky situation consists in estimating the probability of collision with nearby obstacles while, simultaneously, planning feasible paths. To do so, there are planning methods that allow considering different sources of uncertainty, however, most of them are too computationally expensive, especially for applications that must meet online computation constraints, such as the ones presented in this study.
One relevant contribution that seeks to overcome this computation limitation proposes to express the probabilistic obstacle avoidance problem as a Disjunctive Linear Program using linear chance constraints (CC; Blackmore & Williams, 2006). This concept has also been extended to sampling-based methods such as the CC-RRT (Luders, Karaman, Frazzoli, & How, 2010) and the CC-RRT* (Luders, Karaman, & How, 2013). Nonetheless, it is important to notice that the original concept and the subsequent extensions include some strong assumptions that cannot be made in underwater environments, such as considering convex obstacles. The authors of this paper have discussed and extended the aforementioned works by removing the convex obstacles assumptions, among other considerations for meeting online computation constraints (Pairet, Hernández, Lahijanian, & Carreras, 2018). However, such an approach is still limited to planning 2D motions. Therefore, to guarantee that the vehicle safely navigates unexplored environments, our objective is to extend the planner to plan short, feasible paths that minimize the risk of colliding with the surroundings. This can be framed as defining an optimization objective for RRT* that combines the length and the safety of the path. The following sections present different approaches to achieve this. a) Path Length + Clearance: A first and straightforward option is to maintain a minimum safe distance, or clearance, to the obstacles.
For doing so, it is necessary to establish a weighted metric that combines path length and clearance to minimize detrimental effects in the path quality. This allows us to define the associated cost of each configuration when planning with an RRT* algorithm.
However, the approach has two main drawbacks: Its high computational cost and the need to correctly specify weights to obtain a balanced metric, which is a nontrivial problem (Tsianos, Sucan, & Kavraki, 2007). b) Risk Zones: Including clearance calculation is especially expensive for sampling-based motion planning methods, since it has to be performed for each sampled configuration and its intermediate steps when connecting to the others (e.g., RRT* algorithm expansion). One alternative is to heuristically establish risk zones around the vehicle, as shown in Figure 11. Red and blue represent the zones with the highest and the lowest risk of collision, respectively, while the others (orange, yellow, and green) have associated risk values that decrease as the zone is further from the vehicle's position. Therefore, the risk associated with a given configuration q is determined by the innermost zone under collision. This risk association can be represented by Equation (5) To combine this function with the path length, the total accumulated cost associated with each configuration q i in the tree produced by RRT* is calculated as the integral of risk with respect to distance along the path between the root of the tree, q 0 , and q i : This cost function is then the optimization criterion for planning feasible and safe paths. A visual comparison between paths calculated using only the path length and the those with risk zones can be observed in Figure 12. (see Figure 13). This approach is here called risk vectors. It is computationally a less expensive alternative, given that checking collision for single points is more efficient than doing the check for zones (multiple points), especially when using Octomaps to represent the environment.
Both the risk zones and the risk vectors have also been extended for 3D motions. In the case of the risk zones, the areas around the vehicle become volumetric shapes of increasing size (e.g., rectangular prisms, or cylinders), whereas with the risk vectors it is enough to add the corresponding vectors for the vertical motion (see Figures   11b and 13b). These alternatives to quantify the risk associated with a path have been evaluated and compared with each other. The results of this analysis will be discussed in Section 4.4.

| Opportunistic state checking
When conducting an autonomous mission without initial information of the surroundings, the AUV is required to incrementally map and continuously (re)plan collision-free paths according to its partial knowledge of the environment. A significant number of configurations (sampled or obtained after expanding the tree) are located in unexplored regions of the environment. In these situations, it is not only impossible but also unnecessary to attempt to determine if a configuration is at risk of collision. We propose to treat any configuration that is out of the explored area as safe (collision-free and minimum risk). This strategy was previously introduced by the authors as opportunistic state checking (Hernández et al., 2016c).
In this incremental mapping and planning approach, the tree expansion is periodically interleaved with updating the map, and such parts initially assumed as safe must be verified and discarded if found under collision as the vehicle explores the environment. The next section describes and analyses two alternative mechanisms to check and reshape the tree.

| Reuse of the last best known solution
The strategy presented in this section is the third main characteristic of the proposed framework and its planning module. It allows an incremental and tree-based path planner, such as the RRT* algorithm, to replan or reshape the solution path according to the new environment information perceived during the mission. However, to fully understand the benefits associated with this strategy, this section first explains another alternative that was initially used, but later discarded due to its high computational cost.

Pruning the tree
One alternative for dealing with partially known environments is to

Reuse of the last best known solution
The previously explained approach used a tree of configurations that is periodically traversed, checked, and pruned as the vehicle moves and explores the environment. The main objective was to preserve the information about collision-free areas and known paths, while discarding those that become invalid. One alternative for not conserving the whole tree, is to use the last best known solution as the remainder of the path calculated in the previous planning cycle, which starts at the point that the vehicle will reach at the next execution cycle.
In this iterative and anytime computation scheme, using the last solution to start a new planning cycle implies not only a new valid solution according to an updated map, but also one that is at least as optimal as the previous one. This also permits to eliminate timeconsuming pruning routines by avoiding checking subtrees, in which many of their configurations have probably become invalid because of the opportunistic state checking explained before. Although this strategy is the one used for the tests and results discussed in Section 5, a further comparison with the tree-pruning strategy will be presented in Section 4.4.3.
Finally, to illustrate the behavior of both strategies, that is, pruning the tree and the reuse of the last best known solution, Figure 14 describes a test scenario in which a vehicle simultaneously maps and plans collision-free paths in an unexplored environment. The vehicle is assumed to be equipped with a forwardlooking range sensor of limited field of view to navigate in an environment that resembles a breakwater. The structure is composed of a series of concrete blocks, which are separated by gaps. The task consisted in a start-to-goal query that required navigating from one side of the blocks to the other.

| Pipeline for online planning feasible and safe paths
Previous sections have presented three new features: An optimization function that combines collision risk and path length; a strategy to avoid unnecessary and expensive state checking routines; and the reuse of the last best known solution as a starting point for an anytime planning approach. These features endow an AUV with the capability to simultaneously map and plan feasible and safe paths online through unexplored environments. To fully understand how such characteristics work together, Algorithm 5 presents the execution pipeline to incrementally solve a start-to-goal query. This pipeline has allowed conducting autonomous missions for the intended AUV applications.
For this reason, the newly proposed features represent the main contributions to the planning module presented in this section.
F I G U R E 1 4 A test scenario in which a vehicle simultaneously maps and plans collision-free paths in an unexplored environment. The vehicle is assumed to be equipped with a forward-looking range sensor of limited FoV to navigate in an environment that contains a series of obstacles, which are separated by gaps. The task consists in a start-togoal query that requires navigating from one side of the obstacles to the other. (a) During the first part of the task when no obstacle has been detected, the solution path corresponds to the shortest one from the vehicle's current state to the goal state (see nodes and branches in blue). (b) Once an obstacle is inside the vehicle's sensor FoV (in purple), different strategies could be used to guarantee that the solution path is still valid. With the tree-pruning strategy the tree is rechecked for collision. Nodes and edges that are in collision, as well as the subtrees attached to them, are discarded. This strategy may occasionally lead to loss of valuable information about reaching the goal, even if only part of the previous solution was affected (see parts of the tree in red). (c) An alternative is keeping the last best known solution (in blue), and then reusing it as an starting point in the next planning cycle, (d) which will discard the affected parts, while attempting to reconnect to other valid parts of the previous solution (in green). FoV: field of view [Color figure can be viewed at wileyonlinelibrary.com] The pipeline presented in Algorithm 5 has as main input parameters the start and goal configurations (position and orientation) of the query to be solved. To initialize the incremental solving routine, the pipeline selects the RRT* algorithm as the planner that computes paths for a Dubins (or extended 3D Dubins) vehicle, sets an empty list as the last best known solution, and defines q new_start as the starting configuration that will change as the vehicle conducts the mission (lines 2-4). To incrementally find a valid path to the goal (line 5), the pipeline's main loop requests an updated version of the map (Octomap, lines 6-7), informs the planner to start from last best known solution (line 8), uses the planner to find or improve the path (line 9), and gets the solution (line 10). At this point, the planner has provided a valid path that must be as optimal as the previous one.
Before concluding a planning cycle, the incremental solving routine checks if a replanning maneuver has been requested. This would imply that the mission handler has detected that the path from the current configuration to the last WP sent to the AUV controllers is not feasible, and might lead the vehicle to a collision, thus requiring that a new path should be found from the current configuration (see lines 11-13). Finally, if a new WP is required, the last q new_start will be sent to the mission handler (lines 14-15).

It is important to note that if the previous (last best known)
solution is not in collision with an updated version of the map, the planning module will reuse it as a starting point (line 8), to attempt to improve such existing solution during a new planning cycle (lines 9 and 10).

| Simulation results of incremental mapping and motion planning in unexplored environments
To validate the proposed motion planning framework, this section presents simulations of the Sparus II AUV conducting autonomous missions in the virtual breakwater structure scenario. This simulated environment corresponds to a real-world structure that is located in the external and open area of a harbor (see Figure 5). In this scenario, start and goal configurations were located in opposite sides of the shown breakwater structure, so that the Sparus II AUV had to move amidst the concrete blocks. All queries were defined to conduct missions at a constant depth, thus the motion was restricted to a 2D task.
For these tests, the vehicle was assumed to be equipped with a mechanically scanning profiling sonar to perceive and detect the surroundings. The sonar was located to cover a scan sector in the horizontal plane along the vehicle's direction of motion. From the software perspective, the Sparus II used the component oriented layer-based architecture for autonomy (Palomeras, El-Fakdi, Carreras & Ridao, 2012), a control architecture fully integrated with the robot operating system (ROS). This software architecture also makes use of the open motion planning library (OMPL) that offers a convenient framework that can be adapted to specific planning problems (Sucan, Moll, & Kavraki, 2012). Further details about the vehicle's hardware and software characteristics can be found in Appendix A.

| Comparison of risk functions
Before evaluating the framework's capabilities over unexplored environments, this section first assesses and establishes the best alternative to estimate the risk associated with a path. This analysis is done over a fully known and mapped scenario. Once this has been determined, tests over an unexplored environment prove the advantages of using opportunistic state check strategy. Then, with this strategy, additional tests compare both the pruning tree and the reuse of the last best known solution approaches, thus allowing to demonstrate the latter one is the most efficient alternative for reaching an anytime computation approach.

Evaluation of the risk functions over fully mapped environments
For these initial tests, an Octomap of the surroundings was assumed to be available a priori. Over this map, different start-to-goal queries were solved by using the alternatives to estimate the risk presented in Section 4.2.1. Figure 15 depicts some examples of the solutions paths obtained. It can be observed how the path maintains a safe distance from nearby obstacles (zones in purple), in contrast to what occurs when only path length criterion was considered (see Figure 12).
Although all of the proposed approaches to estimate risk of the path can generate similar results, their associated computation times differ considerably. Figure 16 presents the average computation time required over 100 runs to solve Task1 and Task2 (shown in Figure 15) by each approach. It can be observed that using only the path length is clearly the least expensive method, while path length + clearance and risk vectors are the most and least expensive, respectively, when including the risk of the path.

Evaluation of the risk functions over unknown environments
From Figure 16, it can be concluded that the best computational alternative to include the risk associated with a path is the risk vectors approach. However, when dealing with unexplored environments, this approach may not permit estimating the risk correctly. In some cases, for instance, if an obstacle is not completely represented in the map so that the risk vectors do not coincide with the available partial information, the configuration can be incorrectly assumed as safe, while the risk zones will correctly estimate the risk (see Figure 17). For this reason, the best alternative when dealing with unexplored (unknown) environments is the risk zones approach.

| The benefits of opportunistic state checking
Section 4.2.2 introduced the opportunistic state checking strategy as one of the mechanisms that can contribute to overcome this situation. To validate its efficiency, a start-to-goal query was solved and simulated 15 times, but now without assuming any previous map of the breakwater structure scenario ( Figure 18 presents the execution of one those tests).
The solutions for queries such as the one presented in Figure 18 were obtained with and without the opportunistic state checking strategy. Although in both cases the framework succeeded in conducting the task, Figure 19a demonstrates that without it almost 80% of the total computation time is dedicated to risk checking routines over the whole mission. When using opportunistic state checking, its associated computation time increases as the environment is progressively explored, but is still computationally more efficient. This is especially noticeable when a mission does not require exploring and mapping the entire environment.
Therefore, the use of the proposed mechanism affects not only the time required to find a solution, since it permits a better tree expansion, but also improves the workspace exploration and the path quality. Figure 19b presents the effect of using opportunistic state checking, in which it can be observed how a low computation time in the first part of the mission, when the environment has not been explored, also enables the construction of a tree with a higher number of nodes.

| Anytime motion planning in unexplored environments
Section 4.2.3 presented a pruning tree scheme and the reuse of the last best known solution. Both are alternative replanning approaches to make the framework capable of providing valid solutions at anytime.
To prove the latter one is the best option, the execution pipeline presented in Section 4.3 has been used with both approaches to run different simulations.
Two different simulated scenarios were used to compare the pruning tree scheme and the reuse of the last best known solution.
The first simulated scenario is the breakwater structure. Over this scenario, two different start-to-goal queries were tested (see Figure 20). After executing these missions 10 times, it was observed that the number of successful attempts is greater and the mean of canceled maneuvers (over the total successful missions) is smaller when reusing the last best known solution with opportunistic state checking (see Table 2). Such canceled maneu- The second test scenario resembles a natural environment composed of rocky formations, which create an underwater canyon (see Figures 21a,b). One start-to-goal query was defined in a way that the vehicle was required to travel through the canyon (see Figures 21c,d). Once again, it was observed that the number of successful attempts is greater and the mean of canceled maneuvers is smaller in the case of reusing the last best known solution (see Table 2).
F I G U R E 1 8 A simulated Sparus II in an equivalent virtual scenario of the breakwater structure. The vehicle starts a mission by submerging to a specified depth. It then maps and solves a start-to-goal query simultaneously. (a) Equipped with a scanning profiler, it incrementally builds a representation of the environment, while continuously reshaping the solution path, (b) to finally approach to the specified goal configuration [Color figure can be viewed at wileyonlinelibrary.com] F I G U R E 2 0 Comparison of the pruning tree scheme and the reuse of the last best known solution when solving start-to-goal queries with the pruning tree scheme and the reuse of the last best known solution approaches. Canceled WPs are presented as circles in red, and correspond to WPs that may lead the vehicle to a collision, thus requiring the path to be completely replanned. notice that the these scenarios do not include long-distance missions, and they were not conducted in areas where the currents that can affect drastically the motion of the vehicle. The following sections will discuss in detail the results obtained for each of these scenarios.

| Planning AUV paths in artificial marine structures
Nowadays, there are a wide range of man-made marine constructions, from large commercial harbors to offshore platforms. These artificial structures often have a regular and known shape, which could be used to preplan trajectories to travel through them.
However, as was explained in Section 1, there are different factors such as low visibility and limited navigation accuracy, which make it difficult to correctly follow such precalculated paths. An example of this kind of structure is the breakwater mentioned in previous sections. An example mission could be one that makes an AUV to move amidst the concrete blocks. The origin for the north-east-depth (NED) inertial system is defined by a latitude-longitude coordinate, which can be obtained from Google Maps©. Likewise, a start-to-goal query can be specified in such a way that the AUV is required to Note. The execution of each simulated mission in the breakwater structure scenario lasted on average 2 min, with a planning time set to 1 , requiring, approximately, 120 planning executions, for a total of 240 for both Task1and Task2. In the case of the simulated missions in the underwater canyon, the missions lasted on average 4 min, with a planning time set to 1.2 s , thus requiring approximately 288 planning executions.
F I G U R E 2 1 Comparison of the pruning tree scheme and the reuse of the last best known solution when solving start-to-goal queries. The simulated scenario resembles a natural environment. Canceled WPs are presented as circles in red, and correspond to WPs that may lead the vehicle to a collision, thus requiring the path to be completely replanned. Although these goals could be defined in advance, this preliminary information does not include unknown obstacles that the vehicle may encounter during the mission execution. However, the proposed approach has the capabilities to avoid colliding with such unexplored obstacles, and in case that such obstacles do not allow to complete the start-to-goal mission, the vehicle will stop.
If a map of the surroundings is not available a priori, the proposed framework requires the vehicle to be equipped with exteroceptive sensors. These sensors allow detecting the surroundings to incrementally create a map online. For this test scenario, a set of four echosounders and one mechanical-scanning imaging sonar were located within the vehicle's payload (front) area, all of them pointing in the horizontal plane (see Figure 22). Three of the echosounders were separated by 45 ∘ , with the central one looking forward and parallel to the vehicle's direction of motion, while the fourth one was perpendicular to the central one (see Figure 22a). The imagining sonar was set up to cover a scan sector in the vehicle's direction of motion (see Figure 22b). Since both sensors cover the horizontal plane, missions with this configuration were executed at a constant depth. For more details about the complete Sparus II's hardware configuration, the interested reader is referred to Appendix A.
During the first simulation and in-water trials conducted over this test scenario, the Sparus II AUV only used the echosounders to perceive the environment. Nevertheless, the four echosounders beams were insufficient to rapidly establish the validity of the path along the direction of motion, which triggered multiple replanning maneuvers. coordinate, thus requiring to move through to one of the gaps once again (see Figure 23b). Likewise, the third and last query was defined to conclude the mission in the outer area (see Figures 23c).
Along the mission, the Sparus II did not surface to obtain global positioning system (GPS) fixes. Figure 23d depicts the vehicle's trajectory and the Octomap built overlapped with a satellite imagine of the breakwater structure. Although the Octomap is coherent with respect to the real-world structure, there are some differences due to the accumulation of navigation error. Yet this did not prevent the AUV from successfully completing the mission. Although Figure 23 represents a successful mission, it is important to analyze the repeatability. Table 3 presents a comparison of the rate of successful attempts while using pruning tree scheme and the reuse of the last best known solution for two different in-water tasks: Single crossing, and multiple consecutive crossings. It can be observed that the reuse of the last best known solution proves to be a better strategy, as was already demonstrated in simulation (see Table 2).

| Planning AUV paths in natural marine formations
Although the mission in the breakwater structure represents a valid application example, it is also true that the obstacles in it, that is, the concrete blocks, have regular shapes. Their vertical walls, for instance, were correctly detected by a mechanical-scanning imaging sonar, despite its great beamwidth. Natural environments, on the other hand, present less regular and more challenging scenarios. To test the proposed framework under more challenging conditions, we therefore also conducted autonomous missions over a real-world natural environment. The testing area is also located in Sant Feliu de Guíxols (Spain), and contains rocky formations that create an underwater canyon (see Figure 24).
For this scenario, the Sparus II AUV used a mechanic-scanning profiling sonar, which has a smaller beamwidth of 1-2 ∘ . This sensor, which covered the horizontal plane, permitted not only to perceive the obstacles shape with more accuracy, but could also be set to a higher maximum range without being affected by false-positive detections from the bottom. The AUV was also equipped with a set of three GoPro™ Hero 4 Black edition cameras. They were used to gather the optical images required to create a 3D reconstruction of the surroundings. The cameras were positioned systematically to ensure the highest possible coverage, where two of them were placed in a downward configuration at an angle of 20 ∘ , while the third camera was positioned forward looking at 40 ∘ (see Figure 25).
Two different start-to-goal queries were defined using coordinates obtained from Google Maps©. The first query required the Sparus II AUV to traverse the canyon towards the shore. Then, the second query goal was chosen on the outside of the rocky formation in such a way that the vehicle had to circumnavigate the outer rock. Figure 26 T A B L E 3 Comparison of solving two different in-water tasks in the breakwater structure using pruning tree scheme and the reuse of the last best known solution Breakwater structure tests Note. Task 1 consisted in crossing once the breakwater structure by moving through one of the gaps between the concrete blocks. Task 2 consisted in crossing three consecutive times, each time using a different gap (see Figure 23). For both tasks, the reuse of the last best known solution has a higher rate of success, thus proving to be a more effective approach. In this kind of exploration, the data that is gathered along the mission can be used to create a more detailed survey of the area. In this particular mission, for instance, the images were extracted and were used to build a photorealistic 3D model, which is depicted in Figure 27a. This 3D reconstruction allows us to better understand complex environments by generating arbitrary user-defined views (see . For more details about these 3D reconstructions, the interested reader is referred to Hernández et al. (2016a), (2016b).
As it was also explained for the in-water tests in the breakwater structure, in the underwater canyon the start-to-goal queries could be tentatively defined in advance, but this information does not include obstacles or environment limitations that the vehicle may encounter during the mission execution. For this particular case, for instance, the canyon's minimum aperture could be estimated as 8-10 m if it is observed from the surface. However, the canyon gets narrower as the vehicle navigates deeper (see Figure 27b). The proposed framework has the capabilities to deal with this kind of unexplored environment, and it will stop the vehicle in case that the information detected along the mission avoids reaching the desired goal.
The main challenges associated in the experiment depicted in Figure 26 are not immediately apparent. The factor that most likely had the biggest impact on the success was the correct payload setup.
Once the mechanic-scanning profiling was selected as the perception sensor, and it was correctly mounted and setup, it was possible to conduct four successful missions out of six attempts. the AUV's DR system seems to be colliding with the rock. This is mainly due to the accumulation of errors in the navigation system, as was already mentioned before.

| Planning AUV paths in confined natural environments
In this section, we will present experimental results for a mission that requires 3D motion. One example of a mission that require a 3D motion planner is to move through confined natural environments (e.g., underwater caves and tunnels). Some technical aspects that make it more difficult to conduct autonomous missions in these environments, include the navigation error associated with incorrect data provided by the DVL when traveling over rocky surfaces.
In dealing with this latter aspect, Mallios et al. (2015) presented the exploration of an underwater caves complex (see Figure 29a), in which a diver guided the Sparus AUV to gather environment acoustic information. To do so, the vehicle was equipped with two mechanically scanning imaging sonars to cover the horizontal and vertical planes. Such data were used to prove a scan-matching algorithm over a simultaneous localization and mapping (SLAM) framework, which allows reducing and bounding the AUV navigation error. This survey provides an extensive data set that includes not only the sonars' raw data, but also a detailed meshed map (see Figure   29b; Mallios et al., 2015;Mallios, Vidal, Campos, & Carreras, 2017).
For more details about this survey, the interested reader is referred to the cited references.
Although the prior work is considered a significant step towards  To conduct this test mission, the meshed map was added into underwater simulator (UWSim) as a 3D simulation environment (see Figure 29c). To perceive the surroundings, the AUV's payload was assumed to be equipped with an additional link capable of rotating 120 ∘ around the vehicle's z axis. Over this link, a forward-looking multibeam sonar was mounted. The sonar had 240 beams distributed over a total aperture of 120 ∘ around the vehicle's x axis. With this payload configuration, the simulated Sparus II was capable of gathering 3D range data of the environment along its direction of motion.
The first start-to-goal query was defined to guide the AUV closer to the caves complex location. From there, the second query sought to explore the first and biggest single-branch cave. This meant navigating to the end of the cave. Figure 30a depicts part of the execution of these two queries, including the AUV's trajectory, the calculated path, and the goals. From the second query's goal configuration, a third query was defined to take the Sparus II closer to the second single-branch cave's entrance. To accomplish this part of the mission, the AUV not only had to find a way out of the first cave, but also had to traverse a tunnel that connects the entrances of both single-branch caves. Figure 30b depicts part of the execution of this query.
Once the Sparus II AUV had crossed the tunnel, the fourth query was defined to navigate to the end of the second single-branch cave.
Once this was accomplished, the fifth query was set to guide the AUV close to a third cave's entrance. Figure 31 depicts part of the execution of these two queries.
Finally, Figure 32 presents  To allow an AUV to navigate environments without an initial map, we proposed two complementary strategies that permit to efficiently F I G U R E 3 0 Simulated mission in the caves complex. In (a), the first start-to-goal query guided the AUV closer to the complex location. The second query required the vehicle to navigate through the first single-branch cave. The third start-to-goal query guided the AUV to the second single-branch cave's entrance. In (b) the vehicle's trajectory appears in light blue, and the remaining of the calculated path towards waypoint (3) appears in yellow. This query required the vehicle not only to find a way out of the first cave, (c) but also to traverse a tunnel that connects the entrances of both single-branch caves. reshape the solution path as the environment is incrementally explored. The first one is to reuse the last best known solution to avoid the need of pruning the tree of configurations. The second one is to opportunistically check the states' risk of collision. Both strategies sought to meet online computation limitations, thus providing a computationally efficient alternative to fully considering the sensing and motion uncertainty.
To validate the proposed approach and its characteristics, we This study has also provided a good overview of the main challenges and limitations that have to be overcome to conduct fully autonomous underwater missions. A critical one is the computational power available in underwater vehicles. While in other autonomous systems, such as terrestrial vehicles, a common solution for this limitation is the use of dedicated computers for the different functional modules (e.g., mapping, planning, control, etc.), in underwater vehicles the volume dedicated for on-board computers is generally fixed, and commonly includes only one computer. In most of the commercial AUVs, such a computer is capable of estimating its position to follow predefined survey paths, but it is not intended for mapping and (re) planning paths online. Furthermore, the increase of the number of onboard computers may also limit the vehicle's autonomy. Therefore, the computational complexity, in time and space, is a critical requirement to be considered when designing and developing software control architectures for these kind of applications.
Another important challenge for conducting autonomous underwater missions is the limited accuracy of the exteroceptive and interoceptive sensors. For the former group, it was a considerable effort to conduct in-water trials in the underwater canyon. Different sensor configurations were tested for getting the best possible representation of this kind of environment. With respect to the interoceptive sensors, the DVL, which is the main sensor used for navigation, is considerably affected when navigating over rocky environments. The difficulties encountered with both kind of sensors provide an idea of the potential challenges that will have to be addressed to conduct in-water missions in complex scenarios such as the underwater caves complex. Furthermore, these technical issues are mainly responsible for the reduced effectiveness when moving from simulation to in-water trials. Many of the failed attempts were caused by navigation or perception errors, which are not directly related to the planning framework. Such sensor errors include noisy DVL measurements when navigating over rocky environments, thus providing incorrect vehicle's velocities estimation, magnetometer bias increase when navigating amidst the concrete blocks, and false-positive detection of obstacles which is mainly due to the imaging sonar aperture.
There are several directions we plan to explore in future work.
First, we plan to further develop the high-level planning layer to automatically select goal configurations or goal regions that the vehicle should navigate towards. The order of the exploration, the strategy to efficiently cover a desired area, dealing with dead ends are decisions all need to be addressed by this high-level planning layer. Second, we plan to explore how we can more accurately account for and exploit the vehicle's dynamics during planning while preserving the fast planning times. The Sparus II AUV also has communication devices such as an acoustic modem for underwater communication with other vehicles or surface stations (e.g., by using an ultrashort baseline system), and a Wi-Fi antenna that can be used when the AUV is at surface.
Moreover, the vehicle includes a configurable payload area in the front, which contains a set of exteroceptive sensors to perceive and detect the surroundings. This latter group of sensors can be modified according to the mission's requirements, and may include optical cameras, single-beam echosounders, mechanical-scanning (profiler and imaging) sonars, multibeam sonars, and so forth. Figure A1 depicts different views of the Sparus II AUV, including one where a possible payload configuration can be observed.
As far as software is concerned, the Sparus II AUV is controlled through the component oriented layer-based architecture for autonomy (COLA2; Palomeras et al., 2012), which is a control architecture, that is, completely integrated with the ROS. Besides operating aboard real robots, COLA2 can interact with the UWSim, which can import 3D environment models. UWSim is the result of a joint effort between our group and the group from University of Jaume I (Castellon, Spain), thus allowing to simulate the Sparus II's sensors and dynamics with high fidelity Prats, Perez, Fernandez, &Sanz, 2012). Furthermore, the use of ROS allows for easy integration of third party tools, such as the OMPL which offers a convenient framework that can be adapted to specific motion planning problems (Sucan et al., 2012).

APPENDIX B: START-TO-GOAL QUERIES OVER THE UNDERWATER CAVES COMPLEX "COVES DE CALA VIUDA"
Detailed information about the underwater caves complex "Coves de Cala Viuda" can be found in (Mallios et al., 2007(Mallios et al., , 2017. However, this appendix provides the exact values of the start and goal configurations, as well as the vehicle's speed used in the simulation test presented in Section 5.3 (see Figure 32). For the first start-to-goal query . This latter query required the planner to find a way out of the second cave, which is considerably narrower than the first one. To cope with this latter situation, the planner used a lower surge speed, ∕ = v m s 0.1 , which allowed the vehicle to turn back with a smaller turning radius.