Unmanned aerial vehicle path planning method based on radar omnidirectional detection and improved A* algorithm

Abstract

The invention discloses an unmanned aerial vehicle path planning method based on radar omnidirectional detection and an improved A * algorithm, and the method comprises the steps: carrying out the down-sampling processing of an elevation topographic map, and calculating an elevation cost matrix of the whole elevation topographic map; detecting a dynamic threat target in a surrounding environment by using a radar omnidirectional detection technology, acquiring speed, direction and position information of the threat target, and performing power distribution on the dynamic threat target; according to the dynamic threat target information, the unmanned aerial vehicle performs flight path planning by adopting an improved A * algorithm to obtain an original path of the unmanned aerial vehicle on the elevation topographic map, and the original path is saved as a series of three-dimensional discrete path points; a cubic spline curve fitting algorithm is adopted to carry out smoothing processing on a series of obtained three-dimensional discrete path points, so that the three-dimensional discrete path points better conform to an actual flight path. According to the method, the problem that the dynamic threat target is difficult to avoid under a certain condition is effectively solved, and the efficiency and safety of unmanned aerial vehicle path planning are improved.

Description

Technical Field

[0001] The invention belongs to the technical field of autonomous track planning, and in particular relates to a method for unmanned aerial vehicle path planning based on radar omnidirectional detection and improved A* algorithm. Background Art

[0002] With the rapid development of UAV technology, UAVs have been widely used in military reconnaissance, disaster relief, environmental monitoring and other fields. However, when UAVs perform tasks in complex environments, their safety and efficiency face many challenges. Especially in complex terrains such as mountains, due to the dramatic terrain fluctuations and dense ground obstacles, the path planning of UAVs is very difficult. In addition, complex terrains are usually accompanied by the existence of dynamic threats, such as the sudden appearance of unknown obstacles or the threat of enemy targets, which further increases the difficulty of mission execution. How to plan a safe and efficient flight path for UAVs in a dynamic threat environment is an urgent problem to be solved. In view of the difficulty of path planning and poor pathfinding effect in mountainous environments, Peng Yi et al. proposed an improved PID search algorithm for mountain UAV path planning, which effectively improved the path planning ability of UAVs. For the regional reconnaissance mission of UAVs in mountainous environments, Wang Cheng et al. proposed an adaptive chaotic dung beetle optimization algorithm for UAV regional coverage path planning, which can efficiently complete the regional coverage task. For the problem of obstacle avoidance in mountainous path planning of UAVs, Hou Chenxi proposed a rolling speed obstacle method to detect whether the UAV collides with obstacles. This method can achieve obstacle avoidance of UAVs, and the obstacle avoidance path is relatively smoother. However, the above studies only consider obstacles in the direction of the drone's advance, and pay less attention to dynamic threats approaching the drone from all sides. During the drone's mission, dynamic threats may approach the drone from all sides, posing a security threat to the drone. Summary of the invention

[0003] Purpose of the invention: The purpose of the present invention is to provide a UAV path planning method based on radar omnidirectional detection and improved A* algorithm, which can effectively improve the safety of UAV path planning under dynamic threats.

[0004] Technical solution: The present invention discloses a UAV path planning method based on radar omnidirectional detection and improved A* algorithm, comprising:

[0005] S1. Downsample the elevation map and calculate the elevation cost matrix of the entire elevation map, so that subsequent UAVs can choose to fly around or over terrain obstacles;

[0006] S2. Use radar omnidirectional detection technology to detect dynamic threat targets in the surrounding environment, obtain the speed, direction and position information of the threat targets, and allocate power to the dynamic threat targets;

[0007] S3, according to the dynamic threat target information obtained in step S2, the UAV uses an improved A* algorithm to plan the flight path, obtains the original path of the UAV on the elevation map, and saves the original path as a series of three-dimensional discrete path points;

[0008] S4. Use a cubic spline curve fitting algorithm to smooth the obtained series of three-dimensional discrete path points to make them more consistent with the actual flight trajectory.

[0009] Furthermore, in step S1, calculating the elevation cost matrix of the entire elevation topographic map includes the following steps:

[0010] Step 1: Assume that the elevation matrix of the elevation map is A n×n , where n represents the number of grids divided by the elevation map. First, let the basic matrix B n×n Equal to A n×n ;

[0011] Step 2, then expand B according to the following formula n×n , in B n×n The four sides of the extended 1 order, assigned a value of ∞, that is, the flight path beyond the elevation map is set to infinity, the UAV is restricted to fly within the elevation map, and the part beyond the elevation map is set as a no-fly zone; the expanded matrix is ​​expressed as:

[0012]

[0013] Among them, the element b in the matrix 11 ,…,b nn Indicates the altitude of each grid point on the elevation map;

[0014] Step 3: Define that the drone can fly in eight directions, forward, backward, left, right, and diagonal, at each two-dimensional plane grid point during the process of flying from the starting point to the end point on the elevation map. Calculate the elevation cost matrix of the drone flying in the kth direction as C k =B ov -A n×n , where B ov is the matrix B (n+2)×(n+2) and the matrix A n×n The overlap matrix, C k Determine the elevation difference cost of a UAV flying to any point in a specified direction on an elevation map.

