A method and system for predicting smooth UAV trajectories based on terrain height.

Abstract

This invention provides a method and system for smoothing and predicting UAV trajectory based on terrain height. The method includes: extracting the original terrain height corresponding to point cloud data and inserting it into a double-ended queue; calculating the terrain height deviation and determining whether the absolute value of the terrain height deviation is less than or equal to a defined height deviation threshold; if so, estimating the terrain height of the current frame and inserting it into the double-ended queue; if not, performing terrain trend filtering on the original terrain height of the current frame and inserting it into the double-ended queue, calculating the estimated terrain height of the current frame and inserting it into the double-ended queue; calculating the current flight altitude of the UAV based on the estimated terrain height of the current frame and inserting it into the double-ended queue; performing spline smoothing on the queue to obtain a smoothed elevation estimate; and calculating the current predicted altitude of the UAV. This invention solves the problem of large variations in UAV flight altitude and insufficient smoothness of flight trajectories in existing technologies.

Description

Technical Field

[0001] This invention relates to the field of unmanned aerial vehicle (UAV) technology, and in particular to a method and system for predicting UAV trajectory smoothing based on terrain altitude. Background Technology

[0002] Terrain-following flight refers to a flight method in which a drone, during flight operations, sets a fixed altitude relative to known three-dimensional terrain, maintaining a constant elevation difference between the drone and the target terrain. By employing terrain-following flight, drones can adapt to terrain at different altitudes and automatically generate variable-altitude flight paths based on the terrain of the survey area, thereby maintaining consistent ground resolution and obtaining better data.

[0003] Current drones primarily acquire ground information through camera photography. However, this method suffers from drawbacks, including the need for real-time 3D reconstruction, low accuracy, and lack of penetration through complex objects, resulting in ineffective or inaccurate ground data and significant computational fluctuations. To address these issues, existing technologies utilize lidar for ground data acquisition. While lidar-based ground data acquisition is now widespread in the drone industry, it still relies heavily on manual pilot control. Drone flight path planning either utilizes low-resolution satellite terrain data or relies solely on 2D planar positioning, leading to poor accuracy in terrain altitude data acquisition and causing drones to consistently fly at the same altitude.

[0004] Even with high-precision terrain height data, when a drone sets its flight altitude based on the terrain for terrain-following flight, inconsistencies exist between consecutive frames and between the terrain heights in front of and behind the drone. Simply averaging adjacent data is insufficient to effectively smooth terrain fluctuations, especially when multiple consecutive estimation errors lead to significant terrain variations. This necessitates rapid ascents and descents during drone flight, resulting in large altitude changes and an uneven flight path. Consequently, drone rework and re-flying are required, increasing the drone's operational cycle and impacting project progress. Summary of the Invention

[0005] This invention provides a terrain-based altitude-based drone trajectory smoothing prediction scheme, which aims to solve the problem of large changes in flight altitude and insufficient smoothness of flight trajectory during the terrain-following flight of drones in the prior art.

[0006] To address the aforementioned problems, according to a first aspect of the present invention, a method for predicting the trajectory smoothing of unmanned aerial vehicles (UAVs) based on terrain height is proposed, comprising:

[0007] Use a drone-borne lidar to acquire multiple frames of point cloud data, and extract the original terrain height corresponding to each frame of point cloud data from the multiple frames of point cloud data.

[0008] Construct a double-ended queue and insert the original terrain height of each frame into the double-ended queue in chronological order.

[0009] The terrain height deviation is calculated based on the estimated terrain height of the previous frame and the original terrain height of the current frame. It is then determined whether the absolute value of the terrain height deviation in the double queue is less than or equal to the limit height deviation threshold.

[0010] If the absolute value of the terrain height deviation is less than or equal to the limit height deviation threshold, the estimated terrain height of the current frame is calculated based on the adjacent height constraint relationship between the estimated terrain height of the previous frame and the original terrain height of the current frame in the double-ended queue, and the estimated terrain height of the current frame is inserted into the double-ended queue.

[0011] If the absolute value of the terrain height deviation is greater than the limit height threshold, then the original terrain height of the current frame is filtered by terrain trend, the filtered estimated terrain height of the current frame is inserted into the double-ended queue, the terrain trajectory matrix of the double-ended queue is constructed, the estimated terrain height of the current frame is calculated based on the terrain trajectory matrix and inserted into the double-ended queue.

[0012] The terrain height is estimated based on the current frame, the current flight altitude of the drone is calculated, and the result is inserted into the double-ended queue.

[0013] The terrain smoothing parameters are used to perform spline smoothing on the data in the double-ended queue to obtain smoothed elevation estimates.

[0014] The current predicted altitude of the drone is calculated based on the maximum permissible slope of the current scene and the smoothed elevation estimate.

[0015] Preferably, the above-mentioned UAV trajectory smoothing prediction method further includes, after the step of constructing a double-ended queue and sequentially inserting the original terrain height of each frame into the double-ended queue in chronological order:

[0016] Determine whether the data length of the double-ended queue is less than or equal to the predetermined minimum data length;

[0017] If the data length of the dequeue is less than or equal to the predetermined minimum data length, the estimated terrain height of the current frame is calculated based on the adjacent height constraint relationship between the estimated terrain height of the previous frame and the original terrain height of the current frame in the dequeue.

[0018] If the data length of the double-ended queue is greater than the predetermined minimum data length, then the step of calculating the terrain height deviation based on the terrain height estimated in the previous frame and the original terrain height in the current frame is executed.

[0019] Preferably, in the above trajectory smoothing prediction method, the step of calculating the estimated terrain height of the current frame based on the adjacency height constraint relationship between the estimated terrain height of the previous frame and the original terrain height of the current frame in the double-ended queue includes:

[0020] Using the terrain height estimated from the previous frame, the original terrain height in the current frame, and the maximum adjacent height deviation, the terrain height is calculated according to the formula for estimating terrain height in the current frame:

[0021] H1 = max{Hraw, Hmean - Hdiff}

[0022] H2 = min{H1, Hmean + Hdiff}

[0023] He = (H2 + Hmean) / 2

[0024] The estimated terrain height for the current frame is calculated, where Hraw is the original terrain height for the current frame, Hmean is the estimated terrain height for the previous frame, Hdiff is the maximum adjacent height deviation, H1 is the first estimated terrain height, H2 is the second estimated terrain height, and He is the estimated terrain height for the current frame.

[0025] The estimated terrain height for the current frame is inserted into a double-ended queue.

[0026] Preferably, in the above trajectory smoothing prediction method, the step of calculating the terrain height deviation based on the estimated terrain height of the previous frame and the original terrain height of the current frame includes:

