Journal of Robotics

Volume 2011 (2011), Article ID 703959, 8 pages

http://dx.doi.org/10.1155/2011/703959

## Unified Approach of Unmanned Surface Vehicle Navigation in Presence of Waves

Department of Mechanical Engineering, Technion, Israel Institute of Technology, Haifa 32000, Israel

Received 1 August 2011; Revised 26 October 2011; Accepted 19 November 2011

Academic Editor: Yangmin Li

Copyright © 2011 Oren Gal. This is an open access article distributed under the Creative Commons Attribution License, which permits unrestricted use, distribution, and reproduction in any medium, provided the original work is properly cited.

#### Abstract

Most of the present work for unmanned surface vehicle (USV) navigation does not take into account environmental disturbances such as ocean waves, winds, and currents. In some scenarios, waves should be treated as special case of dynamic obstacle and can be critical to USV’s safety. For the first time, this paper presents unique concept facing this challenge by combining ocean waves' formulation with the probabilistic velocity obstacle (PVO) method for autonomous navigation. A simple navigation algorithm is presented in order to apply the method of USV’s navigation in presence of waves. A planner simulation dealing with waves and obstacles avoidance is introduced.

#### 1. Introduction

Most of the work on motion planning over the past twenty years has been focused on ground and aerial vehicles. Recently, the research is also focused on unmanned surface and underwater vehicles [1–4]. Motion planning of USV usually do not deal with waves, currents, and winds, which can be very critical in order to ensure *USV’s safety*. Generally, we distinguish between local and global planners. The local planner generates one or a few steps at every time step, whereas the global planner uses a global search to the goal over a time-spanned tree. Examples of local (reactive) planners are [5–7], but most do not guarantee safety as they are too slow and hence their ability to look ahead and avoid inevitable collision states (ICSs) [8] is very limited. Recently, iterative planners [9–14] were developed that compute several steps at a time, subject to the available computational time. The trajectory is generated incrementally by exploring a search tree and choosing the best branch. These planners also do not address the issue of safety and under actuated models and, above all, do not take into account the environmental disturbances (currents, waves, winds, clutter) in marine environments that can be crucial to the safety of an unmanned vehicle.

Only a few works have addressed the safety issue in dynamic environments, which is crucial for partial (local) planning such as USV motion planning. One approach of safe planning is to use braking policies [15]; another is to ensure local avoidance for a limited time [13]. However, neither one considers the dynamic of the USV nor the environmental disturbances. A promising approach to safe motion planning in dynamic environment is the consideration of “regions of inevitable collision,” first introduced in [16] and later extended in [8, 17–19], but still do not treat under actuated model and environmental disturbances.

This paper addresses USV’s safe navigation avoiding moving obstacles considering waves and under actuated constrains. Ocean waves are extremely hard to map or to predict. Wave’s disturbance is taken into account by combining them to occupancy grid using probabilistic velocity obstacle (PVO) concept. Our planner generates near-time optimal trajectories by selecting at each time step a safe velocity that minimizes time to the goal dealing with the most significant phenomena in marine environments/waves. Using a cost function (minimum time or distance to goal, minimum fuel, etc.) The allowed attainable velocities are sorted, and the safe and the optimal one is chosen at each time step, for each time step the possible motion primitives of the USV are computed taking into account kinematic and dynamic constraints. If the primitive is found to be safe enough, it is chosen to be the next USV’s control, otherwise, it is discarded. The planner is demonstrated for online motion planning dealing with waves and USV’s dynamic constraints in marine environment.

*Main Contribution*

For the first time this research generates trajectories of USV in presence of waves. The PVO concept do not deal with USV’s dynamic model nor waves models. We extend the PVO concept to marine environments presenting full planner with optimal trajectories, treating unsolved issue of autonomous navigation with waves constraints.

#### 2. Probabilistic Velocity Obstacle

