An unmanned helicopter low-altitude terrain following flight path guiding method

Abstract

The application provides a low-altitude terrain following flight path guiding method for unmanned helicopters, and is based on an enhanced vector field histogram algorithm and on-board laser radar spatial gridding data, and proposes a low-altitude terrain following flight path guiding method for unmanned helicopters in a three-dimensional space, introduces unmanned aerial vehicle flight performance constraints and safe flight standards, constructs a safe flight pipeline model for unmanned helicopters, designs a laser radar gridding detection information partial loss filling model based on a first-order model, fully utilizes the detection information of the on-board laser radar, guarantees the robustness of algorithm solving and the safety of calculation results, and meets the obstacle avoidance requirements in a three-dimensional space in a complex and severe flight environment. According to the on-board laser radar detection data and the safe flight constraints of the unmanned helicopter, the application can realize the calculation of optimal flight waypoints for the low-altitude terrain following flight path guiding of the unmanned helicopter.

Description

Technical Field

[0001] This invention relates to the field of autonomous flight and route guidance for unmanned aerial vehicles (UAVs), and specifically to a route guidance method for unmanned helicopters. Background Technology

[0002] With the rapid development of electronic and drone technologies in recent years, drone performance has improved dramatically, leading to their increasing application in assisting humans in complex flight missions under harsh conditions. Low-altitude terrain following is a typical flight mission in complex environments with a controllable risk of ground collision. Unmanned aerial vehicles (UAVs) need to stay close to the ground to avoid detection by reconnaissance systems, thus performing medium- to long-range guidance missions. During low-altitude terrain following, the unmanned helicopter uses an onboard forward-facing lidar to acquire terrain and obstacle information within a certain area directly ahead, and integrates this information with a terrain elevation database to obtain terrain and obstacle information of the surrounding operating environment. This allows for obstacle avoidance and predetermined route guidance. The flight status of the unmanned helicopter during low-altitude terrain following is as follows: Figure 1 As shown, the dynamic obstacle avoidance process is as follows: Figure 2 As shown.

[0003] Currently, experts and scholars in related fields typically address obstacle avoidance problems by using route planning algorithms and trajectory tracking control to solve dynamic obstacle avoidance issues. Traditional route planning algorithms utilize methods such as visible maps, random sampling search, probabilistic graphs, heuristic algorithms, and genetic algorithms to plan routes within a fixed area. However, these methods have certain drawbacks. Traditional route planning algorithms require detailed terrain maps and obstacle distribution information within the task area in advance to complete offline route planning. However, due to the difficulty of terrain mapping and the high cost of acquiring terrain data, it is difficult to guarantee perfect terrain and obstacle data for each task, and it is also difficult to handle dynamic and sudden threats. Secondly, traditional methods require the establishment of complete terrain and obstacle models and the discretization of the solution space before the algorithm can be called to complete discrete route planning. This process is time-consuming and labor-intensive, and requires specific mathematical models for specific problems. Finally, currently widely used route planning algorithms typically have high computational complexity, and their computational resource consumption is positively correlated with the size of the task area and the complexity of the terrain, which cannot meet the real-time requirements of low-altitude terrain following. With the rapid development of sensor technology, the performance of UAV onboard detection systems has improved significantly without changing costs. The need for dynamic obstacle avoidance based on real-time detection has gradually become an indispensable issue in route guidance under various complex conditions. The Enhanced Vector Field Histogram Plus (VFH+) algorithm is a real-time obstacle avoidance algorithm for mobile platforms that combines the principles of artificial potential fields and grid methods. This method constructs a grid map and forms an obstacle repulsion field based on obstacle information detected by the mobile platform's sensors. It then combines the target point's gravitational field and the platform's physical dimensions to establish a passable direction histogram, thereby obtaining the optimal forward direction and achieving dynamic obstacle avoidance. Although VFH+ has been widely used in various applications in the field of robotics, solving dynamic obstacle avoidance problems for robots in many practical engineering projects, the currently published related research has the following problems: First, many studies have only used the VFH+ algorithm to solve obstacle avoidance problems in two-dimensional planes, without considering dynamic obstacle avoidance in complex terrains in three-dimensional environments; second, some studies have used VFH+ to solve obstacle avoidance problems in three-dimensional space, but have not considered the data loss problem caused by sensor detection defects; finally, although many related studies have considered the physical size of the platform and simple kinematic constraints when searching for obstacle avoidance directions, there is a lack of sufficient research on complex motion platforms such as UAVs, and there is a lack of safe flight guarantee constraints when searching for the optimal flight direction of the platform. Summary of the Invention

