Global path planning method for robot in complex static environment

Abstract

The invention relates to the technical field of path planning, in particular to a global path planning method for a robot in a complex static environment, and the method comprises the steps: constructing a static environment model; setting parameters; initializing a population position; calculating the fitness; adjusting the position of the sparrow to avoid static obstacles; after each iteration, carrying out local optimization by using cubic spline interpolation; and repeating the steps until the maximum number of iterations or path convergence is achieved, and outputting the optimal path. Compared with the prior art, the method has the advantages that the nonlinear attenuation factor mu is introduced into the discoverer algorithm, so that the discoverer can perform wide exploration in the early stage and can perform accurate exploration on the developed area in the later stage, the early-stage search speed of the discoverer is increased, and the convergence speed is increased; and different search paths are determined according to the alarm value, and large-range search is performed when the alarm value is low, so that the optimal path is quickly searched, and the convergence speed is further improved.

Description

Technical Field

[0001] The present invention relates to the technical field of path planning, and in particular to a global path planning method for a robot in a complex static environment. Background Art

[0002] Path planning is an important branch and basic research content of robot applications. Its research directions mainly include path planning in unknown environments and path planning in known environments. In unknown environments, the environment is mainly judged with the help of sensors, and the focus is on real-time obstacle avoidance. The situation in known environments is different. Because the environment is known, obstacle avoidance is relatively easy, but there are often many or even countless paths. How to find the required or best path among so many paths has become the key research content of path planning in known environments.

[0003] The methods for solving the UAV path planning problem are mainly divided into classical algorithms and swarm intelligence optimization algorithms. Although classical algorithms have the advantages of fast calculation speed and high efficiency, they often have problems such as slow convergence speed and high calculation complexity when facing more complex problems. Therefore, domestic and foreign scholars have proposed many swarm intelligence optimization algorithms for solving complex problems, such as particle swarm algorithm, ant colony algorithm, crow search algorithm, whale algorithm, gray wolf algorithm, sparrow search algorithm, etc. These algorithms can search and optimize the solution space to find the best path, but they still face the problem of slow convergence speed. Summary of the invention

[0004] The purpose of the present invention is to provide a global path planning method for a robot in a complex static environment to solve the problem of slow convergence speed in the prior art.

[0005] A global path planning method for a robot in a complex static environment, comprising:

[0006] Acquire static environment data and build a static environment model;

[0007] Set the sparrow population size, maximum number of iterations and initial position parameters;

[0008] Initialize population position;

[0009] Perform global path search according to the sparrow algorithm, update the sparrow position, and calculate the fitness of each path;

[0010] Adjust the position of the sparrow according to the information of environmental changes to avoid static obstacles;

[0011] Select N sparrow individuals in the current iteration and put them into the corresponding positions of the original population in the next iteration, and the value of N is determined according to the fitness;

[0012] After each iteration, the current optimal path is locally optimized using cubic spline interpolation;

[0013] Repeat the above steps until the maximum number of iterations is reached or the path converges, and output the optimal path.

[0014] Furthermore, the fitness J is set cost =L+ωη, where:

[0015] ηAccording to the formula: to seek;

[0016] L According to the formula: to seek;

[0017] Where: L is the total length of the route, ω is the collision coefficient, η is the n mean (θ n ), xx is the set of all the abscissa interpolation points on the path, yy is the set of all the ordinate interpolation points on the path, n is the number of obstacles, x on ,y on are the horizontal and vertical coordinates of the center of the nth obstacle circle, d n is the set of distances from all interpolation points on the path to the center of the nth obstacle circle, θ n is a set, mean(θ n ) is the set θ n The mean of all the numbers in .

[0018] Further, the sparrow population is divided into discoverers, scouts and joiners;

[0019] The number of discoverers is 15%-21% of the population, and the number of scouts is 18%-22% of the population.

[0020] Furthermore, the step of selecting N sparrow individuals in the current iteration and placing them in corresponding positions of the original population of the next iteration includes:

[0021] At the current number of iterations, the individual sparrows are sorted from high to low according to their fitness;

[0022] Calculate the proportion of the number of sparrow individuals whose fitness is greater than the fitness threshold in the sparrow population and record the proportion as the preferred proportion;

[0023] According to the preferred proportion, N sparrow individuals with higher fitness are selected as elite sparrows to enter the next iteration.

[0024] Furthermore, N sparrow individuals with higher fitness are selected as elite sparrows according to the preferred proportion to enter the next iteration, wherein the value of N is determined as follows:

[0025] Comparing the preferred ratio with a ratio threshold;

[0026] If the preferred proportion is less than the proportion threshold, the value of N is 30% of the number of the sparrow population;

[0027] If the preferred proportion is greater than or equal to the proportion threshold, the number N of sparrow individuals in the elite population is determined according to the number of iterations.

[0028] Furthermore, the number N of sparrow individuals in the elite population is determined according to the number of iterations, and the number N of sparrow individuals in the elite population is set Among them: λ is the number of iterations, M is the number of sparrow population.

[0029] Furthermore, the location of the finder is calculated as follows:

[0030]

[0031] l = a × rand × 2

[0032]

[0033] Among them: κ is a constant, rand is a random number between 0 and 1, iter max is the maximum number of iterations, ω is a constant, and μ is the nonlinear attenuation factor.

[0034] Furthermore, the method for calculating the position of the joiner is:

[0035]

[0036] Where: X p With X worst is the position of the individual with the best and worst fitness in the current region, A is a 1×d dimensional matrix composed of 1 or -1, and A + =A T (AA T ) -1 ; and when i>n / 2, it is determined that the i-th participant with a lower fitness value has not obtained food and needs to leave this area to forage.

[0037] Furthermore, the position of the scout is calculated as follows:

[0038]

[0039] Where: X best is the current global optimal sparrow individual, f i 、f g and f ware the individual fitness value, the best fitness value and the worst fitness value respectively. β and K are step size control parameters. β obeys normal distribution and K ranges from [-1,1]. ε is a sufficiently small constant.

[0040] Furthermore, the interpolation points are points obtained after cubic spline interpolation.

[0041] Compared with the prior art, the beneficial effect of the present invention lies in that, by introducing a nonlinear attenuation factor μ into the discoverer algorithm, the present invention can, on the one hand, enable the discoverer to conduct extensive exploration in the early stage, and on the other hand, enable accurate exploration of the developed area in the later stage, thereby increasing the discoverer's early search speed and thus improving the convergence speed; different search paths are determined according to the warning value, and a large-scale search is performed when the warning value is low, so as to quickly find the best path, thereby further improving the convergence speed.

[0042] Furthermore, the interval is divided into n small intervals through cubic spline interpolation, and a cubic polynomial is constructed with two points in each small interval to construct a curve. The derivatives of the entire curve of the cubic spline function are continuous, and the second-order derivatives at the interpolation points are also continuous, thereby ensuring the smoothness of the curve and making the curve generated by the next iteration more convergent, thereby further improving the convergence speed.

[0043] Furthermore, since the interpolation function on each sub-interval is different, the curve may turn at the node. In the embodiment of the present invention, the node between the segments is called a control point. It can be concluded that the control point determines the direction of the entire curve. The artificial fish school algorithm is used to calculate all the turning control points of the path to find the optimal control point, and then the cubic spline interpolation is performed to obtain the interpolation point. The starting point, interpolation point, turning control point, and end point are connected to obtain the desired path, so that the curve generated by the next iteration is more convergent, thereby further improving the convergence speed.

[0044] Furthermore, the number N of sparrow individuals in the elite population is determined according to the number of iterations, and a monotonically increasing function with a horizontal coordinate ranging from zero to infinity and a vertical coordinate ranging from zero to 0.5 is introduced, so that when the fitness is low, a lower and appropriate amount of elite population is selected, so that a large-scale search for the optimal solution is performed when the number of iterations is low, and when the number of iterations is high and the fitness is high, the function can converge by increasing the proportion of the elite population, thereby further improving the convergence speed. BRIEF DESCRIPTION OF THE DRAWINGS

[0045] Figure 1 A global path planning method for a robot in a complex static environment according to an embodiment of the present invention;

[0046] Figure 2Flow chart of the steps of selecting N sparrow individuals in the current iteration and placing them in the corresponding positions of the original population of the next iteration according to an embodiment of the present invention

[0047] Figure 3 This is a flowchart of the method for obtaining the value of N according to an embodiment of the present invention;

[0048] Figure 4 This is a flow chart of the cubic spline interpolation steps described in an embodiment of the present invention. DETAILED DESCRIPTION

[0049] The technical solution of the present invention will be described clearly and completely below in conjunction with the accompanying drawings. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0050] In the description of the present invention, it should be noted that the orientation or positional relationship indicated by the terms "center", "up", "down", "left", "right", "vertical", "horizontal", "inside", "outside", etc. is based on the orientation or positional relationship shown in the accompanying drawings, and is only for the convenience of describing the present invention and simplifying the description, rather than indicating or implying that the device or element referred to must have a specific orientation, be constructed and operated in a specific orientation, and therefore cannot be understood as limiting the present invention. In addition, the terms "first" and "second" are only used for descriptive purposes and cannot be understood as indicating or implying relative importance. Among them, the terms "first position" and "second position" are two different positions, and the first feature "above", "above" and "above" the second feature include the first feature being directly above and obliquely above the second feature, or simply indicates that the first feature is higher in level than the second feature. The first feature "below", "below" and "below" the second feature include the first feature being directly below and obliquely below the second feature, or simply indicates that the first feature is lower in level than the second feature.

[0051] In the description of the present invention, it should be noted that, unless otherwise clearly specified and limited, the terms "installed", "connected" and "connected" should be understood in a broad sense, for example, it can be a fixed connection, a detachable connection, or an integral connection; it can be a mechanical connection or an electrical connection; it can be a direct connection, or it can be indirectly connected through an intermediate medium, or it can be the internal communication of two components. For ordinary technicians in this field, the specific meanings of the above terms in the present invention can be understood according to specific circumstances.

[0052] Embodiments of the present invention are described in detail below, and examples of the embodiments are shown in the accompanying drawings, wherein the same or similar reference numerals throughout represent the same or similar elements or elements having the same or similar functions. The embodiments described below with reference to the accompanying drawings are exemplary and are only used to explain the present invention, and cannot be understood as limiting the present invention.

[0053] See also Figure 1 As shown, it is a global path planning method for a robot in a complex static environment according to an embodiment of the present invention, comprising:

[0054] Step S1, acquiring static environment data and constructing a static environment model;

[0055] Step S2, setting parameters such as the number of sparrow populations, the maximum number of iterations, and the initial position;

[0056] Step S3, initializing population position;

[0057] Step S4, perform global path search according to the sparrow algorithm, update the sparrow position, and calculate the fitness of each path;

[0058] Step S5, adjusting the position of the sparrow according to the environmental change information to avoid static obstacles;

[0059] Step S6, select N sparrow individuals in the current iteration and put them into the corresponding positions of the original population in the next iteration, and the value of N is determined according to the fitness;

[0060] Step S7, after each iteration, using cubic spline interpolation to locally optimize the current optimal path;

[0061] Step S8, repeat the above steps until the maximum number of iterations is reached or the path converges, and output the optimal path.

[0062] Specifically, in step S4, the fitness of each path is calculated, including:

[0063] Substitute the set of all the abscissa interpolation points on the path, the set of all the ordinate interpolation points on the path, the number of obstacles, and the abscissa and ordinate values ​​of the center of the nth obstacle into the formula to set the fitness J cost =L+ωη, where:

[0064] ηAccording to the formula: to seek;

[0065] L According to the formula: to seek;

[0066] Where: xx is the set of all the abscissa interpolation points on the path; yy is the set of all the ordinate interpolation points on the path; n represents the number of obstacles; x on,y on are the horizontal and vertical coordinates of the center of the nth obstacle circle; d n is the set of distances from all interpolation points on the path to the center of the nth obstacle circle; θ n is a collection;

[0067] When the distance from the interpolation point to the center of the nth obstacle circle is greater than the radius, all numbers in θ are 0. Otherwise, there must be a positive number in θ. n ) represents the set θ n The mean of all numbers in; η represents n mean(θ n ), if the route successfully avoids all obstacles, the value of η is 0, otherwise, η is a positive number. The fitness function is set to: J cost =L+ωη, and, where: (x k ,y k ) is the coordinate of the interpolation point, L is the total length of the route, ω is the collision coefficient, and m is the number of interpolation points.

[0068] Specifically, the number of discoverers is 15%-21% of the population, and the number of scouts is 18%-22% of the population.

[0069] See also Figure 2 As shown, it is a flowchart of the step of selecting N sparrow individuals in the current iteration and placing them into the corresponding positions of the original population in the next iteration according to an embodiment of the present invention, including:

[0070] At the current number of iterations, the individual sparrows are sorted from high to low according to their fitness;

[0071] Calculate the proportion of the number of sparrow individuals whose fitness is greater than the fitness threshold in the sparrow population and record the proportion as the preferred proportion;

[0072] According to the preferred proportion, N sparrow individuals with higher fitness are selected as elite sparrows to enter the next iteration.

