A dynamic adaptive unmanned aerial vehicle path planning method based on RRT*

Abstract

The application relates to a dynamic self-adaptive unmanned aerial vehicle path planning method based on RRT*, which solves the technical problems that the existing unmanned aerial vehicle path planning method based on the RRT* algorithm has slow convergence speed, a high degree of path tortuosity, poor smoothness and to-be-improved efficiency; the method generates sampling points through adaptive Gaussian sampling according to adaptive target bias probability, uses an intelligent variable step length mechanism to use different step lengths in different exploration periods, and further obtains an optimized path by introducing Pareto evaluation of the degree of path tortuosity. The path planned by the application has fast convergence speed, low degree of path tortuosity and good smoothness.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of unmanned aerial vehicle path planning, in particular to a dynamic adaptive unmanned aerial vehicle path planning method based on RRT*. BACKGROUND

[0002] With the development of low-altitude economy, unmanned aerial vehicles are used in more and more industries, such as pesticide spraying, crop pollination, power inspection, disaster search, etc.

[0003] Unmanned aerial vehicle path planning refers to the process of finding the optimal path from the starting point to the target point for the unmanned aerial vehicle under certain environmental and task constraints. Currently, the unmanned aerial vehicle path planning algorithms mainly include A* algorithm, RRT algorithm, artificial potential field method, D* algorithm, genetic algorithm, particle swarm algorithm, etc.

[0004] The RRT algorithm can quickly find a feasible path from the starting point to the target point through random sampling and tree structure expansion. The invention patent application with the application publication number CN112987799A discloses an unmanned aerial vehicle path planning method based on improved RRT algorithm. In order to improve the performance of the RRT algorithm, the RRT* algorithm is proposed, which adds a path reconnection mechanism based on the RRT algorithm and can gradually converge to the optimal path, having the property of asymptotic optimality. However, the convergence speed of the RRT* algorithm is slow, especially in the environment where the target area is small or there are narrow channels, the generated path is highly tortuous, has poor smoothness, usually contains many inflection points, which is not conducive to the actual execution of the unmanned aerial vehicle; when dealing with three-dimensional space and complex obstacles, the efficiency and accuracy of the algorithm need to be improved; in the unmanned aerial vehicle flight, the algorithm has a long sampling time, and the effect is not good when the unmanned aerial vehicle flies autonomously. SUMMARY

[0005] The present application is to solve the technical problems of slow convergence speed, highly tortuous path, poor smoothness and low efficiency of the existing unmanned aerial vehicle path planning method based on RRT* algorithm, and provides a dynamic adaptive unmanned aerial vehicle path planning method based on RRT*.

[0006] The present application is an improvement on the conventional RRT* algorithm of the prior art.

[0007] The present application provides a dynamic adaptive unmanned aerial vehicle path planning method based on RRT*, comprising the following steps:

[0008] Step S1, creating a global map containing obstacles;

[0009] Step S2, determining the starting point and the end point of the unmanned aerial vehicle;

[0010] Step S3, initializing a tree structure from the starting point;

[0011] Step S4, calculating the adaptive target bias probability by the following formula (1) :

[0012] (1);

[0013] In formula (1), t is the exploration iteration progress, ,is the current iteration number, is the maximum iteration number; Step S5, calling a random number generation function to generate a random number r between [0, 1], if r is less than the adaptive target bias probability

[0014] , entering step S6, otherwise entering step S7; Step S6, generating a sampling point in a biased target sampling manner, randomly generating a sampling point at a certain step length in the direction of the new node to the end point vector in the current tree structure; after completion, entering step S8;

[0015] Step S7, generating a sampling point by an adaptive Gaussian sampling manner;

[0016] Step S8, finding the nearest node to the sampling point in the current tree

[0017] ; Step S9, determining the step length by an intelligent variable step length mechanism;

[0018] Step S10, expanding the tree to obtain a new node

[0019] ; Step S11, judging whether the line segment between the new node and the nearest node

[0020] collides with the obstacle by collision detection, if colliding with the obstacle, entering step S12; if not colliding, entering step S18; Step S12, generating a sampling point by an adaptive Gaussian sampling manner;

[0021] Step S13, expanding the tree to obtain a new node ;

[0022] Step S14, judging whether the line segment between the new node and the nearest node collides with the obstacle by collision detection, if colliding with the obstacle, entering step S15; if not colliding, entering step S18;

[0023] Step S15, entering step S16;

[0024] ​Step S15, determining the step length by intelligent variable step length mechanism;

[0025] Step S16, expanding the tree to obtain a new node ;

[0026] Step S17, judging whether the line segment between the new node and the nearest node collides with the obstacle by collision detection, if collides, entering step S15; if does not collide, entering step S18;

[0027] Step S18, adding the new node to the tree structure;

[0028] Step S19, determining the search radius;

[0029] Step S20, screening out the node with the minimum path cost in the circular region with the new node as the center and the search radius as the radius as the parent node of the new node;

