Navigation in the Presence of Obstacles for an Agile Autonomous Underwater Vehicle

03/28/2019 ∙ by Marios Xanthidis, et al. ∙ 0

Navigation underwater traditionally is done by keeping a safe distance from obstacles, resulting in "fly-overs" of the area of interest. An Autonomous Underwater Vehicle (AUV) moving through a cluttered space, such as a shipwreck, or a decorated cave is an extremely challenging problem and has not been addressed in the past. This paper proposed a novel navigation framework utilizing an enhanced version of Trajopt for fast 3D path-optimization with near-optimal guarantees for AUVs. A sampling based correction procedure ensures that the planning is not limited by local minima, enabling navigation through narrow spaces. The method is shown, both on simulation and in-pool experiments, to be fast enough to enable real-time autonomous navigation for an Aqua2 AUV with strong safety guarantees.



There are no comments yet.


page 1

page 5

page 6

page 7

This week in AI

Get the week's most popular data science and artificial intelligence research sent straight to your inbox every Saturday.

I Introduction

This paper addresses the problem of trajectory planning for underwater structure inspection and mapping. Underwater structure mapping is an important capability applicable to multiple domains: marine archaeology, infrastructure maintenance, resource utilization, security, and environmental monitoring. While the proposed approach is not limited to the underwater domain, this work addresses the challenging conditions encountered underwater, with a special focus on shipwreck mapping. Historical shipwrecks tell an important part of history and at the same time have a special allure for most humans, as exemplified by the plethora of movies and artworks of the Titanic; see the work of Eustice et al. [1] for the visual mapping of the Titanic. Underwater mapping has traditionally focused on acoustic sensors [2, 3, 4, 5], however, most inspections require visual input [6, 7, 8]. The stateoftheart in autonomous operations is to observe the target structure from far enough to avoid navigation in cluttered spaces, while remotely controlled operations present entanglement hazards. As a result most inspections suffer from gaps due to occlusions, and lowresolution due to the water effects on the camera sensor. The presented work will enable autonomous operations underwater close to the structure to be inspected; this was only partially possible with teleoperation before.

Maps of underwater structures such as wrecks, caves, dams, and docks are often available either through acoustic sensing [9] or via photogrammetry [10, 11]. In this paper we present an adaptation of the Trajopt [12] open source package for an 3D trajectory planning of an AUV in a known map, realized on an Aqua2 vehicle [13]. The Trajopt planner was augmented with a samplingbased correction scheme that resolved the local minima challenge. Furthermore, different map representations were tested including geometric primitives and pointcloud implementations.

Fig. 1: Aqua2 AUV navigating over the Stavronikita shipwreck, Barbados.

AUVs moving in 3D underwater are prone to external forces, such as currents, and also the effects of inertia. As such, when a robot switches between motion directions, a significant drift appears. This effect was taken into consideration when designing the safety distance and the linear velocity of the vehicle. When the motions were too aggressive, excessive drift was observed.

A variety of environments were constructed to challenge the trajectory generator. Utilizing the gazebo simulator [14] with an underwater extension that emulates the kinematic behaviour of the Aqua2 vehicle, the tested environments demonstrated changes in depth, and attitude in three dimensions for realizing the produced trajectories. Furthermore, tests in our university’s diving pool, with different obstacle setups verified the validity of the proposed approach. The obstacle arrangements in the pool were accurately reproduced with a map which was used as input to the Trajopt planner.

The next section provides an overview of related work. Section III describes the Aqua2 AUV employed. The proposed approach is detailed in Section IV, while experimental results are presented in Section V. The paper concludes with lessons learned and directions of future work.

Ii Related Work

The underwater domain introduces additional complexities to path planning. Because underwater environments often are highly dynamic, generating safe paths becomes more difficult as robots must account for their own drift. In many cases, underwater robots are also affected by currents in the environment. Several methods have been explored to correct the deviations caused by currents, including the FM* planning system [15]. Other methods rely on observations about the structure of the terrain [16] and satellite imagery [17]

to estimate the effects of currents. Genetic algorithms


and mixed integer linear programming

