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

Abstract

The application discloses a kind of based on APF-IRRT fusion algorithm's disaster relief unmanned plane path planning method, comprising the following steps: parameter initialization, node extension, path optimization and smoothing, and using cubic spline curve fitting or polynomial interpolation to the path is smoothed, finally, through setting algorithm termination condition, the optimized and smoothed unmanned plane flight path is output.The application of a kind of based on APF-IRRT fusion algorithm's disaster relief unmanned plane path planning method is combined with attractive force and repulsive force adaptive step and sampling angle optimization, realizes the high efficiency and obstacle avoidance safety of path planning, simultaneously, the smoothness and executability of path are improved by path smoothing processing, suitable for the efficient path planning of unmanned plane in complex disaster relief environment.

Description

TECHNICAL FIELD

[0001] The application belongs to the technical field of unmanned aerial vehicle path planning, and particularly relates to a disaster relief unmanned aerial vehicle path planning method based on an APF-IRRT fusion algorithm. BACKGROUND

[0002] As indispensable intelligent electronic devices in modern life, unmanned aerial vehicles continue to grow in the global trade market. Civil unmanned aerial vehicles can be divided into patrol unmanned aerial vehicles, agricultural unmanned aerial vehicles, meteorological unmanned aerial vehicles, exploration unmanned aerial vehicles, disaster relief unmanned aerial vehicles, etc. Based on this, unmanned aerial vehicles with higher integration, better computing efficiency and faster reaction time have been vigorously researched and developed by countries around the world. However, due to the unpredictability of disaster areas (such as severe weather, disaster areas, etc.), it is very challenging for unmanned aerial vehicles to complete precise identification, obstacle avoidance flight, and collaborative rescue tasks simply relying on radio remote control. Therefore, it is of great significance to develop an autonomous path planning algorithm for disaster relief unmanned aerial vehicles.

[0003] Disaster relief unmanned aerial vehicles generally fly at an altitude of 25 to 150 m. Signal interference caused by low-altitude clouds, smoke and dust is the main natural threat to unmanned aerial vehicle flight, which means that unmanned aerial vehicles should avoid interference from clouds, smoke and dust. Therefore, the path planning algorithm deployed on the unmanned aerial vehicle must be efficient, safe and have local dynamic planning capability.

[0004] In the field of global path planning for unmanned aerial vehicles, most are static, and the planned path is obtained after environment rasterization, which is not a truly optimized path and still needs further optimization. However, for rescue and disaster relief tasks, whether for reconnaissance or transportation tasks, not only the most basic guarantee in path safety is required, but also a higher requirement for running time is proposed. The improved fusion algorithm proposed in this paper has enhanced performance in dynamic and static obstacle avoidance environments. Not only does it meet the requirements of static obstacle avoidance, but it also has good safety performance when encountering moving obstacles. Moreover, although the required computation time is shortened to some extent, the heading angle is improved a lot, and more importantly, it is suitable for complex and variable real environments. SUMMARY

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

[0006] The technical solution of the present application is as follows:

[0007] A disaster relief unmanned aerial vehicle path planning method based on an APF-IRRT fusion algorithm, comprising the following steps:

[0008] Step 1, parameter initialization, comprising the following steps:

[0009] S1, define the task space and state space;

[0010] S2, define the UAV dynamics model;

[0011] S3, parameter initialization, including: defining the starting point Q start and the target point Q goal , determining algorithm parameters, and defining a search tree;

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

[0013] S4, attractive and repulsive force calculation;

