CN116700340A - Track planning method and device and unmanned aerial vehicle cluster - Google Patents
Track planning method and device and unmanned aerial vehicle cluster Download PDFInfo
- Publication number
- CN116700340A CN116700340A CN202310789413.1A CN202310789413A CN116700340A CN 116700340 A CN116700340 A CN 116700340A CN 202310789413 A CN202310789413 A CN 202310789413A CN 116700340 A CN116700340 A CN 116700340A
- Authority
- CN
- China
- Prior art keywords
- unmanned aerial
- aerial vehicle
- neighborhood
- target
- point
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 92
- 230000009977 dual effect Effects 0.000 claims description 41
- 230000002265 prevention Effects 0.000 claims description 9
- 230000017105 transposition Effects 0.000 claims description 4
- 238000005457 optimization Methods 0.000 abstract description 29
- 230000008569 process Effects 0.000 abstract description 15
- 230000007547 defect Effects 0.000 abstract description 5
- 238000000354 decomposition reaction Methods 0.000 abstract description 4
- 230000006870 function Effects 0.000 description 55
- 238000010586 diagram Methods 0.000 description 12
- 238000004364 calculation method Methods 0.000 description 7
- 238000004891 communication Methods 0.000 description 6
- 230000000694 effects Effects 0.000 description 6
- 239000000463 material Substances 0.000 description 6
- 230000001133 acceleration Effects 0.000 description 5
- 238000005516 engineering process Methods 0.000 description 5
- 238000011160 research Methods 0.000 description 4
- 230000008901 benefit Effects 0.000 description 3
- 239000003795 chemical substances by application Substances 0.000 description 3
- 238000011161 development Methods 0.000 description 3
- 239000011159 matrix material Substances 0.000 description 3
- 230000003993 interaction Effects 0.000 description 2
- 230000001737 promoting effect Effects 0.000 description 2
- NAWXUBYGYWOOIX-SFHVURJKSA-N (2s)-2-[[4-[2-(2,4-diaminoquinazolin-6-yl)ethyl]benzoyl]amino]-4-methylidenepentanedioic acid Chemical compound C1=CC2=NC(N)=NC(N)=C2C=C1CCC1=CC=C(C(=O)N[C@@H](CC(=C)C(O)=O)C(O)=O)C=C1 NAWXUBYGYWOOIX-SFHVURJKSA-N 0.000 description 1
- 206010063385 Intellectualisation Diseases 0.000 description 1
- 238000013459 approach Methods 0.000 description 1
- 238000013473 artificial intelligence Methods 0.000 description 1
- 230000003190 augmentative effect Effects 0.000 description 1
- 230000004888 barrier function Effects 0.000 description 1
- 230000006399 behavior Effects 0.000 description 1
- 230000005540 biological transmission Effects 0.000 description 1
- 239000002360 explosive Substances 0.000 description 1
- 230000005251 gamma ray Effects 0.000 description 1
- 238000007689 inspection Methods 0.000 description 1
- 238000011835 investigation Methods 0.000 description 1
- 238000012804 iterative process Methods 0.000 description 1
- 230000007774 longterm Effects 0.000 description 1
- 238000012544 monitoring process Methods 0.000 description 1
- 230000008447 perception Effects 0.000 description 1
- 239000002243 precursor Substances 0.000 description 1
- 238000005096 rolling process Methods 0.000 description 1
- 238000005070 sampling Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
Classifications
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Landscapes
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The invention provides a track planning method and device and an unmanned aerial vehicle cluster, and relates to the field of unmanned aerial vehicles. The method comprises the steps of decomposing a centralized trajectory planning problem of unmanned aerial vehicles constructed based on MPC by using an alternate direction multiplier method to obtain a first cost function and a second cost function, solving each trajectory point from a starting point to an ending point based on the first cost function and the second cost function in the trajectory planning process, and carrying out data exchange with each neighborhood unmanned aerial vehicle in the trajectory point solving process to ensure information coordination between unmanned aerial vehicles to avoid collision. Therefore, the defects of high computational complexity, insufficient reliability and the like caused by a centralized method can be effectively solved, meanwhile, the problem decomposition is carried out by combining a distributed optimization idea such as an alternate direction multiplier method, and the data exchange is carried out with the neighborhood unmanned aerial vehicle to avoid mutual collision, so that a continuous smooth track meeting the requirements can be generated more efficiently.
Description
Technical Field
The invention relates to the field of unmanned aerial vehicles, in particular to a track planning method and device and an unmanned aerial vehicle cluster.
Background
In recent years, with the rapid development of new generation information technologies such as artificial intelligence and network communication, intelligent unmanned aerial vehicle cluster systems inspired by biological cluster behaviors are receiving wide attention. The intelligent unmanned aerial vehicle cluster system refers to an overall system in which a certain number of unmanned aerial vehicles can complete complex tasks through mutual cooperation and information sharing. Compared with a single unmanned aerial vehicle system, the unmanned aerial vehicle cluster system has incomparable advantages in the aspects of timeliness, economy, functionality and the like. The unmanned aerial vehicle cluster technology has become an important development direction of unmanned aerial vehicle system application, is widely applied to civil fields such as search rescue, agricultural plant protection, electric power line inspection and environment monitoring, and plays an important role in special tasks related to firepower striking, electronic countermeasure, radar decoy and the like. Therefore, the related technology for developing unmanned aerial vehicle clusters is required for industry, has great urgency, and has important significance for promoting social development and promoting scientific and technical progress.
As a key component of the unmanned aerial vehicle cluster in task execution, track planning is a bridge between perception and control, and determines the efficiency of the unmanned aerial vehicle cluster to cooperatively execute tasks, so that the unmanned aerial vehicle cluster is a bottleneck technology for realizing autonomous intellectualization of the cluster. Unmanned aerial vehicle cluster collaborative trajectory planning does not simply stack together the trajectories that multiple unmanned aerial vehicles individually plan, but involves numerous factors in the collaborative trajectory planning process, such as complex variability of the flight environment, explosive growth of the problem dimension due to the expansion of the number of unmanned aerial vehicles, collisions between unmanned aerial vehicle tasks, etc., which are interwoven together to make the problem extraordinarily complex.
Aiming at the problem of cluster track planning, the existing research works mostly adopt a centralized architecture system, and combine ideas of pure mathematical optimization, heuristic intelligence and biological population evolution to realize unmanned plane cluster collaborative track planning. The centralized methods require a central computing unit in the cluster system, and the central unit can obtain the state information of the whole system and transmit the planning result to each unmanned plane. Although the centralized methods can plan the globally optimal track, the biggest disadvantage is that the calculated amount is large, the calculated time is long, the communication system is seriously depended on, the whole system is crashed once the central node fails, and the system is difficult to directly expand to a cluster system with larger scale.
Disclosure of Invention
The invention aims to provide a track planning method and device and an unmanned aerial vehicle cluster, so as to effectively solve the defects of high computational complexity, insufficient reliability and the like caused by a centralized method in the prior art, solve the problem of the centralized track planning of the unmanned aerial vehicle constructed based on MPC (Model Predictive Control) by combining a distributed optimization idea such as an alternate direction multiplier method, and perform data exchange with a neighborhood unmanned aerial vehicle to avoid mutual collision, thereby being capable of generating continuous smooth tracks meeting requirements more efficiently.
Embodiments of the invention may be implemented as follows:
in a first aspect, an embodiment of the present invention provides a trajectory planning method, applied to any one target unmanned aerial vehicle in an unmanned aerial vehicle cluster, where the target unmanned aerial vehicle corresponds to a neighboring unmanned aerial vehicle set formed by at least one neighboring unmanned aerial vehicle that needs to avoid collision, and the method includes:
acquiring a starting point and a terminal point, and initializing to obtain an input parameter set;
taking the starting point as a current track point;
according to the input parameter set, the first cost function and the second cost function, carrying out data exchange with each neighborhood unmanned aerial vehicle to determine the input parameter set of the next track point and the target flat input; the first cost function and the second cost function are obtained by decomposing a centralized trajectory planning problem of an unmanned aerial vehicle set constructed based on MPC by using an alternate direction multiplier method;
determining the state information and the output information of the next track point based on the state information of the current track point and the target flat input;
judging whether the next track point is the end point or not;
if yes, state information and output information of K track points from the starting point to the end point are obtained to generate a smooth flight track;
And if not, taking the next track point as the current track point, and returning to execute the step of carrying out data exchange with each neighborhood unmanned aerial vehicle according to the input parameter set, the first cost function and the second cost function to determine the input parameter set of the next track point and the target flat input until the next track point is the end point, and obtaining state information and output information of K track points from the start point to the end point to generate a smooth flight track.
In a second aspect, an embodiment of the present invention further provides a trajectory planning device, which is applied to any one target unmanned aerial vehicle in an unmanned aerial vehicle cluster, where the target unmanned aerial vehicle corresponds to a neighboring unmanned aerial vehicle set formed by at least one neighboring unmanned aerial vehicle that needs to avoid collision, and the device includes:
the data acquisition module is used for acquiring a starting point and an ending point and initializing to obtain an input parameter set;
the track planning module is used for:
taking the starting point as a current track point;
according to the input parameter set, the first cost function and the second cost function, carrying out data exchange with each neighborhood unmanned aerial vehicle to determine the input parameter set of the next track point and the target flat input; the first cost function and the second cost function are obtained by decomposing a centralized trajectory planning problem of an unmanned aerial vehicle set constructed based on MPC by using an alternate direction multiplier method;
Determining the state information and the output information of the next track point based on the state information of the current track point and the target flat input;
judging whether the next track point is the end point or not;
if yes, state information and output information of K track points from the starting point to the end point are obtained to generate a smooth flight track;
and if not, taking the next track point as the current track point, and returning to execute the step of carrying out data exchange with each neighborhood unmanned aerial vehicle according to the input parameter set, the first cost function and the second cost function to determine the input parameter set of the next track point and the target flat input until the next track point is the end point, and obtaining state information and output information of K track points from the start point to the end point to generate a smooth flight track.
In a third aspect, an embodiment of the present invention further provides an unmanned aerial vehicle cluster, where the unmanned aerial vehicle cluster includes N unmanned aerial vehicles, and each of the unmanned aerial vehicles is configured to perform trajectory planning in a preset flight area according to the trajectory planning method of the first aspect, so as to implement a flight task of the unmanned aerial vehicle cluster.
Compared with the prior art, the embodiment of the invention provides a track planning method, a track planning device and an unmanned aerial vehicle cluster, which are used for decomposing a centralized track planning problem of the unmanned aerial vehicle cluster constructed based on MPC by utilizing an alternate direction multiplier method to obtain a first cost function and a second cost function, so that each track point between a starting point and an end point is solved based on the first cost function and the second cost function in the track planning process, and data exchange is carried out with each neighborhood unmanned aerial vehicle in the track point solving process so as to ensure information cooperation between unmanned aerial vehicles to avoid collision. The method can effectively solve the defects of high computational complexity, insufficient reliability and the like caused by a centralized method, meanwhile, the method is combined with a distributed optimization idea such as an alternate direction multiplier method to perform problem decomposition, and data exchange is performed with a neighborhood unmanned aerial vehicle to avoid mutual collision, so that a continuous smooth track meeting the requirements can be generated more efficiently.
Drawings
In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings that are needed in the embodiments will be briefly described below, it being understood that the following drawings only illustrate some embodiments of the present invention and therefore should not be considered as limiting the scope, and other related drawings may be obtained according to these drawings without inventive effort for a person skilled in the art.
Fig. 1 is a schematic flow chart of a track planning method according to an embodiment of the present invention.
Fig. 2 is a second flowchart of a track planning method according to an embodiment of the present invention.
Fig. 3 is a third flowchart of a track planning method according to an embodiment of the present invention.
Fig. 4 is a diagram of the effect of track planning in case one.
Fig. 5 is a schematic diagram of the distance between unmanned aerial vehicles in case one.
Fig. 6 is a schematic diagram of statistical results of distances between the unmanned aerial vehicle and the obstacle in case one.
Fig. 7 is a trace plan effect diagram of case two.
Fig. 8 is a schematic diagram of the distance between unmanned aerial vehicles in case two.
Fig. 9 is a schematic diagram of a statistical result of a distance between the unmanned aerial vehicle and the obstacle in the second case.
Fig. 10 is a schematic structural diagram of a track planning apparatus according to an embodiment of the present invention.
Detailed Description
For the purpose of making the objects, technical solutions and advantages of the embodiments of the present invention more apparent, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present invention, and it is apparent that the described embodiments are some embodiments of the present invention, but not all embodiments of the present invention. The components of the embodiments of the present invention generally described and illustrated in the figures herein may be arranged and designed in a wide variety of different configurations.
Thus, the following detailed description of the embodiments of the invention, as presented in the figures, is not intended to limit the scope of the invention, as claimed, but is merely representative of selected embodiments of the invention. All other embodiments, which can be made by those skilled in the art based on the embodiments of the invention without making any inventive effort, are intended to be within the scope of the invention.
It should be noted that: like reference numerals and letters denote like items in the following figures, and thus once an item is defined in one figure, no further definition or explanation thereof is necessary in the following figures.
Furthermore, the terms "first," "second," and the like, if any, are used merely for distinguishing between descriptions and not for indicating or implying a relative importance.
It should be noted that the features of the embodiments of the present invention may be combined with each other without conflict.
Aiming at the problem of cluster track planning, the existing research works mostly adopt a centralized architecture system, and combine ideas of pure mathematical optimization, heuristic intelligence and biological population evolution to realize unmanned plane cluster collaborative track planning. The centralized methods require a central computing unit in the cluster system, and the central unit can obtain the state information of the whole system and transmit the planning result to each unmanned plane. Although the centralized methods can plan the globally optimal track, the biggest disadvantage is that the calculated amount is large, the calculated time is long, the communication system is seriously depended on, the whole system is crashed once the central node fails, and the system is difficult to directly expand to a cluster system with larger scale.
Based on the findings of the above technical problems, the inventors have made creative efforts to propose the following technical solutions to solve or improve the above problems. It should be noted that the above prior art solutions have all the drawbacks that the inventors have obtained after practice and careful study, and thus the discovery process of the above problems and the solutions to the problems that the embodiments of the present application hereinafter propose should not be construed as what the inventors have made in the inventive process of the present application, but should not be construed as what is known to those skilled in the art.
The inventor finds out through investigation that, in essence, unmanned aerial vehicle cluster collaborative track planning is to solve the problem that a plurality of targets are optimized simultaneously, and not only needs to ensure that the independent track of each unmanned aerial vehicle meets the flight requirement, but also needs to ensure that the tracks of a plurality of unmanned aerial vehicles can cooperate together to achieve the integral target of the cluster. In recent years, distributed optimization theory and application pay more and more attention and gradually penetrate into various aspects of scientific research, engineering application and social life, the distributed optimization effectively realizes the optimization task through cooperation coordination among multiple intelligent agents, and the distributed optimization method can be used for solving the large-scale complex optimization problem that many centralized algorithms are difficult to compete, and is consistent with the characteristics of an unmanned aerial vehicle cluster system. In particular in distributed optimization of multi-agent systems, the goal of distributed optimization is to minimize one global objective function in a distributed way by local computation and communication, while this function is the sum of all agent's sub-objective functions. Although distributed optimization is always a long-term research topic in the optimization field, the application of the distributed optimization in unmanned aerial vehicle cluster technology is still in a primary stage due to the fact that track planning is mostly a non-convex optimization problem with constraints and communication overhead caused by cooperation. Currently, the Alternate Direction Multiplier Method (ADMM), which is one of the distributed optimization algorithms, has proven to be an attractive option to solve the unmanned aerial vehicle cluster trajectory planning problem in terms of superior convergence speed, computational efficiency, and communication efficiency in the robot-related field.
Therefore, for the above-mentioned problems caused by the problem of cluster track planning by adopting the centralized architecture system, the inventor proposes a manner for solving the problem of track planning of unmanned aerial vehicle clusters based on a distributed architecture, wherein a central computing unit is not needed in the distributed architecture, and each unmanned aerial vehicle calculates and generates own track, so that the system has self-adaption, self-organization and good coordination performance, and is more suitable for online real-time track planning compared with the centralized architecture.
In view of this, an embodiment of the present invention provides a trajectory planning method, where a planning objective is that each unmanned aerial vehicle of a cluster system generates a smooth flight trajectory in real time, so that the unmanned aerial vehicle can reach a target position from a starting position, and meanwhile, kinematic constraints of the unmanned aerial vehicles need to be considered, mutual collision between unmanned aerial vehicles is ensured, and collision with an obstacle in an external environment is avoided, so as to ensure minimum or lower total cost. And the method effectively combines the thought of a distributed optimization algorithm according to the track planning characteristics, so that the problem complexity is further reduced, and the efficiency of unmanned aerial vehicle cluster track planning is improved. The following detailed description is made by way of example with reference to the accompanying drawings.
It should be noted that, the trajectory planning method provided by the embodiment of the present invention may be applied to a target unmanned aerial vehicle, where the target unmanned aerial vehicle is any one of N unmanned aerial vehicles in an unmanned aerial vehicle cluster, and the target unmanned aerial vehicle corresponds to a neighborhood unmanned aerial vehicle set formed by at least one neighborhood unmanned aerial vehicle that needs to be avoided from collision. Wherein the neighborhood unmanned aerial vehicle is only a relative concept, for example, assuming that the unmanned aerial vehicle cluster includes 20 unmanned aerial vehicles (unmanned aerial vehicle No. 1-unmanned aerial vehicle No. 20), for unmanned aerial vehicle No. 5, the neighborhood unmanned aerial vehicle set of unmanned aerial vehicle No. 5 may include unmanned aerial vehicles No. 4 and No. 6; and for unmanned aerial vehicle No. 13, unmanned aerial vehicle No. 13's neighborhood unmanned aerial vehicle collection can include unmanned aerial vehicle No. 11, unmanned aerial vehicle No. 12 and unmanned aerial vehicle No. 14. This example is merely an example and is not intended to be limiting herein.
Referring to fig. 1, fig. 1 is a flow chart of a track planning method provided by an embodiment of the present invention, wherein an execution subject of the track planning method is a target unmanned aerial vehicle in an unmanned cluster, and the track planning method includes steps S101-S107:
s101, acquiring a starting point and an ending point, and initializing to obtain an input parameter set.
It will be appreciated that for a flight mission, it is desirable that all of the unmanned aerial vehicles in the unmanned aerial vehicle cluster be co-ordinated, and that the start and end points between different unmanned aerial vehicles may be different or the same, with the start and end points of the target unmanned aerial vehicle being set at the actual mission requirements.
In this embodiment, the target unmanned aerial vehicle needs to be initialized at its own starting point position, and the starting point, the end point and the input parameter set during initialization can be obtained by the initialization.
S102, taking the starting point as the current track point.
And S103, carrying out data exchange with each neighborhood unmanned aerial vehicle according to the input parameter set, the first cost function and the second cost function so as to determine the input parameter set of the next track point and the target flat input.
In this embodiment, the first cost function and the second cost function are obtained by decomposing a centralized trajectory planning problem based on discrete MPC by using the distributed optimization concept of ADMM.
S104, determining the state information and the output information of the next track point based on the state information of the current track point and the target flat input.
The state information may represent a flat state of the target unmanned aerial vehicle, including position, speed, acceleration, and yaw angle information. The output information may characterize a flat output of the target drone, including position and yaw angle. The target flat input may characterize the third derivative of the target drone position and yaw rate.
S105, judging whether the next track point is an end point or not.
If the next track point is the end point, the track of the target unmanned aerial vehicle is planned, i.e. step S107 is executed: obtaining state information and output information of K track points from a starting point to an end point to generate a smooth flight track; if the next track point is not the end point, the following step S106 is executed, and then the above step S103 is executed again until the next track point is the end point, and step S107 is executed: and obtaining state information and output information of K track points from the starting point to the end point to generate a smooth flight track.
S106, taking the next track point as the current track point.
And S107, obtaining state information and output information of K track points from the starting point to the end point to generate a smooth flight track.
The track planning method provided by the embodiment of the invention can effectively solve the defects of high computational complexity, insufficient reliability and the like caused by a centralized method, and simultaneously combines the distributed optimization idea of an alternate direction multiplier method to perform problem decomposition, and performs data exchange with a neighborhood unmanned aerial vehicle to avoid mutual collision, so that a continuous smooth track meeting the requirement can be generated more efficiently.
Here, a process of decomposing a centralized trajectory planning problem based on discrete MPC using the distributed optimization concept of ADMM will be described. The process is roughly divided into the following three steps:
Step 1, acquiring an unmanned aerial vehicle dynamic model, track performance indexes and track constraint information of an unmanned aerial vehicle;
step 2, constructing a centralized trajectory planning problem of an unmanned aerial vehicle based on MPC;
and 3, decomposing the original problem based on the ADMM, and designing a distributed unmanned aerial vehicle cluster track planning method.
First, description is made of each item of data in step 1:
each unmanned aerial vehicle in the first unmanned aerial vehicle cluster can be a four-rotor unmanned aerial vehicle, and due to typical differential flat characteristics of the unmanned aerial vehicle, a feedforward linearization process is used for converting a nonlinear model of the unmanned aerial vehicle into an equivalent linear flat model, so that the dimension of a planning space is reduced, the solving difficulty of a problem is reduced, and meanwhile, a planned track is feasible for the unmanned aerial vehicle. Therefore, the unmanned aerial vehicle dynamics model of the quad-rotor unmanned aerial vehicle can be represented by a dynamic equation:
v k+t+1|k =Av k+t|k +Bu k+t|k
ζ k+t+1|k =Cv k+t+1|k
wherein t=1, 2, …, H represents the prediction domain length, A, B, C are coefficient matrices; v k+t|k 、v k+t+1|k Respectively representing that the unmanned aerial vehicle is in the prediction domain of the kth track point: the flat state of the t-th predicted point and the flat state of the t+1th predicted point; u (u) k+t|k Representing the flat input of a t-th predicted point in the predicted domain of the k-th track point of the unmanned plane; zeta type k+t+1|k The flat output of the t+1th predicted point in the predicted domain of the kth track point of the unmanned plane is represented.
Flat state of unmanned planeIncluding position, speed, acceleration and yaw angle information. Flat input of unmanned plane->Including the third derivative of position and yaw rate. Flat output of unmanned aerial vehicle->Including position and yaw angle.
Wherein x, y and z together represent the position coordinates of the unmanned aerial vehicle;together representing the speed of the drone;together representing the acceleration of the drone. />Third-order derivatives representing position coordinates of the unmanned aerial vehicle; />Representing the yaw angle of the unmanned aerial vehicle +.>Representing the yaw rate of the drone.
Second, the track performance index may include the smoothness of the track, as close as possible to the target location (i.e., end point), etc.
And the track constraint information of the third unmanned aerial vehicle comprises an initial state, a target state, a kinematic constraint, an obstacle collision prevention constraint and an inter-aircraft collision prevention constraint. Wherein, for obstacle collision avoidance constraint and inter-machine collision avoidance constraint, the safety of the planned track can be ensured based on a control obstacle function (CBF) method. By usingThe control obstacle function between the unmanned aerial vehicle i and the neighborhood unmanned aerial vehicle j and the control obstacle function between the unmanned aerial vehicle i and the obstacle o are respectively represented.
Then, in general, the inter-machine collision avoidance constraint may be expressed as the following formula (1):
in general, the obstacle avoidance constraint may be expressed as the following equation (2):
wherein ,pi 、p j 、p o Respectively representing the position information of the unmanned plane i, the neighborhood unmanned plane j and the obstacle o; gamma ray ij 、γ io Are all scale factors, d safe1 、d safe2 The minimum safe distance between the unmanned aerial vehicle i and the neighborhood unmanned aerial vehicle j and the minimum safe distance between the unmanned aerial vehicle i and the obstacle o are respectively carried out. N (N) i A neighborhood unmanned aerial vehicle set for unmanned aerial vehicle i.
In actual flight, the unmanned aerial vehicle may collide due to the influence of aerodynamic effects from surrounding unmanned aerial vehicles, and therefore, inA scaling matrix theta is added, the collision boundary of the unmanned aerial vehicle is modeled as an ellipsoid, the influence of aerodynamic effect is reduced, and theta is a diagonal matrix (namely, elements on diagonal lines are a, b and c).
Next, description is made of step 2:
by introducing the rolling time domain optimization idea of MPC, the unmanned aerial vehicle cluster track planning problem is described as a multi-target optimization problem aiming at constraints such as power performance, obstacle avoidance and collision avoidance and track performance indexes of the unmanned aerial vehicle, so that a centralized track planning problem based on discrete MPC can be constructed, and a target cost function of the centralized track planning problem is as shown in a formula (3):
The objective cost function includes 3 track performance indexes, namely, the deviation of the current state and the objective state, the size of the flat input and the size of the variation of the flat input, namely, the optimization aims at: the unmanned aerial vehicle is enabled to approach the target state as soon as possible, meanwhile, the continuous smoothness of the track is guaranteed, and the unmanned aerial vehicle is convenient to effectively track. Q, R, S are weight coefficient matrices.
Three constraint conditions of the target cost function are:
1. kinematic constraints of unmanned aerial vehicle:
where Ω is a set of state constraints.
2. Inter-plane collision avoidance constraint of unmanned aerial vehicle i and unmanned aerial vehicle j:
3. obstacle collision avoidance constraints for unmanned aerial vehicle i:
for convenience of subsequent presentation, useRepresenting the inter-plane collision prevention constraint between unmanned plane i and unmanned plane j by +.>Representing obstacle collision avoidance constraints for the unmanned aerial vehicle.
Then, description is made of step 3:
the traditional centralized mode can cause huge calculation amount, the reliability of the system is low, the whole cluster system can stop working once a central calculation unit fails, the scale and configuration of the cluster system are not flexible enough, and the expandability of the system is poor. The invention is based on the idea of a distributed optimization algorithm ADMM, which decomposes a high-dimensional unmanned aerial vehicle cluster track planning problem into a plurality of low-dimensional optimization problems, and enables each unmanned aerial vehicle to plan own optimal track, thereby accelerating the on-line solving speed of the track planning problem by parallel calculation, forming a distributed unmanned aerial vehicle cluster collaborative track planning framework, wherein the focus is that data interaction is carried out between the unmanned aerial vehicle and a neighborhood unmanned aerial vehicle thereof to avoid collision between the unmanned aerial vehicles.
For the track planning problem of the unmanned aerial vehicle cluster, the purpose of interaction between the unmanned aerial vehicle and the unmanned aerial vehicle in the neighborhood is to mutually tell the position information of the unmanned aerial vehicle, so that collision is avoided. Based on the idea of ADMM, the invention introduces a flat output copy, ζ i Copy w of (2) i And use w i→j To represent the expected output track proposed by unmanned plane i to neighborhood unmanned plane j, then the inter-plane collision avoidance constraint of unmanned plane i may be restated as:
it can be seen that by introducing a copy of the flat output, the inter-aircraft collision avoidance constraint of the unmanned aerial vehicle can no longer depend on the original trajectory ζ i But rather uses its copy w i and wi→j To realize the cooperative collision avoidance of the clusters.
This converts the target cost function into a new cost function: minf (Z) k ,U k )+g(W k ) Wherein f (Z k ,U k) and g(Wk ) The definition is as follows:
wherein , respectively representing a flat output discrete sequence, a flat input discrete sequence and a flat output discrete sequence copy of the unmanned aerial vehicle i in a prediction domain. I is an indication function, and phi represents obstacle collision prevention constraint and inter-machine collision prevention constraint of the unmanned aerial vehicle I. Thus, the new cost function expression is the following equation (4):
s.t. means constraint conditions.
Equation (4) is a general optimization problem with equality constraints, and its augmented lagrangian form is shown in equation (5):
wherein ,λi And lambda is i→j For the dual variable, ρ is the penalty coefficient and T represents the transpose of the matrix. The optimization problem can then be solved using the ADMM algorithm, updating the iterations in turnAnd->λ i And lambda is i→j Until the stop iteration condition is satisfied.
The following describes a specific procedure for solving the trace point in the present invention.
In an alternative implementation manner, after multiple iterations are required to solve the next track point in the current track point, referring to fig. 2 on the basis of fig. 1, the substeps of step S103 may include S1031-S1034:
s1031, based on the input parameter set, the first cost function and the second cost function, carrying out twice data exchange with each neighborhood unmanned plane to determine a new input parameter set and a flat input discrete sequence;
s1032, judging whether the new input parameter set meets the iteration stopping condition;
s1033, taking the new input parameter set as the input parameter set of the next track point, and determining the target flat input of the next track point from the flat input discrete sequence.
If the new input parameter set meets the stop iteration condition, executing step S1033 to obtain the input parameter set of the next track point; if the new input parameter set does not meet the stop iteration condition, continuing iteration, that is, returning to the execution step S1031, until the new input parameter set meets the stop iteration condition, and obtaining the input parameter set of the next track point.
Optionally, assuming that the target unmanned aerial vehicle is unmanned aerial vehicle i in the unmanned aerial vehicle cluster, at the kth track point, the input parameter set may include: the target unmanned aerial vehicle comprises a flat output discrete sequence, a flat output discrete sequence copy and a local dual variable, expected dual variables of the target unmanned aerial vehicle for each neighborhood unmanned aerial vehicle, and expected dual variables and expected output sequences of each neighborhood unmanned aerial vehicle for the target unmanned aerial vehicle. I.e. the input parameter set comprises:λ i 、/> each time step S1031 is performed, an iteration is performed, and each iteration is divided into the following 5 steps:
1. target unmanned aerial vehicle first updates
2. The target unmanned aerial vehicle performs first data exchange with each neighborhood unmanned aerial vehicle: transmittingTo neighborhood unmanned plane j (j E N) i ) Obtaining new->
3. Target drone next update
4. The target drone then updates lambda i 、
5. The target unmanned aerial vehicle performs second data exchange with each neighborhood unmanned aerial vehicle: send lambda i→j Andto neighborhood unmanned plane j (j E N) i ) Obtaining new->New->
Through the above step 5, the new input parameter set includes: new type of materialNew->Novel lambda i New and newNew->New->
The 5 steps will be described below.
In an alternative implementation, referring to fig. 3, the substeps of step S1031 may include S001-S006.
S001, inputting the local dual variable and the flat output discrete sequence copy of the target unmanned aerial vehicle, and the expected dual variable and the expected output sequence of each neighborhood unmanned aerial vehicle to the target unmanned aerial vehicle into a first cost function to obtain an updated flat output discrete sequence of the target unmanned aerial vehicle and an updated flat input discrete sequence of the target unmanned aerial vehicle.
In this embodiment, the expression of the first cost function may be as shown in formula (6):
in the formula (6), i and k respectively represent the target unmanned aerial vehicle and the current track point, H represents the length of the prediction domain, and H can be set according to the requirements of the flight task and can be generally 10-20.j E N i Neighborhood unmanned aerial vehicle set N representing target unmanned aerial vehicle i i The j-th neighborhood unmanned aerial vehicle in (a).
Respectively representing a flat output discrete sequence, a flat input discrete sequence and a flat output discrete sequence copy of the target unmanned aerial vehicle i at the current track point k; />A flat output of the H predicted point representing the current track point k; />H-1 th pre-cursor representing current trace point kFlat input of measuring point, < >>Representing a flat output copy of the H predicted point of the current trace point k.
The transposition of the target unmanned aerial vehicle i local end dual variable and the transposition of the target unmanned aerial vehicle i expected dual variable of the neighborhood unmanned aerial vehicle j are respectively represented; / >Representing an expected output sequence of the neighborhood unmanned aerial vehicle j to the target unmanned aerial vehicle i; ρ is a penalty coefficient;
to output the cost function->Indicating an endpoint; q, R, S are weight coefficient matrixes; II 2 Represents an L2 norm;
s.t.the constraint condition representing the first cost function is a state constraint set phi, which comprises the kinematic constraint of the unmanned aerial vehicle and the obstacle collision prevention constraint. (1) In the state constraint set phi, the kinematic constraint of the unmanned aerial vehicle is as follows:
(2) In the state constraint set phi, the obstacle collision prevention constraint is thatThe unfolding is as follows:
thus, in equation (6), the update is solved for on the premise that minimization of the cost value of the first cost function is requiredCorrespondingly, the required input data of the first price function includes: the input parameter set used in this iteration is: λi,>then solve and update new ++>New->
S002, sending the updated flat output discrete sequence of the target unmanned aerial vehicle to each neighborhood unmanned aerial vehicle, and receiving the flat output discrete sequence of each neighborhood unmanned aerial vehicle.
In this embodiment, step S002 represents the first data exchange of the target unmanned aerial vehicle: transmission execution step S001New products obtainedTo neighborhood unmanned plane j (j E N) i ) Receiving new +. >
S003, inputting the local end dual variable, the updated flat output discrete sequence of the target unmanned aerial vehicle and the expected dual variable of each neighborhood unmanned aerial vehicle by the target unmanned aerial vehicle into a second cost function, and cooperatively calculating the updated flat output discrete sequence copy of the target unmanned aerial vehicle and the expected output sequence of each neighborhood unmanned aerial vehicle by the target unmanned aerial vehicle.
In this embodiment, the expression of the second cost function may be as shown in formula (7):
in the formula (7) of the present invention,representing the transpose of the desired dual variable of the target drone i to the neighborhood drone j,the expected output sequence of the target unmanned aerial vehicle i to the neighborhood unmanned aerial vehicle j,
s.t.the constraint condition representing the second cost function includes an inter-aircraft collision avoidance constraint between the target unmanned aerial vehicle i and the neighborhood unmanned aerial vehicle j.
The extension of the inter-aircraft collision prevention constraint between the representative target unmanned aerial vehicle i and the neighborhood unmanned aerial vehicle j is as follows:
Θ=diag(a,b,c),a=b=1,c=2
therefore, in equation (7), the update is solved for on the premise that the cost value of the second cost function is minimizedCorrespondingly, the required input data of the second cost function includes: lambda in input parameter set used in this iteration i 、/>And new ++obtained after executing step S001>Then solve and update to obtain new New->
S004, determining the updated local end dual variable based on the local end dual variable, the updated flat output discrete sequence of the target unmanned aerial vehicle and the updated flat output discrete sequence copy of the target unmanned aerial vehicle.
In this embodiment, the updated calculation formula of the local end dual variable of the target unmanned aerial vehicle i is:in the calculation formula, lambda i For the local dual variable in the input parameter set corresponding to the iteration, the user is added with the input parameter set corresponding to the iteration>To perform the new +.sub.F obtained after the above step S001>To execute the new step S003
S005, for each neighborhood unmanned aerial vehicle, determining the updated expected dual variables of the target unmanned aerial vehicle to the neighborhood unmanned aerial vehicle based on the expected dual variables of the target unmanned aerial vehicle to the neighborhood unmanned aerial vehicle, the flat output discrete sequence of the neighborhood unmanned aerial vehicle and the expected output sequence of the target unmanned aerial vehicle to the neighborhood unmanned aerial vehicle.
In this embodiment, for the neighborhood unmanned aerial vehicle j, the calculation formula of the updated expected dual variable of the target unmanned aerial vehicle i to the neighborhood unmanned aerial vehicle j is:in the calculation formula, lambda i→j For the expected dual variable of the target unmanned aerial vehicle i to the neighborhood unmanned aerial vehicle j in the input parameter set corresponding to the iteration, the target unmanned aerial vehicle i is in the presence of the target unmanned aerial vehicle i >New +.A. for the neighborhood unmanned plane j obtained after executing the above step S002>To perform the above step S003, new ∈>
S006, for each neighborhood unmanned aerial vehicle, sending the expected output sequence of the target unmanned aerial vehicle to the neighborhood unmanned aerial vehicle and the updated expected dual variable of the target unmanned aerial vehicle to the neighborhood unmanned aerial vehicle, and receiving the expected dual variable and the expected output sequence of the target unmanned aerial vehicle to the neighborhood unmanned aerial vehicle.
In this embodiment, step S006 represents the second data exchange of the target unmanned aerial vehicle: transmitting the new data obtained by executing the step S003The new lambda obtained after the execution of the step S005 i→j To neighborhood unmanned plane j (j E N) i ) Obtaining new->New->
Steps S001-S006 are an iterative process, and after completing sequential iterations, a new input parameter set is obtained, which may include:
(1) The execution of the above step S001 results in: new type of material
(2) The step S003 is performed: new type of material
(3) The step S004 is performed: novel lambda i ;
(4) The step S005 is performed: new type of material
(5) The following step S006 is performed: new type of materialNew->
The above steps S001-S006 and their detailed description are a complete iteration process, based on which the condition for stopping iteration in S1032 may be:
/>
Wherein epsilon 1 and epsilon 2 both represent preset thresholds. I.e. stopping the iteration condition, only the new input parameter set needs to be used: new type of materialNew->New->
When the three items meet the iteration stop condition, the iteration is stopped. And the new input parameter set obtained from the last iteration can be used as the input parameter set of the next track point, and the new input parameter set obtained from the last iterationTo determine a target flat input to the next track point. Then calculating the state information and output information of the next track point, wherein the formula is as follows:
wherein ,status information indicating the current trajectory point of the target unmanned aerial vehicle i,/->Is->Is->Representing a target flat input, ++>For the status information of the next track point, +.>Is the output information of the next track point.
For the target unmanned aerial vehicle i, when the next track point is the end point, the target unmanned aerial vehicle i completes the track planning of the target unmanned aerial vehicle i, and K track points from the start point to the end point of the target unmanned aerial vehicle i can be fitted into a smooth flight track.
For the unmanned aerial vehicle cluster, when N unmanned aerial vehicles all reach respective end points, the track planning of the whole cluster is completed, and the algorithm is stopped.
It should be noted that, in the above method embodiment, the execution sequence of each step is not limited by the drawing, and the execution sequence of each step is based on the actual application situation.
Two cases are given below to verify the effect of the trajectory planning method provided by the present invention in practical applications.
Case one:
suppose that the unmanned aerial vehicle cluster comprises 10 unmanned aerial vehicles (No. 1-10), the flight mission is: so unmanned aerial vehicle is with flying in same direction to pass through ring that is covered with cylinder barrierThe initial positions (unit is m) of the numbers 1 to 10, namely, the starting points are respectively: [0,1,2] T 、[0,3,2] T 、[0,5,2] T 、[0,7,2] T 、[0,9,2] T 、[0,11,2] T 、[0,13,2] T 、[0,15,2] T 、[0,17,2] T 、[0,19,2] T 。
The target positions of the numbers 1 to 10, namely the end points are respectively: [40,1,2] T 、[40,3,2] T 、[40,5,2] T 、[40,7,2] T 、[40,9,2] T 、[40,11,2] T 、[40,13,2] T 、[40,15,2] T 、[40,17,2] T 、[40,19,2] T 。
Here, it is assumed that the yaw angle is always 0, and the speed, acceleration, third-order derivative of the speed of the unmanned aerial vehicle are all initially 0, the minimum safe distance d safe1 =0.4m、d safe2 =0.2m, number of predicted points in prediction domain h=10, sampling time T s =0.15 s, scaling factor γ ij =γ io The penalty factor ρ=1=0.8. The three weight coefficient matrixes are respectively Q=diag [50,50,50,50 ]]、R=diag[5,5,5,5]、Q=diag[10,10,10,10]. Flight area definition (unit: m) of flight mission is as follows (8), speed definition of unmanned aerial vehicle flight (unit: m/s is as follows (9), acceleration definition of unmanned aerial vehicle flight (unit: m/s) 2 ) The following formula (10):
p min =[0,-3,0] T ,p max =[44,24,6] T (8)
v min =[-3,-3,-3] T ,v max =[3,3,3] T (9)
a min =[-1,-1,-1] T ,a max =[1,1,1] T (10)
based on the data of the first case, the number 1 to the number 10 are respectively based on the complete self-track planning of the track planning method, and the obtained respective flight tracks can be shown in fig. 4.
When the numbers 1 to 10 fly according to the track points of the numbers, a schematic diagram of the distance between unmanned aerial vehicles is shown in fig. 5, and a schematic diagram of the statistical result of the distance between the unmanned aerial vehicles and the obstacles is shown in fig. 6.
Case two:
suppose that the unmanned aerial vehicle cluster comprises 12 unmanned aerial vehicles (No. 1-12), and the flight tasks are: 12 unmanned aerial vehicles (6 on each side) fly in opposite directions to pass through the environment fully covered with the cylindrical barrier. The initial positions (m) of the numbers 1 to 12 are respectively: [0,3,3] T 、[0,6,3] T 、[0,9,3] T 、[0,12,3] T 、[0,15,3] T 、[0,18,3] T 、[40,3,3] T 、[40,6,3] T 、[40,9,3] T 、[40,12,3] T 、[40,15,3] T 、[40,18,3] T The method comprises the steps of carrying out a first treatment on the surface of the The target positions of the No. 1 and the No. 12 are respectively initial positions of the contralateral unmanned aerial vehicle, and other parameters are consistent with the first case.
Based on the data of the second case, the track planning methods of the first and the second cases are respectively based on the complete track planning of the first and the second cases, and the obtained flight tracks can be shown in fig. 7. When the numbers 1 to 12 fly according to the track points of the numbers, a schematic diagram of the distance between unmanned aerial vehicles is shown in fig. 8, and a schematic diagram of the statistical result of the distance between the unmanned aerial vehicles and the obstacles is shown in fig. 9.
From the effect graphs of the first case and the second case, it can be seen that the track planning method based on the distributed architecture provided by the invention can effectively solve the track planning problem of the unmanned aerial vehicle cluster, and the generated track is safe enough and continuous and smooth.
In order to further verify the efficiency of the track planning, on the basis of various parameters proposed in case one, comparing the performances of the track planning method of the invention with the performances of a centralized track planning method (unmanned aerial vehicle cluster track planning method based on MPC) under the condition of different numbers of unmanned aerial vehicles (2, 5, 10, 15), the obtained statistical table is shown in the following table:
as can be derived from the table, the track length obtained by the track planning of the cluster is realized by the track planning method based on the distributed MPC-ADMM, and compared with the track length obtained by the track planning of the cluster by the track planning method based on the centralized MPC, the track length obtained by the track planning of the cluster is not greatly different. The invention is however computationally shorter (i.e. less time consuming for trajectory planning), especially the advantage of less time consuming when the number of unmanned aerial vehicles is larger. Therefore, by applying the track planning method disclosed by the invention, the time consumption of track planning is not influenced by the expansibility of the number of unmanned aerial vehicles in the unmanned aerial vehicle cluster, and even the time consumption is shorter and stronger, so that the track planning method disclosed by the invention is especially suitable for large-scale unmanned aerial vehicle track planning tasks.
In order to perform the above-described method embodiments and corresponding steps in each of the possible embodiments, an implementation of a trajectory planning device is given below.
Referring to fig. 10, fig. 10 is a schematic structural diagram of a track planning apparatus according to an embodiment of the present invention. The trajectory planning device 200 is applied to any one target unmanned aerial vehicle in an unmanned aerial vehicle cluster, and the target unmanned aerial vehicle corresponds to a neighborhood unmanned aerial vehicle set formed by at least one neighborhood unmanned aerial vehicle needing to be prevented from collision. The trajectory planning device 200 includes: a data acquisition module 210, a track planning module 220.
A data acquisition module 210, configured to acquire a start point and an end point, and initialize to obtain an input parameter set;
a trajectory planning module 220 for: taking the starting point as a current track point; according to the input parameter set, the first cost function and the second cost function, carrying out data exchange with each neighborhood unmanned aerial vehicle to determine the input parameter set of the next track point and the target flat input; the first cost function and the second cost function are obtained by decomposing a centralized trajectory planning problem of the unmanned aerial vehicle constructed based on the MPC by using an alternate direction multiplier method; determining the state information and the output information of the next track point based on the state information of the current track point and the target flat input; judging whether the next track point is an end point or not; if yes, state information and output information of K track points from the starting point to the end point are obtained to generate a smooth flight track; if not, taking the next track point as the current track point, and returning to execute the step of carrying out data exchange with each neighborhood unmanned aerial vehicle according to the input parameter set, the first cost function and the second cost function to determine the input parameter set of the next track point and the target flat input until the next track point is the end point, and obtaining the state information and the output information of K track points from the start point to the end point to generate a smooth flight track.
It should be noted that the trajectory planning module 220 may be specifically configured to implement the steps S102-S107 and their respective sub-steps. It will be clear to those skilled in the art that, for convenience and brevity of description, the specific working process of the track planning apparatus 200 described above may refer to the corresponding process in the foregoing method embodiment, which is not repeated herein.
The embodiment of the invention also provides an unmanned aerial vehicle cluster, which comprises N unmanned aerial vehicles, wherein each unmanned aerial vehicle is used for carrying out track planning in a preset flight area according to the track planning method so as to realize the flight task of the unmanned aerial vehicle cluster.
In summary, the embodiment of the invention provides a track planning method, a track planning device and an unmanned aerial vehicle cluster, which are used for decomposing a centralized track planning problem of the unmanned aerial vehicle cluster constructed based on MPC by utilizing an alternate direction multiplier method to obtain a first cost function and a second cost function, so that each track point between a starting point and an ending point is solved based on the first cost function and the second cost function in the track planning process, and data exchange is carried out with each neighborhood unmanned aerial vehicle in the track point solving process so as to ensure information cooperation between unmanned aerial vehicles to avoid collision. Therefore, the defects of high computational complexity, insufficient reliability and the like caused by a centralized method can be effectively solved, meanwhile, the problem decomposition is carried out by combining a distributed optimization idea such as an alternate direction multiplier method, and the data exchange is carried out with the neighborhood unmanned aerial vehicle to avoid mutual collision, so that a continuous smooth track meeting the requirements can be generated more efficiently.
The present invention is not limited to the above embodiments, and any changes or substitutions that can be easily understood by those skilled in the art within the technical scope of the present invention are intended to be included in the scope of the present invention. Therefore, the protection scope of the invention is subject to the protection scope of the claims.
Claims (10)
1. The track planning method is characterized by being applied to any target unmanned aerial vehicle in an unmanned aerial vehicle cluster, wherein the target unmanned aerial vehicle corresponds to a neighborhood unmanned aerial vehicle set formed by at least one neighborhood unmanned aerial vehicle needing to be prevented from collision, and the method comprises the following steps:
acquiring a starting point and a terminal point, and initializing to obtain an input parameter set;
taking the starting point as a current track point;
according to the input parameter set, the first cost function and the second cost function, carrying out data exchange with each neighborhood unmanned aerial vehicle to determine the input parameter set of the next track point and the target flat input; the first cost function and the second cost function are obtained by decomposing a centralized trajectory planning problem of an unmanned aerial vehicle set constructed based on MPC by using an alternate direction multiplier method;
Determining the state information and the output information of the next track point based on the state information of the current track point and the target flat input;
judging whether the next track point is the end point or not;
if yes, state information and output information of K track points from the starting point to the end point are obtained to generate a smooth flight track;
and if not, taking the next track point as the current track point, and returning to execute the step of carrying out data exchange with each neighborhood unmanned aerial vehicle according to the input parameter set, the first cost function and the second cost function to determine the input parameter set of the next track point and the target flat input until the next track point is the end point, and obtaining state information and output information of K track points from the start point to the end point to generate a smooth flight track.
2. The method of claim 1, wherein the step of exchanging data with each of the neighborhood drones to determine the input parameter set for the next trajectory point and the target flat input according to the input parameter set, the first cost function, and the second cost function, comprises:
Performing two times of data exchange with each of the neighborhood unmanned aerial vehicles based on the input parameter set, the first cost function and the second cost function to determine a new input parameter set and a flat input discrete sequence;
judging whether the new input parameter set meets the iteration stopping condition or not;
if yes, the new input parameter set is used as the input parameter set of the next track point, and the target flat input of the next track point is determined from the flat input discrete sequence;
and if not, returning to execute the steps of exchanging the two times with each neighborhood unmanned aerial vehicle based on the input parameter set, the first cost function and the second cost function to determine a new input parameter set and a flat input discrete sequence until the new input parameter set meets the stop iteration condition, and obtaining the input parameter set of the next track point.
3. The method of claim 2, wherein the set of input parameters comprises: the target unmanned aerial vehicle comprises a flat output discrete sequence, a flat output discrete sequence copy and a local dual variable, expected dual variables of the target unmanned aerial vehicle for each neighborhood unmanned aerial vehicle, and expected dual variables and expected output sequences of each neighborhood unmanned aerial vehicle for the target unmanned aerial vehicle;
The step of exchanging data with each of the neighborhood unmanned aerial vehicles twice based on the input parameter set, the first cost function and the second cost function to determine a new input parameter set and a flat input discrete sequence comprises the following steps:
inputting the local dual variable and the flat output discrete sequence copy of the target unmanned aerial vehicle, and the expected dual variable and the expected output sequence of each neighborhood unmanned aerial vehicle to the target unmanned aerial vehicle into the first cost function to obtain an updated flat output discrete sequence of the target unmanned aerial vehicle and an updated flat input discrete sequence of the target unmanned aerial vehicle;
sending the updated flat output discrete sequence of the target unmanned aerial vehicle to each neighborhood unmanned aerial vehicle, and receiving the flat output discrete sequence of each neighborhood unmanned aerial vehicle;
inputting the local dual variables, the updated flat output discrete sequences of the target unmanned aerial vehicle and the expected dual variables of the target unmanned aerial vehicle to each neighborhood unmanned aerial vehicle into the second cost function, and cooperatively calculating the updated flat output discrete sequence copies of the target unmanned aerial vehicle and the expected output sequences of the target unmanned aerial vehicle to each neighborhood unmanned aerial vehicle;
Determining an updated local end dual variable based on the local end dual variable, the updated flat output discrete sequence of the target unmanned aerial vehicle and the updated flat output discrete sequence copy of the target unmanned aerial vehicle;
for each neighborhood unmanned aerial vehicle, determining an updated expected dual variable of the target unmanned aerial vehicle to the neighborhood unmanned aerial vehicle based on an expected dual variable of the target unmanned aerial vehicle to the neighborhood unmanned aerial vehicle, a flat output discrete sequence of the neighborhood unmanned aerial vehicle and an expected output sequence of the target unmanned aerial vehicle to the neighborhood unmanned aerial vehicle;
for each neighborhood unmanned aerial vehicle, sending an expected output sequence of the target unmanned aerial vehicle to the neighborhood unmanned aerial vehicle and an expected dual variable of the updated target unmanned aerial vehicle to the neighborhood unmanned aerial vehicle, and receiving the expected dual variable and the expected output sequence of the neighborhood unmanned aerial vehicle to the target unmanned aerial vehicle;
wherein the new set of input parameters comprises: the method comprises the steps of updating a flat output discrete sequence of a target unmanned aerial vehicle, updating a flat output discrete sequence copy of the target unmanned aerial vehicle, updating a local end dual variable of the target unmanned aerial vehicle, updating expected dual variables of the target unmanned aerial vehicle for each neighborhood unmanned aerial vehicle, and newly receiving expected dual variables and expected output sequences of each neighborhood unmanned aerial vehicle for the target unmanned aerial vehicle.
4. A method according to claim 3, wherein the expression of the first cost function is:
wherein i and k respectively represent the target unmanned aerial vehicle and the current track point, H represents the length of a prediction domain, and j epsilon N i Neighborhood unmanned aerial vehicle set N representing target unmanned aerial vehicle i i The j-th neighborhood unmanned aerial vehicle in (a); respectively representing a flat output discrete sequence, a flat input discrete sequence and a flat output discrete sequence copy of the target unmanned aerial vehicle i; />A flat output of the H predicted point representing the current track point k; />Flat input representing the H-1 st predicted point of the current track point k, +.>A flat output copy of the H predicted point representing the current trace point k;
the transposition of the target unmanned aerial vehicle i local end dual variable and the transposition of the target unmanned aerial vehicle i expected dual variable of the neighborhood unmanned aerial vehicle j are respectively represented; />Representing an expected output sequence of the neighborhood unmanned aerial vehicle j to the target unmanned aerial vehicle i; ρ is a penalty coefficient;
to output the cost function->Indicating an endpoint; q, R, S are weight coefficient matrixes; II 2 Represents an L2 norm;
s.t.the constraint condition representing the first cost function is a state constraint set phi, which comprises the kinematic constraint of the unmanned aerial vehicle and the obstacle collision prevention constraint.
5. The method of claim 4, wherein the second cost function has an expression of:
wherein ,representing the target unmanned plane i pair neighborhoodTranspose of the desired dual variable of unmanned plane j, +.>Desired output sequence of target unmanned aerial vehicle i to neighborhood unmanned aerial vehicle j, s.t. is>The constraint condition representing the second cost function includes an inter-aircraft collision avoidance constraint between the target unmanned aerial vehicle i and the neighborhood unmanned aerial vehicle j.
6. The method of claim 5, wherein the updated local end-pair variables of the target drone i are:
the expected dual variables of the updated target unmanned aerial vehicle i to the neighborhood unmanned aerial vehicle j are as follows:
7. the method of claim 6, wherein the stop iteration condition is:
wherein epsilon 1 and epsilon 2 both represent preset thresholds.
8. The method of claim 6, wherein the status information and the output information of the next track point are:
status information representing the target unmanned aerial vehicle i at said current trajectory point, is->Input flat for the object, < >>Status information for the next track point, < >>Outputting information for the next track point; A. b, C are coefficient matrices.
9. The utility model provides a track planning device, its characterized in that is applied to arbitrary target unmanned aerial vehicle in unmanned aerial vehicle cluster, target unmanned aerial vehicle corresponds there is at least one neighborhood unmanned aerial vehicle that needs avoid collision to constitute the neighborhood unmanned aerial vehicle collection, the device includes:
The data acquisition module is used for acquiring a starting point and an ending point and initializing to obtain an input parameter set;
the track planning module is used for:
taking the starting point as a current track point;
according to the input parameter set, the first cost function and the second cost function, carrying out data exchange with each neighborhood unmanned aerial vehicle to determine an input parameter set and flat input of a next track point; the first cost function and the second cost function are obtained by decomposing a centralized trajectory planning problem of an unmanned aerial vehicle set constructed based on MPC by using an alternate direction multiplier method;
determining the state information and the output information of the next track point based on the state information of the current track point and the flat input;
judging whether the next track point is the end point or not;
if yes, state information and output information of K track points from the starting point to the end point are obtained to generate a smooth flight track;
and if not, taking the next track point as the current track point, and returning to execute the data exchange with each neighborhood unmanned aerial vehicle according to the input parameter set, the first cost function and the second cost function to determine the input parameter set and the flat input of the next track point until the next track point is the end point, so as to obtain state information and output information of K track points from the start point to the end point to generate a smooth flight track.
10. An unmanned aerial vehicle cluster, comprising N unmanned aerial vehicles, each unmanned aerial vehicle being configured to perform trajectory planning in a predetermined flight area according to the trajectory planning method of any one of claims 1 to 8, so as to implement a flight mission of the unmanned aerial vehicle cluster.
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202310789413.1A CN116700340A (en) | 2023-06-29 | 2023-06-29 | Track planning method and device and unmanned aerial vehicle cluster |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202310789413.1A CN116700340A (en) | 2023-06-29 | 2023-06-29 | Track planning method and device and unmanned aerial vehicle cluster |
Publications (1)
Publication Number | Publication Date |
---|---|
CN116700340A true CN116700340A (en) | 2023-09-05 |
Family
ID=87845013
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202310789413.1A Pending CN116700340A (en) | 2023-06-29 | 2023-06-29 | Track planning method and device and unmanned aerial vehicle cluster |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN116700340A (en) |
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN117271099A (en) * | 2023-11-21 | 2023-12-22 | 山东师范大学 | Automatic space data analysis scheduling system and method based on rule base |
CN118238182A (en) * | 2024-05-27 | 2024-06-25 | 青岛庆泰智能科技有限公司 | Quality evaluation method and system for AMR robot cluster track |
-
2023
- 2023-06-29 CN CN202310789413.1A patent/CN116700340A/en active Pending
Cited By (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN117271099A (en) * | 2023-11-21 | 2023-12-22 | 山东师范大学 | Automatic space data analysis scheduling system and method based on rule base |
CN117271099B (en) * | 2023-11-21 | 2024-01-26 | 山东师范大学 | Automatic space data analysis scheduling system and method based on rule base |
CN118238182A (en) * | 2024-05-27 | 2024-06-25 | 青岛庆泰智能科技有限公司 | Quality evaluation method and system for AMR robot cluster track |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN108803349B (en) | Optimal consistency control method and system for nonlinear multi-agent system | |
Kuriki et al. | Formation control with collision avoidance for a multi-UAV system using decentralized MPC and consensus-based control | |
Duan et al. | Imperialist competitive algorithm optimized artificial neural networks for UCAV global path planning | |
Liu et al. | Multi-UAV path planning based on fusion of sparrow search algorithm and improved bioinspired neural network | |
Wu et al. | Multi UAV cluster control method based on virtual core in improved artificial potential field | |
Zhang et al. | Receding horizon control for multi-UAVs close formation control based on differential evolution | |
CN112859913B (en) | Multi-quad-rotor unmanned helicopter attitude consistency optimal control method considering output constraint | |
CN114237041B (en) | Space-ground cooperative fixed time fault tolerance control method based on preset performance | |
Zhang et al. | Dynamic mission planning algorithm for UAV formation in battlefield environment | |
CN116700340A (en) | Track planning method and device and unmanned aerial vehicle cluster | |
Zhou et al. | A novel mean-field-game-type optimal control for very large-scale multiagent systems | |
Xu et al. | Event‐triggered adaptive fuzzy bipartite consensus control of multiple autonomous underwater vehicles | |
Dou et al. | Distributed finite‐time formation control for multiple quadrotors via local communications | |
CN117055605A (en) | Multi-unmanned aerial vehicle attitude control method and system | |
Wang et al. | Multiple quadrotors formation flying control design and experimental verification | |
Li et al. | Large-scale fixed-wing UAV swarm system control with collision avoidance and formation maneuver | |
CN114063652A (en) | Stress matrix optimization-based affine formation control method for multi-agent system | |
Wang et al. | A decentralized decision-making algorithm of UAV swarm with information fusion strategy | |
CN116362109B (en) | Intelligent unmanned system and method based on digital twinning | |
CN115981375B (en) | Design method of multi-unmanned aerial vehicle time-varying formation controller based on event triggering mechanism | |
Ali et al. | Formation control of multiple UAV’s via decentralized control approach | |
Han et al. | Hybrid Algorithm-Based Full Coverage Search Approach With Multiple AUVs to Unknown Environments in Internet of Underwater Things | |
Nizar et al. | Safe and optimal navigation for autonomous multi-rotor aerial vehicle in a dynamic known environment by a decomposition-coordination method | |
CN114610065A (en) | Graph theory-based cluster unmanned aerial vehicle formation flight trajectory optimization method | |
CN113050697A (en) | Unmanned aerial vehicle cluster consistency cooperative control method based on time Petri network |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination |