Multi-unmanned aerial vehicle formation path planning method based on compensation look-ahead algorithm

Abstract

The invention discloses a multi-unmanned aerial vehicle formation path planning method based on a compensation look-ahead algorithm. The method comprises the following steps: S1, obtaining coordinates of all task target points; s2, randomly generating an initial task target point sequence corresponding to a single unmanned aerial vehicle; s3, the unmanned aerial vehicle track between the coordinates of the task target points is set to be a straight line, the task planning model is solved through a bionic algorithm, the cost value is the sum of the straight line distances between the coordinates of the task target points, the path planning result of the task target points is obtained, and a plurality of unmanned aerial vehicle target subsets are obtained; the obtained target subset serves as a target point set needing to be solved; and S4, in each subset Subi, solving a large-scale unmanned aerial vehicle formation cooperative path planning problem for the target point sequence through a compensation look-ahead algorithm, and obtaining an unmanned aerial vehicle formation planning path result meeting requirements. According to the method, the path of the target point can be more efficiently planned, and the calculation efficiency of the system model is optimized by primarily distributing the target point to be distributed.

Description

Technical Field

[0001] The present invention belongs to the field of UAV formation path planning, and in particular relates to a multi-UAV formation path planning method based on a compensated look-ahead algorithm. Background Art

[0002] UAVs are widely used in the fields of coordinated reconnaissance, coordinated strike, flight performances, express logistics and agricultural spraying, which has also led to the control system based on multi-UAV clusters (UAS) becoming the focus of research at home and abroad. In the field of UAV research, path planning is an important research content. A common problem is how to determine the target sequence and obtain the shortest flight path or time. This problem is similar to the multi-traveling salesman problem (MTSP), a variant of the commonly studied traveling salesman problem (TSP), but there are also some differences. In the study of the MTSP problem, the path between the target points is distributed according to geometric straight lines, while the movement of the UAV is a curved motion constrained by the maximum curvature. In particular, fixed-wing UAVs have bounded trajectory curvature, and the curvature setting in the model needs to be more in line with actual needs. Therefore, in research, researchers usually replace the path planning of UAVs with Dubins curves. Although the UAV path planning problem is similar to the MTSP problem, it is often more difficult to solve due to kinematic constraints such as curvature, which requires researchers to propose new solutions.

[0003] Combining the Dubins problem with the MTSP problem is called solving the DTSP problem. Solving the DTSP problem requires not only determining the target sequence, but also different heading angles will lead to different path planning. Therefore, the heading angle must be determined before the target sequence is determined. The method of violently solving the DTSP problem through a greedy algorithm is not advisable. As the number of target points increases, the amount of calculation increases exponentially. Therefore, solving a predetermined target sequence is a common method for solving path planning problems constrained by curvature.

[0004] Ma et al. (2006) proposed an algorithm based on the retreat principle in the article to calculate the smooth path through the sequence of path points, but the authors have not yet developed a complete formula that can characterize the three path points and the initial direction of all possible geometric configurations, so as to uniquely determine the optimal trajectory for any problem; Pantelis Isaiah et al. (2015) combined the k-step prediction and the horizon retreat principle, disrupted the target sequence, and proposed a k-step preview algorithm, but this algorithm is only applicable to the case when there are fewer single-machine target points; Izack Cohen et al. (2017) proposed a new type of discretization-based foresight algorithm (DLAAs), which discretized the HOA of each waypoint to form an integer optimization plan, and used an integer programming solver to calculate its solution, but this algorithm has the problems of large computational complexity and low computational efficiency in large-scale formation calculations.

[0005] In summary, when facing the growing demand for large-scale formation collaborative task planning, the existing technology has the problems of large amount of calculation and low calculation efficiency in the model. How to reduce the calculation scale of single-machine path planning while ensuring that the scale of the target point set remains unchanged, and how to strike a balance between the amount of calculation and the optimized path are the key points that need to be solved. Summary of the invention

[0006] In order to overcome the shortcomings of the above-mentioned prior art, the purpose of the present invention is to provide a multi-UAV formation path planning method based on a compensated look-ahead algorithm. The planning method adds the influence of the heading angle of any adjacent front and rear point on the selection of the intermediate point on the basis of the three-point curve. Secondly, in the multi-UAV formation, the ETSP algorithm is used to initially allocate the target points to be allocated to form multiple target point subsets, and then the compensated look-ahead algorithm is used to plan the path for each subset, achieving a balance between calculation time and optimized path;

[0007] By adding the fourth target point to optimize the heading angle selection, the target point path can be planned more efficiently, and the calculation efficiency of the system model is optimized by initially allocating the target points to be allocated. In order to achieve the above purpose, the technical solution adopted by the present invention is:

[0008] A multi-UAV formation path planning method based on a compensated look-ahead algorithm comprises the following steps;