[0004] To overcome the shortcomings of existing technologies, this invention provides a low-altitude terrain-following path guidance method for unmanned helicopters. This method calculates the optimal flight waypoint based on airborne lidar detection data and unmanned helicopter safety flight constraints. Therefore, addressing the low-altitude terrain-following problem in complex and harsh environments, this invention proposes a three-dimensional low-altitude terrain-following path guidance method for unmanned helicopters based on the enhanced vector field histogram algorithm and airborne lidar spatial rasterized data. It introduces UAV flight performance constraints and safe flight standards, constructs a safe flight pipeline model for unmanned helicopters, and designs a partial missing information filling model based on a first-order lidar rasterized detection information. This fully utilizes the detection information from the airborne lidar, ensuring the robustness of the algorithm solution and the security of the calculation results, thus meeting the obstacle avoidance requirements in three-dimensional space in complex and harsh flight environments.

[0005] The technical solution adopted by this invention to solve its technical problem includes the following steps:

[0006] (1) Calculate the principal value vector field histogram;

[0007] Define the airborne lidar grid coordinate system, or simply the grid system. The origin of the grid system is the current spatial position of the unmanned helicopter, X. grid The positive direction of the axis is the projection of the unmanned helicopter's velocity onto the horizontal plane, Y. grid The positive direction of the axis is perpendicular to X. grid The axis is vertically upward, Z grid The positive direction of the axis satisfies the right-hand screw rule. Figure 3 The layout of the detection range of the airborne lidar for unmanned helicopters within a grid system is shown. The detection range is a rectangular area with a width of n·D. cell The rectangle has a length of m·D cell If the rasterized detection data acquired by the airborne lidar is m·n discrete points, then the rasterized detection data directly in front of the airborne lidar is an m-row n-column matrix. Elements in Y x,z This represents the height of a grid point (x, z) relative to the current flight plane of the unmanned helicopter, where x = 1, ..., m and z = 1, ..., n. When a certain grid point Y... grid When axis data is missing, the corresponding element for that raster point is y. x,z =∞;

[0008] Define the safe flight path for unmanned helicopters, such as Figure 4 As shown, during flight, the unmanned helicopter must maintain a distance of R from the surrounding terrain or obstacles on its side. safe Above, the distance between the directly below and the ground or obstacle must be maintained at R. down Above, the area directly above must be at the maximum ground clearance H. mission Keep in Rup The above means that the unmanned helicopter should fly between the terrain envelope and the maximum altitude envelope, while the distance from the surrounding terrain and obstacles should meet the constraints of the safe flight path. Therefore, let the spherical coordinates of the grid point in the grid system be (ψ, θ, r), where ψ is the azimuth of the grid point relative to the origin, θ is the pitch of the grid point relative to the origin, and r is the distance relative to the origin. All angles and directions satisfy the right-hand screw rule. Further, the principal value vector field histogram is constructed as follows: Among them, H p The definition of the element is shown in equation (1):

[0009]

[0010] In the formula, ψ max δ represents the maximum offset azimuth of the grid point. ψ =2ψ max / t represents the grid azimuth offset step, θ max δ represents the maximum offset pitch of the grid points. θ =2θ max / k is the grid azimuth offset step; in a given direction, it is determined whether all grid points affect the safe flight of the unmanned helicopter according to the definition of safe flight pipeline, as shown in Equation (2) as the grid point safety criterion;

[0011]

[0012] In the formula, (x obs ,y obs ,z obs ) represents the coordinates of the grid points; in addition, since some elevation peaks may be filtered out after the terrain data is rasterized, the grid needs to be dilated. The dilated grid boundary needs to satisfy equation (2), and the dilated grid boundary is defined as shown in equation (3):

[0013]

