Path planning method for dynamic window guided by ant colony optimization Voronoi diagram

Abstract

The invention discloses a path planning method for a dynamic window guided by an ant colony optimization Voronoi diagram, which comprises the following steps of: acquiring an adjacency distance matrix of path key points, defining an edge set of the Voronoi diagram according to the adjacency distance matrix to acquire an initial Voronoi diagram, traversing the edge set of the initial Voronoi diagram, and performing path planning on the dynamic window guided by the Voronoi diagram. Deleting an edge intersecting with the static obstacle to correct the initial Voronoi diagram to obtain a corrected Voronoi diagram; obtaining a global optimal path according to the corrected Voronoi diagram based on an ant colony algorithm and a genetic algorithm, and taking a path key point in the global optimal path as a ship waypoint; and constructing a ship motion model, and obtaining a local optimal path according to the ship waypoint based on an improved dynamic window algorithm, thereby realizing the path planning of the ship. The problems that an existing path planning technology cannot give consideration to global optimality, real-time obstacle avoidance capacity and motion stability to achieve path planning, namely, the global path searching efficiency is low, paths are redundant, the real-time performance is poor, and the dynamic environment adaptability is insufficient are solved.

Description

Technical Field

[0001] The present invention relates to the technical field of unmanned surface vehicle (USV) path planning, and in particular to a path planning method using an ant colony optimized Voronoi diagram to guide a dynamic window. Background Art

[0002] In recent years, with the development and progress of science and technology, unmanned ships, which are gradually developing towards intelligence and autonomy, are being used more and more widely in marine engineering. Unmanned surface autonomous vehicles can replace humans in professional or dangerous operations, saving labor costs. Fully automated ships perform tasks through sensors. Route planning, as an important issue in the marine field, is also an important manifestation of ship intelligence. Therefore, path planning technology has gradually been applied to the field of unmanned ship path planning.

[0003] The algorithms involved in existing traditional path planning technologies have the following problems:

[0004] While global planning algorithms (such as A* and Voronoi diagrams) can generate globally optimal paths, they struggle to cope with dynamic obstacles and real-time obstacle avoidance requirements. Local planning algorithms (such as DWA) possess dynamic response capabilities but lack global guidance, making them prone to local optimality and path redundancy. Traditional fusion methods (such as combining A* with DWA) suffer from low path search efficiency, large speed fluctuations, and poor adaptability to dynamic environments. Furthermore, genetic algorithms (GAs) and ant colony algorithms (ACOs) are prone to premature convergence when used alone, resulting in limited path planning quality. Therefore, existing path planning technologies are unable to achieve path planning while simultaneously achieving global optimality, real-time obstacle avoidance, and motion stability. Consequently, they suffer from low global path search efficiency, path redundancy, poor real-time performance, and insufficient adaptability to dynamic environments. Summary of the Invention

[0005] The present invention provides a path planning method for a dynamic window guided by an ant colony optimization Voronoi diagram to overcome the above technical problems.

[0006] In order to achieve the above object, the technical solution of the present invention is:

[0007] A path planning method for a dynamic window guided by an ant colony optimization Voronoi diagram specifically includes the following steps:

[0008] S1: Pre-build an environment grid map containing known static obstacles for path planning;

[0009] S2: Obtain the cell map around the static obstacle based on the environment grid map;

[0010] And obtain the key point set of the initial global guidance path according to the cell diagram;

[0011] S3: Calculate the Euclidean distance between any two adjacent points in the key point set, obtain the adjacency distance matrix of the path key points, define the edge set of the Voronoi graph based on the adjacency distance matrix, and then obtain the initial Voronoi graph;

[0012] Traverse the edge set of the initial Voronoi diagram, delete the edges intersecting with the static obstacles to correct the initial Voronoi diagram, and obtain the corrected Voronoi diagram;

[0013] S4: Based on the ant colony algorithm and genetic algorithm, the global optimal path is obtained according to the modified Voronoi diagram, and the key points of the path in the global optimal path are used as the ship waypoints;

[0014] S5: Construct a ship motion model and obtain the local optimal path according to the ship waypoints based on the improved dynamic window algorithm to achieve ship path planning.

[0015] Furthermore, the S2 specifically includes the following steps:

[0016] S21: Mark the static obstacles in the environment grid map to obtain obstacle marking points, and use the obstacle marking points as the initialization scattered point set constituting the Voronoi diagram;

[0017] S22: Based on the Fortune algorithm and the initial scattered point set, a cell diagram around the static obstacle is obtained, and the union of all the boundaries of the Voronoi diagram region is obtained based on the cell diagram;

[0018] And the expression of the union of the boundaries of the Voronoi graph region is

[0019]

[0020] V F (p)={q:d(q,p)≤d(q,F / {p}))}

[0021] Where: F represents the boundary point set of the cell graph surrounding the static obstacle; V F (p) represents the neighborhood of the boundary point p; q,p represent the boundary points of the cell diagram surrounding the static obstacle; d(q,p) represents the Euclidean distance between the boundary points q,p; d(q,F / {p})) represents the Euclidean distance between the boundary point q and the other boundary points in the boundary point set except the boundary point p; VD(F) represents the union of the boundaries of the region constituting the Voronoi diagram;

[0022] S23: Obtain a set of key points of the initial global guidance path of the ship according to the union of all the region boundaries constituting the Voronoi diagram;

[0023] And the expression of the key point set is

[0024] I=voronoi(O)

[0025] Where I represents the key point set of the initial global guidance path of the ship; voronoi(O) represents the intersection set of boundary points in the union VD(F) of all the boundary points that constitute the Voronoi diagram region.

[0026] Furthermore, the formula for obtaining the modified Voronoi diagram in S3 is:

[0027]

[0028] Where: isIntersect(edge i ,obs j ) represents a piecewise function that determines whether the edge of the initial Voronoi diagram intersects with the static obstacle; edge i Represents the key point P(x n ,y n ) and Q(x m ,y m ) The edges of the initial Voronoi graph determined by obs j Indicates the position of the static obstacle and obs j (x i ,y i )∈O;d ij Indicates the position of the static obstacle obs j Edge of the initial Voronoi diagram i The distance between them.

[0029] Furthermore, the S4 specifically includes the following steps:

[0030] S41: Setting the starting waypoint and the ending waypoint of the path planning according to the modified Voronoi diagram;

[0031] Set the number of ant individuals m in the ant colony algorithm, randomly set the starting point of the ant individuals and let each ant individual start searching from the key point on the path closest to the starting point;

[0032] And according to the state transition probability, the search ends with the path key point closest to the end waypoint as the end point, and the expression of the state transition probability is

[0033]

[0034] Where: represents the probability that the ant individual k moves from the i-th path key point to the j-th path key point in the t-th cycle; η ij (t) represents the expected heuristic of each ant individual moving from the i-th path key point to the j-th path key point in the t-th cycle, and η ij (t) = 1 / d′ ij +Crandom, Crandom represents mutation factor; d′ij represents the distance between the i-th path key point and the j-th path key point; τ is (t) represents the pheromone concentration of the distance between the i-th path key point and the s-th path key point at the t-th iteration; η js (t) represents the expected heuristic for each ant individual to move from the jth path key point to the sth path key point in the tth cycle, and η js (t) = 1 / d′ js +Crandom; α represents the pheromone importance factor; β represents the heuristic information importance factor; a k represents the set of key points on the path that ant individual k is allowed to visit next; τ ij (t) represents the pheromone concentration of the path between the i-th path key point and the j-th path key point in the t-th cycle;

[0035] S42: When the ant individual traverses all key points on the path and completes one iteration;

[0036] Then update the pheromone concentration of the path between the i-th path key point and the j-th path key point. The update rule formula of the pheromone concentration is:

[0037]

[0038] Where: The pheromone concentration of the path between the i-th path key point and the j-th path key point in the t+1th cycle; ρ represents the pheromone evaporation rate and ρ∈(0,1); Indicates the pheromone change value; Q indicates the pheromone increase intensity; T represents the length of the path that ant individual k has traveled at time t; k represents the path chosen by ant individual k during the search process;

[0039] S43: According to the pheromone concentration update rule, iterate the ant colony algorithm until the pheromone concentration converges to a preset pheromone threshold to obtain a search path;

[0040] S44: Arrange the search paths corresponding to all acquired ant individuals in descending order according to the total distance of the paths to obtain a search path sorting table;

[0041] According to the search path sorting table, the top A% search paths are obtained and used as the initial population P0 of the genetic algorithm. in Indicates the search path corresponding to the Nth ant individual; num_ants represents the total number of ant individuals in the ant colony algorithm;

[0042] S45: Construct the fitness function of the genetic algorithm;

[0043] And the expression of the fitness function is

[0044]

[0045] Where: represents the fitness value of the i-th search path at the t-th iteration; The total distance of the i-th search path at iteration t;

[0046] Obtaining the fitness values of the population individuals of the genetic algorithm at the time of t iterations according to the fitness function, and arranging the fitness values of the population individuals in descending order to obtain a current population individual sequence table;

[0047] S46: Based on the current population individual sequence table, the genetic algorithm is used to repeatedly perform selection operations, crossover operations, and mutation operations to improve the fitness of the genetic algorithm so that the population individuals gradually approach the optimal solution, and finally output the global optimal solution when the preset maximum number of iterations is reached;

[0048] And the global optimal solution is output as the global suboptimal path;

[0049] S47: Obtain the global path point sequence of the global suboptimal path and record it as

[0050] L global ={start,p1,p2,p3…p i ,distination}; where start represents the starting waypoint; distination represents the ending waypoint; p i represents the i-th path key point;

[0051] The global optimal path is obtained based on the global path point sequence of the global suboptimal path using the head-tail double pointer method;

[0052] That is, traverse any two non-adjacent path key points in the global path point sequence and determine whether these two non-adjacent path key points can be directly connected;

[0053] And the rule for judging whether any two non-adjacent path key points can be directly connected is

[0054] Check whether the line connecting any two non-adjacent path key points intersects with a static obstacle;

[0055] If they intersect, confirm that they cannot be directly connected;

[0056] If they do not intersect, confirm that they can be directly connected and delete other key path points between these two key path points;

[0057] Then the global suboptimal path is optimized to obtain the global optimal path, and the key points of the path in the global optimal path are used as ship waypoints.

[0058] Furthermore, the S5 specifically includes the following steps:

[0059] S51: Consider dynamic obstacles during ship navigation to construct a ship motion model;

[0060] And the expression of the ship motion model is

[0061]

[0062] v=randn(1,0.2)*step,angle(t)=angle(t-1)+randn(ω),ω∈(-35°,+35°)

[0063]

[0064] Where: x(t n ),y(t n ) represents t n x(t0), y(t0) represent the ship's horizontal and vertical coordinates corresponding to the time t0; v(t) represents the ship's speed corresponding to the time t; v represents the abbreviated form of v(t) and v = randn(1,0.2)*step; randn(1,0.2) represents the decision variable that enables the ship's speed to be randomly selected in (0,step); step represents the ship's maximum step size, i.e., V max ; angle(t) represents the ship heading angle at time t; angle(t-1) represents the ship heading angle at time t-1; randn(ω) represents the random rudder angle; V a represents the braking limit speed space, i.e., the speed of the ship before the collision with the dynamic obstacle; ω represents the angular velocity of the ship; dist(v,ω) represents the closest distance between the simulated ship trajectory and the obstacle; V d Indicates the acceleration change limit space; v a ,ω a Indicates the current linear velocity and angular velocity of the ship;

[0065] S52: traverse all ship waypoints in the order of the global optimal path;

[0066] Confirm the current ship position to the preset candidate waypoint WP according to the ship motion model n Are there any obstacles in the path?

[0067] If it does not exist, the candidate waypoint WP nUse it as the target waypoint for the current leg and continue sailing;

[0068] If it exists, the optimal speed pair for the ship's navigation is obtained through the constructed improved evaluation function, and collision-free control of the local trajectory of the ship is achieved based on the optimal speed pair, thereby obtaining the local optimal path;

[0069] The ship's path planning is achieved based on the local optimal path.

[0070] Furthermore, the expression of the improved evaluation function constructed in S52 is

[0071] G(v t ,ω t )=K0(α*fitting(v t ,ω t )+β*velocity(v t ,ω t )+γ*obstacles(v t ,ω t )

[0072] +δ*endreach(v t ,ω t )+ε*constrain(v t ,ω t ))

[0073]

[0074] θ=Course-arctan(P t ,P goal )

[0075]

[0076] endreach(v t ,ω t )=C

[0077]

[0078] Where: G(v t ,ω t ) represents the improved evaluation function; K0 represents the smoothing function; α, β, γ, δ, ε represent the weight coefficients; fitting(v t ,ω t ) represents the route fitting reward function; Course represents the ship's heading; θ represents the angle between the ship's heading and the target point relative to the ship's position; arctan(P t ,P goal ) represents the direction of the target point relative to the own ship; Pt Indicates the coordinate point of the ship's position; P traj Indicates the predicted position of the dynamic window; P goal Indicates the current target waypoint; dist(P traj ,P goal ) represents the distance between the predicted position of the dynamic window and the target waypoint; dist(P t ,P goal ) indicates the distance between own ship and target waypoint; velocity(v t ,ω t ) represents the ship speed reward function; K2, K1 represent gain constants; V max Indicates the maximum speed limit; K3 indicates the gain constant; obstacles (v t ,ω t ) represents the reward function of the distance between the ship and the obstacle; limit represents the safe distance between the ship and the obstacle; d obs Indicates the distance between the ship and the obstacle; endreach(v t ,ω t ) represents the reward function for predicting the visibility of the end of the ship path arc and the target point; C represents a constant; constraint(v t ,ω t ) represents the ship oscillation constraint reward function.

[0079] The present invention provides a path planning method for a dynamic window guided by an ant colony optimization Voronoi diagram, which has the following beneficial effects:

[0080] (1) Global efficiency improvement: By obtaining the adjacency distance matrix of the key points of the path, the edge set of the Voronoi diagram is defined according to the adjacency distance matrix to obtain the initial Voronoi diagram. The edge set of the initial Voronoi diagram is traversed and the edges intersecting with static obstacles are deleted to correct the initial Voronoi diagram to obtain the revised Voronoi diagram. The search for redundant paths in the Voronoi diagram is reduced. The global optimal path is obtained according to the revised Voronoi diagram based on the ant colony algorithm and the genetic algorithm, which greatly improves the global search efficiency of path planning.

[0081] (2) Dynamic obstacle avoidance optimization: A ship motion model is constructed, and a local optimal path is obtained according to the ship waypoints based on the improved dynamic window algorithm. The optimal speed pair for the ship's navigation is obtained through the improved DWA algorithm, that is, through a multi-objective evaluation function including ship oscillation suppression (i.e., an improved evaluation function). Based on the optimal speed pair, a collision-free control path for the local trajectory of the ship is achieved, which greatly improves the smoothness of the local path planning.

[0082] (3) Enhanced environmental adaptability: Supports real-time obstacle avoidance for dynamic and unknown obstacles, significantly improving the path success rate in complex environments;

[0083] (4) System robustness: Global path planning and local path planning work together to ensure that the ship can quickly return to the optimal path after obstacle avoidance and avoid local oscillations. BRIEF DESCRIPTION OF THE DRAWINGS

[0084] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the following is a brief introduction to the drawings required for use in the embodiments or the description of the prior art. Obviously, the drawings described below are some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative labor.

[0085] Figure 1 A flow chart of the path planning method for a dynamic window guided by an ant colony optimized Voronoi diagram according to the present invention;

[0086] Figure 2 Schematic diagram of Voronoi diagram correction in this embodiment;

[0087] Figure 3 This is a schematic diagram of global path optimization in this embodiment;

[0088] Figure 4 This is a schematic diagram of target point selection in local path planning in this embodiment;

[0089] Figure 5 This is a monitoring diagram of the linear velocity, angular velocity, and target point distance parameters of the ship's motion under dynamic obstacle conditions based on the A-Star+DWA method in this embodiment;

[0090] Figure 6 This is a monitoring diagram of the linear velocity, angular velocity, and target point distance parameters of the ship motion under dynamic obstacle conditions based on the traditional DWA method in this embodiment;

[0091] Figure 7 This is a monitoring diagram of the linear velocity, angular velocity and target point distance parameters of the ship motion under dynamic obstacle conditions according to the method described in this embodiment. DETAILED DESCRIPTION

[0092] To make the objectives, technical solutions, and advantages of the embodiments of the present invention more clear, the technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the accompanying drawings of the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. All other embodiments obtained by ordinary technicians in this field based on the embodiments of the present invention without making any creative efforts shall fall within the scope of protection of the present invention.

[0093] This embodiment provides a path planning method for a dynamic window guided by an ant colony optimization Voronoi diagram. Figure 1As shown, the specific steps include:

[0094] S1: Pre-build an environment grid map containing known static obstacles for path planning;

[0095] Specifically, the environment grid map can be constructed using MATLAB as a platform, and the specific implementation method of constructing the environment grid map is a well-known technical means, which will not be described in detail here.

[0096] S2: Obtain a cell map around the static obstacle based on the environment grid map, and obtain a set of key points of the initial global guidance path based on the cell map;

[0097] The specific steps include:

[0098] S21: Marking static obstacles in the environment grid map to obtain obstacle marking points, and using the obstacle marking points as an initialization scatter point set for constituting a Voronoi diagram. Specifically, this embodiment performs Douglas–Peucker algorithm fitting and simplifying contours on a preset array of obstacle boundary points to obtain key vertices of the static obstacles and use them as obstacle marking points.

[0099] S22: Based on the Fortune algorithm and the initial scattered point set O, a cell diagram around the static obstacle is obtained, and the union of all the boundaries of the Voronoi diagram region is obtained based on the cell diagram;

[0100] The Fortune algorithm in this embodiment, also known as Fortune's Sweep-line algorithm, is an algorithm for calculating the Voronoi diagram. The main idea of the algorithm is to gradually scan a set of points using a sweep line and maintain some data structures to dynamically obtain the region boundaries of the Voronoi diagram.

[0101] And the expression of the union of the boundaries of the Voronoi graph region is

[0102]

[0103] V F (p)={q:d(q,p)≤d(q,F / {p}))}

[0104] Where: F represents the boundary point set of the cell graph surrounding the static obstacle; V F (p) represents the neighborhood of the boundary point p; q,p represent the boundary points of the cell diagram surrounding the static obstacle; d(q,p) represents the Euclidean distance between the boundary points q,p; d(q,F / {p})) represents the Euclidean distance between the boundary point q and the other boundary points in the boundary point set except the boundary point p; VD(F) represents the union of the boundaries of the region constituting the Voronoi diagram;

[0105] S23: Obtain a set of key points of the initial global guidance path of the ship according to the union of all the region boundaries constituting the Voronoi diagram;

[0106] And the expression of the key point set is

[0107] I=voronoi(O)

[0108] Where: I represents the key point set of the initial global guidance path of the ship; voronoi(O) represents the intersection set of boundary points in the union VD(F) of all the boundary points that constitute the Voronoi diagram region;

[0109] S3: If Figure 2 As shown, the Euclidean distance between any two adjacent points in the key point set is calculated, the adjacency distance matrix of the path key points is obtained, the edge set of the Voronoi graph is defined according to the adjacency distance matrix, and then the initial Voronoi graph is obtained; the edge set of the initial Voronoi graph is traversed, and the edges intersecting with the static obstacles are deleted to correct the initial Voronoi graph, and the corrected Voronoi graph is obtained;

[0110] Specifically, the formula for obtaining the modified Voronoi diagram is:

[0111]

[0112] Where: isIntersect(edge i ,obs j ) represents a piecewise function that determines whether the edge of the initial Voronoi diagram intersects with the static obstacle; edge i Represents the key point P(x n ,y n ) and Q(x m ,y m ) The edges of the initial Voronoi graph determined by obs j Indicates the position of the static obstacle and obs j (x i ,y i )∈O;d ij Indicates the position of the static obstacle obs j Edge of the initial Voronoi diagram i the distance between them;

[0113] S4: Based on the ant colony algorithm and genetic algorithm, the global optimal path is obtained according to the modified Voronoi diagram, and the key points of the path in the global optimal path are used as the ship waypoints;

[0114] In this embodiment, an initial pheromone value is assigned to each edge of the modified Voronoi diagram. Ants are instructed to start their search from the node closest to the starting point (key point on the path). The Voronoi edge to follow is selected based on state transitions and random mutation probabilities, and the search ends with the key point closest to the target point. The pheromone on each edge evaporates over time. When all ants reach the destination, the pheromone is updated. This process is repeated until convergence reaches a suitable pheromone threshold.

[0115] like Figure 3 As shown, the specific steps include:

[0116] S41: Setting the starting waypoint and the ending waypoint of the path planning according to the modified Voronoi diagram;

[0117] Set the number of ant individuals m in the ant colony algorithm, randomly set the starting point of the ant individuals and let each ant individual start searching from the key point on the path closest to the starting point;

[0118] And according to the state transition probability, the search ends with the path key point closest to the end waypoint as the end point, and the expression of the state transition probability is

[0119]

[0120] Where: represents the probability that the ant individual k moves from the i-th path key point to the j-th path key point in the t-th cycle; η ij (t) represents the expected heuristic of each ant individual moving from the i-th path key point to the j-th path key point in the t-th cycle, and η ij (t) = 1d′ ij +Crandom, Crandom represents mutation factor; d′ ij represents the distance between the i-th path key point and the j-th path key point; τ is (t) represents the pheromone concentration of the distance between the i-th path key point and the s-th path key point at the t-th iteration; η js (t) represents the expected heuristic for each ant individual to move from the jth path key point to the sth path key point in the tth cycle, and η js (t) = 1 / d′ js +Crandom; α represents the pheromone importance factor; β represents the heuristic information importance factor; a k represents the set of key points on the path that ant individual k is allowed to visit next; τ ij (t) represents the pheromone concentration of the path between the i-th path key point and the j-th path key point in the t-th cycle;

[0121] S42: When the ant individual traverses all key points on the path and completes one iteration;

[0122] Then update the pheromone concentration of the path between the i-th path key point and the j-th path key point. The update rule formula of the pheromone concentration is:

[0123]