[0015] Further, in step S2, using radar omnidirectional detection technology to detect dynamic threat targets in the surrounding environment includes the following steps:

[0016] Step 1: Set the radar operating frequency f according to the mountain environment and flight mission. req , Scanning Angle Maximum detection distance d max and detection accuracy σ;

[0017] Step 2: The radar starts to perform 360-degree omnidirectional scanning, continuously transmitting and receiving radar signals, covering all directions around the drone;

[0018] Step 3: Analyze the received radar echo signal, apply the Doppler effect to measure the relative speed of the dynamic threat target, and determine whether the dynamic threat target is close to the drone and poses a threat;

[0019] Step 4: Evaluate the detected dynamic threat targets and allocate more detection power to the direction of the dynamic threat targets;

[0020] Step 5: Transmit the dynamic threat target information detected by the radar to the path planning module of the UAV to provide the dynamic threat target position and speed information.

[0021] Furthermore, in step S3, the UAV uses an improved A* algorithm to perform flight path planning, including the following steps:

[0022] Step 1: Input the starting grid coordinate position p of the drone on the elevation map start , the end point grid coordinate position p end and the UAV flight speed v;

[0023] Step 2: Create an OPEN table named T open and CLOSE table is T close ; In each step of the search, calculate the current node p i The loss function f(p i )=g(p i )+h(p i ), where g(p i ) is the cost of the distance traveled, h(p i ) is the distance from the current node to the end point, i is the number of nodes; the height of the current node h z Greater than the height h of the previous node z-1 When △h z =h z -h z-1 >0, the cost of the distance traveled is Otherwise g(p i )=△h x +△h y +△h z , where △h x , △h y Respectively represent the distance difference when the drone flies in the x direction and the y direction;

[0024] Step 3: When Topen If not empty, find T open The loss function f(p i ) minimum point, set it as the current node, and start from T open Remove if the current node is equal to p end , returned by T close The generated path, otherwise the current node is added to T close ;

[0025] Step 4: Calculate the neighboring points in the eight directions of the current node, front, back, left, right, and diagonal, and compare their loss functions f(p i ) and the elevation cost matrix C k If the neighboring point has neither obstacles nor is in the no-fly zone, that is, the neighboring point is valid, then determine whether the neighboring point is in T open If the neighboring point is already in T open Check if there is a smaller g(p i ) value, if any, update T open Otherwise, add the neighboring points to T open middle;

[0026] Step 5: Loop through steps 3 and 4 until p is found. end or T open Until it is empty.

[0027] According to the above steps, the original path of the UAV on the elevation map is obtained, and the original path is saved as a series of three-dimensional discrete path points.

[0028] Furthermore, in step S4, a cubic spline curve fitting algorithm is used to smooth the obtained series of three-dimensional discrete path points, including the following steps:

[0029] Step 1: Extract N discrete path points from a series of three-dimensional discrete path points saved in the original path, and represent the path points as three-dimensional coordinates Path = [(x1, y1, z1), (x2, y2, z2), …, (x N ,y N ,z N )], where (x j ,y j ,z j ) represents the position coordinates of the path point in the three-dimensional coordinate system, j = 0, 1, ..., N;

[0030] Step 2: Define the subinterval between two adjacent path points as [m i ,m i+1 ],i=0,1,…,N-1,where m i Indicates the cumulative arc length from the starting point to the i-th path point, m i+1Indicates the cumulative arc length from the starting point to the i+1th path point. In each subinterval [m i ,m i+1 ], respectively construct cubic spline interpolation functions for the x, y, and z directions; let the cubic spline interpolation function in the x direction be S x,i (f) = a x,i +b x,i (fm i )+c x,i (fm i ) 2 +d x,i (fm i ) 3 , the cubic spline interpolation function in the y direction is S y,i (f) = a y,i +b y,i (fm i )+c y,i (fm i ) 2 +d y,i (fm i ) 3 , the cubic spline interpolation function in the z direction is S z,i (f) = a z,i +b z,i (fm i )+c z,i (fm i ) 2 +d z,i (fm i ) 3 , where f represents the arc length independent variable of the three-dimensional path point on the subinterval, a x,i , b x,i 、c x,i ,d x,i Indicates S x,i (f) Coefficients of the cubic spline interpolation function. Similarly, a y,i , b y,i 、c y,i ,d y,i and a z,i , b z,i 、c z,i ,d z,i They are S y,i (f) and S z,i (f) coefficient;

[0031] Step 3: Construct a linear equation system for the x, y, and z directions respectively, and solve the linear equation system to solve the coefficients of the spline function;

[0032] Step 4: For each subinterval [m i ,mi+1 ], using the cubic spline function S in the x, y, and z directions respectively x,i (f),S y,i (f),S z,i (f) is used to generate the fitted path, and multiple points are taken in the sub-interval according to the accuracy requirements, the coordinates of these points are calculated, and the fitted smooth path can be obtained by connecting these three-dimensional points.