[0030] Step S21, reconnecting and updating the tree structure;

[0031] Step S24, judging whether the new node reaches the terminal point, if yes, entering step S25; otherwise, entering step S4, continuing iteration;

[0032] Step S25, recording the path.

[0033] Preferably, step S7 is as follows:

[0034] Step S701, calculating the mean value of the Gaussian distribution , which is divided into two cases:

[0035] The first case is that, in the initial case, the tree structure only has the starting point, and the terminal point is taken as the mean value , ;

[0036] The second case is that, in the case that the tree structure has the new node and the feasible path,

[0037] (2);

[0038] In formula (2), is the new node in the current tree structure, is the terminal point, is the weight coefficient, , and t is the exploration iteration progress;

[0039] Step S702, calculating the dynamic covariance matrix:

[0040] (3);

[0041] In formula (3), is the standard deviation, is the identity matrix, is the scaling factor, t is the exploration iteration progress;

[0042] Step S703, further get the three-dimensional Gaussian distribution as :

[0043] (4);

[0044] In formula (4), is the spatial dimension;

[0045] Step S704, based on and generate 3 sampling points, respectively x1, x2, x3;

[0046] First, generate 4 groups of uniformly distributed random numbers:

[0047]

[0048]

[0049]

[0050]

[0051] Second, generate 3 groups of standard normal random numbers z by Box-Muller transformation on 4 groups of uniformly distributed random numbers:

[0052] Group 1:

[0053]

[0054]

[0055]

[0056] Group 2:

[0057]

[0058]

[0059]

[0060] Group 3:

[0061]

[0062]

[0063]

[0064] Then, the dynamic covariance matrix Σ is decomposed into a lower triangular matrix by Cholesky decomposition :

[0065] ;

[0066] Then, three sampling points x1, x2, x3 are generated, the first sampling point x1 is

[0067] ;

[0068] Expanded into component form:

[0069] ;

[0070] The second sampling point x2 is

[0071] ;

[0072] Expanded into component form:

[0073] ;

[0074] The third sampling point x3 is

[0075] ;

[0076] Expanded into component form:

[0077] ;

[0078] Finally, obstacle collision detection is performed between the line segments of the three sampling points and the new node in the current tree structure If there is no collision, it meets the requirements, and the corresponding sampling point that does not collide is selected and recorded in the array. If the three sampling points do not meet the requirements, they are regenerated. If the sampling points saved in the array are not unique, the sampling point saved in the array first is selected as the final sampling point. If the sampling points saved in the array are unique, the point is selected as the final sampling point.

[0079] Preferably, the process of determining the step length by the intelligent variable step length mechanism in step S9 is:

[0080] In step S901, the large step length factor is calculated by the following formula (5) :

[0081] (5);

[0082] In formula (5), h is an exploration space parameter, a is a step constant, and t is an exploration iteration progress;

[0083] A small step factor is calculated by the following formula (6) :

[0084] (6);

[0085] In formula (6), h is an exploration space parameter, and a is a step constant;

[0086] In step S902, a distance of the step is calculated by the following formula (7):

[0087] (7);

[0088] In formula (7), a random direction component is:

[0089] ;

[0090] Wherein, is a generated sampling point, is a node in a current tree closest to the sampling point;

[0091] A target direction component is:

[0092] ;

[0093] Wherein, is an end point;

[0094] A synthetic vector is:

[0095] ;

[0096] In step S12, a distance of the step is calculated by the following formula (8):

[0097] (8);

[0098] In formula (8), a random direction component is:

[0099] ;

[0100] Wherein, is a generated sampling point, is a node in a current tree closest to the sampling point;

[0101] A target direction component is:

[0102] ;

[0103] Wherein, is the end point;

[0104] The synthetic vector is:

[0105]

[0106] The process of step S15 is to determine the step length by an intelligent variable step length mechanism:

[0107] The distance of the step length is calculated by the following formula (9):

[0108] (9)

[0109] Simplified as:

[0110] (10)

[0111] In formula (10), the random direction component is:

[0112]

[0113] Wherein, the generated sampling point is The node closest to the sampling point in the current tree is

[0114] Preferably, after step S21, before step S24, steps S22 and S23 are added; step S22, whether a certain number of iterations is performed, if yes, then enter step S23, otherwise enter step S24;

[0115] Step S23, try to connect directly:

[0116] First, judge whether the Euclidean distance dist from the new node to the end point is less than or equal to the threshold dis, if dist≤dis, then perform collision detection, and judge whether the line segment between the new node and the end point collides with the obstacle through the collision detection, if collision, then enter step S24; if no collision, then form the last path segment by the new node and the end point, at this time, the complete path from the starting point to the end point is formed, and enter step S24;

[0117] If dist>dis, enter step S24.

