Low-altitude flight path planning method based on grid coding

Abstract

The invention discloses a low-altitude flight path planning method based on grid coding, which relates to the technical field of path planning, and is characterized in that on the basis of space and time grid coding, an A * path planning algorithm is improved, a neighbor node range in a classical algorithm is expanded, pruning optimization for removing invalid neighbors is applied to the A * path planning algorithm, and the path planning efficiency is improved. Relatively fast searching and establishment of a smooth grid path meeting the motion constraint become possible; an algorithm for interpolating a track on a grid path at equal time intervals is introduced; comprising the following steps of: thinning and smoothing a grid path; carrying out mathematical modeling on a flight state of the unmanned aerial vehicle; calculating flight segment initial and final speeds and flight segment middle cruising speeds which meet the motion performance constraint of the unmanned aerial vehicle and the consistency between flight segments by a backtracking iteration method; and calculating specific flight path points by speed integration. And a relatively accurate unmanned aerial vehicle flight plan track prediction generation method is provided.

Description

Technical Field

[0001] This invention belongs to the field of path planning technology, specifically relating to a low-altitude flight path planning method based on grid coding. Background Technology

[0002] Low-altitude economy generally refers to a three-dimensional economic activity system that relies on airspace resources below 1,000 meters (and may extend to below 3,000 meters in some areas) and uses low-altitude aircraft as carriers to serve urban logistics, inspection and patrol, and passenger transportation. It is a low-altitude flight service industry that provides commercial and regular operations to the whole society.

[0003] As the commercial operation of low-altitude drones gradually matures in the future, the number of daily low-altitude drone take-offs and landings will increase significantly, and low-altitude airspace resources will become increasingly scarce. The low-altitude airspace in cities may even become as busy and congested as roads on the ground. The low-altitude airspace will exhibit traffic flow characteristics of large drone traffic, high density, and diverse flight performance and control methods. The huge demand for low-altitude flights places higher demands on drone safety assurance.

[0004] Faced with the rapidly increasing daily takeoffs and landings under limited low-altitude airspace resources, the traditional air traffic control model mentioned above is inadequate to adapt and match the demands. Therefore, in order to improve the ability to pre-identify potential safety risks in low-altitude airspace, countries have reached a consensus on using air traffic control platforms to conduct refined and comprehensive pre-emptive control of low-altitude drone flight activities.

[0005] Figure 1 This is a flowchart illustrating the approval process for low-altitude unmanned aerial vehicles (UAVs) by the new generation air traffic control platform. In this process, the UAV's flight plan is considered a crucial basis for directly reflecting the UAV's flight intentions and predicting its future trajectory. It is a pre-flight plan determined by the UAV and typically includes a plan ID, planned takeoff time, flight origin and destination, planned waypoints or pathpoints, UAV model, maximum speed, and other flight performance information. Figure 2 As shown, flight plans can, to some extent, model the process of future flight activities.

[0006] exist Figure 1In the approval process, as shown in blue on the left, flight plans play a major role in the pre-flight planning review stage. The low-altitude UAV air traffic control platform needs to assess the potential risks between each flight plan and other plans. Specifically, this involves using information within the plan for effective trajectory prediction, interval calculation, and conflict detection, thereby fully exploring and utilizing flight plan information to lay the decision-making foundation for subsequent flow control and coordination among plans. Assuming the number of flight plans in this review stage is N, the number of future time slots to be calculated is T, and the average cost of calculating the interval relationship between two flight plans at a given time is M, then the most direct approach is to calculate the interval between all flight plans pairwise. As can be intuitively seen from the previous formula, when the number of flight plans N increases significantly, the complexity of interval calculation will rise rapidly. If the complexity M of a single calculation is too high, the overall complexity cost of the calculation will also become unacceptable. Summary of the Invention

[0007] To address the technical shortcomings of the aforementioned pre-flight planning conflict detection task for low-altitude UAVs, this invention proposes a low-altitude flight path planning method based on grid coding.

[0008] A low-altitude flight path planning method based on grid coding is proposed. First, a continuous airspace is discretized into coding units through grid modeling to provide a structured representation for path planning. Then, the velocity state change nodes on each flight segment are calculated using an iterative backtracking method under dynamic constraints. Finally, the precise position and velocity state at equal time intervals are calculated using velocity integrals to form a spatiotemporally continuous flight trajectory, thereby completing the path planning.

[0009] Preferably, the gridded modeling method discretizes a continuous spatial domain into coding units, including the following steps:

[0010] Step A1: Determine the gridding range and the grid coding level. The gridding range can be determined based on the geographical range of the triangular mesh model, and the grid coding level determines the size of each grid.

[0011] Step A2: Construct the acceleration structure and index the vertices of the triangular facets using mesh encoding;

[0012] Step A3: Traverse each grid within the meshed area and perform ray emission and intersection detection;

[0013] Step A4: Determine the position of the mesh based on the number of intersections between the ray and the model: if the number of intersections is odd, the center of the mesh is located inside the model; if the number of intersections is even, the center of the mesh is located outside the model.

[0014] Step A5: Reorganize the data and save whether each grid corresponds to the underlying surface obstacle in the triangular mesh model.

[0015] Preferably, an improved A* path planning algorithm is used to optimize the neighbor node search space in the expansion phase, and invalid neighbor nodes are pruned while increasing the step size.

[0016] Let (x,y,z) be the 3D sequence coordinates of the current node's mesh, used to represent the relative position of the current mesh; (dx,dy,dz) is used to represent the axial mesh sequence difference between the neighboring node mesh and the current node mesh.

[0017] Preliminary pruning strategies include:

[0018]

[0019] Further pruning strategies include:

[0020] (1) Prune according to motion performance constraints. Consider the previous source node of the current node. If the line segment formed by the source node, the current node and the neighbor node exceeds the motion capability of the UAV, remove the neighbor node.

[0021] (2) Remove unreachable neighbor nodes based on the underlying surface environment. If a neighbor node is outside the environment, pruning is required. If a neighbor node is an obstacle, pruning is required. If a neighbor node is already in the closed set, it means that the node has been calculated and pruning is required.

[0022] (3) Remove neighboring nodes that cannot be reached by sight based on the underlying surface environment, and determine whether the current node is visible to neighboring nodes in the local search space. Specifically, construct a local mesh tensor from the current mesh to the neighboring mesh, and determine whether the neighboring node is reachable by detecting the tensor.

[0023] Preferably, based on the path planning results, trajectory interpolation is performed:

[0024] First, the grid sequence obtained from path planning is thinned out by deleting invalid grid points with the same turning angle;

[0025] Next, spatial location-based interpolation smoothing is performed to further optimize the jagged edges of the grid path. Then, the initial speed, intermediate cruising speed, and final speed are planned between each pair of path points, i.e., on each flight segment. Finally, the backtracking iteration method is used to ensure the continuity of speed between flight segments, and the position and heading of the UAV at equal time intervals are calculated using speed integrals. This completes the construction of the refined track corresponding to the grid sequence, which facilitates subsequent interval calculations.

[0026] Preferably, the backtracking iterative method for calculating the velocity profile includes the following steps:

[0027] First, position interpolation is performed on the large-angle turns on the grid path using the circular motion model from the previous section. The interpolation calculates the turning radius based on the maximum tilt angle and maximum turning speed allowed when the UAV turns, and the corresponding position of the turning circular trajectory grid is re-inserted into the grid path sequence.

[0028] Next, the three-stage velocity model for each flight segment is calculated, namely the initial velocity, intermediate velocity, and final velocity of each flight segment, as well as the time and distance from the initial velocity to the intermediate velocity, the flight time and distance at the intermediate velocity, and the time and distance from the intermediate velocity to the final velocity. The vertical and horizontal directions are calculated separately when calculating the three-stage velocity model, but time coordination in the two directions is required, and the velocity between all adjacent flight segments is ensured to be continuous through a backtracking iteration mechanism.

[0029] Preferably, time coordination involves replanning the three-stage velocity model in the flight direction with shorter flight time, so that the flight time in the two directions tends to be consistent; the inputs for time coordination calculation are the initial initial velocity v0 and the initial final velocity v0. t Planned flight time (E), flight distance (D), maximum speed constraint (v) max Maximum acceleration constraint a max The output is the calculated feasible intermediate velocity v. mid The time [T1,T2,T3] and displacement [S1,S2,S3] for each of the three stages.

[0030]

[0031] Preferably, if the new initial velocity is not equal to the initial initial velocity before time coordination, or if the need to adjust the initial velocity for backtracking iteration has already occurred when calculating the initial three-stage velocity model, then the initial terminal velocity of the previous segment needs to be modified accordingly, and then backtracking to the previous segment to recalculate the previous segment; if the new terminal velocity is not equal to the initial velocity before time coordination, then the initial initial velocity of the next segment needs to be modified accordingly, and the calculation is performed based on the new value when iterating to the next segment.

[0032] Finally, the entire backtracking iterative method can converge and calculate all segments. The initial velocity, intermediate velocity, and terminal velocity of all segments are successfully calculated. The initial and terminal velocities between segments are perfectly matched and continuous. The time and distance of each of the three stages are also obtained at the same time.

[0033] Preferably, after the three-stage velocity model is established for each flight segment, the position of the UAV at each moment is obtained by performing velocity integration at a certain time interval based on the known velocity changes on each flight segment. The magnitude and direction of the velocity at each position are determined, thereby finally establishing a complete velocity profile, horizontal position profile, elevation position profile, and heading profile, and calculating the trajectory at equal time intervals.

[0034] Preferably, the velocity integral calculation of equal time interval track points includes the following steps:

[0035] Step B1: Establish a time reference system within the flight segment and calculate the cumulative time prefix sum of the flight segment. Where T j The flight time for the j-th segment is based on the total flight time T for all segments. total =T sum (n) and time step Δt generate a uniformly distributed time series [0, Δt, 2Δt, ..., T]. total ];

[0036] Step B2, calculate the position of the time within the flight segment, from the time series [0, Δt, 2Δt, ..., T]. total Each time point t extracted from ] i Perform the following operations: locate t i The flight segment k satisfies

[0037] T sum (k)≤t i ≤T sum (k+1), calculate t i The relative time t within flight segment k seg =t i -T sum (k);

[0038] Step B3: Calculate the velocity and displacement at time t. seg Within flight segment k, which motion segment in the three-stage velocity model does it belong to: In the horizontal direction, j∈0,1,2, satisfying... In the vertical direction, for l∈0,1,2, the following condition is met: in and These are the prefix sums of the times taken for the three stages of the horizontal and vertical flight segments, respectively.

[0039] According to t seg Calculate the horizontal velocity v based on the position in the three-stage velocity model. hor (t) and displacement S hor (t):

[0040] (a) Phase 1,

[0041]

[0042] (d) Stage 2, j = 1:

[0043] (c) Stage 3, j = 2:

[0044]

[0045] Vertical velocity v ver (t) and displacement S ver (t) The calculation process is consistent with the horizontal direction;

[0046] Step B4, 3D position and velocity synthesis:

[0047] Based on the horizontal displacement S hor and direction unit vector Calculate the horizontal position component Wherein, the direction unit vector It is calculated directly from the start and end points of the flight segment; based on the vertical displacement S ver and direction d ver Calculate the vertical position component P ver =S ver ·d ver , where direction d ver A value of 1 indicates vertically upward, and -1 indicates vertically downward;

[0048] Synthetic 3D Position in It is the starting position of the current flight segment; horizontal velocity vector. Vertical velocity component V ver =v ver (t)·d ver Three-dimensional velocity vector

[0049]

[0050] Step B5: Organize all data, including the position, speed, and heading of all waypoints according to time, to form an equally timed waypoint corresponding to a flight plan.

[0051] This invention improves the A* path planning algorithm by expanding the range of neighbor nodes in the classic algorithm and applying pruning optimization to remove invalid neighbors, making it possible to establish a smooth grid path that satisfies motion constraints relatively quickly. It introduces an algorithm for interpolating tracks on the grid path at equal time intervals, including thinning and smoothing the grid path, mathematical modeling of the UAV flight state, backtracking iterative calculation of the initial and final velocities and intermediate cruising speeds of the flight segment that satisfy UAV motion performance constraints and inter-segment consistency, and velocity integral calculation of specific track points. A relatively accurate method for predicting and generating UAV flight plan tracks is presented. Attached Figure Description

[0052] Figure 1 The diagram illustrates the Z-order filling curve, where the left side represents a two-dimensional scene and the right side represents a three-dimensional scene.

[0053] Figure 2 A schematic diagram of the binary bit interleaving method for Morton coding;

[0054] Figure 3 This is a schematic diagram of the height field encoding method;

[0055] Figure 4 This is a schematic diagram of the latitude and longitude encoding bits for GeoSOT-3D.

[0056] Figure 5 This is a schematic diagram of the time coding structure, where Figure 5 (a) is the date portion. Figure 5 (b) is the time portion;

[0057] Figure 6 This is a schematic diagram of the time-partitioned binary tree structure for levels 17 to 21.

[0058] Figure 7 This is a schematic diagram of the neighborhood grid of the grid;

[0059] Figure 8 A schematic diagram illustrating the classification of three-dimensional adjacent topological relationships;

[0060] Figure 9 Example diagram of a triangular mesh model that retains only location information;

[0061] Figure 10 This is a schematic diagram of the 26 neighboring nodes of a 3D mesh, where the left side is the 3D view and the right side is the XY plane, i.e., the top view;

[0062] Figure 11 This is a schematic diagram of neighbor nodes in a 3D mesh with a step size of 4.

[0063] Figure 12 It is a 4*4*3 local tensor with no internal obstructions, where the current mesh is green and the neighboring mesh to be detected is blue;

[0064] Figure 13 This is a 4x4x3 local tensor containing obstacles, where the obstacle mesh is red;

[0065] Figure 14 A diagram illustrating the coordination effect of flight time in both horizontal and vertical directions. Detailed Implementation

[0066] The present invention will now be described in further detail with reference to the accompanying drawings and specific embodiments. The embodiments of the present invention are given for illustrative and descriptive purposes only, and are not intended to be exhaustive or to limit the invention to the forms disclosed. Many modifications and variations will be apparent to those skilled in the art. The embodiments were chosen and described to better illustrate the principles and practical application of the invention, and to enable those skilled in the art to understand the invention and design various embodiments with various modifications suitable for a particular purpose.

[0067] The low-altitude flight path planning method disclosed in this invention is based on grid coding; therefore, the spatial and temporal grid coding methods must first be constructed. This part is not the core technology of this invention, but it is the foundation of path planning, and is described below.

[0068] I. Spatial Grid Coding

[0069] Regarding spatial grid coding, partitioned grid coding technology, due to its unique discretization and hierarchical representation capabilities, has become a key technical approach for addressing the challenges of low-altitude airspace management and low-altitude unmanned aerial vehicle (UAV) management. Through spatiotemporal discretization coding, partitioned grid coding achieves excellent spatiotemporal data organization, accelerates index lookup speed, and, to some extent, avoids complex spatiotemporal relationship calculations by utilizing coding computation.

[0070] The two main foundations of existing spatial coding are partitioning rules and coding rules. The rules for mesh partitioning determine the shape of the mesh, the scale and size of each layer, and the hierarchical organization of space. Partitioned mesh coding often involves multiple levels with hierarchical relationships between them. The correspondence between different levels also determines the spatial index structure used when building an index using mesh coding, such as quadtrees, octrees, and R-trees. The rules for mesh coding determine the order of the mesh codes and the relative spatial positions of the corresponding meshes. To facilitate spatial querying and management, coding rules are often designed to ensure that the data before and after coding maintain a certain degree of proximity in space. Therefore, the most common coding rule is to use space-filling curves for spatial coding, such as Z-order curves, Hilbert curves, and Peano curves.

[0071] This invention primarily employs Z-order coding based on quadrilateral grids, namely Morton coding. This coding method is a spatial coding approach based on binary interleaving. Its core idea lies in converting the coordinates of multi-dimensional spatial points into binary form and generating a one-dimensional code through interleaving and merging. See [link to relevant documentation]. Figure 1 , Figure 2 The encoding and decoding process of Z-order encoding is relatively simple, making it easy for computers to process efficiently through bitwise operations, and adjacent codes usually correspond to nearby points or regions in space.

[0072] When using Z-order encoding for coordinates (x, y, z) in a decimal Cartesian three-dimensional coordinate system, the corresponding binary coordinates (x, y, z) are first obtained. (2) ,y (2) ,z (2) The subsequent Morton crossover process can be represented as follows: The encoding method first converts the three coordinate components into binary form, ensuring that the number of bits in each component is consistent. Any insufficient bits are padded with 0s. Then, the bits are interleaved to generate a new encoded value. During this process, the number of bits N for each component's encoding must satisfy the following condition to guarantee sufficient bits to encode the maximum coordinate value 2. N -1≥max{x,y,z}.

[0073] If the 3D coordinates are not given in decimal units, but in the form of (longitude, latitude, geodetic height), then GeoSOT-3D Earth Mesh Encoding can be used. The main difference between GeoSOT-3D and the encoding mentioned above is that it performs special processing on data such as latitude and longitude stored in degrees, minutes, and seconds, and specifies the levels corresponding to different encoding bit lengths. The advantage of this is that the octal codes converted from different levels of mesh encoding correspond to the parent-child octree relationship between different levels of mesh.

[0074] GeoSOT-3D encodes three coordinate quantities of Earth: longitude, latitude, and altitude, based on the geographic reference frame and reference ellipsoid parameters of the CGCS2000 national geodetic coordinate system. Each dimension is encoded with 32 bits, resulting in a 96-bit encoding. Because latitude and longitude are represented in a sexagesimal system (degrees, minutes, seconds), GeoSOT-3D needs to expand the latitude and longitude system to ensure that each grid level conforms to the Morton coding Z-order. The overall longitude range of the Earth is expanded from [-180°, 180°] to [-512°, 512°], and the latitude range is expanded from [-90°, 90°] to [-512°, 512°]. Integer degrees are expanded from 1° to 64′, and integer minutes are expanded from 1′ to 64″. This has the advantage of allowing degrees, minutes, and seconds to be encoded separately, but it may result in some codes existing but not corresponding to specific geographic locations. Furthermore, for the height component, to maintain the approximate hexahedral shape of the corresponding grid, the height's span at each level is approximately related to the latitude and longitude spans. Therefore, at level 0, the height span is set to approximately the surface distance corresponding to a longitude span of 512° at the Earth's equator. Subsequent levels can be divided in half accordingly. See [link to relevant documentation]. Figure 3 .

[0075] GeoSOT-3D uses a three-dimensional octree for hierarchical decomposition. At each level, longitude and latitude are binary-coded into four parts: degrees, minutes, seconds (integer), and seconds (decimal). Each part is then padded with bits to ensure sufficient encoding. These padded parts are then concatenated to obtain a single binary code. Specifically, the degree part is padded to 9 bits, the minute part to 6 bits, the integer part of the second to 6 bits, and the decimal part of the second to 10 bits. Figure 4 As shown. The height component needs to calculate the binary code corresponding to the current height dimension based on the selected coding level. Binary encoding is performed recursively according to the size of each height interval. One bit of the converted binary height code roughly corresponds to the distance of 1 / 2048″ of longitude span at the equator, thus aligning with the granularity of the longitude and latitude components. The binary codes of height, latitude, and longitude are sequentially arranged in a Morton cross-interval to obtain the GeoSOT-3D mesh code.

