Mobile robot path planning method based on egret group optimization

Abstract

The invention provides a mobile robot path planning method based on egret group optimization, and the method comprises the steps: carrying out the population initialization through Tent chaotic mapping in an exploration stage, and reducing the path search cost; a sine and cosine algorithm and a greedy strategy are adopted to improve the position updating of the egret individuals so as to balance the local development and global search capability of the method; a Levy flight method is adopted to escape from a local optimal trap; and a safe and reliable planning path is obtained by using an area limit value method and a feasible point coordinate fine adjustment strategy. And in the optimization stage, a shortest path method, redundancy removal operation and a segmented Bezier curve method are adopted to optimize the trajectory, so that the final motion trajectory of the mobile robot is obtained. The mobile robot path planning method based on egret group optimization has the advantages of being high in path planning efficiency, short in total consumed time, better in path, and capable of reducing the number of path turning times, reducing the number of path nodes and enhancing the local optimal escape ability.

Description

Technical Field

[0001] The present invention belongs to the field of path planning of mobile robots, and relates to Tent chaotic mapping, Levy flight method, regional limit method, feasible point coordinate fine-tuning strategy, and egret flock optimization method, and is applicable to path planning of mobile robots. Background Art

[0002] When the existing swarm intelligence global path planning method plans a path, it generally performs path planning based on a traditional grid map. Although the path planning method based on the traditional grid map has the advantages of simple principle and easy implementation; however, the path generated by the path planning method based on the traditional grid map is usually discrete, consisting of straight line segments and corners, resulting in a significant increase in the path length planned by the existing swarm intelligence global path planning method, and because the grid-based path planning method has low computational efficiency, it is difficult to apply it to large-scale maps or complex environments.

[0003] Therefore, a path search method based on the egret flock optimization method is proposed. In the egret flock optimization method, three egrets in the egret team respectively adopt corresponding improved position update strategies to obtain the next position update matrix of the egret team. The method is continuously iterated until the path end point is searched and ends to obtain the original search path. During the path search process of the egret flock optimization method, since it does not rely on a grid map, the search path length is shorter, and during the search process, due to the change of the position update strategy, the path search efficiency of the robot is improved.

[0004] When the existing swarm intelligence global path planning method searches for a path, it usually depends on the balance between global exploration and local exploitation. However, if the method tends to focus on local optimal regions for exploitation too early and ignores the exploration of other regions, it is easy to fall into local optima.

[0005] Therefore, a local trap escape method based on Levy flight is proposed. Levy flight has the characteristics of moving in an arbitrary direction for an arbitrary distance irregularly, with both short-step movements and long-step flights, which expands the search range of feasible solutions. Therefore, for the elements in the current position update matrix of the egret team that do not pass the collision test, after adopting Levy flight, they can pass the collision test and search for feasible solutions.

[0006] In addition, after the method search stage of the existing swarm intelligence global path planning method ends, there will be a situation where some path point coordinates in the obtained original point path matrix are too close to obstacles, thus affecting the path safety of the robot.

[0007] Therefore, a feasible point coordinate fine-tuning strategy is proposed. During the method search process, for the coordinates of the feasible path points obtained by the search, according to the given detection threshold b, within the threshold range, with the current point as the center, it is detected whether there are obstacles in the surrounding 8 directions. If there is an obstacle in a certain direction, a fallback strategy is adopted for the current point coordinates in that direction, and the fallback step length is a, to ensure that there is an appropriate free space between the current position and the obstacle. Summary of the Invention

[0008] A mobile robot path planning method based on egret swarm optimization, characterized in that it combines Tent chaotic mapping, Levy flight method, regional limit method, feasible point coordinate fine-tuning strategy and egret swarm optimization method for mobile robot path planning, which can improve the path planning efficiency, shorten the planning time, reduce the number of path turns and the number of path nodes, optimize the path of the mobile robot, and reduce the length of the global planning path of the robot; the steps in the use process are as follows:

[0009] Step (1): Build a map environment and read the global environment information;

[0010] Step (2): Initialize the egret population using Tent chaotic mapping to obtain a chaotic initial solution sequence; the determination method of the chaotic initial solution sequence is, first construct a chaotic initial solution sequence all-zero matrix, and then generate a Tent chaotic mapping random number sequence according to formula (1); on this basis, map the generated Tent chaotic mapping random number sequence to the map boundary according to the map range; and store the points mapped to the map boundary into the chaotic initial solution sequence.

[0011]

[0012] where x is a random number in the closed interval from 0 to 1, and x tent is a chaotic mapping random number;

[0013] Step (3): Set the maximum number of iterations N, the current number of iterations iteration, and initialize the current number of iterations iteration to 1;

[0014] Step (4): According to the initialized coordinate matrix of egret A, use the improved waiting strategy to calculate the position update matrix of egret A; the improved waiting strategy is that let the position coordinate vector of the i-th egret team be x i ∈R n , where n is the dimension of the path planning task, F(*) is the estimate of egret A for the existence of prey at its current position, is the estimated value of egret A for prey at the current position, then the formula for the estimated value of egret A for prey at the current position is:

[0015]

[0016] The weight w of the Egrets A estimation method i ∈R n Parameterize Equation (2) into Equation (3), and obtain the error θ from Equation (4) i , the error θ i is as follows

[0017]

[0018] where f i is the fitness value of the i-th Egrets team at the position x i ; the fitness function f is defined as the Euclidean metric from the current position point = (ε 1 , ξ 1 ) to the target position goal = (ε 2 , ξ 2 ), and the Euclidean metric formula is

[0019]

[0020] The actual gradient vector can be obtained by taking the partial derivative of w i , is its direction vector, and the expressions are respectively

[0021]

[0022] During the predation process, Egrets will show a following behavior, that is, combining their own experience with the experience of the stronger members in the population's predation ability to correct the optimal position direction vector k of the current Egrets team h,i ∈R n and the global optimal position direction vector k of the Egrets population g,i ∈R n ;

[0023]

[0024] where x ibest is the historical optimal position of the current Egrets team; x gbest is the global optimal position of the Egrets population; f ibest , f gbest , f i are the corresponding fitness values; k ibest , k gbest are the historical optimal direction vector of the current Egrets team and the global optimal direction vector of the Egrets population respectively;

[0025] In the waiting strategy for the position update of the Egrets group optimization method Egrets A, l h and lg If the value is not selected properly, the method will converge prematurely and the global search ability of the method will deteriorate; combining the sine-cosine algorithm with the position update method of Egret A to obtain the improved integral gradient g i , improving the global search ability of the method; if r q < 0.5, the improved integral gradient g i As shown in Equation (10), otherwise as shown in Equation (11), the integral gradient g i ∈R n is described as:

[0026]

[0027] where l h ∈ [0, 0.5), l g ∈ [0, 0.5), r q is a uniformly distributed random number in the interval (0, 1), r m = θ(1 - t / t max ), r n ∈ [0, π / 2]; θ is a constant, sin() is the sine function, cos() is the cosine function, t and t max correspond to the current iteration number and the maximum iteration number respectively, t max = N;

[0028] The adaptive weight update is described as:

[0029]

[0030] where: λ 1 , λ 2 are constants; q i and u i are moment estimators, both initialized as all-zero matrices; according to the improved integral gradient g i and the judgment of Egret A on the current situation, its foraging behavior is described as:

[0031]

[0032] f a,i = f(x a,i ) (14)

[0033] where x a,i is the position update matrix of Egret A, exp() is the exponential function, step a is the step factor of Egret A; t and t max correspond to the current iteration number and the maximum iteration number respectively, t max = N; is the difference matrix between the starting point and the ending point of the map; f a,iis x a,i 's fitness value;

[0034] Step (5): According to the initialized coordinate matrix of Egret B, use the improved attack strategy to calculate the position update matrix of Egret B; the improved attack strategy is to improve the position update method of Egret B using the greedy strategy, so that the position update x of Egret B b,i always performs a random walk near the optimal value to improve the search efficiency of Egret B; if x ibest < x gbest , the improved position update strategy of Egret B is shown in Equation (15), otherwise as shown in Equation (16);

[0035]

[0036]

[0037] f b,i = f(x b,i ) (17)

[0038] where step b is the step factor of Egret b; l b,i ∈(-π / 2,π / 2); f b,i is the fitness value of x b,i , and tan() is the tangent function;