[0118] The application also provides a dynamic adaptive unmanned aerial vehicle path planning method based on RRT*, which repeatedly performs the path planning method of any one of the above n times to obtain n paths, and performs Pareto evaluation on the n paths, the process being as follows:

[0119] Step 1), select the path with a dominance count of 0 from the n paths to obtain the Pareto front : ​​​

[0120] ;

[0121] wherein, is the path dominance count; is the path with path dominance count of 0;

[0122] If the Pareto frontier has only one path, the path is the optimal path, i.e., the post-optimization path; if the Pareto frontier has multiple paths, subsequent congestion degree calculation is performed;

[0123] Step 2), sort the multiple paths in the Pareto frontier according to the path length from small to large, and let the sorted path be:

[0124] ;

[0125] For , the congestion distance calculation formula in the path length dimension is:

[0126] ;

[0127] wherein, is the congestion degree of the path length, is the path length; is the maximum value of the path length; is the minimum value of the path length;

[0128] For and , let ;

[0129] Step 3), sort the multiple paths in the Pareto frontier according to the tortuosity from small to large, and let the sorted path be:

[0130] ;

[0131] For , the congestion distance calculation formula in the tortuosity dimension is:

[0132] ;

[0133] wherein, is the congestion degree distance of the path tortuosity, is the path tortuosity; is the maximum value of the path tortuosity; is the minimum value of the path tortuosity;

[0134] For and , let ;

[0135] Step 4), calculate the total congestion distance of the path

[0136] ;

[0137] In the formula, is the path corresponding path length congestion distance, is the path corresponding path tortuosity congestion distance;

[0138] Step 5), select the path with the maximum total congestion as the optimal path, that is, generate the optimized path.

[0139] The beneficial effects of the present application are: not only can the convergence speed of the path be accelerated, but also the smoothness of the planned path can be effectively improved, making it more suitable for the actual flight needs of the unmanned aerial vehicle. The path generation efficiency is high. The accuracy of path planning is improved for complex dense obstacles in three-dimensional space. In the 3D complex environment, efficient, smooth and reliable path planning is realized, achieving a more comprehensive and optimal path planning effect, suitable for navigation scenes of autonomous mobile devices such as unmanned aerial vehicles.

[0140] The intelligent variable step length mechanism uses different step lengths at different exploration periods, and can quickly explore when facing dense obstacles, improving the exploration efficiency. In the early exploration stage, the large step length factor affects the large-scale exploration in the early stage, and as the algorithm explores the space, the large step length factor also decreases, which cooperates with the fine exploration in the later period.

[0141] In the later exploration period, the target direct connection is introduced, which reduces the inflection points of the tree structure and speeds up the exploration speed.

[0142] The dynamic balance of "global exploration-local focus" is realized through adaptive Gaussian sampling and target bias strategy.

[0143] Further features and aspects of the present application will be made apparent from the following detailed description of specific embodiments, made with reference to the accompanying drawings. BRIEF DESCRIPTION OF DRAWINGS

[0144] Figure 1 is a flowchart of steps S1 to S11 of the dynamic adaptive unmanned aerial vehicle path planning method based on RRT*;

[0145] Figure 2 is a flowchart of steps S11 to S25 of the dynamic adaptive unmanned aerial vehicle path planning method based on RRT*;

[0146] Figure 3 is the effect of adaptive Gaussian sampling in tree structure expansion at 1 iteration; ​

[0147] Figure 4 Effect of adaptive Gaussian sampling in tree structure expansion at 10 iterations;

[0148] Figure 5 Effect of adaptive Gaussian sampling in tree structure expansion at 20 iterations;

[0149] Figure 6 Effect of adaptive Gaussian sampling in tree structure expansion at 30 iterations;

[0150] Figure 7 Effect of adaptive Gaussian sampling in tree structure expansion at 40 iterations;

[0151] Figure 8 Distribution of sampling points in early exploration of adaptive Gaussian sampling;

[0152] Figure 9 Distribution of sampling points in middle exploration of adaptive Gaussian sampling;

[0153] Figure 10 Distribution of sampling points in late exploration of adaptive Gaussian sampling;

[0154] Figure 11 Effect of adaptive Gaussian sampling in 2D space path planning at 1 iteration;

[0155] Figure 12 Effect of adaptive Gaussian sampling in 2D space path planning at 5 iterations;

[0156] Figure 13 Effect of adaptive Gaussian sampling in 2D space path planning at 10 iterations;

[0157] Figure 14 Effect of adaptive Gaussian sampling in 2D space path planning at 20 iterations;

[0158] Figure 15 Effect of adaptive Gaussian sampling in 2D space path planning at 30 iterations;

[0159] Figure 16 Pareto front illustration;

[0160] Figure 17 Congestion degree illustration;

[0161] Figure 18 10 paths planned;

[0162] Figure 19 Multi-objective optimization evaluation on paths in Figure 18 Pareto optimal solution selected;