[0124] Where: The pheromone concentration of the path between the i-th path key point and the j-th path key point in the t+1th cycle; ρ represents the pheromone evaporation rate and ρ∈(0,1); Indicates the pheromone change value; Q indicates the pheromone increase intensity; T represents the length of the path that ant individual k has traveled at time t; k represents the path chosen by ant individual k during the search process;

[0125] S43: According to the pheromone concentration update rule, iterate the ant colony algorithm until the pheromone concentration converges to a preset pheromone threshold to obtain a search path;

[0126] S44: Arrange the search paths corresponding to all acquired ant individuals in descending order according to the total distance of the paths to obtain a search path sorting table;

[0127] According to the search path sorting table, the top A% search paths are obtained and used as the initial population P0 of the genetic algorithm. in Indicates the search path corresponding to the Nth ant individual; num_ants represents the total number of ant individuals in the ant colony algorithm; A% is preferably 20%;

[0128] S45: Construct the fitness function of the genetic algorithm;

[0129] And the expression of the fitness function is

[0130]

[0131] Where: represents the fitness value of the i-th search path at the t-th iteration; The total distance of the i-th search path at iteration t;

[0132] Obtaining the fitness values of the population individuals of the genetic algorithm at the time of t iterations according to the fitness function, and arranging the fitness values of the population individuals in descending order to obtain a current population individual sequence table;

[0133] S46: Based on the current population individual sequence table, the genetic algorithm is used to repeatedly perform selection operations, crossover operations, and mutation operations to improve the fitness of the genetic algorithm so that the population individuals gradually approach the optimal solution, and finally output the global optimal solution when the preset maximum number of iterations is reached;

[0134] And the global optimal solution is output as the global suboptimal path;

[0135] Specifically, in this embodiment, the selection operation: That is, the better 50% of the population of the previous generation is used as the initial population of the next generation;

[0136] Mutation operation: Mutation(0.1*P t ), that is, a certain path is selected with a probability of 10%, and a section of the path is randomly selected for path reconstruction to form a new population member;

[0137] Crossover operation: P t =Replacement(P t ,P t-1 ), select the better 50% of the individuals in the population as parents, and randomly select a common node as the crossover site for segment exchange;

[0138] S47: Obtain the global path point sequence of the global suboptimal path and record it as

[0139] L global ={start,p1,p2,p3…p i ,distination}; where start represents the starting waypoint; distination represents the ending waypoint; p i represents the i-th path key point;

[0140] The global optimal path is obtained based on the global path point sequence of the global suboptimal path using the head-tail double pointer method;

[0141] That is, traverse any two non-adjacent path key points in the global path point sequence and determine whether these two non-adjacent path key points can be directly connected;

[0142] And the rule for judging whether any two non-adjacent path key points can be directly connected is

[0143] Check whether the line connecting any two non-adjacent path key points intersects with a static obstacle;

[0144] If they intersect, confirm that they cannot be directly connected;

[0145] If they do not intersect, confirm that they can be directly connected and delete other key path points between these two path key points to further shorten the global path length;

[0146] For example: If WP i .next→WP j ;

[0147] Then the global suboptimal path is optimized to obtain the global optimal path, and the key points of the path in the global optimal path are used as ship waypoints;

[0148] S5: Build a ship motion model and obtain the local optimal path according to the ship waypoints based on the improved dynamic window algorithm, thereby realizing the ship's path planning;

[0149] like Figure 4 As shown, the specific steps include:

[0150] S51: Consider dynamic obstacles during ship navigation to construct a ship motion model;

[0151] And the expression of the ship motion model is

[0152]

[0153] v=randn(1,0.2)*step,angle(t)=angle(t-1)+randn(ω),ω∈(-35°,+35°)

[0154]

[0155] Where: x(t n ),y(t n ) represents t n x(t0), y(t0) represent the ship's horizontal and vertical coordinates corresponding to the time t0; v(t) represents the ship's speed corresponding to the time t; v represents the abbreviated form of v(t) and v = randn(1,0.2)*step; randn(1,0.2) represents the decision variable that enables the ship's speed to be randomly selected in (0,step); step represents the ship's maximum step size, i.e., V max ; angle(t) represents the ship heading angle at time t; angle(t-1) represents the ship heading angle at time t-1; randn(ω) represents the random rudder angle; V a represents the braking limit speed space, i.e., the speed of the ship before the collision with the dynamic obstacle; ω represents the angular velocity of the ship; dist(v,ω) represents the closest distance between the simulated ship trajectory and the obstacle; represents the ship's linear acceleration; represents the angular acceleration of the ship; V d Indicates the acceleration change limit space; v a ,ω a Indicates the current linear velocity and angular velocity of the ship;

[0156] S52: traverse all ship waypoints in the order of the global optimal path;

[0157] Confirm the current ship position to the preset candidate waypoint WP according to the ship motion model n Are there any obstacles in the path?

[0158] If it does not exist, the candidate waypoint WP n As the target waypoint of the current segment, goal, that is, goal←WP n , and continue sailing in the current state;

[0159] If it exists, the optimal speed pair for the ship's navigation is obtained through the constructed improved evaluation function, and collision-free control of the local trajectory of the ship is achieved based on the optimal speed pair, thereby obtaining the local optimal path;

[0160] Implement ship path planning based on local optimal path;

[0161] Specifically, the expression of the constructed improved evaluation function is:

[0162] G(v t ,ω t )=K0(α*fitting(v t ,ω t )+β*velocity(v t ,ω t )+γ*obstacles(v t ,ω t )+δ*endreach(v t ,ω t )+ε*constrain(v t ,ω t ))

[0163] It includes the route fitting reward function, the ship speed reward function, the reward function of the distance between the ship and the obstacle, the reward function of predicting the visibility between the end of the ship path arc and the target point, and the ship oscillation constraint reward function;