[0009] Step S1: Get the coordinates of all task target points, and set the target point coordinate set as Target = [(x1, y1), (x2, y2), ..., (x n ,y n )]∈R 2 , the drone formation set is U;

[0010] Step S2: Randomly generate an initial mission target point sequence corresponding to a single UAV;

[0011] Step S3: The UAV trajectory between the task target point coordinates is set as a straight line (ETSP model), that is, the task planning model. The task planning model is solved and calculated using a bionic algorithm. The cost value is the sum of the straight-line distances between the task target point coordinates. The path planning result of the task target point is obtained, and several UAV target subsets Sub are obtained. i ={(x i ,y i )}∈Target, the obtained target subset is used as the target point set to be solved;

[0012] Step S4: In each subset Sub i Inside the system, a compensated look-ahead algorithm is used to solve the large-scale UAV formation collaborative path planning problem for the target point sequence, and a UAV formation planning path result that meets the requirements is obtained. The UAV formation planning path result includes the mission target point sequence corresponding to each UAV and the heading angle at the corresponding target point.

[0013] In step S1, the target point coordinates are obtained through preset data. In step S2, a corresponding initial mission target point sequence is randomly generated for a single UAV.

[0014] In step S3, a solution method based on ETSP pre-grouping is adopted to simplify the amount of calculation while ensuring the shortest path;

[0015] The total distance from the starting point of the UAV formation to the return of all UAVs is taken as the optimization target of the large-scale UAV collaborative path planning model. The coordinates of all target points are expressed as Target = [(x1, y1), (x2, y2), …, (x n ,y n )] indicates that the distance between two points after the UAV formation mission planning is expressed as:

[0016] d i,j =|x i -x j |+|y i -y j | (1)

[0017] The UAV is a fixed-wing UAV, and the trajectory change of the fixed-wing UAV is a linear change process. The distance formula between the two points is added with constraints that conform to the UAV's motion trajectory.

[0018] The linear variation Dubins curve equation is:

[0019] d i,j =l+r+D (2)

[0020] In formula (2), D represents the straight line part of the Dubins curve between two adjacent task points in the task planning trajectory, and l and r are the arc lengths between the two adjacent task points and the straight line.

[0021] Furthermore, the optimization objective of the UAV formation Dubins multi-traveling salesman model is the shortest flight range of the UAV formation. According to formula (2), the objective function is expressed as follows:

[0022]

[0023] In the above formula (3), k i,j,q represents whether the qth UAV has a planned mission between the ith and jth target points, k i,j,q The value is 0 or 1.

[0024] The constraint function of the constraint condition is as follows:

[0025]

[0026] Formula (4) is the number of targets assigned to each UAV. This constraint ensures that each UAV is assigned no less than k targets;

[0027] Formula (5) is the total number of targets assigned to the drone cluster. This constraint ensures that all target points are assigned;

[0028] Formula (6) is the number of times any target point is assigned to a drone. This constraint ensures that each point is assigned only once;

[0029] Formula (7) is the judgment formula for the starting point and the ending point, (x w ,y w ) is the coordinate of the last target point planned for a single UAV, which requires that the starting point and end point of each aircraft are the same.

[0030] In step S4, the motion trajectory of the fixed-wing UAV changes linearly, and its motion trajectory is defined as a curve:

[0031] X(t)=[x(t),y(t),θ(t)] T ∈R 2 ×S 1 (8)

[0032] The motion of the drone in a two-dimensional environment, x and y represent the Cartesian coordinate system position of the drone, and θ represents the angle of the drone relative to the X-axis, that is, the heading angle of the drone.

[0033] The UAV motion rate in the model is set to 1, and the maximum curvature is 1 / γ, which can be expressed by the following kinematic equation:

[0034]

[0035] Where θ∈[-1 / γ1 / γ], the constant u is the control variable of the UAV that changes with time, |u|≤1.

[0036] In the UAV motion model, the UAV motion rate is set to 1, and the performance functional of the shortest path of the Dubins curve is expressed by integration, as shown in formula (12);

[0037]

[0038] In the above formula, t0 and t f Represents the initial time and terminal time of the drone, set is the parameter of the shortest path. Combining the kinematic equations of the UAV in equations (9), (10), and (11) with the performance functional in equation (12), the Hamiltonian function is constructed according to the theorem in the optimal control theory. That is, the shortest path Hamiltonian function of the UAV is:

[0039]

[0040] According to the Pontryagin maximum principle, the necessary conditions for obtaining the shortest path of the Dubins curve are obtained;

[0041]

[0042] The above formula shows that and In the UAV motion cycle [t0, t f ] is a piecewise constant;

[0043] Since the Hamiltonian in the control function u is linear, there are two types of optimal control;

[0044] if but if u∈[-1, 1], the first condition corresponds to the maximum curvature arc with radius r, and the second condition corresponds to the direction The straight line trajectory.