[0076] The specific calculation process for encoding latitude, longitude, and altitude coordinates (lon, lat, h) is shown below, where the subscript deg indicates integer degrees in latitude and longitude, the subscript min indicates integer minutes, the subscript sec indicates the integer part of seconds, the subscript secFrc indicates the fractional part of seconds, R is the average radius of the Earth, and bin(·) indicates converting the data into a 32-bit binary number.

[0077] code lon =bin((lon) deg <<22)|(lonmin <<16)|(lon sec <<6)|lon secFrc )

[0078] code lat =bin((lat deg <<22)|(lat min <<16)|(lat sec <<6)|lat secFrc )

[0079]

[0080] Below is an example of encoding conversion. The input coordinates are (121°0′0″, 31°0′0″, 0m), the target encoding level is 9, and the longitude component is calculated as a 32-bit binary number.

[0081] The code 00111100100000000000000000000000 represents the following binary code: Latitude component 00001111100000000000000000000000, Altitude component 0000000000000000000000000000000000000000. Morton crossover is then performed to obtain the final binary code: 00000000 1001011011010010 0110000 00000000 00000000 00000000 00000000 00000000 000000000 0000000000000000 00000000 corresponds to the decimal value 182332340791028077817757696 and the octal code 001133223.

[0082] For each level of GeoSOT-3D mesh encoding, the numerical order of the mesh codes follows a Z-order spatial arrangement. If each level's mesh code is represented as an octal code, and the code length is directly taken as the size of the corresponding level, then from... Figure 6 It can be clearly seen that the grid codes have a parent-child relationship in adjacent layers. Each parent grid is divided into 8 sub-grids, and each sub-grid code uses the parent grid code as a prefix.

[0083] Table 1 shows the grid coding range and corresponding geospatial scale corresponding to different grid coding levels. It can be seen that the advantage of GeoSOT-3D is that it designs corresponding grid levels for integer degrees, integer minutes, and integer seconds. The scale between adjacent grid levels in a single dimension conforms to a power of 2 relationship.

[0084] Table 1

[0085] hierarchy Grid size Approximate scale near the equator hierarchy Grid size Approximate scale near the equator 0 512° worldwide 1 <![CDATA[256 ° ]]> 1 / 4 Earth 17 16" 512 meters 2 <![CDATA[128 ° ]]> 18 8" 256 meters 3 <![CDATA[64 ° ]]> 19 4" 128 meters 4 <![CDATA[32 ° ]]> 20 2" 64 meters 5 <![CDATA[16 ° ]]> 21 1" 32 meters 6 <![CDATA[8 ° ]]> 1024 kilometers 22 1 / 2" 16 meters 7 <![CDATA[4 ° ]]> 512 kilometers 23 1 / 4" 8 meters 8 <![CDATA[2 ° ]]> 256 kilometers 24 1 / 8" 4 meters 9 <![CDATA[1 ° ]]> 128 kilometers 25 1 / 16" 2 meters 10 32' 64 kilometers 26 1 / 32" 1 meter 11 16' 32 kilometers 27 1 / 64" 0.5 meters 12 8' 16 kilometers 28 1 / 128" 25 cm 13 4' 8 kilometers 29 1 / 256" 12.5 cm 14 2' 4 kilometers 30 1 / 512" 6.2 cm 15 1' 2 kilometers 31 1 / 1024" 3.1 cm 16 32" 1 kilometer 32 1 / 2048" 1.5 cm

[0086] II. Time Grid Coding

[0087] Regarding time grid coding, Unix timestamps are the most basic time coding method. They have a single coding level, a time resolution of 1 second, and record the number of seconds elapsed since 00:00:00 UTC on January 1, 1970. The code length is fixed at 32 bits. If the first bit is used to indicate positive or negative, its time span covers from 20:45:52 on December 13, 1901 to 03:14:07 on January 19, 2038.

[0088] When time data is not presented directly in seconds, but exists as a combination of multiple dimensions such as year, month, day, hour, minute, and second, in order to ensure that date and time information is accurately expressed during the encoding process, the encoding mechanism of GeoSOT-3D can be used as a reference to independently encode each time measurement unit and construct a complete date and time encoding system accordingly.

[0089] Taking the date format YMD h:m:s.SmSu as an example, where Y represents the year, M represents the month, D represents the day, h represents the hour, m represents the minute, s represents the second, Sm represents the millisecond, and Su represents the microsecond, the encoding process for this time is similar to the latitude and longitude encoding in GeoSOT-3D encoding. Simply convert each component to binary form, pad each component with a fixed number of binary bits, and then concatenate them sequentially. The number of bits for each component must follow Formula 2. N -1 ≥ max{x,y,z}, the specific encoding bit length should be determined according to the following criteria:

[0090] 1) The number of digits in the year code can be determined as needed. For example, it can be selected as 17 digits, which can represent the years from 65536 BC to 65535 AD.

[0091] 2) The month code is set to 4 digits, with 0, 13, 14, and 15 being invalid month codes;

[0092] 3) The date encoding should be 5 digits, where 0 represents an invalid date;

[0093] 4) The hour code is set to 5 bits, with 24 to 31 being invalid hour codes;

[0094] 5) The minute encoding is set to 6 bits, with bits 60-63 being invalid minute encodings;

[0095] 6) The number of bits for the second encoding is 6, of which 60-63 are invalid second encodings;

[0096] 7) The encoding bit length for milliseconds is chosen to be 10 bits, where 1000 to 1023 are invalid millisecond codes;

[0097] 8) The encoding of microseconds can be determined according to requirements. If microsecond time scales are required, the number of bits can be selected as 10.

[0098] Bits, of which 1000 to 1023 are invalid microsecond codes.

[0099] If the encoding is performed according to the above bit length rules, with the first binary bit left empty, the final result will be a total of 64 bits of time code. The specific structure of the encoding is as follows: Figure 5 As shown. In practice, the range of encoded components and the number of bits can be adjusted according to specific needs. For example, in the scenario of conflict detection for low-altitude UAV flight plan pre-approval, the calculated data range is usually within one week, and the minimum time resolution is milliseconds, i.e., the time format used is D:h:m:s:ms. In this case, only encoding from date to millisecond is needed, resulting in a final encoding of 31 bits. Leaving the first bit empty is also suitable for directly storing the data using a 32-bit unsigned integer type (int). However, for the sake of broad applicability, the following explanation will still use 64-bit encoding as an example.

[0100] This encoding process is equivalent to expanding time units such as months, days, and seconds using a base system. For example, it expands a year from 12 months to 24 = 16 months, an hour from 60 minutes to 25 = 32 minutes, a minute from 60 seconds to 26 = 64 seconds, a second from 1000 milliseconds to 210 = 1024 milliseconds, and a millisecond from 1000 microseconds to 210 = 1024 microseconds. Each additional bit in the encoding represents a further binary refinement of the time granularity. Therefore, the encoding process itself can directly construct a binary tree structure, such as... Figure 6 As shown.

[0101] The aforementioned time encoding suffers from a problem similar to GeoSOT-3D encoding: a single encoding result cannot clearly indicate its encoding level. Due to padding operations, encoding results at different levels may exhibit consistency. Unlike GeoSOT-3D encoding, which employs a variable-length octal encoding strategy, this invention chooses to add interval bits and padding bits to the time encoding to avoid the need for introducing additional variables to record the encoding level, directly reflecting its level information through the encoding itself.

[0102] For a code with encoding level L, the lower 64-L bits can be marked as binary 1s; this part is called the supplementary bits, while the encoded part consists of the valid bits. Therefore, code can be represented as code′=(i L-1 i L-2 …i1i0)0(1 62-L 130-L-1 …1110). The 0s in the middle of the code′ are used as separator bits to distinguish between valid bits and padding bits. For any N-bit L-level code, the operation of expanding the code by adding padding bits can be expressed as code′=code|(1<<(NL)-1).

[0103] By inserting a gap bit between the valid bits and the padding bits, the coding levels can be distinguished. Specifically, for a code of N bits in total length, the intermediate result Mid = code^(code+1) is first calculated. Here, the "^" symbol represents the "exclusive OR" operation, which involves comparing the bits of two binary numbers one by one. When corresponding bits are the same, the result is 0; when they are different, the result is 1. Subsequently, the required level can be obtained by calculating the number of 1s on the right side of Mid.

[0104] The above describes the specific implementation methods of spatial grid coding and temporal grid coding. The coded values ​​are not only directly used for data indexing, but also carry the spatiotemporal location information contained in the corresponding grid, thus enabling calculations based on the coded values ​​to effectively perform the required spatiotemporal query tasks. In the application scenario of low-altitude UAV flight plan conflict detection, grid coding mainly plays the role of indexing the spatiotemporal data of UAV tracks, while efficient management and querying of spatiotemporal data can be achieved through the calculation and comparison of coded values.

[0105] The coding computation design also includes algorithms for parent-child coding computation, neighborhood coding computation, buffer coding computation, and range-based coding query computation using coded values. The parent-child coding computation mechanism achieves rapid location of data in a specific spatiotemporal region through the correlation between parent and child node codes. Neighborhood coding computation enables rapid retrieval of spatiotemporal data around a specific grid, which is particularly important for real-time monitoring and dynamic planning. The buffer coding computation mechanism is used to identify the data set within the influence range of a specific event or object, playing a crucial role in conflict detection and early warning systems. Range-based grid coding query makes it possible to accurately retrieve data within a specific spatiotemporal range, thus supporting decision support in complex scenarios.

