Unmanned Aerial Vehicle Path Planning Method and System Based on FLC-APF-RRT*

Abstract

The present invention relates to a UAV path planning method based on FLC-APF-RRT*. The specific steps are described as follows: preset a target bias threshold P bias , randomly generate a probability P, and according to the size relationship between P bias and P, divide the random tree expansion into two stages: exploration and convergence; design a fuzzy controller PbiasFuzzy to dynamically output the target bias threshold according to environmental factors; combine the artificial potential field method with the target bias threshold to propose an improved new node generation method. The present invention fully considers the advantages of the fuzzy controller that does not require precise modeling and the environmental factors of UAV path planning, optimizes the running speed, path length and number of nodes of UAV path planning, and provides a reference for the expansion method of the search random tree. Figure 1 is a flow chart of the method involved in this application.

Description

Technical Field

[0001] The present invention relates to the technical field of unmanned aerial vehicles, and particularly to an unmanned aerial vehicle path planning method and system based on FLC-APF-RRT*. Background Art

[0002] Path planning is one of the key issues in unmanned aerial vehicle mission planning. In recent years, a large number of studies have been dedicated to finding faster and more efficient path planning methods. The unmanned aerial vehicle path planning problem can be described as finding a collision-free feasible path that meets the performance requirements of the unmanned aerial vehicle between the initial position and the target position under certain constraints. Existing traditional algorithms have the disadvantages of poor performance and long time consumption. Existing swarm intelligence algorithms generally have problems such as slow convergence speed, dependence on accurate modeling of the environment, and being easily trapped in local optimal solutions, and are difficult to be applied to path planning in high-dimensional spaces.

[0003] Therefore, the present invention analyzes a fuzzy controller with the advantage of not requiring accurate modeling, and gives a design method of the fuzzy controller. The growth stage of the random tree is divided according to the output dynamic target bias threshold. On this basis, combined with the artificial potential field method and the target bias threshold, the present invention designs an improved new node generation method. The present invention fully considers the distribution ratio of the artificial potential field force and the new node effect in the process of generating new nodes, and guides the generation of new nodes according to the environment through the target bias threshold, which well optimizes the average path length, search time and number of nodes of the path planning, and provides a reference for the unmanned aerial vehicle path planning algorithm.

[0004] In addition, on the one hand, there are differences in the understanding of those skilled in the art; on the other hand, although the applicant has studied a large number of documents and patents when making the present invention, all details and contents are not listed in detail due to space limitations. However, this does not mean that the present invention does not possess the features of these prior arts. On the contrary, the present invention already possesses all the features of the prior arts, and the applicant reserves the right to add relevant prior arts in the background art. Summary of the Invention

[0005] The purpose of the present invention is to provide an unmanned aerial vehicle path planning method based on FLC-APF-RRT*. Considering the problems existing in existing algorithms such as accurate modeling of obstacles and too long search time, the present invention designs an unmanned aerial vehicle path planning method based on FLC-APF-RRT* through an improved new node generation method. The improved new node generation method includes the following steps: designing a target bias threshold for the output of the fuzzy controller, and dividing the sampling stage according to the target bias threshold.

[0006] To achieve the above object of the invention, the unmanned aerial vehicle path planning method based on FLC-APF-RRT* involved in the present invention includes the following steps:

[0007] (1) Preset a target bias threshold P bias , randomly generate a probability P, and according to the size relationship between P bias and P, divide the random tree expansion into two stages: exploration and convergence;

[0008] (2) Design a fuzzy controller PbiasFuzzy to dynamically output the target bias threshold according to environmental factors;

[0009] (3) Combine the artificial potential field method with the target bias threshold to propose an improved method for generating new nodes.

[0010] According to a preferred implementation, preset a target bias threshold P bias , randomly generate a probability P, and according to P bias and P's size relationship, divide the random tree expansion into two stages: exploration and convergence.

[0011] Artificial Potential Field (APF) is a path planning method that drives a robot or vehicle to avoid obstacles and reach the target by introducing a potential field in the environment. Fuzzy Logic Control (FLC) is a fuzzy logic system used to process fuzzy information and uncertainty and is often used in control systems. The Rapidly-exploring Random Tree (RRT*) algorithm can find the optimal path faster and better, but its search efficiency is still not high in complex and restricted environments, with a large number of invalid area searches and redundant nodes. Therefore, the sampling stage is divided into two stages: exploration and convergence. The specific steps include: preset a target bias threshold P bias , randomly generate a probability value P from a uniform probability distribution, and divide the random tree expansion into two stages: the exploration stage ( Figure 2 ) and the convergence stage ( Figure 3 ).