[0045] The shortest path of the Dubins curve is closely related to the heading angle. Different heading angles will lead to different choices in the path details. Therefore, how to determine the heading angle in the Dubins curve is the key to determining the shortest path for the UAV.

[0046] The middle point (x1, y1) is added between the initial point (x0, y0) and the terminal point (x2, y2) of the Dubins curve, and the boundary relationship of the middle point is obtained;

[0047] The red trajectory is the Dubins optimal path passing through the initial point, the middle point and the terminal point, θ0 is the direction angle of the initial point (x0, y0) (relative to the X axis), θ1 and θ2 are the angles of the line (relative to the X axis), and θ m is the direction angle (relative to the X axis) of the middle point (x1, y1). A line from the middle point (x1, y1) to the intersection point A of the two straight line segments bisects the intersection angle. On the line segment of the optimal path,

[0048]

[0049] From the above formula, we can deduce that k1 / k2=-tan(θ1+θ2) / 2. The Hamiltonian function is continuous on the optimal trajectory of the point (x1, y1), so it can be expressed as follows.

[0050]

[0051] From the above formula, we can know that:

[0052] k1 cosθ m +k2 sinθ m =0 (17)

[0053] From equation (15) and equation (17), we can get equation (18);

[0054]

[0055] The midpoint heading angle of the shortest path is obtained by equation (18), and the solution of the coupled trigonometric equation is obtained;

[0056] Based on the algorithm for calculating the heading angle of the four-point optimization, a general formula for calculating the heading angle of the target point is given:

[0057] First, consider the situation when the distances between the initial point, the middle point, and the terminal point are far. The red solid line is the Dubins trajectory, and point F (x4, y4) and point D (x5, y5) are the tangent points of the two circle trajectories.

[0058] The direction angle θ of the midpoint is solved by equation (18): m However, when the distance between the initial point and the middle point is far, the error between the angle θ1 (relative to the X-axis) of the line segment between point F (x4, y4) and point D (x5, y5) and the angle θ3 (relative to the X-axis) of the line segment (red dashed line segment) between the initial point B (x0, y0) and the tangent point D (x5, y5) is negligible, and the coordinates (x0, y0) of the initial point B are directly substituted into formula (18) to solve the direction angle θ of the middle point. m (relative to the X axis);

[0059] The total distance L between the three points after mission planning is obtained as follows:

[0060]

[0061] In the above formula, a is the distance from the initial point B to the tangent point D, b is the distance from the terminal point C to the tangent point E, α and β are the angles, S 弧DE is the length of arc DE. It can be seen from the formula that the value of L is related to the values ​​of α and β, as shown in formula (20):

[0062]

[0063] It can be seen from the formula that when the value of α+β is smaller and α=β, the value of the formula is the smallest. When the center O is on the angle bisector of angle C, the total flight distance L takes the minimum value, which is formula (19) The heading angle of the middle point (x1, y1)

[0064] Secondly, consider the situation when the distances between the initial point, the middle point and the terminal point are close. When the distance between the initial point and the middle point is close, the error between the angle θ1 (relative to the X-axis) of the line segment where point F (x4, y4) and point D (x5, y5) are located and the angle θ3 (relative to the X-axis) of the line segment where the initial point B (x0, y0) and the tangent point D (x5, y5) are located (red dashed line segment) cannot be ignored. Therefore, the coordinates of the initial point B (x0, y0) cannot be directly substituted into formula (18) to solve the direction angle θ of the middle point. m (Relative to the X-axis) In this case, the heading angles of the initial point and the terminal point will affect the values ​​of θ1 and θ2.

[0065] When the target point is close, the original θ1 and the actual θ3 are quite different. If θ1 is used for calculation, it will cause a large error. The solution of the fully coupled trigonometric equation is difficult to find. A four-point optimized heading angle algorithm is proposed to minimize the deviation caused by the error term.

[0066] When the distance between the initial point and the intermediate point is close, the heading angle of the initial point (x0, y0) will affect the heading angle of the intermediate point (x1, y1). An adjacent point (x6, y6) of the initial point (x0, y0) is introduced. The target point (x6, y6) is preprocessed with the optimization algorithm when the initial point is far from the intermediate point to obtain a tangent point Q1 (x4, y4). This tangent point Q1 (x4, y4) replaces the initial point (x0, y0) as the new target point, and the previous step is performed on the intermediate point (x1, y1). In this way, a heading angle θ of the compensated midpoint is obtained. m, θ1 is the angle of the line segment where the tangent point Q1 (x4, y4) and the tangent point Q2 (x5, y5) are located, θ2 is the angle of the line segment where the tangent point Q3 (x7, y7) and the terminal point C are located, O1 and O2 are the center points of the two circle trajectories, the black solid line is the first step to solve the replacement point Q1, and the black dotted line is the replacement point Q1 instead of the initial point B to solve the direction angle θ of the intermediate point m process.

