CN110262543B

CN110262543B - Cluster four-dimensional trajectory planning design method under constraint of simultaneous arrival of multiple target points

Info

Publication number: CN110262543B
Application number: CN201910433651.2A
Authority: CN
Inventors: 全权; 陈恩民; 董程博; 杨坤
Original assignee: Beihang University
Current assignee: Beihang University
Priority date: 2019-05-23
Filing date: 2019-05-23
Publication date: 2020-07-21
Anticipated expiration: 2039-05-23
Also published as: CN110262543A

Abstract

The invention discloses a cluster four-dimensional trajectory planning design method under the constraint of simultaneous arrival of multiple target points, which comprises the following steps: step 1: the optimization of task allocation is completed by using a traditional Hungarian algorithm; step 2: planning a safety track, namely firstly, providing a method for preventing collision of the unmanned aerial vehicles at boundary points of a dangerous area by defining collision prevention dangerous relations of two unmanned aerial vehicles and dangerous sections on the flight tracks of the unmanned aerial vehicles; and then, a cluster anti-collision method based on a greedy thought is provided by combining a minimum separation distance method to realize the speed distribution of the unmanned aerial vehicle. The method solves the problem of complexity of the aircraft tracking track, optimizes the task allocation of the aircraft, and reduces the energy consumption of the aircraft cluster to the minimum.

Description

Cluster four-dimensional trajectory planning design method under constraint of simultaneous arrival of multiple target points

Technical Field

The invention relates to a cluster four-dimensional trajectory planning design method under the constraint of simultaneous arrival of multiple target points, in particular to a space-time four-dimensional trajectory design method for an unmanned aerial vehicle cluster to simultaneously arrive at multiple target points. The design method mainly comprises two steps of task allocation optimization and safe track planning. The optimization of task allocation enables the unmanned aerial vehicle group to reach a target point at the same time with minimum energy consumption, the planning of safe tracks ensures that the unmanned aerial vehicles do not collide with each other in flight, and the design method can be applied to any unmanned aerial vehicles such as unmanned aerial vehicles and guided missiles. The invention belongs to the field of flight path planning.

Background

With the development of the information age, the cluster trajectory mission planning has wide application in multiple aspects, and the multi-aircraft cooperative flight mission technology is mature day by day. One of the aircraft cluster flight missions is to reach multiple target points simultaneously. The specific task may be that multiple missiles simultaneously destroy a target, or multiple unmanned aerial vehicles simultaneously arrive at a designated position of a battlefield for reconnaissance, attack and the like.

At present, many algorithms are available for solving the planning scheme of multi-unmanned aerial vehicle target allocation, such as an ant colony algorithm with a system evolution strategy, a decomposition type planning algorithm, an expansion Voronoi diagram and the like. The algorithms for enabling the aircrafts to arrive at the target point at the same time are mostly equal-length trajectory planning methods and derived algorithms thereof, namely, under the constraint of the mechanical characteristics of the aircrafts, the generated paths are subjected to smoothing treatment, the cluster aircrafts are assumed to fly at the same speed at a constant speed, and the equal-length paths are planned and designed based on the idea that the flying trajectories are regarded as chains connecting the extensible springs with the starting point and the target point, so that the constraint of the simultaneous arrival and the dynamic optimization of the paths are met, and the derived algorithms comprise a smoothing algorithm and a path shortening algorithm. However, in practical situations, the distances from the starting points of the aircrafts to the target point are generally different, and the designed flight trajectory is a curve according to a planning method of equal-length flight trajectories, so that the complexity of the aircraft tracking trajectory is increased, the time for the aircraft to execute tasks is limited, and the energy consumption of the aircraft is further increased.

Disclosure of Invention

Aiming at the difficulties and the defects of the multi-target task track planning of the existing unmanned aerial vehicle, the invention provides a cluster four-dimensional track planning design method under the constraint of simultaneous arrival of multiple target points, so as to solve the problem of complexity of tracking tracks of an aircraft, optimize the task allocation of the aircraft and reduce the energy consumption of the cluster of the aircraft for executing tasks to the minimum. The design method is based on the global trajectory planning, optimization of flight trajectory task allocation is completed by using a Hungarian algorithm, theoretical derivation and implementation of an anti-collision algorithm are completed by using a minimum separation distance method, and three-dimensional expected waypoints of different unmanned aerial vehicles at different moments are finally given.

The design method is completed based on the following assumptions:

1. the flight trajectories of all the unmanned aerial vehicles flying from the respective starting point positions to the target point positions are straight lines.

