3D smooth path planning of AUV based on improved ant colony optimization considering heading switching pressure

A smooth and secure spatial path planning algorithm that integrates the improved ant colony optimization with the corrective connected spatial search strategy is proposed, aiming at heavy heading switching pressure of autonomous underwater vehicles sailing in complex marine environment. On the one hand, to overcome the low-dimensional search domain and inaccurate spatial communication information in traditional spatial path planning, the spatial connectivity adjacency domain search strategy is designed based on grid environment model. On the other hand, to alleviate heading switching pressure due to large path steering angles and redundant path turning points, the heuristic functions and pheromone update criterion based on ant colony optimization are introduced to improve the solution quality of smooth paths. The simulation results show that the space search strategy can improve the success probability of safe path planning without reducing the scope of explorable free space. Additionally, the simulations demonstrate that the improved ant colony optimization using the spatial search strategy can guarantee the shortest path with lowest tortuous degree and fewest turning times in the same grid environment.

Environmental model.Applying the grid method to environment modeling, the principle is to discretize the whole working environment into a non-overlapping adjacent grid set domain by grids with appropriate granularity.The environmental information of each grid contains the positioning information and traffic state of the corresponding actual space 29 .Focusing on the dimensionality of the AUV operating environment, using a unify-size cubic gird body to discrete environment space.The new grid granularity partitioning principle: the AUV solid structure is upgraded to a cube, and the longest edge length of the cube l is set to be the size of the grid body l*l*l, the centroid coordinates of a grid represent its spatial location.Setting the activity value of obstacle grid as Inf and the activity value of free grid as 0. The environment model formed by discretizing the 3D environment space B*E*F is the spatial grid set domain b*e*f.B, E and F are the length, width and height parameters of the 3D environment space, respectively.b, e and f are the row, column and layer parameters of the spatial grid set domain, respectively.ceil in formula (1) is the integer up operation.www.nature.com/scientificreports/ The representation of the environment model spatially using the grid method is shown in figure 1.
Summarizing the distributed condition of the obstacles constitutes the grid map's the environment information matrix, which holds the traffic situation of the location.Let the grid's serial number equal to the index of the environment information matrix, facilitating the computer calculation.The conversion between centroid coordinates (x i , y i , z i ) and serial number i of the grid body such as formula (2), Mod(•) is the remainder opera- tion, Floor(•) is the round-down operation.

Mathematical model.
Path result represents as formula (3), k is the path ordinal number, g i is the grid body serial number.A suc- cessful path is formed by starting point S , target point E , and n − 2 free grid bodies conforming in order.Formula (4) is the first objective function of minimizing the path length.Path length PL is calculated by formula ( 6) and (7).d g i−1 ,g i is the linear distance from g i−1 to g i .Formula (5) is the second objective function of minimizing the path tortuosity.Path tortuosity Pθ is calculated by formula (8) and (9).θ g i−1 ,g i is the path turning degree from g i−1 to g i ,θ Formula (10) is the constraint of path to safely arrive the target location.P k 1 in formula (11) is the arrival path constraint.If the path can reach the target location P k 1 = 1 , then the arrival path constraint is satisfied.P k 2 in formula (12) is the safe path constraint.N(g i−1 , g i ) represents the connection relation between adjacent grids, as formula (13).When the connection relation is correct, N(g i−1 , g i ) = 1 .If all adjacent grids are connected correctly in the path result P k 2 = 1 , then the safe path constraint is satisfied.
The Min Max normalization method is used to normalize the two indicators PL and Pθ , as formula ( 14). (2) 1, g i−1 to g i available 0, g i−1 to g i unavailable www.nature.com/scientificreports/P k in formula ( 15) is comprehensive restraint punishment.As formula (16), fit k is bi-objective comprehensive fitness, the smaller fit k , the higher the superiority of the path.

Spatial search strategy
With the grid as the center, aggregating adjacent free grids as the path space search domain.A grid can have at most 26 directional path choices spatially, as shown in Fig. 2.
The traditional method of screening grid activity value constructing the spatial search domain, the path may have through-type and touch-type connectivity errors, as shown in Figs. 3 and 4. To illustrate the types of connectivity errors, Figs. 3 and 4 are fixed viewing angles.The structures shown in Figs. 3 and 4 can be rotated in three dimensions to derive all spatial connectivity errors.
In view of the above connectivity errors, this paper proposes a search strategy of spatial connectivity adjacency domain search strategy (SCADSS) to correct the connectivity relation in the search domain.SCADSS constructs the spatial search domain by centering on a free grid body then expanding the correct connectivity adjacency free grid body with three adjacency expansion rules.The three adjacency expansion rules: face adjacency expansion ( 16)   www.nature.com/scientificreports/rule (FAER), edge adjacency expansion rule (EAER), point adjacency expansion rule (PAER).Each adjacency expansion rule contains two criterions: the adjacency location criterion and the adjacency connectivity criterion.Applying SCADSS, the correct connectivity relation can ensure secure spatial path planning.The three adjacency expansion rules are as follows.
Setting: central free grid body as u(x u , y u , z u ) , unknown connectivity free grid body as v(x v , y v , z v ) , auxiliary judgment grid body as w(x w , y w , z w ) .Recording : the number of auxiliary judgment free grid bodies for FAER as Number F−w (u, v) , the number of auxiliary judgment free grid bodies for EAER as Number E−w (u, v) , the number of auxiliary judgment free grid bodies for PAER as Number P−w (u, v) .N(u, v) is defined in Formula (13), holding the correct connectivity relation between the two grid bodies.

