Unmanned aerial vehicle ground-hugging flight path automatic planning method

Abstract

This invention provides an automatic planning method for UAV terrain-following flight paths. Step 1: Construct a planar target trajectory for the target flight mission, wherein the target flight mission includes a start point, an end point, and n key points. Step 2: Construct a two-dimensional mission flight profile between every two key points, and perform terrain-following path search on the two-dimensional mission flight profile to obtain the target two-dimensional trajectory. Step 3: Repeat step 2, obtaining a trajectory segment between every two key points, and then synthesize the trajectory segments to obtain the final UAV terrain-following flight trajectory. By constructing the planar target trajectory for the target flight mission, constructing a two-dimensional mission flight profile between every two key points, and performing terrain-following path search on the two-dimensional mission flight profile to obtain the target two-dimensional trajectory, the final UAV terrain-following flight trajectory satisfies preset trajectory height adjustment constraints, and automatically plans the flight path based on the start node and target node, eliminating the need for manual trajectory point marking and effectively improving the efficiency of flight path planning.

Description

Technical Field

[0001] This invention relates to the field of UAV terrain-following flight path technology, specifically to an automatic planning method for UAV terrain-following flight paths. Background Technology

[0002] With the continuous development and maturation of UAV technology, UAVs are increasingly widely used in agriculture, surveying, security, transportation, and other fields, placing higher demands on the accuracy and efficiency of path planning. UAV path planning typically employs semi-automatic terrain-following flight path planning technology. This technology mainly involves two steps: manually calibrating waypoints and adjusting flight path altitude. First, several waypoints are manually selected on a two-dimensional map to form a two-dimensional flight path that meets the mission requirements. Then, the altitude of each waypoint is calculated according to the mission requirements to determine the final UAV terrain-following flight path. However, due to the widespread application of UAVs and their complex working environment, coupled with the need for obstacle avoidance, the semi-automatic manual waypoint calibration path planning method is inefficient and cannot meet the requirements of flight missions.

[0003] Therefore, the research and application of automatic planning algorithms for UAV terrain-following flight is of great significance. It can not only improve the efficiency and safety of UAV flight missions, but also promote the wider application and development of UAV technology in various fields. Summary of the Invention

[0004] This invention provides an automatic route planning method for UAVs to follow terrain, aiming to solve the problems mentioned in the background.

[0005] This invention is implemented as follows: An automatic route planning method for UAV terrain-following flight includes the following steps:

[0006] Step 1: Construct the planar target trajectory for the target flight mission, wherein the target flight mission includes a starting point, an ending point, and n key points;

[0007] Step 2: Construct a two-dimensional mission flight profile between every two key points, and perform a terrain-following path search on the two-dimensional mission flight profile to obtain the target two-dimensional trajectory;

[0008] Step 3: Repeat step 2. A flight path is obtained between every two key points. Each flight path is combined to obtain the final UAV terrain-following flight path.

[0009] Specifically, the acquisition of the target two-dimensional trajectory in step two includes the following steps:

[0010] Step S1: Obtain the geospatial coordinates of the target area corresponding to the target flight mission, wherein the geospatial coordinates include terrain longitude, terrain latitude and altitude; and convert the geospatial coordinates to coordinate positions in a preset rectangular coordinate system.

[0011] Step S2: Interpolate between two key points to obtain intermediate terrain information; process the interpolated points to find local maxima and regard them as mountain peaks.

[0012] Step S3: Add the ground defense height h to the height of each local maximum point to obtain the simulated location, and adjust the height of the simulated location according to the trajectory constraints to obtain a feasible simulated location.

[0013] Step S4: Repeat steps S2 and S3 to create a feasible simulation location array from the obtained feasible simulation locations;

[0014] Step S5: Add the start point and end point to the simulated location array. Repeat step S4 until the last simulated location is the end point. Then, the points in the feasible simulated location array are the searched simulated location path points; obtain the target two-dimensional track.

[0015] Preferably, in step S1, the geospatial coordinates are converted to coordinate positions in a pre-constructed rectangular coordinate system. The specific method is as follows: starting from the starting point, a route is formed between every two key points. The two endpoints of each route are called the starting node and the ending node, respectively. The coordinate position corresponding to the starting node is selected as a reference point, and the rectangular coordinate system is constructed based on the reference point. The vertical coordinate is the altitude, and the horizontal coordinate is the distance of the point on this route from the starting point.

[0016] Preferably, in step S2, interpolation is performed between two key points to obtain intermediate terrain information. Specifically, the latitude and longitude of the starting point and the ending point are input. Starting from the starting point, a point is inserted every d meters towards the ending point to obtain the altitude information of this point. The distance between the starting point and the ending point is L. Then, a point is inserted between the starting point and the ending point. Interpolation points.

