A method for UAV route planning and flight control in complex terrain

Abstract

The present invention discloses a method for route planning and flight control of a UAV in complex terrain. When any path is determined, the route altitude is designed, and terrain characteristics are filtered and smoothed in the route planning stage before flight. A route is given that can reflect the terrain trend characteristics while ignoring local unfavorable terrain details. Finally, a safe flight is achieved through route tracking.

Description

Technical Field

[0001] The present invention belongs to the field of unmanned aerial vehicle (UAV) route planning, and in particular, relates to a UAV route planning and flight control method under complex terrain. Background Art

[0002] Drone route planning involves assigning a reasonable flight path to a drone, enabling it to efficiently complete a specific mission. This involves considering factors such as the drone's takeoff point, target point, obstacle avoidance, and energy consumption to ensure flight safety and efficiency. Drone route planning is a complex and varied task, requiring comprehensive consideration of multiple factors to ensure successful and safe flight. Advanced flight control systems and route planning algorithms can help optimize drone route planning.

[0003] Mountainous areas (especially plateau mountainous areas) have extremely undulating terrain, with elevation differences ranging from a few meters to hundreds of meters over short distances. When drones perform ultra-low-altitude missions over such complex terrain, due to their high forward flight speed and delayed altitude tracking response, they may not have enough time and space to adjust their flight attitude and altitude in a timely manner, posing a huge safety challenge to route planning and flight control.

[0004] When a UAV performs an ultra-low-altitude flight mission, a closed-loop feedback control method based on altitude error is generally used in the longitudinal altitude control channel. The controller formula is as follows:

[0005] Θ = f(Hset-H);

[0006] Where Θ is the output command of the longitudinal height channel, Hset is the height setting value, and H is the current height feedback measurement value. The height here can be either absolute or relative.

[0007] Based on the above formula, most of the current technical solutions for ultra-low altitude flight adopt a real-time feedback control solution for filtering the feedback measurement value H: in terms of path, through digital map path planning or field surveys, areas with flat surfaces or relatively slow terrain changes are selected for flight, such as the sea, plains, Gobi Desert or mountain rivers, to actively avoid the problem of drastic changes in terrain causing the drone to pitch and sink sharply, thereby affecting longitudinal control stability and flight safety; in terms of altitude, one or a limited number of expected relative altitude values ​​are pre-set as Hset, and a ranging device such as a radio altimeter is used to obtain the original measurement value H of the relative ground altitude in real time before filtering, and then the measurement value is smoothed and denoised by a low-pass filtering method. Finally, the filtered measurement value H is fed back to the altitude closed-loop controller to achieve terrain following flight.

[0008] The existing real-time feedback control scheme for filtering the feedback measurement value H has the following problems:

[0009] 1. There are safety risks. When filtering terrain data, the characteristics of the low-pass filter will filter out local peaks in the data, causing amplitude attenuation. Although this method reduces the impact of altitude fluctuations on longitudinal control stability, it ignores the terrain details such as peaks, cliffs, and other sharp protrusions commonly found in mountainous areas. It still cannot solve the problem of flight safety when the terrain is extremely undulating.

[0010] 2. The terrain is quite limited. The smoothing effect of the filter is limited. It is good for smoothing terrain undulations below the meter level, but it is difficult to smooth larger-scale undulations. The filter still retains the large-scale trend characteristics of the terrain undulations. When the rate of change of the terrain undulation exceeds the dynamic response capability of the altitude controller, it is easy to cause longitudinal control oscillation or fail to raise the altitude in time due to control lag, resulting in collision with the mountain. Therefore, it can only be adapted to the sea, plains, Gobi Desert or mountain rivers where the terrain changes are relatively gentle;

[0011] 3. Increased path costs. Using optimal path planning methods or manual methods to search for smooth, continuous paths on a map means that the selection area is limited and the path is not the shortest. This will lead to longer flight times and increased fuel consumption. It is also not suitable for flight missions with a specific path. Summary of the Invention

[0012] In view of the above shortcomings, the present invention provides a method for UAV route planning and flight control in complex terrain, including UAV route planning. The specific steps of the route planning are as follows:

[0013] S1. Determine the flight path;

[0014] S2, obtaining terrain height data;

[0015] S3. Plan the route, including:

[0016] S3-1. Find the appropriate envelope;

