A UAV Path Optimization Method Based on an Improved 3D D Lite Algorithm

Abstract

The present application relates to a kind of unmanned aerial vehicle path optimization method based on improved three-dimensional D Lite algorithm, belong to mobile robot field.The method includes the following steps:1.D Lite algorithm is extended to three-dimensional space, and the priority queue of algorithm is optimized using the data structure model of minimum binary heap, to improve the search efficiency of algorithm in three-dimensional space.2.On the basis of the first step three-dimensional improvement, introduce a 'path node optimization strategy', the path node generated by three-dimensional D Lite is optimized, to shorten path length and reduce unnecessary path turning point.3.Considering that unmanned aerial vehicle motion planning can be converted into a four-dimensional flat output space for calculation, interpolation operation is carried out on the path node obtained in step S2.4.Combined with the polynomial interpolation method of MinimumSnap, the path is smoothed under the premise of meeting the collision-free constraint of trajectory, to ensure that smooth feasible unmanned aerial vehicle running trajectory is generated.

Description

Technical Field

[0001] This invention belongs to the field of mobile robots and relates to a method for optimizing UAV paths based on an improved 3D D Lite algorithm. Background Technology

[0002] Geometric methods are easily adapted to three-dimensional dynamic spatial domains, offering the advantage of low computational overhead. In the geometric grid method, the A* algorithm, a heuristic that uses an evaluation function to traverse the non-obstacle neighbors of the current node to find the shortest path, boasts advantages such as simplicity, ease of operation, and high accuracy. However, the path calculated by the A* algorithm is actually the shortest path among discrete cells in the given map model, which differs significantly from the actual optimal path. Furthermore, A* is inefficient in obstacle avoidance in dynamic environments, requiring a complete recalculation of the path after map changes. In 2002, Sven Koenig and Maxim Likhachev proposed the D Lite algorithm, a dynamic path planning algorithm based on A* and LPA*, primarily for robot pathfinding. When the environment changes and pathfinding resumes, D Lite does not need to completely recalculate; instead, it utilizes the data from the previous pathfinding attempt for rapid pathfinding. Therefore, it can be applied to dynamic pathfinding for robots in unknown environments. In 2011, Mellinger D and Kumar V, in their published paper, transformed the state space of UAV motion from a 12-dimensional state space to a 4-dimensional state space through algebraic transformation. This significantly reduced the difficulty of solving UAV motion planning and modeling compared to a complex 12-dimensional state space. Based on this, polynomial interpolation can be used to solve the UAV trajectory generation problem. This method utilizes the known function values ​​at several points within a certain interval to construct an appropriate specific polynomial function. The values ​​of this specific function are then used as approximations of the function at other points within the interval. Combining the polynomial interpolation function with the actual UAV operation, the trajectory generation problem can be modeled as a constrained QP optimization problem. Solving this problem yields the desired UAV trajectory. Summary of the Invention

[0003] In view of this, the purpose of this invention is to provide a UAV path optimization method based on an improved 3D D*Lite algorithm. First, the D*Lite algorithm is extended to 3D space, and a minimum binary heap data structure model is used to optimize the algorithm's priority queue. Second, based on the 3D algorithm, a 'path node optimization strategy' is introduced to optimize the path nodes generated by the 3D D*Lite algorithm. Then, considering that UAV motion planning can be transformed into a four-dimensional flat output space for calculation, interpolation operations are performed on the path nodes obtained in step S2. Finally, combined with the Minimum Snap seventh-order polynomial interpolation method, the path is smoothed under the premise of satisfying the trajectory collision-free constraint to generate a continuous UAV runnable trajectory with position, velocity, acceleration, jerk, etc., at each interpolation point.

[0004] To achieve the above objectives, the present invention provides the following technical solution:

[0005] The UAV path optimization method based on the improved 3D D Lite algorithm includes the following steps:

[0006] S1 extends the D*Lite algorithm to three-dimensional space and uses a minimum binary heap data structure model to optimize the priority queue of the algorithm.

[0007] S2, based on the S1 3D transformation algorithm, introduces a 'path node optimization strategy' to optimize the path nodes generated by 3D D Lite;

[0008] S3, considering that the motion planning of the UAV can be transformed into a four-dimensional flat output space for calculation, interpolation operation is performed on the path nodes obtained in step S2.

