Cruise low-altitude penetration task route planning method based on improved A* algorithm

Abstract

The invention relates to a cruise low-altitude penetration task route planning method based on an improved A * algorithm, and belongs to the field of route planning of unmanned aerial vehicles. The method comprises the steps of firstly planning and generating a track result for avoiding enemy ground radar threats in a battlefield environment through an improved A * algorithm; secondly, on the basis of the two-dimensional flight path planning result, height planning meeting the performance constraint of the fixed-wing unmanned aerial vehicle is achieved through a particle sedimentation method; and finally obtaining a three-dimensional air route of the fixed-wing unmanned aerial vehicle for completing the established combat task. According to the method, off-line and on-line planning of the reference flight path in a combat mission scene of the fixed-wing unmanned aerial vehicle is realized, the planning time before launching is shortened, the airborne flight path calculation efficiency is improved, the reaction time is shortened, and thus the adaptive capacity of the fixed-wing unmanned aerial vehicle to a complex battlefield environment is improved.

Description

Technical Field

[0001] The present invention belongs to the field of trajectory planning of unmanned aerial vehicles (UAVs), and specifically relates to a trajectory planning method for a cruise low-altitude penetration mission based on an improved A* algorithm, which can be widely used in the low-altitude obstacle avoidance penetration trajectory planning of various types of fixed-wing and other unmanned aerial vehicles. Background Art

[0002] With the continuous improvement of UAV design and control technology, UAVs can now undertake more complex tasks in a variety of application scenarios; in the military field, UAVs are often used for medium- and short-range reconnaissance missions, patrol and control missions, and strike missions against high-value and easily destructible targets; in the civilian field, UAVs are often used for tracking and photographing targets, fire fighting on high floors, long-distance post-disaster communication relay, and long-term tracking and prediction of natural disasters such as floods, landslides, avalanches, and earthquakes.

[0003] Among them, the most far-reaching impact on the completion of the given mission by the drone is the drone's trajectory planning method. Equipping the drone's onboard computer with an applicable trajectory planning method can free the drone from the restriction of pilot control, and is a key means to enable the drone to have autonomous planning and intelligent control. During the execution of the mission, the drone may encounter changes in the mission target position, sudden detection of threats or obstacles at the waypoint, and change of the drone's current mission. At this time, the drone needs to calculate the flyable trajectory that guides itself to the new target destination based on the current attitude state and position state. Therefore, the planning algorithm carried on the aircraft needs to meet the following requirements: when solving the trajectory, it can combine the drone's own flight performance; when solving the trajectory, it can combine the currently detected threats, obstacles, no-fly zones, etc. so that the generated trajectory does not pass through the above areas; when solving the trajectory, it can generate a three-dimensional trajectory based on the ground undulations; the computing power requirements and computing resources occupied by the solution during the solution meet the hardware capability constraints of the onboard computer.

[0004] The current mainstream search algorithms are mainly divided into the following categories: heuristic-based search algorithms, which select the direction of travel by continuously calculating the cost function of the path, and ultimately optimize the arrival at the terminal node; probability graph-based sampling algorithms, which perform probability sampling on the global map and continuously connect various nodes. When a point is finally generated near the end point, the entire track is connected by backtracking; intelligent optimization algorithms based on global optimization mainly optimize the suboptimal track that is being sought through continuous iteration, collision, evolution, etc. in the entire space, and ultimately find the optimal track in a local or global range.

[0005] The A* algorithm is a typical heuristic search method, which originated from the traditional Dijkstra algorithm. It uses the grid map as the minimum search unit and searches for the optimal solution of the current path at each step, thereby finding a trajectory path with a lower overall path cost. The A* algorithm has a fast search speed, occupies less computing resources, and can be well combined with the constraints of UAV flight performance after appropriate improvements. Therefore, it is widely used in the online trajectory solution of UAVs.