[0017] Preferably, in step S3, the altitude of the simulated location is adjusted according to the trajectory constraints, including preset trajectory constraints such as pitch angle constraints and dive angle constraints. Since the altitude of the key point is the terrain altitude plus the simulated flight altitude by default, a feasible ground defense route cannot be generated between two key points. If any simulated location between the starting point and the ending point satisfies the following relationship with the starting point:

[0018]

[0019] Then a feasible ground defense path cannot be generated, where h i x is the current height. i The horizontal distance from the current point to the starting point. h is the maximum pitch angle of the UAV. start The starting height.

[0020] Preferably, the adjustment strategy in step S3 is to increase the height of the starting point by a certain step size. After the increase, all points between the starting point and the ending point are judged. If there are still points that satisfy formula (1), the height of the starting point is increased until all simulated points no longer satisfy formula (1).

[0021] The beneficial effects of this invention due to the above-mentioned scheme are as follows: By constructing a planar target trajectory for the target flight mission, wherein the target flight mission includes a starting point, an ending point, and n key points; constructing a two-dimensional mission flight profile between every two key points, performing terrain-following path search on the two-dimensional mission flight profile to obtain the target two-dimensional trajectory; obtaining a trajectory segment between every two key points, and synthesizing each trajectory segment to obtain the final UAV terrain-following flight trajectory; thus obtaining a UAV terrain-following flight trajectory that ultimately meets the preset trajectory altitude adjustment constraints, and automatically planning the flight path according to the starting node and the target node, without the need for manual calibration of trajectory points, effectively improving the efficiency of flight path planning. Attached Figure Description

[0022] Figure 1 This is a schematic diagram of the target flight mission of the present invention;

[0023] Figure 2 This is a schematic diagram illustrating the coordinate position transformation of the geospatial coordinates of this invention to the corresponding coordinate position in a rectangular coordinate system;

[0024] Figure 3 This is a schematic diagram of interpolation points and local maxima points in this invention;

[0025] Figure 4 This is a schematic diagram illustrating the location of the simulated location in this invention;

[0026] Figure 5 This is a schematic diagram illustrating the relationship between the pitch angle limitation in this invention and the inability to reach the point from the starting point.

[0027] Figure 6 This is a schematic diagram illustrating the relationship between the dive angle limitation at this point and the inability to descend to the endpoint in this invention;

[0028] Figure 7 This is a schematic diagram illustrating the relationship between simulated location i and simulated location j in this invention;

[0029] Figure 8 This is a schematic diagram illustrating the ascending and descending relationships of points 1, 2, and 3 in this invention.

[0030] Figure 9 This is a schematic diagram of the latitude and longitude of the defense path in this invention. Detailed Implementation

[0031] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the invention.

[0032] An automatic route planning method for UAV terrain-following flight includes the following steps: constructing a planar trajectory of a target flight mission, wherein the target flight mission includes a series of key points, such as... Figure 1 As shown;

[0033] A two-dimensional mission flight profile is constructed between every two key points, and a terrain-following path search is performed on the two-dimensional mission flight profile to obtain the target two-dimensional trajectory.

[0034] Specifically, a two-dimensional mission flight profile is constructed between every two key points, and a terrain-following path search is performed on the two-dimensional mission flight profile to obtain the target two-dimensional trajectory.

[0035] Obtain the geospatial coordinates of the target area corresponding to the target flight mission, wherein the geospatial coordinates include terrain longitude, terrain latitude, and altitude;

[0036] The geospatial coordinates are transformed to their positions in a pre-constructed Cartesian coordinate system; starting from the starting point, a flight path is formed between every two key points. The two endpoints of each flight path are called the starting node and the ending node, respectively. For example... Figure 2 As shown, the coordinates of the starting node are selected as a reference point, and a Cartesian coordinate system is constructed based on the reference point. The vertical axis represents altitude, and the horizontal axis represents the distance of a point on this route from the starting point.

[0037] In step S2, interpolation is performed between two key points to obtain intermediate terrain information. Specifically, the latitude and longitude of the starting point and the ending point are input. Starting from the starting point, a point is inserted every d meters towards the ending point to obtain the altitude information of this point. The distance between the starting point and the ending point is L. Then, points are inserted between the starting point and the ending point. One interpolation point;

[0038] The interpolation points are processed to find local maxima, which are then considered as mountain peaks. Local maxima are found among these interpolation points using a local optimization algorithm, and these local maxima are considered as mountain peaks. Figure 3 As shown, black dots are interpolation points, and red dots are local maxima points.