Face adjacency expansion rule.
• Adjacency location criterion of FAER: If the free grid body v and free grid body u satisfy , the free grid body v is at the face adjacency location of grid body u.
• Adjacency connectivity criterion of FAER: The free grid body v is at the face adjacency location of grid body u.When field(u) = 0 and field(v) = 0 , an auxiliary judgment grid body w exists, the grid body w is the grid body u itself.If Number F−w (u, v) = 1 , then N(u, v) = 1 , the correct connectivity relation between grid body v and grid body u is established.
In Figure 5, the left figure is the interpretation of FAER and the right figure is the optimal result for FAER.With red grid body as the center grid, the application FAER can expand the path search domain of 6 directions connected correctly at most.

Edge adjacency extension rule.
• Adjacency location criterion of EAER: If the free grid body v and free grid body u satisfy one of above three conditions, the free grid body v is at the edge adjacency location of grid body u.

• Adjacency connectivity criterion of EAER:
The free grid body v is at the edge adjacency location of free grid body u .When the grid body w is both at the face adjacency location of the grid body u and the grid body v , an auxiliary judgment grid body w exists.
If Number E−w (u, v) = 2 , then N(u, v) = 1 , the correct connectivity relation between grid body v and grid body u is established.
In Figure 6, the left figure is the interpretation of EAER and the right figure is the optimal result for EAER.With red grid body as the center grid, the application EAER can expand the path search domain of 12 directions connected correctly at most.

Point adjacency extension rule.
• Adjacency location criterion of PAER: If free grid body v and free grid body u satisfy , the free grid body v is at the point adjacency location of grid body u.
• Adjacency connectivity criterion of PAER: The free grid body v is at the point adjacency location of free grid body u .When the grid body w is both at the edge adjacency location of the grid body u and the grid body v , an auxiliary judgment grid body w exists.If Number P−w (u, v) = 3 , then N(u, v) = 1 , the correct connectivity relation between grid body v and grid body u is established.
In Figure 7, the left figure is the interpretation of PAER and the right figure is the optimal result for PAER.With red grid body as the center grid, the application PAER can expand the path search domain of 8 directions connected correctly at most.
Through the three rules of SCADSS, all the grid bodies v correctly connecting with grid bodies u could be expanded.The grid bodies v form set A u .A u is the space search domain that ensures path security by modifying connective relation.

AUV autonomous path planning algorithm
The AUV updates the current position i in real time from the start location, and picks the next better position j in the search domain A i of i .The search operation is repeated until the search domain is empty or the target location is reached.Heuristics are designed to adapt the ACO algorithm to the optimization requirements.The pheromone update mechanism has been optimized to improve the convergence rate of the algorithm.
Improved distance heuristic function.The Euclidean distance d jE from the search position to the target point is introduced to enhance the optimal path guidance of the distance heuristic function η′(t) , as formula (17).To avoid the problem that the traditional ACO algorithm only relies on path visibility d ij to produce a large number of poor initial solutions in the initial operation.
Local turning heuristic function.The local path turning angles that may be generated by the path heading change include:0 In order to reach the shortest path, the same path point cannot be selected twice when searching a path, so there is no possibility of 180 • .0 • means heading straightly.A larger value of θ ij increases the space and energy burden of AUV path deflection.Consider the burden of AUV deflection course, especially the safety of turning operation while avoiding obstacles.The local turning angle heuristic function µ(t) is designed to measure the rationality of path direction selection, as the formula 18.
(17) η′(t) = 1 www.nature.com/scientificreports/Global comprehensive guidance heuristic function.A global comprehensive guidance heuristic function ϕ(t) is designed, as shown in formula (19), to evaluate the influence of location selection on the overall path tortuosity.Avoid path searching generates the shortest path and the gentlest path.The trend of comprehensive guidance path exploration tends to the optimal solution of dual objective path planning problem.The integrated guidance angle θ jE is shown in Fig. 8.