[0006] However, the current A* algorithm still has the following shortcomings: 1. Fixed-wing UAVs have a long range, a high speed, and a large range of motion, so their planning space is large. For a large planning space, it is difficult for the A* algorithm to determine the grid unit when searching in a discrete grid map. If the grid is small, the search track speed is slow and the track is distorted. If the grid is large, it may be impossible to complete the search and find a reasonable track. 2. Fixed-wing UAVs need to be as close to the ground as possible when performing certain tasks, so they need to constantly change their height and avoid obstacles according to the undulations of the ground. However, the current A* algorithm will seriously affect its search efficiency when expanded to three dimensions, causing the search speed to drop sharply and occupying a large amount of computing resources. Summary of the invention

[0007] The technical problems to be solved by the present invention are:

[0008] Aiming at the problems that three-dimensional trajectory planning of fixed-wing UAVs in a wide range faces pre-launch planning difficulties, long preparation time, and complex combat missions, the present invention provides a trajectory planning method for cruise low-altitude penetration missions based on an improved A* algorithm.

[0009] In order to solve the above technical problems, the technical solution adopted by the present invention is:

[0010] A method for cruising low-altitude penetration mission trajectory planning based on an improved A* algorithm, characterized by comprising:

[0011] Get the starting point of the route, and use the improved A* trajectory planning algorithm based on the starting point to search in the planned two-dimensional plane to generate two-dimensional trajectory points;

[0012] Determine the initial track height of the two-dimensional track points, and based on the maximum climb / dive angle constraints and the minimum flight altitude constraints of the aircraft, gradually sink the two-dimensional track points from the initial track height that meets the constraints, and stop sinking when they gradually sink to the minimum relative flight altitude while meeting the constraints; when all track points stop sinking, the track height planning is completed and a feasible three-dimensional track is obtained.

[0013] A further technical solution of the present invention is to search and generate two-dimensional track points in a planned two-dimensional plane based on a starting point using an improved A* track planning algorithm, including:

[0014] The minimum expansion step is used to divide the two-dimensional space of planning, and the weight value of the cost function of the improved A* trajectory planning algorithm is determined;

[0015] Initialize the node, create an empty open table and closed table, add the starting point to the open table, and each node includes a two-dimensional coordinate value, a parent node, and a cost function;

[0016] If the open table is empty, it means that the search has failed, and the initial parameters of the algorithm should be readjusted to search the node again;

[0017] Find the node with the smallest cost function value in the open table, regard it as the current node and add it to the closed table, and expand from the current node;

[0018] Determine whether the distance between the current node and the target point meets the convergence condition. If so, the current node is regarded as the parent node of the target point. After the search is completed, all parent nodes are traced back to obtain the track of track planning. If the convergence condition is not met, the node is expanded from the current node, and the open table is readjusted to the minimum heap until the convergence condition is met.

[0019] A further technical solution of the present invention: The cost function of the improved A* trajectory planning algorithm is:

[0020] f=ω1·L x +ω2·L h +ω3·L d +ω4·W+ω5·T+ω6·U

[0021] Among them, L x To determine the actual distance between the current node and the starting point using the Manhattan method, L h To represent the actual distance from the current node to the target point, L d is the flight cost affected by the elevation of the ground projection point corresponding to the current track node, W is the flight cost corresponding to the threat zone and the no-fly zone, T is the cost of the number of turns from the starting point to the current track point, and U is the terminal angle constraint cost value; ω1 is the weight coefficient corresponding to the Manhattan distance, ω2 is the weight coefficient corresponding to the actual distance from the current node to the target point, ω3 is the weight coefficient corresponding to the influence of the terrain elevation on the two-dimensional track planning, ω4 is the weight coefficient corresponding to the influence of the threat zone and the no-fly zone, ω5 is the weight coefficient corresponding to the cost of the number of turns, and ω6 is the cost coefficient corresponding to the terminal angle constraint.

[0022] A further technical solution of the present invention is as follows: after generating the two-dimensional track points, it also includes interpolating multiple track points with equal steps between two adjacent track points.

[0023] A further technical solution of the present invention is to determine the initial track height of the two-dimensional track point, including:

[0024] Calculate the terrain height corresponding to all track points through DEM data and obtain the maximum elevation in the height profile;

[0025] Determine the initial track altitude of the track point based on the maximum altitude, minimum relative ground altitude, and the altitude of the starting point:

[0026] h=max{h0,(H max +Δh min +1000m)}

[0027] Among them, h0 is the height of the starting point, H max is the maximum elevation, Δh min is the minimum relative ground height.

[0028] A further technical solution of the present invention is: based on the maximum dive angle constraint and the minimum flight altitude constraint of the aircraft, the two-dimensional track point is gradually sunk from the initial track altitude that satisfies the constraints, and the sinking is stopped when the two-dimensional track point gradually sinks to the minimum relative flight altitude when the constraints are satisfied; comprising:

[0029] Determine the settlement margin of the track point based on the distance between two adjacent track points, the dive angle constraint, the climb angle constraint, and the maximum settlement step length;

[0030] The initial track height of the track point is settled based on the settlement margin, and the height after settlement is compared with the minimum relative ground height: if the height after settlement is higher than the minimum relative ground height, the track point can be settled, otherwise it cannot be settled;

[0031] Check whether the current track point meets the constraints after settling. If the current track point can settle and the previous track point cannot settle, it is necessary to check whether the track point after settlement and the previous track point meet the aircraft dive constraint; if the current track point can settle and the next track point cannot settle, it is necessary to check whether the track point after settlement and the next track point meet the aircraft climb constraint; if the constraints are met, reduce the current altitude by the settlement margin.

[0032] A further technical solution of the present invention is to determine the settlement margin of a track point based on the distance between two adjacent track points, the dive angle constraint, the climb angle constraint, and the maximum settlement step length, specifically:

[0033] ΔH=min{ΔH max ,δ×L i ×min(tan(θ min ),tan(θ max ))}

[0034] Among them, δ is the safety factor, L i is the distance between two adjacent track points, θ minis the dive angle constraint, θ max is the climb angle constraint, ΔH max is the maximum settlement step length.

[0035] A computer system, characterized in that it comprises: one or more processors, and a computer-readable storage medium for storing one or more programs, wherein when the one or more programs are executed by the one or more processors, the one or more processors implement the above-mentioned method.

[0036] A computer-readable storage medium, characterized in that it stores computer-executable instructions, which are used to implement the above method when executed.

[0037] A computer program product, characterized by comprising computer executable instructions, wherein the instructions are used to implement the above method when executed.

[0038] A computer system, characterized in that it comprises: one or more processors, and a computer-readable storage medium for storing one or more programs, wherein when the one or more programs are executed by the one or more processors, the one or more processors implement the above-mentioned method.

[0039] A computer-readable storage medium, characterized in that it stores computer-executable instructions, which are used to implement the above method when executed.

[0040] A computer program product, characterized by comprising computer executable instructions, wherein the instructions are used to implement the above method when executed.

[0041] The beneficial effects of the present invention are:

[0042] The present invention provides a method for cruising low-altitude penetration mission trajectory planning based on an improved A* algorithm. The method adopts an improved three-dimensional A* algorithm combined with altitude settlement planning, and combines typical combat scenarios, such as reconnaissance and attack missions, to study and formulate offline and online trajectory planning schemes for fixed-wing UAVs, so as to achieve offline and online planning of reference trajectory in combat mission scenarios of fixed-wing UAVs, shorten the planning time before launch, improve the efficiency of on-board trajectory calculation, and reduce reaction time, thereby improving the adaptability of fixed-wing UAVs to complex battlefield environments. BRIEF DESCRIPTION OF THE DRAWINGS

[0043] The drawings are only for the purpose of illustrating particular embodiments and are not to be considered limiting of the present invention. Like reference symbols denote like components throughout the drawings.

[0044] Figure 1 A schematic diagram illustrating a method for generating estimated terminal points according to the present invention.

[0045] Figure 2 It is a schematic diagram of the angle constraint cost function of the present invention.

[0046] Figure 3 It is a schematic diagram of the flow of the two-dimensional A* track generation method of the present invention.

[0047] Figure 4 This is a schematic diagram of the coordinate conversion performed in the present invention.

[0048] Figure 5 A schematic diagram of the present invention taking into account the influence of elevation conversion.

[0049] Figure 6 This is a schematic diagram of selecting coordinate interpolation approximate points for the present invention.

[0050] Figure 7 Schematic diagram of the method for equal-step interpolation of track points according to the present invention.

[0051] Figure 8 This is the flight path simulated in MATLAB by the present invention.

[0052] Fig. 9 This is a schematic diagram of the present invention in simulating the change of track elevation.

[0053] Fig.10 This is a schematic diagram of search results using real terrain data of a certain area in the present invention. DETAILED DESCRIPTION

[0054] In order to make the purpose, technical solutions and advantages of the present invention more clearly understood, the present invention is further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not intended to limit the present invention. In addition, the technical features involved in the various embodiments of the present invention described below can be combined with each other as long as they do not conflict with each other.

[0055] The A* algorithm is based on the Dijkstra algorithm, which introduces heuristic information in the search process, so that the search nodes tend to the optimal path rather than the global overall optimization, thereby greatly reducing the number of extended nodes and simplifying the algorithm. The improved A* algorithm combines constraints into the search expansion process on the basis of the original standard A* algorithm, and effectively uses constraints to screen the track expansion points in the planning space. The present invention first generates track results that avoid the threat of enemy ground radar in a battlefield environment through the improved A* algorithm planning; secondly, based on the two-dimensional track planning results, the particle sedimentation method is used to achieve a high degree of planning that meets the performance constraints of the fixed-wing UAV itself; and finally, the three-dimensional route for the fixed-wing UAV to complete the established combat mission is obtained.

[0056] (1) Generate a two-dimensional plane track:

[0057] First, search for expansion nodes in the two-dimensional plane, and simplify the expansion node search to generate expansion points with a minimum angle range of a certain angle value on a circle with the current point as the center and the search step as the radius. Combined with the maximum heading angle constraint of track planning, the safety factor δ is set to 0.8 to ensure that the performance of the fixed-wing UAV is met during the track point search, and the direction angle of the search target is determined as:

[0058] Δβ=δ·β max (1)

[0059] Therefore, according to the algorithm step size and the pitch angle and turning angle constraints, the planning space can be discretized into track extension points for search, and the search is carried out based on the four elements of horizontal coordinate information, turning angle and evaluation in the north-east coordinate system. cur (x cur ,y cur ,β cur ,F cur ), corresponding to the parent node P par (x par ,y par ,β par ,F par ), then the expanded node is recorded as P next = {P next,i |i=1,2,…}.

[0060] On the basis of searching for expansion nodes, the cost function and heuristic information of the expansion node search are further determined. In the standard A* algorithm, by introducing the cost function as heuristic information, the search node tends to the optimal path. The corresponding cost function is shown in the following formula:

[0061] f=L x +L h (2)

[0062] Among them, L x To determine the actual distance between the current node and the starting point using the Manhattan method, L h It represents the actual distance from the current node to the target point.

[0063] However, the two-dimensional A* trajectory planning method needs to further consider the impact of mountain elevation on the climb and dive costs of fixed-wing UAVs in three-dimensional trajectory planning, based on the constraints of the fixed-wing UAV's own performance and motion characteristics. To this end, the improved A* trajectory planning algorithm for the two-dimensional plane adopts the following cost function:

[0064] f=ω1·L x +ω2·L h +ω3·L d +ω4·W+ω5·T+ω6·U (3)