[0163] Figure 20 This is a comparison chart of the optimal path obtained through Pareto evaluation and existing paths based on RRT*, GB-RRT*, and Informed-RRT* algorithms;

[0164] Figure 21 yes Figure 20 XY projection of the path in the middle. Detailed Implementation

[0165] The present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments.

[0166] like Figure 21 and Figure 20 As shown, the dynamic adaptive UAV path planning method based on RRT* mainly includes the following steps:

[0167] Step S1: Create a global map that includes obstacles.

[0168] Step S2: Determine the starting point and ending point of the drone.

[0169] Step S3: Starting from the origin, initialize a tree structure.

[0170] Step S4, calculate the adaptive target bias probability using the following formula (1). :

[0171] (1);

[0172] In formula (1), t is the exploration iteration progress (ranging from 0 to 1). , It is the current iteration number. Maximum number of iterations. It's manually set. For example, if the maximum number of iterations is set to 100, initially the tree structure only has a starting point, and the current iteration count is 1, then t = 1 / 100 = 0.01, meaning t ≈ 0, and the target bias probability is 0.1, which is a low probability. Target bias probability The maximum value is 0.5. When 0 ≤ t < 0.3, it is the early stage of iteration. When 0.3 ≤ t < 0.8, it is the middle stage of iteration. When 0.8 ≤ t ≤ 1, it is the late stage of iteration.

[0173] Step S5: Call the random number generation function (rand()) to generate a random number r between [0,1]. If r is less than the adaptive target bias probability... If yes, proceed to step S6; otherwise, proceed to step S7.

[0174] Step S6, generating the sampling points in a target-biased sampling manner. Randomly generate the sampling points at a certain step length in the direction of the end-point vector of the new node in the current tree structure. The certain step length distance at this time is :

[0175]

[0176] In the above formula, h is a parameter of the exploration space, a is a step constant, and t is the exploration iteration progress. The new node in the current tree structure refers to the new node generated in the last iteration. After completion, go to step S8.

[0177] Step S7, generating the sampling points in an adaptive Gaussian sampling manner.

[0178] Step S701, calculating the mean value of the Gaussian distribution , in two cases:

[0179] In the first case, in the initial case, the tree structure only has a start point, and the end point is taken as the mean value , . In this way, the exploration can be preferentially performed in the direction of the end point, and the random dispersion of the sampling points in the initial stage can be avoided. In the second case, in the case of a new node in the tree structure and an existing feasible path,

[0180] (2)

[0181] In formula (2), is the new node in the current tree structure (the new node generated in the last iteration), is the end point, is a weight coefficient, , and t is the exploration iteration progress. The mean value is used to control the sampling center, preferentially explore in the direction of the target, and avoid the random dispersion of the sampling points in the initial stage;

[0182] Step S702, calculating the dynamic covariance matrix:

[0183] (3)

[0184] In formula (3), is the standard deviation, is the unit matrix (a three-dimensional diagonal matrix), is a scaling factor, , and t is the exploration iteration progress;

[0185] Step S703, further obtaining the three-dimensional Gaussian distribution as ,

[0186] (4)

[0187] In formula (4), for spatial dimensions;

[0188] Step S704, generating sampling points. Based on and Generate 3 sampling points, x1, x2, x3 respectively; each sampling point is a 3x1 vector;

[0189] First, generate 4 groups of uniformly distributed random numbers:

[0190]

[0191]

[0192]

[0193]

[0194] U(0,1) represents the uniform distribution in the interval (0,1);

[0195] Second, generate 3 groups of standard normal random numbers z through Box-Muller transformation on the 4 groups of uniformly distributed random numbers:

[0196] The first group (z corresponding to the first sampling point x1):

[0197]

[0198]

[0199]

[0200] The second group (z corresponding to the second sampling point x2):

[0201]

[0202]

[0203]

[0204] The third group (z corresponding to the third sampling point x3):

[0205]

[0206]

[0207]

[0208] Then, Cholesky decomposition is performed on the dynamic covariance matrix Σ, which is decomposed into a lower triangular matrix :

[0209]

[0210] There are 8 standard normal random numbers in total;

[0211] Then, 3 sampling points x1, x2, x3 are generated, the first sampling point x1 is:

[0212]

[0213] Expanded into component form:

[0214]

[0215] The second sampling point x2 is:

[0216]

[0217] Expanded into component form:

[0218] ;

[0219] The third sampling point x3 is:

[0220]

[0221] Expanded into component form:

[0222]

[0223] Finally, obstacle collision detection is performed between the line segments of the 3 sampling points and the new nodes in the current tree structure, and if there is no collision, the corresponding sampling point is selected and recorded in an array. If the 3 sampling points do not meet the requirements, they are regenerated. If the sampling points saved in the array are not unique, the sampling point saved in the array first is selected as the final sampling point, and the final sampling point is used for subsequent processing. If the sampling points saved in the array are unique, the point is selected as the final sampling point. Step S8, find the node closest to the sampling point in the current tree

