Disaster relief unmanned aerial vehicle path planning method based on APF-IRRT fusion algorithm

Abstract

The invention discloses a disaster relief unmanned aerial vehicle path planning method based on an APF-IRRT fusion algorithm, and the method comprises the following steps: initializing parameters, expanding nodes, optimizing and smoothing a path, carrying out the smoothing of the path through cubic spline curve fitting or polynomial interpolation, and finally, carrying out the planning of a disaster relief unmanned aerial vehicle path. And outputting the optimized and smoothed flight path of the unmanned aerial vehicle by setting an algorithm termination condition. According to the disaster relief unmanned aerial vehicle path planning method based on the APF-IRRT fusion algorithm, the attraction force and the repulsive force are combined with adaptive step length and sampling angle optimization, the high efficiency of path planning and the obstacle avoidance safety are realized, and meanwhile, the smoothness and the performability of the path are improved through path smoothing processing; the method is suitable for efficient path planning of unmanned aerial vehicles in complex disaster relief environments.

Description

Technical Field

[0001] The present invention belongs to the technical field of unmanned aerial vehicle path planning, and in particular relates to a disaster relief unmanned aerial vehicle path planning method based on an APF-IRRT fusion algorithm. Background Art

[0002] As an indispensable intelligent electronic device in modern life, drones continue to grow in the global trade market. Civilian drones can be divided into patrol drones, agricultural drones, meteorological drones, exploration drones, disaster relief drones, etc. Based on this, drones with higher integration, better computing efficiency, and faster response time have received strong R&D investment from countries around the world. However, due to the unpredictability of disaster areas (such as bad weather, disaster areas, etc.), it is very challenging for drones to complete tasks such as accurate identification, obstacle avoidance flight, and coordinated rescue by relying solely on radio remote control. Therefore, it is of great significance to develop autonomous path planning algorithms for disaster relief drones.

[0003] Disaster relief drones generally fly at altitudes between 25 and 150 meters above sea level. Signal interference caused by low-altitude clouds, smoke, and dust is the main natural threat to drone flight, which means that drones should avoid interference from clouds, smoke, and dust. Therefore, the path planning algorithm deployed on the drone must be efficient, safe, and have local dynamic planning capabilities.

[0004] In the field of global path planning for drones, most of them are static, and the planned paths are obtained after the environment is rasterized. They are not truly optimized paths and still need further optimization. However, for disaster rescue missions, whether it is reconnaissance or transportation missions, not only must the most basic guarantee be made in terms of path safety, but also higher requirements are placed on the running time. The improved fusion algorithm proposed in this paper has enhanced performance in both dynamic and static obstacle avoidance environments. It not only meets the requirements for static obstacle avoidance, but also has good safety performance when encountering moving obstacles. Moreover, although the required calculation time is shortened, the heading angle is improved a lot, and more importantly, it is suitable for complex and changeable real environments. Summary of the invention

[0005] The technical problem to be solved by the present invention is to provide a disaster relief UAV path planning method based on the APF-IRRT fusion algorithm, which can achieve high efficiency of path planning and obstacle avoidance safety, improve the smoothness and executability of the path, and is suitable for efficient path planning of UAVs in complex disaster relief environments.

[0006] The technical solution of the present invention is:

[0007] A disaster relief UAV path planning method based on APF-IRRT fusion algorithm includes the following steps:

[0008] Step 1: Parameter initialization, including the following steps:

[0009] S1. Define task space and state space;

[0010] S2, define the UAV dynamics model;

[0011] S3, initialization of planning parameters, including: defining the starting point Q start and the target point Q goal , determine the algorithm parameters and define the search tree;

[0012] Step 2: Node expansion, including the following steps:

[0013] S4, calculation of attraction and repulsion;

[0014] S5, adaptive search range, including: limiting the sampling angle range according to the direction of the resultant force and searching for random sampling points P within the angle range rand , collision detection and node selection;

[0015] S6, dynamic step length adjustment;

[0016] S7, new node generation;

[0017] Step 3: Path optimization and smoothing, including the following steps:

[0018] S8, calculating the steering angle and setting constraints;

[0019] S9, path cutting and optimization, including: path cutting and local path optimization, the path cutting specifically includes: segmenting the generated global path, and the local path optimization specifically includes: for each path segment, checking whether the straight line between the starting point and the end point crosses an obstacle, if there is no collision, deleting the intermediate node, directly connecting the two nodes, reducing the number of turns, and ensuring that the steering angle meets the constraint conditions;

