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.
(a)
(b)
(c)
(a)
(b)
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.