[0012] When P ≥ P bias , the random tree is in the exploration stage, randomly generate a sampling point X rand , and perform lateral expansion of the random tree; when P < P bias , the random tree is in the convergence stage, the direction of the sampling point is the direction of the target point, and the next sampling point is the sampling position at a distance of step on the extension line of X nearest and X goal . The expansion of the new node (X new ) is defined as shown in Equation (1):

[0013]

[0014] Among them, X' goal is Xnearest Point on the extension line of X goal at a distance of step; X rand is the newly extended random point.

[0015] According to a preferred embodiment, a fuzzy controller PbiasFuzzy is designed to dynamically output the target bias threshold according to environmental factors. The fuzzy controller has two inputs and one output. Input parameter one is the obstacle density near the sampling point, and input parameter two is the Euclidean distance from the sampling point to the target point. The output value is the target bias threshold. The obstacle density ObsDensity and the distance from the sampling point to the target point GoalDist are configured with the same subsets {NL, NM, ZE, PM, PL}, and after normalization, their value ranges are [0, 1] and [0, 10]. The output target bias threshold P bias has an output range of [0, 0.7], and the fuzzy subsets are {NL, NM, NS, ZE, PS, PM, PL}. The purpose of the above settings is to divide the output range of P bias more finely to reduce errors. Table 1 shows the symbolic representation meanings of the membership functions. The membership functions of the input ObsDensity, GoalDist and the output P bias are respectively as Figure 4 shown (from top to bottom). According to experience, the fuzzy controller rule set is designed as shown in Table 2. The fuzzy value of the fuzzy controller output P bias is as Figure 5 shown.

[0016] The present invention combines the artificial potential field method with the target bias threshold to propose an improved new node generation method. The new node generation method of the traditional APF-RRT* algorithm only considers the effects of the artificial potential field and the random point, without considering the proportion of the influence of these two factors on the new node, making the new node generation method of the traditional APF-RRT* algorithm lack adaptability to the environment and prone to falling into local optima. Taking P bias as the influencing factor, dynamically allocate the random point and the artificial potential field that have an impact on the new node. The improved configuration method of the new node is shown in Equation (2):

[0017]

[0018] where, X' goal is the point on the extension line of X nearest and X goal at a distance of step, ||X rand -X nearest || represents the Euclidean distance between X rand and X nearest . F att is the gravitational force, F repIt is a repulsive force, and its definition is shown in Equations (3) and (4):

[0019]

[0020]

[0021] where K a represents the gravitational constant; X nearest represents the current node of the random tree expansion; X goal is the target point; ρ(X goal , X nearest ) is a vector, the length of which is the Euclidean distance from X nearest to X goal , and the direction is from X nearest pointing to X goal ; K r represents the repulsive force constant; X obstacle represents the position of the obstacle; ρ(X obstacle , X nearest ) is a vector, the length of which is the Euclidean distance from X obstacle to X nearest , and the direction is from X obstacle pointing to X nearest ; ρ0 is the action range of the repulsive force field;

[0022]

[0023] As can be seen from the above, P bias has an obvious guiding effect on the new node. The smaller P bias is, the more complex the environment the node is currently in, the more the random tree tends to the exploration stage, the influence of the random point on the generation of the new node is enhanced, and the influence of the artificial potential field is weakened; the larger P bias is, the simpler the environment the node is currently in, the random tree tends to the convergence stage, the acting force of the artificial potential field on the new node increases, and the influence of the random point decreases. By introducing P bias to correct the expansion position of the new node, the flexibility and adaptability of the algorithm search are increased. The principle of generating the new node after improvement is as shown in Figure 6 .

[0024] The algorithm flow of the UAV path planning algorithm based on the FLC-APF-RRT* algorithm is as shown in Figure 7 , and the flow steps are as follows:

[0025] The algorithm performs N iterations;

[0026] Use numNodes, the maximum number of nodes, to control the total number of generated nodes. If this limit is exceeded, it is regarded as a planning failure.

[0027] During the iteration process, the obstacle density ObsDensity and the distance GoalDist from the sampling point to the target point are input into the fuzzy controller PbiasFuzzy to obtain the output Pbias. If a probability value rand(1) randomly generated from a uniform probability distribution is less than Pbias, the random point is designated as the target point; otherwise, a new random point X is sampled within the map range. rand .

[0028] After determining the target point (the new random point X rand or the random point), the node X closest to the random point is found in the path. nearest , and then the improved new node generation method proposed by the present invention is used to generate a new node X new [generated by formula (2)]. If the connection line between the new node and the neighboring node does not collide with the obstacle, the new node is added to the random tree, and the new node is re-wired and the parent node is re-selected, and the result is updated to the random tree.

[0029] An unmanned aerial vehicle path planning system based on FLC-APF-RRT*, characterized in that the system is configured to: (1) preset a target bias threshold P bias , randomly generate a probability P, and according to the size relationship between P bias and P, the expansion of the random tree is divided into two stages: exploration and convergence; (2) design a fuzzy controller to dynamically output the target bias threshold according to environmental factors; (3) combine the artificial potential field method with the target bias threshold to propose an improved new node generation method.

[0030] The beneficial effects of the present invention compared with the prior art are:

[0031] Based on the characteristics of the fuzzy controller that does not require precise data modeling and is easy to implement, the present invention establishes an environmental target bias threshold control method based on the fuzzy controller. Based on the artificial potential field method and the target bias threshold, the present invention proposes an improved new node generation method. The present invention fully considers the advantages of the fuzzy controller that does not require precise modeling and the environmental factors of path planning, optimizes the running speed, path length and number of nodes of the unmanned aerial vehicle path planning, and provides a reference for the expansion method of the search random tree. Description of the Drawings

[0032] Figure 1 is the method flow chart involved in this application;

[0033] Figure 2 is the schematic diagram of the exploration stage;

[0034] Figure 3 is the schematic diagram of the convergence stage;

[0035] Figure 4For ObsDensity, GoalDist, P bias Membership function graphs of (from top to bottom);

[0036] Figure 5 For P bias Fuzzy values;

[0037] Figure 6 For the schematic diagram of the improved new node generation;

[0038] Figure 7 For the algorithm flow chart;

[0039] Figure 8 For two kinds of maps, where the left map is Figure 1 , and the right map is Figure 2 ;

[0040] Figure 9 For Figure 1 Comparison graph of each algorithm parameter of

[0041] Figure 10 For Figure 1 Comparison graph of each algorithm path of

[0042] Figure 11 For Figure 2 Comparison graph of each algorithm parameter of

[0043] Figure 12 For Figure 2 Comparison graph of each algorithm path of Specific implementation manner

[0044] In order to make the objectives, technical solutions and advantages of the present invention clearer, the present invention will be described in detail below with reference to the accompanying drawings and specific embodiments. For those of ordinary skill in the art, the specific meanings of the terms in the present invention can be understood according to specific circumstances.

[0045] This embodiment provides a UAV path planning method based on FLC-APF-RRT*. To verify the feasibility, applicability and effectiveness of the present invention, Matlab is used to construct maps, and the average path length, average time, average number of nodes and success rate of the improved algorithm and the existing algorithm in different map environments are compared and analyzed.

[0046] The present invention respectively simulates and emulates narrow channel environments and dense environments in complex environments, and the two initial maps are Figure 8 .

[0047] The initial parameters are set as follows: the maximum number of iterations is set to 500, the maximum number of nodes in the narrow passage environment is set to 5000, the maximum number of nodes in the dense environment is set to 3000, both the attraction factor and the repulsion factor are set to 1, and the step size is fixed at 25; the map sizes of all experimental environments are standardized to [1000, 1000], and the starting point and the ending point are defined as [0, 0] and [999, 999] respectively.

[0048] (1) Experimental comparison in the narrow passage environment

[0049] To verify the reliability of the method of the present invention in the narrow passage environment, two narrow channels are set up on the ground Figure 1 with widths of 35 units and 50 units respectively. To ensure the reliability of the experiment and reduce the influence of accidental errors on the experiment, the method of the present invention conducts 50 repeated experiments, and each experiment performs 500 iterations.

[0050] Table 3 is a summary table of the average path length, average time, average number of nodes, and success rate of 4 algorithms. The target bias thresholds of APF-RRT*, IRRT*, and RRT-connect are all set to the commonly used 0.3.

[0051] The experimental results are as shown in Table 3 and Figure 9 compared with APF-RRT*, the method of the present invention reduces the average path length from 1939.02 to 1905.62, a decrease of 1.72%, significantly improving the path quality; the average number of nodes is reduced by 7.75%, effectively eliminating redundant path nodes; the average time is reduced by 39.79%, greatly improving the search efficiency, and the search success rate is also increased by 4.33%, further verifying the superiority of FLC-APF-RRT*.