[0020] S10, path smoothing: use curve fitting or polynomial interpolation to smooth the path and reduce the curvature change of the path;

[0021] Step 4: Algorithm termination conditions include: reaching the target point and reaching the maximum number of iterations. The specific conditions for reaching the target point are: when the new node Q new and the target point Q goal The distance between them is satisfied;

[0022] ||Q new -Q goal ||≤∈,

[0023] Among them, ∈ represents the preset convergence threshold, that is, the distance tolerance from the new node to the target point;

[0024] End the search and output the final path;

[0025] The reaching of the maximum number of iterations is specifically: if the maximum number of sampling times COUNT_MAX is reached, the search is terminated and the information that no feasible path is found is returned;

[0026] Step 5. Output results: Output the optimized and smoothed UAV flight path.

[0027] Furthermore, the step S1 specifically includes: defining the task space Z:

[0028] Z=Z free ∪Z obs ,

[0029] Among them, Z free Indicates the safe flying area, i.e. the obstacle-free area, Z obs Indicates a dangerous area with obstacles;

[0030] Then the expression of the state space is:

[0031]

[0032] That is, the safety area and the obstacle area do not overlap.

[0033] Furthermore, the expression of the UAV dynamics model in step S2 is:

[0034] x(0)=x0,

[0035] in, represents the rate of change of the drone state x(t), x(t) represents the drone state, including: position information and speed information, h(t) represents the control input, Γ represents the system dynamics function, x(0) represents the state of the system at the starting time t=0, and x0 represents the initial state of the system.

[0036] Furthermore, the algorithm parameters in step S3 include: attraction coefficient, repulsion coefficient, maximum step length, safety distance, maximum steering angle, sampling angle initial value, sampling angle increment and maximum sampling times, and the expression of the search tree is:

[0037] T={Q start},

[0038] Q start =(x0,y0),

[0039] Among them, T represents the search tree, Q start Indicates the starting coordinates of the path planning.

[0040] Furthermore, the expression for calculating the attractive force and the repulsive force in step S4 is:

[0041] F att =ζ(Q goal -Q near ),

[0042]

[0043] F total =F att +F rep ,

[0044] Among them, Q goal represents the coordinates of the target point of path planning, Q near represents the coordinates of the nearest node in the current search tree, ζ represents the attraction coefficient, and F att represents the attraction vector, Q obs represents the coordinates of the obstacle, η represents the repulsion coefficient, d=||Q near -Q obs || represents the distance from the current node to the obstacle, d * Indicates the safety distance, F rep represents the repulsive force vector, F total represents the resultant force vector.

[0045] Furthermore, the expression for limiting the sampling angle range according to the direction of the resultant force in step S5 is:

[0046]

[0047] [θ init -△θ,θ init +△θ],

[0048] Among them, θ init Indicates the initial value of the sampling angle, that is, the angle of the resultant force direction, F total,x represents the component of the resultant force in the x direction, F total,y represents the component of the resultant force in the y direction, and Δθ represents the sampling angle increment;

[0049] The collision detection and node selection in step S5 are specifically as follows: near and random sampling point P rand The path between them is collided. If the path does not cross the obstacle, the random sampling point P is retained. rand , as a candidate node; if the path passes through an obstacle, increase the angle range:

[0050] θ init ←θ init +△θ,

[0051] And continue sampling in the new angle range; if no suitable node is found in all angles, the random sampling point P is discarded rand , and re-randomly sample new points.

[0052] Furthermore, the expression for dynamic step length adjustment in step S6 is:

[0053]

[0054] Among them, S v represents the dynamic step size, k represents the step size adjustment proportional constant, d represents the distance from the current node to the obstacle, and d * Indicates safe distance, step max Indicates the maximum step size, that is, the maximum movement distance allowed when a node is expanded.

[0055] Furthermore, step S7 includes the following steps: calculating the new node Q new And perform collision detection, the new node Q new The calculation formula is:

[0056]

[0057] S v ≤step max ,

[0058]

[0059] Among them, Q new represents the new node coordinates, Q near Represents the coordinates of the nearest node in the current search tree, S v represents the dynamic step size, F total Represents the resultant force vector, step max Indicates the maximum step length, F x represents the component of the resultant force in the x direction, F y represents the component of the resultant force in the y direction;

[0060] The collision detection is specifically as follows: near and Q new The paths between them are collided. If there is no collision, Q new Add to the search tree; if there is a collision, return to step S5, adjust the angle range, and resample.