The probabilistic velocity obstacle (PVO) [20] concept extends the velocity obstacle (VO) [21–24] method by considering uncertainty of the obstacles both in velocity and position. The PVO concept implemented as discrete occupancy grid that is based on dividing the space into grid and attending different statistical values in each cell [25]. The value of each cell in the grid represents the probability of an obstacle to be on a specific location based on the other obstacles velocities. Each cell stores probabilistic value:(i)a value of probability occupation ;(ii)a probabilistic distribution function on a histogram of possible velocities , .

The predicted occupation of each cell is: where represents the cell, denotes the probability of occupancy of the cell , denotes the probability that the cell’s velocity is . We define as time to collision between the robot and the obstacle, and as stopping time including delay time of the robot to change current velocity, , where is the time delay of the robot dynamics. Estimating time to collision value is related to the highest probability to collision, , that we can deal with during robot’s motion. Considering a robot velocity from present time to some specific future time leads to a collision, if there is a collision in the interval , denote as , or if there is a collision at time instant , . A cumulative probability of collision from time 0 to time step is recursively computed, : where where represents the probability to collision, Time to collision is the minimum between and , which represents the time period of a constant motion models reliability: If , then the velocity is dangerous and discarded.

This paper presents waves formulation in PVO concept and a basic formulation of PVO concept, which is not intuitive. Extended description and mathematical proofs of PVO can be found in [20].

#### 3. Waves

The PVO concept deals with static and dynamic obstacles with uncertainties, in this paper this concept is extended for a special terrain, water. As mentioned before, ocean waves can cause some unwanted effects such as drifts and tracking control errors, and in some cases should be treated as “virtual” obstacles and must be taken into account. For that, a basic knowledge of ocean waves is needed.

##### 3.1. Theory of Ocean Waves

Ocean waves are usually approximated by models of winds generating waves. Common waves modeling are based on nonlinear models of wave spectrum, using wave response transfer function that can be implemented as low-level control system, such as autopilot wave filtering. The waves spectrum depends on a large number of parameters and in some cases include empirical results [26].

All of the spectrum modes only take into account wave’s peak as a dangerous part that can cause collision, ignoring wave’s lower parts. In this paper, we will assume that all kinds of waves are characterized with constant direction and frequency, therefore we do not consider superposition of two waves. In order to predict waves, we have to measure obstacles, waves, USV’s velocity, and location. The next section will focus on practical ways to measure waves, obstacles, and USV’s motion parameters.

##### 3.2. Measuring Waves Parameters and USV State

In order to navigate safely, USV’s environmental parameters are needed. Usually, motion planning methods assume accurate and available position and velocity of the vehicle. However, in marine environment, most of the measurement equipment produce false alarms and inaccuracy caused by clutter and environmental disturbances.

First, we have to know the exact position of the USV in real time; for that a standard GPS with one pulse per second (PPS) can be satisfied (higher accuracy estimation can be achieved by using Kalman filter). Moreover, USV’s and obstacle’s location and velocity also need to be measured. The obstacles’ parameters can be measured by yacht sailing radar, nevertheless, such radars have several limitations such as: identifying objects that located close to radar’s antenna, limited number of targets that can be tracked, and so forth. USV’s close range obstacles identification can be handled by vision methods. These methods are based on cameras that located around the USV and can deal with short-range obstacles detection combining image process concept known as automatic target detection (ATD) algorithms.

Height and the direction waves measured by standard equipment (such as ADCP Waves Array sensor of Teledyne company that can be used for such application). These measurements can be integrated with off-line measurements and estimated parameters for a specific day from hydroanalysis based on winds measurements. Sensors inaccuracy and noises are not included in this model.

##### 3.3. Wave Probability