[0052] Compared with IRRT*, the method of the present invention shortens the average path length by 1.46%, shortens the average time by 29.05%, reduces the average number of nodes by 39.91%, and increases the search success rate by 2.76%. Compared with the RRT-connect algorithm, the method of the present invention reduces the average path length and average time by 18.69% and 14.5% respectively, and the success rate is also increased by 8.91%. Although the RRT-connect algorithm has a slight advantage in the average number of nodes, the method of the present invention significantly improves both the path quality and the success rate and significantly reduces the running time through the path finding strategy and the new node generation method.

[0053] In summary, the method of the present invention is generally superior to the other three algorithms in terms of average path length, average time, average number of nodes, and success rate, providing a more efficient and reliable method for path planning.

[0054] Figure 10Shows the path comparison diagram of the method of the present invention and three comparison algorithms on the ground Figure 1 below. The red path represents the found feasible path, and the blue represents the generated random tree. According to Figure 10 it can be seen that the method of the present invention has a certain guiding property during the path search process. When approaching the end point and where the obstacles are sparse, it directly moves towards the end point, avoiding unnecessary random expansion; near the starting point or near narrow passage obstacles, it increases the random exploration intensity to ensure that a feasible path can be searched. From the comparison of the four algorithms, it can be seen that the path of the method of the present invention is more concise and smooth.

[0055] (2) Experimental comparison in a dense environment

[0056] To verify the reliability of the method of the present invention in a dense environment, 50 dense obstacles are randomly distributed on the ground Figure 2 and 500 iterations are still carried out for each experiment. After the iteration ends, the experimental data are recorded. Table 4 records the average data of 50 repeated experiments. The target bias thresholds of APF-RRT*, IRRT* and RRT-connect are still 0.3.

[0057] From Table 4 and Figure 11 it can be seen that the method of the present invention performs excellently in a dense map. Compared with the APF-RRT* algorithm, IRRT* algorithm, and RRT-connect algorithm, the method of the present invention shortens the average path length by 3.59%, 5.29%, and 1.98%. At the time level, even compared with the RRT-connect algorithm that performs two-way expansion simultaneously, the method of the present invention still shows high efficiency, with the time consumption reduced by 42.86%; compared with APF-RRT* and IRRT*, the time is reduced by 39.62% and 54.29%. In addition, compared with APF-RRT* and IRRT*, the method of the present invention reduces the average number of nodes by 39.85% and 63.63%, indicating that the invalid nodes expanded by the method of the present invention are reduced and the occupied memory is less, but due to the double-tree expansion method of RRT-connect, its number of nodes is slightly better than that of the method of the present invention. In terms of success rate, compared with RRT-connect, the success rate of the method of the present invention is increased by 2.37%. The above results prove the stability and reliability of the method of the present invention in a complex environment.

[0058] Figure 12 (a), (b), (c), (d) respectively show the four algorithms on the ground Figure 2Path comparison diagram below. It can be observed that the method of the present invention is more purposeful when expanding the random tree, effectively reducing redundant nodes and random tree bifurcations. However, in areas with dense obstacle density and close to the starting point, the algorithm will explore more roads. Once it finds that the quality of the new path is inferior to the original quality, it will quickly adjust the strategy. The algorithm well balances the path exploration stage and the convergence stage. Compared with the APF-RRT*, IRRT*, and RRT-connect algorithms, the path generated by the method of the present invention is smoother and of higher quality, ultimately achieving a better path planning effect.

[0059] Table 1 Meanings of symbol representations

[0060]

[0061] Table 2 Fuzzy rule set

[0062]

[0063]

[0064] Table 3 Figure 1 Algorithm performance comparison

[0065]

[0066] Table 4 Figure 2 Algorithm performance comparison

[0067]

[0068] It should be noted that the above specific embodiments are exemplary. Those skilled in the art can come up with various solutions inspired by the disclosed content of the present invention, and these solutions also fall within the scope of the disclosure of the present invention and within the protection scope of the present invention. Those skilled in the art should understand that the specification and drawings of the present invention are illustrative and do not constitute a limitation on the claims. The protection scope of the present invention is defined by the claims and their equivalents. The specification of the present invention contains multiple inventive concepts. For example, "according to a preferred embodiment" indicates that the corresponding paragraph discloses an independent concept. The applicant reserves the right to file divisional applications based on each inventive concept. Throughout the text, the features guided by "according to a preferred embodiment" are only optional and should not be construed as must be provided. Therefore, the applicant reserves the right to waive or delete relevant preferred features at any time.

