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

Authors: Oren Gal
Journal: Journal of Robotics
 (2011)

---

## 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.


---

## Body

## 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 occupationP(Occ);(ii)
a probabilistic distribution function on a histogram of possible velocitiesP(vn), n=1,…,N.The predicted occupation of each cell is:(1)Pc(Occ)=∑nPc(n)(Occ)⋅Pc(n)(vn),
where c represents the cell, Pc(n)(Occ) denotes the probability of occupancy of the cell c, Pc(n)(vn) denotes the probability that the cell’s velocity is vn. We define Tbreak as time to collision between the robot and the obstacle, and Tsafe as stopping time including delay time of the robot to change current velocity, vr,
(2)Tsafe(v)=ε+Tbreak(v),
where ε is the time delay of the robot dynamics. Estimating time to collision Tbreak(v) value is related to the highest probability to collision, Psafe, that we can deal with during robot’s motion. Considering a robot velocity vr from present time to some specific future time t leads to a collision, if there is a collision in the interval [0,t-1], denote as Po⋯t-1(vr), or if there is a collision at time instant t, Pt(vr). A cumulative probability of collision from time 0 to time step t is recursively computed, Po⋯t:(3)Po⋯t(vr)=Po⋯t-1(vr)+(1-Po⋯t-1(vr))⋅Pt(vr),
where
(4)Pcoll,o=0,
where Pcoll represents the probability to collision, Time to collision Tcoll is the minimum between t and Tpred, which represents the time period of a constant motion models reliability:
(5)Tcoll(vr)=min⁡(Tpred,t∣Pcoll,o⋯t(vr)>Psafe).
If Tcoll(v)<Tsafe(v), 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 isH1/3 (also known as Hs, the significant wave height). This variable represents the average of the third highest waves measured for a long period of time. H1/3 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 H1/3. After many measurements and observations (which can be done off-line), a statistical connection between H1/3 and height of a single wave with A1 amplitude can be determined. From empirical model [26], the well-accepted assumption of wave’s probability with amplitude A>A1 is
(6)P(A>A1)=e-8A12/H1/32,
where A1 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, Pcoll,t, and decide whether it is safe to ignore the waves (which means that a special maneuver is needed to avoid the virtual obstacle).Figure 1
Waves height over time.


## 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 isH1/3 (also known as Hs, the significant wave height). This variable represents the average of the third highest waves measured for a long period of time. H1/3 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 H1/3. After many measurements and observations (which can be done off-line), a statistical connection between H1/3 and height of a single wave with A1 amplitude can be determined. From empirical model [26], the well-accepted assumption of wave’s probability with amplitude A>A1 is
(6)P(A>A1)=e-8A12/H1/32,
where A1 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, Pcoll,t, and decide whether it is safe to ignore the waves (which means that a special maneuver is needed to avoid the virtual obstacle).Figure 1
Waves height over time.


## 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 Figure2, 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
(7)Pwavei,j=e-8A12/H1/32⋅cos⁡(ωx+ϕi,j),
where ω is the wave’s frequency and ϕi,j is the phase at cell c[i,j]. The phase at each cell c[i,j] is
(8)ϕi,j=2πxλ,
where x is the distance to the next wave peak and λ is the wave length.Figure 2
Wave simulation and occupancy grid.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 Figure2, 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, 1/w.


### 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 defineTsafe(vr) as the time it takes to change USV’s current course to a vertical course regarding wave’s direction. We define Tdanger(vr) as the first time when Pcoll>Psafe so the USV enters unsafe state:
(9)Tdanger(vr)=t,
where
(10)Pcoll,o⋯t(vr)>Psafe.
In a case of Tdanger<Tsafe, USV cannot change his current course moving vertically to the wave’s direction, therefore the velocity vr 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.


## 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 Figure2, 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
(7)Pwavei,j=e-8A12/H1/32⋅cos⁡(ωx+ϕi,j),
where ω is the wave’s frequency and ϕi,j is the phase at cell c[i,j]. The phase at each cell c[i,j] is
(8)ϕi,j=2πxλ,
where x is the distance to the next wave peak and λ is the wave length.Figure 2
Wave simulation and occupancy grid.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 Figure2, 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, 1/w.