[0067]

[0068] Beneficial effects of the present invention:

[0069] The present invention has the capability of path planning for large-scale UAV formations in complex environments.

[0070] In order to solve the problems of formation path planning efficiency and model calculation efficiency, the present invention designs a large-scale UAV formation path planning method based on a compensated look-ahead algorithm, thereby improving the formation's collaborative capability.

[0071] The present invention adopts the ETSP pre-allocation model, which greatly reduces the calculation scale of the path planning model and improves the planning efficiency.

[0072] The present invention solves the problem of selecting the optimal heading angle of the UAVs at the mission target point by using a multi-UAV formation path planning method based on a compensated look-ahead algorithm. BRIEF DESCRIPTION OF THE DRAWINGS

[0073] Figure 1 It is the flow chart of ETSP model pre-allocation model.

[0074] Figure 2 It is a schematic diagram of the three-point optimization algorithm.

[0075] Figure 3 It is a schematic diagram of approximate calculation of the three-point optimization algorithm.

[0076] Figure 4 It is a schematic diagram of the compensation look-ahead algorithm. DETAILED DESCRIPTION

[0077] The present invention will be further described in detail below in conjunction with the accompanying drawings.

[0078] like Figure 1 As shown, a multi-UAV formation path planning method based on a compensation look-ahead algorithm comprises the following steps;

[0079] Step S1: Get the coordinates of all task target points, and set the target point set as Target = [(x1, y1), (x2, y2), ..., (x n ,y n )]∈R 2 , the drone formation is set as U.

[0080] Step S2: Randomly generate an initial mission target point sequence corresponding to a single UAV.

[0081] Step S3: Set the UAV trajectory between the mission target points as a straight line (ETSP model), use the bionic algorithm to solve the mission planning model, and the cost value is the sum of the straight-line distances between the mission target points. Get the path planning result of the mission target points and obtain several UAV target subsets Sub i ={(x i ,y i )}∈Target, the obtained target subset is used as the target point set that needs to be solved.

[0082] Step S4: In each subset Sub i Inside, the large-scale UAV formation collaborative path planning problem is solved for the target sequence through the compensated look-ahead algorithm, and the UAV formation planning path results that meet the requirements are obtained, including the mission target point sequence corresponding to each UAV and the heading angle at the corresponding target point;

[0083] The guiding path planning result, including the heading angle, is the result obtained by solving the model of this application. The large-scale UAV formation collaborative path planning problem is solved for the target sequence through the compensation look-ahead algorithm. Obtaining the overall planning path is the purpose of this application.

[0084] In step S1, the target point set is Target=[(x1,y1),(x2,y2),…,(x n ,y n )]∈R 2 , the drone formation is set as U.

[0085] In step S2, a corresponding initial mission target point sequence is randomly generated for a single UAV.

[0086] In step S3, the total distance from the starting point of the drone cluster to the return of all drones is used as the optimization target of the large-scale drone collaborative path planning model. The coordinates of all target points are expressed as Target = [(x1, y1), (x2, y2), ..., (x n ,y n )], then the distance between two points after the UAV cluster mission planning can be expressed as:

[0087] d i,j =|x i -x j |+|y i -y j | (1)

[0088] The drones involved in the present invention are all fixed-wing drones. Fixed-wing drones are different from rotary-wing drones. The track change of fixed-wing drones is a linear change process. Therefore, the distance formula between two points needs to add constraints that conform to the drone's motion trajectory. Here, the Dubins curve with a turning radius of 100 meters is used to simulate the actual track change capability of the drone:

[0089] d i,j =l+r+D (2)

[0090] In formula (2), D represents the straight line part of the Dubins curve between two adjacent task points in the task planning trajectory, and l and r are the arc lengths between the two adjacent task points and the straight line.

[0091] The optimization objective of the UAV cluster Dubins multi-traveling salesman model is the shortest flight range of the UAV cluster. According to formula (2), the objective function can be expressed as follows:

[0092]

[0093] In the above formula (3), k i,j,q represents whether the qth UAV has a planned mission between the ith and jth target points, k i,j,q The value is 0 or 1. The constraint function is as follows:

[0094]

[0095]

[0096] Formula (4) is the number of targets assigned to each drone. This constraint ensures that each drone is assigned no less than k targets. Formula (5) is the total number of targets assigned to the drone cluster. This constraint ensures that all target points are assigned. Formula (6) is the number of times any target point is assigned to a drone. This constraint ensures that each point is assigned only once. Formula (7) is the judgment formula for the starting point and the ending point. (x w ,y w ) is the coordinate of the last target point planned for a single UAV, which requires that the starting point and end point of each aircraft are the same.