[0039] like Figure 4 Add the ground defense height h to the height of each local maximum point to obtain the simulated location.

[0040] In this application, the altitude of the simulated location is adjusted according to the trajectory constraints; the preset trajectory constraints include pitch angle constraints and dive angle constraints. Since the altitude of the key point is the terrain altitude plus the simulated flight altitude by default, in some cases, a feasible ground defense route cannot be generated between two key points.

[0041] If any point between the starting point and the ending point (including the ending point) satisfies the following relationship with the starting point:

[0042]

[0043] Then a feasible ground defense path cannot be generated, where h i x is the current height. i The horizontal distance from the current point to the starting point. h is the maximum pitch angle of the UAV. start The starting height.

[0044] like Figure 5 As shown, this situation indicates that the point cannot be reached from the starting point due to pitch angle limitations. To resolve this issue, the starting point height needs to be adjusted.

[0045] The specific adjustment strategy is as follows: adjust the height of the starting point in steps of 50 meters in this embodiment. After the adjustment, judge all points between the starting point and the ending point (including the ending point). If there are still points that satisfy the formula (1), continue to adjust the height of the starting point until all simulated points no longer satisfy the formula (1).

[0046] In this application, if any point between the starting point and the ending point (including the ending point) satisfies the following relationship with the starting point:

[0047]

[0048] Where h i x is the current height. end x is the horizontal distance between the endpoint and the starting point. i The horizontal distance from the current point to the starting point. h is the maximum dive angle of the drone. end The final height.

[0049] Depend on Figure 6 This situation indicates that due to the limited dive angle, the target cannot descend from this point to reach the destination.

[0050] Similarly, to solve this problem, the endpoint height needs to be adjusted by increasing it in steps. In this embodiment, the endpoint height is increased in steps of 50 meters. After the increase, all points between the starting point (including the starting point) and the endpoint are judged. If any point still satisfies formula (2), the endpoint height is increased until all simulated points no longer satisfy formula (2).

[0051] In this application, simulated locations are filtered by calculating the intersection points of rays to obtain simulated paths; two rays are drawn through each simulated location, with the angle of the ray pointing to the left representing the maximum pitch angle of the aircraft. This is called the descent ray at that point. The angle of the ray to the right is the aircraft's maximum dive angle. This is called the ascending ray of that point.

[0052] like Figure 7 As shown, l up Ascending ray, l down For the descending ray, angle θ1 is related to the maximum pitch angle of the UAV. The angles are complementary, and angle θ2 is the maximum pitch angle of the UAV. They are complementary angles;

[0053] Starting from the origin, if the following relationship exists between the simulated location i and the simulated location j:

[0054]

[0055] Where h j Let x be the height of point j. j Let j be the horizontal distance from the starting point, and n be the total number of key points and the defense points between them. If two simulated points satisfy the first equation, they are said to satisfy a descent relationship. Similarly, if they satisfy the second equation, they are said to satisfy an ascending relationship. If two simulated points satisfy both an ascending and a descent relationship, it means that the descending ray of the first point intersects the ascending ray of the second point.

[0056] like Figure 8 As shown, point 1 does not satisfy the descent relationship with the starting point; that is, even descending at the maximum pitch angle from the starting point, it cannot reach point 1. Points 2 and 3 satisfy both the ascent and descent relationships with the starting point. Therefore, the descent ray from the starting point intersects with the ascent rays of points 2 and 3 at two points: intersection point 1 and intersection point 2. The altitude of intersection point 1 is greater than the altitude of intersection point 2. The drone should begin its ascent earlier to ensure it can reach point 3. If the drone reaches point 2 first and then begins its ascent, the angle between the line connecting point 3 and point 2 will be greater than the maximum pitch angle, preventing the drone from reaching point 3.

[0057] The starting point is directly added to the feasible simulation point array. Starting from the starting point, the intersection of the descending ray of the starting point with the ascending rays of all simulation points that satisfy both ascending and descending relationships with the starting point is calculated. A set of ray intersections is obtained; the simulation point corresponding to the intersection with the highest height is found and added to the feasible simulation point array. Then, starting from this point, the same calculation is performed, finding the intersection of the descending ray of this point with the ascending rays of all simulation points that satisfy both ascending and descending relationships with this point. The simulation point corresponding to the highest height is found and becomes the next feasible simulation point; this point is also added to the feasible simulation point array. This process is repeated until a feasible simulation point becomes the endpoint. The points in the feasible simulation point array are then the searched simulation path points.