[0061] Furthermore, the calculation formula of the steering angle in step S8 is:

[0062]

[0063] Where γ represents the steering angle, Q new represents the new node coordinates, Qnear Indicates the coordinates of the nearest node in the current search tree, Q prev Indicates Q near The parent node coordinates of , · represents the vector dot product;

[0064] The constraint condition is: if |γ|>γ max , then the path needs to be optimized, γ max Indicates the maximum permissible steering angle.

[0065] Furthermore, the curve fitting smoothing in step S10 is spline curve smoothing, comprising the following steps:

[0066] (1) Input discrete path points: Assume that the original path consists of a series of nodes:

[0067] Q={Q1,Q2,...,Q n}, Q i =(x i ,y i ),

[0068] Among them, Q1 represents the starting point, Q n Indicates the end point, Q i represents the i-th node in path planning, x i Represents node Q i The coordinate on the x-axis, y i Represents node Q i The coordinate on the y-axis, n represents the total number of points in the path;

[0069] (2) Construct a cubic spline function: Q between two adjacent path points i and Q i+1 , construct a cubic polynomial function S i (t):

[0070] S i (t) = a i +b i t+c i t 2 +d i , t 3 , t∈[0,1],

[0071] Among them, S i (t) represents the i-th segment spline function, t represents the standardized parameter, a i , b i ,c i , d i represents the polynomial coefficients, solved by boundary conditions;

[0072] (3) Set boundary conditions, including position continuity, first-order derivative continuity, second-order derivative continuity, and start and end point conditions. Position continuity means that the curve must be continuous at each node, that is:

[0073] S i (1)) = S i+1 (0),

[0074] Among them, S i (1) represents the value of the i-th segment spline function at t = 1, S i+1 (0) represents the value of the i+1th segment spline function at t=0;

[0075] The first-order derivative continuity means that the velocity at the junction of adjacent segments must be continuous, that is:

[0076] S′ i (1) = S′ i-1 (0),

[0077] Among them, S′ i (1) represents the first-order derivative of the i-th segment spline function at t = 1, S′ i+1 (0) represents the first-order derivative of the i+1th segment of the spline function with respect to t=0;

[0078] The second-order derivative is continuous: at the intersection of adjacent segments, the acceleration must be continuous, that is:

[0079] S″ i (1) = S″ i+1 (0),

[0080] Among them, S″ i (1) represents the second-order derivative of the i-th spline function at t=1, S″ i+1 (0) represents the second-order derivative of the i+1th segment of the spline function with respect to t=0;

[0081] The expressions of the starting point and end point conditions are:

[0082] S′1(0)=v start , S′ n (1) = v end ,

[0083] Among them, v start represents the velocity boundary condition at the starting point, v end represents the velocity boundary condition at the end point;

[0084] (4) Solve the polynomial coefficients: According to the boundary conditions, construct a linear equation system and solve a i , b i , c i , d i, use Gaussian elimination method to solve the equation system, the expression of the equation system is:

[0085] S i (0) = y i , S i (1) = y i+1 ,

[0086] a i =y i , a i +b i +c i +d i =y i+1 , i=1,2,...,n-1,

[0087] b i +2c i +3d i =b i+1 , i = 1, 2, ..., n-2,

[0088] 2c i +6d i =2c i+1 , i = 1, 2, ..., n-2,

[0089] b1=v start ,

[0090] b n-1 +2c n-1 +3d n-1 =v end ,

[0091] Among them, i represents the index number in the path, n represents the total number of nodes in the path, and y i Indicates the ordinate of the i-th node in the path, y i+1 Represents the ordinate of the i+1th node in the path;

[0092] (5) Generate a smooth path: On each segment, use the spline function Si(t) to calculate the continuous points to form a smooth path:

[0093] P smooth ={S1(t), S2(t), ..., S n (y)}, t∈[0,1],

[0094] Among them, P smooth represents the set of smoothed path points, and t represents the standardized parameter;

[0095] (6) Output smoothed path: Output the smoothed path points, making the path smoother and more suitable for UAV flight.

[0096] Beneficial effects of the present invention:

[0097] 1. Improve path planning efficiency: The present invention integrates the artificial potential field method (APF) and the improved fast random tree algorithm (IRRT) to guide the search tree to efficiently expand toward the target point, reduce invalid searches, and improve path search efficiency;

[0098] 2. Enhanced obstacle avoidance capability: The present invention introduces the combined force direction of attraction and repulsion, dynamically adjusts the search angle range and step length, enables the drone to effectively avoid obstacles, and improves the safety of path planning;

