Flight path planning method meeting performance constraint of unmanned helicopter

Abstract

The invention discloses a flight path planning method meeting unmanned helicopter performance constraints. The method belongs to the field of unmanned helicopter trajectory planning, and comprises the following specific steps: establishing a grid digital map according to an actual environment, and planning an optimal path meeting performance constraints in the grid digital map by using a heuristic search algorithm; removing redundant points by using a path optimization algorithm to form an optimal leg; and processing the generated leg by using a local planning algorithm to obtain a feasible trajectory meeting performance constraints, and avoiding new obstacle threats based on an existing path in combination with a surrounding environment during local planning. According to the method, the problems of dynamic planning, real-time obstacle avoidance and less consideration of performance constraints of the unmanned helicopter in the existing path planning method can be solved, and the practicability and the reliability are improved.

Description

Technical Field

[0001] This invention belongs to the field of unmanned helicopter trajectory planning and relates to a flight trajectory planning method that satisfies the performance constraints of unmanned helicopters. Background Technology

[0002] Unmanned helicopters (UHHs) possess flexible flight characteristics and complex flight control, thus attracting significant attention from researchers. Compared to traditional fixed-wing drones, UHHs offer advantages such as vertical takeoff and landing, hovering, arbitrary flight direction, and lower requirements for takeoff and landing sites, making them promising for both civilian and military applications. When performing missions in flight environments with known obstacles, UHHs require the ability to plan optimal trajectories in real-time while meeting constraints to achieve autonomous flight. Trajectory planning is crucial for the successful completion of missions by UHHs. Existing technologies primarily focus on path planning in two-dimensional space, neglecting three-dimensional space considerations and offering limited consideration of performance constraints. Summary of the Invention

[0003] The purpose of this invention is to provide a flight trajectory planning method that satisfies the performance constraints of unmanned helicopters, thereby addressing the problem that existing path planning methods do not adequately consider the performance constraints of unmanned helicopters.

[0004] To achieve the above objectives, the present invention adopts the following technical solution:

[0005] A flight trajectory planning method that satisfies the performance constraints of an unmanned helicopter, the specific steps of which are as follows:

[0006] Step (1): Use an improved heuristic search algorithm in the grid map to plan the optimal path that meets the performance constraints;

[0007] Step (2): Remove redundant points in the optimal path using a path optimization algorithm to form a straight flight segment;

[0008] Step (3): Process the generated straight-line segments using the improved dynamic window algorithm to generate the optimal trajectory that satisfies the performance constraints of the unmanned helicopter;

[0009] Furthermore, the expression for the heuristic search algorithm that integrates high-level evaluation information is:

[0010]

[0011] In the formula, the evaluation function F(n) represents the total cost from the starting point to the current node n and then to the target point; g(n) is the dissipation function, representing the actual cost from the starting node to the current node n; h(n) is the heuristic function, representing the estimated cost from the current node n to the target point; altitude1(n) and altitude2(n) are the height evaluation functions, and λ h The dynamic weights are used to improve the directional guidance of the algorithm, reduce the number of turns in the path, and improve the efficiency of the algorithm. e1 is the weight of altitude1(n), and e2 is the weight of altitude2(n).

[0012] The expression for the function altitude1(n) is:

[0013]

[0014] Where h n h is the height of node n. min For safe flight altitude. Its main function is to ensure that the planned flight path remains at a low altitude.

[0015] The expression for the function altitude2(n) is:

[0016]

[0017] Where h m This represents the altitude of the destination. Its main function is to ensure that the planned path altitude does not differ too much from the destination altitude, preventing significant differences in horizontal distance and altitude in the final flight segment. Dynamic weight λ h The expression is:

[0018]

[0019] Furthermore, the search node method of the heuristic search algorithm integrating high-level evaluation information is as follows:

[0020] When searching for nodes, yaw and pitch angle constraints for the unmanned helicopter are introduced. The expanded nodes searched are the three-layer neighbor nodes of the current node. First, it is necessary to ensure that the grid where the node is located is a free grid. Second, it is necessary to check whether the angle between the line connecting the expanded node and the current node and the current heading is less than or equal to the maximum yaw angle of the unmanned helicopter. At the same time, it is also necessary to check the pitch angle between the expanded node and the current node, discarding expanded nodes with excessive yaw and pitch angles to ensure that the path does not exceed the performance range of the unmanned helicopter. Finally, path collision detection is required to ensure that the line connecting the expanded node and the current node does not cross obstacles and that the path does not collide with obstacles.