[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 rand within the angle range, collision detection, and node selection;

[0015] S6, dynamic step size adjustment;

[0016] S7, new node generation;

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

[0018] S8, calculate the turning angle and set the constraint condition;

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

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

[0021] Step 4, algorithm termination condition, including: reaching the target point and reaching the maximum number of iterations, the specific condition for reaching the target point being: when the distance between the new node Q new and the target point Q goal satisfies

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

[0023] where ∈ represents a preset convergence threshold, i.e., the distance tolerance of the new node to the target point;

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

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

[0026] Step 5, output result: output the optimized and smoothed UAV flight path.

[0027] Further, the step S1 is specifically: defining the task space Z:

[0028] Z=Z free ∪Z obs ,

[0029] Wherein, Z free represents a safe region of flight, i.e. an obstacle-free region, Z obs represents a dangerous region with obstacles;

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

[0031]

[0032] That is, the safe region and the obstacle region do not overlap each other.

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

[0034] x(0)=x0,

[0035] Wherein, represents the rate of change of the UAV state x(t), x(t) represents the UAV 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, x0 represents the initial state of the system.

[0036] Further, the algorithm parameters in the step S3 include: attractive force coefficient, repulsive force coefficient, maximum step length, safety distance, maximum turning angle, sampling angle initial value, sampling angle increment and maximum sampling number, and the expression of the search tree is:

[0037] T={Q start},

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

[0039] Wherein, T represents the search tree, Q start represents the starting point coordinates of the path planning.

[0040] Further, the expression of the attractive force and the repulsive force calculation in the step S4 is:

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

[0042]

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

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

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

[0046]

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

[0048] wherein θ init represents the initial value of the sampling angle, i.e. 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 the step S5 are specifically: performing collision detection on the path between the nearest node Q near in the current search tree and the randomly sampled point P rand , if the path does not cross the obstacle, retaining the randomly sampled point P rand as a candidate node; if the path crosses the obstacle, increasing the angle range:

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

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

[0052] Further, the expression of the dynamic step adjustment in the step S6 is:

[0053]

[0054] wherein S v represents the dynamic step, k represents the step adjustment proportion constant, d represents the distance from the current node to the obstacle, d * represents the safety distance, step max represents the maximum step, i.e. the maximum moving distance allowed when the node is expanded.

[0055] Further, the step S7 comprises the following steps: calculating a new node Q new and performing collision detection, the calculation formula of the new node Q new is:

[0056]

[0057] S v ≤step max ,

[0058]

[0059] wherein Q new represents the new node coordinate, Q near represents the coordinate of the nearest node in the current search tree, S v represents the dynamic step, F total represents the resultant force vector, step max represents the maximum step, 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 specifically comprises: performing collision detection on the path between Q near and Q new , if there is no collision, adding Q new to the search tree; if there is a collision, returning to the step S5, adjusting the angle range and re-sampling.

[0061] Further, the calculation formula of the turning angle in the step S8 is:

[0062]

[0063] wherein γ represents the turning angle, Q new represents the new node coordinate, Qnear Q represents the coordinates of the nearest node in the current search tree, Q prev represents Q near , the parent node coordinates of Q

[0064] The constraint condition is: if |γ|>γ max , the path needs to be optimized, γ max represents the maximum allowed turning angle.

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

[0066] (1) Input discrete path points: suppose that the original path is composed of a series of nodes:

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

[0068] wherein Q1 represents the starting point, Q n represents the end point, Q i represents the i-th node in the path planning, x i represents the coordinates of the node Q i on the x-axis, y i represents the coordinates of the node Q i on the y-axis, and n represents the total number of points in the path.

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

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

[0071] wherein S i (t) represents the i-th spline function, t represents the normalized parameter, a i , b i , c i , d i represent polynomial coefficients, which are solved by boundary conditions.

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

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

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

[0075] The first derivative continuity is that the velocity must be continuous at the junction of adjacent segments, i.e.

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

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

[0078] The second derivative continuity is that the acceleration must be continuous at the junction of adjacent segments, i.e.

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

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

[0081] The expression of the start and end point conditions is:

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

[0083] where v start represents the velocity boundary condition of the start point, v end represents the velocity boundary condition of the end point;

[0084] (4) Solve the polynomial coefficients: according to the boundary conditions, construct a linear equation group, and solve a i , b i , c i , d i, the equation group is solved by using the Gaussian elimination method, and the expression of the equation group 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] Wherein, i represents the index number in the path, n represents the total number of nodes in the path, y i represents the vertical coordinate of the i-th node in the path, y i+1 represents the vertical coordinate of the i+1-th node in the path;

[0092] (5), generating a smooth path: on each segment, calculate the continuous points with spline function Si(t) to form a smooth path:

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

[0094] Wherein, P smooth represents the smooth path point set, and t represents the standardization parameter;

[0095] (6), output the smooth path: output the smooth path point, the path is smoother, suitable for unmanned aerial vehicle flight.

[0096] The beneficial effects of the present application are:

[0097] 1. Improve path planning efficiency: The present application fuses artificial potential field method (APF) and improved rapid random tree algorithm (IRRT), guides the search tree to expand efficiently towards the target point, reduces invalid search, and improves path search efficiency;

[0098] 2. Enhance obstacle avoidance capability: The present application introduces the direction of the resultant force of attraction and repulsion, dynamically adjusts the search angle range and step length, so that the unmanned aerial vehicle can effectively avoid obstacles, and improves the safety of path planning;

[0099] 3. Realize dynamic step length adjustment: The present application dynamically adjusts the expansion step length according to the distance between the unmanned aerial vehicle and the obstacle, realizes the balance between path smoothing and obstacle avoidance in complex environment, and reduces the degree of path tortuosity;

[0100] 4. Optimize path smoothness: The present application smoothes the path by cubic spline curve fitting or polynomial interpolation, effectively reduces the curvature change of the path, generates a continuous and smooth path, and is suitable for actual flight of the unmanned aerial vehicle;

[0101] 5. Steering angle constraint and path optimization: The present application 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 the consumption of computing resources: The present application limits the sampling angle by adaptive search range, reduces blind sampling, reduces the computational amount and memory occupation of the algorithm, and improves the real-time performance of the algorithm;

[0103] 7. Provide termination condition: The present application sets the distance threshold to the target point and the maximum number of iterations to ensure that the algorithm can terminate on time, avoid infinite search, and improve the reliability of the planning result;

[0104] 8. Adapt to complex disaster relief environment: The present application is suitable for disaster relief tasks in complex environment, and can ensure that the unmanned aerial vehicle realizes efficient and stable path planning and flight control in the environment with obstacles and dynamic conditions;

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

[0106] Figure 1 is a node expansion mode schematic diagram of the disaster relief unmanned aerial vehicle path planning method based on the APF-IRRT fusion algorithm of the present application.

[0107] Figure 2 is an angle increasing sampling method of a disaster relief unmanned aerial vehicle path planning method based on an APF-IRRT fusion algorithm. DETAILED DESCRIPTION

[0108] As shown in the figure, a disaster relief unmanned aerial vehicle path planning method based on an APF-IRRT fusion algorithm includes the following steps: Figures 1-2

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

[0110] S1, define the task space and the state space;

[0111] S2, define the unmanned aerial vehicle dynamics model;

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

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

[0114] S4, attractive force and repulsive force calculation;

[0115] S5, adaptive search range, including: limiting the sampling angle range according to the direction of the resultant force and searching for a random sampling point P rand in the angle range, collision detection and node selection (it should be noted that in the RRT algorithm, the sampling mechanism of the algorithm itself does not change, and the slow convergence speed problem still exists, therefore, on the basis of the RRT algorithm, the angle increasing sampling method is introduced, that is, the original full map random sampling is retained, the search range is restricted in a specific angle space, Figure 2 which demonstrates the basic principle of the method: taking the direction of the resultant force F of Q near as the starting axis, and the counterclockwise direction as the positive direction, sampling within ±θ around the F direction, then performing collision detection on the connection between the new node Q new and Q near , if the detection fails, then increase △ to the current angle θ, and then sample and perform collision detection again within ±(θ+△), the process continues until (θ+△) falls within the range [0, π], this method can effectively reduce the randomness and blindness of node growth in the search process, speed up the convergence speed, and improve the efficiency);

[0116] S6, dynamic step adjustment;

[0117] S7, new node generation;

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

[0119] S8, calculate the steering angle and set the constraint condition;

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

[0121] S10, path smoothing: using curve fitting or polynomial interpolation, the path is smoothed to reduce the curvature change of the path;

[0122] Step 4, algorithm termination condition, including: reaching the target point and reaching the maximum iteration number, the specific condition of reaching the target point is: when the distance between the new node Q new and the target point Q goal satisfies:

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

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

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

[0126] The maximum iteration number is specifically: if the maximum sampling number COUNT_MAX is reached, the search is ended and the information of not finding a feasible path is returned;

[0127] Step 5, output result: output the optimized and smoothed UAV flight path.

[0128] Further, the step S1 is specifically: defining the task space Z:

[0129] Z=Z free ∪Z obe ,

[0130] Wherein, Z free represents the safe area of flight, that is, the obstacle-free area, and Z obs represents the dangerous area with obstacles;

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

[0132]

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

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

[0135] x(0) = x0,

[0136] wherein, represents the rate of change of the UAV state x(t), x(t) represents the UAV state, including: position information and velocity 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] Further, the algorithm parameters in the step S3 include: attractive force coefficient, repulsive force coefficient, maximum step length, safety distance, maximum steering angle, sampling angle initial value, sampling angle increment and maximum sampling number, and the expression of the search tree is:

[0138] T = {Q start},

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

[0140] wherein, T represents the search tree, Q start represents the starting point coordinates of the path planning.

[0141] Further, the expression of the attractive force and the repulsive force calculation in the step S4 is:

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

[0143]

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

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

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

[0147]

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

[0149] wherein θ init represents the initial value of the sampling angle, i.e. 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: performing collision detection on the path between the nearest node Q near in the current search tree and the random sampling point P rand If the path does not cross the obstacle, the random sampling point P rand is kept as a candidate node; if the path crosses the obstacle, the angle range is increased:

[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 rand is discarded and a new point is randomly sampled.

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

[0154]

[0155] wherein S v represents the dynamic step, k represents the step adjustment proportion constant, d represents the distance from the current node to the obstacle, d * represents the safety distance, and step max represents the maximum step, i.e. the maximum movement distance allowed when expanding the node.

[0156] Further, step S7 includes the following steps: calculating a new node Q new and performing collision detection, wherein the calculation formula of the new node Q new is:

[0157]

[0158] S v ≤step max ,

[0159]

[0160] wherein Q new represents a new node coordinate, Q near represents the latest node coordinate in the current search tree, S v represents a dynamic step size, F total represents a resultant force vector, step max represents a maximum step size, F x represents a component of the resultant force in the x direction, F y represents a component of the resultant force in the y direction;

[0161] The collision detection specifically comprises: performing collision detection on a path between Q near and Q new If there is no collision, Q new is added to the search tree; if there is a collision, step S5 is returned to, the angle range is adjusted, and resampling is performed.

[0162] Further, the calculation formula of the turning angle in step S8 is:

[0163]

[0164] wherein γ represents a turning angle, Q new represents a new node coordinate, Q near represents the latest node coordinate in the current search tree, Q prev represents a parent node coordinate of Q near , and · represents a vector dot product.

[0165] The constraint condition is: if |γ|>γ max , the path needs to be optimized, γ max represents a maximum allowable turning angle.

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

[0167] (1) input discrete path points: suppose that an original path is composed of a series of nodes:

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

[0169] wherein Q1 represents a starting point, Q n represents an ending point, Q i represents an i-th node in path planning, x i represents a node Qi The coordinates on the x-axis, y i Represents node Q i The coordinates on the y-axis, where n represents the total number of points in the path;

[0170] (2) Constructing 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 spline function of the i-th segment, t represents the standardized parameter, and a i b i c i d i Denotes the polynomial coefficients, which are solved using boundary conditions;

[0173] (3) Set boundary conditions, including: positional continuity, first derivative continuity, second derivative continuity, and start and end point conditions. Positional continuity means that the curve must be continuous at each node, i.e.:

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

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

[0176] The continuity of the first derivative means that the velocity must be continuous at the boundary point between adjacent segments, i.e.: S′ i (1)=S′ i+1 (0),

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

[0178] The second derivative continuity is defined as follows: at the boundary between adjacent segments, the acceleration must be continuous, that is:

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

[0180] where S" i (1) represents the second derivative of the i-th segment of the spline function at t = 1, S" i+1 (0) represents the second derivative of the i+1-th segment of the spline function at t = 0;

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

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

[0183] where v start represents the velocity boundary condition at the start point, and v end represents the velocity boundary condition at the end point;

[0184] (4) Solving the polynomial coefficients: according to the boundary conditions, a linear equation set is constructed, and a i , b i , c i , d i are solved, and the equation set is solved by using the Gaussian elimination method, and the expression of the equation set 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] wherein i represents the index number in the path, n represents the total number of nodes in the path, y i represents the longitudinal coordinate of the i-th node in the path, y i+1 represents the longitudinal coordinate of the i+1-th node in the path;

[0192] (5) generating a smooth path: on each segment, calculate the continuous points with a spline function S i (t) to form a smooth path:

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

[0194] wherein P smooth represents the set of smooth path points, and t represents the normalized parameter;

[0195] (6) outputting the smooth path: output the smooth path points, the path is smoother, and is suitable for unmanned aerial vehicle flight.

[0196] The specific embodiments of the present application are introduced above in combination with the drawings, but the protection scope of the present application is not limited. Those skilled in the art should understand that the technical solutions of the present application are modified or replaced equivalently without departing from the protection scope of the present technical solutions, and should be covered in the protection scope of the claims of the present application.

Claims