[0224] .

[0225] Step S9, determine the step length by an intelligent variable step length mechanism.

[0226] Step S901, calculate the large step length factor by the following formula (5): :

[0227] (5)

[0228] ​In formula (5), h is an exploration space parameter, a is a step constant, and t is an exploration iteration progress. A small step factor is calculated by formula (6) below :

[0229] (6)

[0230] In formula (6), h is an exploration space parameter, and a is a step constant.

[0231] In step S902, a step distance is calculated by formula (7) below

[0232] (7)

[0233] In formula (7), a random direction component is

[0234]

[0235] wherein, is a generated sampling point, is a node in a current tree closest to the sampling point;

[0236] A target direction component is

[0237]

[0238] wherein, is an end point;

[0239] A synthetic vector is

[0240]

[0241] In step S10, a tree is expanded to obtain a new node , .

[0242] In step S11, it is judged by collision detection whether a line segment between the new node and a nearest node collides with an obstacle, and if the line segment collides with the obstacle, step S12 is entered; if the line segment does not collide with the obstacle, step S18 is entered.

[0243] In step S12, a step distance is calculated by formula (8) below

[0244] (8)

[0245] In formula (8), a random direction component is

[0246]

[0247] wherein, the generated sampling point, the node in the current tree closest to the sampling point;

[0248] The target direction component is:

[0249]

[0250] wherein, the end point; and the resultant vector is:

[0251]

[0252] Step S13, expanding the tree to obtain a new node , .

[0253] Step S14, judging whether a line segment between the new node and the node closest to the sampling point collides with an obstacle through collision detection, if the line segment collides with the obstacle, entering step S15; if the line segment does not collide with the obstacle, entering step S18.

[0254] Step S15, only using a pure random direction component, canceling the target direction component, and calculating the distance of the step length through the following formula (9):

[0255] (9)

[0256] Simplified as:

[0257] (10)

[0258] In formula (10), the random direction component is:

[0259]

[0260] wherein, the generated sampling point, the node in the current tree closest to the sampling point.

[0261] Step S16, expanding the tree to obtain a new node , .

[0262] Step S17, judging whether a line segment between the new node and the node closest to the sampling point collides with an obstacle through collision detection, if the line segment collides with the obstacle, entering step S15; if the line segment does not collide with the obstacle, entering step S18.

[0263] Step S18, adding the new node to the tree structure.

[0264] Step S19, determine the search radius.

[0265] The determination method of search radius in the conventional RRT* algorithm can be adopted. The search radius can also be determined as 2 times of the maximum step length, Generally, as the tree structure expands, the nodes of path search need to be more and more refined, and the search radius changes with the exploration process, and the dynamic radius formula is:

[0266] (11)

[0267] In formula (11), is the space dimension (the path in three-dimensional space, = 3), n is the number of space nodes, and γ is a constant related to the environment, usually taken as:

[0268] (12)

[0269] In formula (12), is the volume of a unit sphere in n-dimensional space, in three-dimensional space, , and free-space volume is the volume of free space.

[0270] Step S20, in the circular region with the new node as the center and the search radius determined, the node with the minimum path cost is selected as the parent node of the new node.

[0271] Step S21, reconnect and update the tree structure.

[0272] Step S22, whether the iteration is performed for 50 times, if yes, enter step S23, otherwise enter step S24.

[0273] Step S23, attempt direct connection. First, judge whether the Euclidean distance dist from the new node to the end point is less than or equal to the threshold dis, and the threshold dis is set as 1.15 times of the Euclidean distance between the start point and the end point. If dist≤dis, collision detection is performed, and it is judged through the collision detection whether the line segment between the new node and the end point collides with the obstacle, if collision, one-step direct connection cannot be performed, enter step S24; if no collision, the last path segment is formed by the new node and the end point, at this time the complete path from the start point to the end point is formed, enter step S24. If dist>dis, it is indicated that the distance is too far and cannot be connected in one step, enter step S24.

[0274] It can be seen that the target direct connection mechanism is introduced, and the current new node is periodically connected with the target point, such as attempting to connect once in the 50th iteration and attempting to connect once in the 100th iteration. The path winding caused by the expansion of the tree structure can be reduced, the path cost can be greatly reduced, the path between the current new node and the terminal point is locally optimal, the redundant iteration near the terminal point is avoided, and fast convergence to the terminal point is achieved.

[0275] In step S24, it is judged whether the new node reaches the terminal point. If yes, step S25 is entered; otherwise, step S4 is entered, and the iteration is continued. The judgment method of whether the new node reaches the terminal point can be performed according to the method in the conventional RRT* algorithm in the prior art.

[0276] In step S25, the path is recorded.

