Unified Trajectory Planning Algorithms for Autonomous Underwater Vehicle Navigation

This paper presents two efficient methods for obstacle avoidance and path planning for Autonomous Underwater Vehicle (AUV). These methods take into account the dynamic constraints of the vehicle using advanced simulator of AUV considering low level control and stability effects. We present modified visibility graph local avoidance method and a spiral algorithm for obstacle avoidance. The algorithms were tested in challenged scenarios demonstrating safe trajectory planning.


Introduction
Path planning and obstacle avoidance are an important issues for Autonomous Underwater Vehicles (AUVs), and as can be noticed lately, these fields are extensively studied.
Currents disturbances can have a big influence in a different water depth and should be considered in underwater environments [1]. For the first time, Soulignac et al. [2] faced with strong current situations. Online replanning in 3D underwater environments with strong, dynamic, and uncertain currents was presented in [3].
AUV also suffers from a limited energy source, which can be minimized by planning optimal trajectories, extending the working time of the vehicle. Optimal path planning using A * algorithm search was presented by Carroll et al. [4]. Garau et al. [5]. Pêtrès et al. [6,7] used similar concepts using BF (breadth first) search named FM and FM * .
As the presented methods in this paper, other methods do not use grid-based search. The path is presented with a series of points, which are connected one by one. Path planning problem transformed to a constrained optimization problem in terms of the coordinates of these points, generating optimal paths considering AUV's dynamic constraints [1,[8][9][10].
One of the most known limitations in motion planning algorithms related to real-time computation ability. Planning methods based on local perceptions are computationally less expensive and thus time efficient. Bui and Kim [11] and Kanakakis et al. [12] apply fuzzy logic approaches to AUV path planning. Antonelli et al. [13,14] integrate virtual force field (VFF); all of these are not optimal planning paths methods.
This paper presents several different AUV path planning algorithms avoiding obstacle based on local perception abilities based on forward looking sonar. The introduced algorithms inherently take into account AUVs dynamic and kinematic constraints. AUV trajectory is simulated as described later. Simulations in typical underwater environments are presented, demonstrating algorithm's capabilities.

Vehicle Simulator
AUV platforms are known as underactuated vehicles models; these kinds of models generate dynamic constraints. In such cases, obstacle avoidance algorithms must consider the dynamic envelop of the vehicles not entering unstable states. We developed and implemented AUV simulator. The simulator takes into account the dynamic model of the AUV and the behavior of the sensors. The simulator uses only Forward Looking Sonars (FLS) for obstacle detection. In addition to FLS we have Side Scan Sonars (SSS) for detail scanning of the environment. Block diagram of simulator is presented in Figure 1.

Visibility Graph Algorithm
3.1.1. General Description. The main idea behind our algorithm is the well-known visibility graph method. However, visibility graph algorithm is an abstract one that cannot be used on-line in real time applications, so many changes and adjustments are needed.
Basically, visibility graph describes the concept of connecting nodes from start point to the goal; nodes are connected to each other if and only if there are no obstacles between them, and the next node is visible from the current node.
Another major issue of the algorithm logic is concerned with node's creation. One of the assumptions of the abstract visibility graph algorithm is that the obstacles are polygonal. On the real world we cannot assume that every obstacle is polygonal. Although, we can trap the obstacle in a polygonal shape but performances are likely to be very conservative. We propose a new concept constructing nodes around obstacles, enabling safety motion of the vehicle.
After constructing a full graph of nodes and arcs, search over the graph can be used for optimal trajectory, such as A * algorithm.
Algorithm completeness is based on the fact that the marine environment is quasi dynamic and a free visible node can be found at each time step, taking into account kinematic and dynamic models.

Algorithm Description.
Our algorithm can be divided into three steps: (1) building visibility graph; (2) connecting nodes; and (3) searching over the graph for the best solution.
(1) Building the graph's nodes: first, it should be noted that the visibility graph is a global algorithm and thus is not an efficient computation time one. Therefore, we try to minimize node's number to achieve fast calculation times.