A common statistical variable used in hydrology is (also known as , the significant wave height). This variable represents the average of the third highest waves measured for a long period of time. assumed to be known at each time step in real time, based on measured technique detailed in Section 3.2. In Figure 1, we can see an example for wave heights over time. Between the two broken lines, the bold line is the average of the third highest lines . After many measurements and observations (which can be done off-line), a statistical connection between and height of a single wave with amplitude can be determined. From empirical model [26], the well-accepted assumption of wave’s probability with amplitude is where is the constant amplitude, the wave height is approximately twice the amplitude. According to that we can calculate at any given time for the current wave’s parameters the collision probabilities which risks the USV, , and decide whether it is safe to ignore the waves (which means that a special maneuver is needed to avoid the virtual obstacle).

#### 4. Marine Probabilistic Velocity Obstacles

In this section we present new formulation of the PVO concept that can be used for USV motion planning in presence of waves using occupancy grid. Unlike the classic PVO concept, the MPVO defines the values over the probability grid and time limitation regarding waves in marine environment.

##### 4.1. Waves Occupancy Grid

Probabilistic occupancy grids are well-known structures used for environmental representation [25]. The space is divided in finite number of cells, each cell stores a probabilistic estimation that combines waves and obstacles. Just like any other obstacles, we would like to include wave’s properties: size (height, direction) and speed. By measuring and estimating wave’s shape frequency and direction, we can calculate the occupancy grid of probabilities for different waves by using (6). By measuring wave’s frequency and height as cycled disturbance with a constant frequency, wave characters on a grid can be defined.

In Figure 2, several waves moving from the top left corner to the bottom right. The probability for each squared cell is calculated based on sinusoidal equations of the wave’s shape. Integrating this information with measured values (wave’s shape and frequency), the grid in Figure 2 can be represented. In the right side of Figure 2, we can see the grid for classic wave movement. The darker cells represent higher probability for wave’s spectrum. On the left side of Figure 2, the grid represents wave’s shape. The probability on each cell in the grid is where is the wave’s frequency and is the phase at cell . The phase at each cell is where is the distance to the next wave peak and is the wave length.

As we mentioned before, the low part of the wave does not oppose as much danger as the peaks of the waves, therefore, only the peaks are taken into account. As can see in Figure 2, the probability of the low part of waves is approximately zero (white cells). USV’s location is updated from the GPS at every time step (averagely 1 second), for that, moderated waves influence is minor. The occupancy grid representing waves is combined with PVO method for moving obstacles and implemented as detailed in Section 5. The occupancy grid is updated at each wave cycle, .

##### 4.2. Danger Time

One of the most significant parameters in PVO concept is the time danger, but unlike other terrains stopping in front of the wave is not practice. As well known, the best way of big vessels to deal with high wave is to move vertically (normally) to the wave’s direction (neglecting stability analysis and ratio of the USV to wave height). We define as the time it takes to change USV’s current course to a vertical course regarding wave’s direction. We define as the first time when so the USV enters unsafe state: where In a case of , USV cannot change his current course moving vertically to the wave’s direction, therefore the velocity is unsafe.

##### 4.3. Obstacles’ Properties

Obstacle’s location (latitude, longitude, course) and velocity in marine environment are usually measured by Radar sensor at each time step (1 sec) and identical to nonlinear velocity obstacles (NLVO) assumptions [23, 24]. We assume nonlinear velocity of the obstacles with general trajectory without wave’s dynamic effect regarding obstacle’s location. We model the obstacles as convex shape. However, since the Radar measure the obstacle’s state at each time step, wave’s effect and obstacle velocity are taken into account.

#### 5. The Planner

In this section we describe planner’s principles. The planner based on third highest waves value that can be measured online using drift actuators or offline before mission, which enable to compute for each cell . We seek for single velocity that will be safe from obstacles and waves, that is, allows the USV to avoid the obstacles and if needed to move vertically to the waves. The algorithm sorts out the possible velocities for the next time step by the cost function, and then checks each velocity from the best available velocity to the worst. If the velocity causes to collision with any obstacle in the near future it’s discarded immediately. If the velocity was not discarded due to risk of collision, then it is checked from the wave’s aspect. If waves do not oppose any danger, this velocity will be chosen for the next time step of the USV, otherwise, other velocities are also being checked. If no velocity is found to satisfy completely the waves’ conditions, then the velocity that holds the least risk to the USV (and of course does not have risk of collision) will be chosen for the next time step of the USV, in that way we maintain minimum risk from waves. The planner search based on one step ahead greedy algorithm by exploring the best node at each time step using local planning. Algorithm pseudocode is detailed in Section 5.4.