2. The number of the target points of each group of target point groups is consistent with the number of the unmanned aerial vehicles.

3. The positions of all the target points are fixed, and the position coordinates thereof do not change with time.

4. The speed of the unmanned aerial vehicle in different flight paths is abrupt, namely the transition time of the speed change of the unmanned aerial vehicle is not considered.

5. Consider a drone as a particle model.

The method comprises the following specific steps:

step 1: task allocation optimization

In the task that unmanned aerial vehicles arrive at multiple target points simultaneously, the step of allocating different flight task targets to each unmanned aerial vehicle is the first step. In order to minimize the energy consumption of all the drones and the total cost of executing tasks, the sum of the flight path lengths of the drones is required to be minimized. This problem is similar to the assignment problem in the field of operations research, which can be essentially abstracted as the minimum matching problem for a given bipartite graph. And as can be known from the previous hypothesis, the problem is a target balance distribution problem, so that the traditional Hungarian algorithm is used for completing the optimization of task distribution; the method comprises the following specific steps:

s11, performing linear transformation of row specifications and column specifications on the distance relation matrix of the departure point and the target point of the unmanned aerial vehicle;

s12, finding out independent 0 elements of the transformed distance matrix, and counting the number of the independent 0 elements;

s13, covering all independent 0 elements by the least drawn line;

s14, performing trial assignment according to the number of the independent 0 elements to obtain an optimization matrix with the independent 0 elements, wherein the number of the independent 0 elements is consistent with that of the unmanned aerial vehicles, and the row and column numbers of the independent 0 elements are the optimized task allocation result.

Step 2: safe trajectory planning

The task allocation of step1 can determine the starting point and the target point of each unmanned aerial vehicle flight. And step2, distributing the speed of the unmanned aerial vehicles, and planning an expected waypoint of each unmanned aerial vehicle in a time dimension, so that the unmanned aerial vehicles are prevented from colliding in flight, and the constraint condition that the unmanned aerial vehicles reach a target point at the same time is met. Firstly, providing a method for preventing collision of an unmanned aerial vehicle in a dangerous area boundary point by defining collision prevention dangerous relations of two unmanned aerial vehicles and dangerous sections on a flight path of the unmanned aerial vehicle; and then, a cluster anti-collision method based on a greedy thought is provided by combining a minimum separation distance method to realize the speed distribution of the unmanned aerial vehicle.

The cluster anti-collision method comprises the steps that the flight reference speed of the unmanned aerial vehicles, the collision danger threshold distance of the unmanned aerial vehicles and the coordinates of the starting point and the stopping point of the flight tracks of the unmanned aerial vehicles are input, and the speed distribution results of the unmanned aerial vehicles on the respective flight tracks are output. The cluster collision avoidance method has the core idea that the speed of the unmanned aerial vehicle with a longer flight path is preferentially distributed, so that enough time difference can be provided between every two unmanned aerial vehicles to fly away from respective dangerous areas.

And inputting a given moment, and planning a safety expected waypoint on the four-dimensional track of the unmanned aerial vehicle by combining a speed distribution result output by the cluster anti-collision method.

The method for preventing the unmanned aerial vehicle from colliding the boundary point of the dangerous area can realize judgment of the dangerous relationship of the two unmanned aerial vehicles and determination of the boundary point position of the dangerous area, and comprises the following specific steps of:

step1, calculating the track linear direction vectors and the reference speed vectors of the two unmanned aerial vehicles;

step2, calculating the shortest space distance between the two unmanned aerial vehicle track lines and the position of the shortest point on the corresponding track;

step3 carries out danger relation judgment

If the two unmanned aerial vehicles have a dangerous relation, performing Step 4;

if no dangerous relation exists between the two unmanned aerial vehicles and no dangerous area exists, the algorithm is ended;

step4, carrying out critical point verification calculation on the dangerous area

Verifying and calculating that if the distance between any points on the respective tracks of the two unmanned aerial vehicles close to the starting point direction is equal to the danger threshold distance, and the distance between any points on the respective tracks close to the end point direction is equal to the danger threshold distance, the position of any point selected on the respective track at the moment is directly output as the critical point position of the danger area;

if the distance between any points on the respective tracks of the two unmanned aerial vehicles close to the starting point direction is smaller than the danger threshold distance, or the distance between any points on the respective tracks close to the terminal point direction is smaller than the danger threshold distance, Step5 is executed;

step5 updating critical point position of danger area

And updating the critical point position of the danger area according to the input iteration time Step and the unmanned aerial vehicle reference speed vector, and executing Step4 after the position is updated by one Step.