[0073] The method for selecting the fitness threshold is to select multiple fitness thresholds for multiple experiments under the premise of using the same initial parameters, the same environmental changes, and the same number of iterations, and substitute the sparrow individuals with fitness higher than the fitness threshold as the elite population for the next iteration, and select the one with the fastest convergence speed as the fitness threshold under the current environmental conditions. The value of the fitness threshold of the present invention is 0.8 (the fitness described in the present invention is standardized and the value range is [0,1]).

[0074] See also Figure 3 As shown, it is a flowchart of the method for obtaining the value of N according to an embodiment of the present invention, including:

[0075] Comparing the preferred ratio with a ratio threshold;

[0076] If the preferred proportion is less than the proportion threshold, the value of N is 15% of the number of the sparrow population, and if a decimal is obtained after calculation, it is rounded up;

[0077] If the preferred proportion is greater than or equal to the proportion threshold, the number N of sparrow individuals in the elite population is determined according to the number of iterations.

[0078] Among them: the method for determining the proportion threshold is: in order to make the value of N in a smooth transition state when switching the value method, that is, under the current conditions, the number of sparrow individuals N in the elite population determined by the number of iterations and 15% of the number of sparrow population, the numerical difference is within 2. According to the above conditions, it can be seen that the specific method for obtaining this value is related to factors such as the actual number of individuals, and is not specifically limited here.

[0079] Specifically, the number N of sparrow individuals in the elite population is determined according to the number of iterations, and the number N of sparrow individuals in the elite population is set Where: λ is the number of iterations, M is the number of sparrow populations, and if the calculated result is a decimal point, it is rounded up.

[0080] In the above embodiment, when M sparrow individuals are generated during population initialization, they are arranged according to their fitness, and the individuals with the best fitness form an elite population as the initial population for algorithm iteration, thereby improving the quality of the initialized population and further improving the optimization efficiency of the algorithm.

[0081] In detail, the function It is a monotonically increasing function with a horizontal axis (λ) ranging from zero to infinity and a vertical axis N ranging from zero to 0.5. As the number of iterations increases, the value tends to gradually increase. Therefore, when the fitness is low, a relatively low and appropriate number of elite populations are selected, so that a large-scale search for the optimal solution is performed when the number of iterations is low. When the number of iterations is high, the function can converge by increasing the proportion of elite populations.

[0082] Specifically, the method for calculating the position of the finder is:

[0083]

[0084] l = a × rand × 2

[0085]

[0086] Where: κ is a constant, which is 10 in this paper, rand is a random number between 0 and 1, iter maxis the maximum number of iterations, ω is a constant and is set to be 5.5 in the embodiment of the present invention, and μ is the nonlinear attenuation factor.

[0087] In the above embodiment: Although the discoverer's position update calculation method can maximize the search range, it also brings about the problem of insufficient search accuracy. Therefore, the embodiment of the present invention introduces a nonlinear attenuation factor μ, so that the algorithm can widely search different areas in the early stage, and focus on developing known areas in the middle and late stages, thereby improving the search accuracy and convergence speed.

[0088] Specifically, the joiners in the population improve their predation rate by monitoring the behavior of the discoverers and competing for the food resources of the discoverers. The calculation method of the position of the joiners is:

[0089]

[0090] Where: X p With X worst is the position of the individual with the best and worst fitness in the current region, A is a 1×d dimensional matrix composed of 1 or -1, and A + =A T (AA T ) -1 ; and when i>n / 2, it is determined that the i-th participant with a lower fitness value has not obtained food and needs to leave this area to forage.

[0091] Specifically, the method for calculating the position of the scout is:

[0092]

[0093] Where: X best is the current global optimal sparrow individual, f i 、f g and f w are the individual fitness value, the best fitness value and the worst fitness value respectively. β and K are step size control parameters. β obeys normal distribution and K ranges from [-1,1]. ε is a sufficiently small constant.

[0094] Specifically, the interpolation points are points obtained after cubic spline interpolation, see Figure 4 As shown, it is a flow chart of the cubic spline interpolation steps described in an embodiment of the present invention, wherein:

[0095] In the path function, select a group of data points (x0, y0), (x1, y1), (x2, y2)... (x n ,y n );

[0096] Calculate the interval between each adjacent data point, h i=x i+1 -x i (i=0,1,......,n-1);

[0097] For each interval construct a cubic polynomial S i (x) = a i +b i (xx i )+c i (xx i ) 2 +di(xx i ) 3 ;

[0098] Set parameters for difference conditions, first-order derivative continuity, second-order derivative continuity, and boundary conditions;

[0099] Set up the linear equation and solve for c i ;