[0097] Dijkstra algorithm is a classic algorithm for solving MTSP problem, but it is only applicable to the case of small sample size. The calculation scale increases exponentially with the sample size, so it is not applicable to the current large-scale UAV formation. Discrete DTSP method (DDTSP) is another classic method for dealing with MTSP problem, that is, for each target, consider h possible angles and divide the interval [0,2π) into h subintervals. The obtained discrete DTSP (DDTSP) can be converted into integer linear programming (ILP). The DDTSP method is set as a generalized asymmetric traveling salesperson problem with n clusters and 1+h(n-1) cities (Noon Bean, 1991). Considering the difficulty of calculation, it is necessary to keep a small h value. However, the Dubins function is discontinuous. Therefore, DDTSP approximates the optimal solution of MTSP problem, which requires a larger h value. When the number of task points is small, DDTSP can give the optimal path, but there are still problems of large amount of calculation and low efficiency when there are large task points and multiple traveling salesmen. The solution method based on ETSP pre-grouping is adopted to simplify the calculation amount while ensuring the shortest path.

[0098] In step S4, the motion trajectory of the fixed-wing UAV changes linearly, and its motion trajectory is defined as a curve:

[0099] X(t)=[x(t),y(t),θ(t)] T ∈R 2 ×S 1 (8)

[0100] The present invention only considers the motion of the UAV in a two-dimensional environment, where x and y represent the Cartesian coordinate system position of the UAV, and θ represents the angle of the UAV relative to the X-axis, that is, the heading angle of the UAV.

[0101] The UAV motion rate in the model is set to 1, and the maximum curvature is 1 / γ, so it can be expressed by the following kinematic equation:

[0102]

[0103] Where θ∈[-1 / γ1 / γ], the constant u is the control variable of the UAV that changes with time, |u|≤1.

[0104] In mathematical models, the Dubins curve is usually used to simulate the curved motion of a UAV with a curvature of 1 / γ. JD. Boissonnat et al. gave specific derivations of the Dubins curve for different situations. The Dubins curve is usually divided into the following two major families: CCC family: RLR, LRL; CSC family: RSR, LSL, RSL, LSR. Among them, C represents an arc with a curvature not exceeding 1 / γ, S represents a straight line, L is a counterclockwise curve, and R is a clockwise curve. The Dubins curve draws a circle with the maximum curvature radius tangent to the initial state, and then draws a straight line tangent to the two circles. By determining the coordinate position and heading angle, a shortest path can be determined.

[0105] The Dubins shortest path problem belongs to the problem of solving the minimum value in traditional optimal control. The initial state of the drone is uncertain, the terminal state is also uncertain, and the control vector is a continuous function, which can be solved using the variational method. In the UAV motion model of this paper, the motion rate of the drone is set to 1, and the performance functional of the shortest path of the Dubins curve can be expressed by integration, as shown in Equation (12).

[0106]

[0107] In the above formula, t0 and t f Represents the initial time and terminal time of the drone, set is the parameter of the shortest path,

[0108] Combining the kinematic equations of the UAV in equations (9), (10), and (11) with the performance functional in equation (12), the Hamiltonian function is constructed according to the theorem in the optimal control theory, that is, the shortest path Hamiltonian function of the UAV is:

[0109]

[0110] According to the Pontryagin maximum principle, the necessary conditions for obtaining the shortest path of the Dubins curve are obtained.

[0111]

[0112] The above formula shows that and In the UAV motion cycle [t0, t f ] is a piecewise constant, so it can be expressed as follows.

[0113] Since the Hamiltonian in the control function u is linear, there are two types of optimal control. but if u∈[-1, 1]. The first condition corresponds to the arc of maximum curvature with radius r, and the second condition corresponds to the arc with direction The straight line trajectory.

[0114] Sussmann et al. proved in their 1991 paper that there is a solution to the Dubins path problem, and its optimal path is C α C β C δ or C α S d C β , where 0≤α,δ<2π,π<β<2π, and d≥0. Since the solution of the Dubins path is not unique in a sense, in general, the minimum value can be reached through multiple trajectories. From the above formula (14), it can be seen that the shortest path of the Dubins curve is closely related to the heading angle. Different heading angles will lead to different choices in the path details. Therefore, how to determine the heading angle in the Dubins curve is the key to determining the shortest path of the UAV.

[0115] Ma et al. added an intermediate point (x1, y1) between the initial point (x0, y0) and the terminal point (x2, y2) of the Dubins curve, and obtained the boundary relationship of the intermediate point, such as Figure 2 shown.

[0116] The red trajectory is the Dubins optimal path passing through the initial point, the middle point and the terminal point, θ0 is the direction angle of the initial point (x0, y0) (relative to the X axis), θ1 and θ2 are the angles of the line (relative to the X axis), and θ m is the direction angle of the middle point (x1, y1) (relative to the X axis). Ma et al. concluded that a line from the middle point (x1, y1) to the intersection point A of the two straight line segments bisects the intersection angle, that is, Figure 2 As shown by the blue line segment. On the line segment of the optimal path,