Claims

1. A UAV path planning method based on FLC-APF-RRT*, characterized in that: The steps include: (1) Preset a target deviation threshold P bias , randomly generate probability P, according to P bias The relationship between the size of P and random tree expansion is divided into two stages: exploration and convergence; (2) Design a fuzzy controller PbiasFuzzy to dynamically output the target bias threshold according to environmental factors; (3) Combining the artificial potential field method with the target bias threshold, an improved method for generating new nodes is proposed; New Node X new The extended definition is shown in formula (1): Among them, X′ goal For X nearest Point to X goal The point on the extension line of X that is step away, rand is the newly extended random point, X nearest represents the current node of the random tree expansion, X goal Indicates the target point; P bias As influencing factors, random points and artificial potential fields that have an impact on new nodes are dynamically allocated, where the configuration method of improving new nodes is shown in formula (2): Among them, X' goal For X nearest With X goal Extend the line to a point with a distance of step, ||X rand -X nearest || represents X rand With X nearest The Euclidean distance between Among them, F att is the gravitational force, which is defined as shown in formula (3): U att represents the gravitational function; K a represents the gravitational constant; ρ(X goal ,X nearest ) is a vector with a length of X nearest To X goal Euclidean distance, direction is X nearest Point to X goal , Among them, F rep is the repulsive force, which is defined as shown in formula (4): X obstacle represents the position of the obstacle; ρ(X obstacle ,X nearest ) is a vector with a length of X obstacle To X nearest Euclidean distance, direction is X obstacle Point to X nearest ; K r represents the repulsive force constant; ρ0 is the range of action of the repulsive field; 2. The UAV path planning method according to claim 1, characterized in that: When P <P bias When , the random tree is in the convergence stage, the direction of the sampling point is the direction of the target point, and the next sampling point is X nearest With X goal The sampling position on the extended line is step away.

3. The UAV path planning method according to claim 2, characterized in that: When P ≥ P bias When the random tree is in the exploration phase, it randomly generates sampling points X rand , and perform horizontal expansion of random trees.

4. The UAV path planning method according to claim 3, characterized in that: The fuzzy controller has two inputs and one output. The first input parameter is the obstacle density near the sampling point, the second input parameter is the Euclidean distance from the sampling point to the target point, and the output value is the target deviation threshold. The obstacle density ObsDensity and the distance from the sampling point to the target point GoalDist are configured with the same subset {NL, NM, ZE, PM, PL}. After normalization, the value range is [0, 1] and [0, 10]. The output target deviation threshold P bias The output range is [0,0.7] and the fuzzy subsets are {NL,NM,NS,ZE,PS,PM,PL}.

5. The UAV path planning method according to claim 4, characterized in that: The method comprises the following steps: Perform N iterations based on the FLC-APF-RRT* algorithm; Use numNodes to control the total number of generated nodes. If this limit is exceeded, the planning is considered a failure.

6. A UAV path planning system based on FLC-APF-RRT*, characterized in that: The system is configured to: (1) Preset a target deviation threshold P bias , randomly generate probability P, according to P bias The relationship between the size of P and random tree expansion is divided into two stages: exploration and convergence; (2) Design a fuzzy controller PbiasFuzzy to dynamically output the target bias threshold according to environmental factors; (3) Combining the artificial potential field method with the target bias threshold, an improved method for generating new nodes is proposed; New Node X new The extended definition is shown in formula (1): Among them, X′ goal For X nearest Point to X goal The point on the extension line of X that is step away, rand is the newly extended random point, X nearest represents the current node of the random tree expansion, X goal Indicates the target point; P bias As influencing factors, random points and artificial potential fields that have an impact on new nodes are dynamically allocated, where the configuration method of improving new nodes is shown in formula (2): Among them, X' goal For X nearest With X goal Extend the line to a point with a distance of step, ||X rand -X nearest || represents X rand With X nearest The Euclidean distance between Among them, F att is the gravitational force, which is defined as shown in formula (3): U att represents the gravitational function; K a represents the gravitational constant; ρ(X goal ,X nearest ) is a vector with a length of X nearest To X goal Euclidean distance, direction is X nearest Point to X goal , Among them, F rep is the repulsive force, which is defined as shown in formula (4): X obstacle represents the position of the obstacle; ρ(X obstacle ,X nearest ) is a vector with a length of X obstacle To X nearest Euclidean distance, direction is X obstacle Point to X nearest ; K r represents the repulsive force constant; ρ0 is the range of action of the repulsive field;

Images