[0033] The system corresponding to the method includes:

[0034] The elevation cost matrix calculation unit is used to downsample the elevation terrain map and calculate the elevation cost matrix of the entire elevation terrain map, so that subsequent UAVs can choose to fly around or over terrain obstacles;

[0035] The target detection and power distribution unit is used to detect dynamic threat targets in the surrounding environment using radar omnidirectional detection technology, obtain the speed, direction and position information of the threat targets, and distribute power to the dynamic threats;

[0036] The path planning unit is used to plan the flight path of the UAV using the improved A* algorithm according to the acquired dynamic threat target information, obtain the original path of the UAV on the elevation map, and save the original path as a series of three-dimensional discrete path points;

[0037] The path smoothing processing unit is used to use a cubic spline curve fitting algorithm to smooth a series of three-dimensional discrete path points obtained to make them more consistent with the actual flight trajectory.

[0038] Optionally, the drone uses an improved A* algorithm to plan the flight path, including the following steps:

[0039] Step 1: Input the starting grid coordinate position p of the drone on the elevation map start , the end point grid coordinate position p end and the UAV flight speed v;

[0040] Step 2: Create an OPEN table named T open and CLOSE table is T close ; In each step of the search, calculate the current node p i The loss function f(p i )=g(p i )+h(p i ), where g(p i ) is the cost of the distance traveled, h(p i ) is the distance from the current node to the end point, i is the number of nodes; the height of the current node h z Greater than the height h of the previous node z-1 When △hz =h z -h z-1 >0, the cost of the distance traveled is Otherwise g(p i )=△h x +△h y +△h z , where △h x , △h y Respectively represent the distance difference when the drone flies in the x direction and the y direction;

[0041] Step 3: When T open If not empty, find T open The loss function f(p i ) minimum point, set it as the current node, and start from T open Remove if the current node is equal to p end , returned by T close The generated path, otherwise the current node is added to T close ;

[0042] Step 4: Calculate the neighboring points in the eight directions of the current node, front, back, left, right, and diagonal, and compare their loss functions f(p i ) and the elevation cost matrix C k If the neighboring point has neither obstacles nor is in the no-fly zone, that is, the neighboring point is valid, then determine whether the neighboring point is in T open If the neighboring point is already in T open Check if there is a smaller g(p i ) value, if any, update T open Otherwise, add the neighboring points to T open middle;

[0043] Step 5: Loop through steps 3 and 4 until p is found. end or T open Until it is empty.

[0044] According to the above steps, the original path of the UAV on the elevation map is obtained, and the original path is saved as a series of three-dimensional discrete path points.

[0045] Optionally, a cubic spline curve fitting algorithm is used to smooth the obtained series of three-dimensional discrete path points, including the following steps:

[0046] Step 1: Extract N discrete path points from a series of three-dimensional discrete path points saved in the original path, and represent the path points as three-dimensional coordinates Path = [(x1, y1, z1), (x2, y2, z2), …, (x N ,y N ,z N)], where (x j ,y j ,z j ) represents the position coordinates of the path point in the three-dimensional coordinate system, j = 0, 1, ..., N;

[0047] Step 2: Define the subinterval between two adjacent path points as [m i ,m i+1 ],i=0,1,…,N-1,where m i Indicates the cumulative arc length from the starting point to the i-th path point, m i+1 Indicates the cumulative arc length from the starting point to the i+1th path point. In each subinterval [m i ,m i+1 ], respectively construct cubic spline interpolation functions for the x, y, and z directions; let the cubic spline interpolation function in the x direction be S x,i (f) = a x,i +b x,i (fm i )+c x,i (fm i ) 2 +d x,i (fm i ) 3 , the cubic spline interpolation function in the y direction is S y,i (f) = a y,i +b y,i (fm i )+c y,i (fm i ) 2 +d y,i (fm i ) 3 , the cubic spline interpolation function in the z direction is S z,i (f) = a z,i +b z,i (fm i )+c z,i (fm i ) 2 +d z,i (fm i ) 3 , where f represents the arc length independent variable of the three-dimensional path point on the subinterval, a x,i 、b x,i 、c x,i d x,i Indicates S x,i (f) Coefficients of the cubic spline interpolation function. Similarly, a y,i 、b y,i 、c y,i d y,i and az,i , b z,i 、c z,i ,d z,i They are S y,i (f) and S z,i (f) coefficient;

[0048] Step 3: Construct a linear equation system for the x, y, and z directions respectively, and solve the linear equation system to solve the coefficients of the spline function;

[0049] Step 4: For each subinterval [m i ,m i+1 ], using the cubic spline function S in the x, y, and z directions respectively x,i (f),S y,i (f),S z,i (f) is used to generate the fitted path, and multiple points are taken in the sub-interval according to the accuracy requirements, the coordinates of these points are calculated, and the fitted smooth path can be obtained by connecting these three-dimensional points.

[0050] An electronic device for storing and executing the method, the device comprising:

[0051] A memory storing executable program code;

