A fast method for generating the shortest path of multiple waypoints based on fixed-wing UAV

Abstract

This paper proposes a fast path generation algorithm for a given set of waypoints with initialized start and end heading angles. This algorithm considers the kinematic constraints of fixed-wing drones and the directional constraints of the start and end points, generating a smooth trajectory using the Dubins curve. We decouple a set of waypoints into subpaths consisting of paths between two points. We mathematically prove that the shortest path type is CS (where C represents a curve and S represents a straight line) when the end heading angle is not fixed. Furthermore, we expand the two-point approach to three points, fixing the start and end positions and heading angles, and randomly generating a waypoint as the intermediate point. Since the shortest path between two points exists and is unique, the optimal trajectory for the three points can be determined, demonstrating that RSR outperforms CS. Finally, multiple points are inserted between the start and end points. Using the above conclusions, an iterative method is used to determine the heading of the current task point based on the headings of the previous and next task points, effectively shortening the total path length. This shortened solution is then used as the initial solution, and a neighborhood search algorithm is used to iteratively optimize the solution until convergence. Because of the excellent initial solution, our algorithm can quickly converge to the optimal value.

Description

Technical Field

[0001] The present invention relates to the field of multi-waypoint path planning for fixed-wing UAVs. It is mainly used to plan a trajectory that meets the flight requirements of fixed-wing UAVs with low time complexity. By optimizing the heading of intermediate waypoints, the driving distance can be reduced, making the flight more efficient. Background Art

[0002] Unmanned aerial vehicle (UAV) technology has seen widespread development in recent years. However, due to energy constraints, UAVs' ability to execute missions is limited. Planning an optimal path for them can significantly improve their efficiency. Since UAVs may need to add additional mission points during their missions, this places high demands on the real-time performance and computational complexity of the planning algorithm. Summary of the Invention

[0003] Purpose of the invention: In the past, the path planning of UAVs may have only considered the shortest distance of the optimized path and traversed it from the perspective of discrete waypoints. However, when the number of waypoints increases and the accuracy of discreteness improves, the combination of all solutions will increase exponentially, and the time of the path planning algorithm will increase exponentially, which is not feasible for tasks with high real-time requirements.

[0004] To address this technical problem, this paper proposes a fast multi-waypoint shortest path generation algorithm for fixed-wing UAVs. This algorithm can plan a UAV path within a limited time, and the algorithm execution time increases linearly with the number of waypoints. Furthermore, it supports adding waypoints during flight without affecting the flight path before the waypoint, thus improving flight efficiency.

[0005] Technical solution: To achieve the above technical effects, the technical solution proposed by the present invention is:

[0006] The algorithm for quickly generating the shortest path of multiple waypoints based on a fixed-wing UAV is characterized by comprising the steps of:

[0007] (1) Construct an initialization waypoint model. This model is mainly aimed at the multi-waypoint path planning problem of fixed-wing UAVs, taking into account the directional constraints of the start and end points as well as the UAV's own kinematic constraints:

[0008] 1) Specify the location and direction of the starting point and end point. The location information of the starting point and end point is expressed as follows:

[0009] P s (x s ,y s ,θ s ), P f (x f ,y f ,θ f ) (1)

[0010] 2) There are several waypoints between the starting point and the midpoint. The location information of the midpoints is as follows:

[0011] P i (x i ,y i )i∈{1,2,3......} (2)

[0012] where θ s and θ f are the directions of the starting point and the end point respectively. There is at least one intermediate point and the direction is not specified. (x, y) is the two-dimensional plane R 2 Our goal is to optimize the heading of each intermediate waypoint so that the path through all waypoints is the shortest.

[0013] 3) To generate a smooth flight trajectory, this method introduces the Dubins curve to generate the UAV's flight path. The UAV's configuration can be represented by three state variables: the planar Cartesian inertial coordinates x and y, and the UAV's heading θ. Its kinematic equation is as follows:

[0014]

[0015] Where, V is the speed of the UAV, R min is the minimum turning radius, and c is the servo control coefficient. The flight path derived from this model, namely the Dubins curve, is used for the UAV's path generation.

[0016] (2) In order to make the total path of multiple points as short as possible, we need to optimize the heading of the intermediate waypoints. In this section, we assume that there is only one waypoint in the middle. s (x s ,y s ,θ s ), P m (x m ,y m ) and P f (x f ,y f ,θ f ), here is our objective equation:

[0017]