[0106] 1. Parent-child code calculation

[0107] The aforementioned grid coding has a nested relationship between different levels. A parent grid is divided into 8 sub-grids (spatial coding) or 2 sub-grids (temporal coding). Under a specific coding base conversion, each sub-grid code uses the parent grid code as a prefix. The following discussion focuses on finding the parent grid code and sub-grid codes of a given code. Let the current code be 'code'. cThe original component dimension corresponding to the encoding is d, the current encoding level is L, and the parent encoding of the LF level is required, where F is the level difference between grandparent and grandchild grids. Calculate the parent grid code. f =(code) c >>(F×d))<<(F×d).

[0108] If the level L of the trellis coding is unknown, and the trellis coding adds a padding bit at the end, then the level L needs to be obtained first (see above on how to distinguish coding levels by inserting a gap bit between the valid bit and the padding bit). The newly obtained parent code also needs to add a padding bit at the end according to the formula code′=code|(1<<(NL)-1).

[0109] 2. Calculation of coordinate values ​​corresponding to the code

[0110] The grid encoding chosen in this invention uses a Z-order encoding format, where the binary bits of each component are interleaved. Each bit can be extracted sequentially to retrieve the original values ​​of the z, y, and x components. Furthermore, if the grid encoding level is L, extracting the first L bits for each component actually represents the row, column, and height number of the current level grid, calculated from the origin.

[0111] For mesh encoding like GeoSOT-3D, after extracting the three components, the longitude and latitude components need to be processed as follows: Figure 4 The process involves extracting the integer parts of degrees, minutes, and seconds, as well as the fractional parts of seconds, and converting them to their corresponding values ​​to reconstruct the original latitude and longitude. Altitude, however, needs to be recalculated based on the extracted altitude code value. Time encoding is similar, requiring... Figure 5 Extract the bits from each time segment and then convert them to numerical values.

[0112] The above method becomes less effective when calculating the original coordinate components of a large number of grid codes because it requires repeatedly traversing each bit of the code. The improved speedup algorithm uses a binary mask to first extract the bit of each component, and then compresses the extracted binary number to obtain the component.

[0113] The mask is represented in hexadecimal below:

[0114] 0x92492492492492492492492492492492492492

[0115] 49249249249249249249249. The mask is actually composed of binary (100100100…100)2, with a total length of 64 bits. By performing an AND operation between this mask and the original encoding, a coordinate component with a 3-bit interval can be obtained. Then, by shifting the original encoding, components on multiple coordinate axes can be obtained. Finally, the 0-bit intervals are compressed and removed.

[0116] 3. Neighborhood coding calculation

[0117] The neighborhood of a grid refers to the adjacent grids surrounding a single grid. Typically, it's a two-neighborhood for a one-dimensional grid, an eight-neighborhood for a two-dimensional grid, and a twenty-six-neighborhood for a three-dimensional grid. Figure 7 As shown. For a grid encoding with original coordinate components of dimension d, neighborhood encoding calculation will yield 3. d Encoding of -1 neighboring grids.

[0118] The implementation of neighborhood coding is relatively simple. It only requires extracting the binary values ​​corresponding to each coordinate component of the grid code to obtain the position coordinate components of the grid at its level. Then, it directly performs subtraction and addition operations on each component of the position coordinate, and then combines them to obtain the neighborhood coordinates around the grid. Finally, the neighborhood coordinates are converted back to the grid code.

[0119] 4. Adjacency determination of codes

[0120] For a grid, it corresponds to a certain region in the coding space. The relative topological relationship of the corresponding region can be determined by the calculation between grid codes. This study mainly needs to determine whether they are adjacent. This invention provides a method for determining the adjacency of two grids. For grid coding... i and grid encoding code j If the two encoding levels are different, it is necessary to find all the encodings of the lower level that are at the same level as the higher level encoding, and then compare them one by one. The following algorithm description assumes that the two encodings are at the same level.

[0121] The calculation process is relatively simple. First, extract the binary code value of each coordinate component corresponding to the encoding. Then, perform an XOR operation on each pair of coordinate components between the two codes. If the calculation results for all coordinate axes are either 0 or 1, and at least one of them is 1, then the two codes are adjacent in the encoding space. In fact, based on the differences between different coordinate axes, the specific adjacent topological relationship between the two codes can be further determined, such as corner adjacent, edge adjacent, face adjacent, etc. Figure 8 As shown, the corresponding relationships are shown in Table 2.

[0122] Table 2

[0123] x1^x2 y1^y2 z1^z2 Adjacent topological relationships 1 1 1 Adjacent points 1 1 0 Z-axis adjacent sides 1 0 1 Y-axis adjacent 1 0 0 YZ plane adjacent 0 1 1 Adjacent to the X-axis 0 1 0 Adjacent to XZ plane 0 0 1 Adjacent XY planes

[0124] 5. Buffer encoding calculation for single encoding

[0125] In geography, a buffer zone typically refers to a region within a certain radius around a target geographic feature, and its size represents the extent to which the geographic feature can influence. In low-altitude UAV conflict detection, buffer zone calculation is used to determine the potential risk area within a specified distance around the target.

[0126] The most direct and simple method for buffer computation is to use the seed filling method. Starting from the center grid encoding, a breadth-first search is performed. For each grid found, the distance between the current grid and the center grid is calculated. If the condition is met, the grid is added to the queue. This breadth-first search continues until the queue is empty.

[0127] Because the seed-fill algorithm requires repeated breadth-first searches, essentially filling outwards from the starting point until the expanded boundary, its efficiency is poor when the number of grids within the buffer is large. Below, we introduce the midpoint circle algorithm, a further efficiency optimization based on the seed-fill method. The midpoint circle algorithm originates from efficient graphics rendering in computer graphics. Its core idea is to utilize the symmetry of a circle, calculating only one-eighth of the circumference, and then generating the complete circle through mirroring. This method cleverly combines the symmetry of coordinates within the buffer circle, calculating only a small number of points and then symmetrically generating all buffer coordinate points that satisfy the buffer distance. Applied to encoding calculations, it is only necessary to convert the "calculation method of corresponding coordinate values" mentioned earlier for the center grid encoding into corresponding components and then apply the midpoint circle algorithm.

[0128] After constructing the spatial and temporal grid coding method, the key issue becomes how to transform the flight plan of a low-altitude UAV into a computable, fine-grained trajectory. This invention addresses the complex low-altitude airspace environment by proposing a trajectory generation framework combining partitioned grid coding: First, a gridded modeling method discretizes the continuous airspace into coding units, providing a structured representation for path planning; then, the traditional A* path search algorithm is improved by incorporating a grid-coded, neighbor-node-assisted rapid judgment mechanism; finally, an iterative backtracking method under dynamic constraints calculates the nodes of velocity state change on each segment, and velocity integrals are used to calculate the precise position and velocity state at equal time intervals, forming a spatiotemporally continuous flight trajectory. This process deeply integrates geometric space, kinematic constraints, and coding computation, establishing a quantifiable and indexable data foundation for subsequent conflict detection.

[0129] III. Methods for Modeling Underlying Surface Environmental Data

[0130] When planning flight paths for low-altitude unmanned aerial vehicles (UAVs), detailed understanding of the three-dimensional spatial information of the flight environment, namely, the underlying surface data, is required. This data represents the three-dimensional topography, buildings, vegetation, and other obstacles within the flight area and is fundamental to the safe flight of UAVs. Currently, the mainstream underlying surface data sources are OSGB binary files (Open SceneGraph Binary) or OBJ text files generated from oblique photogrammetry. Both store geometric information in the form of a triangular mesh. The basic building block of the triangular mesh model is a triangular facet, which describes the geometric shape of surface features through vertex coordinates, normals, textures, and other information. When planning the flight path of a UAV, the positional information of vertices, edges, and faces stored in the triangular mesh model is mainly used, without considering surface information such as textures or mappings. Data examples are shown below. Figure 9 As shown.

[0131] Triangulated mesh models accurately depict complex 3D terrain and obstacles. However, directly using triangular faces in trajectory planning calculations leads to excessive computational complexity in collision detection and other tasks. Therefore, it's typically necessary to convert the triangulated mesh into a more suitable spatial mesh (or voxel). Each voxel represents a specific region in space, used to mark whether that region is occupied by an obstacle. Specifically, the 3D space containing the triangulated mesh model is discretized into a 3D mesh, and each voxel is determined to be either on the model (surface meshing) or inside the model (solid meshing).

[0132] For urban low-altitude drone flight scenarios, this invention develops a computational method accelerated by mesh encoding based on the traditional ray casting method, unifying surface meshing and solid meshing for computation. The core idea of ​​the ray casting method is to determine whether a point is inside the model by emitting rays from a specified point and calculating the number of intersections with the model surface. The specified point is selected as the center point of each mesh, and the ray direction is selected as the coordinate axis direction. To avoid repeatedly calculating the intersection judgment of each mesh with each triangular facet of the model, it is necessary to pre-establish a mesh encoding index for the vertices of the triangular facets using mesh encoding.

[0133] The grid-based modeling method discretizes a continuous spatial domain into coding units, including the following steps:

[0134] 1. Determine the gridding range and the grid coding level. The gridding range can be determined based on the geographical range of the triangular mesh model, and the grid coding level determines the size of each grid.

[0135] 2. Construct an accelerated structure and use mesh encoding to index the vertices of the triangular facets.

[0136] 3. Traverse each grid within the gridded area and perform ray emission and intersection detection.