[0117]

[0118] From the above formula, we can deduce that k1 / k2=-tan(θ1+θ2) / 2. The Hamiltonian function is continuous on the optimal trajectory of the point (x1, y1), so it can be expressed as follows.

[0119]

[0120] From the above formula, we can know that:

[0121] k1 cosθ m +k2 sinθ m =0 (17)

[0122] Formula (18) can be obtained from formula (15) and formula (17).

[0123]

[0124] The midpoint heading angle of the shortest path can be obtained by formula (18), and the solution of the coupled trigonometric equation can be completed numerically, but it is difficult to describe with a general formula. Ma et al. did not give a complete formula in the paper that can characterize the three path points and the initial direction of all possible geometric configurations, so as to uniquely determine the optimal trajectory of any problem. The solution of the coupled trigonometric equation can be completed numerically, but it is difficult to describe with a general formula. Therefore, based on the principle of horizon retreat, the present invention proposes an algorithm for calculating the heading angle based on four-point optimization, and gives a general formula for calculating the heading angle of the target point.

[0125] First, consider the situation where the distances between the three task points, namely the initial point, the middle point and the terminal point, are far. Figure 3 is the path planning diagram of the initial point, the middle point and the terminal point. The red solid line is the Dubins trajectory. Point F(x4, y4) and point D(x5, y5) are the tangent points of the two circular trajectories. The direction angle θ of the middle point can be solved by formula (18): m However, when the initial point and the middle point are far apart, the error between the angle θ1 (relative to the X-axis) of the line segment where point F (x4, y4) and point D (x5, y5) are located and the angle θ3 (relative to the X-axis) of the line segment where the initial point B (x0, y0) and the tangent point D (x5, y5) are located (red dashed line segment) can be ignored. It is difficult to solve the coordinates of the tangent point F (x4, y4). Then the coordinates of the initial point B (x0, y0) can be directly substituted into formula (18) to solve the direction angle θ of the middle point. m (relative to the X axis).

[0126] from Figure 3 The total distance L between the three points after mission planning can be obtained as follows:

[0127]

[0128] In the above formula, a is the distance from the initial point B to the tangent point D, and b is the distance from the terminal point C to the tangent point E. It can be seen from the formula that the value of L is related to the values ​​of α and β, as shown in formula (20):

[0129]

[0130] It can be seen from the formula that when the value of α+β is smaller and α=β, the value of the formula is the smallest. It can be seen from the figure that when the center O is on the angle bisector of angle C, the total flight distance L takes the minimum value, which is formula (19) The heading angle of the middle point (x1, y1)

[0131] Secondly, consider the situation when the distances between the three task points, namely the initial point, the middle point and the terminal point, are relatively close. Figure 3 is the path planning diagram of the initial point, the intermediate point and the terminal point, and the red solid line is the Dubins trajectory. However, when the distance between the initial point and the intermediate point is close, the error between the angle θ1 (relative to the X-axis) of the line segment where point F (x4, y4) and point D (x5, y5) are located and the angle θ3 (relative to the X-axis) of the line segment where the initial point B (x0, y0) and the tangent point D (x5, y5) are located (red dashed line segment) cannot be ignored, so the coordinates of the initial point B (x0, y0) cannot be directly substituted into formula (18) to solve the direction angle θ of the intermediate point. m (Relative to the X-axis) In this case, the heading angles of the initial point and the terminal point will affect the values ​​of θ1 and θ2.

[0132] from Figure 3 It can be seen that when the target point is close, the original θ1 and the actual θ3 are quite different. If θ1 is continued to be used for calculation, a large error will be caused. The solution of the fully coupled trigonometric equation is difficult to find. Therefore, a four-point optimized heading angle algorithm is proposed to minimize the deviation caused by the error term.

[0133] When the distance between the initial point and the intermediate point is close, the heading angle of the initial point (x0, y0) will affect the heading angle of the intermediate point (x1, y1). This paper introduces an adjacent point (x6, y6) of the initial point (x0, y0) to Figure 3 The optimization algorithm for when the initial point is far from the middle point pre-processes the target point (x6, y6) to obtain a tangent point Q1 (x4, y4), and uses this tangent point Q1 (x4, y4) to replace the initial point (x0, y0) as the new target point, and performs the previous operation on the middle point (x1, y1), so as to obtain a heading angle θ of the compensation midpoint. m .like Figure 4 As shown, θ1 is the angle of the line segment where the tangent point Q1 (x4, y4) and the tangent point Q2 (x5, y5) are located, θ2 is the angle of the line segment where the tangent point Q3 (x7, y7) and the terminal point C are located, O1 and O2 are the center points of the two circle trajectories, the black solid line is the first step to solve the replacement point Q1, and the black dotted line is the replacement point Q1 instead of the initial point B to solve the intermediate point direction angle θ m process.

