**7.3 Obstacle avoidance in cluttered environments**

Another application which benefits from the aforementioned trajectory generation algorithm is real-time OA in a highly cluttered environment. Figure 24 illustrates simulated trajectories for avoiding a field of point-like objects in the 2D horizontal plane (e.g. a kelp forest) and in all three dimensions (e.g. a mine field). In both simulations, the performance index was designed to minimize deviations from a predefined survey track line while avoiding all randomly generated obstacles via a CPA calculation. Terminal boundary conditions for the OA manoeuvre were chosen to ensure the UUV rejoined the desired track line before reaching the next waypoint (i.e. the manoeuvre terminated at a position 95% along the track segment). Initial boundary conditions were chosen to simulate a random obstacle detection which triggers an avoidance manoeuvre after the UUV has completed about 10% of the predefined track segment. For illustration purposes, Fig.24 includes several candidate trajectories evaluated during the optimization process although the algorithm ultimately converged to the trajectory depicted with a thicker (red) line (CPA distances to each obstacle appear as dashed lines).

Figure 25 shows the results from an initial sea trial of 3D OA that took place in Monterey Bay on 9 December 2008. This experiment tested periodic trajectory generation and replanning on the REMUS UUV using a simulated obstacle map comprised of oriented bounding boxes. As seen in Fig.25, initially the REMUS UUV follows a predefined track segment (dash-dotted line) at 4 meters altitude. At some point the vehicle's FLS simulator "detects" an obstacle (i.e. the current REMUS position and orientation place the virtual obstacle within the range and aperture limits of the FLS). This activates the OA mode, and the planner generates an initial trajectory (green) from the current vehicle position to the final waypoint. REMUS follows this trajectory until the next planning cycle (4 seconds later) when the vehicle generates a new trajectory and continues this path planning-path following cycle.

#### **7.4 Obstacle avoidance in restricted waterways**

The NPS CAVR in collaboration with Virginia Tech (VT) is developing technologies to enable safe, autonomous navigation by USVs operating in unknown riverine environments. This project involves both surface (laser) and subsurface (sonar) sensing for obstacle detection, localization, and mapping as well as global-scale (wide area) path planning, localscale trajectory generation, and robust vehicle control. The developed approach includes a hybrid receding horizon control framework that integrates a globally optimal path planner with a local, near-optimal trajectory generator (Xu et al., 2009).

The VT global path planner uses a Fast Marching Method (Sethian, 1999) to compute the optimal path between a start location and a desired goal location based on all available map information. While resulting paths are globally optimal, they do not incorporate vehicle dynamics and thus cannot be followed accurately by the USV autopilot. Moreover, since level set calculations are computationally expensive, global plans are recomputed only when necessary and thus do not always incorporate recently detected obstacles. Therefore, a complimentary local path planner operating over a short time horizon is required to incorporate current sensor information and generate feasible OA trajectories. The IDVDbased trajectory generator described above is ideally suited for this purpose. VT has developed a set of matching conditions which guarantee the asymptotic stability of this 92 Autonomous Underwater Vehicles

Another application which benefits from the aforementioned trajectory generation algorithm is real-time OA in a highly cluttered environment. Figure 24 illustrates simulated trajectories for avoiding a field of point-like objects in the 2D horizontal plane (e.g. a kelp forest) and in all three dimensions (e.g. a mine field). In both simulations, the performance index was designed to minimize deviations from a predefined survey track line while avoiding all randomly generated obstacles via a CPA calculation. Terminal boundary conditions for the OA manoeuvre were chosen to ensure the UUV rejoined the desired track line before reaching the next waypoint (i.e. the manoeuvre terminated at a position 95% along the track segment). Initial boundary conditions were chosen to simulate a random obstacle detection which triggers an avoidance manoeuvre after the UUV has completed about 10% of the predefined track segment. For illustration purposes, Fig.24 includes several candidate trajectories evaluated during the optimization process although the algorithm ultimately converged to the trajectory depicted with a thicker (red) line (CPA distances to

Figure 25 shows the results from an initial sea trial of 3D OA that took place in Monterey Bay on 9 December 2008. This experiment tested periodic trajectory generation and replanning on the REMUS UUV using a simulated obstacle map comprised of oriented bounding boxes. As seen in Fig.25, initially the REMUS UUV follows a predefined track segment (dash-dotted line) at 4 meters altitude. At some point the vehicle's FLS simulator "detects" an obstacle (i.e. the current REMUS position and orientation place the virtual obstacle within the range and aperture limits of the FLS). This activates the OA mode, and the planner generates an initial trajectory (green) from the current vehicle position to the final waypoint. REMUS follows this trajectory until the next planning cycle (4 seconds later) when the vehicle generates a new trajectory and continues this path planning-path

The NPS CAVR in collaboration with Virginia Tech (VT) is developing technologies to enable safe, autonomous navigation by USVs operating in unknown riverine environments. This project involves both surface (laser) and subsurface (sonar) sensing for obstacle detection, localization, and mapping as well as global-scale (wide area) path planning, localscale trajectory generation, and robust vehicle control. The developed approach includes a hybrid receding horizon control framework that integrates a globally optimal path planner

The VT global path planner uses a Fast Marching Method (Sethian, 1999) to compute the optimal path between a start location and a desired goal location based on all available map information. While resulting paths are globally optimal, they do not incorporate vehicle dynamics and thus cannot be followed accurately by the USV autopilot. Moreover, since level set calculations are computationally expensive, global plans are recomputed only when necessary and thus do not always incorporate recently detected obstacles. Therefore, a complimentary local path planner operating over a short time horizon is required to incorporate current sensor information and generate feasible OA trajectories. The IDVDbased trajectory generator described above is ideally suited for this purpose. VT has developed a set of matching conditions which guarantee the asymptotic stability of this

**7.3 Obstacle avoidance in cluttered environments** 

each obstacle appear as dashed lines).