The cluster anti-collision algorithm comprises the following specific steps:

step1, calculating the track length of each unmanned aerial vehicle;

step2, sorting the track lengths of all the unmanned aerial vehicles from long to short;

step3, determining the speed of the reference unmanned aerial vehicle and initializing algorithm parameters;

step4, judging the danger relation between the unmanned aerial vehicle i and the unmanned aerial vehicle j;

step5, distributing the speed of the unmanned aerial vehicle i;

step6, updating parameters such as the i number of the unmanned aerial vehicle, distributing the speed of the next unmanned aerial vehicle, and finally outputting the speed and the corresponding speed flight time distribution result.

Preferably, the method of the present invention further comprises: and step 3: trajectory correction design

In order to enable the unmanned aerial vehicle to well track the planned expected waypoints, step3 is to perform correction design on the expected waypoints planned in step 2. The specific modified design is related to the controller of the unmanned aerial vehicle. The corrected and designed expected track waypoints can be directly input into the unmanned aerial vehicle controller.

The correction design is mainly divided into two steps, wherein in the first step, an uncorrected expected route point is input into an unmanned aerial vehicle system as an instruction signal, the tracking delay error of the unmanned aerial vehicle is calculated according to the measured actual response position of the unmanned aerial vehicle, and the approximate delay time constant of the unmanned aerial vehicle system is estimated. And the second step is to carry out corresponding time advance correction on the planned track route points according to the delay time constant, so that the tracking delay error of the unmanned aerial vehicle system is reduced.

The invention discloses a cluster four-dimensional trajectory planning design method under the constraint of simultaneous arrival of multiple target points, which has the advantages and effects that: the problem of complexity of tracking the track of the aircraft is solved, the task allocation of the aircraft is optimized, and the energy consumption of the aircraft cluster for executing the task is reduced to the minimum.

Drawings

Fig. 1a is a diagram of all possible task trajectories of 5 drones, and fig. 1b is a diagram of trajectories of the 5 drones after task allocation is optimized, and the optimized trajectories are thickened. FIG. 1c is a block diagram of an implementation of task allocation optimization

Fig. 2 is a schematic diagram of a danger zone in which two drones may collide on the trajectory.

FIG. 3 is a flow chart of a collision avoidance algorithm

FIGS. 4a, 4b, 4c and 4d are diagrams illustrating steps of four-dimensional trajectory planning, respectively

FIG. 5 is a flow chart of a four-dimensional trajectory planning design

FIG. 6 is a simulation initialization diagram

FIG. 7 is a verification simulation diagram of correctness of the Hungarian algorithm

Fig. 8a is a simulation result diagram of task allocation by the hungarian algorithm, and fig. 8b, 8c, and 8d are simulation result diagrams of random task allocation.

FIG. 9 is a diagram of a simulation result of a collision avoidance algorithm

Fig. 10 is a diagram of the relationship between the number of drones and the minimum distance between drones in the flight mission of the drones.

FIG. 11 is a four-dimensional trajectory planning design simulation.

Fig. 12a and b are graphs comparing the results of the trajectory correction simulation.

Fig. 13a and b are tracking distance error comparison diagrams.

Detailed Description

The technical solution of the present invention is further described with reference to the accompanying drawings and specific examples.

Step 1: task allocation optimization

In the task that unmanned aerial vehicles arrive at multiple target points simultaneously, the step of allocating different flight task targets to each unmanned aerial vehicle is the first step. In order to minimize energy consumption of all unmanned aerial vehicles and total cost for executing tasks, optimal task allocation needs to be made to enable the sum of paths of the unmanned aerial vehicles to be the shortest. This problem is similar to the assignment problem in the field of operations research, which can be essentially abstracted as the minimum matching problem for a given bipartite graph. As can be seen from the previous assumptions, this problem is a target balanced assignment problem, so we use the traditional hungarian algorithm to accomplish the optimization of task assignment.

The Hungarian algorithm effectively solves the one-to-one optimization assignment problem by citing the independent 0 element quantity relation theorem in the matrix of the Hungarian mathematician Conniger.

The steps of solving the problem of task allocation optimization of the unmanned aerial vehicle by the Hungarian algorithm are explained by combining with an example.

As shown in fig. 1a, the existing 5 drones must arrive at the target points (numbers 1 '-5') from their respective starting points (numbers 1-5) at the same time. The steps for optimizing task allocation are as follows.

And S11, calculating the distance between each starting point and each ending point. The constructed distance matrix is shown in table 1 below.

TABLE 1

And S12, performing row specification transformation, namely subtracting the minimum number of the row from each row to obtain a distance array after row reduction as shown in Table 2.

TABLE 2

And S13, performing column reduction transformation, namely subtracting the minimum number of each column to obtain a distance array after column reduction as shown in Table 3.

TABLE 3

And S14, performing trial assignment on the table 3 matrix. The trial assignment operation is determined according to the result of zero line marking of the next step, and mainly comprises the following four steps:

s141, traversing all 0 elements which are not drawn with lines, counting the number of the 0 elements of the row and column where the 0 elements are located, selecting the 0 element with the least number, and marking the row and column.

S142, the 0 element which is not marked in the marked row and column is called as an independent 0 element. Lines are drawn on the rows and columns of the 0 element.

S143, repeating the steps S141 and S142 until no line can be drawn (the element drawn by the line is not seen first).

S144, counting the number n of 0 elements found in step S142, and if n is equal to 5, the operation succeeds, otherwise, the operation fails.

And S15, drawing a zero line, and drawing the least straight line to cover all 0 elements, wherein the straight line is the number of independent 0 elements.

S16, updating the matrix, wherein the operation comprises the following three steps:

s161, finding the minimum number from the numbers which are not marked by the lines.

And S162, subtracting the minimum number from the number which is not marked by the line.

S164, add the smallest number to the numbers drawn by 2 lines.

The distance matrix of table 3 is shown in table 4 after the steps S12, S13, S14 and S15, the values of which become the updated distance matrix:

TABLE 4

And S17, repeating the steps S12, S13, S14 and S15 until the number n of independent 0 elements is 5. The rank serial number corresponding to the independent 0 element is the number of the ending point and the starting point of the unmanned aerial vehicle. If 1 is used to replace independent 0 element, and other numbers are all 0, the optimized distribution matrix obtained by the algorithm is shown in table 5:

TABLE 5

This indicates that the optimal allocation is that the 1 st drone should fly from 1 to 5 ' points, the 2 nd from 2 to 1 ' points, the 3 rd from 3 to 2 ' points, the 4 th from 4 to 4 ' points, and the 5 th from 5 to 3 ' points. The optimized trajectory has been shown in bold in fig. 1 b.

It can be seen that the sum of the shortest distances of the optimized tracks is 4+7+6+6+ 9-32.

FIG. 1c is a block diagram for implementing task allocation optimization of Hungarian algorithm

Step 2: safe trajectory planning

The task allocation of step1 can determine the starting point and the target point of each unmanned aerial vehicle flight. And step2, distributing the speed of the unmanned aerial vehicles, and planning an expected waypoint of each unmanned aerial vehicle in a time dimension, so that the unmanned aerial vehicles are prevented from colliding in flight, and the constraint of reaching a target point at the same time is met.

Now, the multi-target point three-dimensional space is divided into two groups of points, and the space vectors of the points relative to the origin are as follows:

{p_I,1,…,p_I,M},{p_E,1,…,p_E,M} (1)

here, the

Is provided with

Representing the three-dimensional coordinate of the unmanned aerial vehicle i at the moment t, and setting M unmanned aerial vehicles to start at the moment 0 at the same time, wherein the initial position p of the unmanned aerial vehicle i_U,i(0) And velocity v_U,i(0) Satisfy the requirement of

p_U,i(0)＝p_I,i,v_U,i(0)＝0,(i＝1,2,...,M) (2)

At T_fAll unmanned planes arrive at the target point at the same time, and the positions and the speeds of the unmanned planes are

p_U,i(T_f)＝p_E,σ(i),v_U,i(T_f)＝0 (3)

Wherein: σ {1, …, M } → {1, …, M } is a one-to-one mapping.

Let l_i＝||p_E,σ(i)-p_I,iAnd | l is the track length of the ith unmanned aerial vehicle. The following relationships exist:

wherein, the unmanned aerial vehicle a is taken as a reference unmanned aerial vehicle, v_rFor unmanned aerial vehicle reference speed, T_fTime is referenced for the task.

Defining the dangerous threshold distance between two unmanned planes as d_th,d_i,j(t)＝||p_U,i(t)-p_U,j(t) | | is the distance between drone i and drone j at time t.

If t is present^*Meet at the moment

d_i,j(t^*)＜d_th,t^*∈[0,T_f](6)

Then drone i and drone j are considered to be within the danger zone at time t, and they have a danger relationship, the specific relationship being shown in fig. 2.

If two unmanned aerial vehicles can avoid respective danger areas, the boundary point positions of the danger areas need to be found first. Therefore, the dangerous area boundary point algorithm can judge the dangerous relation of two unmanned aerial vehicles and determine the position of the dangerous area boundary point.

Algorithm 1: dangerous area boundary point algorithm

Inputting: coordinates of starting point and ending point of unmanned aerial vehicle i track { p_I,i,p_E,σ(i)Coordinates of start point and end point of j track of unmanned plane { p }_I,j,p_E,σ(j)H, iteration time step delta t, danger threshold distance d_th

And (3) outputting: unmanned planei and j of unmanned aerial vehicle have danger relation, and the position of a boundary point of a danger zone on the respective tracks of the i and the j of unmanned aerial vehicle is { p }_ij(start),p_ij(end)},{p_ji(start),p_ji(end)}

Algorithm flow pseudo code:

step1 calculates the track straight direction vector and the reference velocity vector of the two unmanned planes.

Step2, calculating the shortest space distance between the two unmanned aerial vehicle track lines and the position of the shortest point on the corresponding track.

Let p_i(m)For moving points on the i track of the drone, p_j(n)Is a moving point on the j track of the unmanned plane. Where the parameter m, n describes the magnitude of the displacement of the instantaneous position of the moving point relative to the position of the starting point, i.e.:

the shortest spatial distance between the trajectories is:

wherein

The positions of the shortest points on the unmanned plane i track and the unmanned plane j track are respectively.

The solution of (c) is divided into the following cases:

case 1, if vector l_i,l_jCoplanar, if the straight lines of the two track segments intersect, the intersection point is defined as p_ij

a if p_ijOn trajectories of drone i and drone j, then

b if p_ijNot on drone i and drone j trajectories,

then

Case 2 if vector l_i,l_jOn the other hand, n is set_ijIs 1_i,l_jThe normal vector of (2).

Calculating out the out-of-plane distance between two flight paths by vector method

Calculating the positions of two end points of the different-plane distance line segment to be p respectively_ij′,p_ji′

a if p_ij' on unmanned aerial vehicle i track and p_ji' on unmanned plane j track, then

b if p_ij' not on drone i track or p_jiNot on drone j trajectory, then

Step3 carries out danger relation judgment

If d is_min(i,j)≤d_thThen the output unmanned plane i and the unmanned plane j have dangerous relation and order

Then Step4 is carried out;

if d is_min(i,j)＞d_thAnd outputting that the unmanned aerial vehicle i and the unmanned aerial vehicle j have no dangerous relation and no dangerous area, and ending the algorithm.

Verifying if the calculation has | | | p_ij(start)-p_ji(start)||＝d_thAnd p_ij(end)-p_ji(end)||＝d_thDirectly output the critical point position of the danger area

{p_ij(start),p_ij(end)},{p_ji(start),p_ji(end)}

If p_ij(start)-p_ji(start)||＜d_thOr p_ij(end)-p_ji(end)||＜d_thThen Step5 is executed;

step5 updating critical point position of danger area

Updating the critical point position of the danger area according to the input iteration time step and the unmanned aerial vehicle reference speed vector

After the position is updated by one Step, Step4 is executed;

after the result output by the algorithm is obtained, the following analysis is carried out:

if the unmanned aerial vehicle i and the unmanned aerial vehicle j are in the range of 0-T_fNo dangerous relation exists in the moment, and when the respective flight speeds of the two flying objects are in proportional relation with the respective flight path lengths, the constraint conditions that the two flying objects simultaneously reach the target point at the time T can be met.

If the unmanned aerial vehicle i and the unmanned aerial vehicle j have a dangerous relation, the respective speeds of the unmanned aerial vehicle i and the unmanned aerial vehicle j are redistributed by combining the calculated boundary point positions of the dangerous area, so that the unmanned aerial vehicle j enters the dangerous area after the unmanned aerial vehicle i leaves the dangerous area. When p is_U,i(t₀)＝p_ij(start)When is, p_U,j(t₀)＝p_ji(end)

Let n_iRepresenting the number of hazard zones on the drone i trajectory. Mixing the aboveThe speed distribution method of two unmanned aerial vehicles is expanded to a plurality of unmanned aerial vehicles, and the following cluster collision avoidance algorithm can be obtained.

And 2, algorithm: cluster collision avoidance algorithm

Inputting: reference velocity v_rThreshold distance of danger d_thStarting point position { p of flight path of M unmanned aerial vehicles_I,1,…,p_I,MAnd end point position p_E,σ(1),…,p_E,σ(M)}

And (3) outputting: speed of each section of air route of M unmanned aerial vehicles and flight time of corresponding speed