[19] have also been used to support the computation of paths in dynamic underwater environments.

Another challenge in underwater path planning is to generate paths quickly enough to be able to compute and execute paths online. Green and Kelly demonstrated a branching-based method for quickly generating safe paths in 2D [20] which has since been the basis of several optimizations [21, 22, 23, 24]. Path planning has also been optimized by reducing candidate paths to equivalence classes [25].

Optimal sampling-based techniques [26, 27, 28, 29] although they are providing near-optimal solutions and have improved with time, in general require more resources, more time, and often an exhaustive search of the configuration space. In addition, it is very challenging for these techniques to satisfy minimum distances from the obstacles. Recent works on online underwater navigation with sampling-based techniques do provide fast safe paths, however they are limited to 2D motions [30] or require assumptions for vertical relief [31] without exploiting the full potential of performing 3D motions, often essential for our purposes.

Not only is it important for AUVs to be able to plan in a known environment, but it is also often necessary for AUVs to navigate in an environment without global knowledge of the environment. In such cases, obstacles are observed, often by stereo vision as has been done on aerial vehicles [32]. Exploration of an unknown environment by aerial vehicles has been represented using a 3D occupancy grid using probabilistic roadmaps (PRM) and the D* Lite algorithm for planning as in [33]. Although underwater and aerial domains provide different challenges, both require path planning in 3D. For an AUV such as Aqua2 whose movements do not correlate exactly with control inputs, planning becomes even more difficult. Other AUVs have also been used for path planning, such as RAIS [34] and DeepC [35]. Another AUV, REMUS [36], used obstacle avoidance specifically for exploration of shallow waters.

Enabling Aqua2 AUVs to perform complex tasks has been attempted a few times. In the beginning basic patterns were used, then complex swimming gaits were developed in order to perform patterns such as swimming on the side, in a corkscrew motion, or performing a barrel roll [37]. Visual tags placed on structures were used to enable the AUV to navigate [38], while a learned reactive controller had the vehicle maintain safe distance while moving over a coral reef [39, 40].

Iii System Overview

The target system is the Aqua2 platform [41], depicted in Figure 1

. Aqua2 is an amphibious hexapod robot, approximately 65cm by 45cm by 13cm in size and weighing approximately 10kg. In its aquatic configuration, it uses motion from six flippers, each independently actuated by an electric motor, to swim. Aqua2 has 6 degrees of freedom, of which five are controllable: two directions of translation (forward/backward and upward/downward), along with roll, pitch and yaw. The robot’s pose is described using the vector


in which represents the position of the robot in the world frame and contains the coefficients of the unit quaternion specifying the robot’s orientation.

The robot’s computational system consists of two units, one responsible for vision and high-level planning and the other responsible for control related computations Each of these computers is based on dual-core Intel Core i3-6100U CPU @ 2.30GHz running Ubuntu 16.04 LTS.

The robot’s primary sensing modality is vision. It is equipped with three iDS USB 3.0 UEye cameras: two facing forward and one in the back. The front-facing cameras are used for navigation and data collection, whereas the back camera is used for communication with diver using AR tags to send commands [42]. In addition to these cameras, Aqua2 also has an IMU and a pressure sensor which are used for controlling the motions and can be utilized for visualinertial state estimation [43].

Iv Proposed Method

The objective of this project is to develop algorithms that enable the Aqua2 robot to navigate reliably and safely through dense fields of obstacles. It is assumed that an accurate map of the obstacles is known, and that start and goal poses and are given.

Figure 2 is an overview of the proposed system for achieving this goal. The architecture is composed of two high-level elements: a motion planner, which utilizes a trajectory optimization technique to form plans that connect the to via a collision-free path; and a controller, which executes these paths in a closed-loop manner. Details about these two components appear in Section IV-A and IV-B, respectively. Note that a complete system in the field would require additional components for state estimation and mapping. Both of those problems are well-studied in this domain, so we do not consider them directly in this paper.

Fig. 2: System architecture.

Iv-a Motion planner