[0039] Step (6): According to the initialized coordinate matrix of Egret C, use the attack strategy of the original method to calculate the position update matrix of Egret C; the attack strategy of the original method is described as:

[0040]

[0041] where A h is the difference matrix between the historical optimal position and the current position of the Egret team; and A g is the difference matrix between the global optimal position of the Egret population and the current position; x c,i is the next position update of Egret C, and l i is a random number in the interval [0,0.5);

[0042] Step (7): After the position updates of the 3 egrets in the Egret team are completed, combine to obtain x s,i , f s,i ;

[0043] x s,i = [x a,i x b,i x c,i (20)

[0044] f s,i= [f a,i f b,i f c,i (21)

[0045] where x s,i is the position update matrix of the i-th Little Egret team; f s,i is the fitness value vector of x s,i .

[0046] Step (8): Conduct a collision test on the current position update matrix of the Little Egret team; the collision test is to traverse each element in the position update matrix of the Little Egret team. If each element is not on an obstacle and the connection line between each element and the corresponding element of the position update matrix generated in the previous iteration does not cross an obstacle, then the current position update matrix of the Little Egret team passes the collision test;

[0047] Step (9): For the elements of the current position update matrix of the Little Egret team that do not pass the collision test, use Lévy flight to make them pass the collision test;

[0048] Step (10): Calculate the Lévy flight random step size s using formula (22);

[0049]

[0050] where μ and ν are normally distributed random numbers, and μ ~ (0, σ 2 ), ν ~ (0, 1), β is the Lévy flight stability parameter, and σ is the Lévy flight standard deviation; Step (11): Calculate the Lévy flight standard deviation σ using formula (23); the Lévy flight standard deviation σ is described as:

[0051]

[0052] where Γ(*) is the gamma function, sin() is the sine function, and π is the pi;

[0053] Step (12): According to formula (24), calculate the coordinates x levy of the feasible point generated after Lévy flight and add it to the position update matrix x s,i ;

[0054] x levy = x′ + α·s (24)

[0055] where x' is the coordinate of the element in the current position update matrix x s,i that does not pass the collision test, and α is the Lévy flight step factor;

[0056] Step (13): For the position update matrix x s,iPerform regional limit; the regional limit is that in the iterative process of the method, 3 egrets in the team can only search within the area where the vertical distance from the starting point to the target point is δ, and the position update matrix elements that do not meet the range of the limit threshold δ are excluded, so as to improve the quality of the planned path and reduce the path search cost;

[0057] Step (14): According to the feasible point coordinate fine-tuning strategy, fine-tune the position update matrix x after regional limit s,i Perform coordinate fine-tuning: The feasible point coordinate fine-tuning strategy is that after the position update of the egret team is completed, the generated egret positions may be too close to obstacles, which will affect the path safety of the robot. Therefore, inspired by the parent and child nodes of the A* method, a feasible point coordinate fine-tuning strategy is proposed, that is, according to the given detection threshold b, within the threshold range, with the current point as the center, detect whether there are obstacles in its surrounding 8 directions. If there is an obstacle in a certain direction, the current point coordinates will be retreated in that direction, and the retreat step length is a, to ensure that there is an appropriate free space between the current position and the obstacle; Point is an element of the position update matrix x s,i For a certain element of Point, the first child node node1 of Point detects that there is an obstacle on its right within the detection range defined by the detection threshold b, and the Point point will move to the left with the retreat step length a to ensure that there is a certain free space between it and the obstacle. The processing methods for the remaining 7 directions are similar to the situation where node1 detects an obstacle within the threshold range;

[0058] Step (15): Find the position vector m corresponding to the optimal fitness value i ; m i is:

[0059] m i = argmin(f s,i ) (25)

[0060] where argmin() is a function that returns the corresponding independent variable when f s,i obtains the minimum value;

[0061] Step (16): Update the current position of the egret team according to the discriminant condition; the discriminant condition is described as:

[0062]

[0063] where value is a constant; if the minimum value of f s,i is less than the current fitness value f i , then the egret team will adopt the coordinate vector m corresponding to the minimum value of f s,i ; iPerform position update; or if the random number l ∈ (0, 1) is less than value, the Little Egret team will also update the current position, and the position update at this time aims to enrich the solution space of the method and prevent the method from falling into the local optimal trap;

[0064] Step (17): Perform a collision test on the updated matrix x of the final position of the currently generated Little Egret team i Perform a collision test;

[0065] Step (18): Add the x that passes the collision test i to the original path point set Path, iteration = iteration + 1;

[0066] Step (19): If iteration exceeds the maximum number of iterations N, prune the original path point set Path using the shortest path method to obtain the pruned matrix Path'; the basic principle is to start from the starting point, successively connect the subsequent path points to the starting point until the connection between the starting point and a certain subsequent path point crosses an obstacle, then return to the upper node whose connection to the starting point does not cross the obstacle, and at the same time remove the redundant nodes between the starting point and the upper node and use the connection between the two as the pruned path. After the path pruning between the starting point and this point is completed, this point is used as the new starting point to continue the pruning operation until the end of the path is traversed;

[0067] Step (20): According to the path point matrix Path' after pruning the original path point set Path, perform a redundancy removal operation to remove the meaningless nodes in the pruned path point matrix to obtain the redundancy-removed path point matrix Path”; the redundancy removal operation is to start from the first element in the pruned path point matrix Path', take three consecutive path nodes as a group, traverse the entire path point matrix, and if the nodes in the same group are all on the same straight line, remove the middle node to achieve the redundancy removal operation;

[0068] Step (21): Connect the elements in the redundancy-removed path point matrix Path” in order to form the original path, and use the piecewise Bezier curve method to smooth the original path; calculate the nth-order Bezier curve using formula (27). Since the high-order Bezier curve has poor robustness and cannot effectively utilize the path point information and is difficult to effectively optimize the pruned path, the second-order Bezier curve is used for trajectory optimization, and the expression of the second-order Bezier curve is shown in formula (28);

[0069]

[0070] B 2 (τ) = (1 - τ 2 )B 0 + 2τ(1 - τ)B1 +τ 2 B 2 (28)

[0071] Among them, B 0 , B 1 , … B n , are the control points of the Bessel curve, τ∈[0,1] is the curve curvature control parameter, i represents the i-th control point, and n represents the order of the Bessel curve;

[0072] The piecewise Bessel curve method is to ensure the continuity of the trajectory curvature after optimization by the second-order Bessel curve. An improved method of the Bessel curve is used for trajectory optimization. Three consecutive elements B k , B k+1 , B k+2 are taken from the matrix Path” of the path points after removing redundancy, starting from the starting point. And the midpoints of B k+1 and B k , B k+1 , B k+1 , B k+2 are used as control points to generate a Bessel curve. If the curve crosses an obstacle, the corresponding midpoints of the control points are continuously taken to generate a Bessel curve. The above operations are repeated until the optimized curve does not cross the obstacle, and finally a smooth and continuous robot motion trajectory is formed;

[0073] Step (22): Output the final path planning result according to the path optimized by the piecewise Bessel curve method and return it;

[0074] The present invention has the following advantages and effects compared with the prior art:

[0075] (1) The present invention proposes a path search method based on the heron flock optimization method, which increases the universality of the global path planning method for the map environment and takes into account the requirements of improving the global path planning efficiency and shortening the planned path length.

[0076] (2) The present invention proposes a local trap escape method based on Lévy flight, which enables the path search method based on the heron flock optimization method to effectively escape local traps and is not easily trapped in local optima.

[0077] (3) The present invention proposes a feasible point coordinate fine-tuning strategy, which leaves a certain free space between the feasible path points obtained by the heron flock optimization method and the obstacles, improving the safety and feasibility of the planned path.

[0078] (4) The present invention proposes a regional limit method, which limits the randomness of the path search method in the path search stage, improves the quality of the planned path, and reduces the path search cost.

[0079] (5) The present invention improves the position update strategies of Egrets A and B in the egret swarm optimization method to solve the problems of premature convergence of the path search method and poor global search ability.

[0080] (6) The present invention proposes a piecewise Bezier curve method to perform piecewise smoothing on the searched path, so that the optimized path meets the kinematic and dynamic constraints of the robot. Description of the Drawings

[0081] Figure 1 is the overall framework diagram of the method of the present invention.

[0082] Figure 2 is the specific flowchart of the search stage of the egret swarm optimization method.

[0083] Figure 3 is the specific schematic diagram of the regional limit method.

[0084] Figure 4 is the fine-tuning strategy for the coordinates of the feasible points when the obstacle appears on the right side of the feasible path point.

[0085] Figure 5 is the shortest path method for pruning the original path point matrix.

[0086] Figure 6 is the redundancy removal operation for eliminating meaningless nodes in the path point matrix after pruning.

[0087] Figure 7 is the piecewise Bezier curve method for optimizing the path. Detailed Embodiment

[0088] A mobile robot path planning method based on egret swarm optimization proposed by the present invention is described in detail with reference to the accompanying drawings as follows:

[0089] Figure 1It is the overall framework diagram of the method of the present invention. First, build a map environment, initialize the egret population according to the Tent chaotic mapping, and determine whether the current iteration number is less than the maximum iteration number N. If not, update the positions of all egret squads synchronously. Otherwise, the search stage of the path planning method ends and enters the optimization stage. Conduct a collision test on the updated position coordinates. If the collision test is passed, perform regional limitation on the updated position coordinates. If not, make the path search method escape from the local optimal trap according to Lévy flight. Then, execute the discriminant condition. If the discriminant condition is passed, update the global optimal position x_g_best. If not, x_g_best remains unchanged. Conduct a collision test on x_g_best. If passed, add it to the path point set Path, and continue to determine whether the current iteration number is less than the maximum iteration number N. If not passed, continue to execute the Lévy flight method for position update. Finally, use the shortest path method to prune the generated path point set Path and perform redundancy removal operations, and use the piecewise Bézier curve method to optimize the pruned path, output the optimized path, and end the process.

[0090] Figure 2 It is the specific flowchart of the search stage of the egret swarm optimization method. First, the search stage of the egret swarm optimization method consists of three parts: the waiting strategy, the offensive strategy, and the discriminant condition. Then, during the solution process of the egret swarm optimization method, each egret squad consists of three egrets in total. Egret A adopts the waiting strategy, that is, guiding the advance. Both Egret B and Egret C adopt the offensive strategy, that is, using random walk and the surrounding mechanism. Finally, after the three egrets in the egret squad adopt the corresponding strategies to obtain their respective next position updates, execute the discriminant condition to determine whether the global optimal position is updated or remains unchanged.

[0091] Figure 3 It is the specific schematic diagram of the regional limitation method. First, to limit the randomness introduced by the position update method of the three egrets in the egret squad during the search process of the egret swarm optimization method, delimit a limitation area, that is, the symmetric area between the starting point and the ending point connection. Then, set the limitation threshold δ according to the map size. Finally, during the iteration process of the path search method, the three egrets in the egret squad can only search within the gray area with a vertical distance of δ from the connection line between the starting point and the target point, and delete the path points beyond this area.

[0092] Figure 4It is a feasible point coordinate fine-tuning strategy when an obstacle appears on the right side of a feasible path point. First, set the detection threshold b. Then, within the threshold range, with the current point as the center, detect whether there are obstacles in its surrounding 8 directions. Finally, if there is an obstacle in a certain direction, a fallback strategy will be adopted for the current point coordinates in that direction, and the fallback step length is a, to ensure that there is an appropriate free space between the current position and the obstacle. If the first child node node1 of Point is detected to have an obstacle on its right side within the detection range defined by the detection threshold b, as shown by the dashed box in Figure 4 Point will move to the left with the fallback step length a to ensure that there is a certain free space between it and the obstacle. The processing methods for the remaining 7 directions are similar to the case where node1 detects an obstacle within the threshold range.

[0093] Figure 5 It is the shortest path method for pruning the original path point matrix. First, starting from the starting point P 1 connect the subsequent path points to the starting point P 1 in sequence until the connection line between the starting point and a subsequent path point P 5 crosses an obstacle. Then, return to the upper-level node P 4 whose connection line to the starting point does not cross the obstacle, and at the same time remove the redundant nodes P 1 between the starting point P 4 and the upper-level node P 2 、P 3 and use the connection line between them as the pruned path. Finally, after the path pruning between P 1 P 4 is completed, P 4 is used as the new starting point to continue the pruning operation until the path end point is traversed and the operation ends.

[0094] Figure 6 It is a redundancy removal operation to remove meaningless nodes in the pruned path point matrix. First, after the pruning operation, it is necessary to remove the meaningless nodes in the pruned path. Then, take 3 consecutive path nodes as a group and traverse the entire path point matrix. Finally, if the nodes in the same group are all on the same straight line, remove the middle node, as shown by R Figure 6 、R 2 、R 3 、R 5 in

[0095] Figure 7It is a piecewise Bezier curve method for optimizing paths. First, since high-order Bezier curves have poor robustness and cannot effectively utilize path point information, it is difficult to effectively optimize the pruned path. Therefore, a second-order Bezier curve is used for trajectory optimization. Then, to ensure the continuity of the curvature of the trajectory after optimization by the second-order Bezier curve, the Bezier curve is improved, that is, the midpoints of points B 2 and B 1 , B 2 and B 2 , B 3 are taken as control points to generate Bezier curve C 1 C 2 , as shown in Figure 7 . Finally, if the curve crosses an obstacle, continue to take the midpoint to generate curve C' 1 C' 2 , and repeat the above operations until the optimized curve does not cross the obstacle, and finally connect to form a smooth and continuous robot motion trajectory.

[0096] The above is only the preferred embodiment of the present invention, and does not limit the patent scope of the present invention. Any equivalent structure or equivalent process transformation made by using the description and drawings of the present invention, or directly or indirectly applied in other related technical fields, shall be equally included in the patent protection scope of the present invention.

Claims

1. A mobile robot path planning method based on egret swarm optimization, characterized in that: Combining Tent chaos mapping, Levy flight method, regional limit method, feasible point coordinate fine-tuning strategy and egret swarm optimization method for mobile robot path planning can improve path planning efficiency, shorten planning time, reduce the number of path turns and path nodes, optimize the path of the mobile robot, and reduce the length of the robot's global planning path; the steps in the use process are: Step (1): Build a map environment and read global environment information; Step (2): Initialize the egret population using Tent chaotic mapping to obtain a chaotic initial solution sequence; The method for determining the chaotic initial solution sequence is to first construct a zero matrix of the chaotic initial solution sequence, and then generate a Tent chaotic mapping random number sequence according to formula (1); on this basis, the generated Tent chaotic mapping random number sequence is mapped to the map boundary according to the map range; and the points mapped to the map boundary are stored in the chaotic initial solution sequence; Where x is a random number between 0 and 1. tent is the random number of chaos mapping; Step (3): Set the maximum number of iterations N, the current number of iterations iteration, and initialize the current number of iterations iteration to 1; Step (4): Based on the initialization coordinate matrix of Egret A, use the improved sit-and-wait strategy to calculate the position update matrix of Egret A; the improved sit-and-wait strategy is to set the position coordinate vector of the i-th Egret team as x i ∈R n , where n is the dimension of the path planning task, F(*) is the estimation of egret A that there is prey at its current location, is the estimated value of egret A to the prey at the current position, then the estimated value of egret A to the prey at the current position is: The weight w of the Egret A estimation method i ∈R n Parameterize equation (2) into equation (3), and use equation (4) to obtain the error θ i , error θ i for: Among them, f i For the i-th Egret team at position x i The fitness value at the point; the fitness function f is defined as the Euclidean metric from the current position point = (ε1, ξ1) to the target position goal = (ε2, ξ2), and the Euclidean metric formula is: Actual gradient vector Can be achieved through i Taking the partial derivative we get, is its direction vector, The expressions are: The egrets will follow the predation process, that is, combining their own experience with the experience of those with stronger predation ability in the population to correct the optimal position direction vector k of the current egret team. h,i ∈R n and the global optimal position direction vector k of the egret population g,i ∈R n ; Among them, x ibest The best position in the history of the current Egret team; x gbest is the global optimal position of the egret population; f ibest 、f gbest 、f i are the corresponding fitness values ​​respectively; k ibest , k gbest They are the historical optimal direction vector of the current egret team and the global optimal direction vector of the egret population respectively; In the wait-and-see strategy for egret A’s position update in the egret group optimization method, l h and l g Improper selection of the value of will cause the method to converge prematurely and deteriorate the global search capability of the method. Combining the sine-cosine algorithm with the position update method of Egret A, an improved integral gradient g is obtained. i , improve the global search ability of the method; if r q <0.5, improved integrated gradient g i As shown in formula (10), conversely, as shown in formula (11), the integral gradient g i ∈R n Described as: Among them, l h ∈[0,0.5), l g ∈[0,0.5), r q is a uniformly distributed random number in the interval (0,1), r m =θ(1-t / t max ), r n ∈[0,π / 2]; θ is a constant, sin() is the sine function, cos() is the cosine function, t and t max Corresponding to the current number of iterations and the maximum number of iterations, t max =N; The adaptive weight update is described as: Among them: λ1, λ2 are constants; q i and u i is the moment estimator, and the initialization is all zero matrices; according to the improved integral gradient g i And Egret A's judgment of the current situation, its predation behavior is described as: f a,i =f(x a,i ) (14) Among them, x a,i is the position update matrix of egret A, exp() is the exponential function, step a is the step size factor of Egret A; t and t max Corresponding to the current number of iterations and the maximum number of iterations, t max =N; is the difference matrix between the starting point and the end point of the map; f a,i For x a,i The fitness value of Step (5): Based on the initialization coordinate matrix of egret B, the position update matrix of egret B is calculated using the improved attack strategy; the improved attack strategy is to improve the position update method of egret B using the greedy strategy so that the position update matrix of egret B is x b,i Always perform random walks near the optimal value to improve the search efficiency of Egret B; if x ibest <x gbest , the improved position update strategy of Egret B is shown in formula (15), and vice versa as shown in formula (16); f b,i =f(x b,i ) (17) Among them, step b is the step length factor of Egret b; l b,i ∈(-π / 2,π / 2); f b,i For x b,i The fitness value of , tan() is the tangent function; Step (6): Based on the initialization coordinate matrix of Egret C, the position update matrix of Egret C is calculated using the attack strategy of the original method; the attack strategy of the original method is described as: Among them, A h is the gap matrix between the historical best position and the current position of the Egret Team; and A g is the gap matrix between the global optimal position of the egret population and the current position; x c,i is the next position update of Egret C, l i is a random number in the interval [0,0.5); Step (7): After the positions of the three egrets in the egret team are updated, the combination is x s,i 、f s,i ; x s,i =[x a,i x b,i x c,i ] (20) f s,i =[f a,i f b,i f c,i ] (21) Among them, x s,i Update the matrix for the position of the i-th egret team; f s,i For x s,i The fitness value vector of ; Step (8): performing a collision test on the current position update matrix of the Egret Squad; the collision test is to traverse each element in the position update matrix of the Egret Squad. If each element is not on an obstacle, and the line connecting each element with the corresponding element of the position update matrix generated in the previous iteration process does not pass through the obstacle, then the current position update matrix of the Egret Squad passes the collision test; Step (9): For the elements of the Egret Squadron's current position update matrix that have not passed the collision test, use Levy flight to make them pass the collision test; Step (10): Calculate the random step length s of the Levy flight using formula (22); Among them, μ, ν are normally distributed random numbers, and μ~(0,σ 2 ), ν~(0,1), β is the Levy flight stability parameter, σ is the Levy flight standard deviation; Step (11): Calculate the Levy flight standard deviation σ using formula (23); the Levy flight standard deviation σ is described as: Among them, Γ(*) is the gamma function, sin() is the sine function, and π is the circumference of a circle; Step (12): According to formula (24), calculate the coordinates x of the feasible point generated after Levy flight levy , and add it to the position update matrix x s,i ; x levy =x′+α·s (24) Where x' is the current position update matrix x s,i The coordinates of the elements that failed the collision test, α is the Levy flight step length factor; Step (13): Update the matrix x for the positions that pass the collision test s,i Perform regional limits; the regional limits are the method that during the iteration process, the three egrets in the team can only search in the area with a vertical distance δ from the line connecting the starting point and the target point, and eliminate the position update matrix elements that do not meet the limit threshold δ range, thereby improving the quality of the planned path and reducing the path search cost; Step (14): According to the feasible point coordinate fine-tuning strategy, update the matrix x for the position after the regional limit s,i Fine-tune the coordinates: The feasible point coordinate fine-tuning strategy is that after the Egret team position update is completed, the generated Egret position will be too close to the obstacle, thus affecting the path safety of the robot. Therefore, inspired by the parent and child nodes of the A* method, a feasible point coordinate fine-tuning strategy is proposed, that is, according to the given detection threshold b, within the threshold range, with the current point as the center, detect whether there are obstacles in the 8 directions around it. If there is an obstacle in a certain direction, a fallback strategy will be adopted for the current point coordinate in that direction, and the fallback step length is a to ensure that there is appropriate free space between the current position and the obstacle; Point is the position update matrix x s,i For an element of Point, the first child node node1 of Point, an obstacle is detected on its right side within the detection range defined by the detection threshold b. Point will move to the left with a back-off step of a to ensure that there is a certain amount of free space between it and the obstacle. The processing methods for the remaining 7 directions are similar to the case where node1 detects an obstacle within the threshold range. Step (15): Find the position vector m corresponding to the optimal fitness value i ;m i for: m i =argmin(f s,i ) (25) Among them, argmin() is to make f s,i When the minimum value is obtained, the function of the corresponding independent variable is returned; Step (16): Update the current position of the Egret Squadron according to the judgment condition; the judgment condition is described as: Where value is a constant; if f s,i The minimum value is less than the current fitness value f i , then the Egret Team will adopt f s,i The coordinate vector m corresponding to the minimum value i Update the position; or if the random number l∈(0,1) is less than value, the Egret team will also update the current position. The position update at this time is to enrich the solution space of the method and prevent the method from falling into the local optimal trap; Step (17): Update the matrix x for the final position of the generated current egret team i Conduct crash tests; Step (18): x that passes the collision test i Add to the original path point set Path, iteration = iteration + 1; Step (19): If iteration exceeds the maximum number of iterations N, the shortest path method is used to prune the original path point set Path to obtain the pruned matrix Path'; its basic principle is to start from the starting point and connect the subsequent path points to the starting point in sequence until the line between the starting point and a certain subsequent path point passes through an obstacle, then return to the parent node whose line with the starting point does not pass through the obstacle, and at the same time remove the redundant nodes between the starting point and the parent node and use the line between the two as the pruned path. After the path between the starting point and the point is pruned, the point is used as the new starting point to continue the pruning operation until the traversal reaches the end of the path; Step (20): according to the path point matrix Path' obtained by pruning the original path point set Path, a redundancy removal operation is performed to remove meaningless nodes in the pruned path point matrix to obtain a redundancy removal path point matrix Path". The redundancy removal operation is to start from the first element in the pruned path point matrix Path', take three consecutive path nodes as a group, traverse the entire path point matrix, and if the nodes in the same group are all on the same straight line, remove the intermediate nodes to achieve the redundancy removal operation. Step (21): connect the elements in the path point matrix Path" after removing redundancy in sequence to form the original path, and use the piecewise Bezier curve method to smooth the original path; use formula (27) to calculate the n-order Bezier curve. Since the high-order Bezier curve has poor robustness and cannot effectively utilize the path point information, it is difficult to effectively optimize the pruned path. Therefore, the second-order Bezier curve is used for trajectory optimization. The expression of the second-order Bezier curve is shown in formula (28); B2(τ)=(1-τ 2 )B0+2τ(1-τ)B1+τ 2 B2 (28) where B0, B1, … B n , is the control point of the Bezier curve, τ∈[0,1] is the curve curvature control parameter, i represents the i-th control point, and n represents the order of the Bezier curve; The piecewise Bezier curve method is to optimize the trajectory using an improved Bezier curve method to ensure the continuity of the trajectory curvature after the second-order Bezier curve optimization. The three continuous elements B starting from the starting point in the path point matrix Path" after removing the redundant ones are taken. k , B k+1 , B k+2 , and B k+1 and B k , B k+1 , B k+1 , B k+2 The midpoint of is used as the control point to generate a Bezier curve. If the curve crosses an obstacle, the corresponding midpoint of the control point is used to generate a Bezier curve. The above operation is repeated until the optimized curve does not cross an obstacle and finally forms a smooth and continuous robot motion trajectory. Step (22): Based on the path optimized by the piecewise Bezier curve method, output the final path planning result and return it.

Images