[0065] Among them, L d is the flight cost affected by the elevation of the ground projection point corresponding to the current track node, W is the flight cost corresponding to the threat zone and the no-fly zone, T is the cost of the number of turns from the starting point to the current track point, and U is the terminal angle constraint cost value; ω1 is the weight coefficient corresponding to the Manhattan distance, ω2 is the weight coefficient corresponding to the actual distance from the current node to the target point, ω3 is the weight coefficient corresponding to the influence of the terrain elevation on the two-dimensional track planning, ω4 is the weight coefficient corresponding to the influence of the threat zone and the no-fly zone, ω5 is the weight coefficient corresponding to the cost of the number of turns, and ω6 is the cost coefficient corresponding to the terminal angle constraint.

[0066] Among them, the Manhattan distance calculation method backtracks the number of parent nodes n of the current node and takes the search step length h as the unit distance, then:

[0067] L h =n·h (4)

[0068] The actual distance from the current track point to the end point is expressed as the straight-line distance between the two points in the North Celestial East coordinate system:

[0069]

[0070] Assume that the threat cost of the extension point in the threat area is proportional to the distance the extension point crosses the boundary of the threat area. The greater the distance across the boundary, the greater the threat cost. At the same time, in order to ensure that the aircraft track is as far away from the threat area as possible, a vector field is introduced to push the generated track points away from the center of the threat area, that is:

[0071]

[0072] Among them, K thre is the weight coefficient of the impact of a given threat zone, d is the distance between the current expansion node and the projection point of the threat center on the horizontal plane, L1 represents the distance from the current point to the center of the threat zone, and ε1 represents the range coefficient of the threat vector.

[0073]

[0074] Similarly, since the expansion point cannot be within the no-fly zone R f If the search is performed inside the no-fly zone, the constraint cost of the no-fly zone is considered to be infinite. Similarly, in order to ensure that the aircraft track is as far away from the no-fly zone as possible, a vector field is introduced to push the generated track points away from the center of the no-fly zone:

[0075]

[0076] Among them, L2 represents the distance from the current point to the center of the no-fly zone, and ε2 represents the range coefficient of the threat vector. Therefore, the impact of the threat zone and the no-fly zone on the fixed-wing UAV in the battlefield environment can be expressed as:

[0077]

[0078] When the track point is generated close to the end point, it is usually expected that the terminal will drive out in a certain direction. The present invention proposes the following solution strategy to solve the terminal constraint problem when the track point is generated to a certain extent.

[0079] First, generate an estimated track terminal point instead of the actual terminal point. When searching for the A* route, search from the starting point to this point. This point is generated at the position of one track point step in the desired reverse terminal angle. The position and direction of this point are as follows: Figure 1 shown.

[0080] At the same time, the terminal angle constraint cost is calculated as follows:

[0081]

[0082] Among them, L u1 Indicates the distance from the current point to the extension line of the estimated terminal point, L u2 Indicates the distance from the starting point to the estimated terminal point, such as Figure 2 shown.

[0083] In summary, the two-dimensional A* trajectory planning algorithm process is as follows Figure 3 As shown:

[0084] When the A* algorithm expands nodes in a grid map, the influence of elevation and computational complexity is considered, and an approximate node selection method used in the node expansion process is given.

[0085] Since the aircraft dynamics model is applicable to the North-Sky-East coordinate system, but not to the geographic coordinate system, when searching the map and designing the track, the North-Sky-East coordinate system must be used. If the route calculation is performed in the geographic coordinate system, the generated route may exceed the aircraft's own capability constraints. Figure 4 shown.

[0086] The 2D A* track search algorithm will take terrain undulations into account when calculating the route, so as to determine whether the aircraft is flying around or climbing a mountain. At this time, the actual height of a point must be considered. When a point is mapped to a 3D map, its height coordinates will interfere with the coordinates in the North Celestial East coordinate system. Figure 5 shown.

[0087] For a two-dimensional track point with a height of 0 in the North Celestial East coordinate system, when its altitude changes after being mapped to the geographic coordinate system, the corresponding X, Y values ​​of the North Celestial East coordinate system will drift. Therefore, when the initial relative position S0 is calculated, it is necessary to look up the elevation data in the table, re-position it, and use the height of its precise coordinate P1 for A* track planning. When looking for the height data of a point, take the map point closest to the point as the height of the point, such as Figure 6 shown.