[0099] 3. Dynamic step length adjustment: The present invention dynamically adjusts the extended step length according to the distance between the drone and the obstacle, achieving both path smoothness and obstacle avoidance in complex environments, and reducing the tortuosity of the path;

[0100] 4. Optimize path smoothness: The present invention smoothes the path through cubic spline curve fitting or polynomial interpolation, effectively reducing the curvature change of the path and generating a continuous and smooth path suitable for actual UAV flight;

[0101] 5. Steering angle constraint and path optimization: The present invention checks and deletes redundant nodes through path cutting and local path optimization, reduces unnecessary turns, ensures that the path meets the maximum steering angle constraint, and improves flight stability;

[0102] 6. Reduce computing resource consumption: The present invention uses an adaptive search range to limit the sampling angle, which reduces blind sampling, reduces the amount of algorithm calculation and memory usage, and improves the real-time performance of the algorithm;

[0103] 7. Providing termination conditions: The present invention sets the distance threshold to the target point and the maximum number of iterations to ensure that the algorithm can terminate on time, avoid falling into infinite search, and improve the reliability of the planning results;

[0104] 8. Adapt to complex disaster relief environments: The present invention is suitable for disaster relief missions in complex environments, and can ensure that the UAV can achieve efficient and stable path planning and flight control in environments with obstacles and dynamic conditions;

[0105] In summary, the disaster relief UAV path planning method based on the APF-IRRT fusion algorithm of the present invention effectively improves the efficiency, obstacle avoidance capability and path smoothness of UAV path planning, and is suitable for disaster relief applications in complex scenarios. BRIEF DESCRIPTION OF THE DRAWINGS

[0106] Figure 1 It is a schematic diagram of a node expansion method of a disaster relief UAV path planning method based on an APF-IRRT fusion algorithm of the present invention.

[0107] Figure 2 It is a schematic diagram of an angle-increasing sampling method of a disaster relief UAV path planning method based on an APF-IRRT fusion algorithm of the present invention. DETAILED DESCRIPTION

[0108] like Figure 1-2 As shown, a disaster relief UAV path planning method based on the APF-IRRT fusion algorithm includes the following steps:

[0109] Step 1: Parameter initialization, including the following steps:

[0110] S1. Define task space and state space;

[0111] S2, define the UAV dynamics model;

[0112] S3, initialization of planning parameters, including: defining the starting point Q start and the target point Q goal , determine the algorithm parameters and define the search tree;

[0113] Step 2: Node expansion, including the following steps:

[0114] S4, calculation of attraction and repulsion;

[0115] S5, adaptive search range, including: limiting the sampling angle range according to the direction of the resultant force and searching for random sampling points P within the angle range rand , collision detection and node selection (It should be noted that: in the RRT algorithm, the sampling mechanism of the algorithm itself has not changed, and the problem of slow convergence speed still exists. Therefore, the angle-increasing sampling method is introduced on the basis of the RRT algorithm, that is, the original full-image random sampling is retained, and the search range is constrained, and the initial search range is limited to a specific angle space. Figure 2 The basic principle of the method is demonstrated: near The direction of the resultant force F is the starting axis, the counterclockwise direction is the positive direction, sampling is performed within the range of ±θ around the F direction, and then the new node Q new and Q near The connection between them is collided. If the detection fails, △ is increased to the current angle θ, and then sampling and collision detection are performed again within the range of ±(θ+△). This process continues until (θ+△) falls within the range [0,π]. This method can effectively reduce the randomness and blindness of node growth during the search process, speed up convergence, and improve efficiency);

[0116] S6, dynamic step length adjustment;

[0117] S7, new node generation;

[0118] Step 3: Path optimization and smoothing, including the following steps:

[0119] S8, calculating the steering angle and setting constraints;

[0120] S9, path cutting and optimization, including: path cutting and local path optimization, the path cutting specifically includes: segmenting the generated global path, and the local path optimization specifically includes: for each path segment, checking whether the straight line between the starting point and the end point crosses an obstacle, if there is no collision, deleting the intermediate node, directly connecting the two nodes, reducing the number of turns, and ensuring that the steering angle meets the constraint conditions;

[0121] S10, path smoothing: use curve fitting or polynomial interpolation to smooth the path and reduce the curvature change of the path;

[0122] Step 4: Algorithm termination conditions include: reaching the target point and reaching the maximum number of iterations. The specific conditions for reaching the target point are: when the new node Q new and the target point Q goal The distance between them is satisfied;