## 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 defineTsafe(vr) as the time it takes to change USV’s current course to a vertical course regarding wave’s direction. We define Tdanger(vr) as the first time when Pcoll>Psafe so the USV enters unsafe state:
(9)Tdanger(vr)=t,
where
(10)Pcoll,o⋯t(vr)>Psafe.
In a case of Tdanger<Tsafe, USV cannot change his current course moving vertically to the wave’s direction, therefore the velocity vr 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 valueH1/3 that can be measured online using drift actuators or offline before mission, which enable to compute Pwave for each cell (i,j). 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:
(11)η̇=J(ψ)ν,Mν̇+C(ν)ν+D(ν)ν=τ,
where M is system inertia matrix, C-Coriolis-centripetal matrix, D-damping matrix, η=(x,y,ψ)T∈R3 represents the USV’s position and orientation in NED system (North-East-Down coordinate system, x-axis points toward true north point, etc. [26]), ν=(u,v,r)T∈R3 is the velocity vector, τ=(u,r)T∈R2 represents the USV’s controls, where u is the propulsion force along surge DOF and r represents the propulsion moment along yaw DOF. J(ψ) 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:r≠0:
(12)x(t+Δt)=x(t)+(usin⁡(rΔt)+v(cos⁡(rΔt)-1))r,y(t+Δt)=y(t)+(u(1-cos⁡(rΔt))+v(sin⁡(rΔt)))r,Δψ=rt,r=0:
(13)x(t+Δt)=x(t)+uΔt,y(t+Δt)=y(t)+vΔt,Δψ=0.