[0027] Using the terrain height estimated from the previous frame and the original terrain height in the current frame, the terrain height deviation is calculated according to the terrain height deviation calculation formula: dh=Hmean–Hraw, where dh is the terrain height deviation, Hmean is the terrain height estimated from the previous frame, and Hraw is the original terrain height in the current frame.

[0028] Calculate the variance of terrain height for a predetermined number of frames in the double-ended queue;

[0029] The variance of terrain height is used to calculate the height deviation threshold using a predetermined number of frames, based on the following formula:

[0030] Hdelta1=2*S

[0031] Hdelta2=3*S

[0032] The first and second limited height deviation thresholds are calculated respectively, where Hdelta1 is the first limited height deviation threshold, Hdelta2 is the second limited height deviation threshold, and S is the variance of the terrain height of a predetermined number of frames.

[0033] Preferably, in the above trajectory smoothing prediction method, the step of performing terrain trend filtering on the original terrain height of the current frame and inserting the filtered estimated terrain height of the current frame into the double-ended queue includes:

[0034] Using the terrain height estimated from the previous frame and the second defined height deviation threshold, a filtering formula is applied based on the terrain trend:

[0035] Hf = Hmean – Hdelta2 (dh≥0)

[0036] Hf = Hmean + Hdelta2 (dh < 0)

[0037] The filtered estimated terrain height of the current frame is calculated; where Hf is the filtered estimated terrain height of the current frame, Hmean is the estimated terrain height of the previous frame, and Hdelta2 is the second limiting height deviation threshold.

[0038] The filtered terrain height estimate for the current frame is inserted into a double-ended queue.

[0039] Preferably, in the above trajectory smoothing prediction method, the steps of constructing a terrain trajectory matrix for a double-ended queue, calculating the estimated terrain height for the current frame based on the terrain trajectory matrix, and inserting it into the double-ended queue include:

[0040] Use a double-ended queue to construct the terrain trajectory matrix;

[0041] The terrain trajectory matrix is ​​decomposed into singular values ​​to obtain the left matrix, the right matrix, and the singular values.

[0042] Using the left matrix, right matrix, and singular values, calculate according to the matrix formula:

[0043]

[0044] Construct a terrain height transpose matrix of the same size as the terrain trajectory matrix, where E0 is the largest singular value and U0 is the first column of the left matrix U. The first column of the right matrix V is transposed;

[0045] Convert the terrain height transpose matrix into a terrain height sequence, and select the last terrain height element value in the terrain height sequence as the estimated terrain height for the current frame;

[0046] The estimated terrain height for the current frame is inserted into a double-ended queue.

[0047] Preferably, in the above trajectory smoothing prediction method, the step of estimating the terrain height based on the current frame, calculating the current flight altitude of the UAV, and inserting it into the double-ended queue includes:

[0048] Extract the maximum height of ground features from point cloud data;

[0049] The minimum flight altitude of the UAV is obtained by calculating the sum of the maximum ground feature height and the flight clearance height.

[0050] The sum of the estimated terrain altitude and the set flight altitude in the current frame is calculated to obtain the drone's relative terrain flight altitude;

[0051] The maximum value between the drone's minimum flight altitude and its flight altitude relative to the terrain is selected as the drone's current flight altitude;

[0052] Insert the drone's current flight altitude value into the double-ended queue.

[0053] Preferably, in the above trajectory smoothing prediction method, the step of using terrain smoothing parameters to perform spline smoothing on the data in the double-ended queue to obtain smoothed elevation estimates includes:

[0054] Construct a cubic polynomial function of the UAV's current flight altitude;

[0055] Construct a residual sum of squares minimization function based on the relationship between the cubic polynomial function and the terrain smoothing parameters;

[0056] The terrain smoothing parameters are obtained by solving the cubic polynomial function using the least squares method.

[0057] Construct the transpose matrix of the terrain smoothing parameters and the elevation estimate, solve the transpose matrix to obtain the elevation estimate;

[0058] The elevation estimates are processed by moving average to obtain smoothed elevation estimates.

[0059] Preferably, in the above trajectory smoothing prediction method, the step of calculating the current predicted altitude of the UAV based on the maximum allowable slope of the current scene and the smoothed elevation estimate includes:

[0060] Using the maximum permissible slope of the current scene and the terrain distance between the estimated terrain height from the previous frame and the original terrain height in the current frame, the maximum elevation gain or loss is calculated using the formula:

[0061]

[0062] Calculate the maximum lift-up or descent altitude of the drone, where D is the maximum lift-up or descent altitude of the drone, and ds is the terrain distance. This represents the maximum permissible slope for the current scenario.

[0063] Using the drone's maximum lift or descent altitude and terrain altitude deviation, the flight altitude is calculated using the formula:

[0064] d = min(D, dh)

[0065] Calculate the drone's flight altitude, where d is the drone's flight altitude, D is the drone's maximum lift or descent altitude, and dh is the terrain altitude deviation;

[0066] Using the drone's flight altitude and smoothed elevation estimate, the predicted altitude is calculated according to the formula:

[0067] H pre =min{max{H move H last -d},H last +d}

[0068] Calculate the current predicted altitude of the drone, where H move Here is the smoothed elevation estimate, where d is the UAV's flight altitude, and H is... pre This represents the current predicted altitude of the drone.

[0069] According to a second aspect of the present invention, the present invention also provides a terrain-height-based unmanned aerial vehicle (UAV) trajectory smoothing prediction system, comprising:

[0070] The memory, the processor, and the terrain-height-based UAV trajectory smoothing prediction program stored in the memory and running on the processor, wherein the UAV trajectory smoothing prediction program, when executed by the processor, implements the steps of the UAV trajectory smoothing prediction method provided by any of the above technical solutions.

[0071] In summary, the UAV trajectory smoothing prediction scheme based on terrain height provided in this application acquires multiple frames of point cloud data using UAV-borne LiDAR, extracts the original terrain height corresponding to each frame of point cloud data from these multiple frames of point cloud data, constructs a double-ended queue, inserts the original terrain height of each frame into the double-ended queue in chronological order, and then calculates the terrain height deviation based on the estimated terrain height of the previous frame and the original terrain height of the current frame. In this way, a constraint relationship between the original terrain height of the current frame and the estimated terrain height of the previous frame can be established based on the terrain height deviation. Specifically, when the absolute value of the terrain height deviation is less than or equal to the limit height deviation threshold, the estimated terrain height of the current frame is calculated based on the adjacent height constraint relationship between the estimated terrain height of the previous frame and the original terrain height of the current frame in the double-ended queue. Then, the estimated terrain height of the current frame is inserted into the double-ended queue. In this way, the terrain height of the next frame can be estimated based on the constraint relationship between the terrain heights of the previous and next frames. Alternatively, when the absolute value of the terrain height deviation is greater than the limit height threshold, the original terrain height of the current frame is filtered for terrain trends, and the filtered estimated terrain height of the current frame is inserted into the double-ended queue. In this way, the terrain trajectory matrix of the double-ended queue can be constructed based on the terrain trends related to the terrain height of all frames in the queue. Then, the estimated terrain height of the current frame is calculated and inserted into the double-ended queue. Then, the current flight altitude of the UAV is calculated based on the estimated terrain height of the current frame and inserted into the double-ended queue. By performing spline smoothing processing on the double-ended queue through terrain smoothing parameters, a smoothed elevation estimate can be obtained. Then, the maximum allowable slope of the current scene is combined to calculate the current predicted altitude of the UAV. Thus, the flight altitude of the UAV is controlled according to the terrain trend change of the terrain height, so as to achieve a smooth transition of the UAV flight trajectory. This addresses the problem of large changes in flight altitude and insufficient smoothness in flight trajectories during the terrain-following flight of existing drones. Attached Figure Description