[0021] Furthermore, the heuristic search algorithm utilizing integrated altitude assessment information to plan ground-hugging flight paths in real time on the grid map specifically involves:

[0022] Step 1: Establish the openset, closeset, and dist tables to store node data within the space. The openset table stores the cost and index of nodes that have been searched. The closeset table stores the optimal node, i.e., the node with the lowest cost in the openset table. The dist table stores the actual cost from the starting point to that node. At the start of the algorithm, data initialization is performed, and it is determined whether the starting and ending points are obstructed by obstacles. If they are obstructed, the program terminates, and the search fails.

[0023] Step 2: Set the starting point as the current node, set the actual cost of the starting point to 0, store the distance traversed from the starting point to the current node (i.e., the actual cost) in the dist table, put the cost of the starting point (the actual cost is 0, which is the estimated value) and the node number of the starting point into the openset table, and put the current node into the closeset table.

[0024] Step 3: Set the cost value of the current node in the openset table to infinity, and search for the extended nodes of the current node. Check if each extended node is in the openset table and if it meets the performance constraints of the unmanned helicopter. If not, add it to the openset table, calculate its cost value, and store it in the openset table. Calculate the actual cost value of the extended node, store it in the dist table, and set the current node as the parent node of this extended node (the parent node of a node is its previous immediate neighbor). If the extended node is in the openset table, use the actual cost value as the criterion to check if the path is better. Compare the current actual cost value with the previous actual cost value stored in the dist table. The path with the smaller actual cost value is considered a better route. If the current actual cost value is smaller, update the data in dist and the cost value in the openset table, and update the parent node to the current node. If the current actual cost value is larger, do nothing and retain the original value.

[0025] Step 4: Is the number of nodes searched greater than the number of grid cells? If it is, the program ends and the search fails. Find the node m with the smallest cost value from the openset table. Determine if node m is the endpoint. If it is the endpoint, jump to step 5. If not, set node m as the current node, put node m into the closeset table, and then repeat step 3.

[0026] Step 5: When the current node is the target node, the nodes stored in the closeset table represent the optimal path found. To store the path, starting from the destination, sort all nodes in reverse order along their parent nodes back to the starting point; this will yield the feasible paths.

[0027] Furthermore, the redundant waypoint elimination algorithm is specifically as follows:

[0028] Step 1, set the starting point As the first waypoint At the same time, as a reference point, sequentially... and subsequent waypoints Connect the dots;

[0029] Step two, when the reference point is aligned with the first... If the line connecting the points collides with an obstacle or fails to meet the performance constraints of the unmanned helicopter, then... As the next waypoint ;

[0030] Step 3, set waypoints As a new reference point, connect it to subsequent path points in turn to check for connections, and repeat the above steps until the destination is reached;

[0031] Step 4: Output the optimized waypoints and straight segments, perform secondary optimization on the generated route, and repeat steps 1 to 3.

[0032] Step 5: Output the waypoints and straight segments after secondary optimization.

[0033] Furthermore, the velocity sampling space of the improved dynamic window method is specifically as follows:

[0034]

[0035]

[0036]

[0037] Where V s This represents the set of ranges for the unmanned helicopter's own horizontal, vertical, and angular velocities between its minimum and maximum. s1min v s1max This represents the minimum and maximum horizontal speeds of the unmanned helicopter, v. s2min v s2max This represents the minimum and maximum vertical speeds of the unmanned helicopter. ω smin ω smax This represents the minimum and maximum yaw rates of the unmanned helicopter. s1c v s2c ωsc This represents the horizontal speed, vertical speed, and yaw rate of the unmanned helicopter at the current moment. s11 a s12 This represents the maximum horizontal deceleration and maximum horizontal acceleration of the unmanned helicopter, a. s21 a s22 This represents the maximum vertical deceleration and maximum vertical acceleration of the unmanned helicopter, α. s1 α s2 This represents the maximum angular deceleration and maximum angular acceleration of the unmanned helicopter, dist(v). s1 , v s2 , ω s This represents the closest distance to the obstacle. In addition to the three speed constraints mentioned above, constraints on vertical and horizontal speeds also need to be added to prevent excessively large glide angles. Specifically:

[0038]

[0039] Where θa represents the maximum glide angle of the unmanned helicopter.

[0040] Furthermore, the evaluation function of the improved dynamic window method is specifically as follows:

[0041]

[0042] Where μ, ρ, and τ are all weighting coefficients, and the function his1(v s1 , v s2 , ω s ) and the function his2(v s1 , v s2 , ω s The two are sub-functions used to evaluate the height, function d(v) s1 , v s2 , ω s His1(v) is a function used to evaluate the distance to the destination. s1 , v s2 , ω s The expression for ) is:

[0043]

[0044] Where h s This represents the absolute value of the difference between the height of the trajectory's end position and the height of its endpoint. The purpose of this function is to ensure that the selected trajectory maintains the same height as the endpoint, allowing the optimized trajectory to decrease its height in a shorter time and reducing the risk of detection. The closer the height of the trajectory's end position is to the height of the endpoint, the larger this value becomes, and the higher the trajectory's score will be. The function his2(vs1 , v s2 , ω s The expression for ) is:

[0045]

[0046] Where H is a constant, h d h represents the height of the end position of the trajectory. min The minimum safe flight altitude for unmanned helicopters, h max This represents the upper limit of flight altitude. If the final altitude is higher than the starting altitude, the function his1(v) will be used instead. s1 , v s2 , ω s This approach doesn't allow the unmanned helicopter to maintain a low altitude; instead, the higher final altitude causes it to ascend more quickly and complete the flight at a higher altitude. Therefore, the function his2(v) is designed... s1 ,v s2 , ω s The main function of this feature is to ensure that the unmanned helicopter flies within the permitted altitude range for the mission. When the destination altitude is relatively high, this function allows the unmanned helicopter to increase its flight altitude as it approaches the destination, and to fly at a lower altitude when it is far from the destination. The lower the altitude at the end of the trajectory, the larger the value of this function, and the higher the trajectory score will be. However, the altitude of the unmanned helicopter should not be too low. When the altitude at the end of the trajectory is below the safe flight altitude, the value of this feature is 0, and the trajectory score will decrease accordingly. The function d(v s1 , v s2 , ω s The expression for ) is:

[0047]

[0048] Where d s This represents the distance between the end position of the trajectory and the endpoint. The main function of this function is similar to the function `heading(v)`. s1 , v s2 , ω s The differences are minimal; both aim to move towards the destination. Another function of this function is to help the trajectory reach the destination in a shorter time. The closer the trajectory's end is to the destination, the larger the value of this function, resulting in a higher score for the trajectory. This allows the optimized trajectory to move towards the destination while reaching it in a shorter time, improving algorithm efficiency.

[0049] Furthermore, the improved dynamic window method optimizes the flight path to form an executable trajectory for the unmanned helicopter, specifically as follows:

[0050] Step 1: Set the initial parameter sampling time Δt, start velocity sampling from the starting point of the trajectory, and obtain the velocity and angular velocity range of the unmanned helicopter.

[0051] Step two: Based on the velocity sampling space and sampling time, simulate the flight trajectory using the kinematic model of the unmanned helicopter to obtain several trajectories that satisfy the performance constraints of the unmanned helicopter. The kinematic model of the unmanned helicopter is as follows:

[0052]

[0053] Where x s y s , z s ψ represents the position of the unmanned helicopter at time t. s This represents the yaw angle of the unmanned helicopter at time t, v s1 This represents the horizontal velocity of the unmanned helicopter at time t, v s2 This represents the vertical velocity of the unmanned helicopter at time t.

[0054] Step 3: Use the evaluation function to evaluate feasible trajectories and select the optimal trajectory. Determine if the trajectory end is the end of a segment or the target point. If not, set the trajectory end as the new search starting point and repeat steps 1 to 3. If it is the end of a segment, set the starting point of the next segment as the new search starting point and repeat steps 1 to 3. If it is the target point, the program ends and outputs the optimal trajectory.

[0055] The beneficial effects of this invention compared to the prior art are as follows:

[0056] This invention can solve the problems of insufficient consideration of performance constraints in the dynamic planning and real-time obstacle avoidance of unmanned helicopters in existing path planning methods, thereby improving practicality and reliability. Attached Figure Description

[0057] Figure 1 Improved A* algorithm extension diagram

[0058] Figure 2 Flowchart of the improved A* algorithm in this invention

[0059] Figure 3 The principle of redundant waypoint elimination algorithm in this invention Figure 1 ;

[0060] Figure 4 The principle of redundant waypoint elimination algorithm in this invention Figure 2 ;

[0061] Figure 5 Flowchart of the improved dynamic window algorithm in this invention;

[0062] Figure 6Comparison diagram of the improved A* algorithm and the ordinary A* algorithm in this invention;

[0063] Figure 7 Comparison of paths planned by the algorithm with and without dynamic weights;

[0064] Figure 8 Comparison chart of the optimization of redundant waypoint elimination algorithms in this invention;

[0065] Figure 9 A schematic diagram of trajectory simulation using the dynamic window method in this invention;

[0066] Figure 10 Figure showing the improved dynamic window method for optimizing trajectory testing in the invention; Detailed Implementation

[0067] To more clearly illustrate the technical solution of the present invention, the technical solution of the present invention will be further described in detail below with reference to the accompanying drawings:

[0068] As shown in the figure, the specific steps of the flight trajectory planning method for unmanned helicopters that meets performance constraints according to the present invention are as follows:

[0069] Step 1: Use a heuristic search algorithm that integrates altitude assessment information, namely the improved A* algorithm, to plan a ground-hugging flight path on a grid map;

[0070] The improved A* algorithm is extended as follows: The yaw and pitch angles of the unmanned helicopter are limited, and the searched expanded nodes are the three-layer neighbors of the current node. First, it is necessary to ensure that the grid containing the node is a free grid. Second, it is necessary to check whether the angle between the line connecting the expanded node and the current node and the current heading is less than or equal to the maximum yaw angle of the unmanned helicopter. It is also necessary to check the pitch angle between the expanded node and the current node, discarding expanded nodes with excessively large yaw and pitch angles to ensure that the path does not exceed the performance range of the unmanned helicopter. Finally, path collision detection is required to ensure that the line connecting the expanded node and the current node does not cross obstacles, and to ensure that the path does not collide with obstacles. An extended schematic diagram is shown below. Figure 1 As shown.

[0071] The expression for the evaluation function of the improved A* algorithm is as follows:

[0072]

[0073] In the formula, the evaluation function F(n) represents the total cost from the starting point to the current node n and then to the target point; g(n) is the dissipation function, representing the actual cost from the starting node to the current node n; h(n) is the heuristic function, representing the estimated cost from the current node n to the target point; altitude1(n) and altitude2(n) are the height evaluation functions, and λ h These are dynamic weights, designed to improve the algorithm's directional guidance, reduce the number of turns in the path, and increase algorithm efficiency. e1 is the weight of altitude1(n), and e2 is the weight of altitude2(n). The function altitude1(n) is expressed as: 0 < e1 < 1, 0 < e2 < 1;

[0074]

[0075] Where h n h is the height of node n. min For safe flight altitude. Its main function is to ensure that the planned flight path remains at a low altitude.

[0076] The expression for the function altitude2(n) is:

[0077]

[0078] Where h m This represents the altitude of the destination. Its main function is to ensure that the planned path altitude does not differ too much from the destination altitude, preventing significant differences in horizontal distance and altitude in the final flight segment. Dynamic weight λ h The expression is:

[0079]

[0080] Increase dynamic weight λ h It can effectively improve algorithm efficiency. For example, comparing the paths planned by the algorithm with and without dynamic weights... Figure 7 As shown, the improved A* algorithm with added dynamic weight coefficients takes 0.067s to plan the path, while the improved A* algorithm without added dynamic weight coefficients takes 0.1131s. This demonstrates that dynamic weight coefficients can improve algorithm efficiency and save time. Figure 6 As shown.

[0081] Specifically, the improved global path planning A* algorithm is used to plan global paths on the raster map, as shown in the flowchart below. Figure 2 The instructions include the following steps:

[0082] Step 1.1: The openset, closeset, and dist tables are used to store node data within the space. The openset table stores the cost value and node index of nodes that have been searched. The closeset table stores the optimal node, i.e., the node with the lowest cost value in the openset table. The dist table stores the actual cost value from the starting point to that node. At the beginning of the algorithm, data initialization is performed, and it is determined whether the starting and ending points are occupied by obstacles. If they are occupied, the program ends and the search fails.

[0083] Step 1.2: Set the current node, set the actual cost of the starting point to 0, store the distance traversed from the starting point to the current node (i.e., the actual cost) in the dist table, put the cost of the starting point (the actual cost is 0, which is the estimated value) and the node number of the starting point into the openset table, and put the current node into the closeset table.

[0084] Step 1.3: Set the cost value of the node in the openset table to infinity, and search for the extended nodes of the current node. Check if the extended node is in the openset table and if it meets the performance constraints of the unmanned helicopter. If not, add it to the openset table, calculate its cost value, and store it in the openset table. Calculate the actual cost value of the extended node, store it in the dist table, and set the current node as the parent node of this extended node (the parent node of a node is its previous immediate neighbor). If the extended node is in the openset table, use the actual cost value as the criterion to check if the path is better. Compare the current actual cost value with the previous actual cost value stored in the dist table. The path with the smaller actual cost value is considered a better route. If the current actual cost value is smaller, update the data in dist and the cost value in the openset table, and update the parent node to the current node. If the current actual cost value is larger, do not perform any processing and retain the original value.

[0085] Step 1.4: Is the number of points greater than the number of grid cells? If it is, the program ends and the search fails. Find the node m with the smallest cost value from the openset table. Determine if node m is the endpoint. If it is the endpoint, jump to step 5. If not, set node m as the current node, put node m into the closeset table, and then repeat step 3.

[0086] Step 1.5: When the target point is the endpoint, the nodes stored in the closeset table represent the optimal path. To store the path, starting from the endpoint, sort all nodes in reverse order along their parent nodes until the starting point; this yields the feasible paths.

[0087] To prevent the path planning algorithm from becoming unsolvable or entering an infinite loop due to unreasonable selection of the starting and ending points, this paper sets up three layers of protection for the path planning algorithm, specifically:

[0088] First, determine whether the starting point is reasonable. Ensure that the starting point is within a non-obstacle grid. If the starting point is within an obstacle grid, exit the path planning algorithm directly.

[0089] Second, determine whether the destination is reasonable. Ensure that the destination is within a non-obstacle grid. If the destination is within an obstacle grid, exit the path planning algorithm directly.

[0090] Third, determine whether the number of searched nodes has reached the maximum number of grid cells. If the maximum number of grid cells has been reached, exit the path planning algorithm to prevent it from entering a search dead loop.

[0091] in,

[0092] The grid number is calculated as follows:

[0093]

[0094] in, To set the grid length, To set the grid width, To set the grid height.

[0095] The flowchart of the path planning algorithm after adding protection measures is as follows: Figure 1 As shown.

[0096] Step 2: Use a redundant waypoint elimination algorithm to remove redundant pathpoints and form straight flight segments. This allows for the subsequent allocation of task time to segments of different lengths, better meeting actual task requirements. Figure 3 , Figure 4 , Figure 8 As shown, the specific steps include:

[0097] Step 2.1, set the starting point As the first waypoint At the same time, as a reference point, sequentially... and subsequent waypoints Connect the dots;

[0098] Step 2.2, when the reference point is... If the line connecting the points collides with an obstacle or fails to meet the performance constraints of the unmanned helicopter, then... As the next waypoint ;

[0099] Step 2.3, set waypoints As a new reference point, connect it to subsequent path points in turn to check for connections, and repeat the above steps until the destination is reached;

[0100] Step 2.4: After the waypoints and straight segments are optimized, the generated route is further optimized by repeating steps 2.1 and 2.3.

[0101] Step 2.5: Output the waypoints and straight segments after secondary optimization.

[0102] Step 3, as follows Figure 9 As shown, the improved window method is used to optimize the planned path and generate a trajectory that meets the performance constraints of the UAV, specifically:

[0103] Step 3.1: Initial parameter sampling time Δt. Start sampling the velocity from the starting point of the trajectory to obtain the velocity and angular velocity range of the unmanned helicopter.

[0104] Step 3.2: Velocity sampling space and sampling time. The flight trajectory is simulated using the kinematic model of the unmanned helicopter to obtain several trajectories that satisfy the performance constraints of the unmanned helicopter. The kinematic model of the unmanned helicopter is as follows:

[0105]

[0106] Where x s y s , z s ψ represents the position of the unmanned helicopter at time t. s This represents the yaw angle of the unmanned helicopter at time t, v s1 This represents the horizontal velocity of the unmanned helicopter at time t, v s2 This represents the vertical velocity of the unmanned helicopter at time t.

[0107] Step 3.3, as follows Figure 5 As shown, the feasible trajectory is evaluated using an evaluation function to select the optimal trajectory. It is determined whether the trajectory end is the end point of a flight segment or the target point. If not, the trajectory end is set as the new search starting point, and steps one through three are repeated. If it is the end point of a flight segment, the starting point of the next flight segment is set as the new search starting point, and steps 3.1 through 3.3 are repeated. The program then ends and outputs the optimal trajectory.

[0108] The velocity sampling space of the improved dynamic window method is specifically as follows:

[0109]

[0110]

[0111]

[0112] Where V sThis represents the set of ranges for the unmanned helicopter's own horizontal, vertical, and angular velocities between its minimum and maximum. s1min v s1max This represents the minimum and maximum horizontal speeds of the unmanned helicopter, v. s2min v s2max This represents the minimum and maximum vertical speeds of the unmanned helicopter. ω smin ω smax This represents the minimum and maximum yaw rates of the unmanned helicopter. s1c v s2c ω sc This represents the horizontal speed, vertical speed, and yaw rate of the unmanned helicopter at the current moment. s11 a s12 This represents the maximum horizontal deceleration and maximum horizontal acceleration of the unmanned helicopter, a. s21 a s22 This represents the maximum vertical deceleration and maximum vertical acceleration of the unmanned helicopter, α. s1 α s2 This represents the maximum angular deceleration and maximum angular acceleration of the unmanned helicopter, dist(v). s1 , v s2 , ω s This represents the closest distance to the obstacle. In addition to the three speed constraints mentioned above, constraints on vertical and horizontal speeds also need to be added to prevent excessively large glide angles. Specifically:

[0113]

[0114] Where θa represents the maximum glide angle of the unmanned helicopter.

[0115] The evaluation function for the improved dynamic window method is as follows:

[0116]

[0117] in , , , μ, ρ, and τ are all weighting coefficients, and their values ​​are all between (0, 1). This represents the corresponding simulated trajectory generated in space at that speed, the function his1(v s1 , v s2 , ω s ) and the function his2(v s1 ,v s2 , ω s The two are sub-functions used to evaluate the height, function d(v)s1 , v s2 , ω s His1(v) is a function used to evaluate the distance to the destination. s1 , v s2 , ω s The expression for ) is:

[0118]

[0119] Where h s This represents the absolute value of the difference between the height of the trajectory's end position and the height of its endpoint. The purpose of this function is to ensure that the selected trajectory maintains the same height as the endpoint, allowing the optimized trajectory to decrease its height in a shorter time and reducing the risk of detection. The closer the height of the trajectory's end position is to the height of the endpoint, the larger this value becomes, and the higher the trajectory's score will be. The function his2(v s1 , v s2 , ω s The expression for ) is:

[0120]

[0121] Where H is a constant, h d h represents the height of the end position of the trajectory. min The minimum safe flight altitude for unmanned helicopters, h max This indicates the upper limit of flight altitude, with H ranging from 0.7 to 0.9 times h. max If the endpoint height is higher than the starting point, relying solely on the function his1(v) will not resolve the issue. s1 , v s2 , ω s This approach doesn't allow the unmanned helicopter to maintain a low altitude; instead, the higher final altitude causes it to ascend more quickly and complete the flight at a higher altitude. Therefore, the function his2(v) is designed... s1 , v s2 , ω s The main function of this feature is to ensure that the unmanned helicopter flies within the permitted altitude range for the mission. When the destination altitude is relatively high, this function allows the unmanned helicopter to increase its flight altitude as it approaches the destination, and to fly at a lower altitude when it is far from the destination. The lower the altitude at the end of the trajectory, the larger the value of this function, and the higher the trajectory score will be. However, the altitude of the unmanned helicopter should not be too low. When the altitude at the end of the trajectory is below the safe flight altitude, the value of this feature is 0, and the trajectory score will decrease accordingly. The function d(v s1 , v s2 , ω s The expression for ) is:

[0122]

[0123] Where d s This represents the distance between the end position of the trajectory and the endpoint. The main function of this function is similar to the function `heading(v)`. s1 , v s2 , ω s The differences are minimal; both aim to move towards the destination. Another function of this function is to minimize the time required to reach the destination. The closer the trajectory's end is to the destination, the larger the value of this function, resulting in a higher score for the trajectory. This optimizes the trajectory's movement towards the destination while minimizing time, thus improving algorithm efficiency. Figure 10 As shown. The simulated trajectory with the largest evaluation function value is recorded as the optimal trajectory. When the h value corresponding to the optimal trajectory is... s With d s When all values ​​are less than the preset threshold, the end of the trajectory at this time is recorded as the end point of the straight segment.

[0124] The above are merely preferred embodiments of the present invention. The scope of protection of the present invention is not limited to the above embodiments. All technical solutions falling within the scope of the present invention's concept are within the scope of protection of the present invention. It should be noted that for those skilled in the art, any improvements and modifications made without departing from the principles of the present invention should be considered within the scope of protection of the present invention.

Claims

1. A flight trajectory planning method that satisfies the performance constraints of an unmanned helicopter, characterized in that, The specific steps are as follows: Step (1): Use an improved heuristic search algorithm on the grid map to plan the optimal path that satisfies the performance constraints of the unmanned helicopter; Step (2): Remove redundant points in the optimal path using a path optimization algorithm to form a straight flight segment; Step (3): The generated straight-line segments are processed using an improved dynamic window algorithm to generate the final trajectory that meets the performance constraints of the unmanned helicopter.

2. The flight trajectory planning method for satisfying the performance constraints of an unmanned helicopter according to claim 1, characterized in that, The specific steps of step (1) are as follows: Step 101: Perform a neighborhood search based on the performance constraints of the unmanned helicopter; Step 102: For the searched nodes, use a heuristic search algorithm to calculate the value of each node. ； Wherein, the evaluation function F(n) represents the total cost from the starting point to the current node n and then to the target point; g(n) is the dissipation function, representing the actual cost from the starting point to the current node n; h(n) is the heuristic function, representing the estimated cost from the current node n to the target point; altitude1(n) and altitude2(n) are the height evaluation functions, λ h These are dynamic weights, whose purpose is to improve the directional guidance of the algorithm, reduce the number of turns in the path, and improve the efficiency of the algorithm. e1 is the weight of altitude1(n), and e2 is the weight of altitude2(n). The expression for the function altitude1(n) is: ； Where h n h is the height of node n. min For safe flight altitude; The expression for the function altitude2(n) is: ； Where h m This indicates the height of the target point; Dynamic weight λ h The expression is: ； Step 103: Construct the openset table, closeset table, and dist table. The openset table is used to store the total generation value of the nodes that have been searched and the node number. The closeset table is used to store the optimal node, that is, the node with the smallest total generation value F(n) in the openset table. The dist table is used to store the actual generation value g(n) from the starting point to the node. Taking into account both the actual generation value and the total generation value, the optimal path from the starting point to the target point is intelligently found.

3. The flight trajectory planning method for satisfying the performance constraints of an unmanned helicopter according to claim 2, characterized in that, The specific steps in step 101 are as follows: When searching for nodes, the yaw and pitch angle limits of the unmanned helicopter are introduced. The expanded nodes to be searched are the three-layer neighbor nodes of the current node. First, it is necessary to ensure that the grid where the node is located is a free grid, that is, an obstacle-free grid. Secondly, it is necessary to check whether the angle between the line connecting the expanded node and the current node and the current heading is less than or equal to the maximum yaw angle of the unmanned helicopter. It is also necessary to check the pitch angle between the expanded node and the current node, discarding expanded nodes with excessive yaw and pitch angles to ensure that the path does not exceed the performance range of the unmanned helicopter. Finally, path collision detection is required to ensure that the line connecting the expanded node and the current node does not pass through obstacles and that the path does not collide with obstacles.