### 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 3×3 grid points in (u,r) as set controllers from U and R, respectively, denoted as τmp(i):
(14)(U,R)(U,0)(U,-R)(0,R)(0,0)(0,-R)(-U,R)(-U,0)(-U,-R).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, w(τr,i(t+Δt)), for the updated USV controller τr,i(t+Δt):
(15)τr,i(t+Δt)=τr,i(t)+τmp(i),
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 goalw(x,ẋ,xf,ẋf) from the current state (x,ẋ), where (x,ẋ)=(u,r) to the target point (xf,ẋf) for each axis [29, 30]:
(16)w(x,ẋ,xf,ẋf)={-ẋ-ẋf+2-x+xf+ẋ22+ẋf22,ifx∈S,ẋ+ẋf+2x-xf+ẋ22+ẋf22,otherwise,
where S is the region below and above the switching curve in the state space (analytic solution from Bang-Bang optimal control problem):
(17)S(x,ẋ)={ẋ2-2(x-xf+ẋf22)>0,ẋ2+2(x-xf-ẋf22)<0(x-xf+ẋf22)},
where x and ẋ are the USV’s controller τr,i(t+Δt) from U and R respectively, and xf and ẋf are the target point, Xt, 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 Algorithm1.Algorithm 1:Planner pseudocode.
Set Target PointXtMeasure USV’s locationXrMeasure USV’s velocityvrSetTsafe, Psafefort=0 to tfinal  doMeasureH1/3if  Xr=Xt  thenGet a New WaypointXtelsefor  i=1 to n  dofor  j=1 to n  doCalculatePwavei,jend forend forfor  each node i=1⋯n  doCalculatePcoll,tif  Pcoll,t<Psafe  thenτr,i(t+Δt)=τr,i(t)+τmp(i)elseτr,i(t+Δt)=τr,i(t)+τmp(max(Tdanger))end ifCalculatewi(τr,i(t+Δt))end forFindMinwi(τr,i(t+Δt))i=1⋯nSetτr,imin(t+Δt)UpdateXrend ifend for


### 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.


## 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:
(11)η̇=J(ψ)ν,Mν̇+C(ν)ν+D(ν)ν=τ,
where M is system inertia matrix, C-Coriolis-centripetal matrix, D-damping matrix, η=(x,y,ψ)T∈R3 represents the USV’s position and orientation in NED system (North-East-Down coordinate system, x-axis points toward true north point, etc. [26]), ν=(u,v,r)T∈R3 is the velocity vector, τ=(u,r)T∈R2 represents the USV’s controls, where u is the propulsion force along surge DOF and r represents the propulsion moment along yaw DOF. J(ψ) 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:r≠0:
(12)x(t+Δt)=x(t)+(usin⁡(rΔt)+v(cos⁡(rΔt)-1))r,y(t+Δt)=y(t)+(u(1-cos⁡(rΔt))+v(sin⁡(rΔt)))r,Δψ=rt,r=0:
(13)x(t+Δt)=x(t)+uΔt,y(t+Δt)=y(t)+vΔt,Δψ=0.


## 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 3×3 grid points in (u,r) as set controllers from U and R, respectively, denoted as τmp(i):
(14)(U,R)(U,0)(U,-R)(0,R)(0,0)(0,-R)(-U,R)(-U,0)(-U,-R).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, w(τr,i(t+Δt)), for the updated USV controller τr,i(t+Δt):
(15)τr,i(t+Δt)=τr,i(t)+τmp(i),
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 goalw(x,ẋ,xf,ẋf) from the current state (x,ẋ), where (x,ẋ)=(u,r) to the target point (xf,ẋf) for each axis [29, 30]:
(16)w(x,ẋ,xf,ẋf)={-ẋ-ẋf+2-x+xf+ẋ22+ẋf22,ifx∈S,ẋ+ẋf+2x-xf+ẋ22+ẋf22,otherwise,
where S is the region below and above the switching curve in the state space (analytic solution from Bang-Bang optimal control problem):
(17)S(x,ẋ)={ẋ2-2(x-xf+ẋf22)>0,ẋ2+2(x-xf-ẋf22)<0(x-xf+ẋf22)},
where x and ẋ are the USV’s controller τr,i(t+Δt) from U and R respectively, and xf and ẋf are the target point, Xt, 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 Algorithm1.Algorithm 1:Planner pseudocode.
Set Target PointXtMeasure USV’s locationXrMeasure USV’s velocityvrSetTsafe, Psafefort=0 to tfinal  doMeasureH1/3if  Xr=Xt  thenGet a New WaypointXtelsefor  i=1 to n  dofor  j=1 to n  doCalculatePwavei,jend forend forfor  each node i=1⋯n  doCalculatePcoll,tif  Pcoll,t<Psafe  thenτr,i(t+Δt)=τr,i(t)+τmp(i)elseτr,i(t+Δt)=τr,i(t)+τmp(max(Tdanger))end ifCalculatewi(τr,i(t+Δt))end forFindMinwi(τr,i(t+Δt))i=1⋯nSetτr,imin(t+Δt)UpdateXrend ifend for


## 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 ofH1/3, and with several values of Psafe. Figure 3 shows the complete simulation environment: USV’s current position Xr represented by a blue point, target point Xt 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: A1=3 [m],  Psafe=0.5±0.1, Tsafe=5 [sec], Δt=0.1 [sec], H1/3=3+rand(0,1) [m], w=0.06 [Hz]. Figure 4 shows planner simulation in crowded dynamic environment with Psafe=0.6. 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 t: Pcoll,t<Psafe. In Figure 5 the USV deals with the same scenario but Psafe was set to lower value (0.4), the vehicle react earlier to the obstacle ahead thats because in a specific time Pcoll,t>Psafe, so USV chose conservative trajectory to avoid collision. In Figure 6 global running time algorithms versus Δt 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.Figure 3
Planner simulation environment.Simulation in crowded dynamic environmentPsafe=0.6.
(a)(b)(c)Simulation in crowded dynamic environmentPsafe=0.4.
(a)(b)Figure 6
Running time versusΔt.


## 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.



---

*Source: 703959-2011-12-19.xml*