[0134]

[0135] On the basis of the three-point curve, the present invention adds the influence of the heading angle of any adjacent front and rear point on the selection of the intermediate point. Secondly, in the multi-UAV formation, the ETSP algorithm is used to initially allocate the target points to be allocated, forming multiple target point subsets, and then the compensation look-ahead algorithm is used to plan the path for each subset, thus achieving a balance between the calculation time and the optimization path.

[0136] By adding the fourth target point to optimize the heading angle selection, the target point path can be planned more efficiently, and the calculation efficiency of the system model is optimized by initially allocating the target points to be allocated.

Claims

1. A multi-UAV formation path planning method based on a compensated look-ahead algorithm. It is characterized in that The steps include: Step S1: Get the coordinates of all task target points, and set the target point coordinate set as Target = [(x 1 ,y 1 ),(x 2 ,y 2 ),…,(x n ,y n )]∈R 2 , the drone formation set is U; Step S2: Randomly generate an initial mission target point sequence corresponding to a single UAV; Step S3: The UAV trajectory between the task target point coordinates is set as a straight line (ETSP model), that is, the task planning model. The task planning model is solved and calculated using a bionic algorithm. The cost value is the sum of the straight-line distances between the task target point coordinates. The path planning result of the task target point is obtained, and several UAV target subsets Sub are obtained. i ={(x i ,y i )}∈Target, the obtained target subset is used as the target point set to be solved; Step S4: In each subset Sub i Inside the system, a compensated look-ahead algorithm is used to solve the large-scale UAV formation collaborative path planning problem for the target point sequence, and a UAV formation planning path result that meets the requirements is obtained. The UAV formation planning path result includes the mission target point sequence corresponding to each UAV and the heading angle at the corresponding target point.

2. According to claim 1, a multi-UAV formation path planning method based on a compensation look-ahead algorithm, It is characterized in that In the step S1, the target point coordinates are obtained through preset provided data; In step S2, a corresponding initial mission target point sequence is randomly generated for a single UAV.

3. According to the multi-UAV formation path planning method based on the compensation look-ahead algorithm of claim 1, It is characterized in that In step S3, a solution method based on ETSP pre-grouping is adopted to simplify the amount of calculation while ensuring the shortest path; The total distance from the starting point of the UAV formation to the return of all UAVs is taken as the optimization target of the large-scale UAV collaborative path planning model. The coordinates of all target points are expressed as Target = [(x 1 ,y 1 ),(x 2 ,y 2 ),…,(x n ,y n )] indicates that the distance between two points after the UAV formation mission planning is expressed as: d i,j =|x i -x j |+|y i -y j | (1)。 4. According to claim 1, a multi-UAV formation path planning method based on a compensation look-ahead algorithm, It is characterized in that The UAV is a fixed-wing UAV, and the trajectory change of the fixed-wing UAV is a linear change process. The distance formula between the two points is added with constraints that conform to the UAV's motion trajectory.

5. According to claim 4, a multi-UAV formation path planning method based on a compensation look-ahead algorithm, It is characterized in that The linear variation Dubins curve equation is: d i,j =l+r+D (2) In formula (2), D represents the straight line part of the Dubins curve between two adjacent task points in the task planning trajectory, and l and r are the arc lengths between the two adjacent task points and the straight line.

6. A multi-UAV formation path planning method based on a compensation look-ahead algorithm according to claim 5, It is characterized in that The optimization objective of the UAV formation Dubins multi-traveling salesman model is the shortest flight range of the UAV formation. According to formula (2), the objective function is expressed as follows: In the above formula (3), k i,j,q represents whether the qth UAV has a planned mission between the ith and jth target points, k i,j,q The value is 0 or 1.

7. The multi-UAV formation path planning method based on the compensation look-ahead algorithm according to claim 4, It is characterized in that The constraint function of the constraint condition is as follows: Formula (4) is the number of targets assigned to each UAV. This constraint ensures that each UAV is assigned no less than k targets; Formula (5) is the total number of targets assigned to the drone cluster. This constraint ensures that all target points are assigned; Formula (6) is the number of times any target point is assigned to a drone. This constraint ensures that each point is assigned only once; Formula (7) is the judgment formula for the starting point and the ending point, (x w ,y w ) is the coordinate of the last target point planned for a single UAV, which requires that the starting point and end point of each aircraft are the same.

8. The multi-UAV formation path planning method based on the compensation look-ahead algorithm according to claim 7, It is characterized in that In step S4, the motion trajectory of the fixed-wing UAV changes linearly, and its motion trajectory is defined as a curve: The motion of the drone in a two-dimensional environment, x and y represent the Cartesian coordinate system position of the drone, and θ represents the angle of the drone relative to the X axis, that is, the heading angle of the drone; The UAV motion rate in the model is set to 1, and the maximum curvature is 1 / γ, which can be expressed by the following kinematic equation: Where θ∈[-1 / γ1 / γ], the constant u is the control variable of the UAV that changes with time, |u|≤1.