[0164] The expression of the route fitting reward function in this embodiment is:

[0165]

[0166] θ=Course-arctan(P t ,P goal )

[0167] The route fitting reward function rewards the distance between the ship and the target waypoint, the direction of the target waypoint relative to the ship, and the angle between the ship's heading. The heading reward is related to the angle θ between the ship's heading and the target point relative to the ship's position. It increases as the angle decreases and takes a value between (0, 1). To evaluate the distance between the ship's position and the target point, it is necessary to evaluate the position of all time points of the predicted curvature arc (up to a fixed time of 3 seconds) and save the minimum Euclidean distance from this position to the target waypoint, so that the distance reward varies within (0, 1).

[0168] The expression of the ship speed reward function is:

[0169]

[0170] The ship speed reward function is used to reward the ship's sailing speed, thereby reducing the global time to reach the target. There are two scenarios for speed rewards: when the target is in front of or behind the robot (defined by the angular difference between the robot's position and the target's position). If the target is in front of the robot, that is, when the target is directly in front of the ship, that is, 0≤θ<π / 2, then the reward value of this item depends only on the linear speed and tends to reward high speed. The reward value is normalized between 0 and 1 and increases with increasing linear speed. If the target is behind the robot, that is, when the target is laterally behind the ship, that is, π / 2≤θ≤π, it is necessary to penalize high speed to reduce the speed of the ship and reduce the distance from the target point.

[0171] The reward function for the distance between the ship and the obstacle is expressed as

[0172]

[0173] This reward function is used to encourage the ship to stay away from obstacles when traveling. If the predicted path point is close to the obstacle cell, the reward value will decrease as it approaches the obstacle. In this embodiment, to prevent the fluctuation of the distance between the ship and the obstacle from being too large, a sigmoid activation function is used to make the reward value increase exponentially with increasing distance outside the restricted distance, so as to prevent the reward value from accounting for too large a proportion and weakening the influence of other factors. Within the restricted distance, due to the greater risk of collision, its weight needs to increase relatively quickly. Therefore, the reward value is made to decrease exponentially as the distance between the ship and the obstacle cell decreases, so as to encourage the predicted path point to stay away from the obstacle as much as possible.

[0174] The expression of the reward function for predicting the visibility of the end of the ship path arc and the target point is:

[0175] endreach(v t ,ω t )=C

[0176] In this embodiment, if the end of the predicted path line of the ship's trajectory is visible to the target, the reward value for this speed is increased by a constant value, so that the screening system prioritizes the predicted path that is reachable to the target point, prompting the ship to approach the target point;

[0177] The expression of the ship oscillation constraint reward function is:

[0178]

[0179] This reward function is used to control the stability of the ship's motion, increasing the likelihood of selecting a predicted path close to the current speed. By constraining the ship's speed, the ship's motion curve is smoothed. To be consistent with the speed reward, this embodiment also needs to be divided according to the position of the target point relative to the ship. When the target is located behind the positive beam, that is, when π / 2≤θ≤π, the reward change of the oscillation constraint needs to be weakened to prevent it from being contrary to the speed reward.

[0180] The symbols in the above formulas mean: G(v t ,ω t ) represents the improved evaluation function; K0 represents the smoothing function; α, β, γ, δ, ε represent the weight coefficients, which are [1, 0.5, 3, 1, 0.5] in this embodiment; fitting(v t ,ω t ) represents the route fitting reward function; Course represents the ship's heading; θ represents the angle between the ship's heading and the target point relative to the ship's position; arctan(P t ,P goal ) represents the direction of the target point relative to the own ship; P t Indicates the coordinate point of the ship's position; P traj Indicates the predicted position of the dynamic window; P goal Indicates the current target waypoint; dist(P traj ,P goal ) represents the distance between the predicted position of the dynamic window and the target waypoint; dist(P t ,P goal ) indicates the distance between own ship and target waypoint; velocity(v t ,ω t ) represents the ship speed reward function; K2, K1 represent gain constants; V max Indicates the maximum speed limit; K3 indicates the gain constant; obstacles (v t ,ω t) represents the reward function of the distance between the ship and the obstacle; limit represents the safe distance between the ship and the obstacle; d obs Indicates the distance between the ship and the obstacle; endreach(v t ,ω t ) represents the reward function for predicting the visibility of the end of the ship path arc and the target point; C represents a constant; constraint(v t ,ω t ) represents the ship oscillation constraint reward function.

[0181] Compared with existing technologies, Figures 5 to 7 As shown, the beneficial effects of the method described in this embodiment are as follows:

[0182] (1) Global efficiency improvement: By obtaining the adjacency distance matrix of the key points of the path, the edge set of the Voronoi diagram is defined according to the adjacency distance matrix to obtain the initial Voronoi diagram, and the edge set of the initial Voronoi diagram is traversed and the edges intersecting with the static obstacles are deleted to correct the initial Voronoi diagram to obtain the modified Voronoi diagram; the search for redundant paths in the Voronoi diagram is reduced; the global optimal path is obtained according to the modified Voronoi diagram based on the ant colony algorithm and the genetic algorithm, which greatly improves the global search efficiency of path planning; the method of this embodiment shortens the path length by 18.5% and the search time by 45.7% compared with the traditional A* method;

[0183] (2) Dynamic obstacle avoidance optimization: A ship motion model is constructed, and a local optimal path is obtained according to the ship waypoints based on an improved dynamic window algorithm. The optimal speed pair for the ship's navigation is obtained through an improved DWA algorithm, that is, a multi-objective evaluation function that includes ship oscillation suppression (i.e., an improved evaluation function). Based on the optimal speed pair, a collision-free control path for the local trajectory of the ship is achieved, and the linear speed fluctuation is reduced by 44.2%, greatly improving the smoothness of the local path planning.