##### 5.1. System Dynamics

We consider 3 DOF horizontal model neglecting heave, pitch and roll. The dynamic model of the USV is under actuated and suitable for small marine vehicles similar to the common USV’s scale [26] in the industry. USV actuators dynamic’s and delays are neglected in this model. We introduce the basic marine model and later on we detail kinematic explicit form for our planner: where is system inertia matrix, -Coriolis-centripetal matrix, -damping matrix, represents the USV’s position and orientation in NED system (North-East-Down coordinate system, -axis points toward true north point, etc. [26]), is the velocity vector, represents the USV’s controls, where is the propulsion force along surge DOF and represents the propulsion moment along yaw DOF. is the transformation matrix between velocities. USV’s velocities and rotation standard definition can be seen in [26].

The planner is based on explicit form for kinematic model:: :

##### 5.2. Motion Primitives

We use trims and primitive libraries as discrete point in control space and motion primitives connect those points similar to Maneuver Automaton [27, 28]. The vehicle model is under actuated with two control inputs, therefor we control only the propulsion force at surge DOF and propulsion moment at yaw DOF. We use a simple set of speed controller in grid points in as set controllers from and , respectively, denoted as : A motion primitive moves from one speed controller set point to another. At each time step we compute the cost as detailed in Section 5.3, , for the updated USV controller : the safe controller with the lower cost is explored in the next time step, until the USV gets to the target.

##### 5.3. Cost Function

Our search is guided by a minimum time cost function to produce near-time optimal trajectories to the goal satisfying HJB equation. The cost function for each primitive is the minimum time to the goal from the current USV’s state to the target point. It is determined by first computing the minimum time to the goal from the current state , where to the target point for each axis [29, 30]: where is the region below and above the switching curve in the state space (analytic solution from Bang-Bang optimal control problem): where and are the USV’s controller from and respectively, and and are the target point, , and the velocity at the target which is zero in our case. Considering both axes, the minimum time to the goal used in the cost function is the largest of the times computed for both axes [29]. This cost function produces sub time-optimal trajectories to the goal.

##### 5.4. Pseudocode

See Algorithm 1.

##### 5.5. Planner Convergence

Planning in dynamic environments is a well-known NP-hard problem, and convergence can not be proofed for general dynamic environment. On the other hand, grid-based planning methods are known to be resolution complete based on the fact that the grid can be very dense and completeness can be achieved. MPVO algorithm is a grid-based concept and for that convergence is the same as resolution completeness. The convergence is a theoretical one, so a very dense grid leads to very long computation time, which is not applicable in dynamic environments.

#### 6. Simulation Results

We implemented the algorithm in Matlab application and tested it in various simulated environments. The online planner was implemented and tested for obstacle-free, and crowded static and dynamic marine environments. The marine environment was simulated for random values of , and with several values of . Figure 3 shows the complete simulation environment: USV’s current position represented by a blue point, target point marked by a yellow triangle, initial USV’s location marked by a blue triangle and the obstacles marked by red circles. Wave’s values based on maneuver capabilities of small USV (10 meters length) was simulated as: [m], , [sec], [sec], [m], [Hz]. Figure 4 shows planner simulation in crowded dynamic environment with . The USV avoid from the first obstacle ahead and traveling to the goal avoiding other obstacles considering wave’s parameter as mentioned above, in this case for all : . In Figure 5 the USV deals with the same scenario but was set to lower value (0.4), the vehicle react earlier to the obstacle ahead thats because in a specific time , so USV chose conservative trajectory to avoid collision. In Figure 6 global running time algorithms versus introduced. Time step was set to 0.1 [sec] from practical sensors abilities measuring wave parameters. The algorithm overall running time for 0.1 [sec] time step measured to be 10 [sec] for approximate time mission of 6000 [sec], which indicates planner online planning abilities.