[0072] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on the structures shown in these drawings without creative effort.

[0073] Figure 1 This is a flowchart illustrating a method for predicting the trajectory smoothing of a UAV based on terrain height, provided in an embodiment of the present invention.

[0074] Figure 2 This is a flowchart illustrating a method for determining the data length of a double-ended queue according to an embodiment of the present invention;

[0075] Figure 3 yes Figure 2 The illustrated embodiment provides a flowchart of a method for estimating terrain height in the current frame;

[0076] Figure 4 yes Figure 1 The illustrated embodiment provides a flowchart of a method for calculating terrain height deviation;

[0077] Figure 5 yes Figure 1 The illustrated embodiment provides a flowchart of a terrain trend filtering method;

[0078] Figure 6 yes Figure 1 The illustrated embodiment provides a flowchart of a filtering method for estimating terrain height in the current frame;

[0079] Figure 7 This is a flowchart illustrating a method for calculating the current flight altitude of a drone according to an embodiment of the present invention;

[0080] Figure 8 yes Figure 1 The illustrated embodiment provides a flowchart of a spline smoothing method for a double-ended queue.

[0081] Figure 9 yes Figure 1 The illustrated embodiment provides a flowchart of a method for calculating the current predicted altitude of a drone.

[0082] Figure 10 This is a schematic diagram of the structure of a UAV trajectory smoothing prediction system based on terrain height provided in an embodiment of the present invention.

[0083] The realization of the objective, functional features and advantages of the present invention will be further explained in conjunction with the embodiments and with reference to the accompanying drawings. Detailed Implementation

[0084] It should be understood that the specific embodiments described herein are merely illustrative of the invention and are not intended to limit the invention.

[0085] The main technical problem solved by the embodiments of the present invention is:

[0086] Even with existing technologies capable of acquiring high-precision terrain height data, when drones set their flight altitude based on the terrain for terrain-following flight, inconsistencies exist between consecutive frames and between the terrain heights in front of and behind the drone. Simply averaging adjacent data is insufficient to effectively smooth terrain fluctuations, especially when multiple consecutive estimation errors lead to significant terrain variations. This necessitates rapid ascents and descents during drone flight, resulting in large altitude changes and an uneven flight path. Consequently, drone rework and re-flying are required, increasing the drone operation cycle and impacting project progress.

[0087] To address the aforementioned issues, the following embodiments of the present invention provide a terrain-height-based drone trajectory smoothing prediction scheme. After acquiring the original terrain height corresponding to multiple frames of point cloud data, a double-ended queue is constructed, and the original terrain height is inserted into the double-ended queue. Then, based on the relationship between the terrain height deviation and the defined height deviation threshold, the terrain height of the current frame is estimated. Finally, based on the terrain height estimated in the current frame, the current flight altitude of the drone is calculated and inserted into the double-ended queue. This solves the problem of large fluctuations in drone flight altitude and insufficient smoothness of flight trajectory in the prior art, achieving the goal of controlling the drone to fly smoothly and smoothing its flight trajectory.

[0088] To achieve the above objectives, see [link to relevant documentation]. Figure 1 , Figure 1 This is a flowchart illustrating a method for predicting UAV trajectory smoothing based on terrain height, provided in an embodiment of the present invention. Figure 1 As shown, the terrain-height-based UAV trajectory smoothing prediction method includes:

[0089] S110: Uses an onboard LiDAR to acquire multi-frame point cloud data, and extracts the original terrain height corresponding to each frame of point cloud data. The original terrain height is obtained by performing DEM modeling on the multi-frame point cloud data and extracting the lowest point corresponding to each element in the DEM model. By obtaining the original terrain height, the UAV can smoothly process its flight altitude and flight trajectory by following the terrain height corresponding to the multi-frame point cloud data.

[0090] Simply averaging adjacent point cloud data is not effective in smoothing out terrain fluctuations, especially when there are multiple consecutive errors in terrain height estimation leading to large terrain fluctuations. In such cases, it is necessary to use a long-term terrain height data series to estimate the overall terrain trend and use the judgment of terrain trend to reduce terrain fluctuations.

[0091] S120: Construct a double-ended queue and insert the original terrain height of each frame into the double-ended queue in chronological order. The constructed double-ended queue enables rapid insertion of data and deletion of existing data at the front of the queue. In this embodiment, the average terrain height extracted from each frame is inserted into the back of the double-ended queue in chronological order. The minimum and maximum data lengths of the double-ended queue are determined to be Kmin and Kmax, respectively. This allows for appropriate processing of the double-ended queue when the data length reaches the minimum and maximum data lengths, as detailed in the next step.

[0092] S130: Calculate the terrain height deviation based on the estimated terrain height of the previous frame and the original terrain height of the current frame, and determine whether the absolute value of the terrain height deviation in the double-ended queue is less than or equal to a limited height deviation threshold. In this embodiment of the invention, let the estimated terrain height dimension of the previous frame be Hmean, the maximum adjacent height deviation be Hdiff, the original terrain height of the current frame be Hraw, and the estimated terrain height of the current frame be He. Then, the terrain height deviation dh is equal to the estimated terrain height Hmean of the previous frame minus the original terrain height Hraw of the current frame. This terrain height deviation reflects the magnitude of terrain change between two adjacent frames. Since each frame corresponds to an adjacent ground area during the drone's flight, this reflects the magnitude of terrain change between adjacent ground areas. Determining whether the absolute value of the terrain height deviation in the double-ended queue is less than or equal to a limited height threshold determines whether the terrain undulation change between two adjacent frames is drastic.

[0093] S140: If the absolute value of the terrain height deviation is less than or equal to the limit height deviation threshold, then the estimated terrain height of the current frame is calculated based on the adjacent height constraint relationship between the estimated terrain height of the previous frame and the original terrain height of the current frame in the double-ended queue, and the estimated terrain height of the current frame is inserted into the double-ended queue.