[0123] ||Q new -Q goal ||≤∈,

[0124] Among them, ∈ represents the preset convergence threshold, that is, the distance tolerance from the new node to the target point;

[0125] End the search and output the final path;

[0126] The reaching of the maximum number of iterations is specifically: if the maximum number of sampling times COUNT_MAX is reached, the search is terminated and the information that no feasible path is found is returned;

[0127] Step 5. Output results: Output the optimized and smoothed UAV flight path.

[0128] Furthermore, the step S1 specifically includes: defining the task space Z:

[0129] Z=Z free ∪Z obe ,

[0130] Among them, Z free Indicates the safe flying area, i.e. the obstacle-free area, Z obs Indicates a dangerous area with obstacles;

[0131] Then the expression of the state space is:

[0132]

[0133] That is, the safety area and the obstacle area do not overlap.

[0134] Furthermore, the expression of the UAV dynamics model in step S2 is:

[0135] x(0)=x0,

[0136] in, represents the rate of change of the drone state x(t), x(t) represents the drone state, including: position information and speed information, h(t) represents the control input, Γ represents the system dynamics function, x(0) represents the state of the system at the starting time t=0, and x0 represents the initial state of the system.

[0137] Furthermore, the algorithm parameters in step S3 include: attraction coefficient, repulsion coefficient, maximum step length, safety distance, maximum steering angle, sampling angle initial value, sampling angle increment and maximum sampling times, and the expression of the search tree is:

[0138] T={Q start},

[0139] Q start =(x0,y0),

[0140] Among them, T represents the search tree, Q start Indicates the starting coordinates of the path planning.

[0141] Furthermore, the expression for calculating the attractive force and the repulsive force in step S4 is:

[0142] F att =ζ(Q goal -Q near ),

[0143]

[0144] F total =F att +F rep ,

[0145] Among them, Q goal represents the coordinates of the target point of path planning, Q near represents the coordinates of the nearest node in the current search tree, ζ represents the attraction coefficient, and F att represents the attraction vector, Q obs represents the coordinates of the obstacle, η represents the repulsion coefficient, d=||Q near -Q obs || represents the distance from the current node to the obstacle, d * Indicates the safety distance, F rep represents the repulsive force vector, F total represents the resultant force vector.

[0146] Furthermore, the expression for limiting the sampling angle range according to the direction of the resultant force in step S5 is:

[0147]

[0148] [θ init -△θ,θ init +△θ],

[0149] Among them, θ init Indicates the initial value of the sampling angle, that is, the angle of the resultant force direction, F total,x represents the component of the resultant force in the x direction, F total,y represents the component of the resultant force in the y direction, and Δθ represents the sampling angle increment;

[0150] The collision detection and node selection in step S5 are specifically as follows: near and random sampling point P rand The path between them is collided. If the path does not cross the obstacle, the random sampling point P is retained. rand , as a candidate node; if the path passes through an obstacle, increase the angle range:

[0151] θ init ←θ init +△θ,

[0152] And continue sampling in the new angle range; if no suitable node is found in all angles, the random sampling point P is discarded rand , and re-randomly sample new points.

[0153] Furthermore, the expression for dynamic step length adjustment in step S6 is:

[0154]

[0155] Among them, S v represents the dynamic step size, k represents the step size adjustment proportional constant, d represents the distance from the current node to the obstacle, and d * Indicates safe distance, step max Indicates the maximum step size, that is, the maximum movement distance allowed when a node is expanded.

[0156] Furthermore, step S7 includes the following steps: calculating the new node Q new And perform collision detection, the new node Q new The calculation formula is:

[0157]

[0158] S v ≤step max ,

[0159]

[0160] Among them, Q new represents the new node coordinates, Q near Represents the coordinates of the nearest node in the current search tree, S v represents the dynamic step size, F total Represents the resultant force vector, step max Indicates the maximum step length, F x represents the component of the resultant force in the x direction, F y represents the component of the resultant force in the y direction;

[0161] The collision detection is specifically: near and Q new The paths between them are collided. If there is no collision, Q new Add to the search tree; if there is a collision, return to step S5, adjust the angle range, and resample.

[0162] Furthermore, the calculation formula of the steering angle in step S8 is:

[0163]

[0164] Where γ represents the steering angle, Q new represents the new node coordinates, Q near Indicates the coordinates of the nearest node in the current search tree, Q prev Indicates Q near The parent node coordinates of , · represents the vector dot product;

