Unmanned ship dynamic obstacle avoidance method based on fusion of improved ant colony algorithm and dynamic window method

Abstract

The invention discloses an unmanned ship dynamic obstacle avoidance method based on fusion of an improved ant colony algorithm and a dynamic window method. The method mainly comprises the following steps: S1, initializing a grid map, and setting a starting point and a target point of path planning; s2, performing global path planning by adopting an improved ant colony algorithm, improving an initial pheromone concentration, improving a transition probability, and adding a collision risk degree to ensure safety and reliability of a global planning path; s3, deleting redundant points, reducing the number of turning points, and enabling the path to better meet actual requirements; and S4, performing local path planning by adopting a dynamic window method, and adding a near global path evaluation sub-function on a dynamic window method evaluation function, thereby realizing dynamic obstacle avoidance of the unmanned ship in a complex water area environment, and ensuring that the unmanned ship safely and reliably arrives at a destination.

Description

Technical Field

[0001] The invention relates to the technical field of unmanned boat path planning, and in particular to an unmanned boat dynamic obstacle avoidance method based on the fusion of an improved ant colony algorithm and a dynamic window method. Background Art

[0002] Unmanned surface vehicles (USVs) have broad application prospects in the fields of marine environmental monitoring, marine rights protection, and military, and have become a research hotspot in the field of marine intelligent equipment. A safe and reliable path planning method is a prerequisite for USVs to perform various complex surface missions.

[0003] At present, the algorithms used for USV autonomous obstacle avoidance mainly include A* algorithm, genetic algorithm, ant colony algorithm, artificial potential field method, dynamic window method, etc. Ant colony algorithm is a distributed intelligent bionic algorithm that simulates the foraging behavior of ants in nature and has a positive feedback effect. It is widely used in itinerary planning, network wiring, data mining and image processing. However, the traditional ant colony algorithm has problems such as slow convergence, easy to fall into local optimality, blind search, etc. Many scholars have improved the ant colony algorithm and achieved good results, mainly from the aspects of parameter optimization, pheromone initialization and update rules, and combined the ant colony algorithm as a global path planning with the local path planning algorithm to learn from each other and make up for the shortcomings. Commonly used local path planning algorithms such as dynamic window method and artificial potential field method still have problems such as too long path planning, blind search, and inconsistency with the actual path.

[0004] In order to solve the problems of blind search, slow convergence speed, long planning path, easy collision with obstacles, etc. in the early stage of the ant colony algorithm, the present invention proposes a dynamic obstacle avoidance method for unmanned boats based on the fusion of improved ant colony algorithm and dynamic window method. By performing non-uniform distribution of initial pheromones, improving state transfer rules, pheromone update method and fusing dynamic window method, it can quickly realize smooth processing of dynamic obstacles while ensuring the safety of the unmanned boat itself, and can achieve the optimal feasible solution based on global information. Summary of the invention

[0005] In view of the above-mentioned deficiencies in the prior art, the present invention proposes a dynamic obstacle avoidance method for an unmanned boat based on the fusion of an improved ant colony algorithm and a dynamic window method.

[0006] In order to solve the above technical problems, the technical solution adopted by the present invention is as follows:

[0007] A method for unmanned boat dynamic obstacle avoidance based on the fusion of improved ant colony algorithm and dynamic window method includes the following steps:

[0008] S1: rasterize the real water map information collected in advance, save the rasterized map information, and determine the initial location and target point of the unmanned boat;

[0009] Furthermore, the specific steps of S1 are:

[0010] The rasterized map information can display obstacles and surrounding environment information. Each grid corresponds to its position coordinates with a unique serial number, and the corresponding relationship is:

[0011]

[0012] Among them, (x i ,y i ) is the position coordinate of the ith grid, i is the serial number of the ith grid, mod() is a redundant operator, ceil() represents an integer operation, and width is the number of grids in each row.

[0013] S2: On the rasterized map, the ant colony algorithm is started to plan the global path according to the improved initial pheromone concentration and the transfer probability with the added risk factor to form a preliminary global path;

[0014] Furthermore, the specific steps of S2 are:

[0015] The improved initial pheromone concentration is:

[0016]

[0017] Among them, d SG is the distance between the starting point S and the end point G on the map, d ij is the distance between node i and node j, d jG is the distance between node j and end point G, λ0 is a positive constant, allowed i is the next feasible node set of node i.