[0014] In the formula, (x′ obs ,y′ obs ,z′ obs () represents the expanded raster boundary. D is the grid dilation radius. cell ×D cell To determine the dilated grid size, the boundary (x′) of each dilated grid needs to be determined in each discrete flight direction. obs ,y′ obs ,z′ obs Does it meet the safe flight pipeline constraints, i.e., equation (2)? Figure 5To establish the relationship between lidar gridded detection information and the safe flight path of the unmanned helicopter, the grid point closest to the origin is calculated in each flight direction (ψ,θ) according to the constraints of the safe flight path of the unmanned helicopter, and it is recorded as the principal value in that flight direction, as shown in Equation (4):

[0015]

[0016] In the formula, r obs Let W be the distance of a grid point from the origin, and let W be the set of grid points in the given direction (ψ,θ) that do not satisfy equations (2)-(3).

[0017] When calculating the principal value vector field histogram, due to the influence of terrain undulations and obstacle occlusion, the lidar is unable to detect some grid cells, resulting in missing grid data. Therefore, to ensure the flight safety of the unmanned helicopter, a first-order model is used to fill in the missing grid data. Figure 6 This is a schematic diagram of a method for filling in missing raster data. Based on the definition of elements in the rasterized detection matrix Y of an airborne lidar, when a certain raster point data... If a grid point is missing, then it is necessary to fill in the missing grid point data to ensure the calculation of the vector field histogram. This invention uses a linear regression equation passing through the origin and the previous non-missing grid point to calculate the grid data of the missing point. Let the missing point be... Along a straight line

[0018]

[0019] Find the previous non-missing obstacle grid point Then, the equation of the straight line as shown in equation (6) is constructed.

[0020]

[0021] Fill in the missing points according to formula (6). elevation data

[0022] (2) Calculate the histogram of the binary vector field;

[0023] When the principal values ​​h of obstacles in all flight directions are obtained p After (ψ,θ), construct the binary vector field histogram. Based on the requirements of low-altitude terrain following tasks, a binary vector field histogram hysteresis parameter (τ) is defined. low ,τ high The binary vector field value h is updated according to equation (7). b (ψ,θ):

[0024]

[0025] In the formula, h at time t b The values ​​of (ψ,θ), the hysteresis parameter (τ) low ,τ high It is related to the prior guidance waypoint, and its calculation method is shown in equation (8):

[0026]

[0027] In the formula, d f This parameter is related to the prior guidance waypoint, and is used to determine the waypoint distance. Let v be the minimum cruising speed of the unmanned helicopter, and v be the speed of the unmanned helicopter. The maximum acceleration for decelerating an unmanned helicopter; d τ The hysteresis length is a parameter typically related to the performance of unmanned helicopters. Then, if h... b If (ψ,θ) is 1, then the low-altitude terrain following waypoint (x) should be calculated along the flight direction (ψ,θ). f ,y f ,z f ) ψ,θ ,like Figure 7 The diagram shown is a schematic diagram of the flight waypoint calculation.

[0028] Construct the set of waypoints to be flown, D = {(x f ,y f ,z f ) ψ,θ Afterwards, to ensure the flyability of each waypoint, it is necessary to determine whether each waypoint meets the low-altitude terrain following altitude constraint, and further reduce the set. Figure 8 The diagram shows the flight waypoint altitude constraints. If a flight waypoint does not meet the terrain following altitude constraints, the point is removed from set D, which means that set D is reduced. Equation (9) is the low-altitude terrain following altitude constraint condition:

[0029]

[0030] In the formula, For (x) f ,z f ) ψ,θ Raster point height data, H mission To determine the maximum ground clearance for low-altitude terrain following by unmanned helicopters, the simplified set of waypoints D is finally obtained. - ;

[0031] (3) Calculate the optimal waypoint;

[0032] Theoretically, D -All the waypoints that should be flown can be regarded as the flight target waypoints of the unmanned helicopter at the current moment. In order to fly to the prior guidance waypoint as soon as possible, the optimal model of the waypoints should be constructed by combining the performance constraints of the unmanned helicopter and the prior guidance waypoint, as shown in Equation (10):

[0033]

[0034] In the formula, g1 is the climb slope cost factor, g2 is the glide slope cost factor, g3 is the turning radius cost factor, g4 is the target azimuth cost factor, g5 is the target pitch cost factor, and the weighting factor for the corresponding cost is ω. i For i = 1, ..., 5, finally, the optimal waypoint is solved using optimization methods.

[0035] In step (3), the climbing slope cost factor is:

[0036] When the slope of the ascent angle is k climb Greater than When g1 is defined, it is as shown in equation (11):