9. The multi-UAV formation path planning method based on the compensation look-ahead algorithm according to claim 8, It is characterized in that In the UAV motion model, the UAV motion rate is set to 1, and the performance functional of the shortest path of the Dubins curve is expressed by integration, as shown in formula (12); In the above formula, t 0 ,t f Represents the initial time and terminal time of the drone, set is the parameter of the shortest path. Combining the kinematic equations of the UAV in equations (9), (10), and (11) with the performance functional in equation (12), the Hamiltonian function is constructed according to the theorem in the optimal control theory. That is, the shortest path Hamiltonian function of the UAV is: According to the Pontryagin maximum principle, the necessary conditions for obtaining the shortest path of the Dubins curve are obtained; The above formula shows that and During the UAV motion cycle [t 0 , t f ] is a piecewise constant; There are two types of optimal control; if but if u∈[-1, 1], the first condition corresponds to the maximum curvature arc with radius r, and the second condition corresponds to the direction The straight line trajectory of At the initial point of the Dubins curve (x 0 ,y 0 ) and terminal point (x 2 ,y 2 ), adding the middle point (x 1 ,y 1 ), and the boundary relation of the middle point is obtained; θ 0 is the initial point (x 0 ,y 0 ) direction angle (relative to the X axis), θ 1 ,θ 2 is the angle of the line (relative to the X axis), θ m is the middle point (x 1 ,y 1 ) direction angle (relative to the X axis), from the middle point (x 1 ,y 1 ) to the intersection point A of the two straight line segments bisects the intersection angle. On the line segment of the optimal path, From the above formula, we can deduce k 1 / k 2 = -tan(θ 1 +θ 2 ) / 2, the Hamiltonian function is in (x 1 ,y 1 ) is continuous on the optimal trajectory of the point, so it can be expressed as follows; From the above formula, we can know that: k 1 cosθ m +k 2 sinθ m =0 (17) Formula (18) is obtained from formula (15) and formula (17); The midpoint heading angle of the shortest path can be obtained by formula (18), and the solution of the coupled trigonometric equation can be obtained.

10. A multi-UAV formation path planning method based on a compensation look-ahead algorithm according to claim 9, It is characterized in that Based on the algorithm for calculating the heading angle of the four-point optimization, a general formula for calculating the heading angle of the target point is given: First, consider the situation where the distances between the initial point, the middle point, and the terminal point are far. 4 ,y 4 ), point D(x 5 ,y 5 ) is the tangent point of the two circular trajectories; The direction angle θ of the midpoint is solved by equation (18): m , but when the initial point and the middle point are far apart, point F(x 4 ,y 4 ) and point D(x 5 ,y 5 ) is the angle θ of the line segment 1 With the initial point B(x 0 ,y 0 ) and the tangent point D(x 5 ,y 5 ) is the angle θ of the line segment 3 The error between them is negligible, and the coordinates of the initial point B (x 0 ,y 0 ) is directly substituted into equation (18) to solve the direction angle θ of the middle point m ; The total distance L between the three points after mission planning is obtained as follows: In the above formula, a is the distance from the initial point B to the tangent point D, b is the distance from the terminal point C to the tangent point E, α and β are the angles, S 弧DE is the length of arc DE. It can be seen from the formula that the value of L is related to the values ​​of α and β, as shown in formula (20): It can be seen from the formula that when the value of α+β is smaller and α=β, the value of the formula is the smallest. When the center O is on the angle bisector of angle C, the total flight distance L takes the minimum value, which is formula (19) Midpoint (x 1 ,y 1 ) When the distance between the initial point and the middle point is close, the initial point (x 0 ,y 0 ) will affect the heading angle of the middle point (x 1 ,y 1 ) heading angle, introducing an initial point (x 0 ,y 0 )'s adjacent points (x 6 ,y 6 ), the optimization algorithm for the target point (x 6 ,y 6 ) to obtain a tangent point Q 1 (x 4 ,y 4 ), and use this as the tangent point Q 1 (x 4 ,y 4 ) instead of the initial point (x 0 ,y 0 ) becomes the new target point, and the middle point (x 1 ,y 1 ) Perform the previous step to obtain a heading angle θ of the compensation midpoint m ,θ 1 The tangent point Q 1 (x 4 ,y 4 ) and the tangent point Q 2 (x 5 ,y 5 ) is the angle of the line segment, θ 2 The tangent point Q 3 (x 7 ,y 7 ) and the angle between the line segment where the terminal point C is located, O 1 , O 2 is the center point of the two circle trajectories, replacing point Q 1 Substitute the initial point B to solve the direction angle θ of the intermediate point m process;

Images