Algorithm flow pseudo code:

step1 calculating track length of each unmanned aerial vehicle

l_i＝||p_I,i-p_E,σ(i)||,(i＝1,2,...,M)

Step2 sequences the flight path lengths of all unmanned aerial vehicles from long to short

Let λ: {1, …, M } → {1, …, M } be a one-to-one mapping from large to small.

Then the sequence l₁,l₂,...,l_MAfter sorting from long to short, becomes l_λ(1),l_λ(2),...,l_λ(M)

I.e. |_λ(1)≥l_λ(2)≥,...,≥l_λ(M)

Step3 references the unmanned aerial vehicle speed and initializes the algorithm parameters

As shown in the formula (4), the unmanned plane with the number of lambda (1) has the longest flight path and is taken as a reference unmanned plane, and the speed v of the reference unmanned plane_λ(1)1For reference speed, time of flight t_λ(1)1For a task time T_fObtainable from the formula (5)

The following parameters are initialized

m＝2,i＝λ(m),k＝m-1,j＝λ(k)，n_i＝0,S_i＝[]

Wherein the parameters i, j, k and m all represent the number cyclic variable of the unmanned aerial vehicle, and the parameter n_iNumber of dangerous sections on the i track of the unmanned aerial vehicle, array S_iThe nearest distance from the starting point to the boundary point of the danger area of the unmanned aerial vehicle i is recorded.

Step4 judges the danger relation between unmanned aerial vehicle i and unmanned aerial vehicle j

And calling an algorithm 1 to judge whether the unmanned aerial vehicle i and the unmanned aerial vehicle j have a dangerous relation.

If algorithm 1 outputs no-risk relationships, then j ═ λ (k-1), n_iIf j is 0, then Step5 is executed, otherwise Step4 is executed.

If the algorithm 1 outputs a dangerous relation, calculating the shortest distance l from the starting points of the unmanned aerial vehicles i and j to the boundary point of the dangerous area according to the boundary point of the dangerous area output by the algorithm 1_ij,l_jiAnd updates the parameter n_i,S_i,j

l_ij＝||p_ij(end)-p_I,i||,l_ji＝||p_ji(start)-p_I,j||,n_i＝n_i+1,S_i＝S_i∪l_ij,j＝λ(k-1)

Recording the time required by starting the unmanned plane i from the starting point to the boundary point of the danger area associated with the unmanned plane j as tau_ij，

If j is 0, then Step5 is executed, otherwise Step4 is continuously executed.

Step5 speed distribution for unmanned plane i

If n is_iAnd (5) distributing the speed of each section of route of the unmanned aerial vehicle i and the flight time of the corresponding speed as follows:

if n is_iNot equal to 0, and comparing the array S_iSorting from small to large.

The one-to-one mapping of the ordering is κ: {1, …, M } → {1, …, M }, i.e., the ordering results in

The speeds of all sections of routes of the unmanned plane j and the flight time of the corresponding speeds are distributed as follows:

......

step6, updating parameters such as the i number of the unmanned aerial vehicle, and distributing the speed of the next unmanned aerial vehicle

m＝m+1,i＝λ(m),k＝m-1,j＝λ(k)

If i is less than or equal to M, executing Step4, otherwise, outputting the speed and the corresponding speed flight time distribution result, and ending the algorithm.

The whole flow diagram of the algorithm 2 is shown in fig. 3, after the speed distribution result output by the algorithm 2 is obtained, a given moment is input, and then the safety expected waypoints on the four-dimensional track of the unmanned aerial vehicle can be planned according to the following steps.

1. For drone i, at 0 to T_fThe flight time axis of (2) is marked with a variable speed time node. Is provided with

As can be seen from the output of algorithm 2, the gear shifting timing of drone i is as follows:

2. given any time t, finding k satisfies

τ_ki≤t≤τ_(k+1)i,k＝0,1,2,...,n_i

3. Let l_ik(t) unmanned plane i is at time t relative to its nearest neighborDisplacement vector of critical point of the hazard zone.

Then the expected position of drone i at time t:

fig. 4a to 4d are schematic diagrams of trajectory planning steps in which 5 drones fly to 3 groups of target point groups sequentially and simultaneously (only trajectory planning steps of No. 1 and No. 3 drones are schematically labeled in the drawings).

The overall block diagram of step1 and step2 is shown in fig. 5.

And step 3: trajectory correction design

In practice it is impossible for the drone to track the upper desired trajectory completely with zero error. Let p_U,i(t) the actual position of the unmanned aerial vehicle, and the expected position of the unmanned aerial vehicle is known from the planning design of the step1 and the step2