[0137] Rays are emitted from the center point of each grid along the three coordinate axes (usually the X-axis is preferred). Where O is the ray origin (voxel center), This is the ray direction vector. For each ray, calculate its intersections with all triangular faces. The intersections of a ray and a triangular face can be determined by... - The Trumbore algorithm is implemented, and its mathematical expression is: Where V0, V1, and V2 are the three vertices of the triangle, u and v are the coordinates of the centroid, and t is the ray parameter. If u,v≥0 and u+v≤1, and t>0, then the ray intersects the triangle.

[0138] 4. Determine the position of the mesh based on the number of intersections between the ray and the model: if the number of intersections is odd, the center of the mesh is located inside the model; if the number of intersections is even, the center of the mesh is located outside the model.

[0139] 5. Reorganize the data and save the data to ensure that each grid corresponds to the underlying obstacle in the triangular mesh model.

[0140] During the meshing process, the mesh encoding level directly determines the mesh size. Table 1 shows the geographic size of different mesh levels. If the mesh is too small, the number of meshes after meshing will be enormous, causing data redundancy and significantly increasing the search space for flight path planning, thus slowing down the planning time. If the mesh is too large, the meshing model of the triangulation will be too coarse, reducing the mesh positioning and obstacle avoidance accuracy of the UAV during flight path planning. To achieve a balance between mesh accuracy and computational speed, the following constraints can be considered:

[0141] (1) The total number of grids cannot exceed the storage limit. As the range of the triangular mesh model increases and the side length of the grid decreases, the number of grids after meshing will increase rapidly. The selection of the grid encoding level must ensure that the gridded data can be stored completely.

[0142] (2) The mesh size should be appropriately matched to the size and motion performance of the UAV. From the perspective of balancing computational accuracy and speed, the mesh side length needs to be slightly larger than the side length of the UAV's outer envelope. A mesh that is too fine, i.e., smaller than the UAV's size, is not computationally meaningful. The mesh side length needs to be smaller than the UAV's average turning radius. The degree of spatial discretization using the mesh should not be too high, to the point that it cannot represent the UAV's turning process. Based on relevant UAV flight scenarios, Table 3 below provides a correspondence between mesh encoding levels and related constraint classifications, facilitating selection according to actual usage needs.

[0143] Table 3

[0144]

[0145] IV. Improved A* Path Planning Method

[0146] After completing the meshed modeling of the underlying surface environment, path planning is required based on the waypoints in the UAV flight plan. Considering that in the flight plan pre-review research scenario of this paper, it is necessary to add UAV motion performance constraints while adapting to the meshed environment modeling, and the planning results need to have a certain degree of determinism and optimality, this invention chooses to use the A* algorithm to plan between pairs of waypoints. The planning results will be used as input for subsequent track interpolation.

[0147] The A* algorithm is a classic heuristic search algorithm widely used in path planning and graph search problems. It combines the optimality of Dijkstra's algorithm with the efficiency of greedy algorithms, using a heuristic function to guide the search direction, thereby improving search efficiency while guaranteeing the optimal solution.

[0148] The A* algorithm maintains two cost functions, g(n) and h(n), during the search process. g(n) represents the actual cost from the starting point to the current node n, ensuring that the actual cost of the path is taken into account. h(n) represents the heuristically estimated cost from the current node n to the target node, providing an estimate of the target and guiding the search direction; it is often referred to as the heuristic function. By adding the two, we obtain the comprehensive cost function f(n) = g(n) + h(n). The goal of the A* algorithm is to start from the starting point and progressively expand the nodes along the path with the minimum comprehensive cost until the target node is found.

[0149] In the traditional A* algorithm, if the selected heuristic function h(n) satisfies the consistency (or monotonicity) condition, the A* algorithm can guarantee finding the optimal solution. Based on the characteristics of the scenario presented in this invention, the h(n) function is chosen as the distance from the current node n to the endpoint end, expressed as h(n) = Distance(n,end).

[0150] While the A* algorithm performs excellently in path planning, its performance significantly degrades in high-dimensional or dense search spaces. This is because in high-dimensional or dense search spaces, the number of nodes increases exponentially with increasing dimension and density, resulting in a large number of nodes that the algorithm needs to evaluate. In gridded environments, the A* algorithm typically searches directly for neighboring grids when finding neighboring nodes, with a short and relatively fixed search step size. This leads to a relatively fixed relative angle between each pair of path points in the resulting path sequence; for example, the turning angle between two points in a two-dimensional scene is an integer multiple of 45°. To address these issues, this invention optimizes the neighbor node search space during the node expansion phase.

[0151] Traditional 3D neighborhood grids are 26-neighbor grids, meaning the maximum absolute value of the single coordinate difference between a neighboring node and the current node is 1. Figure 10 As shown, the black dots represent the current node, and the red dots represent the relative positions of neighboring nodes. From... Figure 10 It can be intuitively seen that the absolute value of the heading angle deviation between the neighboring nodes formed by the 26-neighborhood and the current node is always a multiple of 0° or 45°, and the absolute value of the pitch angle deviation is always a multiple of 0° or 45°. Under this neighborhood search condition, the search step size in a single coordinate axis direction is [0,1]. If the starting point and the ending point are far apart, a huge amount of search is often required to complete the path planning under such a short search step size. If the grid level selected for path planning is small and the grid side length is large relative to the size of the UAV, the turning angle of the final grid path will be too fixed, which may present a severe jagged state and deviate significantly from the actual flight of the UAV. Even with path smoothing, it is difficult to ensure that the smoothed new path will not collide with obstacles in the environment.

[0152] To address the issues with the aforementioned expansion of the 26 neighboring nodes, the most direct improvement is to increase the step size. This expands the spatial range of each probe, optimizes the sampling space of deflection angles during the search from the starting point to the end point, and allows for a denser range of feasible deflection angles that more closely approximate the diverse turning angles encountered during actual UAV flight. However, the direct result of increasing the step size is a cubic increase in the number of neighbors searched for each node. In practice, the number of nodes searched may actually increase rather than decrease. Therefore, it is necessary to balance the search performance gain from increasing the step size with the search time loss caused by the increased search volume.

[0153] The solution of this invention is to increase the step size while pruning invalid neighbor nodes, thereby improving the quality of path planning and relatively reducing the number of nodes that need to be evaluated. Furthermore, it utilizes environmental gridded data to make fine judgments within each local search space, thus avoiding conflicts between local paths and obstacles under large step sizes.

[0154] For ease of explanation, let (x,y,z) be the 3D sequence coordinates of the current node's mesh, used to represent the relative position of the current mesh; (dx,dy,dz) is used to represent the axial mesh sequence difference between the neighboring node's mesh and the current node's mesh. For example, (dx,dy,dz) = (1,1,1) means that the neighboring node moves one position in the positive directions of the three coordinate axes (X-axis, Y-axis, Z-axis) based on the current node's mesh.

[0155] Preliminary pruning strategies include:

[0156] (1) Control the number of neighboring nodes in the elevation direction. For multi-rotor UAVs, the flight motion capability in the horizontal direction is stronger than that in the vertical direction (usually the maximum flight speed in the horizontal direction is twice the maximum flight speed in the vertical direction, and the maximum acceleration has a similar performance relationship). The search step size in the vertical direction does not need to be completely consistent with that in the horizontal direction. The search step size in the horizontal direction can be controlled to always be 1, that is, |dz|≤1, to avoid the UAV from having too large a step in the vertical direction.

[0157] (2) Controlling the search altitude range. The scenario in this study is the preliminary review of the UAV flight plan. Path planning is mainly used for path planning between waypoints in the plan. When searching for neighboring nodes, the elevation of the neighboring nodes can be controlled to not exceed the elevation range [minZ, maxZ] of the starting and ending waypoints. That is, the node expansion search is controlled to be carried out in the altitude layer between every two waypoints, i.e., minZ≤z≤maxZ, to avoid excessive invalid dense search in three-dimensional space.

[0158] (3) Avoid repeated searches at the same angle. After increasing the step size, there will be multiple neighbor nodes in the same search angle direction. For example, if the coordinate differences between two neighbor nodes relative to the current node are (0,1,0) and (0,2,0), the search turning angles corresponding to these two neighbor nodes are the same. Similar relationships exist in other directions. Therefore, the search step sizes dx and dy in the two horizontal coordinate axes should satisfy the following expression:

[0159]

[0160] Figure 11 A schematic diagram of neighbor nodes with a step size of 4 on a single coordinate axis after pruning according to the above method shows that the types of angles from the current node to neighbor nodes have greatly increased, and no two neighbor nodes have the same corresponding turning angle. The coverage of the single search space has also been greatly expanded.

[0161] Following the initial pruning operations described above, it is necessary to prune neighboring nodes based on the drone's motion performance constraints and the obstacle environment constraints of the underlying surface, further compressing the search space during a single node expansion. The specific pruning rules are described below:

[0162] (1) Prune according to motion performance constraints. Consider the previous source node of the current node. If the line segment formed by the source node, the current node and the neighbor node exceeds the motion capability of the UAV, remove the neighbor node.

[0163] (2) Remove unreachable neighbor nodes based on the underlying surface environment. If a neighbor node is outside the environment, pruning is required. If a neighbor node is an obstacle, pruning is required. If a neighbor node is already in the closed set, it means that the node has been calculated and pruning is required.

[0164] (3) Remove neighboring nodes that cannot be reached by sight based on the underlying surface environment, and determine whether the current node is visible to neighboring nodes in the local search space. Specifically, construct a local mesh tensor from the current mesh to the neighboring mesh, and determine whether the neighboring node is reachable by detecting the tensor.