[0037]

[0038] In the formula, k climb =tanθ;

[0039] In step (3), the glide slope cost factor is:

[0040] When the slope of the glide slope k glide Greater than When g2 is defined, it is as shown in equation (12):

[0041]

[0042] In the formula, k glide =|tanθ|;

[0043] In step (3), the turning radius cost factor is:

[0044] When the turning radius r f When the threshold r0 is less than 0, g3 is defined as shown in equation (13):

[0045]

[0046] Where, r f The calculation formula is shown in equation (14), which defines:

[0047]

[0048] In the formula, ψ is the grid system azimuth of the waypoint;

[0049] In step (3), the target orientation cost factor is:

[0050] The degree of distance from the target location is used as a cost factor in the optimization. The unmanned helicopter will incur a greater cost when it is far from the target location, as shown in Equation (15):

[0051]

[0052] In the formula, δ α =|ψ f -ψ t |For the flight direction ψ f Relative target bearing ψ t Deviation;

[0053] In step (3), the target pitch cost factor is:

[0054] The degree of distance from the target point is used as a cost factor in the optimization. The helicopter moves along the azimuth and pitch, and the greater the pitch deviation, the greater the cost, as shown in Equation (16):

[0055]

[0056] In the formula, δ β =|θ f -θ t | Pitch θ in the direction of flight f Pitch θ relative to the target direction t The deviation.

[0057] In step (3), when solving the optimization method, the AHP-entropy weight method is used to determine each weight factor.

[0058] The beneficial effect of this invention is that it proposes a route guidance method for low-altitude terrain following of unmanned helicopters based on an improved VFH+, which addresses the problem of low-altitude terrain following for unmanned helicopters. First, based on UAV flight performance constraints and safe flight standards, a safe flight pipeline model for unmanned helicopters is designed. A principal value vector field histogram based on the gridded detection information from the airborne lidar is constructed. Second, to address the issue of partially missing gridded detection information due to obstacles, specular reflection, etc., a first-order estimation model is introduced to fill in the missing data, and the grid is expanded to ensure the safety of feasible flight directions. Then, based on the hysteresis update principle and the principal value vector field histogram, a binary vector field histogram is generated. Reasonable hysteresis parameters are set based on UAV performance constraints and prior route target information to further ensure the robustness of the algorithm. Finally, based on the hysteresis update parameters and prior route target information, a set of feasible waypoints is calculated. The set of feasible waypoints is reduced based on low-altitude terrain following altitude constraints. A feasible waypoint optimization model based on UAV performance and prior route target point constraints is designed, and the optimal waypoint is generated from the reduced set of waypoints. This invention extends the problem context of the traditional VFH+ algorithm from a two-dimensional plane to a three-dimensional space, effectively solving the problem of obstacle avoidance in three-dimensional space for unmanned helicopters during low-altitude terrain following. It also deeply considers the issue of ensuring safe flight for unmanned helicopters, constructing a safe flight path to fully guarantee the safety of optimal waypoints. Furthermore, considering the data gaps in gridded detection information, a first-order estimation model is used to fill in the missing data, further avoiding the risk of controlled ground collisions. A feasible waypoint optimization model is constructed, improving the platform's obstacle avoidance efficiency while ensuring unmanned helicopter flight safety and low-altitude terrain following. Attached Figure Description

[0059] Figure 1 This is a schematic diagram of an unmanned helicopter following the terrain at low altitude.

[0060] Figure 2 This is a schematic diagram of an unmanned helicopter following and avoiding obstacles at low altitudes.

[0061] Figure 3 This is a schematic diagram of the detection range layout of the airborne lidar on an unmanned helicopter.

[0062] Figure 4 This is a schematic diagram defining the safe flight path for unmanned helicopters.

[0063] Figure 5 This is a schematic diagram comparing the gridded detection information from airborne lidar with the safe flight path.

[0064] Figure 6This is a schematic diagram of a method for filling in missing raster elevation data.

[0065] Figure 7 This is a schematic diagram illustrating the calculation of waypoints for low-altitude terrain following.

[0066] Figure 8 This is a schematic diagram illustrating the altitude constraints for low-altitude terrain following of unmanned helicopters.

[0067] Figure 9 This is a flowchart of a method for guiding unmanned helicopters to follow low-altitude terrain. Detailed Implementation