We hope p_U,i(t) and

the error of (2) is as small as possible and the convergence speed is as fast as possible.

For better performance of the unmanned aerial vehicle executing the task on tracking the planned trajectory, even if the actual position p of the unmanned aerial vehicle output_U,i(t) better tracking of the Up-planned desired position

We correct the planned trajectory waypoints as follows

The time constant of an approximate inertia link in the unmanned aerial vehicle system is reciprocal, and the numerical value is obtained by the following two-step test method:

1. expected position of uncorrected plan

As command signal input into the drone controller, recording the actual position p at which the drone responds_U,i(t)。

2. Comparing the actual position of the test output with the input expected position data, approximating the relation between the input and the output as an inertia link, and calculating the approximate inertia link by using the input and the output data

Fine tuning the parameters may allow tracking performance to be further improved. The specific adjustment is related to the unmanned aerial vehicle tracking controller. We use the PID drone tracking controller as an example for simulation, which shows that when the tuning parameter is 0.025,

corrected trajectory waypoints

So that

The unmanned aerial vehicle tracking performance is optimal.

Example simulation

1 simulation setup

Setting the number M of unmanned aerial vehicles as 10 frames and reference speed v_r5m/s, and the danger threshold distance is 4m, and only two groups of target point groups are taken as an example. Coordinates of the starting point and the target point of each unmanned aerial vehicle are uniformly and randomly generated and visualized as shown in fig. 6, wherein the star point is the starting point, the square point is the first group of target points, and the circular point is the second group of target points.

2 task allocation simulation

The hungarian algorithm is first verified, as shown in fig. 7, with the asterisk as the starting point, the square as the first set of target points, and the circle as the second set of target points. FIG. 7 demonstrates the correctness of the Hungarian algorithm.

And then, the tasks of each unmanned aerial vehicle in the simulation set are distributed by utilizing the Hungarian algorithm. Since there are two groups of target points, the Hungarian algorithm needs to be applied twice for matching. The hungarian matching results are shown in fig. 8a, for comparison with the matching of the hungarian algorithm, and fig. 8b, 8c and 8d are results of arbitrary matching.

The longest track is found out firstly and substituted into the reference speed value to calculate the task time.

As calculated for the FIG. 8a match, the longest trajectory to the first set of target points is from the departure point 0 to the first set of target points 4, and the longest square trajectory to the second set of target points is from the first set of target points 5 to the second set of target points 9. The simulation found the starting 1 point coordinates to be (-15,0,0), the first set of target 5 point coordinates to be (6.51, -11.53,17), and the second set of target 9 point coordinates to be (6.93, -4.98, 34.8).

So that the longest trajectory length from the departure point to the first set of target points is s₁29.5523m, the longest trajectory length between the first set of target points and the second set of target points is s₂＝23.1101m

From this, can obtain that unmanned aerial vehicle total task time is:

the sum of the respective track lengths of fig. 8a, 8b, 8c and 8d, the longest track length and the total task time are calculated as follows in table 6:

TABLE 6

The comparative data of the tables illustrate that the assignment of tasks applying the hungarian algorithm is successful.

3 Collision avoidance Algorithm simulation

Taking the trajectory of two drones as an example, the coordinates a of the intersection point of the 0-4 trajectory and the 2-4 trajectory of the trajectories of the two drones as shown in fig. 9 are (-1.9009, -6.7201, 10.3574). The minimum distance point B of the 4-8 track line is (0.82873147, -8.47258373,24.52429909), and the minimum distance point C of the 9-4 track line is (2.52501745, -8.14613575, 25.69413467).

The coordinates of dangerous critical points of each track calculated by the algorithm 1 are

A1(-4.0509,-5.6171,8.6574),A2(-3.3404,-7.3472,8.3774),

B0(1.7592,-8.8914,23.2919),C0(3.1651,-7.2669,24.5206)；

The speed of the unmanned aerial vehicle 1 is taken as a reference speed v_rLet the speed of the drone 2 and the corresponding speed flight time be { (v) 5m/s₂₁,t₂₁),(v₂₂,t₂₂),(v₂₃,t₂₃),(v₂₄,t₂₄)}

The speed and corresponding speed flight time results of the drone 2 output by the algorithm 2 are:

{(3.6440,2.16620),(5.1818,3.74426),(3.9394,1.53329),(3.8688,3.08873)}

fig. 10 verifies the relationship between each moment in the flight mission of the drone and the minimum risk distance between the drones.

4 track correction simulation