[0052] a processor coupled to the memory;

[0053] The processor calls the executable program code stored in the memory to execute the steps of the UAV path planning method based on radar omnidirectional detection and improved A* algorithm.

[0054] A computer-readable storage medium for storing and executing the method, wherein the computer-readable storage medium stores computer instructions, and when the computer instructions are called, they are used to execute the steps of the drone path planning method based on radar omnidirectional detection and improved A* algorithm.

[0055] Beneficial effects: Compared with the prior art, the significant technical effects of the present invention are: first, through the radar omnidirectional detection technology, dynamic threat targets in all angles around the drone can be detected in real time, which can effectively solve the problem of difficult dynamic threat target detection. Based on this, an improved A* algorithm is used in global path planning, and through dimensionality reduction planning and improved heuristic functions, the drone can achieve the shortest path and threat avoidance. BRIEF DESCRIPTION OF THE DRAWINGS

[0056] Figure 1 It is the overall implementation flow chart of the present invention;

[0057] Figure 2 It is the flow chart of the improved A* algorithm of the present invention;

[0058] Figure 3 is a mountain environment elevation topographic map downloaded from SRTM in an embodiment of the present invention;

[0059] Figure 4 is a planning path diagram for the UAV to avoid dynamic threat targets in an embodiment of the present invention;

[0060] Figure 5 is a power allocation diagram of a radar detecting a dynamic threat target in an embodiment of the present invention;

[0061] Figure 6 It is the result of smoothing the original planned path in the embodiment of the present invention. DETAILED DESCRIPTION

[0062] The present invention is further described below in conjunction with the embodiments and drawings. The following embodiments are only used to more clearly illustrate the technical solution of the present invention, and cannot be used to limit the protection scope of the present invention.

[0063] In this example, for simplicity, the elevation map used is downsampled and the number of grids is processed to 200×200. Figure 1 As shown, the present invention proposes a UAV path planning method based on radar omnidirectional detection and improved A* algorithm, comprising:

[0064] S1. Import the elevation map downloaded by SRTM, downsample the elevation map, and calculate the elevation cost matrix of the entire elevation map, so that subsequent drones can choose to fly around terrain obstacles or over terrain obstacles. The specific content of the elevation cost matrix is ​​as follows;

[0065] Step 1: Assume that the elevation matrix of the elevation map is A n×n , where n represents the number of grids divided by the elevation map. First, let the basic matrix B n×n Equal to A n×n ;

[0066] Step 2, then expand B according to the following formula n×n , in B n×n The four sides of the extended 1 order, assigned a value of ∞, that is, the flight path beyond the elevation map is set to infinity, the UAV is restricted to fly within the elevation map, and the part beyond the elevation map is set as a no-fly zone; the expanded matrix is ​​expressed as:

[0067]

[0068] Among them, the element b in the matrix 11 ,…,b nn Indicates the altitude of each grid point on the elevation map.

[0069] Step 3: Define that the drone can fly in eight directions (front and back, left and right, and diagonal directions) at each two-dimensional plane grid point during the process of flying from the starting point to the end point on the elevation map. Define the direction matrix as D = [0 1; 1 0; 0 -1; -1 0; 1 1; 1 -1; -1 1; -1 -1], and calculate the elevation cost matrix of the drone flying in the kth direction as C k =B ov -A n×n , where B ov is the matrix B (n+2)×(n+2) and the matrix A n×n The overlap matrix, C k Determine the elevation difference cost of a UAV flying to any point in a specified direction on an elevation map.

[0070] S2. Use radar omnidirectional detection technology to detect dynamic threat targets in the surrounding environment, obtain the speed, direction and position information of dynamic threat targets, and allocate power to dynamic threat targets; the specific contents are as follows:

[0071] Step 1: Set the radar operating frequency f according to the mountain environment and flight mission. req , Scanning Angle Maximum detection distance d max and detection accuracy σ;

[0072] Step 2: The radar starts to perform 360-degree omnidirectional scanning, continuously transmitting and receiving radar signals, covering all directions around the drone;

[0073] Step 3: Analyze the received radar echo signal, apply the Doppler effect to measure the relative speed of the dynamic threat target, and determine whether the dynamic threat target is close to the drone and poses a threat;

[0074] Step 4: Evaluate the detected dynamic threat targets and allocate more detection power to the direction of the dynamic threat targets;

[0075] Step 5: Transmit the dynamic threat target information detected by the radar to the path planning module of the UAV to provide the dynamic threat target position and speed information.

[0076] S3. Based on the dynamic threat target information obtained in step S2, the drone uses the improved A* algorithm to plan the flight path. The traditional 3D-A* algorithm expands the neighbor nodes to 26 directions (up and down, front and back, left and right, and diagonal directions), and the cost function is calculated in the complete three-dimensional space. The improved A* algorithm limits the expansion direction to only the eight directions of front and back, left and right, and diagonal on the two-dimensional plane, and separates the elevation processing from the expansion direction, and determines the elevation cost separately. Through dimensionality reduction planning, the algorithm efficiency and calculation complexity are improved. The improved A* algorithm performs drone path planning process as follows. Figure 2 As shown, the specific contents are:

[0077] Step 1: Input the starting grid coordinate position p of the drone on the elevation map start , the end point grid coordinate position p end and the UAV flight speed v;

[0078] Step 2: Create an OPEN table named T open and CLOSE table is T close In each step of the search, calculate the current node p i The loss function f(p i )=g(p i )+h(p i ), where g(p i ) is the cost of the distance traveled, h(p i ) is the distance from the current node to the end point, i is the number of nodes. The height of the current node h z Greater than the height h of the previous node z-1 When △h z =h z -h z-1 >0, the cost of the distance traveled is Otherwise g(p i )=△h x +△h y +△h z , where △h x , △h y Respectively represent the distance difference when the drone flies in the x direction and the y direction;

[0079] Step 3: When T open If not empty, find T open The loss function f(p i ) minimum point, set it as the current node, and start from T open Remove if the current node is equal to p end , returned by T close The generated path, otherwise the current node is added to T close ;

[0080] Step 4: Calculate the neighboring points in the eight directions of the current node, front, back, left, right, and diagonal, and compare their loss functions f(p i ) and the elevation cost matrix C k If the neighboring point has neither obstacles nor is in the no-fly zone, that is, the neighboring point is valid, then determine whether the neighboring point is in T open If the neighboring point is already in T open Check if there is a smaller g(p i ) value, if any, update T open Otherwise, add the neighboring points to T open middle;

[0081] Step 5: Loop through steps 3 and 4 until p is found. end or T open Until it is empty.

[0082] According to the above steps, the original path of the UAV on the elevation map is obtained, and the original path is saved as a series of three-dimensional discrete path points.

[0083] S4. Use a cubic spline curve fitting algorithm to smooth the obtained series of three-dimensional discrete path points to make them more consistent with the actual flight trajectory. The specific content is as follows:

[0084] Step 1: Extract N discrete path points from a series of three-dimensional discrete path points saved in the original path, and represent the path points as three-dimensional coordinates Path = [(x1, y1, z1), (x2, y2, z2), …, (x N ,y N ,z N )], where (x j ,y j ,z j ) represents the position coordinates of the path point in the three-dimensional coordinate system, j = 0, 1,…, N.

[0085] Step 2: Define the subinterval between two adjacent path points as [m i ,m i+1 ],i=0,1,…,N-1,where m i Indicates the cumulative arc length from the starting point to the i-th path point, m i+1 Indicates the cumulative arc length from the starting point to the i+1th path point. In each subinterval [m i ,m i+1 ], respectively construct cubic spline interpolation functions for the x, y, and z directions. Let the cubic spline interpolation function in the x direction be S x,i (f) = a x,i +b x,i (fm i)+c x,i (fm i ) 2 +d x,i (fm i ) 3 , the cubic spline interpolation function in the y direction is S y,i (f) = a y,i +b y,i (fm i )+c y,i (fm i ) 2 +d y,i (fm i ) 3 , the cubic spline interpolation function in the z direction is S z,i (f) = a z,i +b z,i (fm i )+c z,i (fm i ) 2 +d z,i (fm i ) 3 , where f represents the arc length independent variable of the three-dimensional path point on the subinterval, a x,i 、b x,i 、c x,i d x,i Indicates S x,i (f) Coefficients of the cubic spline interpolation function. Similarly, a y,i 、b y,i 、c y,i d y,i and a z,i 、b z,i 、c z,i d z,i They are S y,i (f) and S z,i The coefficient of (f).

[0086] Step 3: Construct a linear equation system for the x, y, and z directions respectively, and solve the linear equation system to solve the coefficients of the cubic spline interpolation function;

[0087] Step 4: For each subinterval [m i ,m i+1 ], using the cubic spline function S in the x, y, and z directions respectively x,i (f),S y,i (f),S z,i (f) is used to generate the fitted path, and multiple points are taken in the sub-interval according to the accuracy requirements, the coordinates of these points are calculated, and the fitted smooth path can be obtained by connecting these three-dimensional points.

[0088] The system corresponding to the method includes:

[0089] The elevation cost matrix calculation unit is used to downsample the elevation terrain map and calculate the elevation cost matrix of the entire elevation terrain map, so that subsequent UAVs can choose to fly around or over terrain obstacles;

[0090] The target detection and power distribution unit is used to detect dynamic threat targets in the surrounding environment using radar omnidirectional detection technology, obtain the speed, direction and position information of the threat targets, and distribute power to the dynamic threats;

[0091] The path planning unit is used to plan the flight path of the UAV using the improved A* algorithm according to the acquired dynamic threat target information, obtain the original path of the UAV on the elevation map, and save the original path as a series of three-dimensional discrete path points;

[0092] The path smoothing processing unit is used to use a cubic spline curve fitting algorithm to smooth a series of three-dimensional discrete path points obtained to make them more consistent with the actual flight trajectory.

[0093] An electronic device for storing and executing the method, the device comprising:

[0094] A memory storing executable program code;

[0095] a processor coupled to the memory;

[0096] The processor calls the executable program code stored in the memory to execute the steps of the UAV path planning method based on radar omnidirectional detection and improved A* algorithm.

[0097] A computer-readable storage medium for storing and executing the method, wherein the computer-readable storage medium stores computer instructions, and when the computer instructions are called, they are used to execute the steps of the drone path planning method based on radar omnidirectional detection and improved A* algorithm.

[0098] In this example, the topographic map is the elevation data numbered 59_05 downloaded from SRTM. The topographic environment is as follows Figure 3 As shown in Figure 2. Assume that the starting coordinates of the drone are [100,50] and the ending coordinates are [10,77]. The speeds of the dynamic threats may be different but they all keep a constant speed. The drone path planned by the improved A* algorithm is shown in Figure 2. Figure 4 As shown in the figure, the black arrow points to the direction of the dynamic threat flight. Figure 4 It can be obtained that the improved A* algorithm can quickly find the shortest path from the starting point to the end point, and can avoid dynamic obstacles to achieve safe flight. Figure 5is the power allocation result of the airborne radar to the dynamic threat target during the entire flight time, which is given by Figure 5 It can be obtained that: threat target 1 flies away from the drone, and a certain amount of power is initially allocated. As time goes by, the allocated power gradually decreases; threat target 2 flies towards the drone, and as time goes by, the allocated power gradually increases; the location where threat target 3 appears is far away from the drone's track and flies away from the drone, so the allocated power also gradually decreases. The result after smoothing the original path using the cubic spline interpolation method is as follows Figure 6 As shown by Figure 6 It can be obtained that the original path is not smooth enough and has many corners, which is not enough to fit the actual flight trajectory of the drone. The cubic spline interpolation curve is used to optimize the original path. It can be found that the discontinuous turns are smoothed to obtain a smoother curve.

[0099] In summary, the use of radar omnidirectional detection technology can detect dynamic threat targets in all angles around the drone and allocate power to dynamic threats, which can effectively solve the problem of dynamic threat detection. The improved A* algorithm is used in global path planning. Through dimensionality reduction planning and improved heuristic functions, the drone can achieve the shortest path and threat avoidance.

[0100] Although the embodiments of the present invention are shown and described above, it is understood that the above embodiments are exemplary and cannot be understood as limiting the present invention. A person of ordinary skill in the art can change, modify, replace and modify the above embodiments within the scope of the present invention. Any other corresponding changes and modifications made according to the technical concept of the present invention should be included in the protection scope of the claims of the present invention.

Claims

1. A UAV path planning method based on radar omnidirectional detection and improved A* algorithm, characterized in that: include: S1, downsampling the elevation topographic map, and calculating the elevation cost matrix of the entire elevation topographic map; The elevation cost matrix is ​​used to determine the elevation difference cost of a UAV flying to any point in a specified direction on an elevation terrain map; S2. Use radar omnidirectional detection technology to detect dynamic threat targets in the surrounding environment, obtain the speed, direction and position information of the threat targets, and allocate power to the dynamic threat targets; S3, according to the dynamic threat target information obtained in step S2, the UAV uses an improved A* algorithm to plan the flight path, obtains the original path of the UAV on the elevation map, and saves the original path as a series of three-dimensional discrete path points; S4. Use a cubic spline curve fitting algorithm to smooth the obtained series of three-dimensional discrete path points to make them more consistent with the actual flight trajectory.

2. The UAV path planning method based on radar omnidirectional detection and improved A* algorithm according to claim 1 is characterized in that: In step S1, calculating the elevation cost matrix of the entire elevation topographic map includes the following steps: Step 1: Assume that the elevation matrix of the elevation map is A n×n , where n represents the number of grids divided by the elevation map. First, let the basic matrix B n×n Equal to A n×n ; Step 2, then expand B according to the following formula n×n , in B n×n The four sides of the extended 1 order, assigned a value of ∞, that is, the flight path beyond the elevation map is set to infinity, the UAV is restricted to fly within the elevation map, and the part beyond the elevation map is set as a no-fly zone; the expanded matrix is ​​expressed as: Among them, the element b in the matrix 11 ,…,b nn Indicates the altitude of each grid point on the elevation map; Step 3: Define that the drone can fly in eight directions, forward, backward, left, right, and diagonal, at each two-dimensional plane grid point during the process of flying from the starting point to the end point on the elevation map. Calculate the elevation cost matrix of the drone flying in the kth direction as C k =B ov -A n×n , where B ov is the matrix B (n+2)×(n+2) and the matrix A n×n The overlap matrix, C k Determine the elevation difference cost of a UAV flying to any point in a specified direction on an elevation map.