[0018] L1 and L2 are the distances from the starting point to the middle point and from the middle point to the end point, respectively. Our goal is to minimize the sum of these two distances. To calculate the shortest distance, we must know the turning direction and tangent point, and then determine the optimal direction. To quickly calculate the optimal direction, we designed a fast matching algorithm for optimal path types.

[0019] 1) Quickly determine the center and tangent point of the turning circle:

[0020] In the set coordinate system, the direction is 0 to 180 degrees when it is above the x-axis, and 0 to -180 degrees when it is below the x-axis. The angle between the line connecting the starting point and the midpoint and the positive direction of the x-axis is β. If β is above the x-axis and β∈[θ s -180, θ s ], turn left. If β is below the x-axis and β∈[θ s ,θ s +180] turn right, After determining the turning circle, we can obtain the two tangent points T1 and T2 where the straight line passing through the midpoint is tangent to the turning circle. represents the direction vector of the starting point, Represents the direction vector from the starting point to the tangent point T1, Represents the direction vector from the starting point to the tangent point T2, Represents the angle between two direction vectors. If The tangent point is T1, otherwise it is T2. The following are the calculation formulas for the coordinates of the center of the right-turning circle and the center of the left-turning circle:

[0021]

[0022] 2) Quickly determine the optimal heading range for intermediate waypoints:

[0023] According to the degradation model of Dubins curve, m When the direction of the point is uncertain, P s to P m The shortest path is composed of the straight line segment through the end point and the tangent point of the turning circle and the arc from the starting point to the tangent point. m The direction of That is the slope of the tangent, and the shortest path at this time exists and is unique. f to P m The shortest path P m The opposite direction of the direction is recorded as when When two paths (P s →P m and P m →P f ) can simultaneously achieve the shortest, P m The direction of The total path is the shortest. When , the two paths cannot reach the shortest point at the same time, so P m The best direction must be superior( and The size relationship of is not fixed and is determined by the turn type). Because when P m Not present When , according to the existence and uniqueness theorem, the lengths of L1 and L2 will become larger, so the sum of L1 and L2 cannot be minimized.

[0024] 3) Optimal path type fast matching algorithm:

[0025] After determining the optimal heading for the intermediate waypoint, we can know that when P m The angle at The type of the entire path is C1S2C3S4C5 (C represents an arc and S represents a straight line). C3 consists of two arcs. Since the two arcs have the same turning direction and the same turning radius, they can be merged into one arc. Depending on the turning direction, the specific path type (R represents a right turn and L represents a left turn) is: RSRRSR, RSLLSR, RSLLSL, LSRRSR, LSLLSL, LSLLSR, RSRRSL, LSRRSL. Taking the RSRRSR path type as an example, calculate its total path length:

[0026]

[0027] From the above formula, we can see that because θ s and θ f It is a constant, and the length of the total path is only related to the distance between the centers of the turning circles at the three points.

[0028] Depending on the type of path, the center of the circle selected when calculating the path length is also different. The direction of the turning circle at the start and end points depends on the direction of the turning circle at the intermediate point, so the following table can be obtained:

[0029]

[0030] Table 1 Path quick matching table

[0031] The fixed points represent the coordinates of the center of the turning circle at the starting point and the end point, and the moving point represents the coordinates of the center of the turning circle at the intermediate point. If P m It is a right turning circle, so when we calculate the optimal angle P s and P f The turning circle at the point is also the right turning circle even if it may actually be a left turn. m The turning direction at and The size relationship is determined by Select the right turning circle when Select the left turning circle.

[0032] 4) Calculate the optimal heading:

[0033] according to and The size relationship of the comparison table 1 quickly obtains the fixed points and moving points. The complete path length can be known from (6). The main thing is to optimize the distance from the two fixed points to the moving point. The directions of the starting point and the end point are fixed, so their turning circle centers are also fixed. Because P m The optimal heading at So P m The center of the turning circle at is changing. m The heading changes at P m The center of the turning circle is at P m The problem is thus transformed into the shortest distance problem from two fixed points to the circle. Using the property of light reflection and the shortest optical path, we know that the sum of the distances is the shortest when the normal of the tangent line on the circle bisects the angle formed by the fixed point to the tangent point. From the tangent point, we can get P m The optimal direction θ m .

[0034] (3) Expand the three-point shortest path problem to the multi-waypoint (number of waypoints greater than 3) shortest path problem. In the three-point shortest path, we know that the optimal heading is within a certain angle range. Therefore, we propose an angle iteration algorithm to make the headings of the intermediate waypoints fall within this range to optimize the length of the entire path. The specific steps are as follows:

[0035] Step 1: When calculating the shortest path from the starting point to the first intermediate waypoint, the heading of the waypoint is recorded as When calculating the shortest path from the end point to the last intermediate waypoint, the opposite direction of the last intermediate waypoint is recorded as

[0036] Step 2: When calculating the shortest distance from the first intermediate waypoint to the second intermediate waypoint, the second intermediate waypoint heading is recorded as

[0037] Step 3: and The middle value of and will As the actual heading of the first waypoint. And use this heading as the heading of the first intermediate waypoint to calculate the heading to the second intermediate waypoint again Replace the

[0038] Step 4: Determine whether the waypoint after the second waypoint is the end point. If it is the end point, and The middle value is used as the true heading of the second intermediate waypoint. If it is not the end point, mark the starting point as the planned point and execute step 1 to calculate the true heading of the remaining waypoints.

[0039] Step 5: Obtain the initial solution and the optimal solution angle range list.

[0040] (4) We need to iteratively optimize the initial solution to find a better set of solutions within the optimal solution angle range. Here we use the domain search algorithm. The specific steps are as follows:

[0041] Step 1: Set the discrete precision σ, that is, perform discrete operations on the optimal angle range according to the precision, and set the number of iterations m;

[0042] Step 2: Add or subtract 2σ from the initial solution at the first waypoint, recalculate the path length, and compare it with the initial solution. If it is shorter, continue adding or subtracting 2σ until the length no longer decreases. At this time, add or subtract σ to replace the heading of the initial solution at that point;

[0043] Step 3: Repeat step 2 for all subsequent intermediate waypoints;

[0044] Step 4: Iterate steps 2 and 3 m times. BRIEF DESCRIPTION OF THE DRAWINGS

[0045] Figure 1 It is a path planning flowchart.

[0046] Figure 2 It is a schematic diagram of two-point Dubins path planning.

[0047] Figure 3 This is a path diagram of the Dubins degradation model (the end point direction is not fixed).

[0048] Figure 4 It is a schematic diagram of the optimal range of the heading of the intermediate waypoint.

[0049] Figure 5 It is the optimal positioning map of the intermediate waypoint heading.

[0050] Figure 6 This is a comparison chart of multi-waypoint path planning trajectories.

[0051] Figure 7 It is a comparative advantage graph of multi-waypoint path planning trajectories. DETAILED DESCRIPTION

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

[0053] Figure 1 To create the algorithm flow chart, we first need to set some necessary parameters, such as the coordinates of the initial waypoint and the directions of the start and end points. We first determine the number of waypoints. If there are only three waypoints, we can directly determine the optimal heading for the intermediate points and then use the Dubins path generation algorithm to generate the path. If there are more than three waypoints, we optimize the heading for each intermediate point based on the shortest path between the three waypoints and finally generate the complete path using the Dubins path generation algorithm.

[0054] Figure 2 An example of a two-point Dubins path of RSR type:

[0055] As shown in the figure, it can be seen that it consists of an arc and straight line segments The central angles of the arcs are and The straight line segment can be regarded as the center of two turning circles and The circle formed by the drone turning left along the nose direction is called the left turning circle, and the circle formed by turning right is called the right turning circle.

[0056] Figure 3 is the two-point Dubins path degradation model:

[0057] Will Figure 2 When the destination direction is changed to unfixed, the shortest path is as follows Figure 3 As shown, the shortest path exists and is unique. The shortest path is the path passing through the end point P. m The length of the path that is tangent to the turning circle, and the end point direction is recorded as β2. m The direction θ m When it is greater than β2, the Dubins curve type is RSL (R is right turn, L is left turn, S is straight line). m The direction θ m When it is less than β2, the Dubins curve type is RSR.

[0058] Figure 4 For the optimal direction quick positioning map, when the end point direction is not fixed, the shortest path from the starting point to the end point is as follows Figure 3 As shown, the shortest path exists and is unique. s and the end point P f The tangent of the turning circle intersects at P m , so that the shortest paths from the starting point to the middle point and from the end point to the middle point can be obtained and recorded as L1 and L2 respectively. Taking RSRRSR as an example, Figure 4 Taking RSRRSR as an example, we can get mThe maximum incident angle θ at sm and the minimum emission angle θ mf When θ sm =θ mf When θ , both paths can be shortened at the same time, and the total length is also the shortest. sm ≠θ mf When we get an angle interval [θ mf ,θ sm ], since the shortest path exists and is unique, so when θ m >θ sm or θ m <θ mf , L1 and L2 will become larger, which is known from the properties of the derivative function, so when When L1+L2 is minimized, it is impossible to achieve the minimum. Therefore, in order to minimize L1+L2, P m The heading angle θ at m It must be [θ mf ,θ sm ]superior.

