Cognition-based hybrid path planning for autonomous underwater vehicle target following

Obstacles and target modelling

The digital scanning sonar is the device that can not only detect the target and the obstacle but also measure their distances from the vehicle. The range of scanning sonar is 20° with the radius as R. The representation of sonar detection space is important for environmental modelling and target tracking. In the sonar detection space, an occupancy grid map of environment has been constructed. Each cell of the map in the discrete region is characterized through two states: empty and occupied. The state is obtained through the projection of sonar profile. From the data flow of projection profile, one can obtain whether the cell is occupied or empty. The sonar can be applied to model and find uncertain static and dynamic object with Gaussian distribution. The operation profile of scanning sonar is shown in Figure 3.

OPEN IN VIEWER

Figure 3.

The operation range of the digital scanning sonar (the space of sonar includes seven radial directions, that is, left (l), front small left (Sfl), front left (fl), front (f), front right (fr), front large right (fr), right).

Through Demster–Shafer theory, the goal of the occupancy grids is to determine the cell possibilities of empty and occupied. If we define O as the states of occupation by obstacles, and E as the empty states, one can obtain the set of discernment Θ as

Θ ={ O , E }

1

Each grid in the workspace is defined by the cell state U ( i , j ) , which is used to describe the assignment of basic probability m i , j to each label in Ω

Ω = { ϕ , E , O , { E , O } } = 2 Θ

2

Each cell U ( i , j ) in the grid is defined as

m i , j ( ϕ ) + m i , j ( E ) + m i , j ( O ) + m i , j ( { E , O } ) = 1

3

with every cell in this map is initialized as

m i , j ( O ) = m i , j ( E ) = 0 and m i , j ( { E , O } ) = 1

4

In the path planning process of potential field, the attractive forces come from the goal and the observing targets, while the repulsive forces come from the obstacles. On the basis of sonar profile, the repulsive force between the vehicle and the obstacles can be obtained to prevent collisions through the sum of gradients of the potential field