[0165] The constraint condition is: if |γ|>γ max , then the path needs to be optimized, γ max Indicates the maximum permissible steering angle.

[0166] Furthermore, the curve fitting smoothing in step S10 is spline curve smoothing, comprising the following steps:

[0167] (1) Input discrete path points: Assume that the original path consists of a series of nodes:

[0168] Q={Q1,Q2,...,Q n}, Q i =(x i ,y i ),

[0169] Among them, Q1 represents the starting point, Q n Indicates the end point, Q i represents the i-th node in path planning, x i Represents node Qi The coordinate on the x-axis, y i Represents node Q i The coordinate on the y-axis, n represents the total number of points in the path;

[0170] (2) Construct a cubic spline function: Q between two adjacent path points i and Q i+1 , construct a cubic polynomial function S i (t):

[0171] S i (t) = a i +b i t+c i t 2 +d i t 3 , t∈[0,1],

[0172] Among them, S i (t) represents the i-th segment spline function, t represents the standardized parameter, a i , b i , c i , d i represents the polynomial coefficients, solved by boundary conditions;

[0173] (3) Set boundary conditions, including position continuity, first-order derivative continuity, second-order derivative continuity, and start and end point conditions. Position continuity means that the curve must be continuous at each node, that is:

[0174] S i (1)) = S i+1 (0),

[0175] Among them, S i (1) represents the value of the i-th segment spline function at t = 1, S i+1 (0) represents the value of the i+1th segment spline function at t=0;

[0176] The first-order derivative is continuous: the velocity at the intersection of adjacent segments must be continuous, that is: S' i (1) = S′ i+1 (0),

[0177] Among them, S′ i (1) represents the first-order derivative of the i-th segment spline function at t = 1, S′ i+1 (0) represents the first-order derivative of the i+1th segment of the spline function with respect to t=0;

[0178] The second-order derivative is continuous: at the intersection of adjacent segments, the acceleration must be continuous, that is:

[0179] S″i (1) = S″ i+1 (0),

[0180] Among them, S″ i (1) represents the second-order derivative of the i-th spline function at t=1, S″ i+1 (0) represents the second-order derivative of the i+1th segment of the spline function with respect to t=0;

[0181] The expressions of the starting point and end point conditions are:

[0182] S′ i (0) = v start , S′ n (1) = v end ,

[0183] Among them, v start represents the velocity boundary condition at the starting point, v end represents the velocity boundary condition at the end point;

[0184] (4) Solve the polynomial coefficients: According to the boundary conditions, construct a linear equation system and solve a i , b i , c i , d i , use Gaussian elimination method to solve the equation system, the expression of the equation system is:

[0185] S i (0) = y i , S i (1) = y i+1 ,

[0186] a i =y i , a i +b i +c i +d i =y i+1 , i=1,2,...,n-1,

[0187] b i +2c i +3d i =b i+1 , i = 1, 2, ..., n-2,

[0188] 2c i +6d i =2c i+1 , i = 1, 2, ..., n-2,

[0189] b1=v start ,

[0190] bn-1 +2c n-1 +3d n-1 =v end ,

[0191] Among them, i represents the index number in the path, n represents the total number of nodes in the path, and y i Indicates the ordinate of the i-th node in the path, y i+1 Represents the ordinate of the i+1th node in the path;

[0192] (5) Generate a smooth path: On each segment, use the spline function S i (t) Calculate the continuous points to form a smooth path:

[0193] P smooth ={S1(t), S2(t), ..., S n (0)}, t∈[0,1],

[0194] Among them, P smooth represents the set of smoothed path points, and t represents the standardized parameter;

[0195] (6) Output smoothed path: Output the smoothed path points, making the path smoother and more suitable for UAV flight.

[0196] Although the specific implementation of the present invention is introduced above in conjunction with the accompanying drawings, it does not limit the protection scope of the present invention. Those skilled in the art should understand that those non-creative modifications or equivalent replacements of the technical solution of the present invention, which do not depart from the protection scope of the technical solution, should be included in the scope of the claims of the present invention.

Claims