[0094] When the absolute value of the terrain height deviation is less than or equal to the defined height deviation threshold and is less than the adjacent height deviation threshold, it indicates that the terrain height fluctuation in the current frame is small. The estimated terrain height of the current frame can be estimated using the adjacent height constraint relationship between adjacent frames, and this estimated terrain height is inserted into the dequeue. The defined height deviation threshold can be calculated based on the variance of Kmin terrain height data points in the dequeue.

[0095] S150: If the absolute value of the terrain height deviation is greater than the limited height threshold, terrain trend filtering is applied to the original terrain height of the current frame. The filtered estimated terrain height of the current frame is inserted into the double-ended queue, and a terrain trajectory matrix for the double-ended queue is constructed. The estimated terrain height of the current frame is calculated based on the terrain trajectory matrix and inserted into the double-ended queue. If the absolute value of the terrain height deviation is greater than the limited height threshold, it indicates that the original terrain height of the current frame fluctuates significantly. It is necessary to use the fluctuation trend of terrain height over multiple frames for constraint, thereby estimating a more reasonable estimated terrain height for the current frame as He, and inserting this estimated terrain height value into the aforementioned double-ended queue. Here, the terrain trend filtering method is used to obtain the estimated terrain height of the current frame. First, the initial filtered terrain height is calculated, and the filtered estimated terrain height of the current frame is inserted into the double-ended queue. A terrain trajectory matrix for the double-ended queue is constructed, and the estimated terrain height of the current frame is calculated based on this terrain trajectory matrix and inserted into the double-ended queue. In this way, the terrain trajectory of the terrain height of each frame in the double-ended queue can be calculated. The estimated terrain height of the current frame is estimated based on the changing trend of the terrain trajectory, and then the flight altitude of the UAV is constrained by the terrain height of the current frame.

[0096] S160: Estimate the terrain height based on the current frame, calculate the current flight altitude of the drone, and insert it into the double-ended queue. After obtaining the estimated terrain height of the current frame, the drone's flight trajectory needs to be smoothed. Specifically, the minimum flight altitude and relative terrain flight altitude of the drone are calculated based on the estimated terrain height of the current frame. Then, the current flight altitude is selected from the minimum flight altitude and relative terrain flight altitude. Because to ensure the flight safety of the drone, the maximum value between the minimum flight altitude and the relative terrain flight altitude needs to be selected as the current flight altitude of the drone and inserted into the double-ended queue.

[0097] S170: Use terrain smoothing parameters to perform spline smoothing on the data in the double-ended queue to obtain smoothed elevation estimates.

[0098] When the amount of data in the dequeue reaches the number of parameters M that can be used for spline smoothing, spline smoothing is performed on the dequeue, and the smoothed elevation estimate is obtained. This elevation estimate reflects the current elevation of the terrain after terrain smoothing, based on the terrain change trend. Using this elevation estimate, you can calculate the predicted altitude of the UAV.

[0099] S180: Calculate the current predicted altitude of the drone based on the maximum permissible slope and the smoothed elevation estimate of the current scene. Because there are certain slope limitations during drone ascent and descent, and to reduce fluctuations during flight, it is necessary to set the maximum permissible slope for the current scene. Using the maximum permissible slope and elevation estimate of the current scene, the current predicted altitude of the drone can be calculated, thus achieving a smooth drone flight trajectory.

[0100] In summary, the UAV trajectory smoothing prediction method based on terrain height provided in this application acquires multiple frames of point cloud data using a UAV-borne LiDAR, extracts the original terrain height corresponding to each frame of point cloud data from these multiple frames of point cloud data, constructs a double-ended queue, inserts the original terrain height of each frame into the double-ended queue in chronological order, and then calculates the terrain height deviation based on the estimated terrain height of the previous frame and the original terrain height of the current frame. In this way, a constraint relationship between the original terrain height of the current frame and the estimated terrain height of the previous frame can be established based on the terrain height deviation. Specifically, when the absolute value of the terrain height deviation is less than or equal to the limit height deviation threshold, the estimated terrain height of the current frame is calculated based on the adjacent height constraint relationship between the estimated terrain height of the previous frame and the original terrain height of the current frame in the double-ended queue. Then, the estimated terrain height of the current frame is inserted into the double-ended queue. In this way, the terrain height of the next frame can be estimated based on the constraint relationship between the terrain heights of the previous and next frames. Alternatively, when the absolute value of the terrain height deviation is greater than the limit height threshold, the original terrain height of the current frame is filtered for terrain trends, and the filtered estimated terrain height of the current frame is inserted into the double-ended queue. In this way, the terrain trajectory matrix of the double-ended queue can be constructed based on the terrain trends related to the terrain height of all frames in the queue. Then, the estimated terrain height of the current frame is calculated and inserted into the double-ended queue. Then, the current flight altitude of the UAV is calculated based on the estimated terrain height of the current frame and inserted into the double-ended queue. By performing spline smoothing processing on the double-ended queue through terrain smoothing parameters, a smoothed elevation estimate can be obtained. Then, the maximum allowable slope of the current scene is combined to calculate the current predicted altitude of the UAV. Thus, the flight altitude of the UAV is controlled according to the terrain trend change of the terrain height, so as to achieve a smooth transition of the UAV flight trajectory. This addresses the problem of large changes in flight altitude and insufficient smoothness in flight trajectories during the terrain-following flight of existing drones.

[0101] In the process of terrain height smoothing, a minimum data length of Kmin is set for the data in the double-ended queue. When the data length is less than or equal to the predetermined minimum data length Kmin, the estimated terrain height of the current frame in the double-ended queue needs to be calculated. This allows us to obtain the estimated terrain height corresponding to the original terrain height of each frame in the double-ended queue, thereby further realizing the smoothing of continuous terrain heights.

[0102] Specifically, as a preferred embodiment, such as Figure 2 As shown, the above-mentioned UAV trajectory smoothing prediction method, after step S120: constructing a double-ended queue and inserting the original terrain height of each frame into the double-ended queue in chronological order, further includes:

[0103] S210: Determine whether the data length of the double-ended queue is less than or equal to the predetermined minimum data length.

[0104] S220: If the data length of the dequeue is less than or equal to the predetermined minimum data length, the estimated terrain height of the current frame is calculated based on the adjacent height constraint relationship between the estimated terrain height of the previous frame and the original terrain height of the current frame in the dequeue. Through the above adjacent height constraint relationship and the estimated terrain height of the previous frame, the terrain height of the current frame can be estimated based on the terrain height of the previous frame, thereby achieving smooth processing of the terrain height of each frame based on the continuous change relationship between terrains.

[0105] Specifically, as a preferred embodiment, such as Figure 3 As shown, the step of calculating the estimated terrain height for the current frame based on the adjacency height constraint relationship between the estimated terrain height of the previous frame and the original terrain height of the current frame in the double-ended queue includes:

[0106] S221: Using the terrain height estimated from the previous frame, the original terrain height of the current frame, and the maximum adjacent height deviation, calculate the terrain height based on the current frame's terrain height estimation formula:

[0107] H1 = max{Hraw, Hmean - Hdiff}

[0108] H2 = min{H1, Hmean + Hdiff}

[0109] He = (H2 + Hmean) / 2

[0110] The estimated terrain height for the current frame is calculated, where Hraw is the original terrain height for the current frame, Hmean is the estimated terrain height for the previous frame, Hdiff is the maximum adjacent height deviation, H1 is the first estimated terrain height, H2 is the second estimated terrain height, and He is the estimated terrain height for the current frame.

[0111] S222: Insert the estimated terrain height of the current frame into the dequeue.

[0112] In this embodiment of the invention, when the data length in the double-ended queue is less than Kmin, the height smoothing calculation method is to use the adjacent height constraint relationship and average summation. The current frame estimated terrain height calculation method is to determine whether the original terrain height of the current frame is within the upper and lower constraint range of the deviation between the estimated terrain height of the previous frame and the adjacent maximum height. If so, it can be determined that the height fluctuation of the current frame is small. At this time, the smaller value between the second estimated terrain height H2 and the estimated terrain height of the previous frame is used to calculate the estimated terrain height of the current frame. Then, the calculation result is inserted into the double-ended queue, thereby smoothing the terrain height of multiple consecutive frames and reducing the fluctuation of terrain height.

[0113] Additionally, if the data length of the double-ended queue is greater than the predetermined minimum data length, then step S130 is executed: the terrain height deviation is calculated based on the terrain height estimated in the previous frame and the original terrain height in the current frame.

[0114] The technical solution provided by this invention determines whether the original terrain height of the current frame is within the constraint range of the deviation between the estimated terrain height of the previous frame and the adjacent maximum height when the data length of the double-ended queue is less than or equal to the predetermined minimum data length. If so, it indicates that the fluctuation of terrain height in adjacent frames is small. At this time, the constraint relationship between the original terrain height of the current frame, the estimated terrain height of the previous frame and the deviation between the adjacent maximum height is used to calculate the estimated terrain height of the current frame, thereby realizing the calculation of the estimated terrain height of the current frame and the smoothing of terrain height in multiple frames.

[0115] In addition, when the number of data in the dequeue is greater than Kmin, it is necessary to calculate the average height Havg and variance of the last Kmin data in the dequeue, so as to use the variance to estimate the current terrain fluctuation.

[0116] Specifically, as a preferred embodiment, such as Figure 4 As shown, in the above trajectory smoothing prediction method, step S130: calculating the terrain height deviation based on the estimated terrain height of the previous frame and the original terrain height of the current frame, including:

[0117] S131: Using the terrain height estimated from the previous frame and the original terrain height of the current frame, the terrain height deviation is calculated according to the terrain height deviation calculation formula: dh=Hmean–Hraw, where dh is the terrain height deviation, Hmean is the terrain height estimated from the previous frame, and Hraw is the original terrain height of the current frame.

[0118] S132: Calculate the variance of terrain height for a predetermined number of frames in the double-ended queue;

[0119] S133: Calculate the variance of terrain height using a predetermined number of frames, based on a defined height deviation threshold formula:

[0120] Hdelta1=2*S

[0121] Hdelta2=3*S

[0122] The first and second limited height deviation thresholds are calculated respectively, where Hdelta1 is the first limited height deviation threshold, Hdelta2 is the second limited height deviation threshold, and S is the variance of the terrain height of a predetermined number of frames.

[0123] The technical solution provided in this invention, when the number of data (estimated terrain height for each frame) in the double-ended queue is greater than Kmin, calculates the average height (Havg) and variance of the last Kmin data in the double-ended queue, and uses the variance to estimate the current terrain fluctuation. Furthermore, according to the terrain height deviation calculation formula, the terrain height deviation is calculated using the estimated terrain height of the previous frame and the original terrain height of the current frame. When the absolute value of the terrain height deviation is less than or equal to the aforementioned first defined height deviation threshold, and less than the adjacent height deviation threshold, it indicates that the current terrain height fluctuation is small, and the estimated terrain height for the current frame can be calculated using the calculation method in step S221 above, and this estimated terrain height for the current frame is inserted into the double-ended queue. If the absolute value of the current terrain height deviation is greater than the aforementioned first defined height deviation threshold, it indicates that the current height fluctuation is large, and it is necessary to use the changing trend of terrain height in consecutive frames to constrain the terrain height of the current frame to obtain a more reasonable terrain height value.

[0124] If the original terrain height of the current frame deviates significantly from the estimated terrain height of the previous frame, meaning the current height fluctuates greatly and it is difficult to smooth the height using adjacent height constraints, then the original terrain height of the current frame is processed using the height trend of terrain heights from multiple consecutive frames to estimate a more reasonable height value, which is then inserted into the double-ended queue.

[0125] Specifically, as a preferred embodiment, such as Figure 5 As shown, in the above trajectory smoothing prediction method, step S150: performing terrain trend filtering on the original terrain height of the current frame and inserting the filtered estimated terrain height of the current frame into the double-ended queue includes:

[0126] S151: Using the terrain height estimated from the previous frame and the second defined height deviation threshold, filter according to the terrain trend formula:

[0127] Hf = Hmean – Hdelta2 (dh≥0)

[0128] Hf = Hmean + Hdelta2 (dh < 0)

[0129] The filtered estimated terrain height of the current frame is calculated; where Hf is the filtered estimated terrain height of the current frame, Hmean is the estimated terrain height of the previous frame, and Hdelta2 is the second limiting height deviation threshold.

[0130] S152: Insert the filtered terrain height estimate of the current frame into the double-ended queue.

[0131] The technical solution provided by this invention can calculate the filtered estimated terrain height of the current frame according to the terrain trend filtering formula and insert it into the queue when the terrain fluctuation is large and the original terrain height of the current frame deviates significantly from the estimated terrain height of the previous frame. This achieves smooth processing of the terrain height change trend over multiple consecutive frames.

[0132] If the absolute value of the terrain height deviation is greater than the defined height threshold, terrain trend filtering is performed on the original terrain height of the current frame based on continuous terrain trajectories. For details on the filtering method for estimating terrain height in the current frame, please refer to [link to relevant documentation]. Figure 6 .

[0133] As a preferred embodiment, such as Figure 6 As shown, in the above trajectory smoothing prediction method, step S150: constructing a terrain trajectory matrix for a double-ended queue, calculating the estimated terrain height for the current frame based on the terrain trajectory matrix, and inserting it into the double-ended queue, includes:

[0134] S153: Construct a terrain trajectory matrix using a deque. Expanding the deque generates an L-row, K-column terrain trajectory matrix. The specific method for expanding the deque is to traverse the data from beginning to end, with indices 0 to K-1 forming the first row, indices 1 to K forming the second row, and so on, with L-1 to L+K-1 forming the last row, ultimately constructing the terrain trajectory matrix X.

[0135] S154: Perform singular value decomposition on the terrain trajectory matrix to obtain the left matrix, right matrix, and singular values. Performing singular value decomposition on the terrain estimation matrix X yields the left and right matrices U and V, and the singular value vector E. These matrices and vectors reflect the changing trend of terrain height over multiple consecutive frames.

[0136] S155: Using the left matrix, right matrix, and singular values, calculate according to the matrix formula:

[0137]

[0138] Construct a terrain height transpose matrix of the same size as the terrain trajectory matrix, where E0 is the largest singular value and U0 is the first column of the left matrix U. This is the transpose of the first column of the right matrix V.

[0139] S156: Convert the terrain height transpose matrix into a terrain height sequence, select the last terrain height element value in the terrain height sequence, and use it as the estimated terrain height for the current frame.

[0140] Specifically, the elements in the other terrain height transpose matrix Y are Where i and j are the row and column of the matrix, respectively, 1≤i≤L, 1≤j≤K, let L * =min(L,K), K * =max(L,K), N=L+K-1, using the diagonal averaging method to convert the terrain height transpose matrix Y into a sequence y1,y2,…,y N The specific calculation method is as follows:

[0141]

[0142] The original sequence can be recovered using the above calculation formula, and the last terrain height element value y can be extracted. N Estimate the terrain height He for the current frame obtained from the re-estimation, and then insert He into the aforementioned double-ended queue.

[0143] S157: Insert the estimated terrain height of the current frame into the dequeue.

[0144] The technical solution provided by this invention constructs a terrain trajectory matrix using a double-ended queue, then performs singular value decomposition on the terrain trajectory matrix, and then constructs a terrain height transpose matrix of the same size as the terrain trajectory matrix according to the matrix calculation formula. The terrain height transpose matrix is ​​then converted into a terrain height sequence. The terrain height sequence is used to obtain the estimated terrain height for the current frame and insert it into the double-ended queue. In this way, the terrain height change trend can be estimated through continuous multi-frame terrain height estimation, and the terrain height for the current frame can be estimated through this terrain height change trend.

[0145] In addition, as a preferred embodiment, such as Figure 7 As shown, in the above trajectory smoothing prediction method, step S160: estimating the terrain height based on the current frame, calculating the current flight altitude of the UAV, and inserting it into the double-ended queue, includes:

[0146] S161: Extract the maximum ground feature height from point cloud data. The maximum ground feature height can be obtained through DSM modeling. The maximum ground feature height needs to be considered during UAV flight to avoid dangerous accidents.

[0147] S162: Calculate the sum of the maximum ground feature height and the flight clearance height to obtain the minimum flight altitude of the UAV. Let the UAV's flight altitude be d1 and the flight clearance height be d2. Then, the UAV's minimum flight altitude Hminf and relative terrain flight altitude Hterra can be expressed as: Hminf = Hmax + d2. The minimum flight altitude of the UAV can be calculated using the above formulas, thereby preventing dangerous accidents involving the UAV.

[0148] S163: Calculate the sum of the estimated terrain altitude and the set flight altitude in the current frame to obtain the drone's relative terrain flight altitude. After obtaining the drone's estimated terrain altitude He in the current frame, and combining it with the drone's set flight altitude, the drone's relative terrain flight altitude can be calculated using the formula Hterra = He + d1.

[0149] S164: Select the maximum value between the drone's minimum flight altitude and its relative terrain flight altitude as the drone's current flight altitude.

[0150] S165: Insert the drone's current flight altitude value into the double-ended queue.

[0151] By selecting the maximum value from the minimum flight altitude and relative terrain flight altitude of the above-mentioned UAV, the current flight altitude of the UAV can be obtained, that is, Hrawf = max{Hminf, Hterra}. Finally, the current flight altitude value is added to the data queue to obtain an initial flight altitude of the UAV, and then the flight trajectory of the UAV is smoothed.

[0152] In addition, if the number of parameters M in the double-ended queue is sufficient for spline smoothing, spline smoothing is performed on the data queue and the predicted height after smoothing is estimated. If the amount of data is insufficient, the next step is performed.

[0153] Specifically, as a preferred embodiment, such as Figure 8 As shown, in the above trajectory smoothing prediction method, step S170, which involves using terrain smoothing parameters to perform spline smoothing on the data in the double-ended queue to obtain the smoothed elevation estimate, includes:

[0154] S171: Construct a cubic polynomial function representing the current flight altitude of the UAV. The form of this cubic polynomial function is as follows: f(x) = a0 + a1x + a2x 2 +a3x 3 x is the current flight altitude, and a0, a1, a2 and a3 are the parameters of the cubic polynomial function.

[0155] S172: Construct a residual sum of squares minimization function based on the relationship between the cubic polynomial function and the terrain smoothing parameter. Let the regularized terrain smoothing parameter be λ, and the elevation value be y.i Then the function for minimizing the sum of squared residuals can be expressed as:

[0156]

[0157] Where Y is the terrain height transpose matrix, x is the horizontal coordinate, f(x) is the cubic polynomial function mentioned above, and f""x" is the second derivative of f(x).

[0158] S173: Solve the cubic polynomial function using the least squares method to obtain the terrain smoothing parameters.

[0159] In the above formula, the terrain smoothing parameter λ can be used to adjust the curve smoothness. Then, the formula is solved by least squares to obtain the estimated parameters, which are a = [a0, a1, a2, a3]. T Then the matrix representation of the estimated parameters can be expressed as:

[0160] a=(B T B+λΩ) -1 B T y

[0161] Where y is the observed value,

[0162]

[0163] S174: Construct the transpose matrix of the terrain smoothing parameters and elevation estimates, solve for the transpose matrix to obtain the elevation estimates. The elevation estimates are then obtained by solving the polynomial equation coefficients. The smoothed elevation is then obtained.

[0164] S175: Elevation estimates A moving average is applied to obtain a smoothed elevation estimate.

[0165] The above elevation estimates are then averaged using a moving average. Specifically, the estimated terrain height values ​​obtained from the previous frame, the last N-1 data points in the sequence, and the current predicted height are combined with a total of N data points for smoothing. Each of these is then assigned a corresponding weight, resulting in the smoothed height value Hmove.

[0166] In addition, as a preferred embodiment, such as Figure 9 As shown, in the above trajectory smoothing prediction method, step S180: calculating the current predicted altitude of the UAV based on the maximum allowable slope of the current scene and the smoothed elevation estimate, includes:

[0167] S181: Using the maximum permissible slope of the current scene and the terrain distance between the estimated terrain height of the previous frame and the original terrain height of the current frame, calculate the maximum elevation gain or loss using the following formula:

[0168]

[0169] Calculate the maximum lift-up or descent altitude of the drone, where D is the maximum lift-up or descent altitude of the drone, and ds is the terrain distance. This represents the maximum permissible slope for the current scenario.

[0170] S182: Using the drone's maximum lift or descent altitude and terrain altitude deviation, calculated according to the flight altitude formula:

[0171] d = min(D, dh)

[0172] Calculate the drone's flight altitude, where d is the drone's flight altitude, D is the drone's maximum lift or descent altitude, and dh is the terrain altitude deviation;

[0173] S183: Using the drone's flight altitude and smoothed elevation estimate, the predicted altitude is calculated according to the formula:

[0174] H pre =min{max{H move H last -d},H last +d}

[0175] Calculate the current predicted altitude of the drone, where H move Here is the smoothed elevation estimate, where d is the UAV's flight altitude, and H is... pre H represents the current predicted altitude of the drone. last This is the previous predicted altitude for the drone.

[0176] In the technical solution provided by the embodiments of the present invention, because there are certain slope limitations for the take-off or descent of the drone, and in order to reduce altitude fluctuations during flight, the maximum allowable slope in a given scenario is considered. By constraining the angles of adjacent prediction points, the flight stability of the UAV can be further adjusted. Let dh be the deviation between the estimated terrain height in the current frame and the height in the previous frame, and ds be the distance. Then the maximum allowable height can be obtained by the above prediction height calculation formula. Through the above formula, the flight altitude of the UAV can be smoothly processed in continuous time.

[0177] Additionally, see Figure 10 , Figure 10 This is a flowchart illustrating a terrain-height-based unmanned aerial vehicle (UAV) trajectory smoothing prediction system provided in an embodiment of the present invention. Figure 10 As shown, the UAV trajectory smoothing prediction system includes:

[0178] The processor 1001, communication line 1002, communication module 1003, memory 1004, and a terrain-height-based UAV trajectory smoothing prediction program stored in memory 1004 and running on processor 1001, wherein when the UAV trajectory smoothing prediction program is executed by processor 1001, it implements the steps of the UAV trajectory smoothing prediction method provided in any of the above embodiments.

[0179] In summary, the above-mentioned technical solution of this application adopts UAV-borne lidar technology to process the laser point cloud collected in real time during the flight of the UAV and estimate the terrain altitude, confirm the altitude deviation between the UAV and the terrain in real time, reasonably predict the next flight position by combining historical terrain and flight trajectory, and adjust the flight state in real time according to the preset flight altitude to maintain a relatively stable flight altitude with the ground, and finally achieve the effect of terrain-following flight. It can realize real-time terrain extraction and effectively solve the fluctuation problem in the terrain extraction process.

[0180] Those skilled in the art will understand that embodiments of the present invention can be provided as methods, systems, or computer program products. Therefore, the present invention can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, the present invention can take the form of a computer program product embodied on one or more computer-usable storage media (including, but not limited to, disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0181] This invention is described with reference to flowchart illustrations and / or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of the invention. It will be understood that each block of the flowchart illustrations and / or block diagrams, and combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, special-purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, generate instructions for implementing the flowchart illustrations and / or block diagrams. Figure 1 One or more processes and / or boxes Figure 1 A device that provides the functions specified in one or more boxes.

[0182] These computer program instructions may also be stored in a computer-readable storage medium that can direct a computer or other programmable data processing device to function in a particular manner, such that the instructions stored in the computer-readable storage medium produce an article of manufacture including instruction means, which are implemented in a process Figure 1 One or more processes and / or boxes Figure 1 The function specified in one or more boxes.

[0183] These computer program instructions may also be loaded onto a computer or other programmable data processing equipment to cause a series of operational steps to be performed on the computer or other programmable equipment to produce a computer-implemented process, thereby providing instructions that execute on the computer or other programmable equipment for implementing the process. Figure 1 One or more processes and / or boxes Figure 1 The steps of the function specified in one or more boxes.

[0184] It should be noted that any reference signs placed between parentheses in the claims should not be construed as limiting the claims. The word "comprising" does not exclude the presence of components or steps not listed in the claims. The word "a" or "an" preceding a component does not exclude the presence of a plurality of such components. The invention can be implemented by means of hardware comprising several different components and by means of a suitably programmed computer. In a unit claim enumerating several means, several of these means may be embodied by the same item of hardware. The use of the words first, second, and third, etc., does not indicate any order. These words can be interpreted as names.

[0185] Although preferred embodiments of the invention have been described, those skilled in the art, upon learning the basic inventive concept, can make other changes and modifications to these embodiments. Therefore, the appended claims are intended to be interpreted as including both the preferred embodiments and all changes and modifications falling within the scope of the invention.

[0186] Obviously, those skilled in the art can make various modifications and variations to this invention without departing from its spirit and scope. Therefore, if these modifications and variations fall within the scope of the claims of this invention and their equivalents, this invention also intends to include these modifications and variations.

Claims

1. A method for trajectory smoothing prediction of an unmanned aerial vehicle (UAV) based on terrain height, the method comprising: The method comprises the following steps: acquiring multiple frames of point cloud data by using an unmanned aerial laser radar, and extracting original terrain height corresponding to each frame of point cloud data from the multiple frames of point cloud data; constructing a double-ended queue, and sequentially inserting each frame of original terrain height into the double-ended queue in time sequence; calculating terrain height deviation according to last frame estimated terrain height and current frame original terrain height, and judging whether the absolute value of the terrain height deviation is less than or equal to a limited height deviation threshold value; if the absolute value of the terrain height deviation is less than or equal to the limited height deviation threshold value, calculating current frame estimated terrain height according to the adjacent height constraint relationship between the last frame estimated terrain height and the current frame original terrain height in the double-ended queue, and inserting the current frame estimated terrain height into the double-ended queue; if the absolute value of the terrain height deviation is greater than the limited height threshold value, performing terrain trend filtering on the current frame original terrain height, inserting the filtered current frame estimated terrain height into the double-ended queue, constructing a terrain trajectory matrix of the double-ended queue, and calculating the current frame estimated terrain height according to the terrain trajectory matrix and inserting the current frame estimated terrain height into the double-ended queue; calculating the current flight height of the unmanned aerial vehicle according to the current frame estimated terrain height and inserting the current flight height into the double-ended queue; performing spline smoothing processing on the data in the double-ended queue by using a terrain smoothing parameter to obtain a smoothed elevation estimate value; calculating the current predicted height of the unmanned aerial vehicle according to the maximum allowed slope of the current scene and the smoothed elevation estimate value. 2.The method of claim 1, wherein, After the step of constructing a double-ended queue and sequentially inserting each frame of original terrain height into the double-ended queue in time sequence, the method further comprises the following steps: judging whether the data length of the double-ended queue is less than or equal to a predetermined minimum data length; if the data length of the double-ended queue is less than or equal to the predetermined minimum data length, calculating the current frame estimated terrain height according to the adjacent height constraint relationship between the last frame estimated terrain height and the current frame original terrain height in the double-ended queue; if the data length of the double-ended queue is greater than the predetermined minimum data length, performing the step of calculating terrain height deviation according to last frame estimated terrain height and current frame original terrain height.