[0088] Point P0 is closest to P2, so the height of P2 is used as the height of point P0. The height of point P2 is loaded into P0 for calculation and comparison.

[0089] (2) Generating a three-dimensional track by subtracting the height of the two-dimensional track

[0090] Based on the two-dimensional trajectory planning results, in order to improve the efficiency of three-dimensional trajectory planning, the improved settlement method is used to plan the height of the track points. The basic idea is: considering the maximum climb / dive angle constraints and the minimum flight altitude constraints of the aircraft, the two-dimensional track points are gradually settled from the initial height that meets the constraints. When the constraints are met, the settlement is stopped when the climb / dive limit is gradually settled to obtain a feasible height of the aircraft track points. When all track points stop settling, the track altitude planning is completed and a feasible three-dimensional track is obtained.

[0091] In order to realize the segmented flight path of multiple fixed-wing UAV missions, a certain relative height h above the ground is maintained. srh Continuous patrolling, based on the constraints of the aircraft's dive and climb performance, quickly reaches the desired flight altitude and iso-altitude patrolling, mainly includes: interpolation of alternative track points, given strategy settlement, altitude compensation, track point optimization and other four steps, which are described as follows:

[0092] 1) Interpolation of alternative track points

[0093] In order to ensure the feasibility and safety of the route generated by the settlement method height planning, Figure 7 As shown in the equal-step interpolation track points, the method of linear interpolation of terrain data with equal step length ΔL is used to generate U i 2D track P k →P k+1 m equal-step-length track candidate points {P k,0 ,P k,1 ,P k,2 ,…,P k,m ,P k+1}, that is, satisfying:

[0094]

[0095] 2) Given strategy settling

[0096] After obtaining the two-dimensional track containing the candidate track points, the ground elevation of the two-dimensional track points can be obtained by interpolating the DEM data. The basic idea of ​​this strategy is: considering the maximum climb / dive angle constraints and the minimum flight altitude constraints of the aircraft, the two-dimensional track points are gradually sunk from the initial altitude that meets the constraints, and the sinking stops when they gradually sink to the minimum relative flight altitude (i.e., ultra-low altitude flight altitude) while meeting the constraints. When all track points stop sinking, the track altitude planning is completed and a feasible three-dimensional track is obtained.

[0097] In order to enable those skilled in the art to better understand the present invention, the present invention is described in detail below in conjunction with specific embodiments.

[0098] Embodiment 1:

[0099] Step 1: During the launch preparation phase, the UAV collects basic scene information for this mission through external cables, including three-dimensional elevation map data, target location status information, external threat location and range, etc.

[0100] Step 2: Algorithm initialization. When the command vehicle issues a task, it takes its own coordinates after catapult launch as the starting point and the target coordinates as the end point, and stores them in the algorithm entry. It also obtains threat information and aircraft constraints during the planning process.

[0101] Step 3: Select the appropriate minimum expansion step to divide the planning space and determine the weight value of the cost function;

[0102] Step 4: Initialize the nodes, create empty open and closed tables, add the initial nodes to the open table, and each node includes a two-dimensional coordinate value (x n ,y n ), parent node node, cost function and other information.

[0103] Step 5: If the open table is empty, it means that the search has failed. Re-adjust the initial parameters of the algorithm and go to step 3 to search the node again.

[0104] Step 6: Find the node m with the smallest cost function f(m) in the open table, treat the node m as the current node and add it to the closed table, and expand from the current node;

[0105] Step 7: Determine whether the distance between the current node and the target point meets the convergence condition. If so, the current node is regarded as the parent node of the target point, the A* algorithm search is completed, and the track of the track planning can be obtained by backtracking all parent nodes; if the convergence condition is not met, go to step 6 to continue expanding the node from the current node, and readjust the open table to the minimum heap until the convergence condition is met.

[0106] Step 8: Generate candidate track points by interpolation step length and calculate the terrain height corresponding to all track points through DEM data to obtain the maximum elevation H in the height profile max ;