[0009] S4, combined with the seventh-order polynomial interpolation method of Minimum Snap, smooths the path under the premise of satisfying the trajectory collision-free constraint, so as to generate a continuous UAV runnable trajectory with position, velocity, acceleration, jerk, etc. at each interpolation point.

[0010] Specifically, for steps S1 and S2, the main task is to extend the two-dimensional D Lite algorithm to a three-dimensional voxel grid for pathfinding. In response to the problems of reduced running efficiency and deteriorated path quality caused by the three-dimensional transformation, the algorithm has been improved accordingly.

[0011] First, the two-dimensional D Lite algorithm is extended to a three-dimensional voxel grid space for pathfinding. The steps are as follows:

[0012] S101 constructs a three-dimensional raster voxel environment used by the path planning algorithm for path finding.

[0013] S102 expands the search direction of the D Lite algorithm, which was originally a traditional two-dimensional path planning algorithm, from the original eight-directional neighbor nodes in the plane to 26-directional neighbor nodes in the three-dimensional space, based on the pathfinding characteristics in three-dimensional space.

[0014] After the neighbor node expansion is completed, step S103 checks whether these neighbor nodes are obstacles based on the 3D grid map established in step S101. Neighbor nodes that are not obstacles are assigned corresponding position values.

[0015] S103 After the neighbor node expansion is completed, based on the 3D grid map established in step S101, it detects whether these neighbor nodes are obstacles. For neighbor nodes that are not obstacles, corresponding g and rhs values ​​are assigned, with the following expressions:

[0016]

[0017]

[0018] In the obstacle detection process, S104 should pay attention to the fact that in the 3D environment, the path connection between voxel grids may pass through two or more obstacles. There are 27 such cases, which need to be checked and eliminated one by one after S103 to ensure that the path generated in 3D space is a safe and collision-free path.

[0019] After the S105 expansion node and obstacle detection are completed, the corresponding node will be placed in the priority queue.

[0020] Secondly, to address the issue of reduced operational efficiency caused by 3D rendering, this invention improves the data storage structure of the priority queue in the algorithm. Specifically, it uses a minimum binary heap to improve the data storage structure of the original priority queue, which has the following characteristics;

[0021] (1) It is a priority data structure built on a minimum binary heap;

[0022] (2) Essentially, it is a queue that uses an array for storage;

[0023] (3) This data structure guarantees that the first element of the array is the smallest key value, so the time complexity of obtaining the smallest key value is only O(1).

[0024] (4) The minimum critical value is calculated as follows:

[0025] k(s) = [k1(s), k2(s)]

[0026] k1(s)=min(g(s),rhs(s))+h(s,s) goal )

[0027] k2(s) = min(g(s), rhs(s))

[0028] Finally, to address the issue of poor path quality resulting from the D Lite algorithm's 3D transformation, this invention employs a 'path node optimization strategy' to improve the generated path quality. The specific implementation steps are as follows:

[0029] (1) First, according to the reverse search principle of the D Lite algorithm, when the algorithm finds the starting point p s Since the node has reached a steady state where rhs and g values ​​are equal, the path can be completed until the endpoint p based on the node inheritance relationship. e The obtained set of path nodes is represented as parentlist = start S2, S3..., S i ,...,S j ,...S goal >

[0030] (2) Set two position pointers x1 and x2 to record the node position during the detection process. Point x1 to S... start Then, x2 is set to point to the next node in the parentlist, and reachability tests are performed on each node in turn until a test result indicates that the node is unreachable. Finally, x2 is changed to point to the node preceding the currently tested node, S. i The nodes pointed to by x1 and x2 are directly changed to direct inheritance relationships, thus completing one optimization of the path nodes.

[0031] (3) In the reachability test, the two nodes S pointed to by x1 and x2 are checked according to the three-dimensional LOS algorithm. i S j Visibility. When S i S j If the coordinates of two nodes are equal in one or two dimensions, it indicates that the two nodes are located on a two-dimensional plane of coordinate axes xy, yz, or xz. Therefore, the two-dimensional LOS algorithm is used to determine S using the Bresenham line algorithm. i S j The specific grid cells that the line connecting them passes through. When S i S j If no two coordinates are equal in one dimension, it indicates that the two coordinates are not in the same coordinate system plane. In this case, spatial node sampling detection based on spatial linear equations is required to complete the reachability detection between the two three-dimensional spatial points.