[0068] The present invention will be further described below with reference to the accompanying drawings and embodiments.

[0069] This invention addresses the problem of low-altitude terrain following for unmanned helicopters under complex and adverse conditions, proposing a VFH+-based route guidance algorithm. First, a principal value vector field histogram is constructed based on airborne lidar data to characterize the terrain and obstacle distribution directly in front of the unmanned helicopter. Second, a binary vector field histogram is generated by reducing the unmanned helicopter's flight constraints, forming a solution space for the unmanned helicopter's flyable directions. Finally, combining the unmanned helicopter's flight performance constraints and discrete route guidance targets, the optimal waypoint for the unmanned helicopter is calculated. This algorithm effectively avoids unknown obstacles during route guidance under complex and adverse conditions, meeting the real-time computation requirements for low-altitude terrain following by unmanned helicopters.

[0070] In this embodiment, it is assumed that the prior waypoint is (ψ) nav ,θ nav ,r nav ), where ψ nav To guide the waypoint's orientation within the grid system, θ nav To guide the pitch of waypoints in the grid system, r nav The distance of the waypoint within the grid system.

[0071] The present invention adopts the following technical solution, and the implementation process is as follows:

[0072] (1) Calculate the principal value vector field histogram

[0073] In a grid system, the gridded detection information directly in front of the airborne lidar is: Among them, the element y in Y x,z (x = 1, ..., m; z = 1, ..., n) represents the height of a grid point (x, z) relative to the current flight plane of the unmanned helicopter. Based on the measurement resolution of the airborne lidar, the number of rows and columns in Y is taken as m = 100, n = 120. When a certain grid point Y... grid When axis data is missing, the corresponding element for that raster point is y. x,z =∞.

[0074] During flight, the unmanned helicopter must maintain a distance of R from the surrounding terrain or obstacles to the side. safe =25 or higher, the distance between the person directly below and the ground or obstacle must be maintained at R. down =5 or higher, and the top must be at the maximum ground clearance H. mission =30 remains in R up =3 or higher. This means the unmanned helicopter should fly between the terrain envelope and the maximum altitude envelope, while maintaining safe flight path constraints at distances to surrounding terrain and obstacles. Therefore, let the spherical coordinates of a grid point in the grid system be (ψ, θ, r), where ψ is the azimuth of the grid point relative to the origin, θ is the pitch of the grid point relative to the origin, and r is the distance relative to the origin, with all angles satisfying the right-hand screw rule. Further, the principal value vector field histogram is constructed as follows: Among them, H p The definition of the elements is shown in equation (17).

[0075]

[0076] In the formula, ψ max =90° is the maximum offset azimuth of the grid point, δ ψ =2ψ max / 10 represents the grid azimuth offset step, θ max =30° grid point maximum offset pitch, δ θ =2θ max / 6 represents the grid azimuth offset step. In a given direction, based on the definition of safe flight pipeline, it is determined whether all grid points affect the safe flight of the unmanned helicopter, as shown in Equation (18), which is the grid point safety criterion.

[0077]

[0078] In the formula, (x obs ,y obs ,z obs ) represents the coordinates of the grid points. In addition, since some elevation peaks may be filtered out after the terrain data is rasterized, the grid needs to be dilated. The dilated grid boundary needs to satisfy equation (18), and the dilated grid boundary is defined as shown in equation (19).

[0079]