1. A disaster relief UAV path planning method based on APF-IRRT fusion algorithm, characterized in that: The following steps are involved: Step 1: Parameter initialization, including the following steps: S1. Define task space and state space; S2, define the UAV dynamics model; S3, initialization of planning parameters, including: defining the starting point Q start and the target point Q goal , determine the algorithm parameters and define the search tree; Step 2: Node expansion, including the following steps: S4, calculation of attraction and repulsion; S5, adaptive search range, including: limiting the sampling angle range according to the direction of the resultant force and searching for random sampling points P within the angle range rand , collision detection and node selection; S6, dynamic step length adjustment; S7, new node generation; Step 3: Path optimization and smoothing, including the following steps: S8, calculating the steering angle and setting constraints; S9, path cutting and optimization, including: path cutting and local path optimization, the path cutting specifically includes: segmenting the generated global path, and the local path optimization specifically includes: for each path segment, checking whether the straight line between the starting point and the end point crosses an obstacle, if there is no collision, deleting the intermediate node, directly connecting the two nodes, reducing the number of turns, and ensuring that the steering angle meets the constraint conditions; S10, path smoothing: use curve fitting or polynomial interpolation to smooth the path and reduce the curvature change of the path; Step 4: Algorithm termination conditions include: reaching the target point and reaching the maximum number of iterations. The specific conditions for reaching the target point are: when the new node Q new and the target point Q goal The distance between them is satisfied; ||Q new -Q goal ||≤∈, Among them, ∈ represents the preset convergence threshold, that is, the distance tolerance from the new node to the target point; End the search and output the final path; The reaching of the maximum number of iterations is specifically: if the maximum number of sampling times COUNT_MAX is reached, the search is terminated and the information that no feasible path is found is returned; Step 5. Output results: Output the optimized and smoothed UAV flight path.

2. According to claim 1, a disaster relief drone path planning method based on APF-IRRT fusion algorithm is characterized in that: The step S1 specifically includes: defining the task space Z: Z=Z free ∪Z obs , Among them, Z free Indicates the safe flying area, i.e. the obstacle-free area, Z obs Indicates a dangerous area with obstacles; Then the expression of the state space is: That is, the safety area and the obstacle area do not overlap.

3. The method for disaster relief UAV path planning based on APF-IRRT fusion algorithm according to claim 1 is characterized in that: The expression of the UAV dynamics model in step S2 is: in, represents the rate of change of the drone state x(t), x(t) represents the drone state, including: position information and speed information, h(t) represents the control input, Γ represents the system dynamics function, x(0) represents the state of the system at the starting time t=0, and x0 represents the initial state of the system.

4. The method for disaster relief UAV path planning based on APF-IRRT fusion algorithm according to claim 1 is characterized in that: The algorithm parameters in step S3 include: attraction coefficient, repulsion coefficient, maximum step length, safety distance, maximum steering angle, sampling angle initial value, sampling angle increment and maximum sampling times. The expression of the search tree is: T={Q start }, Q start =(x0,y0), Among them, T represents the search tree, Q start Indicates the starting coordinates of the path planning.

5. The method for disaster relief UAV path planning based on APF-IRRT fusion algorithm according to claim 1 is characterized in that: The expression for calculating the attractive force and the repulsive force in step S4 is: F att =ζ(Q goal -Q near ), F total =F att +F rep , Among them, Q goal represents the coordinates of the target point of path planning, Q near represents the coordinates of the nearest node in the current search tree, ζ represents the attraction coefficient, and F att represents the attraction vector, Q obs represents the coordinates of the obstacle, η represents the repulsion coefficient, d=||Q near -Q obs || represents the distance from the current node to the obstacle, d * Indicates the safety distance, F rep represents the repulsive force vector, F total represents the resultant force vector.

6. The method for disaster relief UAV path planning based on APF-IRRT fusion algorithm according to claim 1 is characterized in that: The expression for limiting the sampling angle range according to the direction of the resultant force in step S5 is: [i init -Dθ,θ init +Δθ], Among them, θ init Indicates the initial value of the sampling angle, that is, the angle of the resultant force direction, F total,x represents the component of the resultant force in the x direction, F total,y represents the component of the resultant force in the y direction, and Δθ represents the sampling angle increment; The collision detection and node selection in step S5 are specifically as follows: near and random sampling point P rand The path between them is collided. If the path does not cross the obstacle, the random sampling point P is retained. rand , as a candidate node; if the path passes through an obstacle, increase the angle range: θ init ←θ init +Δθ, And continue sampling in the new angle range; if no suitable node is found in all angles, the random sampling point P is discarded rand , and re-randomly sample new points.

7. The method for disaster relief UAV path planning based on APF-IRRT fusion algorithm according to claim 1 is characterized in that: The expression for dynamic step length adjustment in step S6 is: Among them, S v represents the dynamic step size, k represents the step size adjustment proportional constant, d represents the distance from the current node to the obstacle, and d * Indicates safe distance, step max Indicates the maximum step size, that is, the maximum movement distance allowed when a node is expanded.