3. The UAV path planning method based on radar omnidirectional detection and improved A* algorithm according to claim 1 is characterized in that: In step S2, the dynamic threat target in the surrounding environment is detected using radar omnidirectional detection technology, including the following steps: Step 1: Set the radar operating frequency f according to the mountain environment and flight mission. req , Scanning Angle Maximum detection distance d max and detection accuracy σ; Step 2: The radar starts to perform 360-degree omnidirectional scanning, continuously transmitting and receiving radar signals, covering all directions around the drone; Step 3: Analyze the received radar echo signal, apply the Doppler effect to measure the relative speed of the dynamic threat target, and determine whether the dynamic threat target is close to the drone and poses a threat; Step 4: Evaluate the detected dynamic threat targets and allocate more detection power to the direction of the dynamic threat targets; Step 5: Transmit the dynamic threat target information detected by the radar to the path planning module of the UAV to provide the dynamic threat target position and speed information.

4. The UAV path planning method based on radar omnidirectional detection and improved A* algorithm according to claim 1 is characterized in that: In step S3, the UAV uses the improved A* algorithm to plan the flight path, including the following steps: Step 1: Input the starting grid coordinate position p of the drone on the elevation map start , the end point grid coordinate position p end and the UAV flight speed v; Step 2: Create an OPEN table named T open and CLOSE table is T close ; In each step of the search, calculate the current node p i The loss function f(p i )=g(p i )+h(p i ), where g(p i ) is the cost of the distance traveled, h(p i ) is the distance from the current node to the end point, i is the number of nodes; the height of the current node h z Greater than the height h of the previous node z-1 When △h z =h z -h z-1 >0, the cost of the distance traveled is Otherwise g(p i )=△h x +△h y +△h z , where △h x , △h y Respectively represent the distance difference when the drone flies in the x direction and the y direction; Step 3: When T open If not empty, find T open The loss function f(p i ) minimum point, set it as the current node, and start from T open Remove if the current node is equal to p end , returned by T close The generated path, otherwise the current node is added to T close ; Step 4: Calculate the neighboring points in the eight directions of the current node, front, back, left, right, and diagonal, and compare their loss functions f(p i ) and the elevation cost matrix C k If the neighboring point has neither obstacles nor is in the no-fly zone, that is, the neighboring point is valid, then determine whether the neighboring point is in T open If the neighboring point is already in T open Check if there is a smaller g(p i ) value, if any, update T open Otherwise, add the neighboring points to T open middle; Step 5: Loop through steps 3 and 4 until p is found. end or T open Until it is empty. According to the above steps, the original path of the UAV on the elevation map is obtained, and the original path is saved as a series of three-dimensional discrete path points.

5. The UAV path planning method based on radar omnidirectional detection and improved A* algorithm according to claim 1 is characterized in that: In step S4, a cubic spline curve fitting algorithm is used to smooth the obtained series of three-dimensional discrete path points, including the following steps: Step 1: Extract N discrete path points from a series of three-dimensional discrete path points saved in the original path, and represent the path points as three-dimensional coordinates Path = [(x1, y1, z1), (x2, y2, z2), …, (x N ,y N ,z N )], where (x j ,y j ,z j ) represents the position coordinates of the path point in the three-dimensional coordinate system, j = 0, 1, ..., N; Step 2: Define the subinterval between two adjacent path points as [m i ,m i+1 ],i=0,1,…,N-1,where m i Indicates the cumulative arc length from the starting point to the i-th path point, m i+1 Indicates the cumulative arc length from the starting point to the i+1th path point. In each subinterval [m i ,m i+1 ], respectively construct cubic spline interpolation functions for the x, y, and z directions; let the cubic spline interpolation function in the x direction be S x,i (f) = a x,i +b x,i (fm i )+c x,i (fm i ) 2 +d x,i (fm i ) 3 , the cubic spline interpolation function in the y direction is S y,i (f) = a y,i +b y,i (fm i )+c y,i (fm i ) 2 +d y,i (fm i ) 3 , the cubic spline interpolation function in the z direction is S z,i (f) = a z,i +b z,i (fm i )+c z,i (fm i ) 2 +d z,i (fm i ) 3 , where f represents the arc length independent variable of the three-dimensional path point on the subinterval, a x,i , b x,i 、c x,i ,d x,i Indicates S x,i (f) Coefficients of the cubic spline interpolation function. Similarly, a y,i , b y,i 、c y,i ,d y,i and a z,i , b z,i 、c z,i ,d z,i They are S y,i (f) and S z,i (f) coefficient; Step 3: Construct a linear equation system for the x, y, and z directions respectively, and solve the linear equation system to solve the coefficients of the spline function; Step 4: For each subinterval [m i ,m i+1 ], using the cubic spline function S in the x, y, and z directions respectively x,i (f),S y,i (f),S z,i (f) is used to generate the fitted path, and multiple points are taken in the sub-interval according to the accuracy requirements, the coordinates of these points are calculated, and the fitted smooth path can be obtained by connecting these three-dimensional points.