[0277] It can be seen that the above unmanned aerial vehicle path planning method, on the one hand, in the sampling point generation link, the terminal point is biased with a certain probability, and the sampling in the adaptive Gaussian space range is performed with a certain probability, so that the sampling strategy of “exploring in the early iteration and converging in the later iteration” is realized. In the early exploration, the lower target bias probability can make the algorithm preferentially explore the whole space. In the middle exploration, the moderate target bias probability can balance the global space exploration and the target exploration. In the later exploration, at this time, the large-scale space exploration is not the focus, and the fine exploration near the terminal point is performed. On the other hand, the intelligent variable step length mechanism is used to use different step lengths in different exploration periods, so that the exploration in different environments (open area, obstacle area, obstacle dense area) can be effectively performed, the exploration can be quickly performed when facing dense obstacles, and the exploration efficiency is effectively ensured. On the other hand, in the iteration process, the target direct connection mechanism is introduced, and fast convergence to the terminal point is achieved.

[0278] In order to observe the effect of adaptive Gaussian sampling in the expansion of the tree structure, the overall morphological evolution of adaptive Gaussian sampling is constructed in a three-dimensional space, the starting point (1, 2, 3) is set, the terminal point (8, 8, 8) is set, and the effect is as shown in Figure 21 Figures 3-7 ​In the upper left three-dimensional probability distribution diagram, the probability density of each point in the three-dimensional space is visualized by the color depth of the scatter points, and the "probability cloud" shape of the Gaussian distribution is intuitively displayed; in the lower right covariance ellipsoid, the geometric shape of the covariance matrix is visualized by the three-dimensional ellipsoid, and the spatial expansion characteristics of the distribution are directly displayed; in the lower left probability density evolution diagram, the horizontal coordinate value (axis value) represents the position where the mean values of the X, Y and Z axes in the three-dimensional space are the value, and the vertical coordinate represents the probability density value at the position, and the curves of different iteration numbers are distinguished by color mapping, clearly reflecting the iteration progress; in the lower right contour map, the spatial distribution of the probability density is displayed by the three-dimensional contour surface (Z value is equal), and the "shape contour" of the distribution is highlighted. The initial state surface has a large volume and an irregular shape, and with the increase of the iteration number, it shrinks into a compact surface close to a sphere (because the target covariance is a diagonal matrix).

[0279] The role of the dynamic covariance matrix in the algorithm, the sampling characteristics in the initial iteration (global exploration stage): the standard deviation Σ is close to the initial value, the Gaussian distribution covers a wide range, and the sampling points form a "loose probability cloud" near the target, as shown in Figures 3-7 The sampling characteristics in the middle iteration (β linear decay, transition stage): β gradually decreases from 1, the variance of the Gaussian distribution shrinks, the sampling points "focus" on the target direction, the randomness decreases, and the certainty increases, as shown in Figure 8 The sampling characteristics in the late iteration (β close to 0.3, local development stage): the standard deviation is reduced to 30% of the initial value, the sampling points are highly concentrated in the target neighborhood, and almost degenerate into "target-oriented" deterministic sampling, as shown in Figure 9 .

[0280] The effect of adaptive Gaussian sampling in two-dimensional space path planning is shown in Figure 10 , wherein the red point is the starting point, the green point is the target point, the pink triangle is the mean position, and the pink circular area is the dynamic sampling area. Figures 11-15 is 1 iteration, Figure 11 is 5 iterations, Figure 12 is 10 iterations, Figure 13 is 20 iterations, Figure 14 is 30 iterations.

[0281] Further to optimize, repeat the above steps S3-S25 ten times to obtain 10 paths. The 10 paths are evaluated by Pareto and the Pareto optimal path is selected as the optimized path. The two indicators of path length and tortuosity are evaluated, and the tortuosity is defined as the angle between the vector between two nodes and the vector of the adjacent next node (the tortuosity commonly used in the prior art). If the path length and tortuosity cost of a path are lower than those of other paths, the path is selected as the optimized path. The specific process is:

[0282] Step 1), select the path with a dominance count of 0 from n paths (such as 10 paths) to obtain the Pareto frontier :

[0283]

[0284] wherein, is the path dominance count; is the path with a dominance count of 0; as shown in Figure 15 ;

[0285] If the Pareto frontier has a unique path, the path is the optimal path, i.e., the optimized path is generated. If the Pareto frontier has multiple paths, subsequent congestion degree calculation is performed;

[0286] Step 2), sort the multiple paths in the Pareto frontier according to the path length from small to large, and let the sorted paths be:

[0287] ;

[0288] For , the congestion distance calculation formula in the path length dimension is:

[0289] ;

[0290] wherein, is the congestion degree of the path length, is the path length; is the longest value of the path length; is the shortest value of the path length;

[0291] For and , let ;

[0292] Step 3), sort the multiple paths in the Pareto frontier according to the tortuosity from small to large, and let the sorted paths be:

[0293] ;

[0294] For , the congestion distance calculation formula in the tortuosity dimension is:

[0295] ;