[0184] (3) Enhanced environmental adaptability: Supports real-time obstacle avoidance for dynamic and unknown obstacles, significantly improving the path success rate in complex environments by 30%;

[0185] (4) System robustness: Global path planning and local path planning work together to ensure that the ship can quickly return to the optimal path after obstacle avoidance and avoid local oscillations.

[0186] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it. Although the present invention has been described in detail with reference to the above embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the above embodiments, or replace some or all of the technical features therein with equivalents. However, these modifications or replacements do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of the present invention.

Claims

1. A path planning method for dynamic windows guided by ant colony optimization Voronoi diagram, characterized in that: The specific steps include: S1: Pre-build an environment grid map containing known static obstacles for path planning; S2: Obtain the cell map around the static obstacle based on the environment grid map; And obtain the key point set of the initial global guidance path according to the cell diagram; S3: Calculate the Euclidean distance between any two adjacent points in the key point set, obtain the adjacency distance matrix of the path key points, define the edge set of the Voronoi graph based on the adjacency distance matrix, and then obtain the initial Voronoi graph; Traverse the edge set of the initial Voronoi diagram, delete the edges intersecting with the static obstacles to correct the initial Voronoi diagram, and obtain the corrected Voronoi diagram; S4: Based on the ant colony algorithm and genetic algorithm, the global optimal path is obtained according to the modified Voronoi diagram, and the key points of the path in the global optimal path are used as the ship waypoints; S5: Construct a ship motion model and obtain the local optimal path according to the ship waypoints based on the improved dynamic window algorithm to achieve ship path planning.

2. The path planning method of a dynamic window guided by an ant colony optimization Voronoi diagram according to claim 1, characterized in that: The S2 specifically includes the following steps: S21: Mark the static obstacles in the environment grid map to obtain obstacle marking points, and use the obstacle marking points as the initialization scattered point set constituting the Voronoi diagram; S22: Based on the Fortune algorithm and the initial scattered point set, a cell diagram around the static obstacle is obtained, and the union of all the boundaries of the Voronoi diagram region is obtained based on the cell diagram; And the expression of the union of the boundaries of the Voronoi graph region is V F (p)={q:d(q,p)≤d(q,F / {p}))} Where: F represents the boundary point set of the cell graph surrounding the static obstacle; V F (p) represents the neighborhood of the boundary point p; q,p represent the boundary points of the cell diagram surrounding the static obstacle; d(q,p) represents the Euclidean distance between the boundary points q,p; d(q,F / {p})) represents the Euclidean distance between the boundary point q and the other boundary points in the boundary point set except the boundary point p; VD(F) represents the union of the boundaries of the region constituting the Voronoi diagram; S23: Obtain a set of key points of the initial global guidance path of the ship according to the union of all the region boundaries constituting the Voronoi diagram; And the expression of the key point set is I=voronoi(O) Where I represents the key point set of the initial global guidance path of the ship; voronoi(O) represents the intersection set of boundary points in the union VD(F) of all the boundary points that constitute the Voronoi diagram region.

3. The path planning method of a dynamic window guided by an ant colony optimization Voronoi diagram according to claim 2, characterized in that: The formula for obtaining the modified Voronoi diagram in S3 is: Where: isIntersect(edge i ,obs j ) represents a piecewise function that determines whether the edge of the initial Voronoi diagram intersects with the static obstacle; edge i Represents the key point P(x n ,y n ) and Q(x m ,y m ) The edges of the initial Voronoi graph determined by obs j Indicates the position of the static obstacle and obs j (x i ,y i )∈O; d ij Indicates the position of the static obstacle obs j Edge of the initial Voronoi diagram i The distance between them.