Motion planning in this context must balance several competing constraints, including the need for efficiency, the possible need to replan, and the limited computational power available on the robot (particularly when one considers the other essential tasks of perception, mapping, etc.). In addition, because of its complex dynamics and kinematics combined with the unpredictable nature of maritime currents, the system is quite sensitive to drifting. Thus, the motion planner should provide solutions that satisfy strict guarantees in terms of minimum distance from the obstacles to counteract the potential effects of drift. At the same time, some of the most challenging factors in many traditional motion planning settings, including narrow passages and high-dimensional configuration spaces, are absent in this problem.

To satisfy this challenging trade-off, the proposed system utilizes an optimization-based planning approach. Specifically, the implementation uses Trajopt [12], which has been proven as a very robust method for manipulators and mobile manipulators in 2D. To the authors’ knowledge, Trajopt has not yet been used for 3D motion planning for mobile robots in the past.

Trajopt was selected over other optimization-based techniques such as CHOMP [44] and STOMP [45], not only due to its computational efficiency but more importantly due to its ability to use swept-out volumes as shown in Figure 3. This feature helps to ensure a minimum distance from obstacles is maintained from just the states that are optimized to also the transitions between these states, which is a very desired characteristic for a mobile system sensitive to drifting. Additionally, new cost functions can be added easily allowing future extensions.

Fig. 3: Path optimization with Trajopt [12]

in three different stages: (a) The initial path is generated by simple interpolation from

to with possibly some states in collision. (b) An intermediate stage during optimization. (c) The final trajectory, shown with the distances to the swept-out volumes.

The essential idea behind Trajopt is to represent the path from to as an ordered list of waypoints, each of which is a pose for the robot. Starting from an initial set of waypoints —generally based on linear interpolation between and , though see Section IV-A2 below— Trajopt forms a convex optimization program, in which each degree of freedom of each waypoint is a variable, and the obstacles are encoded as constraints, and the objective is to minimize a weighted form of the path length. Because of this general form, it is straightforward to insert additional constraints, based on the specific context. Trajopt solves this optimization problem using a customized sequential method. Details are available in [12]. The next few sections describe the specific adaptations made to Trajopt to achieve strong performance in this domain.

Iv-A1 Forming an effective cost function

The effectiveness of any optimization-based method is likely to depend heavily on the objective function employed in the optimization formulation. For Trajopt, the objective function is parameterized by coefficients for the path length and the obstacle avoidance and by a distance parameter , measuring the maximum distance from the obstacles where the cost will be applied. These parameters required some tuning; Section V describes the specific values utilized in our experiments.

The system employs an addition term in cost to ensure that the robot will not reach the surface, and will remain entirely underwater. The cost function was applied on the of all the states and defined as:


where . The condition in Equation 2 penalizes every state above ensuring that the robot will remain underwater, accounting also for drifting similarly to the obstacles.

Iv-A2 Sampling-based corrections for optimization local minima

A problem that many optimization-based motion planners face is the possibility that the optimization may converge to a local minimum. Though generally rare in the implemented system, this situation can present a safety hazard for the robot, because the completed path may not necessarily maintain safe distance from the obstacles. Fortunately, it is straightforward to verify whether or not the output of Trajopt does indeed avoid the obstacles correctly. If this check fails, our algorithm restarts Trajopt, using a modified initial condition, in a renewed effort to find a valid path.

To produce this modified initial path, the algorithm examines the (non-obstacle-free) path generated by the failed Trajopt run, and identifies the state in collision on that path with the highest cost. Then a collision free state is picked, through sampling, as a waypoint from the 2D plane which (a) is perpendicular to the straight line from to , (b) passes from and (c) is defined given some reasonable limits. The new initial path used to seed the next run of Trajopt is formed by interpolating from to and then from there to . If this too fails repeatedly, the algorithm falls back to randomized selection of a waypoint to modify.

Although not used in the current proposed method, for more challenging scenarios with multiple narrow passages, fast sampling based motion planners such as BiT-RRT [46] producing a set of paths for Trajopt could be used to find solutions faster obstacle-free initial paths [47].

Iv-A3 Map representations

As discussed before, Trajopt optimizes the distance between the swept-out volumes from the robot’s trajectory and the obstacles. For efficiency, Trajopt expects the map be stored in a form where the normals between the robot’s body and the obstacles can be extracted rapidly.

The experiments in this paper use a geometric method for presenting the obstacles to Trajopt, specifying their shapes and locations as instances of a set of built-in shapes, typically rectangluar boxes. A more complete implementation would utilize a point-cloud map representation, which is much more readily produced from raw sensor data. Since Trajopt is not capable of dealing effectively with point clouds directly, but only with convex shapes, an extra convex-decomposition pre-processing step would needed before the optimization. Currently, Trajopt is only able to directly process organized point-clouds, such as those produced by range sensors. However, since such high-quality point-clouds cannot be produced from online-SLAM methods, the convex decomposition stage was extended to unorganized point-clouds following the method of Zoltan et al. [48]. Though processing unorganized point clouds is significantly slower—in the authors’ informal experiments up to 3 times slower—the number of points from feature extractors used by most SLAM methods, in total, are expected to be much less reducing the delays.

Iv-B Path Tracker

The optimization stage of the proposed pipeline produces in a timely manner a path , as an ordered set of consecutive goal states that should be sequentially achieved by the robot. Directly solving online the dynamics and kinematics of the robot to achieve accurately these positions is highly challenging due to the complexity of its propulsion system [38].

For this purpose a linear PD controller proposed by Meger et al. [38] was utilized, which deploys the IMU and the depth sensor data. This closed-loop controller accepts commands in the form of the following tuple:


where is the desired forward linear velocity, is the desired heave linear velocity, is the desired depth to reach and the desired orientation for the robot to move in the world frame. The current framework, for simplicity considers only purely forward motion setting .

The PD controller, as expected, tries to fit the desired depth hastily. Assuming that the current state of the robot is and the current goal position is in the world frame, to guarantee smooth transitions that are bounded inside the calculated safe swept-out volumes of the optimization stage, the desired depth is calculated as


in which is the depth of the previous achieved goal (base case ), the depth of the current goal, and is the current measured depth. The idea is to ensure the linear change of the depth from the one position to the other and ensure straight line transitions similarly, that are assumed by Trajopt.

Regarding the desired orientation , the pitch is adjusted automatically from the desired depth and the roll doesn’t affect the direction of the motion, thus only computation of the desired yaw is needed. With respect to the translation error :


The equations above ensure that given the position of the robot, the changes in such a way that the AUV will always face the goal.

The only remaining component for the path tracker, is to provide the conditions under which a goal is declared achieved. Assuming the possible drifting is bounded by for a given speed and that the optimization was successful, the AUV should safely navigate from one goal to the next one. As a result of the above, given a threshold of , the goal is declared reached, if the error is less than and a local minimum is detected, since the robot will be sliding away due to drifting.

This policy is simple and although aggressive, it showed to be very robust on reaching the goals with sufficiently small errors. Avoiding unnecessary efforts to reach previous goals that the robot missed, and letting the robot smoothly transition to new goals, proved to be a key for following the path sufficiently.

In the case the optimization fails locally and the proposed path tracker could endanger the robot, it can be retried with a lower velocity and a shorter , so safety guarantees would be ensured during navigation from narrow passages.

V Experiments

Fig. 4: Simulated trajectories executed by the robot in Gazebo. (a-b) show a side and top perspective of the Window environment (the ceiling is not shown for the top view). (c-d) show the Pipes environment. Finally (e-f) show the Cluttered environment

Extensive experiments were performed in gazebo simulator, and in addition to that, pool trials for validation.

The main objective of the work was to show the reliable navigation functionality of Aqua2 using the proposed framework. In all the experiments the Aqua2 had a constant speed of — the expected maximum operational speed — and a minimum obstacle avoidance distance . Obstacle avoidance and path length coefficients were adjusted to relatively high values and , respectively, favoring safety over path length optimality.

V-a Simulation

Multiple challenging environments were created aiming to test the capability of the Aqua2 to overcome drifting, to successfully avoid the obstacles, to follow reasonable paths, and to surpass the local minimum problems expected to occur in man-made environments with Trajopt. The executed paths for these environments are shown in Fig. 4.