3. The trajectory smoothing prediction method according to claim 2, wherein, The step of calculating the current frame estimated terrain height according to the adjacent height constraint relationship between the last frame estimated terrain height and the current frame original terrain height in the double-ended queue comprises the following steps: using the last frame estimated terrain height, the current frame original terrain height and the adjacent maximum height deviation to calculate the current frame estimated terrain height according to a current frame estimated terrain height calculation formula: ； calculating the current frame estimated terrain height, wherein Hraw is the current frame original terrain height, Hmean is the last frame estimated terrain height, Hdiff is the adjacent maximum height deviation, H1 is a first estimated terrain height, H2 is a second estimated terrain height, and He is the current frame estimated terrain height; inserting the current frame estimated terrain height into the double-ended queue.

4. The trajectory smoothing and predicting method according to claim 1 or 2, characterized in that, The step of calculating the terrain height deviation according to the terrain height of the previous frame and the original terrain height of the current frame comprises: The terrain height deviation is calculated according to a terrain height deviation calculation formula: dh = Hmean - Hraw, using the terrain height of the previous frame and the original terrain height of the current frame, wherein dh is the terrain height deviation, Hmean is the terrain height of the previous frame, and Hraw is the original terrain height of the current frame; The variance of the predetermined number of frame terrain heights in the double-ended queue is calculated; The predetermined number of frame terrain height variances are used to calculate a limited height deviation threshold according to a limited height deviation threshold calculation formula: ； The first limited height deviation threshold and the second limited height deviation threshold are respectively calculated, wherein Hdelta1 is the first limited height deviation threshold, Hdelta2 is the second limited height deviation threshold, and S is the variance of the predetermined number of frame terrain heights.

5. The trajectory smoothing prediction method according to claim 4, wherein, The step of performing terrain trend filtering on the original terrain height of the current frame and inserting the filtered terrain height of the current frame into the double-ended queue comprises: The terrain trend filtering formula is used to calculate the filtered terrain height of the current frame, using the terrain height of the previous frame and the second limited height deviation threshold, wherein Hf is the filtered terrain height of the current frame, Hmean is the terrain height of the previous frame, and Hdelta2 is the second limited height deviation threshold; ； The filtered terrain height of the current frame is inserted into the double-ended queue. The step of constructing the terrain trajectory matrix of the double-ended queue and calculating the terrain height of the current frame according to the terrain trajectory matrix and inserting it into the double-ended queue comprises:

6. The trajectory smoothing and predicting method of claim 1, wherein, The terrain trajectory matrix is constructed using the double-ended queue; The terrain trajectory matrix is singular value decomposed to obtain a left matrix, a right matrix, and singular values; The left matrix, the right matrix, and the singular values are used to calculate the terrain height transposed matrix according to a matrix calculation formula: The terrain height transposed matrix is converted into a terrain height sequence, and the terrain height element value at the last position in the terrain height sequence is selected as the terrain height of the current frame; ； constructing a terrain height transpose matrix of the same size as the terrain trajectory matrix, wherein, is the largest singular value, is the first column of the left matrix U, is the first column transpose of the right matrix V; The terrain height of the current frame is inserted into the double-ended queue. The step of calculating the current flight height of the unmanned aerial vehicle according to the terrain height of the current frame and inserting it into the double-ended queue comprises:

7. The trajectory smoothing and predicting method of claim 1, wherein, The maximum ground object height of the ground is extracted from the point cloud data; The sum of the maximum ground object height and the flight clearance height is calculated to obtain the minimum flight height of the unmanned aerial vehicle; The sum of the terrain height of the current frame and the set flight height is calculated to obtain the relative terrain flight height of the unmanned aerial vehicle; The maximum value of the minimum flight height of the unmanned aerial vehicle and the relative terrain flight height is selected as the current flight height of the unmanned aerial vehicle; The current flight height value of the unmanned aerial vehicle is inserted into the double-ended queue. The step of using the terrain smoothing parameter to perform spline smoothing processing on the data in the double-ended queue to obtain the smoothed elevation estimation value comprises:

8. The trajectory smoothing and predicting method according to claim 1 or 7, characterized by, A cubic polynomial function of the current flight height of the unmanned aerial vehicle is constructed; ​ constructing a residual square sum minimization function according to a relationship between the cubic polynomial function and a terrain smoothing parameter; solving the cubic polynomial function according to a least square method to obtain the terrain smoothing parameter; constructing a transpose matrix of the terrain smoothing parameter and an elevation estimation value, and solving the transpose matrix to obtain the elevation estimation value; performing a moving average processing on the elevation estimation value to obtain a smoothed elevation estimation value.

9. The trajectory smoothing and predicting method of claim 1, wherein, The step of calculating the current predicted height of the UAV according to the maximum allowed slope of the current scene and the smoothed elevation estimation value comprises: using the maximum allowed slope of the current scene and a terrain distance between a terrain height of a previous frame and a raw terrain height of the current frame, and according to a maximum lift or drop height calculation formula: ， calculating a maximum pull-up or pull-down height of the drone, wherein D is the maximum pull-up or pull-down height of the drone, ds is the terrain distance, is the maximum allowed slope for the current scene; using the maximum lift or drop height of the UAV and the terrain height deviation, and according to a flight height calculation formula: ； calculating the flight height of the UAV, wherein d is the flight height of the UAV, D is the maximum lift or drop height of the UAV, and dh is the terrain height deviation; using the flight height of the UAV and the smoothed elevation estimation value, and according to a predicted height calculation formula: ； calculating a current predicted altitude of the drone, wherein H move is the smoothed elevation estimate, d is the flight altitude of the drone, H pre is the current predicted altitude of the drone, H last is the last predicted altitude of the drone. 10.A terrain height based trajectory smoothing and prediction system for unmanned aerial vehicles, the system comprising: comprises: a memory, a processor, and a terrain height-based UAV trajectory smoothing prediction program stored in the memory and running on the processor, the terrain height-based UAV trajectory smoothing prediction program, when executed by the processor, implements the steps of the UAV trajectory smoothing prediction method in any one of claims 1 to 9.

Images