**7.4 Obstacle avoidance in restricted waterways** 

with a local, near-optimal trajectory generator (Xu et al., 2009).

following cycle.

Fig. 24. Simulated 2D (a) and 3D (b) near-optimal OA trajectories

Fig. 25. REMUS sea trial results demonstrating periodic planning and path following

Real-Time Optimal Guidance and Obstacle Avoidance for UMVs 95

map with red and green colour channels representing the probability that a cell is occupied or unoccupied, respectively. Black pixels represent cells with unknown status. A short green line segment depicts the USV's orientation when the local planner is invoked, and the resulting trajectory is shown in yellow. The first simulation (Fig.26a) shows a local trajectory which deviates from the stale global plan to avoid a sand bar detected with sonar. In the second simulation (Fig.26b) the USV is initially heading in a direction opposite from the global path, but the local planner generates a dynamically feasible trajectory to turn around

To track these local trajectories, the 2D controller described in Section 6.1 was implemented on the SeaFox USV by mapping the controller's turn rate commands into rudder commands understood by the SeaFox autopilot. After validating the turn rate controller design during sea trials on Monterey Bay, the direct method trajectory generator and closed-loop path following controller were tested on the Pearl River in Mississippi on 22 May 2010 (Fig.27). For this test, the local planner used a sonar map of the operating area to generate the trajectory (the cyan line) from an initial orientation (depicted by the yellow arrow) to a

and rejoin the global path later.

Fig. 27. Path-following controller test on the Pearl River

framework. When these matching conditions are satisfied, the sequence of local trajectories will converge to the global path's goal location. If the local trajectories no longer satisfy these conditions (usually because the global path is no longer compatible with recently detected obstacles), the global path is recomputed.

Simulation results demonstrate the need for local trajectories that incorporate vehicle dynamics and real-time sensor data (Fig.26). For this simulation, an initial level set map was computed using an occupancy grid created by masking land areas as occupied and water areas as unoccupied in an aerial image of the Sacramento River operating area. Performing gradient descent on the level set from the USV's initial position produces an optimal path shown in blue. To simulate local trajectory generation with a stale global plan, the initial level set map was not updated during the entire simulation. Meanwhile, to simulate access to realtime sensor data, the local planner was provided with a complete sonar map generated during a previous SeaFox survey of the area. In Fig.26, this sonar map has been overlaid on the *a priori*

a)

b)

Fig. 26. Simulated local OA trajectories

94 Autonomous Underwater Vehicles

framework. When these matching conditions are satisfied, the sequence of local trajectories will converge to the global path's goal location. If the local trajectories no longer satisfy these conditions (usually because the global path is no longer compatible with recently

Simulation results demonstrate the need for local trajectories that incorporate vehicle dynamics and real-time sensor data (Fig.26). For this simulation, an initial level set map was computed using an occupancy grid created by masking land areas as occupied and water areas as unoccupied in an aerial image of the Sacramento River operating area. Performing gradient descent on the level set from the USV's initial position produces an optimal path shown in blue. To simulate local trajectory generation with a stale global plan, the initial level set map was not updated during the entire simulation. Meanwhile, to simulate access to realtime sensor data, the local planner was provided with a complete sonar map generated during a previous SeaFox survey of the area. In Fig.26, this sonar map has been overlaid on the *a priori*

a)

b)

Fig. 26. Simulated local OA trajectories

detected obstacles), the global path is recomputed.

map with red and green colour channels representing the probability that a cell is occupied or unoccupied, respectively. Black pixels represent cells with unknown status. A short green line segment depicts the USV's orientation when the local planner is invoked, and the resulting trajectory is shown in yellow. The first simulation (Fig.26a) shows a local trajectory which deviates from the stale global plan to avoid a sand bar detected with sonar. In the second simulation (Fig.26b) the USV is initially heading in a direction opposite from the global path, but the local planner generates a dynamically feasible trajectory to turn around and rejoin the global path later.