In the first environment, called the Window environment, the robot has a wall in front of it, a wall on its left, and is enclosed by a floor and a ceiling, with only one opening on the wall 8.6 meters away from the robot, simulating a submerged structure environment. This environment was intended to test the capability of the proposed method to quickly overcome not only the local minimum problem, that could occur in human-made environments, but also the drifting bounds since the Aqua2 must pass from a narrow window and change orientation rapidly. The initial path passes through the wall and the optimization is trapped to a local minimum in feasible solution. The robot after sampling with given bounds many infeasible paths are discarded quickly and the path shown is found. Also in the top view, although the drifting effect is clear, the robot still tightly passes through the window.

The second environment, called the Pipes Environment, is intended to test the ability of the Aqua2 to safely maneuver through an environment that requires a 3D motion and consecutive change on yaw, pitch and roll. It can be observed that the robot moves around the obstacles while keeping the safety distance. Trajopt produced naturally also the roll orientation that reduced the drift, smoothing the motion. Such behaviour intuitively could be explained by understanding the bounded distance optimization of the swept-out volumes between two states. A rotation is expected to occur when an obstacle is in proximity, and the roll will be adjusted so that during the motion the robot will be perpendicular to the normal of the obstacle, thus naturally reducing the drag.

The third environment, called the Cluttered Environment, focuses more on the capability of our method inherited by Trajopt, to minimize drifting passing through a sequence of obstacles that need motions with accurate and fast orientation changes. In this environment not only the roll was adjusted during optimization naturally to the motion, but also the Aqua2 was passing parallel in between each couple of pillars, maximizing the distance from both of them, thus increasing safety.

V-B Pool Trials

Fig. 5: Aqua2 AUV during deployment in the pool and a side view of the path as executed in the simulation.
Fig. 6: Top-down view of the executed path in Gazebo with representative photos from the real deployment in the pool.

Successful pool trials where conducted in the Solomon Blatt Physical Education Center’s pool of dimensions to experimentally validate the proposed approach. One of the future plans is to use the Aqua2 for shipwreck inspection and to study online state estimation in deep underwater environments, where most of the features are likely to be at the bottom. For this purpose a constraint was added to not allow roll rotations during the motion in order to keep the stereo cameras facing the floor. A similar environment to the Poles was used, with the exception that due to the expected larger drifting effect, the last pole was slightly moved further as shown in the executed path of the Gazebo simulation in Fig. 4 and Fig 6. The IMU and the depth sensor were used to guide the robot through the pool trials. The robot travelled 15m across the pool avoiding the obstacles. Fig 6 through Fig 6 present representative snapshots of the robots behaviour.

Vi Conclusions

This paper demonstrated novel capabilities for the Aqua2 AUV navigating through cluttered spaces. A known map was used to plan an efficient trajectory from an initial to a goal pose, through the space, while avoiding all obstacles, and taking into account the kinematic constraints of the vehicle. In particular, AUVs as they free float suffer from drifting as it is nearly imposible to stop in place. Numerous simulations highlighted the abilities of this agile platform to navigate in narrow spaces. Experiments at the pool demonstrated the feasibility of the proposed method and highlighted the drifting challenge.

Fig. 7: Aqua2 AUV moving over a wreck.

Verifying the robustness of underwater visualinertial state estimation SVIn [43] and ensuring effectively deployment will enable the exploration of unknown environments where the state estimation will generate partial maps online. Figure 7 presents an enclosed environment on the Pamir shipwreck, Barbados, where no prior map exists. The AUV was deployed there to performed a single straight line transect. Future work will integrate Trajopt with a point cloud generated from SVIn state estimation package to produce partial trajectories.

A variety of more challenging gaits have been produced for the Aqua2 vehicles [37]. Treating them as motion primitives and combining them with Trajopt generated trajectories will enable complex behaviours for the inspection and mapping of underwater structures, including moving inside wrecks and caves.