#### 7. Conclusions

In this paper, a new concept dealing with ocean waves and planning safe trajectory of USV was introduced. In order to deal with the waves disturbances, we used a grid of probabilities and rephrased the PVO concept dealing with waves. A practical aspect to measure obstacles and waves in marine environments was also introduced. A planner simulations navigating through waves and avoiding obstacles considering USV’s dynamic model constrains was developed. Future work will focus on considering smaller waves as well as their long-term effect over the USV’s platform.

#### Acknowledgment

The author would like to thank Yair Steren for his help during the research.

#### References

- M. R. Benjamin, J. A. Curcio, J. J. Leonard, and P. M. Newman, “Navigation of unmanned marine vehicles in accordance with the rules of the road,” in
*Proceedings of the IEEE International Conference on Robotics and Automation (ICRA '06)*, pp. 3581–3587, Orlando, Fla, USA, May 2006. View at Publisher · View at Google Scholar - G. Casalino, A. Turetta, and E. Simetti, “Real time path planning and obstacle avoidance for security related usvs operating in harbour fields,” in
*Proceedings of the 8th International Conference on Computer Applications and Information Technology in the Maritime Industries (COMPIT '09)*, 2009. - G. Casalino, A. Turetta, and E. Simetti, “A three-layered architecture for real time path planning and obstacle avoidance for surveillance USVs operating in harbour fields,” in
*Proceedings of the IEEE Bremen: Balancing Technology with Future Needs (OCEANS '09)*, May 2009. View at Publisher · View at Google Scholar - Y. Li and K. M. Lo, “Dynamic and kinematics of novel underwater vehicle-manipulator for cleaning water pool,” in
*Proceedings of the IEEE International Conference on Mechatronics and Automation (ICMA '09)*, pp. 1196–1201, 2009. - L. Ulrich and J. Borenstien, “Vfh+: reliable obstacle avoidance for fast mobile robots,” in
*Proceedings of the IEEE International Conference on Robotics and Automation*, pp. 1572–1577, 1999. - N. Y. Ko and R. Simmons, “The lane-curvature method for local obstacle avoidance,” in
*Proceedings of the International Conference on Intelligence Robots and Systems*, pp. 1615–1621, 1998. - J. Minguez and L. Montano, “Nearest diagram navigation. a new realtime collision avoidance approach,” in
*Proceedings of the International Conference on Intelligence Robots and Systems*, pp. 2094–2100, 2000. - T. Fraichard, “A short paper about motion safety,” in
*Proceedings of the IEEE International Conference on Robotics and Automation (ICRA '07)*, pp. 1140–1145, April 2007. View at Publisher · View at Google Scholar - T. Fraichard, “Trajectory planning in a dynamic workspace: a state-time space approach,”
*Advanced Robotics*, vol. 13, no. 1, pp. 75–94, 1999. View at Google Scholar - D. Hsu, R. Kindel, J.-C. Latombe, and S. Rock, “Randomized kinodynamic motion planning with moving obstacles,”
*Algorithmics and Computational Robotics*, vol. 4, pp. 247–264, 2000. View at Google Scholar - O. Brock and O. Khatib, “Real-time replanning in high-dimensional configuration spaces using sets of homotopic paths,” in
*Proceedings of the IEEE International Conference on Robotics and Automation (ICRA '00)*, pp. 550–555, April 2000. - N. Simeon, J. Minguez, L. Montano, and R. Alami, “Global nearest diagram navigation,” in
*Proceedings of the IEEE International Conference on Robotics and Automation*, pp. 33–39, 2001. - E. Feron, M. A. Daleh, and E. Frazzoli, “Real time motion planning for agile autonomous vehicles,”
*AIAA Journal of Guidance Control and Dynamics*, vol. 25, pp. 116–129, 2002. View at Google Scholar - D. Fox, W. Burgard, and S. Thrun, “The dynamic window approach to collision avoidance,”
*IEEE Robotics and Automation Magazine*, vol. 4, no. 1, pp. 23–33, 1997. View at Google Scholar - T. S. Wikman, W. S. Newman, and M. S. Branicky, “Reflexive collision avoidance: a generalized approach,” in
*Proceedings of the IEEE International Conference on Robotics and Automation*, pp. 31–36, 1993. - S. M. LaValle and J. J. Kuffner, “Randomized kinodynamic planning,”
*International Journal of Robotics Research*, vol. 20, no. 5, pp. 378–400, 2001. View at Publisher · View at Google Scholar - S. Petti and T. Fraichard, “Safe motion planning in dynamic environment,” in
*Proceedings of the International Conference on Intelligence Robots and Systems*, pp. 885–897, 2005. - T. Fraichard and H. Asama, “Inevitable collision states-a step towards safer robots,”
*Advanced Robotics*, vol. 18, no. 10, pp. 1001–1024, 2004. View at Publisher · View at Google Scholar - N. Chan, M. Zucker, and J. Kuffner, “Improved motion planning speed and safety using region of inevitable collision,” in
*Proceedings of the 17th CISM-IFToMM Symposium on Robot Design, Dynamics, and Control (ROMANSY '08)*, pp. 103–114, July 2008. - B. Kluge and E. Prassler, “Recursive probabilistic velocity obstacles for reflective navigation,” in
*Proceedings of the FSR*, pp. 71–79, 2003. - P. Fiorini and Z. Shiller, “Motion planning in dynamic environments using velocity obstacles,”
*International Journal of Robotics Research*, vol. 17, no. 7, pp. 760–772, 1998. View at Google Scholar - Z. Shiller, F. Large, and S. Sekhavat, “Motion planning in dynamic environments: obstacles moving along arbitrary trajectories,” in
*Proceedings of the IEEE International Conference on Robotics and Automation (ICRA '01)*, pp. 3716–3721, May 2001. - O. Gal, Z. Shiller, and E. Rimon, “Efficient and safe on-line motion planning in dynamic environment,” in
*Proceedings of the IEEE International Conference on Robotics and Automation*, pp. 88–93, 2009. - Z. Shiller, O. Gal, and T. Fraichard, “The nonlinear velocity obstacle revisited: the optimal time horizon,” in
*Proceedings of the Workshop of Safety Motion Planning*, 2010. - C. Fulgenzi, A. Spalanzani, and C. Laugier, “Dynamic obstacle avoidance in uncertain environment combining pvos and occupancy grid,” in
*Proceedings of the IEEE International Conference on Robotics and Automation*, pp. 1610–1616, 2007. - T. I. Fossen,
*Marine Control Systems. Marine Cybernetics*, 2002. - E. Frazzoli, M. A. Dahleh, and E. Feron, “Maneuver-based motion planning for nonlinear systems with symmetries,”
*IEEE Transactions on Robotics*, vol. 21, no. 6, pp. 1077–1091, 2005. View at Publisher · View at Google Scholar - E. Frazzoli, “Explicit solutions for optimal maneuver-based motion planning,” in
*Proceedings of the 42nd IEEE Conference on Decision and Control*, pp. 3372–3377, December 2003. - S. Sundar and Z. Shiller, “Time optimal obstacle avoidance,” in
*Proceedings of the IEEE International Conference on Robotics and Automation*, pp. 3075–3080, 1995. - S. Sundar,
*Near-Time optimal feedback control of robotic manipulators*, Ph.D. thesis, University of California, Los Angeles, Calif, USA, 1995.