[0296] wherein, is the congestion degree distance of the path tortuosity, is the path tortuosity; is the maximum value of the path tortuosity; is the minimum value of the path tortuosity;

[0297] For and , let ;

[0298] Step 4), calculate the total congestion distance of the path :

[0299] ;

[0300] In the formula, is the corresponding path length congestion distance of the path ; is the corresponding path tortuosity congestion distance of the path ; the congestion is as shown in Figure 16 ;

[0301] Step 5), select the path with the maximum total congestion as the optimal path, that is, generate the optimized path.

[0302] The above method is verified to a certain extent through simulation. The simulation experiment running environment is: system Windows11, processor Intel(R) Core(TM) i5-13500HX, frequency 2.50 GHz, memory 16GB; a complex obstacle environment is built, and the complex obstacle environment map size is set to (1000, 1000, 1000), the starting point is (150, 150, 150), and the end point is (950, 950, 800). Figure 17 is the 10 paths planned, Figure 18 is the multi-objective optimization evaluation of the path Figure 19 , and the Pareto optimal solution is selected. Figure 18 is the comparison diagram of the optimal path obtained through the Pareto evaluation and the paths based on the RRT*, GB-RRT*, and Informed-RRT* algorithms in the prior art, Figure 20 is the XY projection of the path, and it can be seen that the tortuosity of the optimal path is obviously reduced compared with the other three algorithms, and the path bending is less. The path length and tortuosity are greatly improved. For example, Figure 21 , Figure 20 Figure 21As shown, through the simulation experiment of the complex environment map, it is known that the four algorithms all complete the path planning, and in the path length aspect, compared with the prior art RRT*, GB-RRT* and Informed-RRT*, the present application is shortened by 9.25%, 4.93% and 10.61% respectively, and in the planning time, it is reduced by 45.90%, 57.14% and 80.00% respectively, the extremely short path planning time guarantees the real-time performance of the path planning, and can be used in the scene which has strict requirements on the planning time; in the path bending degree aspect, since the other three algorithms of the prior art do not plan for the path bending degree, the path bending degree value fluctuates greatly, sometimes the bending degree is small, sometimes the bending degree is large, and the bending is more, compared with the algorithm of the present application, the path bending degree is reduced by 65.89%, 52.55% and 68.40% respectively; the node number is reduced by 92.60%, 92.67% and 96.34% respectively. The experimental result statistics are shown in Table 1.

[0303] Table 1 Experimental result statistics table

[0304]

[0305] The above-described specific embodiments are only the preferred embodiments of the present application, and the protection scope of the present application is not limited to the embodiments.

Claims