[0107] Step 9: Assign initial track height to all discrete track points obtained after interpolation. The initial track height setting value is:

[0108] h=max{h0,(H max +Δh min +1000m)} (12)

[0109] Where Δh min is the minimum relative height to the ground, and then traverses the nodes in sequence starting from the second point;

[0110] Step 10: Calculate the settlement margin of the current node. The settlement margin cannot be too large. If it is too large, the settlement will be faster and the algorithm will run in a shorter time, but it may cause the dive constraint to not be satisfied and the track cannot settle. The settlement margin cannot be too small. If it is too small, the settlement accuracy will be high, but the algorithm will run in a long time. Therefore, the settlement margin should be calculated by considering the algorithm time complexity and settlement accuracy. The settlement margin ΔH is the distance L between the current track point and the next adjacent track point. i , dive angle constraint θ min , climb angle constraint θ max and the maximum sedimentation step length ΔH max Joint decision:

[0111] ΔH=min{ΔH max ,δ×L i ×min(tan(θ min ),tan(θ max ))} (13)

[0112] Among them, δ is the safety factor.

[0113] Step 11: Determine whether the current node is below the minimum relative height h after settlement min , if the height after settlement is higher than the minimum relative height h min Then the node can be settled, and go to step 13; otherwise, it cannot be settled;

[0114] Step 12: Check whether the current node satisfies the constraints after settling. If the current node can settle and the previous node cannot settle, then check whether the node after settling and the previous node satisfy the aircraft's dive constraint; if the current node can settle and the next node cannot settle, then check whether the node after settling and the next node satisfy the aircraft's climb constraint. If the constraints are satisfied, lower the current height by ΔH;

[0115] Step 13: Traverse to the next track node, and loop steps 10 to 12 to the current node to traverse to the end of the track point sequence.

[0116] Step 14: Feed the generated route data back to the flight controller of the drone. After the drone is ready, it will be ejected and fly according to the algorithm results.

[0117] The above description is only a specific implementation mode of the present invention, but the protection scope of the present invention is not limited thereto. Any technician familiar with the technical field can easily think of various equivalent modifications or substitutions within the technical scope disclosed in the present invention, and these modifications or substitutions should be included in the protection scope of the present invention.

Claims

1. A method for trajectory planning of a cruise low-altitude penetration mission based on an improved A* algorithm, characterized in that: include: Get the starting point of the route, and use the improved A* trajectory planning algorithm based on the starting point to search in the planned two-dimensional plane to generate two-dimensional trajectory points; Determine the initial track height of the two-dimensional track points, and based on the maximum climb / dive angle constraints and the minimum flight altitude constraints of the aircraft, gradually sink the two-dimensional track points from the initial track height that meets the constraints, and stop sinking when they gradually sink to the minimum relative flight altitude while meeting the constraints; when all track points stop sinking, the track height planning is completed and a feasible three-dimensional track is obtained.

2. According to claim 1, a method for cruising low-altitude penetration mission trajectory planning based on an improved A* algorithm is characterized in that: Based on the starting point, the improved A* trajectory planning algorithm is used to search in the planned two-dimensional plane to generate two-dimensional trajectory points, including: The minimum expansion step is used to divide the two-dimensional space of planning, and the weight value of the cost function of the improved A* trajectory planning algorithm is determined; Initialize the node, create an empty open table and closed table, add the starting point to the open table, and each node includes a two-dimensional coordinate value, a parent node, and a cost function; If the open table is empty, it means that the search has failed, and the initial parameters of the algorithm should be readjusted to search the node again; Find the node with the smallest cost function value in the open table, regard it as the current node and add it to the closed table, and expand from the current node; Determine whether the distance between the current node and the target point meets the convergence condition. If so, the current node is regarded as the parent node of the target point. After the search is completed, all parent nodes are traced back to obtain the track of track planning. If the convergence condition is not met, the node is expanded from the current node, and the open table is readjusted to the minimum heap until the convergence condition is met.