[0032] (4) Iterate through the above process until the x2 pointer points to S. goal And since the reachability check of the two nodes pointed to by x1 and x2 has passed, the path node optimization algorithm ends. Based on the inheritance relationship, the path node optimization algorithm terminates. start To S​goal The new path node between them is the shortest path.

[0033] For steps S3 and S4, based on the differential flatness characteristics of the UAV, the specific steps for generating the trajectory using polynomial interpolation based on the path generated by the improved 3D D Lite algorithm are as follows:

[0034] First, the path set generated by path planning needs to be interpolated using a fixed step size l. The interpolated points are then stored in a set P, resulting in P = [P0, P1, P2, ..., P...]. n ].

[0035] The trackpoint set P generated after interpolation is divided into n segments based on the number of nodes in the set (n+1), and the trajectory is required to be within a specific time t. n The interior passes through these specific points. The piecewise polynomial formula is expressed as follows:

[0036]

[0037] Where t represents the flight time of the drone, from t0 to t... n R represents the time when the drone passes through each endpoint of the path. m (t) represents the path polynomial of the m-th segment, and its corresponding expression is as follows:

[0038] R m (t)=c m0 +c m1 t+c m2 t 2 +c m3 t 3 +c m4 t 4 +c m5 t 5 +c m6 t 6 +c m7 t 7

[0039] By taking multiple derivatives of the above equation, we can calculate the expressions for the drone's velocity, acceleration, jerk, and snap for the corresponding path segment, which correspond to the following formulas.

[0040]

[0041]

[0042]

[0043]

[0044] Since a quadcopter UAV is a fourth-order dynamic system, to ensure optimal trajectory smoothness, this paper formulates the trajectory generation problem as an optimization problem with the objective function being the fourth derivative, as shown in the following formula:

[0045]