4. The flight trajectory planning method for satisfying the performance constraints of an unmanned helicopter according to claim 1, characterized in that, The specific steps of step (2) are as follows: Step 201: Set the origin as the first waypoint; Step 202: Using the latest waypoint as a reference point, connect the latest waypoint with the subsequent path nodes in the optimal path in turn; Step 203, when the reference point is... If the connection between the nth path nodes collides with an obstacle or fails to meet the performance constraints of the unmanned helicopter, then the nth path node will be... Each path node serves as the next waypoint; Step 204: Repeat steps 202 to 203 until the target point is reached; Step 205: Output the optimized waypoints and straight segments; Step 206: Execute steps 201-205 twice to output the waypoints and straight segments after secondary optimization.

5. The flight trajectory planning method for satisfying the performance constraints of an unmanned helicopter according to claim 1, characterized in that, The specific process of step (3) is as follows: Step 301: For each straight segment, set a sampling time Δt, start velocity sampling from the starting point of the trajectory, and obtain the velocity and angular velocity range of the unmanned helicopter. Step 302: Based on the velocity sampling space and sampling time, simulate the flight trajectory using the motion model of the unmanned helicopter to obtain several simulated trajectories that meet the performance constraints of the unmanned helicopter. Step 303: Use the evaluation function to evaluate each simulated trajectory to select the optimal trajectory; determine whether the end of the simulated trajectory is the end point of the straight segment or the target point. If not, set the end of the trajectory as the new search starting point and repeat steps 301 to 303; if it is the end point of the straight segment, set the starting point of the next segment as the new search starting point and repeat steps 301 to 303; if it is the target point, the program ends and the final trajectory is output.

6. The flight trajectory planning method for satisfying the performance constraints of an unmanned helicopter according to claim 5, characterized in that, The velocity sampling space in step 302 is specifically as follows: The speed sampling space of the improved dynamic window algorithm is shown below: ； ； ； ； v s1min v s1max This represents the minimum and maximum horizontal speeds of the unmanned helicopter, v. s2min v s2max This represents the minimum and maximum vertical speeds of the unmanned helicopter; ω smin ω smax This represents the minimum and maximum yaw rates of the unmanned helicopter; v s1c v s2c ω sc This represents the horizontal speed, vertical speed, and yaw rate of the unmanned helicopter at the current moment; a s11 a s12 This represents the maximum horizontal deceleration and maximum horizontal acceleration of the unmanned helicopter, a. s21 a s22 This represents the maximum vertical deceleration and maximum vertical acceleration of the unmanned helicopter, α. s1 α s2 This represents the maximum angular deceleration and maximum angular acceleration of the unmanned helicopter, dist(v). s1 , v s2 , ω s () indicates the closest distance to the obstacle; Add constraints between vertical and horizontal velocities to prevent excessive glide angles, specifically: ； Where θa represents the maximum glide angle of the unmanned helicopter.

7. The flight trajectory planning method for satisfying the performance constraints of an unmanned helicopter according to claim 6, characterized in that, The evaluation function in step 303 is as follows: ； in , , , μ, ρ, and τ are all weighting coefficients, and the function his1(v s1 , v s2 , ω s The expression for ) is: ； in, This indicates the corresponding simulated trajectory generated in space at that speed, h s It represents the absolute value of the difference between the altitude at the end of the trajectory and the altitude at the end of the straight segment; function his2(v s1 , v s2 , ω s The expression for ) is: ；； Where H is a constant, h d h represents the height of the end position of the trajectory. min The minimum safe flight altitude for unmanned helicopters, h max This indicates the upper limit of flight altitude; function d(v s1 , v s2 , ω s The expression for ) is: ； Where d s This indicates the distance between the end of the trajectory and the end of the straight flight segment; The simulated trajectory with the largest evaluation function value is denoted as the optimal trajectory. When the h value corresponding to the optimal trajectory is... s With d s When all values ​​are less than the preset threshold, the end of the trajectory at this time is recorded as the end point of the straight segment.

Images