A trajectory planning method based on optimal estimated cost
By introducing the optimal estimated cost and heuristic functions in the aircraft track planning, combined with the A* algorithm, the problem of inefficiency of the A* algorithm in complex environments is solved, and the planning of high flyability and high-quality tracks is realized. The multiple constraints of the aircraft are met.
Patent Information
- Application Number
- CN202210413110.5
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2022-04-19
- Publication Date
- 2024-12-31
- Estimated Expiration
- 2042-04-19
AI Technical Summary
The existing A* algorithms are difficult to efficiently plan high-flyability and high-quality aircraft tracks in complex environments, and it is difficult to meet the flyability, optimization and safety requirements of the tracks.
Using the aircraft track planning method based on the optimal estimation cost, heuristic functions are constructed through discrete state nodes of the terrain elevation model, combining track length, altitude, horizontal turning, climbing and descending costs, and using the A* algorithm for track planning. And prune neighbor nodes that do not meet the constraints during the planning process.
It realizes the rapid and efficient planning of highly flyable aircraft tracks, meets the aircraft track requirements under different constraints, and improves the track trackability and energy consumption efficiency.
Smart Images
Figure CN115018118B_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to the technical field of aircraft trajectory planning, and in particular to an aircraft trajectory planning method based on graphics and heuristic search. Background Art
[0002] Aircraft have the advantages of good maneuverability, strong adaptability, and high flexibility. They are widely used in various fields of daily life, including traffic patrol, express logistics, environmental monitoring, etc. The trajectory planning function of aircraft is the basis of these functions. The trajectory planning function of aircraft is to autonomously find an optimal obstacle-free route from the starting position to the target position in a known or unknown working environment, in accordance with certain constraints and criteria. The A* algorithm is a basic trajectory planning algorithm that can find a better trajectory. However, the A* algorithm has some defects. For example, as the environment becomes more complex, the efficiency of the A* algorithm in searching for trajectories becomes lower, and the flyability of the searched trajectories will be weakened, making it more difficult to meet the requirements of actual flight. Summary of the invention
[0003] At present, the conventional A* algorithm can only meet the most basic obstacle avoidance requirements in the trajectory planning problem, and the trajectory planning of the aircraft also needs to consider the flyability, optimality and safety issues. Therefore, the present invention proposes an aircraft trajectory planning method based on the optimal estimated cost, the purpose of which is to improve the energy efficiency, flyability and traceability of the aircraft trajectory. The method of the present invention can efficiently plan an aircraft trajectory with high flyability and high quality to meet the requirements for the aircraft trajectory under different constraints and enhance the flyability of the trajectory.
[0004] The optimal estimated cost-based trajectory planning method of the present invention comprises the following steps: (1) first, using a terrain elevation model to discretize the planning space required for flight into state nodes; (2) secondly, constructing a heuristic function of the A* algorithm based on the optimal estimated cost; (3) then, using the A* algorithm to perform trajectory planning.
[0005] In the step (2), five costs, namely, track length, track altitude, horizontal turn, climb and descent, are comprehensively considered as optimization indicators to construct a heuristic function and its constraints.
[0006] Assume the current track node of the aircraft is P i (x i ,y i ,z i ), the successor node is P i+1 (x i+1 ,y i+1 ,z i+1 ), then the track segment P i P i+1 The heuristic function cost(Pi ,P i+1 )as follows:
[0007] cost(P i ,P i+1 )=k1C L +k2r1C h +k3r2C t +k4r2C c +k5r2C d
[0008] Among them, C L , C h , C t , C c and C d They are track segments P i P i+1 The track length cost, track height cost, level turn cost, climb cost and descent cost; k j (j=1,...,5) is the weight corresponding to each cost, ranging from [0 to 1], and k1+k2+k3+k4+k5=1; the k j (j=1,...,5) is pre-set according to user requirements. When a certain cost is paid attention to, the weight of the cost will be increased; r1 and r2 are the normalization coefficients corresponding to the height cost item and each angle item.
[0009] When constructing state nodes, the method of the present invention imposes constraints on the horizontal spacing and vertical spacing of the nodes for construction.
[0010] In the A* algorithm search process, the method of the present invention prunes the neighborhood nodes that do not meet the following constraints: the horizontal turning angle of the track is within the maximum value constraint of the horizontal turning angle.
[0011] Compared with the prior art, the advantages and positive effects of the present invention are as follows: (1) The method of the present invention constructs a heuristic function by introducing length cost, height cost, horizontal turning cost and climbing and descending cost, and can meet the requirements for the aircraft track under different constraints by adjusting the heuristic function. (2) The present invention discretizes the terrain elevation model into state nodes by imposing constraints on the horizontal and vertical spacing of nodes, and then uses the A* algorithm to plan the track in combination with the heuristic function constructed by the various costs introduced, and prunes the neighboring nodes during the planning process to speed up the search efficiency, and finally plans a high-quality track. It has been verified through experiments that the method of the present invention can quickly and efficiently plan a highly flyable aircraft track to meet actual flight requirements. BRIEF DESCRIPTION OF THE DRAWINGS
[0012] Figure 1is an overall flow chart of an aircraft trajectory planning method based on optimal estimated cost according to an embodiment of the present invention;
[0013] Figure 2 It is a schematic diagram of discretizing the planning space into state nodes according to an embodiment of the present invention;
[0014] Figure 3 It is a schematic diagram of cutting neighboring nodes according to an embodiment of the present invention;
[0015] Figure 4 is a top view schematic diagram of a simulation result of the length cost ratio of an embodiment of the present invention;
[0016] Figure 5 is a top view schematic diagram of a simulation result of the height cost ratio of an embodiment of the present invention;
[0017] Figure 6 It is a top view schematic diagram of a simulation result of the proportion of turning cost in an embodiment of the present invention. DETAILED DESCRIPTION
[0018] In order to enable those skilled in the art to better understand the present application, the technical solution of the present application will be clearly and completely described below in conjunction with the embodiments and drawings. Obviously, the described embodiments are only part of the embodiments of the present application, rather than all of the embodiments. Based on the embodiments in the present application, all other embodiments obtained by those skilled in the art without making creative work are within the scope of protection of the present application.
[0019] The embodiment of the present invention is based on the A* algorithm to implement an aircraft trajectory planning method based on the optimal estimated cost. The following first describes the construction of the space state node of the aircraft flight, then describes the heuristic function based on the constraints and optimization indicators of the trajectory planning, and finally describes the specific trajectory construction method. The overall process is as follows: Figure 1 shown.
[0020] Step 1: Constructing state nodes using the terrain elevation model The aircraft trajectory planning method of the present invention first discretizes the planning space required for the aircraft flight into state nodes.
[0021] A terrain elevation model is a data set that describes the elevation or altitude information of the earth's surface and its spatial distribution using a set of ordered numerical arrays. It is a discrete digital expression of the earth's surface topography. In order to plan a highly flyable trajectory, the present invention firstly calculates the horizontal node spacing (d x ,d y ) and vertical node spacing d z Set the state nodes in horizontal and vertical directions.
[0022] In this embodiment, the horizontal and vertical node spacings on the horizontal plane are approximately equal, that is: x ≈d y ≈d xy Among them, d xy is a specific value, which needs to be set according to the requirements for the track before building the node. The higher the quality requirement for the track, the higher the value of d xy The smaller the value is, the more likely it is that the planned flight path will be more flyable. In this paper, constraints are imposed on the horizontal and vertical spacing between nodes. The horizontal node spacing is constrained to a short flight path segment, which is specifically expressed as: i ≥d xy ≥min(d x ,d y )≥l min ,i=1,...,n. Among them, l i represents the length of the i-th track, l min Indicates the shortest distance between track points, and n indicates the number of track segments. In the vertical direction, the climb angle and descent angle constraints are imposed on the node spacing: Where θ represents the climbing angle or descent angle of the aircraft, θ max Indicates the maximum climbing angle or descending angle. Figure 1 The above is a schematic diagram of discretizing the planning space into state nodes in this embodiment.
[0023] Step 2: Construct a heuristic function. The meaning of the heuristic function in the A* algorithm is the minimum cost evaluation value from any state node to the target node. The aircraft trajectory planning method of the present invention is based on the A* algorithm, considers the flyability and optimality of the trajectory, and constructs a heuristic function with five costs, namely, the trajectory length cost, the trajectory height cost, the horizontal turning cost, the climbing cost, and the descending cost, as constraints and optimization indicators.
[0024] The present invention uses the above five costs as the heuristic function composition for the following reasons: First, the track length directly determines the range and flight time required for the aircraft to perform the mission, and is also the main factor affecting the flight energy consumption; and the track point altitude affects the track cost from the two aspects of flight energy consumption and track threat: the higher the flight altitude of the aircraft, the greater its flight energy consumption; in some low-altitude reconnaissance or penetration missions, a lower flight altitude helps the aircraft to use the cover formed by the terrain or obstacles to avoid threats. The reason for considering the horizontal turning cost, the climbing cost and the descending cost is that such a turn, climb or descend in the track will bring more twists and turns and maneuvering energy consumption, which will affect the flyability of the track.
[0025] Compared with the A* trajectory planning algorithm that only uses the trajectory length as the cost function, the present invention can plan a more flyable trajectory by considering the heuristic function constructed by considering different costs, and for different constraints, the requirements can be met by adjusting the weights of various costs in the heuristic function, which has greater flexibility.
[0026] Assume that the aircraft flies along the entire track at a uniform speed, and the current track node is P i (x i ,y i ,z i ), whose predecessor node is P i-1 (x i-1 ,y i-1 ,z i-1 ), the successor node is P i+1 (x i+1 ,y i+1 ,z i+1 ), then in this embodiment, the track segment P i P i+1 The cost expression between is as follows:
[0027] 1) Track length cost:
[0028] Track segment P i P i+1 The track length cost C L The calculation is as follows:
[0029]
[0030] 2) Track height cost:
[0031] Track segment P i P i+1 The track height cost C h The calculation is as follows:
[0032]
[0033] 3) Horizontal turning cost:
[0034] Track segment P i P i+1 The horizontal turning cost C t The calculation is as follows:
[0035]
[0036] 4) Climbing cost:
[0037] Track segment P i P i+1 The climbing cost C c The calculation is as follows:
[0038]
[0039] 5) Reduce costs:
[0040] Track segment P i P i+1 The cost of reduction C d The calculation is as follows:
[0041]
[0042] Finally, the five cost functions are combined to construct the heuristic function cost(P i ,P i+1 )as follows:
[0043] cost(P i ,P i+1 )=k1C L +k2r1C h +k3r2C t +k4r2C c +k5r2C d
[0044] Among them, k j (j=1,...,5) is the weight corresponding to each cost, which is related to the performance of the aircraft and environmental conditions, and the value range is [0~1], and k1+k2+k3+k4+k5=1; r1 and r2 are the normalization coefficients corresponding to the height cost item and each angle item. Before performing trajectory planning, the weights need to be pre-set according to the needs.
[0045] Step 3: Plan the aircraft trajectory.
[0046] After constructing the state nodes and the heuristic function, the present invention performs track planning through the A* algorithm.
[0047] The process of A* algorithm for trajectory planning is as follows:
[0048] 1) Add the starting point to the Open List of points to be retrieved;
[0049] 2) If the Open List is empty, there is no feasible path and the algorithm ends;
[0050] 3) Traverse the Open List, find the node with the smallest current heuristic function, treat it as the currently processed node, and move it to the retrieved list Close List;
[0051] 4) For each neighboring node of the current processing node that is not in the Close List, if it is not in the Open List, add it to the Open List, set the current processing node as the parent node of the neighboring node, and update its heuristic function G value and H value; if it is in the Open List, determine whether the G value obtained from the current processing node to the neighboring node is smaller than the original G value of the neighboring node; if so, set the current processing node as the parent node of the neighboring node. The G value is the actual cost from the starting node along the generated path to the current processing node, which is calculated by calculating the weighted sum of the five costs between every two adjacent points on the path, that is, cost (P i ,P i+1 ) is accumulated; the H value is the estimated cost from the current processing node to the target node, which is obtained by calculating the distance between the current point and the end point;
[0052] 5) Repeat steps 2) to 4) until the end point is added to the Open List;
[0053] Starting from the end point, each node is connected to its parent node to obtain the planned trajectory.
[0054] In order to enhance the flyability of the planned trajectory and improve the efficiency of trajectory planning, the present invention adopts a method of pruning the neighboring nodes that do not meet the constraints during the trajectory planning process, such as Figure 3 As shown. The constraint refers to the arc or turning radius limit formed by the aircraft flight turn. In this embodiment, the turning radius of the track is limited by the maximum value constraint of the horizontal turning angle. The horizontal turning angle α refers to the projection of the track turning angle on the horizontal plane. Let P i (x i ,y i ) represents the current node, P i-1 (x i-1 ,y i-1 ) indicates P i The predecessor node, P i+1 (x i+1 ,y i+1 ) indicates P i The successor node of . Then the horizontal turning angle α constraint is specifically:
[0055]
[0056] Among them, α max is the maximum horizontal turning angle.
[0057] like Figure 3 The black solid lines in (a) and (b) represent the track, the arrows represent the direction of the track, and the dots represent the planning space nodes. Each dot represents three nodes in the vertical direction of the corresponding horizontal point.i+1 It represents the nodes that satisfy the horizontal turning angle constraint, and the nodes without labels are the pruned neighboring nodes. Figure 3 (c) and (d) are three-dimensional examples of neighborhood node pruning, where the hollow circles are the pruned nodes.
[0058] Figure 4 This is a simulated top-down comparison diagram of the track obtained based on the optimal estimated cost of the present invention and the track obtained by the traditional A* algorithm when the parameter K = [k1, k2, k3, k4, k5] = [0.5, 0.2, 0.1, 0.15, 0.05], r1 = 0.001, r2 = 500. From the given parameter K, it can be seen that the length cost accounts for the largest proportion of the cost, so the distance of the track will be more important when performing track planning. Although the length cost accounts for the largest proportion, other costs are also part of the consideration in the track planning process. Therefore, the planned track will be smoother and more flyable. According to the simulation results, it can be seen that the use of the method of the present invention for aircraft track planning can plan an aircraft track with high flyability.
[0059] When there is a certain requirement on the track height, it can be achieved by increasing the weight of the track height cost in the estimated cost.
[0060] Figure 5 This is a simulation comparison diagram of the track obtained based on the optimal estimated cost and the traditional track when K = [0.25, 0.5, 0.1, 0.15, 0]. It can be seen from the figure that the track obtained by the present invention based on the optimal estimated cost is closer to the ground and has a lower altitude than the track obtained by the traditional A* algorithm planning.
[0061] Similarly, if you want fewer turns in the trajectory, you can appropriately increase the weight of the turning cost in the estimated cost.
[0062] Figure 6 This is a simulated top-down comparison diagram of the track obtained by the present invention based on the optimal estimated cost and the track obtained by the traditional A* algorithm when K = [0.3, 0.1, 0.4, 0.15, 0.05]. It can be seen from the figure that under this parameter condition, the track obtained by the present invention based on the optimal estimated cost planning has fewer turning points.
[0063] Table 1 is a comparison of some parameters of the trajectory planned by the traditional A* algorithm and the trajectory planned based on the optimal estimated cost under three different K parameters.
[0064] Table 1 Parameter comparison between the method of the present invention and the traditional A* algorithm under three different K parameters
[0065]
[0066]
[0067] The data in the table further verify that the present invention can plan a high-flyability trajectory that meets different requirements.
[0068] The above embodiments are preferred implementation modes of the present invention, but the implementation modes of the present invention are not limited to the above embodiments. Any other changes, modifications, substitutions, combinations, and simplifications that do not deviate from the spirit and principles of the present invention should be equivalent replacement methods and are included in the protection scope of the present invention.
Claims
1. A trajectory planning method based on optimal estimated cost, characterized in that: The method (1) firstly discretizes the planning space required for flight into state nodes using a terrain elevation model; (2) secondly constructs a heuristic function of the A* algorithm based on the optimal estimated cost; (3) finally uses the A* algorithm to perform trajectory planning; In the above (1), the horizontal node spacing (d x ,,d y ,) and vertical node spacing d z Set the state nodes in the horizontal and vertical directions, and let the horizontal spacing d of the nodes on the horizontal plane x and longitudinal spacing d y Both xy Approximately equal, d xy Before building the nodes, set constraints on the horizontal and vertical spacing of the nodes. Specifically, the constraint of short track segments is imposed on the horizontal spacing of the nodes, expressed as: l i ≥d xy ≥min(d x ,d y )≥l min ,i=1,...,n; where l i represents the length of the i-th track, l min represents the shortest distance between track points, and n represents the number of track segments. The constraints of climb angle and descent angle are imposed on the longitudinal spacing of nodes, which is expressed as: Where θ represents the climbing angle or descent angle of the aircraft, θ max Indicates the maximum climbing angle or descending angle; In (2) above, the heuristic function is constructed by comprehensively considering five costs, namely, track length, track altitude, horizontal turn, climb and descent, as optimization indicators, as follows: Assume the current track node of the aircraft is P i (x i ,y i ,z i ), the successor node is P i+1 (x i+1 ,y i+1 ,z i+1 ), then the track segment P i P i+1 The heuristic function cost(P i ,P i+1 )as follows: cost(P i ,P i+1 )=k1C L +k2r1C h +k3r2C t +k4r2C c +k5r2C d ; Among them, C L , C h , C t , C c and C d They are track segments P i P i+1 The track length cost, track height cost, level turn cost, climb cost and descent cost; k j (j=1,...,5) is the weight corresponding to each cost, ranging from [0 to 1], and k1+k2+k3+k4+k5=1; the k j (j=1,...,5) is pre-set according to user requirements. When a certain cost is paid attention to, the weight of the cost will be increased; r1 and r2 are the normalization coefficients corresponding to the height cost item and each angle item; The costs are calculated as follows: Track segment P i P i+1 The track length cost Track segment P i P i+1 Track height cost between Track segment P i P i+1 The horizontal turn cost P i-1 P i The predecessor node of Track segment P i P i+1 The cost of climbing Track segment P i P i+1 The cost of decline In (3) above, during the A* algorithm search process, the neighboring nodes that do not satisfy the following constraints are pruned: The turning radius of the track is limited by the maximum value constraint of the horizontal turning angle; let α max is the maximum value of the horizontal turning angle, the horizontal turning angle P i-1 P i The predecessor node.