8. The method for disaster relief UAV path planning based on APF-IRRT fusion algorithm according to claim 1 is characterized in that: The step S7 comprises the following steps: calculating the new node Q new And perform collision detection, the new node Q new The calculation formula is: S v ≤step max , Among them, Q new represents the new node coordinates, Q near Represents the coordinates of the nearest node in the current search tree, S v represents the dynamic step size, F total Represents the resultant force vector, step max Indicates the maximum step length, F x represents the component of the resultant force in the x direction, F y represents the component of the resultant force in the y direction; The collision detection is specifically: near and Q new The paths between them are collided. If there is no collision, Q new Add to the search tree; if there is a collision, return to step S5, adjust the angle range, and resample.

9. The method for disaster relief UAV path planning based on APF-IRRT fusion algorithm according to claim 1 is characterized in that: The calculation formula of the steering angle in step S8 is: Where γ represents the steering angle, Q new represents the new node coordinates, Q near Indicates the coordinates of the nearest node in the current search tree, Q prev Indicates Q near The parent node coordinates of , · represents the vector dot product; The constraint condition is: if |γ|>γ max , then the path needs to be optimized, γ max Indicates the maximum permissible steering angle.

10. The method for disaster relief UAV path planning based on APF-IRRT fusion algorithm according to claim 1, characterized in that: The curve fitting smoothing in step S10 is spline curve smoothing, comprising the following steps: (1) Input discrete path points: Assume that the original path consists of a series of nodes: Q={Q1,Q2,...,Q n },Q i =(x i ,y i ), Among them, Q1 represents the starting point, Q n Indicates the end point, Q i represents the i-th node in path planning, x i Represents node Q i The coordinate on the x-axis, y i Represents node Q i The coordinate on the y-axis, n represents the total number of points in the path; (2) Construct a cubic spline function: Q between two adjacent path points i and Q i+1 , construct a cubic polynomial function S i (t): Among them, S i (t) represents the i-th segment spline function, t represents the standardized parameter, a i , b i , c i , d i represents the polynomial coefficients, solved by boundary conditions; (3) Set boundary conditions, including position continuity, first-order derivative continuity, second-order derivative continuity, and start and end point conditions. Position continuity means that the curve must be continuous at each node, that is: S i (1)=S i+1 (0), Among them, S i (1) represents the value of the i-th segment spline function at t = 1, S i+1 (0) represents the value of the i+1th segment spline function at t=0; The first-order derivative continuity means that the velocity at the junction of adjacent segments must be continuous, that is: S′ i (1)=S′ i+1 (0), Among them, S′ i (1) represents the first-order derivative of the i-th segment spline function at t = 1, S′ i+1 (0) represents the first-order derivative of the i+1th segment of the spline function with respect to t=0; The second-order derivative is continuous: at the intersection of adjacent segments, the acceleration must be continuous, that is: S″ i (1)=S″ i+1 (0), Among them, S″ i (1) represents the second-order derivative of the i-th spline function at t=1, S″ i+1 (0) represents the second-order derivative of the i+1th segment of the spline function with respect to t=0; The expressions of the starting point and end point conditions are: S′1(0)=v start ,S′ n (1)=v end , Among them, v start represents the velocity boundary condition at the starting point, v end represents the velocity boundary condition at the end point; (4) Solve the polynomial coefficients: According to the boundary conditions, construct a linear equation system and solve a i , b i , c i , d i , use Gaussian elimination method to solve the equation system, the expression of the equation system is: S i (0)=y i ,S i (1)=and i+1 , a i =y i ,a i +b i +c i +d i =y i+1 ,i=1,2,...,n-1, b i +2c i +3d i =b i+1 ,i=1,2,...,n-2, 2c i +6d i =2c i+1 ,i=1,2,...,n-2, b1=v start , b n-1 +2c n-1 +3d n-1 =in end , Among them, i represents the index number in the path, n represents the total number of nodes in the path, and y i Indicates the ordinate of the i-th node in the path, y i+1 Represents the ordinate of the i+1th node in the path; (5) Generate a smooth path: On each segment, use the spline function S i (t) Calculate the continuous points to form a smooth path: P smooth ={S1(t),S2(t),…,S n (t)},t∈[0,1], Among them, P smooth represents the set of smoothed path points, and t represents the standardized parameter; (6) Output smoothed path: Output the smoothed path points, making the path smoother and more suitable for UAV flight.

Images