[0165] Let the sequence coordinates of the current grid be (x, y, z), and the sequence coordinates of the neighboring grids be (x, y, z). new ,y new ,z new If ), then the constructed local tensor is F[min(x,x)]. new :max(x,x new ),min(y,y new ):max(y,y new ),min(z,z new ):max(z,z new A local tensor F[x:x+4,y:y+4,z:z+3] of size 4×4×3 is shown below. Figure 12 , Figure 13 As shown.

[0166] Figure 12 This shows that there are no obstacles in the local tensor. Figure 13 This shows that there are obstacles in the local tensor, and these obstacles block the path from the current mesh to the neighboring mesh.

[0167] There are two methods for specific accessibility judgment based on tensors, which can be selected according to the actual needs of the project: (1) Use highly optimized GPU tensor operators to judge the number of obstacles in the tensor. For example, all obstacles in the tensor are marked as 1. Summing the local mesh tensor, if the number of obstacles exceeds a certain threshold, it is considered that the neighboring node is unreachable; (2) Use a more accurate visibility judgment algorithm. In the local tensor, use the Bresenham algorithm to quickly generate the mesh path from the current node to the neighboring node, and judge the obstacle type of each mesh on the path. This is equivalent to calculating whether the ray from the current node to the neighboring node intersects with the obstacle mesh.

[0168] This paper optimizes the search step size of the classic A* algorithm to obtain a grid path with corners that more closely resemble the actual flight path of the UAV, reducing the jaggedness problem inherent in the traditional neighborhood expansion method with a step size of 1. However, as the search step size increases, the search time gain brought by the expansion of the search range may be consumed by the increase in the number of nodes searched per instance. Experiments show that as the search step size increases: the search time first changes steadily and then increases rapidly, with the abrupt change occurring at a search step size of 5. This indicates that when the search step size is too large, the gain brought by the large search step size is insufficient to cover the search loss caused by the increase in the search space. During the planning process, the number of neighbor nodes calculated in the search exhibits a U-shaped curve that first decreases and then increases. The number of neighbor nodes searched is minimal when the search step size is 4 and maximum when the search step size is 1. From the perspective of the number of neighbor nodes searched, a step size of 4 is the optimal value.

[0169] V. Equal Time Interval Track Interpolation Method

[0170] The result of path planning is a spatially continuous grid sequence. However, for interval calculations, the grid path sequence is still too coarse. It is necessary to simulate and reconstruct the UAV's flight process on the path sequence under the constraints of the UAV's motion performance, and interpolate waypoints with equal time intervals more suitable for interval calculations. This invention performs waypoint interpolation based on the path planning results:

[0171] First, the grid sequence obtained from path planning is thinned out by deleting invalid grid points with the same turning angle;

[0172] Next, spatial location-based interpolation smoothing is performed to further optimize the jagged edges of the grid path;

[0173] Next, plan the initial speed, intermediate cruising speed, and final speed between each pair of waypoints, that is, on each flight segment;

[0174] Finally, the backtracking iteration method is used to ensure the continuity of speed between segments, and the position and heading of the UAV at equal time intervals are calculated using speed integrals. This completes the construction of the refined track corresponding to the grid sequence, which facilitates subsequent interval calculations.

[0175] There are well-established theories regarding the modeling of UAV flight states, which will not be elaborated upon here. In the grid path, every two adjacent grids define a flight segment with those two grids as endpoints. For each flight segment, the initial velocity should be exactly the same as the terminal velocity of the previous segment. The initial velocity of the first segment, i.e., the takeoff segment, is assumed to be zero, and the terminal velocity of the last segment, i.e., the landing segment, is also assumed to be zero. The vertical and horizontal accelerations on the flight segment should not exceed the maximum vertical or horizontal acceleration determined by the UAV's flight performance, and the vertical and horizontal velocities should also not exceed the maximum vertical or horizontal velocities determined by the UAV's flight performance.

[0176] This invention uses a three-stage model to model the speed change of each flight segment. The model abstracts the speed change of each flight segment into a stage 1 from the initial speed to the intermediate speed, a stage 2 flying at the intermediate speed, and a stage 3 from the intermediate speed to the final speed.

[0177] The backtracking iterative method for calculating the velocity profile includes the following steps:

[0178] 1. For large-angle turns on the grid path, use the circular motion model from the previous section to perform position interpolation. The interpolation is based on the maximum tilt angle and maximum turning speed allowed when the UAV turns to calculate the turning radius, and the corresponding position of the turning circular trajectory grid is re-inserted into the grid path sequence.

[0179] This process is equivalent to discretizing the turning arc into a series of straight segments connected end to end. Subsequent calculations can be performed only on segments composed of pairs of grids, treating the flight on each segment as a linear motion model. To ensure that the speed on the turning segment meets the requirements of the turning radius and turning angle, a lookup table can be created based on the UAV's performance, or the cosine function value of the turning angle can be used as a decay factor to control the flight speed during the turn, ensuring that the initial and final velocities of each segment calculated in the next step do not exceed the turning constraints.

[0180] 2. Calculate the three-stage velocity model for each flight segment, namely the initial velocity, intermediate velocity, and final velocity of each flight segment, as well as the time and distance from the initial velocity to the intermediate velocity, the flight time and distance at the intermediate velocity, and the time and distance from the intermediate velocity to the final velocity. When calculating the three-stage velocity model, the vertical and horizontal directions are calculated separately, but time coordination in both directions is required. A backtracking iteration mechanism is used to ensure that the velocities between all adjacent flight segments are continuous.

[0181] Based on the spatial displacement characteristics of each segment and its position within the path, different segment types are determined. The takeoff segment is the first segment of the path and is typically a vertical ascent. The landing segment is the last segment of the path and is typically a vertical descent. The climb segment is neither the first nor the last segment, and its vertical component is significantly positive. The descent segment is neither the first nor the last segment, and its vertical component is significantly negative. The cruise segment is a middle segment dominated by horizontal movement, with a relatively small vertical component.

[0182] Based on the physical constraints of takeoff and landing, the initial velocity of the takeoff segment is always set to 0, and the terminal velocity of the landing segment is always set to 0. If the subsequent segment of the takeoff segment is the ascent segment, the vertical terminal velocity of the takeoff segment can be set to the maximum vertical speed. If the preceding segment of the landing segment is the descent segment, the vertical initial velocity of the landing segment can be set to the maximum vertical speed. In other cases, the terminal velocity of the takeoff segment is set to 0, and the initial velocity of the landing segment is set to 0.

[0183] For the remaining intermediate segments, the turning angles between them and subsequent segments need to be considered. The maximum turning speeds corresponding to different turning angles, as determined in Table 3, should be set as the initial and final speeds. Alternatively, the maximum speed can be attenuated based on the turning angle and set as the final speed. The initial speed of the intermediate segment is then set as the final speed of the preceding segment. After this step, the initial and final speeds between segments are seamlessly connected.

[0184] The velocity model is easily derived based on existing theory and will not be elaborated here. We will only discuss the issue of inconsistency in different directions. Assume that a horizontal flight segment takes 40 seconds to complete, while a vertical segment only takes 10 seconds. If we do not re-coordinate based on the longest time, the drone will reach the final altitude of the segment at the 10-second mark, leaving only 30 seconds for level flight. This results in a step-zigzag flight trajectory, such as... Figure 14 As shown, the red dashed line is the calculated trajectory without time coordination, the blue curve is the calculated trajectory after time coordination, and the gray short dashed line is the direct connection between the start and end points.

[0185] The flight times in the vertical and horizontal directions need to be aligned and coordinated as much as possible to ensure that the calculated flight path best matches the line connecting the start and end points of the flight segment. The specific time coordination method is described below. Time coordination involves replanning the three-stage velocity model in the flight direction with the shorter time. Since the calculation process for the vertical and horizontal directions is exactly the same, the direction will not be distinguished further below.

[0186] The inputs for time-coordinated calculations are the initial initial velocity v0 and the initial final velocity v0. t Planned flight time (E), flight distance (D), maximum speed constraint (v) max Maximum acceleration constraint a max The output is the calculated feasible intermediate velocity v. midThe time [T1, T2, T3] and displacement [S1, S2, S3] for each of the three phases. If no feasible intermediate speed exists at the initial exit velocity, the acceleration segment needs to adjust the terminal velocity to v, since the planned flight time is relatively longer. t For the deceleration phase, the initial velocity needs to be adjusted to v0'. If reducing only the initial or terminal velocity is insufficient, then both the initial and terminal velocities need to be adjusted to v0' and v'' respectively. t The plan is designed to adapt to the input time. If the initial velocity is adjusted, the plan must be recalculated back to the previous segment, which is then recalculated based on the new terminal velocity.

[0187] (1) The input initial velocity and initial terminal velocity indicate that the current segment is a deceleration process (i.e., v0>v). t ):

[0188] First, calculate the deceleration from the initial initial velocity v0 to the initial final velocity v. t The required time and distance, assuming the entire journey is at maximum deceleration 'a'. max Decelerate from v0 to v t The required time and distance are respectively

[0189] Secondly, calculate the remaining time after subtracting the time required to reach the initial and final velocities from the initial initial velocity.

[0190] T r =ET(v t ), and the margin distance S after subtracting the required distance. r =DS(v t ).