[0058] The defensive paths obtained from the search are as follows: Figure 9 As shown; after obtaining the path points for the simulated terrain, the coordinates of these points are transformed from the rectangular coordinate system back to the geographic coordinate system; the coordinate transformation converts the distance deviation into actual latitude and longitude, and the transformation formula is as follows:

[0059]

[0060] tmp=(lat1+lat2) / 114.5916

[0061]

[0062] Among them, R e =6378165 is the Earth's radius, R lat 1 is the radius corresponding to the current dimension, lon1 is the longitude of the starting key point, lat1 is the latitude of the starting key point, lon2 is the longitude of the ending key point, lat2 is the latitude of the ending key point, l is the distance from the path point to the starting point, and ψ is the angle between the two key points.

[0063] A flight path is obtained between each pair of key points. Each team combines the flight paths to obtain the final UAV ground-following flight path.

[0064] The above description of the embodiments is intended to enable those skilled in the art to understand and use the present invention. It will be apparent to those skilled in the art that various modifications can be made to these embodiments, and the general principles described herein can be applied to other embodiments without inventive effort. Therefore, the present invention is not limited to the above embodiments. Improvements and modifications made by those skilled in the art based on the principles of the present invention without departing from the scope of the invention should be within the protection scope of the present invention. The above descriptions are merely preferred embodiments of the present invention and are not intended to limit the present invention. Any modifications, equivalent substitutions, and improvements made within the spirit and principles of the present invention should be included within the protection scope of the present invention.

Claims

1. An automatic route planning method for UAV terrain-following flight, characterized in that, Includes the following steps: Step 1: Construct the planar target trajectory for the target flight mission, wherein the target flight mission includes a starting point, an ending point, and n key points; Step 2: Construct a two-dimensional mission flight profile between every two key points, and perform a terrain-following path search on the two-dimensional mission flight profile to obtain the target two-dimensional trajectory; Step 3: Repeat step 2. A flight path is obtained between every two key points. Each flight path is combined to obtain the final UAV terrain-following flight path. Specifically, the acquisition of the target two-dimensional trajectory in step two includes the following steps: Step S1: Obtain the geospatial coordinates of the target area corresponding to the target flight mission, wherein the geospatial coordinates include terrain longitude, terrain latitude and altitude; and convert the geospatial coordinates to the coordinate position in a preset rectangular coordinate system; Step S2: Interpolate between two key points to obtain intermediate terrain information; process the interpolated points to find local maxima and regard them as mountain peaks. Step S3: Add the ground defense height to the height of each local maximum point. The simulated location is obtained, and its altitude is adjusted according to the trajectory constraints to obtain a feasible simulated location. Step S4: Repeat steps S2 and S3 to create a feasible simulation location array from the obtained feasible simulation locations; Step S5: Add the start point and end point to the simulated location array. Repeat step S4 until the last simulated location is the end point. Then, the points in the feasible simulated location array are the searched simulated location path points; obtain the target two-dimensional track. In step S1, the geospatial coordinates are converted to coordinate positions in a pre-constructed rectangular coordinate system. The specific method is as follows: starting from the starting point, a route is formed between every two key points. The two endpoints of each route are called the starting node and the ending node, respectively. The coordinate position corresponding to the starting node is selected as a reference point, and the rectangular coordinate system is constructed based on the reference point. The vertical coordinate is the altitude, and the horizontal coordinate is the distance of the point on this route from the starting point. In step S2, interpolation is performed between two key points to obtain intermediate terrain information. Specifically, the latitude and longitude of the starting point and the ending point are input, and starting from the starting point, interpolation is performed towards the ending point at intervals of... Insert a point using meters to obtain the altitude information of that point. The distance between the starting point and the ending point is... Then insert between the start and end points. Interpolation points.

2. The automatic route planning method for UAV terrain-following flight as described in claim 1, characterized in that: In step S3, the altitude of the simulated location is adjusted according to the trajectory constraints. This includes preset trajectory constraints such as pitch angle constraints and dive angle constraints. Since the altitude of the key point is the terrain altitude plus the simulated flight altitude by default, a feasible ground defense route cannot be generated between two key points. If any simulated location between the starting point and the ending point satisfies the following relationship with the starting point: Official (1); Then a feasible defense path cannot be generated, among which The current height. The horizontal distance from the current point to the starting point. This is the maximum pitch angle of the drone. The starting height.

3. The automatic route planning method for UAV terrain-following flight as described in claim 2, characterized in that: The adjustment strategy in step S3 is to increase the height of the starting point by a certain step size. After the increase, all points between the starting point and the ending point are judged. If there are still points that satisfy formula (1), the height of the starting point is increased until all simulated points no longer satisfy formula (1).

Images