3. According to claim 2, a method for cruising low-altitude penetration mission trajectory planning based on an improved A* algorithm is characterized in that: The cost function of the improved A* trajectory planning algorithm is: f=ω1·L x +ω2·L h +ω3·L d +ω4·W+ω5·T+ω6·U Among them, L x To determine the actual distance between the current node and the starting point using the Manhattan method, L h To represent the actual distance from the current node to the target point, L d is the flight cost affected by the elevation of the ground projection point corresponding to the current track node, W is the flight cost corresponding to the threat zone and the no-fly zone, T is the cost of the number of turns from the starting point to the current track point, and U is the terminal angle constraint cost value; ω1 is the weight coefficient corresponding to the Manhattan distance, ω2 is the weight coefficient corresponding to the actual distance from the current node to the target point, ω3 is the weight coefficient corresponding to the influence of the terrain elevation on the two-dimensional track planning, ω4 is the weight coefficient corresponding to the influence of the threat zone and the no-fly zone, ω5 is the weight coefficient corresponding to the cost of the number of turns, and ω6 is the cost coefficient corresponding to the terminal angle constraint.

4. According to claim 1, a method for cruising low-altitude penetration mission trajectory planning based on an improved A* algorithm is characterized in that: After generating the two-dimensional track points, the method further includes interpolating multiple track points with equal steps between two adjacent track points.

5. According to claim 1, a method for cruising low-altitude penetration mission trajectory planning based on an improved A* algorithm is characterized in that: Determine the initial track height of a 2D track point, including: Calculate the terrain height corresponding to all track points through DEM data and obtain the maximum elevation in the height profile; Determine the initial track altitude of the track point based on the maximum altitude, minimum relative ground altitude, and the altitude of the starting point: h=max{h0,(H max +Δh min +1000m)} Among them, h0 is the height of the starting point, H max is the maximum elevation, Δh min is the minimum relative ground height.

6. According to claim 1, a method for cruising low-altitude penetration mission trajectory planning based on an improved A* algorithm is characterized in that: Based on the maximum dive angle constraint and the minimum flight altitude constraint of the aircraft, the two-dimensional track point is gradually sunk from the initial track altitude that meets the constraints, and the sinking stops when the constraint is met and the minimum relative flight altitude is reached; including: Determine the settlement margin of the track point based on the distance between two adjacent track points, the dive angle constraint, the climb angle constraint, and the maximum settlement step length; The initial track height of the track point is settled based on the settlement margin, and the height after settlement is compared with the minimum relative ground height: if the height after settlement is higher than the minimum relative ground height, the track point can be settled, otherwise it cannot be settled; Check whether the current track point meets the constraints after settling. If the current track point can settle and the previous track point cannot settle, it is necessary to check whether the track point after settlement and the previous track point meet the aircraft dive constraint; if the current track point can settle and the next track point cannot settle, it is necessary to check whether the track point after settlement and the next track point meet the aircraft climb constraint; if the constraints are met, reduce the current altitude by the settlement margin.

7. According to claim 6, a method for cruising low-altitude penetration mission trajectory planning based on an improved A* algorithm is characterized in that: The settlement margin of the track point is determined based on the distance between two adjacent track points, the dive angle constraint, the climb angle constraint, and the maximum settlement step length, specifically: ΔH=min{ΔH max ,δ×L i ×min(tan(θ min ),tan(θ max ))} Among them, δ is the safety factor, L i is the distance between two adjacent track points, θ min is the dive angle constraint, θ max is the climb angle constraint, ΔH max is the maximum settlement step length.

8. A computer system, characterized in that include: One or more processors, and a computer-readable storage medium for storing one or more programs, wherein when the one or more programs are executed by the one or more processors, the one or more processors are enabled to implement the method of claim 1.

9. A computer-readable storage medium, characterized in that Computer executable instructions are stored, and when the instructions are executed, they are used to implement the method of claim 1.

10. A computer program product, characterized in that The invention comprises computer executable instructions, which are used to implement the method of claim 1 when being executed.

Images