1. A disaster relief unmanned aerial vehicle path planning method based on an APF-IRRT fusion algorithm, characterized in that, The method comprises the following steps: Step 1, parameter initialization, comprising the following steps: S1, defining the task space and the state space; S2, defining the unmanned aerial vehicle dynamics model; S3, planning parameter initialization, including: defining the starting point Q start with the target point Q goal determining algorithm parameters, defining search tree; Step 2, node expansion, comprising the following steps: S4, calculating the attractive force and the repulsive force; S5, adaptive search range, including: limiting the sampling angle range according to the direction of the resultant force and searching for a random sampling point in the angle range P rand Collision detection and node selection; S6, dynamic step adjustment; S7, generating a new node; Step 3, path optimization and smoothing, comprising the following steps: S8, calculating the steering angle and setting the constraint condition; S9, path cutting and optimization, comprising: path cutting and local path optimization, wherein the path cutting specifically comprises: segment cutting is performed on the generated global path, and the local path optimization specifically comprises: for each path segment, it is checked whether a straight line between the start point and the end point passes through an obstacle, if there is no collision, the intermediate nodes are deleted, the two nodes are directly connected, the number of turns is reduced, and it is ensured that the steering angle meets the constraint condition; S10, path smoothing: using curve fitting or polynomial interpolation, the path is smoothed to reduce the curvature change of the path; Step 4, algorithm termination condition, including: reaching the target point and reaching the maximum number of iterations, the specific condition of reaching the target point is: when the distance between the new node Q new and the target point Q goal satisfies; , wherein, represents a preset convergence threshold, i.e., a distance tolerance of the new node to the target point; The search is ended, and the final path is outputted; The maximum iteration number is reached, the search is ended, and information that a feasible path is not found is returned; Step 5, outputting the result: outputting the optimized and smoothed unmanned aerial vehicle flight path.

2. The disaster relief unmanned aerial vehicle path planning method based on the APF-IRRT fusion algorithm according to claim 1, characterized in that, The step S1 is specifically: defining the task space Z : ， wherein, Z free represents a safe region of flight, i.e. an obstacle-free region, Z obs represents a dangerous region with obstacles; The expression of the state space is: ， That is, the safety area and the obstacle area do not overlap each other.

3. The disaster relief unmanned aerial vehicle path planning method based on the APF-IRRT fusion algorithm according to claim 1, characterized in that, The expression of the unmanned aerial vehicle dynamics model in the step S2 is: ， wherein represents the state of the UAV x ( t ) represents a rate of change of the state of the UAV, x ( t ) represents the state of the UAV, including: position information and velocity information, h ( t ) represents a control input, represents a system dynamics function, x (0) represents the state of the system at the start time t = 0, x 0 represents the initial state of the system.

4. The disaster relief unmanned aerial vehicle path planning method based on the APF-IRRT fusion algorithm according to claim 1, characterized in that, The algorithm parameters in the step S3 comprise: the attractive force coefficient, the repulsive force coefficient, the maximum step length, the safety distance, the maximum steering angle, the initial value of the sampling angle, the sampling angle increment and the maximum sampling number, and the expression of the search tree is: ， ， wherein, T represents a search tree, Q start represents a start point coordinate of path planning.

5. The disaster relief unmanned aerial vehicle path planning method based on the APF-IRRT fusion algorithm according to claim 1, characterized in that, The expression of the attractive force and the repulsive force calculation in the step S4 is: ， ， ， wherein, Q goal represents the goal point coordinate of path planning, Q near represents the coordinate of the nearest node in the current search tree, ζ represents the attraction coefficient, F att represents the attraction force vector, Q obs represents the coordinate of the obstacle, η represents the repulsion coefficient, represents the distance from the current node to the obstacle, represents the safety distance, rep represents the repulsion force vector, total represents the resultant force vector.

6. The disaster relief unmanned aerial vehicle path planning method based on the APF-IRRT fusion algorithm according to claim 1, characterized in that, The expression of the sampling angle range limited according to the resultant force direction in the step S5 is: ， ， wherein θ init represents the initial value of the sampling angle, i.e. the angle of the force direction, F total,x represents the component of the force in the x direction, x represents the component of the force in the y direction, F total,y represents the component of the force in the y direction, Δ θ represents the sampling angle increment; The collision detection and node selection in step S5 specifically involves: selecting the nearest node in the current search tree. Q near and random sampling points P rand Collision detection is performed on the paths between them. If the path does not cross an obstacle, a random sampling point is retained. P rand If the path crosses an obstacle, the angle range is increased. ， and continue sampling in a new range of angles; if no suitable node is found in all angles, discard the randomly sampled point P rand and re-sample a new point randomly.