[0191] Since the planned flight time E is actually longer than the time corresponding to the initial and final velocities from the previous stage (a longer flight time is always used to coordinate), therefore t(v t The condition ≥ 0 is always satisfied. Next, calculate the average velocity v corresponding to the margin distance and margin time. r =S r / T r The following is a categorized discussion:

[0192] ①If the residual average velocity v r ≥0 indicates that under the planned time, this flight segment still has enough time to accelerate to a speed exceeding the initial velocity midway, and maintain a certain constant speed at this intermediate speed. The three speed stages are: accelerating from the initial velocity v0 to the intermediate speed v mid At an intermediate speed v mid Uniform flight, at an intermediate speed v mid Decelerate to V tThen the intermediate velocity v mid Satisfy the following equations

[0193] This equation is about v mid To solve a quadratic equation in one variable, we need to select a solution that satisfies the following conditions:

[0194] ②If the residual average velocity v0 ≥ v r ≥v t This indicates that during the planned flight period, there is a phase in the flight where the aircraft flies at a constant speed between the initial and final velocities. The three speed phases are as follows: decelerating from the initial velocity v0 to the intermediate velocity v... mid At an intermediate speed v mid Uniform flight, at an intermediate speed v mid Decelerate to V t Then the intermediate velocity v mid Satisfy the following equation:

[0195] This equation is about v mid To solve a quadratic equation in one variable, we need to select a solution that satisfies the following conditions:

[0196] ③If the residual average velocity v t ≥v r A value ≥0 indicates that the planning time is very long, to the point that the speed needs to be reduced to below the initial and final speeds midway through the flight segment. The three speed stages are: decelerating from the initial speed v0 to the intermediate speed v mid At an intermediate speed v mid Uniform flight, at an intermediate speed v mid Accelerate to V t Then the intermediate velocity v mid Satisfy the following equation: This equation is about v mid To solve a quadratic equation in one variable, we need to select a solution that satisfies the following conditions:

[0197] ④ If a solution satisfying the above conditions does not exist or the quadratic equation itself has no real solutions, it means that the initial and final velocities cannot be satisfied under the current planning time constraints. Considering that this segment is a deceleration segment, in order to maintain the input initial and final velocities as much as possible, the initial velocity should be reduced, and the reduction should be as small as possible. Therefore, under this constraint, the intermediate velocity v in the three-stage velocity change needs to be eliminated. mid In the second stage of uniform flight, the new initial velocity v0′ and intermediate velocity v mid The following system of equations should be satisfied:

[0198] Solving the equation involves substituting the second equation into the first equation to eliminate v. mid First, solve for v0′. If a solution to v0′ exists, then substitute it into the second equation to obtain v. mid When v mid <0, reset v mid Set the value to 0 and then substitute it into the first equation to find the answer.

[0199] ⑤ If a solution for v0′ that satisfies the conditions does not exist, it means that even if the initial velocity is reduced to the same level as the final velocity, the planned time cannot be met. It is necessary to consider changing this segment to an acceleration segment, while simultaneously reducing both the initial and final velocities. The new initial and final velocities must satisfy the following equations: v′0≥0、v′ t ≥0.

[0200] ⑥ If the above equation has no solution, i.e., D < 1 / 2 × a max E 2 This indicates that the flight distance of this segment is relatively short, even insufficient to maintain an initial velocity greater than 0 under the conditions of meeting the planned time requirements and keeping the terminal velocity as close as possible to the initial terminal velocity. Therefore, the new initial velocity must be set to 0, i.e., v0′=0. The new initial velocity is set to...

[0201] (2) The input initial velocity and initial terminal velocity indicate that the current segment is an acceleration process (i.e., v0≤v). t ):

[0202] Assuming the maximum acceleration a is maintained throughout the entire journey max Accelerating from initial initial velocity v0 to initial final velocity v t The calculation is accelerated directly from v0 to v t Time and distance required

[0203] Calculate the remaining time after subtracting the time required to reach the initial and final velocities from the initial initial velocity.

[0204] T r =ET(v t ), and the margin distance S after subtracting the required distance. r =DS(v t ).

[0205] Since the planned flight time E is actually longer than the time corresponding to the initial and final velocities from the previous stage (a longer flight time is always used to coordinate), therefore t(v t The condition ≥ 0 is always satisfied. Next, calculate the average velocity v corresponding to the margin distance and margin time. r =S r / T rThe following is a categorized discussion:

[0206] ①If the residual average velocity v r ≥v t This indicates that under the planned timeframe, this flight segment still has sufficient speed to accelerate to a speed exceeding the initial velocity midway, and maintain a certain constant speed at this intermediate speed. The three speed stages are: accelerating from the initial velocity v0 to the intermediate speed v0. mid At an intermediate speed v mid Uniform flight, at an intermediate speed v mid Decelerate to V t Then the intermediate velocity v mid Satisfy the following equations

[0207] This equation is about v mid To solve a quadratic equation in one variable, we need to select a solution that satisfies the following conditions:

[0208] ②If the residual average velocity v t ≥v r ≥v0 indicates that, under the planned flight time, there is a phase during this flight segment where the aircraft flies at a constant speed between the initial and final velocities. The three speed phases are: accelerating from the initial velocity v0 to the intermediate velocity v0. mid At an intermediate speed v mid Uniform flight, at an intermediate speed v mid Accelerate to V t Then the intermediate velocity v mid Satisfy the following equations

[0209] This equation is about v mid To solve a quadratic equation in one variable, we need to select a solution that satisfies the following conditions:

[0210] ③ If the residual average velocity v0 ≥ v r A value ≥0 indicates that the planning time is very long, to the point that the speed needs to be reduced to below the initial speed midway through the flight segment. The three speed stages are: decelerating from the initial speed v0 to the intermediate speed v mid At an intermediate speed v mid Uniform flight, at an intermediate speed v mid Accelerate to V t Then the intermediate velocity v mid Satisfy the following equations This equation is about v mid To solve a quadratic equation in one variable, we need to select a solution that satisfies the following conditions:

[0211] ④ If a solution satisfying the above conditions does not exist or the quadratic equation itself has no real solutions, it means that the initial and final velocities are no longer sufficient under the current planning time constraints. Considering that this segment is an acceleration segment, in order to maintain the input initial and final velocities as much as possible, the final velocity should be reduced, and the reduction should be as small as possible. Therefore, under this constraint, the intermediate velocity v in the three-stage velocity change needs to be eliminated. mid The second stage of uniform flight, the new final velocity v t ′ and intermediate velocity v mid The following system of equations should be satisfied:

[0212]

[0213] Solving the equation involves substituting the second equation into the first equation to eliminate v. mid First solve v t ′, if v t The solution to ′ exists, and substituting it into the second equation yields v. mid When v mid <0, reset v mid Set the value to 0 and then substitute it into the first equation to find the answer.

[0214] ⑤ If v satisfies the condition t The solution to ' does not exist, indicating that even reducing the initial velocity to the same level as the final velocity cannot meet the planned time requirement. It is necessary to consider changing this segment to an acceleration segment, while simultaneously reducing both the initial and final velocities. The new initial and final velocities satisfy the following equations: v′0≥0、v t ′≥0.

[0215] ⑥ If the above equation has no solution, i.e., D < 1 / 2 × a max E 2 This indicates that the flight distance of this segment is relatively short, even insufficient to maintain an initial velocity greater than 0 under the conditions of meeting the planned time requirements and keeping the terminal velocity as close as possible to the initial terminal velocity. Therefore, the new initial velocity must be set to 0, i.e., v0. t ′=0, the new initial velocity is set to

[0216] After the above time coordination calculations, the time taken in the vertical and horizontal directions is equal. At the same time, the final three-stage velocity model in the vertical and horizontal directions is obtained, namely the initial velocity, intermediate velocity, and final velocity. The time [T1,T2,T3] and displacement [S1,S2,S3] taken in each stage can be obtained by referring to the following formula.

[0217]

[0218]

[0219] If the new initial velocity is not equal to the initial velocity before time coordination, or if the need for adjusting the initial velocity and backtracking iterations already exists when calculating the initial three-stage velocity model, the initial and final velocities of the previous segment need to be modified accordingly, and then the calculation of the previous segment needs to be re-iterated. If the new final velocity is not equal to the initial velocity before time coordination, the initial velocity of the next segment needs to be modified accordingly, and the calculation of the next segment needs to be based on the new values.

[0220] Ultimately, the entire backtracking iterative method converged and calculated all segments. The initial, intermediate, and final velocities of all segments were successfully calculated, and the initial and final velocities between segments were perfectly matched and continuous. The time and distance of each of the three stages were also obtained simultaneously, providing a feasible velocity profile for subsequent integration to generate the trajectory.

[0221] VI. Calculating the waypoint by velocity integral

[0222] After establishing the three-stage velocity model for each flight segment, the position of the UAV at each moment is obtained by performing velocity integration at certain time intervals based on the known velocity changes on each flight segment. The magnitude and direction of the velocity at each position are determined, thus finally establishing complete velocity profiles, horizontal position profiles, elevation position profiles, and heading profiles, and calculating the trajectory at equal time intervals.

[0223] The velocity integral calculation of equal time interval track points includes the following steps:

[0224] 1. Establish a time reference system within the flight segment and calculate the cumulative time prefix sum of the flight segment. Where T j The flight time for the j-th segment is based on the total flight time T for all segments. total =T sum (n) and time step Δt generate a uniformly distributed time series [0, Δt, 2Δt, ..., T]. total ].

[0225] 2. Calculate the position of the time within the flight segment, for the time series [0, Δt, 2Δt, ..., T]. total Each time point t extracted from ] i Perform the following operations: locate t i The flight segment k satisfies T sum (k)≤t i ≤T sum (k+1), calculate t i The relative time t within flight segment k seg =t i -T sum (k).

[0226] 3. Calculate the velocity and displacement at time t to determine the time t. seg Within flight segment k, which motion segment in the three-stage velocity model does it belong to: In the horizontal direction, j∈0,1,2, satisfying... In the vertical direction, for l∈0,1,2, the following condition is met: in and These are the prefix sums of the times taken for the three stages of the horizontal and vertical flight segments, respectively.