4. The path planning method of a dynamic window guided by an ant colony optimization Voronoi diagram according to claim 3, characterized in that: The S4 specifically includes the following steps: S41: Setting the starting waypoint and the ending waypoint of the path planning according to the modified Voronoi diagram; Set the number of ant individuals m in the ant colony algorithm, randomly set the starting point of the ant individuals and let each ant individual start searching from the key point on the path closest to the starting point; And according to the state transition probability, the search ends with the path key point closest to the end waypoint as the end point, and the expression of the state transition probability is Where: represents the probability that the ant individual k moves from the i-th path key point to the j-th path key point in the t-th cycle; η ij (t) represents the expected heuristic of each ant individual moving from the i-th path key point to the j-th path key point in the t-th cycle, and η ij (t) = 1 / d′ ij +Crandom, Crandom indicates mutation factor; d ij ′ represents the distance between the i-th path key point and the j-th path key point; τ is (t) represents the pheromone concentration of the distance between the i-th path key point and the s-th path key point at the t-th iteration; η js (t) represents the expected heuristic for each ant individual to move from the jth path key point to the sth path key point in the tth cycle, and η js (t) = 1 / d′ js +Crandom; α represents the pheromone importance factor; β represents the heuristic information importance factor; a k represents the set of key points on the path that ant individual k is allowed to visit next; τ ij (t) represents the pheromone concentration of the path between the i-th path key point and the j-th path key point in the t-th cycle; S42: When the ant individual traverses all key points on the path and completes one iteration; Then update the pheromone concentration of the path between the i-th path key point and the j-th path key point. The update rule formula of the pheromone concentration is: Where: The pheromone concentration of the path between the i-th path key point and the j-th path key point in the t+1th cycle; ρ represents the pheromone evaporation rate and ρ∈(0,1); Indicates the pheromone change value; Q indicates the pheromone increase intensity; T represents the length of the path that ant individual k has traveled at time t; k represents the path chosen by ant individual k during the search process; S43: According to the pheromone concentration update rule, iterate the ant colony algorithm until the pheromone concentration converges to a preset pheromone threshold to obtain a search path; S44: Arrange the search paths corresponding to all acquired ant individuals in descending order according to the total distance of the paths to obtain a search path sorting table; According to the search path sorting table, the top A% search paths are obtained and used as the initial population P0 of the genetic algorithm. N = 0.2*num_ants, where Indicates the search path corresponding to the Nth ant individual; num_ants represents the total number of ant individuals in the ant colony algorithm; S45: Construct the fitness function of the genetic algorithm; And the expression of the fitness function is Where: represents the fitness value of the i-th search path at the t-th iteration; The total distance of the i-th search path at iteration t; Obtaining the fitness values of the population individuals of the genetic algorithm at the time of t iterations according to the fitness function, and arranging the fitness values of the population individuals in descending order to obtain a current population individual sequence table; S46: Based on the current population individual sequence table, the genetic algorithm is used to repeatedly perform selection operations, crossover operations, and mutation operations to improve the fitness of the genetic algorithm so that the population individuals gradually approach the optimal solution, and finally output the global optimal solution when the preset maximum number of iterations is reached; And the global optimal solution is output as the global suboptimal path; S47: Obtain the global path point sequence of the global suboptimal path and record it as L global ={start,p1,p2,p3…p i ,distination}; where start represents the starting waypoint; distination represents the ending waypoint; p i represents the i-th path key point; The global optimal path is obtained based on the global path point sequence of the global suboptimal path using the head-tail double pointer method; That is, traverse any two non-adjacent path key points in the global path point sequence and determine whether these two non-adjacent path key points can be directly connected; And the rule for judging whether any two non-adjacent path key points can be directly connected is Check whether the line connecting any two non-adjacent path key points intersects with a static obstacle; If they intersect, confirm that they cannot be directly connected; If they do not intersect, confirm that they can be directly connected and delete other key path points between these two key path points; Then the global suboptimal path is optimized to obtain the global optimal path, and the key points of the path in the global optimal path are used as ship waypoints.

5. The path planning method of a dynamic window guided by an ant colony optimization Voronoi diagram according to claim 4, characterized in that: The S5 specifically includes the following steps: S51: Consider dynamic obstacles during ship navigation to construct a ship motion model; And the expression of the ship motion model is v=randn(1,0.2)*step,angle(t)=angle(t-1)+randn(ω),ω∈(-35°,+35°) Where: x(t n ),y(t n ) represents t n x(t0), y(t0) represent the ship's horizontal and vertical coordinates corresponding to the time t0; v(t) represents the ship's speed corresponding to the time t; v represents the abbreviated form of v(t) and v = randn(1,0.2)*step; randn(1,0.2) represents the decision variable that enables the ship's speed to be randomly selected in (0,step); step represents the ship's maximum step size, i.e., V max ; angle(t) represents the ship heading angle at time t; angle(t-1) represents the ship heading angle at time t-1; randn(ω) represents the random rudder angle; V a represents the braking limit speed space, i.e., the speed of the ship before the collision with the dynamic obstacle; ω represents the angular velocity of the ship; dist(v,ω) represents the closest distance between the simulated ship trajectory and the obstacle; represents the ship's linear acceleration; represents the angular acceleration of the ship; V d Indicates the acceleration change limit space; v a ,ω a Indicates the current linear velocity and angular velocity of the ship; S52: traverse all ship waypoints in the order of the global optimal path; Confirm the current ship position to the preset candidate waypoint WP according to the ship motion model n Are there any obstacles in the path? If it does not exist, the candidate waypoint WP n Use it as the target waypoint for the current leg and continue sailing; If it exists, the optimal speed pair for the ship's navigation is obtained through the constructed improved evaluation function, and collision-free control of the local trajectory of the ship is achieved based on the optimal speed pair, thereby obtaining the local optimal path; The ship's path planning is achieved based on the local optimal path.

6. The path planning method of a dynamic window guided by an ant colony optimization Voronoi diagram according to claim 5, characterized in that: The expression of the improved evaluation function constructed in S52 is: G(v t ,oh t )=K0(α*fitting(v t ,oh t )+β*velocity(v t ,oh t )+γ*obstacles(v t ,oh t )+δ*endreach(v t ,oh t )+ε*constrain(v t ,oh t )) θ=Course-arctan(P t ,P goal ) endreach(v t ,ω t )=C Where: G(v t ,ω t ) represents the improved evaluation function; K0 represents the smoothing function; α, β, γ, δ, ε represent the weight coefficients; fitting(v t ,ω t ) represents the route fitting reward function; Course represents the ship's heading; θ represents the angle between the ship's heading and the target point relative to the ship's position; arctan(P t ,P goal ) represents the direction of the target point relative to the own ship; P t Indicates the coordinate point of the ship's position; P traj Indicates the predicted position of the dynamic window; P goal Indicates the current target waypoint; dist(P traj ,P goal ) represents the distance between the predicted position of the dynamic window and the target waypoint; dist(P t ,P goal ) indicates the distance between own ship and target waypoint; velocity(v t ,ω t ) represents the ship speed reward function; K2, K1 represent gain constants; V max Indicates the maximum speed limit; K3 indicates the gain constant; obstacles (v t ,ω t ) represents the reward function of the distance between the ship and the obstacle; limit represents the safe distance between the ship and the obstacle; d obs Indicates the distance between the ship and the obstacle; endreach(v t ,ω t ) represents the reward function for predicting the visibility of the end of the ship path arc and the target point; C represents a constant; constraint(v t ,ω t ) represents the ship oscillation constraint reward function.

Images