[0018] The improved pheromone update rules are as follows:

[0019]

[0020] Among them, Q is the pheromone strength, maxPL and minPL are the lengths of the optimal path and the worst path of this iteration respectively, mean is the overall mean of the path length of this iteration, mean1 and mean2 are the sub-means of the parts less than the overall mean and greater than the overall mean respectively.

[0021] The improved adaptive pheromone volatilization factor:

[0022]

[0023] Among them, G represents the current iteration number, G max represents the maximum number of iterations, b and c are constants. As the number of iterations increases, the ρ value gradually decreases. At the same time, in order to avoid premature and stagnant phenomena in the algorithm during the search, the present invention regulates the pheromone concentration by setting the upper and lower limits, which are limited to [τ min , τ max ] interval, the formula is as follows:

[0024]

[0025] Among them, τ min represents the lower limit of pheromone concentration, τ max Indicates the upper limit of pheromone concentration.

[0026] The transition probability with added risk factor is:

[0027]

[0028] Among them, τ i,j (t) is the pheromone concentration of the ant colony algorithm at time t, η i,j (t) is the heuristic function, which represents the path heuristic information of moving from the current grid position i to the jth grid at time t, CRI i,j (t) is the collision risk, α and β are the influence factors of pheromone and heuristic function respectively, and γ represents the relative importance of collision risk.

[0029] The definition of collision risk is as follows:

[0030] CRI i,j (t) = α * obsNum j (t)

[0031] That is, the collision risk of the unmanned boat moving from the current grid i to the next grid j is proportional to the number of obstacles around the grid j. In the present invention, a=1.

[0032] S3: Use the improved path node selection strategy to process the nodes in the global path and discard the redundant nodes for path optimization;

[0033] Furthermore, the specific steps of S3 are:

[0034] When the unmanned boat is actually sailing, it may come into contact with obstacles, which may lead to potential collision risks. To avoid this, the following constraints are added when selecting the next path node at each node:

[0035] Starting from the first path node, calculate whether there is an obstacle between its previous node and the next node. If there is an obstacle, keep the node. If there is no obstacle, remove the node from the path and directly connect its previous node with the next node. Continue to make this judgment on the next node until you reach the target point.

[0036] S4: Obtain key turning points in the global optimization path, and use the key turning points as local target points, perform dynamic obstacle avoidance based on an improved dynamic window method, and obtain the optimal motion trajectory;

[0037] Furthermore, the specific steps of S4 are:

[0038] The sub-function pathdist(v,ω) close to the global path evaluation is described as follows:

[0039]

[0040] Among them, (x0, y0) is the coordinate of the end point of the trajectory simulated by the unmanned boat according to the speed combination, and the three coefficients A, B, and C are the coefficients of the equation Ax+By+C=0 of the global path segment at the location of the unmanned boat.

[0041] The evaluation function of the improved dynamic window method is:

[0042] G(v,ω)=σ(α·head(v,ω)+β·dist(v,ω)+γ·vel(v,ω)+λ·pathdist(v,ω))

[0043] Among them, head(v,ω) is the azimuth evaluation function, which represents the azimuth deviation between the simulated predicted path end position and the global path node, dist(v,ω) is the closest distance to the obstacle on the velocity corresponding trajectory, vel(v,ω) represents the speed of the unmanned boat in the current estimation, pathdist(v,ω) is the sub-function close to the global path evaluation, α, β, γ, λ are the weight coefficients of the sub-function responses of the evaluation function, σ represents the normalization process, and the four parameters are normalized to between [0,1] to avoid any one value being too large.

[0044] Compared with the prior art, the present invention has the following advantages:

[0045] The present invention reduces the blind search of ants in the early stage by improving the non-uniform distribution of initial pheromones, so that the ants can move to the target point faster according to the pheromones when searching in the early stage;

[0046] The present invention improves the transfer probability based on the risk coefficient and introduces a collision risk factor to reduce the probability of the unmanned boat entering waters with dense obstacles, effectively reducing the risk of collision and facilitating better exploration of the optimal path.

[0047] The present invention modifies the evaluation function of the dynamic window method, introduces a sub-function close to the global path evaluation, selects the optimal trajectory from the infinite number of feasible motion trajectories generated, and avoids problems such as obstacle collision and target unreachability. BRIEF DESCRIPTION OF THE DRAWINGS