Path selection probability.
Based on the heuristic functions designed, the improved path location selection probability P k ij (t) as shown in formula (20).
Dynamic adjustment strategy of pheromone volatilization factor.ACO algorithm iterates the optimal solution through pheromone.Pheromone volatile factor ρ directly affect the convergence of the algo- rithm.In the early stage of iteration, ρ should be large to attenuate the poor solutions.In the middle stage of iteration, ρ should be moderate accelerate the screening of better solutions.In the later stage of iteration, ρ should be smaller to urge global convergence.The scaling property of the function f (x) as formula (21) satisfies the above requirements and ensures the ρ value is not too small.The dynamic regulation strategy of pheromone volatile factors as shown in formula (22).f t mid is the average fitness of the result of every iteration.f t best is the optimal fitness value of each iteration.ρf (t) is dynamically adjusted according to the solving quality of each generation iteration to avoid the interference of inferior solution and improve the algorithm convergence speed.a is a constant keeping ρ f (t) ∈ (0, 1).
Comprehensive pheromone updating strategy.In order to show the effect of dual-objective planning (shortest path length path and least path tortuosity) in pheromone, the improved integrated pheromone increment is shown in formula ( 24)- (25).fit k t is the fitness.ω is the fitness enhancement factor.�τ k ij represents the pheromone released on grids.(20) www.nature.com/scientificreports/Algorithm procedure.The improved algorithm procedure in this paper, as shown in Figure 9.

Simulation experiment and result analysis
Search strategy validation analysis.In the same 10*10*10 environment map, two groups of simulation experiments are performed with basic ACO.Each group of simulation experiment plans a shortest path from the same starting location to the same target location.The rationality and effectiveness of SCADSS are verified indirectly by the path planning results of simulation experiments.Group-I of simulation experiment: implement the expansion object obstacle treatment and planar search strategy.Group-II of simulation experiment: implement spatial connectivity adjacent domain search strategy.Compare and verify the rationality and effectiveness of SCADSS.The experimental results are shown in Figs. 10, 11.Analyzing the statistical data in Table 1, the implementation of both strategies meets the basic requirements of safe navigation of AUV in path planning.There is no significant difference in the length of experimental path between the two groups' results.However, the experimental of implementing SCADSS showed that the number of turning times decreased from 19 to 15, and the path tortuosity decreased by 21.05%.SCADSS avoids unnecessary obstacles in environment modeling and ensures the scale of traversable space for path search.So a large solution set space is created for the optimal solution, and the success rate of path search is increased by 3%.It can be proved that the SCADSS strategy has obvious rationality and superiority, and is more conducive to smooth path planning of AUV kinematic characteristics.
Parameter optimization.In the experiment, the parameters of the ant colony optimization are initialized AUV path planning simulation experiment.To verify the effectiveness of the IACO algorithm proposed in this paper in robot 3D path planning.ACO and UACO 30 are used as comparison algorithms.The simulation comparison test is conducted in 10 * 10 * 10 of the longitudinal obstacle dense environment map.The three algorithm parameter settings are shown in Table 3.   15.The blue curve is the fit iteration result of IACO.The black curve is the fit iteration result of UACO.The red curve is the fit iteration result of ACO.The experimental results are shown in Table 4.
Table 4 shows that the shortest path length obtained by IACO algorithm is 3.86% shorter than that obtained by UACO algorithm and 41.55% shorter than that obtained by ACO algorithm.In addition, compared with UACO and ACO algorithms, the turning times of the optimal path obtained by IACO algorithm are reduced by 60% and 86.67% respectively.The adaptability of the planned path obtained by IACO algorithm is optimal, balancing the requirements of path length and path transition.According to the iteration curve of the IACO algorithm, there are obvious advantages in the initial stage of iteration.Moreover, the optimization value decreases steadily, almost without sudden change and jump fluctuation, and the search speed is 45.83% and 81.94% higher than that of UACO and ACO respectively.Therefore, the IACO algorithm proposed in this paper has stronger optimization ability in AUV path planning.

Conclusion
Considering the influence of connectivity between search locations on path security, the spatial connectivity adjacency search domain strategy is designed.On the premise of not reducing the free area in the space, the space security search of the path can be realized.
SCADSS broadens the view of path search and is beneficial to the generation of smooth path.Combined with SCADSS, ACO algorithm is optimized to solve the problem of path length and path tortuosity of AUV in space obstacle dense environment.The local turn heuristic function is designed to improve the superiority of local direction selection.The global comprehensive guidance heuristic function and the improved distance heuristic function are designed to improve the ability of searching the equilibrium solution with the shortest path length and the least path tortuosity.Dynamic adjustment strategy of pheromone volatile factor improves the sensitivity of the algorithm to the solution quality.The simulation experiments select the most suitable algorithm parameters, verify the correctness of the spatial search strategy and the superiority and rapidity of the improved ACO algorithm.

Table 1 .
Search stra13,14.alidationanalysis.In the 10 * 10 * 10 grid map, the optimal path simulation of the three algorithms are shown inFigures 12,13,14.The fit iteration curves are shown in Figure