[0080] In the formula, (x′ obs ,y′ obs ,z′ obs () represents the expanded raster boundary. D is the grid dilation radius. cell ×D cellThis is the expanded raster size, a parameter related to the original raster data resolution. In this example, D... cell =4m. Therefore, it is necessary to determine the boundary (x′) of each dilated grid in each discrete flight direction. obs ,y′ obs ,z′ obs Whether the safe flight path constraints are met, i.e., equation (18). For example... Figure 5 The figure shows the definition of the LiDAR information rasterized data in the raster system. In each flight direction (ψ,θ), the raster point closest to the origin is calculated according to the unmanned helicopter safe flight pipeline constraint, and it is recorded as the principal value in that flight direction, as shown in Equation (20).

[0081]

[0082] In the formula, r obs Let W be the distance of a grid point from the origin, and let W be the set of grid points in the given direction (ψ,θ) that do not satisfy equations (2)-(3).

[0083] When calculating the principal value vector field histogram, due to the influence of terrain undulations and obstacle occlusion, the lidar is unable to detect some grid cells, resulting in missing grid data. Therefore, to ensure the flight safety of unmanned helicopters, a first-order model is used to fill in the missing grid data, such as... Figure 6 The diagram illustrates a method for filling in missing raster data. Based on the definition of elements in the rasterized detection matrix Y of an airborne lidar, when a certain raster point data... If a raster point is missing, then data at that raster point needs to be filled to ensure the calculation of the vector field histogram. This invention uses a linear regression equation passing through the origin and the previous non-missing raster point to calculate the raster data at the missing point. Let the missing point be... Along the following straight line

[0084]

[0085] Find the previous non-missing obstacle grid point Then construct the straight line as shown in equation (21).

[0086]

[0087] Fill in the missing points according to the above formula. elevation data

[0088] (2) Calculate the histogram of the binary vector field.

[0089] When the principal values ​​h of obstacles in all flight directions are obtained p After (ψ,θ), construct the binary vector field histogram. Based on the requirements of low-altitude terrain following tasks, a binary vector field histogram hysteresis parameter (τ) is defined. low ,τ high The binary vector field value h is updated according to equation (23). b (ψ,θ).

[0090]

[0091] In the formula, h at time t b The values ​​of (ψ,θ). Hysteresis parameter (τ) low ,τ high ) is related to the prior guidance waypoint, and its calculation method is shown in Equation (24).

[0092]

[0093] In the formula, d f =r nav This refers to the distance to the waypoints. Let v be the minimum cruising speed of the unmanned helicopter, and v be the speed of the unmanned helicopter. The maximum acceleration for decelerating an unmanned helicopter; d τ This is the hysteresis length, a parameter typically related to the performance of unmanned helicopters. Then, if... If the value is 1, then the low-altitude terrain following waypoint (x) should be calculated along the flight direction (ψ,θ). f ,y f ,z f ) ψ,θ ,like Figure 7 As shown.

[0094] Construct the set of waypoints to be flown, D = {(x f ,y f ,z f ) ψ,θ Afterwards, to ensure the flyability of the waypoints, it is determined whether each waypoint meets the low-altitude terrain following altitude constraint, and the set is further reduced, such as... Figure 8 As shown in equation (25), the low-altitude terrain following height constraint condition is given.

[0095]

[0096] In the formula, For (x) f ,z f ) ψ,θ Raster point height data, H max The maximum ground clearance is determined for low-altitude terrain following by the unmanned helicopter. Finally, the reduced set of waypoints D is obtained. - .

[0097] (3) Calculate the optimal waypoint

[0098] Theoretically, D - All the waypoints that should be flown can be regarded as the current flight target waypoints of the unmanned helicopter. In order to fly to the prior guidance waypoint as soon as possible, this invention combines the performance constraints of the unmanned helicopter and the prior guidance waypoints to construct the optimal waypoint model, as shown in Equation (26).

[0099]

[0100] In the formula, g1 is the climb slope cost factor, g2 is the glide slope cost factor, g3 is the turning radius cost factor, g4 is the target azimuth cost factor, g5 is the target pitch cost factor, and the weighting factor for the corresponding cost is ω. i ,i=1,…,5. In this embodiment, the AHP-entropy weighting method is used to determine each cost factor.

[0101] 1) Climbing slope cost factor

[0102] When the slope of the ascent angle is k climb Greater than When, g1 is defined as shown in equation (27).

[0103]

[0104] In the formula, k climb =tanθ.

[0105] 2) Glide slope cost factor

[0106] When the slope of the glide slope k glide Greater than When g2 is defined, it is as shown in equation (28):

[0107]

[0108] In the formula, k glide =|tanθ|.

[0109] 3) Turning radius cost factor

[0110] When the turning radius r f When the threshold r0 = 200 is less than the threshold, g3 is defined as shown in equation (29).

[0111]

[0112] Where, r f The calculation formula is defined as shown in equation (30):

[0113]

[0114] In the formula, ψ represents the grid orientation of the waypoint.

[0115] 4) Target orientation cost factor

[0116] The degree of distance from the target location is used as a cost factor in the optimization. The unmanned helicopter will incur a greater cost when it is far from the target location, as shown in Equation (31).

[0117]