[0227] According to t seg Calculate the horizontal velocity v based on the position in the three-stage velocity model. hor (t) and displacement S hor (t):

[0228] (a) Phase 1, j = 0:

[0229]

[0230] (b) Phase 2, j = 1:

[0231] (c) Stage 3, j = 2:

[0232]

[0233] Vertical velocity v ver (t) and displacement S ver The calculation process for (t) is the same as that for the horizontal direction, only the subscripts are different, so it will not be repeated here.

[0234] 4. Three-dimensional position and velocity synthesis:

[0235] Based on the horizontal displacement S hor and direction unit vector Calculate the horizontal position component Wherein, the direction unit vector It is calculated directly from the start and end points of the flight segment;

[0236] Based on the vertical displacement S ver and direction d ver Calculate the vertical position component P ver =S ver ·d ver , where direction d ver A value of 1 indicates vertically upward, and -1 indicates vertically downward;

[0237] Synthetic 3D Position in It is the starting position of the current flight segment; horizontal velocity vector. Vertical velocity component V ver =v ver (t)·d ver Three-dimensional velocity vector

[0238]

[0239] 5. Organize all data, including the position, speed, and heading of all waypoints according to time, to form an equally timed waypoint corresponding to a flight plan.

[0240] Obviously, the described embodiments are only a part of the embodiments of the present invention, and not all of them. All other embodiments obtained by those skilled in the art and related fields based on the embodiments of the present invention without inventive effort should fall within the scope of protection of the present invention.

Claims

1. A low-altitude flight path planning method based on grid coding, characterized in that, First, the continuous airspace is discretized into coding units using a grid modeling method, providing a structured representation for path planning. Then, the velocity state change nodes on each flight segment are calculated using an iterative backtracking method under dynamic constraints. Finally, the precise position and velocity state at equal time intervals are calculated using velocity integrals to form a spatiotemporally continuous flight trajectory, thereby completing the path planning.

2. The low-altitude flight path planning method based on grid coding according to claim 1, characterized in that, The grid-based modeling method discretizes a continuous spatial domain into coding units, including the following steps: Step A1: Determine the gridding range and the grid coding level. The gridding range can be determined based on the geographical range of the triangular mesh model, and the grid coding level determines the size of each grid. Step A2: Construct the acceleration structure and index the vertices of the triangular facets using mesh encoding; Step A3: Traverse each grid within the meshed area and perform ray emission and intersection detection; Step A4: Determine the position of the mesh based on the number of intersections between the ray and the model: if the number of intersections is odd, the center of the mesh is located inside the model; if the number of intersections is even, the center of the mesh is located outside the model. Step A5: Reorganize the data and save whether each grid corresponds to the underlying surface obstacle in the triangular mesh model.

3. The low-altitude flight path planning method based on grid coding according to claim 1, characterized in that, An improved A* path planning algorithm is adopted to optimize the neighbor search space in the expansion phase, and invalid neighbor nodes are pruned while increasing the step size.

4. The low-altitude flight path planning method based on grid coding according to claim 3, characterized in that, Let (x, y, z) be the three-dimensional sequence coordinates of the current node's mesh, used to represent the relative position of the current mesh; (dx, dy, dz) represents the axial mesh sequence difference between the neighboring node mesh and the current node mesh. The initial pruning strategy includes:

5. The low-altitude flight path planning method based on grid coding according to claim 4, characterized in that, Further pruning strategies include: (1) Prune according to motion performance constraints. Consider the previous source node of the current node. If the line segment formed by the source node, the current node and the neighbor node exceeds the motion capability of the UAV, remove the neighbor node. (2) Remove unreachable neighbor nodes based on the underlying surface environment. If a neighbor node is outside the environment, pruning is required. If a neighbor node is an obstacle, pruning is required. If a neighbor node is already in the closed set, it means that the node has been calculated and pruning is required. (3) Remove neighboring nodes that cannot be reached by sight based on the underlying surface environment, and determine whether the current node is visible to neighboring nodes in the local search space. Specifically, construct a local mesh tensor from the current mesh to the neighboring mesh, and determine whether the neighboring node is reachable by detecting the tensor.

6. The low-altitude flight path planning method based on grid coding according to any one of claims 1-5, characterized in that, Based on the path planning results, track interpolation is performed: First, the grid sequence obtained from path planning is thinned out by deleting invalid grid points with the same turning angle; Next, spatial location-based interpolation smoothing is performed to further optimize the jagged edges of the grid path; Next, plan the initial speed, intermediate cruising speed, and final speed between each pair of waypoints, that is, on each flight segment; Finally, the backtracking iteration method is used to ensure the continuity of speed between segments, and the position and heading of the UAV at equal time intervals are calculated using speed integrals. This completes the construction of the refined track corresponding to the grid sequence, which facilitates subsequent interval calculations.

7. The low-altitude flight path planning method based on grid coding according to claim 6, characterized in that, The backtracking iterative method for calculating the velocity profile includes the following steps: First, position interpolation is performed on the large-angle turns on the grid path using the circular motion model from the previous section. The interpolation calculates the turning radius based on the maximum tilt angle and maximum turning speed allowed when the UAV turns, and the corresponding position of the turning circular trajectory grid is re-inserted into the grid path sequence. Next, the three-stage velocity model for each flight segment is calculated, namely the initial velocity, intermediate velocity, and final velocity of each flight segment, as well as the time and distance from the initial velocity to the intermediate velocity, the flight time and distance at the intermediate velocity, and the time and distance from the intermediate velocity to the final velocity. The vertical and horizontal directions are calculated separately when calculating the three-stage velocity model, but time coordination in the two directions is required, and the velocity between all adjacent flight segments is ensured to be continuous through a backtracking iteration mechanism.

8. The low-altitude flight path planning method based on grid coding according to claim 7, characterized in that, Time coordination involves redesigning the three-stage velocity model in the flight direction with shorter flight time, so that the flight time in both directions tends to be consistent. The inputs for time coordination calculation are the initial initial velocity v0 and the initial final velocity v0. t Planned flight time (E), flight distance (D), maximum speed constraint (v) max Maximum acceleration constraint a max The output is the calculated feasible intermediate velocity v. mid The time [T1,T2,T3] and displacement [S1,S2,S3] for each of the three stages.

9. The low-altitude flight path planning method based on grid coding according to claim 8, characterized in that, If the new initial velocity is not equal to the initial velocity before time coordination, or if the need to adjust the initial velocity for backtracking iteration has already occurred when calculating the initial three-stage velocity model, then the initial terminal velocity of the previous segment needs to be modified accordingly, and then backtracking to the previous segment to recalculate the previous segment; if the new terminal velocity is not equal to the initial velocity before time coordination, then the initial initial velocity of the next segment needs to be modified accordingly, and the calculation should be performed based on the new value when iterating to the next segment. Finally, the entire backtracking iterative method can converge and calculate all segments. The initial velocity, intermediate velocity, and terminal velocity of all segments are successfully calculated. The initial and terminal velocities between segments are perfectly matched and continuous. The time and distance of each of the three stages are also obtained at the same time.

10. The low-altitude flight path planning based on grid coding according to claim 9 The method of division is characterized by, After establishing the three-stage velocity model for each flight segment, the position of the UAV at each moment is obtained by performing velocity integration at certain time intervals based on the known velocity changes on each flight segment. The magnitude and direction of the velocity at each position are determined, thus finally establishing complete velocity profiles, horizontal position profiles, elevation position profiles, and heading profiles, and calculating the trajectory at equal time intervals.

11. The low-altitude flight path planning method based on grid coding according to claim 10, characterized in that, The velocity integral calculation of equal time interval track points includes the following steps: Step B1: Establish a time reference system within the flight segment and calculate the cumulative time prefix sum of the flight segment. Where T j The flight time for the j-th segment is based on the total flight time T for all segments. total =T sum (n) and time step Δt generate a uniformly distributed time series [0, Δt, 2Δt, ..., T]. total ]; Step B2, calculate the position of the time within the flight segment, from the time series [0, Δt, 2Δt, ..., T]. total Each time point t extracted from ] i Perform the following operations: locate t i The flight segment k satisfies T sum (k)≤t i ≤T sum (k+1), calculate t i The relative time t within flight segment k seg =t i -T sum (k); Step B3: Calculate the velocity and displacement at time t. seg Within flight segment k, which motion segment in the three-stage velocity model does it belong to: In the horizontal direction, j∈0,1,2, satisfying... In the vertical direction, for l∈0,1,2, the following condition is met: in and T l ver These are the prefix sums of the times taken for the three stages of the horizontal and vertical flight segments, respectively. According to t seg Calculate the horizontal velocity v based on the position in the three-stage velocity model. hor (t) and displacement S hor (t): (a) Phase 1, j = 0: (b) Phase 2, j = 1: (c) Stage 3, j = 2: Vertical velocity v ver (t) and displacement S ver (t) The calculation process is consistent with the horizontal direction; Step B4, 3D position and velocity synthesis: Based on the horizontal displacement S hor and direction unit vector Calculate the horizontal position component Wherein, the direction unit vector It is calculated directly from the start and end points of the flight segment; Based on the vertical displacement S ver and direction d ver Calculate the vertical position component P ver =S ver ·d ver , where direction d ver A value of 1 indicates vertically upward, and -1 indicates vertically downward; Synthetic 3D Position in It is the starting position of the current flight segment; horizontal velocity vector. Vertical velocity component V ver =v ver (t)·d ver Three-dimensional velocity vector Step B5: Organize all data, including the position, speed, and heading of all waypoints according to time, to form an equally timed waypoint corresponding to a flight plan.

Images