6. A UAV path planning system based on radar omnidirectional detection and improved A* algorithm, characterized in that: include: The elevation cost matrix calculation unit is used to downsample the elevation terrain map and calculate the elevation cost matrix of the entire elevation terrain map, so that subsequent UAVs can choose to fly around or over terrain obstacles; The target detection and power distribution unit is used to detect dynamic threat targets in the surrounding environment using radar omnidirectional detection technology, obtain the speed, direction and position information of the threat targets, and distribute power to the dynamic threats; The path planning unit is used to plan the flight path of the UAV using the improved A* algorithm according to the acquired dynamic threat target information, obtain the original path of the UAV on the elevation map, and save the original path as a series of three-dimensional discrete path points; The path smoothing processing unit is used to use a cubic spline curve fitting algorithm to smooth a series of three-dimensional discrete path points obtained to make them more consistent with the actual flight trajectory.

7. The UAV path planning system based on radar omnidirectional detection and improved A* algorithm according to claim 6 is characterized in that: The drone uses the improved A* algorithm for flight path planning, which includes the following steps: Step 1: Input the starting grid coordinate position p of the drone on the elevation map start , the end point grid coordinate position p end and the UAV flight speed v; Step 2: Create an OPEN table named T open and CLOSE table is T close ; In each step of the search, calculate the current node p i The loss function f(p i )=g(p i )+h(p i ), where g(p i ) is the cost of the distance traveled, h(p i ) is the distance from the current node to the end point, i is the number of nodes; the height of the current node h z Greater than the height h of the previous node z-1 When △h z =h z -h z-1 >0, the cost of the distance traveled is Otherwise g(p i )=△h x +△h y +△h z , where △h x , △h y Respectively represent the distance difference when the drone flies in the x direction and the y direction; Step 3: When T open If not empty, find T open The loss function f(p i ) minimum point, set it as the current node, and start from T open Remove if the current node is equal to p end , returned by T close The generated path, otherwise the current node is added to T close ; Step 4: Calculate the neighboring points in the eight directions of the current node, front, back, left, right, and diagonal, and compare their loss functions f(p i ) and the elevation cost matrix C k If the neighboring point has neither obstacles nor is in the no-fly zone, that is, the neighboring point is valid, then determine whether the neighboring point is in T open If the neighboring point is already in T open Check if there is a smaller g(p i ) value, if any, update T open Otherwise, add the neighboring points to T open middle; Step 5: Loop through steps 3 and 4 until p is found. end or T open Until it is empty. According to the above steps, the original path of the UAV on the elevation map is obtained, and the original path is saved as a series of three-dimensional discrete path points.

8. The UAV path planning system based on radar omnidirectional detection and improved A* algorithm according to claim 6 is characterized in that: The cubic spline curve fitting algorithm is used to smooth the obtained series of three-dimensional discrete path points, including the following steps: Step 1: Extract N discrete path points from a series of three-dimensional discrete path points saved in the original path, and represent the path points as three-dimensional coordinates Path = [(x1, y1, z1), (x2, y2, z2), …, (x N ,y N ,z N )], where (x j ,y j ,z j ) represents the position coordinates of the path point in the three-dimensional coordinate system, j = 0, 1, ..., N; Step 2: Define the subinterval between two adjacent path points as [m i ,m i+1 ],i=0,1,…,N-1,where m i Indicates the cumulative arc length from the starting point to the i-th path point, m i+1 Indicates the cumulative arc length from the starting point to the i+1th path point. In each subinterval [m i ,m i+1 ], respectively construct cubic spline interpolation functions for the x, y, and z directions; let the cubic spline interpolation function in the x direction be S x,i (f) = a x,i +b x,i (fm i )+c x,i (fm i ) 2 +d x,i (fm i ) 3 , the cubic spline interpolation function in the y direction is S y,i (f) = a y,i +b y,i (fm i )+c y,i (fm i ) 2 +d y,i (fm i ) 3 , the cubic spline interpolation function in the z direction is S z,i (f) = a z,i +b z,i (fm i )+c z,i (fm i ) 2 +d z,i (fm i ) 3 , where f represents the arc length independent variable of the three-dimensional path point on the subinterval, a x,i , b x,i 、c x,i ,d x,i Indicates S x,i (f) Coefficients of the cubic spline interpolation function. Similarly, a y,i , b y,i 、c y,i ,d y,i and a z,i , b z,i 、c z,i ,d z,i They are S y,i (f) and S z,i (f) coefficient; Step 3: Construct a linear equation system for the x, y, and z directions respectively, and solve the linear equation system to solve the coefficients of the spline function; Step 4: For each subinterval [m i ,m i+1 ], using the cubic spline function S in the x, y, and z directions respectively x,i (f),S y,i (f),S z,i (f) is used to generate the fitted path, and multiple points are taken in the sub-interval according to the accuracy requirements, the coordinates of these points are calculated, and the fitted smooth path can be obtained by connecting these three-dimensional points.

9. An electronic device, characterized in that: The device comprises: A memory storing executable program code; a processor coupled to the memory; The processor calls the executable program code stored in the memory to execute the steps of the drone path planning method based on radar omnidirectional detection and improved A* algorithm as described in any one of claims 1-5.

10. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores computer instructions, which, when called, are used to execute the steps of the drone path planning method based on radar omnidirectional detection and improved A* algorithm as described in any one of claims 1 to 5.

Images