[0017] S3-2, find the key points of the path;

[0018] S3-3, correct the altitude of the flight path;

[0019] S3-4. Calculate the route altitude;

[0020] S3-5, generate a complete route sequence;

[0021] S3-6, uploading the complete route generated in step S3-5 to the simulation system for simulation verification;

[0022] The invention also includes a flight control method for a UAV according to route planning, wherein the specific steps of the flight control method are as follows:

[0023] P1. Use the altitude output by the onboard integrated navigation system to participate in the absolute altitude closed-loop control;

[0024] P2. In the flight control logic, the relative ground height measured by the wireless ranging device is set as an early warning input for low altitude situations. When the measured relative altitude value falls below a certain threshold, the drone immediately increases the pitch angle command to climb and quickly increase the altitude.

[0025] P3. Based on the route sequence obtained in S3-5 of the route planning step, the input of the altitude controller is set as a time-varying ramp signal, the real-time altitude setting value is calculated, and the value is substituted into the altitude controller for altitude tracking to achieve altitude ramp flight.

[0026] Furthermore, the flight path in step S1 is inputted through an external task, and the longitude and latitude coordinates of key points on the flight path under complex terrain are obtained, and the key points are connected in pairs in sequence to form a flight path.

[0027] Furthermore, the terrain height data in step S2 is obtained through the coordinates of each point of the flight path determined in step S1.

[0028] Furthermore, the terrain height data in step S2 is obtained by obtaining digital elevation tiles with a resolution of not less than 3" including all points on the flight path from an open source geographic information database, and then calculating the terrain height on the path through linear interpolation. Alternatively, the terrain height can be obtained by flying a drone at a safe altitude along the flight path and measuring the relative ground height below using a wireless ranging device carried by the drone. Finally, the terrain height below the flight path is obtained by subtracting the relative height from the flight altitude.

[0029] Furthermore, the envelope line in step S3-1 is drawn by plotting a terrain height curve based on the terrain height data obtained in step S2, and then obtained by using the envelope function of MATLAB based on the terrain height curve.

[0030] Furthermore, the path key points in step S3-2 are obtained by finding an abnormal point sequence set whose envelope height value is lower than the original terrain height, and then screening out the expected path key point sequence set.

[0031] Furthermore, correcting the height of the flight path in step S3-3 includes using the interp1 function in MATLAB to densely discretize the key point sequence set obtained in step S3-2 in the form of linear interpolation to connect the lines between two adjacent points in the path key point sequence set, and correcting the terrain height value in the path key point sequence set.

[0032] Furthermore, the route altitude in step S3-4 is calculated based on the flight path point sequence set generated from the corrected path key point sequence set in step S3-3.

[0033] Furthermore, the complete route sequence in step S3-5 is generated by adding waypoint information of the takeoff segment, the recovery segment, and the transition segment to the flight path point sequence set generated in step S3-4.

[0034] Compared with the prior art, the present invention has the following beneficial effects:

[0035] 1. The flight path can be selected arbitrarily, that is, there is no need to search for the optimal path on the map in advance. The shortest path or any specified path can be selected. After obtaining the terrain height data on the path, the route is planned according to the method of the present invention and it can be used for flight;

[0036] 2. To ensure flight safety while taking into account ultra-low-altitude flight, the envelope method is used to select and trade off terrain features. According to the flight mission requirements, the envelope parameter np can be adjusted to smooth the terrain features, ignore unfavorable local details, and retain the general terrain trend characteristics. In this way, the requirements of ultra-low-altitude terrain following can be met while avoiding the threat to flight stability and safety caused by the drastic fluctuations of terrain following.

[0037] 3. Reduce dependence on wireless ranging equipment. Pre-planned routes determine the desired location information required for drone flight, such as longitude, latitude, and altitude. Altitude is the absolute altitude, which, combined with the measured altitude output by the drone's high-precision integrated navigation system, forms a closed-loop control of the altitude channel. There is no need to worry about the relative altitude to the mountain or the ground during flight. The relative altitude data measured by the onboard wireless ranging equipment is only used for low altitude warning. BRIEF DESCRIPTION OF THE DRAWINGS

[0038] Figure 1 This is a schematic diagram of route planning in step S3 of the present invention.