[0048] Figure 1 The present invention is a flow chart of a method for dynamic obstacle avoidance of an unmanned boat based on the fusion of an improved ant colony algorithm and a dynamic window method.

[0049] Figure 2 Schematic diagram of potential collision risk in an embodiment.

[0050] Figure 3 Schematic diagram of node constraints in the embodiment, where (a) means that both adjacent grids are unobstructed, (b) means that one of the two adjacent grids is an obstacle, and (c) means that both adjacent grids are obstacles.

[0051] Figure 4 Node abandonment graph in the embodiment, where (a) represents all path nodes, (b) represents abandoned point B, and (c) represents abandoned point C.

[0052] Figure 5 Schematic diagram of a sub-function close to the global path evaluation in an embodiment.

[0053] Figure 6 This is a basic ant colony algorithm effect diagram in the embodiment.

[0054] Figure 7 This is a diagram showing the effect of improving the ant colony algorithm in the embodiment.

[0055] Figure 8 Graph showing simulation results of the fusion algorithm of the present invention in the embodiment. DETAILED DESCRIPTION

[0056] In order to provide a further understanding and recognition of the structural features and the effects achieved by the present invention, the technical solution of the present invention is described in detail below with reference to the accompanying drawings.

[0057] The present invention proposes a dynamic obstacle avoidance method for unmanned boats based on the fusion of improved ant colony algorithm and dynamic window method. Figure 1 As shown, the specific steps include:

[0058] S1: In the preprocessing stage, the unmanned boat will rasterize the collected water area information, save the generated raster map, and determine the starting point and target point of the navigation. The raster map can concisely and effectively display obstacles and the surrounding environment, and each grid corresponds to the location coordinates through a unique serial number.

[0059] The corresponding relationship is:

[0060]

[0061] Among them, (x i ,y i ) is the position coordinate of the ith grid, i is the serial number of the ith grid, mod() is a redundant operator, ceil() represents an integer operation, and width is the number of grids in each row.

[0062] S2.1: In the early stages of artificial ant search, ants tend to conduct random blind searches, causing a large number of ants to fall into local optimality. Therefore, before the algorithm iteration begins, pheromones are unevenly distributed on the map, so that when ants search in the early stages, they can move to the target point faster based on these pheromones.

[0063] Improve the initial pheromone concentration, the specific details are as follows:

[0064]

[0065] Among them, d SG is the distance between the starting point S and the end point G on the map, d ij is the distance between node i and node j, d jG is the distance between node j and end point G, λ0 is a positive constant, allowed i is the next feasible node set of node i.

[0066] The iterated path length is divided into two parts: the path length less than the overall mean and the path length greater than the overall mean. Next, the mean path length of these two sub-parts is calculated respectively, and then the path of this iteration is divided into four sub-parts. Finally, a different pheromone update strategy is adopted for each sub-part. The specific update strategy formula is:

[0067]

[0068] Among them, Q is the pheromone strength, maxPL and minPL are the lengths of the optimal path and the worst path of this iteration respectively, mean is the overall mean of the path length of this iteration, mean1 and mean2 are the sub-means of the parts less than the overall mean and greater than the overall mean respectively.

[0069] In the ant colony optimization algorithm, the pheromone volatility factor is a constant that does not change with the number of iterations, resulting in unreasonable pheromone distribution when ants search for paths. The present invention proposes an adaptive pheromone volatility factor ρ, and its update strategy formula is:

[0070]

[0071] Among them, G represents the current iteration number, G max represents the maximum number of iterations, b and c are constants. As the number of iterations increases, the value of ρ gradually decreases. Therefore, the improved algorithm can not only enable ants to obtain more valid paths, but also improve the convergence speed of the algorithm.

[0072] At the same time, in order to avoid premature and stagnant phenomena in the algorithm during the search, the present invention regulates the pheromone concentration by setting the upper and lower limits, which are limited to [τ min , τ max ] interval, the formula is as follows:

[0073]

[0074] Among them, τ min represents the lower limit of pheromone concentration, τ max Indicates the upper limit of pheromone concentration.

[0075] S2.2: When the traditional ant colony algorithm faces a water area with dense obstacles, if the water area is located near the line connecting the starting point and the target point, the ant colony algorithm is likely to guide the unmanned boat through it, which poses a great risk of collision. Therefore, a collision risk factor is introduced when calculating the path transfer probability. This improved strategy can reduce the probability of the unmanned boat entering a water area with dense obstacles, effectively reducing the risk of collision.