Legal node
Unseen node Seen node that will be left out due to foreseen collision with the obstacle Figure 3: Risky nodes being reduced due to maneuver constraints.
We divide the nodes into two different cases.
(a) Vehicle nodes: we add nodes related to the vehicle.
For example, a node straight ahead of the vehicle (that represents straight path without turnings), a node to the right of the vehicle (representing a right turn), and a node to the left. The easiest way is to choose a radius that will allow turning to the node and take nodes on a circle around the start position with this chosen radius. That way we have nodes that represent all turning directions as well as slowing down if needed.
We assume that the obstacles are round by spheres generating smooth trajectories due to dynamic profile and AUVs turning radius. In case of an obstacle which is not round, we bound the obstacle with a circle, considering safety.  (b) Goal nodes: we add nodes that will help achieve the planned mission. For example, if the vehicle mission is to follow a line, adding nodes on that line will help the vehicle stay close to the original line, defining these nodes's cost as favor one when there are no obstacles.
(2) Connecting nodes with arcs: we connect the nodes using the simple idea of vision. If one node can be seen from the other node, these nodes are connected, as can be seen in Figure 3.
We add a simple measure considering the turning radius of the vehicle considering the maneuver constraints based on the turning radius of the vehicle. Otherwise, we do not connect this node.
(3) Search an optimal path: the last step is to search the optimal path over the graph from the current position of the vehicle to the goal. As mentioned above, we used an A * algorithm to search the shortest path in the visibility graph.
We define a cost function, taking into account and the shortest distance from the line connecting targets (the line connecting the first node and the last node at each iteration).
For the two nodes and m, we set the cost of the arc that connects them as Then we define the price for the node as: where is the start node, t is the target node, and the Cost Of Path function is the sum of the costs of the arc connecting this node to the previous node and the cost of the previous. Based on that, the shortest path between start to goal is calculated.

Algorithm Simulations.
In Figure 4, we present dense environment demonstrating visibility graph algorithm. Start point is located in the bottom left and goal point is located in the bottom right. It can be seen in (a) the nodes that were added during the time the vehicle was advancing its path.
Nodes are denoted as blue squares. A square, with an "X" inside it, is a node that was passed by the vehicle. In (b) and (c), we can see a close up of two types of nodes. In (c) an obstacle node and in (b) a vehicle related node are presented.

General Description.
The spiral algorithm presented for the first time in this paper, as its name may suggest, prefers to stay away from the vehicle's predetermined path and locally avoids an obstacle when there is an imminent danger of a collision. In such a case, circumnavigate of an obstacle is done by adding and placing new waypoints in the vehicle's path, in a semicircle around the center of the original, and in such a radius the vehicle can pass safely by the obstacle. For simplicity, we assume all obstacles are spheres. In the next section, a detailed algorithm stages are described.

Algorithm Stages.
In this section, we introduce a more detailed description of the algorithm. We distinguish between two basic situations and the relevant action in a case of an obstacle that may cause collision.
intersect point 1/2 = vessel loc + sol 1/2 × (vessel path ) (5) CheckIntersect variable returns a true value if an intersection occurs within the finite line segment and false otherwise, where obstacle is the obstacle's center coordinates and radius and vessel path is the line segment starting in the vehicle's location, pointing to vehicle's heading. We limit vessel path 's length to be the distance given by min (50, vessel loc − next target loc ) . Obstacles detection ability is related to the vehicle perception abilities, and only local avoidance can be done.
(ii) In the next stage, we calculate Dist (obstacle loc , vessel loc ) − obs radius < safe marge , where Dist(obstacle loc , vessel loc ) is the Euclidean distance between the obstacle and the vehicle, obs radius is the obstacle's radius, and safe marge is the minimum distance at which we would like to consider the obstacle as a dangerous one.
(iii) Finally, we calculate CheckIntersect (obstacle, vessel to target path) function value, where vessel to target path is the line segment connecting the vehicle to the next waypoint as can be seen in Figure 5. The rational is not to consider obstacles which are not cause collision, in a case of vehicle's turns.
(C) Avoiding an obstacle: in a case of a collision course between the obstacle and the vehicle, we add number of waypoints around the obstacle in a spiral trajectory and locally avoid the obstacle, in the following way.
(i) First, we determine whether the center of the obstacle is to the left or the right of the vehicle's heading. We choose to pass the obstacle on the side that minimizes the distance between the updated and the original path.
(ii) Next, a trigonometric calculation is done, to determine how wide is the "bite" taken out of the vehicle's path by the obstacle. This is done in terms of the angle alpha as can be seen in Figure 6.
Alpha is given by

ISRN Robotics
If 135 < alpha ≤ 180, five waypoints are added on the boundary of the sphere centered located at the obstacle's center, with the augmented radius mentioned above. The waypoints are added so that the difference in degree between them is equal, as can be seen in Figure 7.
If 90 < alpha ≤ 135, 4 waypoints are added in the same fashion, and if alpha ≤ 90, 3 are added.

Algorithm Simulations
. The algorithm has been tested using a simulation of a real vehicle simulator as described above. The scenarios chosen were 1 × 1 km squares, in which were scattered 50 to 70 obstacles of radii ≃ 15 meters. The scenarios are modeled after underwater minefields. The algorithm has shown good results so far, both in obstacle avoidance and in adhering to a course. Simulations can be seen in Figure 8.

Conclusions
In this research we presented two different methods for AUV, based on local and global planning methods. One of the major challenges in AUV obstacle avoidance is related to the underactuated model of the vehicle. We challenge these constraints by simulating vehicle trajectory using advanced simulator, modeling the perception abilities of the forward looking sonar, and the obstacles detection in realtime.
We presented a general description for each one of the algorithm and a simulation results with a typical search fields of AUVs.
Further research includes sea test with REMUS 100 AUV, testing the presented algorithms.