[0039] Figure 2 Schematic diagram of two forms of height tracking in step P3 of the present invention. DETAILED DESCRIPTION

[0040] To facilitate understanding of the present invention, the apparatus of the present invention will be described more fully below with reference to the accompanying drawings. The drawings illustrate embodiments of the apparatus. However, the apparatus can be implemented in many different forms and is not limited to the embodiments described herein. Rather, these embodiments are provided to provide a more thorough and comprehensive understanding of the present invention.

[0041] In the description of the present invention, it should be noted that, unless otherwise expressly specified or limited, the terms "installed," "connected," and "disposed" should be understood in a broad sense. For example, they may refer to fixed connection or disposition, detachable connection or disposition, or integral connection or disposition. Those skilled in the art will understand the specific meanings of the above terms in the present invention based on specific circumstances.

[0042] Example

[0043] like Figure 1-2 As shown, this embodiment provides a method for UAV route planning and flight control in complex terrain, including UAV route planning. The specific steps of the route planning are as follows:

[0044] S1. According to the external task input, obtain the latitude and longitude coordinates of the key points on the flight path under complex terrain, connect them in sequence to form a path, and define the path sequence as R (hereinafter referred to as path R), R = {P i |(longitude, latitude), i=1,2,3…,n}, where n is the number of known key points, and a point in it is denoted as P Ri .

[0045] S2. Based on the coordinates of each point on the path R in step S1, obtain the terrain height data below the path. There are two ways to obtain the data:

[0046] First, obtain digital elevation tiles with a resolution of at least 3 inches, including all points on path R, from an open-source geographic information database. Then, use linear interpolation to calculate the terrain height along the path.

[0047] Second, a drone is launched to fly at a safe altitude along path R, while using the wireless ranging equipment it carries to measure the relative height to the ground below. Finally, the terrain height below the flight path is obtained by subtracting the relative height from the flight altitude.

[0048] It should be noted that the terrain heights obtained above are all altitudes, which are stored in order in the terrain sequence. The terrain sequence is defined as G = {P i |(longitude, latitude, distance, terrain height), i = 1, 2, 3…, n}, n is the number of terrain sampling points, distance is the cumulative flight distance on path R starting from P1, and a point within it is denoted as P Gi .

[0049] S3. Plan the route, including:

[0050] S3-1. Use the distance in the terrain sequence G as the horizontal coordinate and the terrain height as the vertical coordinate to draw the terrain height curve. Use the envelope function of MATLAB to quickly calculate the upper envelope of the terrain height curve, which is recorded as S = {P i|(longitude, latitude, distance, envelope height), i=1,2,3…,n};

[0051] In the above formula, n is the number of envelope sampling points, and a point inside it is denoted as P Si The function is used as [yupper,ylower] = envelope(x,np,'peak'), where yupper and yellower are the upper and lower envelopes of the output, respectively (this invention only needs to care about the upper envelope, yupper; all envelopes hereafter refer to the upper envelope), x is the original terrain height sequence in the terrain G, np is the adjustable discrete sampling interval, and 'peak' specifies the envelope mode.

[0052] S3-2. First, find out the abnormal point sequence set U1 (hereinafter referred to as U1) whose envelope height value is lower than the original terrain height.

[0053] In order to make the envelope smooth and continuous, a discrete point P on it Si The height may be lower than the original terrain P Gi To ensure the absolute safety of the flight route, use the formula:

[0054] Height difference ΔH=P Si (Envelope height)-P Gi (terrain height);

[0055] Determine each discrete point P on the envelope Si Is it lower than the mountain surface? If the height difference ΔH≤0, it means that this point is lower than the mountain surface and is recorded in the abnormal point sequence set U1, U1={P i |(longitude, latitude, distance, height of the envelope abnormal point), i=1,2,3…,n}, n is the number of abnormal points, and a point within it is denoted as P U1i ;

[0056] Then, a key point sequence set U2 (hereinafter referred to as U2) of the expected path is screened out from the abnormal point sequence set U1.

[0057] Each turning point on the route path should be a key point of the path. The first and last two abnormal points of the abnormal point sequence set U1 are defaulted as key points of the path. For each of the remaining abnormal points, the distance between it and the adjacent points before and after is calculated. If there is a distance greater than the threshold ΔL, it is defined as the key point of the expected path and recorded in the sequence set U2. U2 = {P i |(longitude, latitude, distance, altitude of key points on the path), i=1,2,3…,n}, where n is the number of key points on the path, and a point within it is denoted as P U2i It should be noted that the threshold ΔL may have different values ​​for different aircraft or at different flight speeds.