[0076] The transition probability with added risk factor is:

[0077]

[0078] Among them, τ i,j (t) is the pheromone concentration of the ant colony algorithm at time t, η i,j (t) is the heuristic function, which represents the path heuristic information of moving from the current grid position i to the jth grid at time t, CRI i,j (t) is the collision risk, α and β are the influence factors of pheromone and heuristic function respectively, and γ represents the relative importance of collision risk.

[0079] The definition of collision risk is as follows:

[0080] CRI i,j (t) = a*obsNum j(t)

[0081] That is, the collision risk of the unmanned boat from the current grid i to the next grid j is proportional to the number of obstacles around grid j, that is, the more obstacles there are around a feasible node, the greater the collision risk of the point, and the smaller the probability of the ant reaching the point. The present invention takes a=1.

[0082] S3: The unmanned boat works in a complex and changeable water environment. The trajectory of the actual operation is difficult to strictly match the planned trajectory. When the ant colony algorithm plans the path for the unmanned boat, it may stick to obstacles, which may cause potential collision risks, such as Figure 2 To avoid this, add the following constraints when selecting the next path node at each node:

[0083] When there are no obstacles in the two adjacent grids, the unmanned boat can directly travel to the diagonal grid; if there is an obstacle in one of the two adjacent grids, the unmanned boat needs to go through the adjacent grid without obstacles before reaching the diagonal grid; if both adjacent grids are obstacle grids, the unmanned boat has no way to reach the diagonal grid. The schematic diagram of this constraint is as follows Figure 3 shown.

[0084] Based on the above analysis, the present invention proposes the following redundant node abandonment strategy:

[0085] Starting from the first path node, calculate whether there is an obstacle between its previous node and the next node. If there is an obstacle, keep the node. If there is no obstacle, remove the node from the path and directly connect its previous node to the next node. Continue to make this judgment on the next node until you reach the target point. The schematic diagram of the above process is as follows Figure 4 shown.

[0086] S4: Use the key turning points retained in S3 as the local target points of the DWA optimization algorithm to guide the local direction, use the evaluation function of the improved DWA algorithm to evaluate the quality of the trajectory, and select the optimal trajectory from the infinite number of feasible motion trajectories generated. The evaluation function of the DWA optimization algorithm is as follows:

[0087] G(v,ω)=σ(α·head(v,ω)+β·dist(v,ω)+γ·vel(v,ω)+λ·pathdist(v,ω))

[0088] Among them, head(v,ω) is the azimuth evaluation function, which represents the azimuth deviation between the simulated predicted path end position and the global path node, dist(v,ω) is the closest distance to the obstacle on the velocity corresponding trajectory, vel(v,ω) represents the speed of the unmanned boat in the current estimation, pathdist(v,ω) is the sub-function close to the global path evaluation, α, β, γ, λ are the weight coefficients of the sub-function responses of the evaluation function, σ represents the normalization process, and the four parameters are normalized to between [0,1] to avoid any one value being too large.

[0089] Close to the global path evaluation subfunction pathdist(v,ω):

[0090] This function uses the distance between the front end of the current predicted trajectory and the global path to evaluate the quality of the path. The closer the distance between the two, the closer the path is to the global path, and the higher the score is. Conversely, the lower the score is. The calculation process of this function is as follows Figure 5 shown.

[0091]

[0092] Among them, (x0, y0) is the coordinate of the end point of the trajectory simulated by the unmanned boat according to the speed combination, and the three coefficients A, B, and C are the coefficients of the equation Ax+By+C=0 of the global path segment where the unmanned boat is located. When the distance value is smaller, it means that the trajectory fits the global path more closely and the score is higher. The cosine function is selected for secondary processing based on its scoring characteristics, and the trajectory with a distance exceeding the threshold is directly scored as 0.

Claims

1. A dynamic obstacle avoidance method for unmanned boats based on the fusion of improved ant colony algorithm and dynamic window method, characterized in that: The following steps are involved: S1: rasterize the real water map information collected in advance, save the rasterized map information, and determine the initial location and target point of the unmanned boat; S2: On the rasterized map, the ant colony algorithm is started to plan the global path according to the improved initial pheromone concentration and the transfer probability with the added risk factor to form a preliminary global path; S3: Use the improved path node selection strategy to process the nodes in the global path and discard the redundant nodes for path optimization; S4: Obtain key turning points in the global optimization path, and use the key turning points as local target points, perform local path planning based on the improved DWA algorithm, and obtain the optimal motion trajectory.