This work was made possible through the generous support of National Science Foundation grants (NSF 1513203, 1637876).


  • [1] R. M. Eustice, H. Singh, J. J. Leonard, and M. R. Walter, “Visually mapping the rms titanic: Conservative covariance estimates for slam information filters,” The international journal of robotics research, vol. 25, no. 12, pp. 1223–1242, 2006.
  • [2] M. Gary, N. Fairfield, W. C. Stone, D. Wettergreen, G. Kantor, and J. M. Sharp, Jr, “3d mapping and characterization of sistema zacatón from depthx (de ep p hreatic th ermal e x plorer),” in Sinkholes and the Engineering and Environmental Impacts of Karst, 2008, pp. 202–212.
  • [3] H. Johannsson, M. Kaess, B. Englot, F. Hover, and J. Leonard, “Imaging sonar-aided navigation for autonomous underwater harbor surveillance,” in 2010 IEEE/RSJ International Conference on Intelligent Robots and Systems.   IEEE, 2010, pp. 4396–4403.
  • [4] P. V. Teixeira, F. S. Hover, J. J. Leonard, and M. Kaess, “Multibeam data processing for underwater mapping,” in 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS).   IEEE, 2018, pp. 1877–1884.
  • [5] R. Przeslawski, S. Foster, J. Monk, T. Langlois, V. Lucieer, and R. Stuart-Smith, “Comparative assessment of seafloor sampling platforms. report to the national environmental science programme,” 2018.
  • [6] A. Abadie, P. Boissery, and C. Viala, “Georeferenced underwater photogrammetry to map marine habitats and submerged artificial structures,” The Photogrammetric Record, vol. 33, no. 164, pp. 448–469, 2018.
  • [7] P. Drap, “Underwater photogrammetry for archaeology,” in Special Applications of Photogrammetry.   IntechOpen, 2012.
  • [8] W. Figueira, R. Ferrari, E. Weatherby, A. Porter, S. Hawes, and M. Byrne, “Accuracy and precision of habitat structural complexity metrics derived from underwater photogrammetry,” Remote Sensing, vol. 7, no. 12, pp. 16 883–16 900, 2015.
  • [9] J. J. Leonard and H. F. Durrant-Whyte, Directed sonar sensing for mobile robot navigation.   Springer Science & Business Media, 2012, vol. 175.
  • [10] F. Menna, E. Nocerino, and F. Remondino, “Photogrammetric modelling of submerged structures: influence of underwater environment and lens ports on three-dimensional (3d) measurements,” In latest developments in reality-based 3D surveying and modelling, MDPI, Basel, Switzerland, pp. 279–303, 2018.
  • [11] P. Agrafiotis, G. I. Drakonakis, D. Skarlatos, and A. Georgopoulos, “Underwater image enhancement before three-dimensional (3d) reconstruction and orthoimage production steps: Is it worth,” Latest Developments in Reality-Based 3D Surveying and Modelling; Remondino, F., Georgopoulos, A., González-Aguilera, D., Agrafiotis, P., Eds, 2018.
  • [12] J. Schulman, J. Ho, A. X. Lee, I. Awwal, H. Bradlow, and P. Abbeel, “Finding locally optimal, collision-free trajectories with sequential convex optimization,” in Robotics: science and systems, vol. 9, no. 1.   Citeseer, 2013, pp. 1–10.
  • [13] G. Dudek, M. Jenkin, C. Prahacs, A. Hogue, J. Sattar, P. Giguere, A. German, H. Liu, S. Saunderson, A. Ripsman, S. Simhon, L. A. Torres-Mendez, E. Milios, P. Zhang, and I. Rekleitis, “A visually guided swimming robot,” in IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Edmonton AB, Canada, Aug. 2005, pp. 1749–1754.
  • [14] N. Koenig and A. Howard, “Design and use paradigms for gazebo, an open-source multi-robot simulator,” in 2004 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)(IEEE Cat. No. 04CH37566), vol. 3.   IEEE, 2004, pp. 2149–2154.
  • [15] C. Petres, Y. Pailhas, P. Patron, Y. Petillot, J. Evans, and D. Lane, “Path planning for autonomous underwater vehicles,” IEEE Transactions on Robotics, vol. 23, no. 2, pp. 331–341, 2007.
  • [16] S. Williams, G. Dissanayake, and H. Durrant-Whyte, “Towards terrain-aided navigation for underwater robotics,” Advanced Robotics, vol. 15, no. 5, pp. 533–549, 2001.
  • [17] B. Garau, M. Bonet, A. Alvarez, S. Ruiz, and A. Pascual, “Path planning for autonomous underwater vehicles in realistic oceanic current fields: Application to gliders in the western mediterranean sea,” Journal of Maritime Research, vol. 6, no. 2, pp. 5–22, 2009.
  • [18] A. Alvarez, A. Caiti, and R. Onken, “Evolutionary path planning for autonomous underwater vehicles in a variable ocean,” IEEE Journal of Oceanic Engineering, vol. 29, no. 2, pp. 418–429, 2004.
  • [19] N. K. Yilmaz, C. Evangelinos, P. F. Lermusiaux, and N. M. Patrikalakis, “Path planning of autonomous underwater vehicles for adaptive sampling using mixed integer linear programming,” IEEE Journal of Oceanic Engineering, vol. 33, no. 4, pp. 522–537, 2008.
  • [20] C. J. Green and A. Kelly, “Toward optimal sampling in the space of paths.” in ISRR.   Citeseer, 2007, pp. 281–292.
  • [21] R. A. Knepper and M. T. Mason, “Empirical sampling of path sets for local area motion planning,” in Experimental Robotics.   Springer, 2009, pp. 451–462.
  • [22] ——, “Real-time informed path sampling for motion planning search,” The International Journal of Robotics Research, vol. 31, no. 11, pp. 1231–1250, 2012.
  • [23] M. Pivtoraiko, R. A. Knepper, and A. Kelly, “Differentially constrained mobile robot motion planning in state lattices,” Journal of Field Robotics, vol. 26, no. 3, pp. 308–333, 2009.
  • [24] M. S. Branicky, R. A. Knepper, and J. J. Kuffner, “Path and trajectory diversity: Theory and algorithms,” in Robotics and Automation, 2008. ICRA 2008. IEEE International Conference on.   IEEE, 2008, pp. 1359–1364.
  • [25] R. A. Knepper, S. S. Srinivasa, and M. T. Mason, “Toward a deeper understanding of motion alternatives via an equivalence relation on local paths,” The International Journal of Robotics Research, vol. 31, no. 2, pp. 167–186, 2012.
  • [26] S. Karaman and E. Frazzoli, “Incremental sampling-based algorithms for optimal motion planning,” Robotics Science and Systems VI, vol. 104, no. 2, 2010.
  • [27]

    J. D. Gammell, S. S. Srinivasa, and T. D. Barfoot, “Informed rrt*: Optimal sampling-based path planning focused via direct sampling of an admissible ellipsoidal heuristic,” in

    2014 IEEE/RSJ International Conference on Intelligent Robots and Systems.   IEEE, 2014, pp. 2997–3004.
  • [28] L. Janson, E. Schmerling, A. Clark, and M. Pavone, “Fast marching tree: A fast marching sampling-based method for optimal motion planning in many dimensions,” The International journal of robotics research, vol. 34, no. 7, pp. 883–921, 2015.
  • [29] J. D. Gammell, S. S. Srinivasa, and T. D. Barfoot, “Batch informed trees (bit*): Sampling-based optimal planning via the heuristically guided search of implicit random geometric graphs,” in 2015 IEEE International Conference on Robotics and Automation (ICRA).   IEEE, 2015, pp. 3067–3074.
  • [30] J. D. Hernández, M. Moll, E. Vidal, M. Carreras, and L. E. Kavraki, “Planning feasible and safe paths online for autonomous underwater vehicles in unknown environments,” in 2016 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS).   IEEE, 2016, pp. 1313–1320.
  • [31] E. Vidal, J. D. Hernández, N. Palomeras, and M. Carreras, “Online robotic exploration for autonomous underwater vehicles in unstructured environments,” in 2018 OCEANS-MTS/IEEE Kobe Techno-Oceans (OTO).   IEEE, 2018, pp. 1–4.
  • [32] L. Zhu, X. Cheng, and F.-G. Yuan, “A 3d collision avoidance strategy for uav with physical constraints,” Measurement, vol. 77, pp. 40–49, 2016.
  • [33] S. Hrabar, “3d path planning and stereo-based obstacle avoidance for rotorcraft uavs,” in 2008 IEEE/RSJ International Conference on Intelligent Robots and Systems.   IEEE, 2008, pp. 807–814.
  • [34] G. Antonelli, S. Chiaverini, R. Finotello, and R. Schiavon, “Real-time path planning and obstacle avoidance for rais: an autonomous underwater vehicle,” IEEE Journal of Oceanic Engineering, vol. 26, no. 2, pp. 216–227, 2001.
  • [35] M. Eichhorn, “A reactive obstacle avoidance system for an autonomous underwater vehicle,” in IFAC world congress, 2005, pp. 3–8.
  • [36] L. R. Fodrea and A. J. Healey, “Obstacle avoidance control for the remus autonomous underwater vehicle,” IFAC Proceedings Volumes, vol. 36, no. 4, pp. 103–108, 2003.
  • [37] D. Meger, J. C. G. Higuera, A. Xu, P. Giguere, and G. Dudek, “Learning legged swimming gaits from experience,” in IEEE International Conference on Robotics and Automation (ICRA).   IEEE, 2015, pp. 2332–2338.
  • [38] D. Meger, F. Shkurti, D. Cortés Poza, P. Giguère, and G. Dudek, “3d trajectory synthesis and control for a legged swimming robot,” in 2014 IEEE/RSJ International Conference on Intelligent Robots and Systems, Sep. 2014, pp. 2257–2264.
  • [39] T. Manderson and G. Dudek, “Gpu-assisted learning on an autonomous marine robot for vision based navigation and image understanding,” in MTS/IEEE OCEANS, Charleston, SC, USA, Oct. 2018.
  • [40] T. Manderson, J. Gamboa Higuera, R. Cheng, and G. Dudek, “Vision-based autonomous underwater swimming in dense coral for combined collision avoidance and target selection,” in Proceedings of IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2018.
  • [41] G. Dudek, P. Giguere, C. Prahacs, S. Saunderson, J. Sattar, L.-A. Torres-Mendez, M. Jenkin, A. German, A. Hogue, A. Ripsman, et al., “Aqua: An amphibious autonomous robot,” Computer, vol. 40, no. 1, pp. 46–53, 2007.
  • [42] M. Fiala, “Artag, a fiducial marker system using digital techniques,” in

    2005 IEEE Computer Society Conference on Computer Vision and Pattern Recognition (CVPR’05)

    , vol. 2, pp. 590–596.
  • [43] S. Rahman, A. Q. Li, and I. Rekleitis, “Sonar Visual Inertial SLAM of Underwater Structures,” in IEEE International Conference on Robotics and Automation, Brisbane, Australia, May 2018, pp. 5190–5196.
  • [44] N. Ratliff, M. Zucker, J. A. Bagnell, and S. Srinivasa, “Chomp: Gradient optimization techniques for efficient motion planning,” 2009.
  • [45] M. Kalakrishnan, S. Chitta, E. Theodorou, P. Pastor, and S. Schaal, “Stomp: Stochastic trajectory optimization for motion planning,” in 2011 IEEE international conference on robotics and automation.   IEEE, 2011, pp. 4569–4574.
  • [46] L. Jaillet, J. Cortes, and T. Simeon, “Transition-based rrt for path planning in continuous cost spaces,” in 2008 IEEE/RSJ International Conference on Intelligent Robots and Systems, Sep. 2008, pp. 2145–2150.
  • [47] L. Li, “Birrtopt: A combined software framework for motion planning applied on atlas robot,” 2016.
  • [48] Z. C. Marton, R. B. Rusu, and M. Beetz, “On Fast Surface Reconstruction Methods for Large and Noisy Datasets,” in Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), Kobe, Japan, May 12-17 2009.