[0058] At this point, the longitude and latitude of the waypoints of the designed route are the n discrete points P in U2 U2i The corresponding latitude, longitude, and altitude values ​​of the waypoint will be determined in subsequent steps.

[0059] S3-3. Correcting the altitude of the flight path includes:

[0060] S3-3-1. Linear interpolation is used to connect two adjacent points in the dense discrete path key point sequence set U2:

[0061] You can use the interp1 function in MATLAB, using the method vq = interp1(x,v,xq,'linear'), where vq is the dense discrete height sequence after interpolation, x and v are the distance and terrain height sequences in U2 respectively, and xq is the expected dense discrete distance sequence;

[0062] S3-3-2. Correct the terrain height value in the path key point sequence set U2:

[0063] The line between two adjacent points in U2 may pass through the mountain. In order to make all line segments higher than the mountain surface and ensure flight safety, the height value should be corrected. U2i and P U2i+1 For a segment, i = 1, 2, ..., the dense discrete lines are compared segment by segment to determine whether there is a point below the terrain height of the mountain surface. If so, the two path key points P of the corresponding segment are connected. U2i and P U2i+1 The height is corrected to the highest point in the segment max{H Xi ,H Xi+1 ,H max}, H Xi P in U2 U2i The height of the point, H Xi+1 P in U2 U2i+1 The height of the point, H max P U2i and U2i+1 The true terrain height of the highest point on the mountain within the segment. The corrected segment is represented as a level flight segment with constant altitude, and the altitude of each position within the segment is not lower than the terrain height of the mountain surface.

[0064] S3-4. The altitude curve represented by U2 after the correction in the above steps can be regarded as a terrain-simulating broken line segment that envelops the general trend of the mountain. It is defined as the mountain surface reference layer, that is, the relative ground height is 0. In this way, the altitude setting of the designed route can be obtained by superimposing the arbitrary relative ground flight altitude requirement input by the mission on the mountain surface reference altitude layer. The generated flight path point sequence set U3 = {P i|(longitude, latitude, mountain surface reference altitude layer + required relative altitude), i = 1, 2, 3…, n}, n is the number of points in the flight path, and a point in it is denoted as P U3i .

[0065] S3-5, based on U3, add the waypoint information of the takeoff segment, recovery segment and transition segment, and finally obtain the complete route sequence W = {P i |(longitude, latitude, altitude), i = 1, 2, 3, ..., n}, where n is the number of waypoints. This route sequence can be uploaded to the drone for route tracking.

[0066] In addition, the absolute safety of the flight altitude can be ensured by flight simulation and manual verification.

[0067] S3-6. Upload the complete route planned in the above steps to the simulation system for simulation verification. Check the relative altitude data in the simulation results to ensure altitude safety. If the altitude is too low, manually adjust the altitude setting value of the route segment and simulate again to ensure altitude safety of the final route.

[0068] The invention also includes a flight control method for a UAV according to route planning, wherein the specific steps of the flight control method are as follows:

[0069] P1. In terms of flight control strategy, the drone uses the altitude output by the onboard integrated navigation system to participate in absolute altitude closed-loop control, and no longer uses the relative ground altitude measured by wireless ranging equipment to participate in relative altitude closed-loop control;

[0070] P2. In the flight control logic, the relative ground height measured by the wireless ranging device is set as an early warning input for low altitude situations. When the measured relative altitude value falls below a certain threshold, the drone immediately increases the pitch angle command to climb and quickly increase the altitude. It should be noted that the drone determines that the altitude is too low through the flight control program logic, then generates a larger pitch angle command, which is then substituted into the pitch angle controller for closed-loop control to achieve automatic altitude increase.

[0071] P3. Since the altitude of the route sequence designed in the previous steps is a number of discrete points, two adjacent points are connected to form multiple broken lines. That is, the altitude setting value on a certain route may be either a fixed value or a variable with a fixed slope. Therefore, the input of the altitude controller should be a time-varying ramp signal. However, during the route flight, the altitude setting value of the current segment is generally the setting value of the target waypoint. The drone will converge to this set altitude in a short time and cannot achieve altitude slope flight, as shown in the attached figure. Figure 2 The conventional height tracking curve of , therefore the present invention adopts the following method:

[0072] According to the distance and height difference between the two waypoints before and after the current segment, the change rate k of the altitude setting value within this segment is obtained. H , as follows:

[0073] k H =(H 目标航点 -H 上一航点 ) / ΔL;

[0074] In the above formula, H 目标航点 Set the altitude of the target waypoint, H 上一航点 is the altitude setting value of the previous waypoint, and ΔL is the distance between the two waypoints;

[0075] Afterwards, during flight, the distance L between the real-time position and the target waypoint is calculated using the formula:

[0076] H set =H 上一航点 +k H *L;

[0077] The real-time altitude setting value is calculated and substituted into the altitude controller for altitude tracking. Figure 2 Height slope tracking curve.

[0078] It should be noted that the structure described in the present invention can be implemented in a variety of different forms and is not limited to the described embodiments. Any equivalent transformations made by ordinary technicians in this field using the contents of the present invention description and drawings, or directly or indirectly applied to other related technical fields, such as the loading and unloading of other items, are included in the scope of protection of the present invention.

Claims

1. A method for UAV route planning and flight control in complex terrain, including UAV route planning, wherein the specific steps of the route planning are as follows: S1. Obtain the latitude and longitude coordinates of key points on the flight path in complex terrain through external task input, and connect them in sequence to determine the flight path; S2. Obtaining terrain height data using the latitude and longitude coordinates of key points on the flight path determined in step S1; S3. Plan the route, including: S3-1. Draw a terrain height curve based on the terrain height data obtained in step S2, and then use MATLAB's envelope function to calculate a suitable envelope line to avoid severe fluctuations during ultra-low altitude flight based on the terrain height curve; S3-2, by finding the abnormal point sequence set whose envelope height value is lower than the original terrain height, and then filtering out the expected path key point sequence set to obtain the path key points; S3-3, correcting the height of the flight path according to the key points of the path obtained in step S3-2; S3-4, calculating the flight path altitude based on the flight path point sequence set generated from the corrected path key point sequence set in step S3-3; S3-5. Generate a complete route sequence by adding waypoint information of the takeoff segment, recovery segment, and transition segment to the flight path point sequence set generated in step S3-4; S3-6, uploading the complete route generated in step S3-5 to the simulation system for simulation verification; The invention also includes a flight control method for a UAV according to route planning, wherein the specific steps of the flight control method are as follows: P1. Use the altitude output by the onboard integrated navigation system to participate in the absolute altitude closed-loop control; P2. In the flight control logic, the relative ground height measured by the wireless ranging device is set as an early warning input for low altitude situations. When the measured relative altitude value falls below a certain threshold, the drone immediately increases the pitch angle command to climb and quickly increase the altitude. P3. Based on the route sequence obtained in S3-5 of the route planning step, the input of the altitude controller is set as a time-varying ramp signal, the real-time altitude setting value is calculated, and the value is substituted into the altitude controller for altitude tracking to achieve altitude ramp flight.

2. The method for route planning and flight control of a UAV in complex terrain according to claim 1, characterized in that: The terrain height data in step S2 is obtained by obtaining digital elevation tiles with a resolution of not less than 3" including all points on the flight path from an open source geographic information database, and then calculating the terrain height on the path through linear interpolation. Alternatively, the terrain height can be obtained by flying a drone at a safe altitude along the flight path and measuring the relative ground height below using the carried wireless ranging device. Finally, the terrain height below the flight path is obtained by subtracting the relative height from the flight altitude.

3. The method for route planning and flight control of a UAV in complex terrain according to claim 1, characterized in that: Correcting the height of the flight path in step S3-3 includes using the interp1 function in MATLAB to densely discretize the key point sequence set obtained in step S3-2 in the form of linear interpolation to connect the lines between two adjacent points in the path key point sequence set, and correcting the terrain height value in the path key point sequence set.

4. The method for route planning and flight control of a UAV in complex terrain according to claim 1, characterized in that: The complete route sequence in step S3-5 is generated by adding the waypoint information of the takeoff segment, the recovery segment and the transition segment to the flight path point sequence set generated in step S3-4.

Images