The specific speed change condition of each unmanned aerial vehicle can be obtained by the anti-collision algorithm. The lower graph plots the positions of the unmanned aerial vehicles at different time nodes, and simultaneously plots the positions of the unmanned aerial vehicles at the intermediate moments of each node time period.

The simulation results are shown in fig. 11.

And selecting the number of 0-4, 4-8 tracks planned by the simulation to perform tracking simulation, and sampling 2000 expected waypoints on the whole planned track. According to

The planned trajectory correction is made (for small parameters related to the drone controller). The approximate first-order constant of the simulated unmanned aerial vehicle is set to be 0.025, and the tracking simulation result is shown in fig. 12a and b. (FIG. 12a shows the effect of tracking an unmodified planned trajectory, and FIG. 12b shows the effect of tracking a modified planned trajectory)

FIGS. 13a and b are graphs of the tracking error distance results for each sampled waypoint (FIG. 13a is the error result for the uncorrected planned trajectory, and FIG. 13b is the error result for the corrected planned trajectory);

simulation results show that the corrected planning track enables the unmanned aerial vehicle to have better track tracking effect.

Claims

1. A cluster four-dimensional trajectory planning design method under the constraint of simultaneous arrival of multiple target points is completed based on the following assumptions:

①, the flight tracks of all unmanned aerial vehicles flying from the respective starting point positions to the target point positions are straight lines;

②, the number of the target points of each group of target point groups is consistent with the number of the unmanned aerial vehicles;

③, the positions of all the target points are fixed, and the position coordinates do not change along with the change of time;

④, the speed of the unmanned aerial vehicle in different flight paths is abrupt, namely the transition time of the unmanned aerial vehicle speed change is not considered;

⑤, regarding the unmanned aerial vehicle as a particle model;

the method is characterized in that: the method comprises the following steps:

step 1: task allocation optimization

The optimization of task allocation is completed by using a traditional Hungarian algorithm; the method comprises the following specific steps:

s13, covering all independent 0 elements by the least drawn line;

s14, performing trial assignment according to the number of the independent 0 elements to obtain an optimization matrix with the independent 0 elements, wherein the number of the independent 0 elements is consistent with that of the unmanned aerial vehicles, and the row and column numbers of the independent 0 elements are the optimized task allocation result;

step 2: safe trajectory planning

Firstly, a method for preventing collision of the unmanned aerial vehicle at a dangerous area boundary point is provided by defining collision-prevention dangerous relations of two unmanned aerial vehicles and dangerous sections on the flight path of the unmanned aerial vehicle; then, a cluster anti-collision method based on a greedy thought is provided by combining a minimum separation distance method to realize the speed distribution of the unmanned aerial vehicle; the cluster anti-collision method comprises the steps that the input of the cluster anti-collision method is the flight reference speed of the unmanned aerial vehicle, the collision danger threshold distance of the unmanned aerial vehicle and the coordinates of the starting point and the stopping point of the flight track of each unmanned aerial vehicle, and the output is the speed distribution result of each unmanned aerial vehicle on the respective flight track;

inputting a given moment, and planning a safety expected waypoint on the four-dimensional track of the unmanned aerial vehicle by combining a speed distribution result output by a cluster anti-collision method;

the method for preventing the unmanned aerial vehicle from colliding the boundary point of the danger area can realize judgment of the danger relationship of the two unmanned aerial vehicles and determination of the boundary point position of the danger area, and comprises the following specific steps:

step3 carries out danger relation judgment

step5 updating critical point position of danger area

Updating the critical point position of the danger area according to the input iteration time Step and the unmanned aerial vehicle reference speed vector, and executing Step4 after the position is updated by one Step;

the cluster anti-collision algorithm specifically comprises the following steps:

step1, calculating the track length of each unmanned aerial vehicle;

step3, setting the speed of the reference unmanned aerial vehicle and initializing algorithm parameters;

step5, distributing the speed of the unmanned aerial vehicle i;

2. The method according to claim 1, wherein the method comprises the following steps: further comprising: and step 3: carrying out track correction design;

the correction design is mainly divided into two steps, wherein in the first step, an uncorrected expected route point is input into an unmanned aerial vehicle system as an instruction signal, the tracking delay error of the unmanned aerial vehicle is calculated according to the measured actual response position of the unmanned aerial vehicle, and the approximate delay time constant of the unmanned aerial vehicle system is estimated; and the second step is to carry out corresponding time advance correction on the planned track route points according to the delay time constant, so that the tracking delay error of the unmanned aerial vehicle system is reduced.