To track these local trajectories, the 2D controller described in Section 6.1 was implemented on the SeaFox USV by mapping the controller's turn rate commands into rudder commands understood by the SeaFox autopilot. After validating the turn rate controller design during sea trials on Monterey Bay, the direct method trajectory generator and closed-loop path following controller were tested on the Pearl River in Mississippi on 22 May 2010 (Fig.27). For this test, the local planner used a sonar map of the operating area to generate the trajectory (the cyan line) from an initial orientation (depicted by the yellow arrow) to a

Fig. 27. Path-following controller test on the Pearl River

Real-Time Optimal Guidance and Obstacle Avoidance for UMVs 97

Furukawa, T. (2006). *Reactive obstacle avoidance for the REMUS underwater autonomous vehicle* 

Gadre, A., Kragelund, S., Masek, T., Stilwell, D., Woolsey, C. & Horner, D. (2009).

Healey, A. J. (2004). Obstacle avoidance while bottom following for the REMUS

Horner, D. & Yakimenko, O. (2007). Recent developments for an obstacle avoidance system

Horner, D., McChesney, N., Kragelund, S. & Masek, T. (2009). 3D reconstruction with an

*on Unmanned Untethered Submersible Technology (UUST09), Durham, NH, USA*

Kaminer, I., Yakimenko, O., Dobrokhodov, V., Pascoal, A., Hovakimyan, N., Cao, C., Young,

Xu, B., Kurdila, A. J. & Stilwell, D. J. (2009). A hybrid receding horizon control method for

Yakimenko, O. & Slegers, N. (2009). Optimal control for terminal guidance of autonomous

Yakimenko, O. (2000). Direct method for rapid prototyping of near optimal aircraft trajectories. *Journal of Guidance, Control, and Dynamics*, 23(5), 865-875 Yakimenko, O. (2011). *Engineering computations and modeling in MATLAB/Simulink*. AIAA

Yakimenko, O. A. (2008). Real-time computation of spatial and flat obstacle avoidance

path planning uncertain environments. In: *Proceedings of the IEEE/RSJ International* 

parafoils. In: *Proceedings of the 20th AIAA Aerodynamic Decelerator Systems Technology* 

trajectories for AUVs. In: *Proceedings of the 2nd IFAC workshop on Navigation, Guidance* 

*AIAA Guidance, Navigation, and Control conference*, *Hilton Head, SC, USA*  Kreuzer, J. (2006). 3D programming – weekly: Bounding boxes. Collision detection tutorial webpage. Available from: www.3dkingdoms.com/weekly/weekly.php?a=21 Masek, T. (2008). *Acoustic image mModels for navigation with forward-looking sonars*. MS Thesis,

Subsurface and surface sensing for autonomous navigation in a riverine environment. In: *Proceedings of the Association of Unmanned Vehicle Systems International (AUVSI) Unmanned Systems North America convention*, *Washington, DC,* 

autonomous underwater vehicle. In: *Proceedings of the IFAC conference*, *Lisbon*,

for a small AUV. In: *Proceedings of the IFAC conference on Control Applications in* 

AUV-mounted forward-looking sonar. In: *Proceedings of the International symposium* 

A. & Patel, V. (2007). Coordinated path following for time-critical missions of multiple UAVs via L1 adaptive output feedback controllers. In: *Proceedings of the*

*using a forward looking sonar*. MS Thesis, NPS, Monterey, CA, USA

*USA* 

*Portugal* 

*Marine Systems*, *Bol*, *Croatia*

www.hydroidinc.com/remus100.html

NPS, Monterey, CA, USA

*conference, Seattle, WA, USA* 

Hydroid, Inc. (2011). REMUS 100 webpage. Available from:

Northwind Marine. (2011). SeaFox webPage. Available from:

Sethian, J. (1999). Fast marching method. *SIAM Review*, 41(2), 199-235

*conference on Intelligent Robots and Systems, St. Louis, MO, USA*

Education Series, ISBN 978-1-60086-781-1, Arlington, VA, USA

*and Control of Underwater Vehicles (NGCUV'08)*, *Killaloe*, *Ireland*

www.northwindmarine.com/military-boats

desired goal point (depicted by a circle). The SeaFox USV then followed it almost precisely (the magenta line). As seen from Fig.27 the trajectory generator was invoked at an arbitrary location while the USV was performing a clockwise turn. Since the USV was commanded to return to its start location upon completion of this manoeuvre, the magenta line includes a portion of this return trajectory as well (otherwise, the actual USV track would be nearly indistinguishable from the reference trajectory on this plot).