[0118] In the formula, δ α =|ψ f -ψ nav |For the flight direction ψ f Relative target bearing ψ nav The deviation.

[0119] 5) Target pitch cost factor

[0120] The degree of distance from the target point is used as a cost factor in the optimization. The helicopter moves along the azimuth and pitch, and the greater the pitch deviation, the greater the cost, as shown in Equation (32).

[0121]

[0122] In the formula, δ β =|θ f -θ nav | Pitch θ in the direction of flight f Pitch θ relative to the target direction nav The deviation. Finally, this embodiment uses an exhaustive search method to find the optimal waypoint.

[0123] In summary, the implementation steps of the unmanned helicopter low-altitude terrain-following route guidance method proposed in this invention are as follows:

[0124] Step 1: Based on the gridded data Y from the airborne lidar and the unmanned helicopter's safe flight path, construct the principal value vector field histogram H. p ;

[0125] Step 2: Based on the hysteresis parameter (τ) low ,τ high Construct a binary vector field histogram H b It generates the set of waypoints D, and reduces it by combining low-altitude terrain and following the maximum flight altitude to generate D. - ;

[0126] Step 3: Determine the set of waypoints D - And based on the optimal waypoint optimization model, the optimal waypoint is solved.

[0127] like Figure 9 The diagram shows a flowchart of a low-altitude terrain-following route guidance method for unmanned helicopters.

Claims

1. A method for guiding an unmanned helicopter along a low-altitude terrain-following flight path, characterized in that... Includes the following steps: (1) Calculate the principal value vector field histogram; Define an airborne lidar grid coordinate system, or simply the grid system, with the origin of the grid system being the current spatial position of the unmanned helicopter. The positive axis is the direction of the projection of the unmanned helicopter's velocity onto the horizontal plane. The positive direction of the axis is perpendicular to The axis is vertically upward. The positive direction of the axis satisfies the right-hand screw rule. The detection range of the airborne lidar on the unmanned helicopter is within a grid system, and the detection range is a rectangular area with a width of [missing information]. The length of the rectangle is The detection data acquired by the airborne lidar, after being rasterized, becomes... A discrete point; Therefore, let the gridded detection data directly in front of the airborne lidar be... OK Column matrix , medium elements Represents grid points The altitude relative to the current flight plane of the unmanned helicopter , When a certain grid point When axis data is missing, the element corresponding to that grid point is: ; During flight, the unmanned helicopter must maintain a safe distance from the surrounding terrain or obstacles to the side. Above, the distance directly below must be maintained from the ground or obstacles. The above must be directly above the maximum ground clearance. Stay The above means that the unmanned helicopter should fly between the terrain envelope and the maximum altitude envelope, while the distance from the surrounding terrain and obstacles should meet the constraints of the safe flight path; therefore, let the spherical coordinates of the grid points in the grid system be... , This represents the orientation of a grid point relative to the origin. The pitch of the grid points relative to the origin. The distance relative to the origin, and the angle direction all satisfy the right-hand screw rule. Further construction of the principal value vector field histogram is as follows: ,in, The definition of the element is shown in equation (1): （1） In the formula, The maximum offset azimuth of the grid point. This represents the grid azimuth offset step. Maximum offset pitch for grid points The grid pitch offset step is used; in a given direction, all grid points are judged according to the definition of safe flight pipeline to determine whether they affect the safe flight of the unmanned helicopter, as shown in Equation (2) as the grid point safety criterion. （2） In the formula, The coordinates of the grid points are given. In addition, since some elevation peaks may be filtered out after the terrain data is rasterized, the grid needs to be dilated. The dilated grid boundary needs to satisfy equation (2), and the dilated grid boundary is defined as shown in equation (3). （3） In the formula, This represents the expanded grid boundary. The radius of the grid expansion is 1. To determine the dilated grid size, the boundary of each dilated grid needs to be determined in each discrete flight direction. Whether the safe flight path constraint is met, i.e., equation (2), in each flight direction Based on the unmanned helicopter's safe flight pipeline constraints, the grid point closest to the origin is calculated and recorded as the principal value in that flight direction, as shown in equation (4): （4） In the formula, The distance of a grid point from the origin. Given direction The set of grid points that do not satisfy equations (2)-(3); To ensure the flight safety of unmanned helicopters, a first-order model is used to fill in the missing grid data, based on the gridded detection matrix of the airborne lidar. The element definition in the middle is when a certain grid point data If a raster point is missing, then it is necessary to fill in the missing data to ensure the calculation of the vector field histogram. The raster data for the missing point is calculated using the linear regression equation of the origin and the previous non-missing raster point. Let the missing point be... along a straight line （5） Find the previous non-missing obstacle grid point Then, the equation of the straight line as shown in equation (6) is constructed: （6） Fill in the missing points according to formula (6). elevation data ; (2) Calculate the histogram of the binary vector field; When the principal values ​​of obstacles in all flight directions are obtained Then, construct a binary vector field histogram. Based on the requirements of low-altitude terrain following missions, a binary vector field histogram hysteresis parameter is defined. Update the binary vector field value according to equation (7). : （7） In the formula, for time The value of the hysteresis parameter It is related to the prior guidance waypoint, and its calculation method is shown in Equation (8): （8） In the formula, This parameter is related to the prior guidance waypoint, and is used to determine the waypoint distance. This is the minimum cruising speed for unmanned helicopters. For the speed of unmanned helicopters, The maximum acceleration for decelerating unmanned helicopters; The hysteresis length is a parameter typically related to the performance of unmanned helicopters; then, if If the value is 1, then the flight direction is calculated. Follow the low-altitude terrain and fly to the appropriate waypoints. ; Construct a set of waypoints to fly Next, to ensure the flyability of each waypoint, it is necessary to determine whether each waypoint meets the low-altitude terrain-following altitude constraint, and further reduce the set. If a waypoint does not meet the terrain-following altitude constraint, it is removed from the set. Remove that point from the set, that is, remove the point from the set. After reduction, equation (9) represents the low-altitude terrain following height constraint: （9） In the formula, for Grid point height data at the grid. To determine the maximum ground clearance for low-altitude terrain following by unmanned helicopters, the simplified waypoint assembly is finally obtained. ; (3) Calculate the optimal waypoint; In theory, All the waypoints that should be flown can be regarded as the flight target waypoints of the unmanned helicopter at the current moment. In order to fly to the prior guidance waypoint as soon as possible, the optimal waypoint model is constructed by combining the performance constraints of the unmanned helicopter and the prior guidance waypoint factors, as shown in Equation (10): （10） In the formula, The climbing slope cost factor, The slope cost factor is the glide slope. The turning radius cost factor. The target orientation cost factor. The target pitch cost factor, and the corresponding cost weighting factor are: Finally, the optimal waypoint is solved using optimization methods. .