7. The disaster relief unmanned aerial vehicle path planning method based on the APF-IRRT fusion algorithm according to claim 1, characterized in that, The expression of the dynamic step adjustment in the step S6 is: ， wherein, S v denotes a dynamic step size, k denotes a step size adjustment proportionality constant, d denotes the distance of the current node to the obstacle, denotes a safety distance, step max denotes a maximum step size, i.e. the maximum distance of movement allowed when the node expands.

8. The disaster relief unmanned aerial vehicle path planning method based on the APF-IRRT fusion algorithm according to claim 1, characterized in that, The step S7 comprises the steps of calculating new nodes Q new and performing collision detection, the new nodes Q new The calculation formula is: ， ， ， wherein, Q new represents the coordinates of the new node, Q near represents the coordinates of the most recent node in the current search tree, S v represents the dynamic step size, F total represents the resultant force vector, step max represents the maximum step size, 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: collision detection is performed on the path between Q near and Q new If there is no collision, the path Q new is added to the search tree; if there is a collision, step S5 is returned, the angle range is adjusted, and resampling is performed.

9. The disaster relief unmanned aerial vehicle path planning method based on the APF-IRRT fusion algorithm according to claim 1, characterized in that, The calculation formula of the steering angle in the step S8 is: ， where γ denotes the steering angle, Q new denotes the new node coordinate, Q near denotes the most recent node coordinate in the current search tree, Q prev denotes Q near the parent node coordinate of denotes the vector dot product; The constraint condition is: if |γ| > γ max max, then the path needs to be optimized, γ max max represents the maximum allowed turning angle.

10. The disaster relief unmanned aerial vehicle path planning method based on the APF-IRRT fusion algorithm according to claim 1, characterized in that, The curve fitting smoothing in the step S10 is spline curve smoothing, comprising the following steps: (1), inputting discrete path points: assuming that an original path is composed of a series of nodes: ， wherein, Q 1 denotes a start, Q n denotes an end, Q i denotes the i-th node in the path planning, i x i denotes a node Q i coordinates in the x axis, y i denotes a node Q i coordinates in the y axis, n denotes the total number of points in the path.​ (2) Constructing a cubic spline function: between two adjacent path points Q i and Q i+1 , constructing a cubic polynomial function S i ( t ) : ， wherein S i t denotes the i segment spline function, t denotes the normalized parameter, denotes the polynomial coefficients, which are solved by the boundary conditions;​ (3), setting boundary conditions, comprising: position continuity, first-order derivative continuity, second-order derivative continuity and start point and end point conditions, wherein the position continuity is that the curve must be continuous at each node, that is: ， wherein S i (1) denotes the first i value of the 1st t segment spline function at S i+1 (0) denotes the first i+ value of the 1st t segment spline function at The first-order derivative continuity is that the velocity must be continuous at the junction points of adjacent segments, that is: ， wherein denotes the first derivative of the i piecewise spline function with respect to t =1, denotes the first derivative of the i+ 1piecewise spline function with respect to t =0; The second-order derivative continuity is that the acceleration must be continuous at the junction points of adjacent segments, that is: ， wherein denotes the first i derivative of the piecewise t =1, the second derivative of the piecewise i+ =0, the third t derivative of the piecewise The expression of the start point and end point conditions is: ， wherein, v start a velocity boundary condition at the start denoted by v end a velocity boundary condition at the end denoted by (4) Solving the polynomial coefficients: according to the boundary conditions, a linear equation set is constructed, and is solved , the expression of the equation set is solved by using the Gaussian elimination method. ， ， ， ， ， ， wherein, i denotes the index number in the path, n denotes the total number of nodes in the path, y i denotes the longitudinal coordinate of the i 1th node in the path, y i+1 denotes the longitudinal coordinate of the i+ 1th node in the path; (5) Generating a smooth path: On each segment, a spline function is used S i ( t ) to calculate consecutive points, forming a smooth path: ， wherein, P smooth denotes the set of smoothed path points, t denotes a normalization parameter; (6), outputting the smoothed path: the path points after smoothing are outputted, the path is smoother, and is suitable for unmanned aerial vehicle flight.

Images