[0059] Figure 5 The idea of ​​the shortest light path in physics is used to calculate the best course. Figure 4 The range of the optimal angle has been determined, and the calculation of the shortest path can be transformed into the problem of the shortest sum of the distances from two fixed points to the arc, such as Figure 5 As shown in Figure 2, the path is shortest when ∠α1=∠α1, where l1 is the tangent line on the circle and l2 is the normal line perpendicular to l1.

[0060] Figure 6 This is the optimized path. We set the start and end points and their directions, and randomly generated 8 intermediate waypoints. The red one is the path generated by our algorithm, and the blue one is the path generated by the less optimized algorithm.

[0061] Figure 7 The comparison of the path lengths of the two algorithms shows that our algorithm is effective. The horizontal axis is the number of intermediate waypoints. It can be seen that the optimization effect becomes more obvious as the number of intermediate waypoints increases.

[0062] The above is only a preferred embodiment of the present invention. It should be pointed out that for ordinary technicians in this technical field, several improvements and modifications can be made without departing from the principles of the present invention. These improvements and modifications should also be regarded as the scope of protection of the present invention.

Claims

1. A method for rapidly generating the shortest path of multiple waypoints based on a fixed-wing UAV, comprising the following steps: (1) Construct an initialization waypoint model. This model is mainly aimed at the multi-waypoint path planning problem of fixed-wing UAVs, taking into account the directional constraints of the start and end points as well as the UAV's own kinematic constraints: 1) Specify the location and direction of the starting point and end point. The location information of the starting point and end point is expressed as follows: P s (x s y s ,θ s ),P f (x f y f ,θ f ) (1) 2) There are several waypoints between the starting point and the midpoint. The location information of the midpoints is as follows: P i (x i ,y i )i∈{1,2,3……} (2) where θ s and θ f are the directions of the starting point and the end point respectively. There is at least one intermediate point and the direction is not specified. (x, y) is the two-dimensional plane R 2 Cartesian coordinates on ; 3) To generate a smooth flight trajectory, the method introduces the Dubins curve to generate the UAV's flight path. The UAV's configuration can be represented by three state variables: the plane Cartesian inertial coordinates x and y, and the UAV's heading θ. Its kinematic equation is as follows: Where, V is the speed of the UAV, R min is the minimum turning radius, c is the servo control coefficient; the flight path derived from 1) and 2) into 3) is the Dubins curve, which is used to generate the UAV path; (2) There is only one waypoint in the middle; the three points are P s (x s ,y s ,θ s ), P m (x m ,y m ) and P f (x f ,y f ,θ f ), here is our objective equation: Where L1 and L2 are the distance from the starting point to the middle point and the distance from the middle point to the end point respectively; 1) Quickly determine the center and tangent point of the turning circle: In the set coordinate system, the direction is 0 to 180 degrees when it is above the x-axis, and the direction is 0 to -180 degrees when it is below the x-axis; the angle between the line connecting the starting point to the midpoint and the positive direction of the x-axis is β; if β is above the x-axis and β∈[θ s -180, θ s ], turn left. Turn right when β is below the x-axis and β∈[θ s ,θ s +180] turn right, Turn left when the turning circle is determined; after determining the turning circle, the two tangent points T1 and T2 where the straight line passing through the midpoint is tangent to the turning circle can be obtained; represents the direction vector of the starting point, Represents the direction vector from the starting point to the tangent point T1, Represents the direction vector from the starting point to the tangent point T2, represents the angle between two direction vectors; if Then the tangent point is T1, otherwise it is T2; the following are the calculation formulas for the coordinates of the center of the right-turning circle and the center of the left-turning circle: 2) Quickly determine the optimal heading range for intermediate waypoints: According to the degradation model of Dubins curve, m When the direction of the point is uncertain, P s to P m The shortest path is composed of the straight line segment through the end point and the tangent point of the turning circle and the arc from the starting point to the tangent point. m The direction of That is the slope of the tangent, and the shortest path at this time exists and is unique; by the same token, we can know that P f to P m The shortest path P m The opposite direction of the direction is recorded as when When there are two paths, P s →P m and P m →P f Can simultaneously achieve the shortest, P m The direction of The total path is the shortest; when When , the two paths cannot reach the shortest point at the same time, so P m The best direction must be superior, and The size relationship of is not fixed and is determined by the type of turn; because when P m Not present When the value is above , according to the existence and uniqueness theorem, the lengths of L1 and L2 will become larger, so the sum of L1 and L2 cannot be minimized. 3) Optimal path type fast matching method: After determining the optimal heading for the intermediate waypoint, we can know that when P m The angle at In the figure above, the type of the entire path is C1S2C3S4C5, where C represents an arc and S represents a straight line. C3 consists of two arcs, which can be merged into one because they have the same turning direction and the same turning radius. Depending on the turning direction, the specific path type is: RSRRSR, RSLLSR, RSLLSL, LSRRSR, LSLLSL, LSLLSR, RSRRSL, LSRRSL. Taking the RSRRSR path type as an example, calculate its total path length: From the above formula, we can see that because θ s and θ f It is a constant value, and the length of the total path is only related to the distance between the centers of the turning circles at the three points; Depending on the type of path, the center of the circle selected when calculating the path length is also different. The direction of the turning circle at the start and end points depends on the direction of the turning circle at the intermediate point, so the following table can be obtained: Table 1 Path quick matching table The fixed points represent the coordinates of the center of the turning circle at the starting point and the end point, and the moving point represents the coordinates of the center of the turning circle at the intermediate point. If P m It is a right turning circle, so when we calculate the optimal angle P s and P f The turning circle at the point is also the right turning circle even if it may actually be a left turn; m The turning direction at and The size relationship is determined by Select the right turning circle when Choose the left turning circle; 4) Calculate the optimal heading: according to and The size relationship comparison table 1 quickly obtains fixed points and moving points. The complete path length is known from (6). The directions of the starting point and the end point are fixed, so the centers of their turning circles are also fixed; because P m The optimal heading at So P m The center of the turning circle at P is variable; m The heading changes at P m The center of the turning circle is at P m = On the arc with the center of the circle; Therefore, the problem is transformed into the shortest distance problem from two fixed points to the circle. Using the property of light reflection and the shortest optical path, we know that the sum of the distances is the shortest when the normal of the tangent on the circle bisects the angle formed by the fixed point to the tangent point; From the tangent point, we can get P m The optimal direction θ m ; (3) Expand the three-point shortest path problem to the multi-waypoint shortest path problem. When the number of waypoints is greater than 3, we know that the optimal heading in the three-point shortest path is within a certain angle range. Therefore, we propose an angle iteration method to make the headings of the intermediate waypoints fall within this range to optimize the length of the overall path. The specific steps are as follows: Step 1: When calculating the shortest path from the starting point to the first intermediate waypoint, the heading of the waypoint is recorded as When calculating the shortest path from the end point to the last intermediate waypoint, the opposite direction of the last intermediate waypoint is recorded as Step 2: When calculating the shortest distance from the first intermediate waypoint to the second intermediate waypoint, the second intermediate waypoint heading is recorded as Step 3: and The middle value of and will As the actual heading of the first waypoint; and use this heading as the heading of the first intermediate waypoint to calculate the heading to the second intermediate waypoint again Replace the Step 4: Determine whether the waypoint after the second waypoint is the end point. If it is the end point, and The middle value is used as the true heading of the second intermediate waypoint. If it is not the end point, mark the starting point as the planned point and execute step 1 to calculate the true heading of the remaining waypoints. Step 5: Obtain the initial solution and the optimal solution angle range list; (4) For the initial solution, we need to iteratively optimize it to find a better set of solutions within the optimal solution angle range. Here we use the domain search method. The specific steps are as follows: Step 1: Set the discrete precision σ, that is, perform discrete operations on the optimal angle range according to the precision, and set the number of iterations m; Step 2: Add or subtract 2σ from the initial solution at the first waypoint, recalculate the path length, and compare it with the initial solution. If it becomes shorter, continue adding or subtracting 2σ until the length no longer decreases. At this time, add or subtract σ to replace the heading of the initial solution at that point. Step 3: Repeat step 2 for all subsequent intermediate waypoints; Step 4: Iterate steps 2 and 3 m times.

2. The method for rapidly generating the shortest path of multiple waypoints based on a fixed-wing UAV according to claim 1 is characterized in that: This method quickly plans a feasible path that meets the kinematic constraints of a fixed-wing UAV in a short period of time; and in the initial solution of this method, the heading of each intermediate waypoint is only related to the headings of adjacent waypoints, reducing the correlation between the waypoint headings.

Images