2. The method for guiding unmanned helicopters to follow low-altitude terrain as described in claim 1, characterized in that: In step (3), the climbing slope cost factor is: When the slope of the ascent angle Greater than hour, The definition is shown in equation (11): （11） In the formula, .

3. The method for guiding unmanned helicopters to follow low-altitude terrain as described in claim 1, characterized in that: In step (3), the glide slope cost factor is: When the slope of the glide slope Greater than hour, The definition is shown in equation (12): （12） In the formula, .

4. The method for guiding unmanned helicopters to follow low-altitude terrain as described in claim 1, characterized in that: In step (3), the turning radius cost factor is: When turning radius Less than the threshold hour, The definition is shown in equation (13): （13） in, The calculation formula is defined as follows (14): （14） In the formula, This refers to the orientation of the waypoint relative to the origin in the grid system.

5. The method for guiding unmanned helicopters to follow low-altitude terrain as described in claim 1, characterized in that: In step (3), the target orientation cost factor is: The degree of distance from the target location is used as a cost factor in the optimization. The unmanned helicopter will incur a greater cost when it is far from the target location, as shown in Equation (15): （15） In the formula, For the direction of flight Relative target bearing The deviation.

6. The method for guiding unmanned helicopters to follow low-altitude terrain as described in claim 1, characterized in that: In step (3), the target pitch cost factor is: The degree of distance from the target point is used as a cost factor in the optimization. The helicopter moves along the azimuth and pitch, and the greater the pitch deviation, the greater the cost, as shown in Equation (16): （16） In the formula, Pitch in the direction of flight Pitch relative to the target direction The deviation.

7. The method for guiding unmanned helicopters to follow low-altitude terrain as described in claim 1, characterized in that: In step (3), when solving the optimization method, the AHP-entropy weight method is used to determine each weight factor.

Images