1. A dynamic adaptive unmanned aerial vehicle path planning method based on RRT*, characterized in that, Comprising the following steps: Step S1, creating a global map containing obstacles; Step S2, determining the starting point and the end point of the UAV; Step S3, initializing a tree structure from the starting point; Step S4, the adaptive target bias probability is calculated by the following formula (1) : （1）； In Equation (1), t is the exploration iteration progress, , is the current iteration number, the maximum iteration number; Step S5, call a random number generating function to generate a random number r between [0, 1], if r is less than the adaptive target bias probability , enter step S6, otherwise enter step S7; Step S6, generating sampling points in a target-biased sampling manner, randomly generating sampling points at a certain step length in the direction of the vector from the new node in the current tree structure to the end point; After completion, go to step S8; Step S7, generating sampling points in an adaptive Gaussian sampling manner: Step S701, calculating the mean value of the Gaussian distribution , two cases: In the first case, the initial situation, the tree structure has only a starting point, the end point being the mean value , ; In the second case, the tree structure has a new node, and there is an existing feasible path, （2）； In formula (2), is a new node in the current tree structure, is the end point, is a weight coefficient, t is the exploration iteration progress; Step S702, calculating the dynamic covariance matrix: （3）； In equation (3), is the standard deviation, is the identity matrix, is the scaling factor, t is the exploration iteration progress; Step S703, further obtain the three-dimensional Gaussian distribution as : （4）； In equation (4), for spatial dimensions; Step S704, based on and Generate 3 sampling points, respectively x1, x2, x3; First, generate 4 groups of uniformly distributed random numbers: ， ， ， ； Second, generate 3 groups of standard normal random numbers z through Box-Muller transformation on the 4 groups of uniformly distributed random numbers: The first group: ， ， ； The second group: ， ， ； The third group: ， ， ； Then, the dynamic covariance matrix Σ is decomposed into a lower triangular matrix by Cholesky decomposition : ； Then, generate 3 sampling points x1, x2, x3, the first sampling point x1 is: ； Expanded into component form: ； The second sampling point x2 is: ； Expanded into component form: ； The third sampling point x3 is: ； Expanded into component form: ； Finally, the three sampling points and the new nodes in the current tree structure were compared respectively. Obstacle collision detection is performed on the line segments between them. If there is no collision, it meets the requirements. The corresponding sampling point without collision is selected and added to the array. If none of the three sampling points meet the requirements, they are regenerated. If the sampling points stored in the array are not unique, the sampling point first saved in the array is selected as the final sampling point. If the sampling point stored in the array is unique, that point is selected as the final sampling point. Step S8, finding the nearest node to the sampling point in the current tree ; Step S9, determining the step length through an intelligent variable step length mechanism: Step S901, calculate the large step factor by the following formula (5) : （5）； In formula (5), h is the exploration space parameter, α is the step length constant, and t is the exploration iteration progress; The small step factor is calculated by the following equation (6) : （6）； In formula (6), h is the exploration space parameter, and α is the step length constant; Step S902, calculate the distance of the step length through the following formula (7): （7）； In formula (7), the random direction component is: ； wherein, is the generated sample point, is the node in the current tree closest to the sample point; The target direction component is: ； wherein end point; The resultant vector is: ； The process of determining the step length through the intelligent variable step length mechanism of step S9 is: calculate the distance of the step length through the following formula (8): （8）； In formula (8), the random direction component is: ； The target direction component is: ； The resultant vector is: ； The process of determining the step length through the intelligent variable step length mechanism of step S15 is: Calculate the distance of the step length through the following formula (9): （9）； Simplified as: （10）； In formula (10), the random direction component is: ； Step S10, extending the tree to get a new node ; Step S11, judging whether the line segment between the new node and the last node collides with the obstacle by collision detection, if collides with the obstacle, entering step S12; if not collides, entering step S18. the last node Step S12, judging whether the line segment between the new node and the last node collides with the obstacle by collision detection, if collides with the obstacle, entering step S13; if not collides, entering step S18. Step S12, generating sampling points in an adaptive Gaussian sampling manner; Step S13, extending the tree to get a new node ; Step S14: Determine the new node through collision detection. With the nearest node Check whether the line segment between them collides with an obstacle. If it collides with an obstacle, proceed to step S15; if it does not collide, proceed to step S18. Step S15, determining the step length through an intelligent variable step length mechanism; Step S16, the extended tree gets a new node ; Step S17, judging whether the line segment between the new node and the nearest node collides with the obstacle by collision detection, if collides with the obstacle, entering step S15; if not collides, entering step S18; with the obstacle If collides with the obstacle, entering step S15; if not collides, entering step S18; Step S18, adding a new node to the tree structure; Step S19, determining the search radius; Step S20, selecting the node with the minimum path cost in the circular region with the new node as the center and the search radius as the radius as the parent node of the new node; Step S21, reconnecting and updating the tree structure; Step S24, judging whether the new node reaches the end point, if yes, go to step S25; otherwise, go to step S4 and continue iteration; Step S25, recording the path.

2. The RRT* based dynamic adaptive UAV path planning method of claim 1, wherein, After step S21 and before step S24, steps S22 and S23 are added; step S22, whether a certain number of iterations has been performed, if yes, go to step S23, otherwise go to step S24; Step S23, attempting direct connection: Firstly, it is judged whether the Euclidean distance dist from the new node to the end point is less than or equal to a threshold dis, if dist≤dis, collision detection is performed, and it is judged through the collision detection whether the line segment between the new node and the end point collides with the obstacle, if collision, step S24 is entered; if no collision, the last path segment is formed by the new node and the end point straight line, at this time the complete path from the start point to the end point is formed, and step S24 is entered; If dist>dis, step S24 is entered.

3. A dynamic adaptive UAV path planning method based on RRT*, characterized in that, The path planning method of claim 1 is repeated for several times to obtain n paths, and the n paths are subjected to Pareto evaluation, and the process is as follows: Step 1), select the path with the dominating count of 0 in n paths, get the Pareto front : ； wherein dominate the path count; dominate the path count is 0. If there is only one path in the Pareto front, the path is the optimal path, that is, the optimized path is generated; if there are multiple paths in the Pareto front, subsequent congestion degree calculation is performed; Step 2), the multiple paths in the Pareto front are sorted in ascending order according to the path length, and the sorted paths are: ； For The congestion distance formula in the path length dimension is: ； In the formula, is the congestion of the path length, is the path length; is the maximum value of the path length; is the minimum value of the path length; For and , let ; Step 3), the multiple paths in the Pareto front are sorted in ascending order according to the tortuosity, and the sorted paths are: ； For The formula for calculating the crowded distance in the tortuosity dimension is: ； In the formula, is a congestion distance of a degree of path tortuosity, is a degree of path tortuosity; is a maximum value of a degree of path tortuosity; is a minimum value of a degree of path tortuosity; For and , let ; Step 4), calculate path Total congestion distance: ； wherein is the path is the corresponding path length congestion distance, is the path is the corresponding path tortuosity congestion distance; Step 5), the path with the maximum total congestion degree is selected as the optimal path, that is, the optimized path is generated.

Images