2. According to claim 1, a method for unmanned boat dynamic obstacle avoidance based on the fusion of improved ant colony algorithm and dynamic window method, characterized in that: The rasterized map information can display obstacles and surrounding environment information. Each grid corresponds to its position coordinates with a unique serial number, and the corresponding relationship is: Among them, (x i ,y i ) is the position coordinate of the ith grid, i is the serial number of the ith grid, mod() is a redundant operator, ceil() represents an integer operation, and width is the number of grids in each row.

3. The unmanned boat dynamic obstacle avoidance method based on the fusion of improved ant colony algorithm and dynamic window method according to claim 1 is characterized by: The improved initial pheromone concentration is: Among them, d SG is the distance between the starting point S and the end point G on the map, d ij is the distance between node i and node j, d jG is the distance between node j and end point G, λ0 is a positive constant, allowed i is the next feasible node set of node i. The improved pheromone update rules are as follows: Among them, Q is the pheromone strength, maxPL and minPL are the lengths of the optimal path and the worst path of this iteration respectively, mean is the overall mean of the path length of this iteration, mean1 and mean2 are the sub-means of the parts less than the overall mean and greater than the overall mean respectively. The improved adaptive pheromone volatilization factor: Among them, G represents the current iteration number, G max represents the maximum number of iterations, b and c are constants. As the number of iterations increases, the ρ value gradually decreases. At the same time, in order to avoid premature and stagnant phenomena in the algorithm during the search, the present invention regulates the pheromone concentration by setting the upper and lower limits, which are limited to [τ min , τ max ] interval, the formula is as follows: Among them, τ min represents the lower limit of pheromone concentration, τ max Indicates the upper limit of pheromone concentration. The transition probability with added risk factor is: Among them, τ i,j (t) is the pheromone concentration of the ant colony algorithm at time t, η i,j (t) is the heuristic function, which represents the path heuristic information of moving from the current grid position i to the jth grid at time t, CRI i,j (t) is the collision risk, α and β are the influence factors of pheromone and heuristic function respectively, and γ represents the relative importance of collision risk. The definition of collision risk is as follows: CRI i,j (t)=a*obsNum j (t) That is, the collision risk of the unmanned boat moving from the current grid i to the next grid j is proportional to the number of obstacles around the grid j. In the present invention, a=1.

4. The unmanned boat dynamic obstacle avoidance method based on the fusion of improved ant colony algorithm and dynamic window method according to claim 1 is characterized by: When the unmanned boat is actually sailing, it may come into contact with obstacles, which may lead to potential collision risks. To avoid this, the following constraints are added when selecting the next path node at each node: Starting from the first path node, calculate whether there is an obstacle between its previous node and the next node. If there is an obstacle, keep the node. If there is no obstacle, remove the node from the path and directly connect its previous node with the next node. Continue to make this judgment on the next node until you reach the target point.

5. The unmanned boat dynamic obstacle avoidance method based on the fusion of improved ant colony algorithm and dynamic window method according to claim 1 is characterized by: The sub-function pathdist(v,ω) close to the global path evaluation is described as follows: Among them, (x0, y0) is the coordinate of the end point of the trajectory simulated by the unmanned boat according to the speed combination, and the three coefficients A, B, and C are the coefficients of the equation Ax+By+C=0 of the global path segment at the location of the unmanned boat. The above-mentioned addition of a sub-function close to the global path evaluation function to the dynamic window method evaluation function: G(v,ω)=σ(α·head(v,ω)+β·dist(v,ω)+γ·vel(v,ω)+λ·pathdist(v,ω)) Among them, head(v,ω) is the azimuth evaluation function, which represents the azimuth deviation between the simulated predicted path end position and the global path node, dist(v,ω) is the closest distance to the obstacle on the velocity corresponding trajectory, vel(v,ω) represents the speed of the unmanned boat in the current estimation, pathdist(v,ω) is the sub-function close to the global path evaluation, α, β, γ, λ are the weight coefficients of the sub-function responses of the evaluation function, σ represents the normalization process, and the four parameters are normalized to between [0,1] to avoid any one value being too large.

Images