f rep = { k rep ∑ i = 1 m ( 1 ‖ P a − P o i ‖ − 1 d max ) P a − P o i ‖ P a − P o i ‖ ​ if ‖ P a − P o i ‖ < d max 0 ​ ​ ​ otherwise

5

where m is the number of ambient obstacles, k rep is the positive constant of the repulsive forces and d max is the obstacle influence distance. From equation (5), the repulsive forces are the sum of several subrepulsive ones. From the obstacles modelling through scanning sonar, three components of obstacle information are expressed with obstacle position ( P o i = ( x o i , y o i , z o i ) ). If the current position of AUV is set as P a = ( x a , y a , z a ) , the distance and angular relationship between the AUV and the obstacle can be described as

{ | d A − O b | = ( x − x o p ) 2 + ( y − y o p ) 2 + ( z − z o p ) 2 ψ A − O b = a tan y − y o p x − x o p θ A − O b = a tan z − z o p ( x − x o p ) 2 + ( y − y o p ) 2

6

In the path planning process, the obstacles must be avoided once they are found, while the targets should be observed and detected with the existence possibilities. According to the theory of potential field, the attractive forces come from the goal and the observing targets. On the other hand, the position of observed target can be described with P t = ( x t , y t , z t ) , target radius rate ( Θ t r ) and target uncertain rate ( Θ U r ). The target uncertainty is modelled with uncertain radius which is varied with normal distribution Θ t r ∼ ( P t , σ ) . If M ₁, M ₂ and M ₃ are used to define the uncertainty difference on the object motion, the relationship between target change rate from t − 1 to t can be described through normal distribution and probability density functions of radius rate as follows

Θ t r ( t ) = M 1 Θ t r ( t − 1 ) + M 2 X ( t − 1 ) + M 3

7

where X ( t − 1 ) = N ( 0 , σ ) is the normal distribution for the target, M 2 = [ 0 1 1 ] T , M 3 = [ 0 0 Θ U r ] T and

M 1 = [ 1 Θ U r ( t ) 0 0 1 0 0 0 1 ]

8

In the local planning, a novel multilayered potential field approach has been proposed for the target detection. The attractive force includes the goal and the observed target

f att = { k att ( λ ( sgn ( P g − P a ) ‖ P g − P a ‖ 2 ) + ( 1 − λ ) Θ U r ( t ) ( sgn ( P t − P a ) ‖ P t − P a ‖ 2 ) ) if ‖ P g − P a ‖ > γ g and ‖ P t − P a ‖ > γ t k att ( λ ( sgn ( P g − P a ) ‖ P g − P a ‖ ) + ( 1 − λ ) Θ U r ( t ) ( sgn ( P t − P a ) ‖ P t − P a ‖ 2 ) ) if ‖ P g − P a ‖ ≤ γ g and ‖ P t − P a ‖ > γ t k att ( λ ( sgn ( P g − P a ) ‖ P g − P a ‖ ) + ( 1 − λ ) Θ U r ( t ) ( sgn ( P t − P a ) ‖ P t − P a ‖ ) ) if ‖ P g − P a ‖ ≤ γ g and ‖ P t − P a ‖ ≤ γ t k att ( λ ( sgn ( P g − P a ) ‖ P g − P a ‖ 2 ) + ( 1 − λ ) Θ U r ( t ) ( sgn ( P t − P a ) ‖ P t − P a ‖ ) ) if ‖ P g − P a ‖ > γ g and ‖ P t − P a ‖ ≤ γ t 2

9

where k att is the positive constant of the attractive forces, P g = ( x g , y g , z g ) represents the goal position, P t = ( x t , y t , z t ) represents the target position, λ represents the coefficient of the attractive forces between the target and the goal and γ g and γ t represent the distance limit for the layer of attractive force.

Therefore, the artificial force can be obtained as

f = f rep + f att

10

From equations (5) to (10), the local obstacle and the target can be remodelled and the path can be planned through Figure 3.

Fuzzy logic descriptions

For the AUV cruising in the complicated environment, it should not only detect the target and avoid obstacles but also overcome oceanic current and uncertain disturbance. The fuzzy logic path planning method includes the following steps:

1. Fuzzification: all the fuzzy logic inputs, including AUV current positions P a = ( x a , y a , z a ) , detected the ith obstacle positions P o i = ( x o i , y o i , z o i ) , target positions P t = ( x t , y t , z t ) , AUV current velocity, heading angle, relative position between the AUV and the target and relative position between AUV and obstacles, are all interpreted into fuzzy linguistic values. And the fuzzy membership functions vary with the fuzzy inputs. The fuzzy outputs are the velocity and expect angle of the AUV.

The initial fuzzy rules are the combinations of these corresponding forces and reactions. The number of fuzzy rules n_r should be correspondent with the sum and combinations of environmental conditions such as obstacles, targets, current and disturbances. The number of the fuzzy rules is related to the number of the input variables and the logical relationships between them. Although the former is easy to be get, the latter is hard to be established. In this article, the interplay between those AUV state variables and the outside information such as velocity, angle and position will aggravate the difficulty of the establishment of the fuzzy rules. The sum and combinations of the environmental conditions refer to the internal relationship of the fuzzy inputs.

The n_r fuzzy rules are designed by using Takagi–Sugeno fuzzy model of ‘IF-THEN’ rules as follows.

The ith rule R_p :

IF ‖ P a − P o i ‖ = d p i , ‖ P g − P a ‖ = d t i , the AUV speed is v a and the current speed is v c , THEN

y p = f rep p + f att p

11

where f rep p = { k rep ∑ i = 1 m ( ξ ‖ v a − v c ‖ ‖ P a − P o i ‖ − 1 d max ) P a − P o i ‖ P a − P o i ‖ ​ i f ‖ P a − P o i ‖ < d max 0 ​ ​ ​ otherwise

f att = { k att ‖ v a − v c ‖ ( λ ( sgn ( P g − P a ) ‖ P g − P a ‖ 2 ) + ( 1 − λ ) Θ U r ( t ) ( sgn ( P t − P a ) ‖ P t − P a ‖ 2 ) ) if ‖ P g − P a ‖ > γ g and ‖ P t − P a ‖ > γ t k att ‖ v a − v c ‖ ( λ ( sgn ( P g − P a ) ‖ P g − P a ‖ ) + ( 1 − λ ) Θ U r ( t ) ( sgn ( P t − P a ) ‖ P t − P a ‖ 2 ) ) if ‖ P g − P a ‖ ≤ γ g and ‖ P t − P a ‖ > γ t k att ‖ v a − v c ‖ ( λ ( sgn ( P g − P a ) ‖ P g − P a ‖ ) + ( 1 − λ ) Θ U r ( t ) ( sgn ( P t − P a ) ‖ P t − P a ‖ ) ) if ‖ P g − P a ‖ ≤ γ g and ‖ P t − P a ‖ ≤ γ t k att ‖ v a − v c ‖ ( λ ( sgn ( P g − P a ) ‖ P g − P a ‖ 2 ) + ( 1 − λ ) Θ U r ( t ) ( sgn ( P t − P a ) ‖ P t − P a ‖ ) ) if ‖ P g − P a ‖ > γ g and ‖ P t − P a ‖ ≤ γ t 2

12

d max is the influence distance of the obstacles and ξ is the speed coefficient, and p = 1, 2,…n_p , represent the number of rules.

3. Inference mechanism: in this process, the reasoning process includes optimized fuzzy inference rules.

y = ∑ p = 1 n p w p y p ∑ p = 1 n p w p

13

where w p is the weight value.

Adaptive rules generation and optimization algorithm

If the fuzzy rules can complete all the possible conditions and fuzzy inputs, one should take the considerations with n r × m n r rules and conditions, where m is the input numbers and n r represents the number of fuzzy rules. Since these rules involve the states of various conditions of target positions, obstacle positions, AUV state and current disturbance state, it is difficult to enumerate all the appropriate linguistic fuzzy rules beforehand to cover all the possible situations. The following algorithm will adaptively fuse the ACO and PSO algorithms in order to optimize designed fuzzy rules. The following will introduce the steps of the rule generation and optimization method in Figure 4.

OPEN IN VIEWER

Figure 4.

The block diagram of adaptive PSO and ant rule optimization algorithm. PSO: particle swarm optimization.

The first step is to code each rule into a particle or an ant, before the application of ACO and PSO. For the ith input variable and the jth rules solution, the coded result can be obtained as the rules solution individually represented in the first row of Figure 4. For PSO, the rule particles can move and expand quickly in the search space but without global communication. Each rule particle has a position and velocity vector. Each rule particle updates its position and velocity through every learning cycle.

While in continuous ACO, ant rules are randomly distributed among particles. Not only the information can be exchanged among the ant individuals but the ACO Gaussian sampling can push individuals to the end of search from local optimum.

In the beginning of this algorithm, if we set K N as the total rules, N_aN as the solutions of ant rules individuals, one obtains K N − N a N as the particle rules individuals. At the end of this algorithm, K end = K N − N a N represents the increase number of ant rules individuals and N_aN represents the decrease number of particle rules individuals. If I max is set as the maximum iteration number, I N is the iteration number, one can obtain the number of ant individuals from

K I c = int ( ( K end − N a N ) I N I max ) + N a N

14

In the second step, the particle rules individuals are generated and expanded to the search space; the generated particle individual number is K N − K I c with the iteration as I N + 1 . Each particle swarm includes many particles which is used to define and assign all particles initialized positions. For the ith particle, the position and velocity vectors of the ith particle are set as p p and v p , p i is the best solution for a certain particle and p g is the global best solution for the particle individuals. Thus, the ant solutions can guide the particles to better particle solution. The particle velocity can be obtained as

v i p ( I N + 1 ) = ς ( v i p ( I N ) + k 1 p ψ 1 ⊗ ( p i ( I N ) − p p ( I N ) ) + k 2 p ψ 2 ⊗ ( p g ( I N ) − p p ( I N ) )

15

where ς is the constrict coefficient, k 1 p and k 2 p are positive parameters for particle acceleration and ψ i is a random vector with the entries as the random distribution in [0,1]. ς = 0.7 , v 1 p = v 2 p = 1 , with each particle changes according to

p p ( I N + 1 ) = p p ( I N ) + v i p ( I N + 1 ) , i = 1 , ... , K N − K I c

16

where K N − K I c + 1 is retained as best performance for the next iteration.

From this approach diagram in Figure 4, the generated fuzzy rules solutions s 1 , ... , s N are sorted from best to the worst according to their performance. In the graph, every row represents the fuzzy rules solution obtained from particle or ant. The route from node to level can be searched by ant. Each route is correspondent to a classification rule, while each node represents the solution vector. The nodes connected through path segment corresponding to the pheromone level τ i and they are selected with high probabilities

τ i ( t + 1 ) = ρ τ i ( t ) + Δ τ i ( t )

17

where ρ is a parameter between 0 and 1, and Δ τ i ( t ) is selected from the quality of solution performance. The stronger the pheromone level τ i , the better the solution is. The order of pheromone levels is sorted from the best to the worst.

The third step is ACO algorithm, and the ant generation process includes three stages:

1. Ant path selection: the new fuzzy rules solution is generated through ant path which is generated through the elite tournament technique. At the I N + 1 iteration, the total K I c solutions are generated in the ant path selection process. The elite selection approach selects L I c best solutions for temporary solutions. Among the ant path segments, the node value with highest pheromone level is selected. The value of L I c in the elite selection varies with iteration

L I c = int ( K I c I N I max )

18

The fuzzy rules solutions are generated through elite selection with the increase of global search. In the selection process, the node can be selected from fuzzy rules particle or fuzzy rules ant.

2. In the Gaussian sampling process, a new value near the temporary solution via Gaussian sampling which means the standard deviation is computed as follows

d i j = ζ ∑ l = 1 N | s ∼ i j − s l j | N − 1

19

where s ∼ i j is the mean of probability density function and ζ is the constant value. The Gaussian sampling operation process introduces new parameter values to every active rule in the temporary solutions through the mean of probability density function s ∼ i j . The standard deviation will dynamically change according to iteration number N.

3. In the solution refinement process, through the consideration with the attraction of ants to the optimal individual in the overall population, the solution can be obtained as

s ∼ i = s ^ i + ψ 3 ⊗ ( s ^ 1 − s ^ i )

20

where ψ 3 is random vector with uniform distribution random number in the interval [0,1]. The random vector is used for the promotion towards the best solution.

In order to realize rules solution iteration and optimization, reinforcement learning is applied to execute and evaluate the AUV path planning actions. The Q-function can realize mapping from rules-action pairs to predict return. ²⁶ The Q-function-based learning will speed up the convergence speed and improve system performance with disturbance. The output of the Q-function can be updated as follows

Q ( R ( t ) , a k ( t ) ) = Q ( R ( t ) , a k ( t ) ) + α [ R ( t + 1 ) + γ Q ∗ ( R ( t + 1 ) ) − Q ( R ( t ) , a k ( t ) ) ]

21

where R ( t ) represent current state of rules, r ( t + 1 ) is the reinforcement reward for the rules, Q ∗ ( R ( t + 1 ) ) is the optimal estimation for the possible actions and γ is discount factor. At each step, the expected Q values are updated through

Q ( R + 1 ) ) = 1 2 ∑ j = 1 n r ( f _ j ( R ( t ) ) ∑ k = 1 n r f _ k ( R ( t ) ) + f ¯ j ( R ( t ) ) ∑ k = 1 n r f ¯ k ( R ( t ) ) ) q j

22

According to equation (22), the Q value can be updated as follows:

Δ Q = r ( t + 1 ) + γ Q ∗ ( R ( t + 1 ) ) − Q ( R ( t ) , a ( t ) )

23

The best Q values correspond with the quality of the rules actions above. For each rule, there will be a greedy action to achieve maximum Q values. For each rules solution and path planning action, the maximum Q value can be obtained from the estimation as

Q * ( R ( t + 1 ) ) = 1 2 ∑ j = 1 n R ( f _ i ( R ( t ) ) ∑ k = 1 n R f _ k ( R ( t ) ) + f ¯ i ( R ( t ) ) ∑ k = 1 n R f ¯ k ( R ( t ) ) ) q i *

24

where q j ( t + 1 ) = q j ( t ) + ε Δ q j ( t ) ​ j = 1 , ... , n R and ε is the learning rate.

Δ q j ( t ) = 1 2 Δ Q ( f _ j ( R ( t ) ) ∑ k = 1 n R f _ k ( R ( t ) ) + f ¯ j ( R ( t ) ) ∑ k = 1 n R f ¯ k ( R ( t ) ) )

25

Case 1: Canal following for the wall detection

For the inland canal, the wall following is very important for the wall detection. Although AUV can carry out delicate detection on underwater target, it should cruise close to the wall of complicated contour under disturbances. Therefore, the motion plan should be made according to the detected wall contour and current distance between canal walls. In order to follow the canal wall, distant measurement sonars have been equipped on the AUV. In order to make comparisons, three kinds of motion planning algorithms including the common fuzzy rules-based motion plan of 3.1 and 3.2, PSO optimized fuzzy rules-based motion plan and adaptive ACO-PSO optimized fuzzy rules-based motion plan are used.

In Figure 5(a), the start point is (0, 0) and the end point is (−120, 1000). The expected distance between the AUV and the canal wall is 4 m. The adaptive ACO-PSO fuzzy planner can obtain the most reasonable trajectory for the canal detection. Since all the planned trajectories are obtained according to the AUV real-time state such as headings, speed, obstacle positions and disturbance, the planned trajectories are complicated curves. The motion control of AUV was realized according to the results of the adaptive ACO-PSO fuzzy planner. From Figure 5, the fuzzy planned motion cannot adapt to the environmental disturbance and the dynamic environment, the fixed rules usually lead to late and inflexible responses, thus the more complicated conditions for the disturbance and the turning of the wall the greater will be the errors. The PSO algorithm with fuzzy rules optimization embodies its advantage in the rule changing according to the real state, but the algorithm cannot converge very quickly at some local regions which cause the planning errors great at some time and even joggle at the place of complicated disturbance and turnings. While the adaptive ACO-PSO fuzzy planner is more adaptable for bend or angular intensively changing canals, because the ACO and reinforcement learning can help the algorithms with regional rules generation and quick convergence respectively.

OPEN IN VIEWER

Figure 5.

Comparisons on the motion plan of canal following. (a) Comparisons on the motion plan of wall following of the canal. (b) Heading plan comparisons of motion plan. (c) Distance of AUV from the canal wall. AUV: autonomous underwater vehicle.

Case 2: Moving target following plan

Moving target following is very important but difficult for AUV advanced detection. The AUV should keep correspondent distance with the same speed and heading angle under real-time disturbance. The target following experiments for motion plan have been made in the Penglai offshore of Chinese Shandong province. The oceanic environmental disturbances were obtained from DVL in Figure 6. In the experiments, the planned trajectories from adaptive ACO-PSO fuzzy planner are obtained according to the AUV real-time state such as headings, speed, target speed and positions and disturbance. The motion control of AUV was realized according to the results of the real-time adaptive ACO-PSO fuzzy planning. The ideal distance between the AUV and the mobile target for the target following is 8.5 m.

OPEN IN VIEWER

Figure 6.

Environmental disturbance obtained from DVL. DVL: Doppler Velocity Log.

From Figure 7, the AUV can realize mobile target following through motion plan of ACO-PSO fuzzy planning algorithm. The distance between the AUV and the mobile target is from 7.5 m to 10 m.

OPEN IN VIEWER

Figure 7.

Motion plan of target following. (a) Following trajectories. (b) Heading plan for target following. (c) Speed plan for target following. (d) Distance from the target during following.