[0100] Calculation parameter b i and d i ;

[0101] Construct a difference function.

[0102] Obviously, the above embodiments of the present invention are merely examples for clearly illustrating the present invention, and are not intended to limit the embodiments of the present invention. For those skilled in the art, other different forms of changes or modifications can be made based on the above description. It is not necessary and impossible to list all the embodiments here. Any modification, equivalent replacement and improvement made within the spirit and principle of the present invention shall be included in the protection scope of the claims of the present invention.

Claims

1. A global path planning method for a robot in a complex static environment, characterized in that: include: Acquire static environment data and build a static environment model; Set the sparrow population size, maximum number of iterations and initial position parameters; Initialize population position; Perform global path search according to the sparrow algorithm, update the sparrow position, and calculate the fitness of each path; Adjust the position of the sparrow according to the information of environmental changes to avoid static obstacles; Select N sparrow individuals in the current iteration and put them into the corresponding positions of the original population in the next iteration, and the value of N is determined according to the fitness; After each iteration, the current optimal path is locally optimized using cubic spline interpolation; Repeat the above steps until the maximum number of iterations is reached or the path converges, and output the optimal path.

2. The global path planning method for a robot in a complex static environment according to claim 1, characterized in that: Set the fitness J cost =L+ωη, where: ηAccording to the formula: to seek; L According to the formula: to obtain; Where: L is the total length of the route, ω is the collision coefficient, η is the n mean (θ n ), xx is the set of all the abscissa interpolation points on the path, yy is the set of all the ordinate interpolation points on the path, n is the number of obstacles, x on ,y on are the horizontal and vertical coordinates of the center of the nth obstacle circle, d n is the set of distances from all interpolation points on the path to the center of the nth obstacle circle, θ n is a set, mean(θ n ) is the set θ n The mean of all the numbers in .

3. The global path planning method for a robot in a complex static environment according to claim 1, characterized in that: The said sparrow population is divided into discoverers, scouts and joiners; The number of discoverers is 15%-21% of the population, and the number of scouts is 18%-22% of the population.

4. The global path planning method for a robot in a complex static environment according to claim 1, characterized in that: The step of selecting N sparrow individuals in the current iteration and placing them in corresponding positions of the original population in the next iteration comprises: At the current number of iterations, the individual sparrows are sorted from high to low according to their fitness; Calculate the proportion of the number of sparrow individuals whose fitness is greater than the fitness threshold in the sparrow population and record the proportion as the preferred proportion; According to the preferred proportion, N sparrow individuals with higher fitness are selected as elite sparrows to enter the next iteration.

5. The global path planning method for a robot in a complex static environment according to claim 4, characterized in that: According to the preferred proportion, N sparrow individuals with higher fitness are selected as elite sparrows to enter the next iteration, where the value of N is determined as follows: Comparing the preferred ratio with a ratio threshold; If the preferred proportion is less than the proportion threshold, the value of N is 30% of the number of the sparrow population; If the preferred proportion is greater than or equal to the proportion threshold, the number N of sparrow individuals in the elite population is determined according to the number of iterations.

6. The global path planning method for a robot in a complex static environment according to claim 5, characterized in that: Determine the number of sparrow individuals N in the elite population according to the number of iterations, and set Among them: λ is the number of iterations, M is the number of sparrow population.

7. The global path planning method for a robot in a complex static environment according to claim 3, characterized in that: The location of the finder is calculated as: l = a × rand × 2 Among them: κ is a constant, rand is a random number between 0 and 1, iter max is the maximum number of iterations, ω is a constant, and μ is the nonlinear attenuation factor.

8. The global path planning method for a robot in a complex static environment according to claim 3, characterized in that: The position of the joiner is calculated as follows: Where: X p With X worst is the position of the individual with the best and worst fitness in the current region, A is a 1×d dimensional matrix composed of 1 or -1, and A + =A T (AA T ) -1 ; and when i>n / 2, it is determined that the i-th participant with a lower fitness value has not obtained food and needs to leave this area to forage.

9. The global path planning method for a robot in a complex static environment according to claim 3, characterized in that: The position of the scout is calculated as follows: Where: X best is the current global optimal sparrow individual, f i 、f g and f w are the individual fitness value, the best fitness value and the worst fitness value respectively. β and K are step size control parameters. β obeys normal distribution and K ranges from [-1,1]. ε is a sufficiently small constant.

10. The global path planning method for a robot in a complex static environment according to claim 1, characterized in that: The interpolation points are points obtained after cubic spline interpolation.

Images