[0046] Express the coefficients of the polynomial function as C m =[c m1 ,c m2 ,…,c m7 As a decision variable, its extension yields:

[0047]

[0048] The objective function for all trajectory segments, when integrated, can be expressed as the QP problem shown below:

[0049]

[0050] In static global path planning, the UAV's trajectory is planned from a fixed starting point with zero velocity to a stopping point at the target. The following equation constraint can then be established:

[0051] R1(t0)=P0,R n (t n ) = P n

[0052] To ensure the continuity of the trajectory, the drone's position, velocity, acceleration, jerk, and snap at the starting point of each trajectory segment must be consistent with the endpoint of the previous trajectory segment. Therefore:

[0053]

[0054] The constraint expression for the safety corridor is:

[0055] P i -D s ≤R i (t i-1 )≤P i +D s (i = 2, 3, ..., n)

[0056] Therefore, the trajectory generation problem under global path planning can be summarized as solving the following problem:

[0057]

[0058]

[0059] The beneficial effects of this invention are as follows: an improved 3D DLite algorithm for efficient pathfinding in 3D voxel grid space is designed, and combined with the differential flatness characteristics of UAV space, the trajectory generation problem is transformed into solving a polynomial interpolation optimization function to generate smooth, runnable 3D spatial trajectories. This method has advantages over traditional algorithms in terms of planning time and generated path quality.

[0060] Pedestrians in public places typically do not change their direction or speed. Assuming the speed of obstacles is known and remains constant within the prediction time, three modifications are proposed: incorporating obstacle speed into the trajectory prediction function, adding a new evaluation term to reduce computation and increase reliability, and constructing search rules for an elliptic safe distance optimization algorithm. These modifications aim to plan more rational and efficient paths in environments with multiple dynamic obstacles, reduce the impact on pedestrian movement, and enhance the intelligence of mobile robots.

[0061] Other advantages, objectives, and features of the invention will be set forth in part in the description which follows, and in part will be apparent to those skilled in the art from the following examination, or may be learned from practice of the invention. The objectives and other advantages of the invention can be realized and obtained through the following description. Attached Figure Description

[0062] To make the objectives, technical solutions, and advantages of the present invention clearer, the preferred embodiments of the present invention will be described in detail below with reference to the accompanying drawings, wherein:

[0063] Figure 1 This is a comparison diagram of the two-dimensional and three-dimensional extended nodes of the S1 algorithm in this invention;

[0064] Figure 2 This is the improved priority queue model S1 in this invention;

[0065] Figure 3 This is a comparison chart of the S2 path node optimization strategy results in this invention;

[0066] Figure 4 This is a running example of the S4 algorithm in this invention. Detailed Implementation

[0067] The following specific examples illustrate the implementation of the present invention. Those skilled in the art can easily understand other advantages and effects of the present invention from the content disclosed in this specification. The present invention can also be implemented or applied through other different specific embodiments, and various details in this specification can be modified or changed based on different viewpoints and applications without departing from the spirit of the present invention. It should be noted that the illustrations provided in the following embodiments are only schematic representations of the basic concept of the present invention. Unless otherwise specified, the following embodiments and features can be combined with each other.

[0068] The accompanying drawings are for illustrative purposes only and are schematic diagrams, not actual pictures. They should not be construed as limiting the invention. To better illustrate the embodiments of the invention, some parts in the drawings may be omitted, enlarged, or reduced, and do not represent the actual product dimensions. It is understandable to those skilled in the art that some well-known structures and their descriptions may be omitted in the drawings.

[0069] In the accompanying drawings of the embodiments of the present invention, the same or similar reference numerals correspond to the same or similar components. In the description of the present invention, it should be understood that if terms such as "upper," "lower," "left," "right," "front," and "rear" indicate the orientation or positional relationship based on the orientation or positional relationship shown in the drawings, they are only for the convenience of describing the present invention and simplifying the description, and do not indicate or imply that the device or element referred to must have a specific orientation, or be constructed and operated in a specific orientation. Therefore, the terms used to describe positional relationships in the drawings are only for illustrative purposes and should not be construed as limiting the present invention. For those skilled in the art, the specific meaning of the above terms can be understood according to the specific circumstances.

[0070] The UAV path optimization method based on the improved 3D D Lite algorithm includes the following steps:

[0071] S11: Based on the environment required for pathfinding, the environment is rasterized into voxels and a three-dimensional matrix is ​​established to separate and identify obstacles from passable areas.

[0072] S12, as Figure 1 As shown, based on the pathfinding characteristics in three-dimensional space, the 26-directional neighbor node expansion in three-dimensional space starts from the endpoint.

[0073] S13: After the neighbor node expansion is completed, based on the 3D grid map established in step S101, detect whether these neighbor nodes are obstacles. Assign corresponding position values ​​to neighbor nodes that are not obstacles.

[0074] S14. There are 27 possible paths between voxel grids that pass through two or more obstacles. When selecting the corresponding parent node, the extended node should exclude these.

[0075] S15, After the expansion node and obstacle detection are completed, the corresponding node is placed into the optimized priority queue, such as... Figure 2 As shown, this completes one node expansion. The above process starts from the endpoint and expands sequentially to the initial node according to the dynamic arrangement order of the priority queue, thus completing one pathfinding process.

[0076] S21, after pathfinding is complete, the path is finished according to the node inheritance relationship until the destination is reached, resulting in the path node parentlist, such as... Figure 3 As shown.

[0077] S22, set two position pointers x1 and x2 to record the current node position during the detection process. Set x1 to point to S... start And set x2 to point to the node following it in the parentlist.

[0078] S23, perform reachability tests on the coordinates of the positions pointed to by pointers x1 and x2 in sequence, until the test result is unreachable, then change the pointer of x2 to point to the previous node S of the currently tested node. i The nodes pointed to by x1 and x2 are directly changed to direct inheritance relationships, thus completing one optimization of the path nodes.

[0079] S24: Iterate through the process described in step S2 until pointer x2 points to the destination and the reachability check of the two nodes currently pointed to by x1 and x2 passes, at which point the path node optimization algorithm ends. Obtain the new path nodes from the starting point to the destination based on the inheritance relationship; this is the shortest path.

[0080] S31, considering the differential flatness of the UAV, transforms the 12-dimensional full-state space into a four-dimensional flat output space. Perform the calculation.

[0081] S32, based on the UAV's relevant spatial state information, the path set generated by the path planning in step S2 is interpolated according to a fixed step size l. The resulting point set after interpolation is stored in set P, then P = [P0, P1, P2, ..., P...]. n ].

[0082] S41, the set of waypoints P generated after interpolation is divided into n segments based on the number of nodes n+1 in the set, and the trajectory is required to be within a specific time t. n The interior passes through these specific points. Establish the corresponding piecewise polynomial formula.

[0083] S42, by decomposing the polynomial function and taking multiple derivatives, the expressions for the UAV's velocity, acceleration, jerk, and snap for the corresponding path segment can be calculated. To ensure the smoothness of the trajectory, the trajectory generation problem is formulated as an optimization problem with a fourth-order derivative objective function, based on the polynomial differentiation formula.

[0084] S43. Establish the constraints for this optimization problem based on the following actual situation:

[0085] (1) The trajectory planning of a drone is to start flying at a fixed starting point with no speed and stop at the target point.

[0086] (2) In order to ensure the continuity of the trajectory, the position, velocity, acceleration, jerk, and snap of the UAV at the starting point of each trajectory segment must be consistent with the end point of the previous trajectory segment.

[0087] (3) In order to ensure that the trajectory generated by the UAV matches the path nodes generated by the original interpolation method and to ensure the safety of the UAV, a safe corridor constraint needs to be applied.

[0088] S44. Solve the above optimization problem with constraints to obtain the coefficients of each segment of the polynomial function. Substitute the time parameter into each segment of the polynomial function, and the resulting set of values ​​is the set of trajectory points of the UAV. Example results of algorithm execution are shown below. Figure 4 As shown.

[0089] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and are not intended to limit it. Although the present invention has been described in detail with reference to preferred embodiments, those skilled in the art should understand that modifications or equivalent substitutions can be made to the technical solutions of the present invention without departing from the spirit and scope of the present invention, and all such modifications or substitutions should be covered within the scope of the claims of the present invention.

Claims

1. A method for UAV path optimization based on an improved 3D D Lite algorithm, characterized by: The method includes the following steps: S1: Extend the D Lite algorithm to three-dimensional space and use a minimum binary heap data structure model to optimize the priority queue of the algorithm; S2: Based on the S1 3D algorithm, a path node optimization strategy is introduced to optimize the path nodes generated by 3D D Lite; The path node optimization strategy is as follows: S201: A set of search nodes for a chain-like path formed by connecting parent nodes sequentially from the starting point to the ending point, according to the path generation rules of the D Lite algorithm. ,in and These are the starting point and the ending point, respectively, and the nodes along the path are indicated by numbers in the middle. S202: From Start to In this approach, reachability tests are performed on every pair of nodes until a test result indicates that the node is unreachable. Then, the parent node pointer of the starting node is changed to point to the node preceding the currently tested node. The characteristics of using a two-node reachability test are as follows: (1) Change of inheritance relationship: If there is a test between two nodes, the relationship between the two nodes will be changed to a direct inheritance relationship, and the redundant node in the middle will be deleted. If there is no test, it indicates that there is an obstacle between the two nodes. (2) The presence of obstacles between two nodes is detected by the line-of-sight (LOS) algorithm. (3) An improved spatial sampling strategy is added to the two-dimensional line-of-sight detection algorithm to enable the improved line-of-sight detection algorithm to detect in three-dimensional space; S203: From Start iterating through S202 sequentially until a node's parent pointer points to a node in the reachability test. This indicates that a walkable path is generated from the starting point to the ending point; S3: The UAV motion planning is transformed into a four-dimensional flat output space for calculation, and interpolation is performed on the path nodes obtained in S2; the interpolation operation on the path generated by the improved algorithm is specifically as follows: S301: Consider that the UAV's full state space is 12-dimensional, as shown in the following formula, which represent position, Euler angles, velocity, and angular velocity, respectively; In the formula, the three parameters are grouped together. These represent the UAV in the world coordinate system. axis, shaft and The position of the axis These represent roll angle, pitch angle, and yaw angle, respectively. These represent drones in axis, shaft and The speed of the shaft, These represent the corresponding angular velocities for roll, pitch, and yaw, respectively. S302: Based on the differential flatness characteristics of UAVs, the 12-dimensional full-state space is transformed into a four-dimensional flat output space. The four dimensions of the σ space represent the positions of the x-axis, y-axis, and z-axis in the world coordinate system, as well as the yaw angle of the UAV itself. The remaining state variables are obtained by transforming the four-dimensional variables and their derivatives into algebraic functions. S303: Based on the spatial state information of the UAV, a set of paths is generated through path planning, and then the paths are defined according to a fixed step size. Perform interpolation and store the resulting point set into a set. Then there is ;in, It contains 3D maps axis, shaft and The coordinates of the starting point of the path on the axis. It contains 3D maps axis, shaft and The coordinates of the endpoint of the path on the axis. It is formed after interpolation. axis, shaft and The coordinates of the intermediate node along the axis's path; S4: Combining the seventh-order polynomial interpolation method of Minimum Snap, the path is smoothed under the premise of satisfying the trajectory collision-free constraint, generating a drone-running trajectory with continuous position, velocity, acceleration and jerk at each interpolation point.

2. The UAV path optimization method based on the improved 3D D Lite algorithm according to claim 1, characterized in that: S1 specifically includes the following steps: S101: Based on the flight characteristics of UAVs, construct a three-dimensional raster voxel environment for path planning algorithms to use for pathfinding; S102: The D Lite algorithm, which is a traditional two-dimensional path planning algorithm, expands the search direction from the original eight-directional neighbor nodes in the plane to 26-directional neighbor nodes in the three-dimensional space according to the path finding characteristics in three-dimensional space. S103: After the neighbor nodes are expanded, based on the 3D grid map established in S101, detect whether these neighbor nodes are obstacles; assign corresponding position values ​​to neighbor nodes that are not obstacle nodes. S104: After S103 detection, a comparison and elimination process is carried out in sequence to ensure that the path generated in three-dimensional space is a safe and collision-free path; S105: After the expansion node and obstacle detection are completed, the corresponding node is placed into the priority queue.

3. The UAV path optimization method based on the improved 3D D Lite algorithm according to claim 1, characterized in that: In S1, the priority queue of the three-dimensional D Lite algorithm has the following characteristics: It is a priority data structure built on a minimum binary heap; It is a queue that uses an array for storage; Its data structure guarantees that the first element of the array is the smallest key value, and the time complexity of obtaining the smallest key value is only [time complexity missing]. .

4. The UAV path optimization method based on the improved 3D D Lite algorithm according to claim 1, characterized in that: S4 specifically includes: S401: Based on the set of waypoints generated in S3, based on the set Number of nodes in Divide the overall path into Segment, and require the trajectory to be within a specific time. The interior passes through these specific points; the piecewise polynomial formula is expressed as follows: In the formula, Indicates the flight time of the drone. to This indicates the time when the drone passes through each endpoint of the path. Indicates the first Segment path polynomial, For the first The polynomial expression coefficients of the segment are then: By taking multiple derivatives of the above equation, the expression for the unmanned snap of the corresponding path segment can be calculated: The above formula is for the first Segment path polynomial The fourth derivative is obtained, where is the fourth-order separation constant of the polynomial coefficients, k is the order of differentiation, and the control variable is time t; S402: Based on the polynomial derivative, the state information of the UAV at a certain moment, including velocity, acceleration, jerk, and snap, is obtained. To ensure accurate UAV control, the trajectory generation problem is formulated as an optimization snap problem with the fourth derivative as the objective function, as shown in the following formula: In the formula, For step S01 The expression, whose position guarantees the non-negativity of the operand, needs to be squared; Indicates the flight time of the drone. and These represent the start and end times of the flight, respectively. to The intermediate value represents the time when the drone passes the endpoint of each path segment; S403: Based on the constraints of the UAV's start and end flight states, the constraint of the continuity of the UAV's state number at a specific point, and the constraint of the collision-free safety corridor, establish the corresponding equations and inequalities: In the formula, and These are the coordinates of the starting point and the ending point, respectively. and Represents the start segment polynomial expression and the end segment polynomial expression; Indicates the corresponding The i-th segment of time The k-th derivative of a polynomial; Indicates the safe corridor distance; S404: Solve the above optimization snap problem to obtain the shortest collision-free trajectory from the starting point to the ending point.

5. A computer-readable storage medium storing a computer program, characterized in that: When the computer program is executed, it can implement the path optimization method according to any one of claims 1 to 4.

Images