CN114265425A - Multi-rotor unmanned aerial vehicle formation anti-collision control method - Google Patents
Multi-rotor unmanned aerial vehicle formation anti-collision control method Download PDFInfo
- Publication number
- CN114265425A CN114265425A CN202111441054.8A CN202111441054A CN114265425A CN 114265425 A CN114265425 A CN 114265425A CN 202111441054 A CN202111441054 A CN 202111441054A CN 114265425 A CN114265425 A CN 114265425A
- Authority
- CN
- China
- Prior art keywords
- unmanned aerial
- aerial vehicle
- collision
- control
- optimization
- 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 63
- 230000015572 biosynthetic process Effects 0.000 title claims abstract description 37
- 238000005457 optimization Methods 0.000 claims abstract description 57
- 230000009194 climbing Effects 0.000 claims description 17
- 230000008569 process Effects 0.000 claims description 14
- 238000005070 sampling Methods 0.000 claims description 12
- 239000002245 particle Substances 0.000 claims description 4
- 230000004044 response Effects 0.000 claims description 3
- 230000001174 ascending effect Effects 0.000 claims description 2
- 230000005484 gravity Effects 0.000 claims description 2
- 238000004422 calculation algorithm Methods 0.000 abstract description 16
- 238000005096 rolling process Methods 0.000 abstract description 16
- 238000001514 detection method Methods 0.000 abstract description 8
- 230000008447 perception Effects 0.000 abstract description 3
- 238000011160 research Methods 0.000 abstract description 2
- 238000012937 correction Methods 0.000 description 7
- 238000004088 simulation Methods 0.000 description 7
- 238000011217 control strategy Methods 0.000 description 5
- 238000010586 diagram Methods 0.000 description 4
- 230000000630 rising effect Effects 0.000 description 3
- 238000010845 search algorithm Methods 0.000 description 3
- RZVHIXYEVGDQDX-UHFFFAOYSA-N 9,10-anthraquinone Chemical group C1=CC=C2C(=O)C3=CC=CC=C3C(=O)C2=C1 RZVHIXYEVGDQDX-UHFFFAOYSA-N 0.000 description 2
- 230000001133 acceleration Effects 0.000 description 2
- 230000004888 barrier function Effects 0.000 description 2
- 230000009977 dual effect Effects 0.000 description 2
- 230000006870 function Effects 0.000 description 2
- 238000000926 separation method Methods 0.000 description 2
- 230000002123 temporal effect Effects 0.000 description 2
- 238000012360 testing method Methods 0.000 description 2
- 230000002411 adverse Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 238000012821 model calculation Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000002265 prevention Effects 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
Images
Landscapes
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The invention discloses a multi-rotor unmanned aerial vehicle formation anti-collision control method, and belongs to the technical field of unmanned aerial vehicles. The method comprises the steps of multi-sensor airspace situation perception, unmanned aerial vehicle kinematics modeling, nonlinear model control prediction, rolling optimization, collision detection, collision avoidance and the like. The invention provides a novel nonlinear model prediction control method, which can realize unmanned aerial vehicle flight path planning and collision avoidance in a dynamic environment; the proposed roll optimization guarantees real-time performance of algorithm execution. The invention can solve the problem of collision protection among multiple unmanned aerial vehicles, effectively improves the working safety of a multi-unmanned aerial vehicle system, and has greater theoretical research value and engineering practice significance.
Description
Technical Field
The invention belongs to the technical field of unmanned aerial vehicles, and particularly relates to a multi-rotor unmanned aerial vehicle formation anti-collision control method.
Background
In recent years, multi-rotor unmanned aerial vehicles are widely applied to the fields of military affairs, rescue, exploration, aerial photography and the like, the cooperative work of the multi-rotor unmanned aerial vehicles has higher working efficiency, and the dynamic anti-collision during the cooperative work of the multi-rotor unmanned aerial vehicles in formation also becomes a problem to be solved urgently.
The traditional unmanned aerial vehicle anti-collision algorithm is only suitable for static environments, and the situation of dynamic obstacles is not considered, however, in order to ensure the safety of the collaborative work of the formation of the unmanned aerial vehicles, the dynamic anti-collision of the formation of the unmanned aerial vehicles during flying must be considered, and therefore the invention provides a dynamic anti-collision method for the formation of the multi-rotor unmanned aerial vehicles.
The unmanned aerial vehicle anti-collision method can be divided into cooperative anti-collision and non-cooperative anti-collision, wherein the cooperative anti-collision needs to share speed, position and route information in the anti-collision process, and formation anti-collision is realized through cooperative control. The non-cooperative collision avoidance situation is more complex, information such as speed, position, altitude and the like of other aircrafts needs to be acquired by means of sensors, unmanned aerial vehicle performance constraints and uncertainty of invading aircrafts need to be considered, and a dynamic planning method needs to be adopted to solve the real-time collision avoidance problem.
The unmanned aerial vehicle dynamic anti-collision method mainly comprises a geometric method, an artificial potential field method, a collision cone method, a random theory and the like. However, these methods do not effectively take into account constraints such as drone speed constraints, turn rate constraints, control input constraints, etc. Optimization-based methods can handle constraints and have proven to be one of the more successful methods to solve the formation control problem, and optimization-based methods commonly used in the field of unmanned aerial vehicle collision avoidance are Nonlinear Model Predictive Control (NMPC) methods.
NMPC is a feedback control method that solves for an optimized trajectory at each sampling instant, selects the first control input of the optimal sequence, and repeats the optimization in each subsequent step. The method can effectively overcome adverse factors such as model precision errors and the like, determines the current optimal control value through online feedback correction, and simultaneously ensures that the controlled object has good stability. However, the NMPC method has the problem of high calculation cost, and the execution efficiency of the algorithm cannot meet the requirement of rapid real-time collision prevention under the condition of formation of multiple unmanned aerial vehicles.
In short, the existing NMPC method has a great room for improvement in real-time performance.
Disclosure of Invention
In order to solve the problems, the invention provides a multi-rotor unmanned aerial vehicle formation anti-collision control method, which adopts an NMPC method of rolling online optimization, designs an obstacle avoidance control strategy based on rules, and can effectively solve the multi-rotor unmanned aerial vehicle formation anti-collision problem.
In order to achieve the purpose, the invention adopts the technical scheme that:
the utility model provides a many rotor unmanned aerial vehicle formation anticollision control method, is applied to each unmanned aerial vehicle in many rotor unmanned aerial vehicle formation, includes the following step:
(1) selecting a system prediction time domain N and a sampling period delta t, setting weighting parameters Q, R and lambda, and setting input constraints according to the performance of the unmanned aerial vehicle;
(2) establishing an unmanned aerial vehicle kinematics model according to the flight information and the flight target of the unmanned aerial vehicle;
(3) establishing an unmanned aerial vehicle flight protection area, and acquiring an intruding aircraft set;
(4) establishing an NMPC nonlinear model predictive control equation, and solving a control sequence for collision avoidance conflict resolution of the unmanned aerial vehicle according to the unmanned aerial vehicle collision avoidance control nonlinear optimal control model;
(5) and (4) carrying out collision avoidance on the collision resolution control sequence solved in the step (4).
Further, the specific mode of the step (2) is as follows:
(201) establishing a navigation coordinate system of the machine; the navigation coordinate system adopts a northeast coordinate system, the carrier coordinate system point is positioned at the center of gravity of the machine body, the X axis points to the right side of the machine body, the Y axis points to the advancing direction of the machine head, and the Z axis points to the upper part of the machine body; the particle description equation of the unmanned plane in the three-dimensional space is as follows:
wherein v is the speed of the unmanned aerial vehicle, psi is the course angle of the unmanned aerial vehicle, and gamma is the track inclination angle of the unmanned aerial vehicle;
modeling the unmanned aerial vehicle collision avoidance process by adopting a first-order linear system:
wherein, tauv、τψAnd τγIs the time constant of the first order response, [ v ]cψcγc]TRepresenting a desired speed, a desired heading angle, and a desired track inclination of the own ship;
combined vertical type (1), formula (2), separating control vector [ vcψcγc]TAnd obtaining a second derivative model of the unmanned aerial vehicle collision avoidance process:
further, the specific mode of the step (3) is as follows:
defining an unmanned aerial vehicle safety protection area, wherein the unmanned aerial vehicle safety protection area has a radius of dmMinimum safety interval ball P:
P＝{x|||x-ra||＜dm} (9)
wherein x ∈ R2And | | · | | represents the euclidean distance, raIs the current location of the conflict target; definition of dsMinimum vertical distance between the intruding vehicle and the unmanned aerial vehicle track when ds≤dmAnd (4) considering collision conflict, and establishing a conflict aircraft set.
Further, the specific mode of the step (4) is as follows:
discretizing the unmanned plane kinematic equation shown in the formula (1) and expressing the unmanned plane kinematic equation as an NMPC system equation:
where x (k) is a state of the drone at time k, Δ t is a sampling period, and the control amount is u (k) ═ ψc(k)γc(k)]T；
The collision avoidance control system needs to enable the drone to achieve collision resolution at minimum cost while flying towards the target point. Order to
Defining a cost function:
wherein phi isTR phi is the energy cost of the collision avoidance process, R is the energy cost weighting coefficient, phi (t)f) Is tfConstantly unmanned aerial vehicle and target point's distance specifically does:
Φ(tf)＝[Xu(tf)-Xc]TQ[Xu(tf)-Xc]
wherein, tfThe position vector of the unmanned aerial vehicle at the moment is Xu(tf)＝[xu(tf)yu(tf)zu(tf)]TThe target point position vector is Xc(tf)＝[xc(tf)yc(tf)zc(tf)]TQ is a distance cost coefficient;
g (t) is the collision cost, defined as:
wherein λ is the weighting coefficient of the cost function, B (t) is the set of intruding drones, ds(xu(t),xj(t))]Minimum vertical distance, omega, for unmanned aerial vehicle and intruding unmanned aerial vehiclejRepresenting the degree of emergency of collision of the unmanned aerial vehicle with different invasive aircrafts;
establishing an unmanned aerial vehicle anti-collision nonlinear discrete model predictive control optimization model:
wherein, N represents a prediction time domain,
is a system in
A predicted state under control output; solving the formula (7) to obtain a control sequence in a prediction time domain N;
the control quantity output at the time k is as follows:
according to the definition of the optimization model in the formula (7), the optimization target value is J when the k moment is recorded and the prediction time domain is Nk(N) J is an optimization target for predicting that the time domain is { τ ═ 1,2, …, N-1} under the same control sequencek(τ), the drone is also flying towards the target point during collision avoidance, which can be:
Jk(τ)＜Jk(N)+Φ(x(k|k)) (10)
where, the cost of Φ (x (k | k)) at x (k | k) is expressed as:
Φ(x(k|k))＝[x(k|k)-xc]TQ[x(k|k)-xc] (11)
according to equation (8), the candidate control sequence at time k +1 is defined as:
the first N-1 elements of the candidate control sequence are the last N-1 elements of the optimal solution at the moment k, and the first N-1 elements and the last N-1 elements meet the control constraint; calculating candidate state sequence of unmanned aerial vehicle according to state equation of unmanned aerial vehicle
Judging collision conflict by combining the state of the invading unmanned aerial vehicle and the candidate state sequence of the unmanned aerial vehicle, establishing a set of conflict unmanned aerial vehicles, and calculating a candidate optimization target value according to the formula (7)
And cost Φ (x (k | k)), traverse the drone maneuver strategy tree to candidate sequences
As the current optimal control sequence U*(k) The first term u*And (k | k) is used as unmanned plane control input, unmanned plane state feedback x (k +1| k +1) is obtained as an initial state of the next moment, and finally k is made to be k +1, and the process is rolled to the step (3).
Further, the specific mode of the step (5) is as follows:
(501) calculating a candidate optimization target value according to equation (7)
And a cost Φ (x (k | k));
(502) constructing an optional maneuvering strategy tree of the unmanned aerial vehicle, wherein the maneuvering strategy tree comprises five maneuvering strategies: the method comprises the following steps of ascending the maximum climbing rate, descending the maximum climbing rate, turning left the maximum turning rate, turning right the maximum turning rate, and keeping the turning rate and the climbing rate at zero, wherein the current flight state is kept when the turning rate and the climbing rate are zero;
(503) traversing the maneuver strategy tree according to the depth-first mode, and calculating Jk(τ), τ ═ 1,2, …, N, if Jk(τ)≥Jk(N) + Φ (x (k | k)), terminating the current branch search, otherwise continuing the search; after the traversal is completed, the candidate sequence
As the current optimal control sequence U*(k) Output the first term u*(k | k) is used as the drone control input, and drone state feedback x (k +1| k +1) is obtained as the initial state at the next time.
The invention has the beneficial effects that:
1. the invention provides a dynamic anti-collision method for multi-rotor unmanned aerial vehicle formation through rolling optimization nonlinear model predictive control, which has the characteristics of high running speed and strong robustness and provides guidance for real-time dynamic anti-collision of multi-rotor unmanned aerial vehicle formation.
2. The method realizes optimization of model calculation parameters, improves timeliness of an NMPC algorithm, converts the formation anti-collision problem of multiple unmanned aerial vehicles into a rolling optimization problem by combining a self-learning real-time algorithm and a multi-step optimization search algorithm of model prediction control, and designs an anti-collision control strategy based on rules aiming at the consistency problem of unmanned aerial vehicle anti-collision maneuver. Test results show that the method can effectively solve the problems of unknown threat avoidance and flight path re-planning of the unmanned aerial vehicle.
Drawings
Fig. 1 is a flow chart of a multi-rotor unmanned aerial vehicle formation collision avoidance control method;
FIG. 2 is a multi-sensor spatial situation awareness diagram;
FIG. 3 is a schematic diagram of kinematics modeling of an unmanned aerial vehicle;
FIG. 4 is a NMPC nonlinear model predictive control schematic;
FIG. 5 is a flow chart of a scroll optimization;
fig. 6 is a schematic view of a unmanned aerial vehicle safety interval;
FIG. 7 is an evasive maneuver search tree;
FIG. 8 is a diagram illustrating a simulation example of a dual unmanned aerial vehicle formation collision avoidance algorithm;
fig. 9 is a simulation example diagram of a formation collision avoidance algorithm of five unmanned aerial vehicles.
Detailed Description
For a better understanding of the present invention, the following examples are given to illustrate the present invention, but the present invention is not limited to the following examples.
A multi-rotor unmanned aerial vehicle formation anti-collision control method is combined with a self-learning real-time algorithm and a multi-step optimization search algorithm of model prediction control, and the multi-rotor unmanned aerial vehicle formation anti-collision problem is converted into a rolling optimization problem, so that the formation anti-collision problem is solved based on a nonlinear model prediction control method.
The method comprises the following steps:
(1) sensing the spatial situation of a plurality of sensors;
(2) modeling unmanned aerial vehicle kinematics;
(3) NMPC model predictive control;
(4) optimizing rolling;
(5) detecting a collision;
(6) and (4) avoiding collision.
The multi-sensor in the step (1) comprises a DSP (digital Signal processor) main control chip, an accelerometer, a gyroscope, a magnetometer, a Beidou Signal receiver, a frequency modulation continuous wave radar and an altimeter, and the multi-sensor is used for acquiring navigation attitude, altitude, position and other information of other aircrafts in a local machine and an airspace.
The airspace situation perception in the step (1) is to process information acquired by the multiple sensors to obtain the speed, the acceleration, the flight height and the radial distance of each aircraft in the airspace, further establish a three-dimensional space coordinate with the own aircraft as a center, and perceive and position the invading aircraft and the barrier.
The unmanned aerial vehicle kinematics modeling in the step (2) is that kinematics equations of a local machine and an invasive aircraft are established in a three-dimensional coordinate system, and the local machine and the invasive aircraft use particle description in a three-dimensional space; the three-dimensional coordinate system includes navigation coordinate system and carrier coordinate system, navigation coordinate system adopts northeast heaven (ENU) coordinate system, carrier coordinate system links firmly with the unmanned aerial vehicle organism, and the coordinate system accords with the right-hand rule, and the initial point is located the organism focus, and the directional organism right side of X axle, the directional aircraft nose advancing direction of Y axle, the directional organism top of Z axle.
The NMPC model predictive control in the step (3) is a multivariable control strategy aiming at a nonlinear system, and consists of a predictive model, rolling optimization and feedback correction. The NMPC method solves an open-loop optimization problem of a finite time domain at each sampling moment according to the information currently measured by the system to obtain a control sequence, acts the first element of the control sequence on the system, repeats the operation at the next sampling moment, and uses a new measured value as the initial condition of the NMPC system to solve a new optimization problem. The NMPC method can effectively improve model precision errors, and the optimal control value of the system is determined through feedback correction and finite time domain rolling optimization, so that the system is guaranteed to have good stability.
And (4) the prediction model in the step (3) is used for predicting the output of a period of time in the future by the system by using the current state of the controlled object and future control information.
The rolling optimization in the step (3) is essentially to solve the open loop optimization of the system, and the optimal control rate of the future finite time domain is solved through the optimization of a certain performance index.
The feedback correction in the step (3) is to perform correction optimization on the system based on the prediction output of the model, use the target performance function and the system constraint condition, take the first element of the output sequence as the input value of the control system at the next moment, correct the prediction model through error feedback, and improve the prediction precision.
The rolling optimization in the step (4) is the core of the NMPC method, and the idea is to obtain the optimal control sequence at each sampling moment by solving a finite time domain optimization problem, thereby realizing the online closed-loop control of the system in the whole time domain. In the rolling optimization process of the unmanned aerial vehicle anti-collision NMPC controller, state parameters of each unmanned aerial vehicle are monitored, the state of the system is predicted through a prediction model and the control quantity parameters, the optimization problem is solved according to preset optimization indexes, and the result is output to the unmanned aerial vehicle flight control unit. Predictive control employs a finite temporal optimization strategy that rolls forward in time, and therefore this step is referred to as roll optimization.
The collision detection in step (5) is based on the detection of a pre-defined unmanned aerial vehicle safety protection area, wherein the unmanned aerial vehicle safety protection area is defined as an unmanned aerial vehicle safety protection area with a radius dmDefined as the following set P:
P＝{x|||x-ra||＜dm} (1)
wherein x ∈ R2And | | · | | represents the euclidean distance, raIs the current position of the conflicting target, dmThe distance margin between the unmanned aerial vehicle and the conflict target is also the minimum safe separation radius of the aircraft. The relationship between the sensing detection radius of the unmanned aerial vehicle and the minimum safety interval sphere radius is assumed to be Rm＜＜dmWhen the invading aircraft enters the unmanned aerial vehicle detection range, the unmanned aerial vehicle calculates according to the sensed state information of the invading aircraft, judges whether collision and conflict are possible between the invading aircraft and the unmanned aerial vehicle, and when the invading aircraft enters the minimum safety interval ball, the invading aircraft is considered to collide.
And (4) the collision avoidance in the step (6) is implemented by carrying out vertical or horizontal maneuvering through the safe path predicted by the NMPC algorithm in the step (3). The unmanned aerial vehicle collision avoidance cuts a search space of an optimization algorithm by specifying the maneuvering behavior of the unmanned aerial vehicle on the premise of meeting the timeliness requirement, and the collision avoidance of the unmanned aerial vehicle is decoupled according to vertical maneuvering and horizontal maneuvering, namely, the unmanned aerial vehicle only uses single collision avoidance maneuvering at any moment, and the collision conflict is eliminated in the shortest possible time by utilizing the maximum maneuvering capability. The invention defines five maneuvering strategies comprising maximum climbing rate rising, maximum climbing rate descending, maximum turning rate left turning, maximum turning rate right turning, turning rate and climbing rate being zero.
The following is a more specific set of examples:
example 1
As shown in fig. 1, the method for preventing collision of formation of multi-rotor unmanned aerial vehicles comprises the following steps:
(1) sensing the spatial situation of a plurality of sensors;
(2) modeling unmanned aerial vehicle kinematics;
(3) NMPC model predictive control;
(4) optimizing rolling;
(5) detecting a collision;
(6) and (4) avoiding collision.
Example 2
As shown in fig. 2, the multiple sensors include a DSP main control chip, an accelerometer, a gyroscope, a magnetometer, a beidou signal receiver, a frequency modulated continuous wave radar, and an altimeter, and are used to obtain the attitude, altitude, position, and other information of the aircraft in the local aircraft and the airspace. The airspace situation perception means that information such as speed, acceleration, flying height, radial distance and the like of each aircraft in the airspace is obtained by processing information obtained by a plurality of sensors, and then a three-dimensional space coordinate with the aircraft as a center is established to perceive and position the invading aircraft and the barrier.
Example 3
As shown in fig. 3, the unmanned aerial vehicle kinematics modeling is to establish kinematics equations of the local vehicle and the intruding aircraft in a three-dimensional coordinate system, and the unmanned aerial vehicle can acquire the altitude, the position vector and the speed vector of the intruding aircraft through the airspace situation awareness as described in example 2. The navigation coordinate system adopts northeast heaven (ENU) coordinate system, and the carrier coordinate system links firmly with the unmanned aerial vehicle organism, accords with right-hand rule, and the initial point is located the organism focus, and the directional organism right side of X axle, the directional aircraft nose advancing direction of Y axle, the directional organism top of Z axle. The local machine and the invasive aircraft use particle description in three-dimensional space, namely:
where v is the speed of the drone, ψ is the heading angle of the drone, and γ is the track inclination of the drone, the collision avoidance process of the drone can be described by a first order linear system as follows.
Wherein, tauv、τψAnd τγIs a time constant of first order response and satisfies vmin≤v≤vmax，
γmin≤γ≤γmax，
[vcψcγc]TRepresenting the desired speed, heading angle and track inclination of the system. Simultaneous (2), (3) separate control vector [ vcψcγc]TAnd obtaining a second derivative model of the unmanned aerial vehicle motion equation:
example 4
As shown in fig. 4, the NMPC model predictive control is a multivariable control strategy for a nonlinear system, and is composed of three parts, namely a predictive model, rolling optimization and feedback correction. The NMPC method solves an open-loop optimization problem of a finite time domain at each sampling moment according to the information currently measured by the system to obtain a control sequence, acts the first element of the control sequence on the system, repeats the operation at the next sampling moment, and uses a new measured value as the initial condition of the NMPC system to solve a new optimization problem. The NMPC method can effectively improve model precision errors, and the optimal control value of the system is determined through feedback correction and finite time domain rolling optimization, so that the system is guaranteed to have good stability.
Discretizing the kinematic equation of the unmanned aerial vehicle in the formula (2) can obtain:
where x (k) is a state of the drone at time k, Δ t is a sampling period, and the control amount is u (k) ═ ψc(k)γc(k)]TThe mathematical optimization model of this equation can be written as the following NMPC optimization equation:
wherein, N represents a prediction time domain,
is a system in
And (3) solving the NMPC optimization problem shown in (8) by controlling the prediction state under the output condition to obtain a control sequence in the prediction time domain N:
example 5
As shown in fig. 5, rolling optimization is the core of the NMPC method, and an optimal control sequence at each sampling time is obtained by solving a finite time domain optimization equation, so as to realize closed-loop control of the system in the whole time domain. In the rolling optimization process of the unmanned aerial vehicle anti-collision NMPC controller, state parameters of each unmanned aerial vehicle are monitored, the state of the system is predicted through a prediction model and the control quantity parameters, the optimization problem is solved according to preset optimization indexes, and the result is output to the unmanned aerial vehicle flight control unit. This step is called roll optimization because predictive control employs a finite temporal optimization strategy that rolls forward in time.
Example 6
As shown in fig. 6, collision detection is performed based on a predefined drone safety zone defined as a minimum safety interval sphere with a radius defined as the set:
P＝{x|||x-ra||＜dm} (10)
wherein x ∈ R2And | | · | | represents the euclidean distance, raIs the current position of the conflicting target, dmThe distance margin between the unmanned aerial vehicle and the conflict target is also the minimum safe separation radius of the aircraft. The relationship between the sensing detection radius of the unmanned aerial vehicle and the minimum safety interval sphere radius is assumed to be Rm＜＜dmWhen the invading aircraft enters the unmanned aerial vehicle detection range, the unmanned aerial vehicle senses that the invading aircraft enters the minimum safety spacing ball, namely the radial distance is less than dmWhen so, the collision is considered to occur.
Example 7
As shown in table 1, the collision avoidance is performed by performing vertical or horizontal maneuver through the safe path predicted by the NMPC algorithm in step (3). The unmanned aerial vehicle collision avoidance cuts a search space of an optimization algorithm by specifying the maneuvering behavior of the unmanned aerial vehicle on the premise of meeting the timeliness requirement, and the collision avoidance of the unmanned aerial vehicle is decoupled according to vertical maneuvering and horizontal maneuvering, namely, the unmanned aerial vehicle only uses single collision avoidance maneuvering at any moment, and the collision conflict is eliminated in the shortest possible time by utilizing the maximum maneuvering capability. The invention defines five maneuvering strategies comprising maximum climbing rate rising, maximum climbing rate descending, maximum turning rate left turning, maximum turning rate right turning, turning rate and climbing rate zero, wherein the five maneuvering strategies form an unmanned aerial vehicle maneuvering strategy avoiding set E ═ U, D, L, R and S, and meet the requirements of the maximum climbing rate rising, the maximum climbing rate descending, the maximum turning rate left turning, the maximum turning rate right turning, the turning rate and the climbing rate zero
As shown in table 1.
TABLE 1 unmanned aerial vehicle maneuver evasion strategy
Example 8
As shown in FIG. 7, the time domain N is predicted, and the optional control sequences form a block with N layers, 5NThe optimization problem of the five-branch tree of each branch is solved by sequentially searching the branches of the maneuvering strategy tree, the obtained result forms a solution space of the formula (8), and when the prediction time domain is increased, the calculated amount is exponentially increased, so that the prediction process is optimized through a search algorithm.
Example 9
As shown in fig. 8, a collision avoidance algorithm simulation of the formation of the dual drones is performed to establish a 400 × 500 × 600m3The three-dimensional space, wherein the black spheroid represents target unmanned aerial vehicle, other spheroids represent other unmanned aerial vehicles of formation, set for the departure coordinate and the target coordinate of target aircraft earlier before the simulation, set for unmanned aerial vehicle safe flight distance and be 100m, the route that satisfies safe crashproof distance is obtained through iteration many times to the model predictive control algorithm, the number of iterations is more, the more accurate of route that obtains, but the calculating time is corresponding longer, in order to satisfy the requirement of unmanned aerial vehicle team real-time crashproof information output, according to the simulation test, when iteration number N sets up to 10, the model has better anticollision effect and can converge fast. The result shows that the method provided by the invention can effectively calculate the safe flight path meeting the anti-collision condition.
Example 10
As shown in fig. 9, in an actual situation, the formation of unmanned aerial vehicles usually has a plurality of aircrafts, the collision avoidance algorithm simulation of the formation of five unmanned aerial vehicles is performed, the algorithm selects the path with the largest mean square distance from the intruding aircrafts as the optimal safe flight path, and the simulation result shows that the collision avoidance method can effectively calculate a collision-free path and effectively avoid other unmanned aerial vehicles.
In a word, aiming at the consistency problem of unmanned aerial vehicle anti-collision maneuver, the invention designs a rule-based anti-collision control strategy, and utilizes the designated maneuver strategy to cut the search space, thereby improving the search efficiency of the method, effectively solving the anti-collision problem of multi-unmanned aerial vehicle formation, effectively improving the flight safety of the unmanned aerial vehicle formation system, and having greater theoretical research value and engineering practice significance.
The above description is only a specific embodiment of the present invention, and not all embodiments, and any equivalent modifications of the technical solutions of the present invention, which are made by those skilled in the art through reading the present specification, are covered by the claims of the present invention.
Claims (5)
1. The anti-collision control method for the formation of the multi-rotor unmanned aerial vehicles is characterized by being applied to each unmanned aerial vehicle in the formation of the multi-rotor unmanned aerial vehicles and comprising the following steps of:
(1) selecting a system prediction time domain N and a sampling period delta t, setting weighting parameters Q, R and lambda, and setting input constraints according to the performance of the unmanned aerial vehicle;
(2) establishing an unmanned aerial vehicle kinematics model according to the flight information and the flight target of the unmanned aerial vehicle;
(3) establishing an unmanned aerial vehicle flight protection area, and acquiring an intruding aircraft set;
(4) establishing an NMPC nonlinear model predictive control equation, and solving a control sequence for collision avoidance conflict resolution of the unmanned aerial vehicle according to the unmanned aerial vehicle collision avoidance control nonlinear optimal control model;
(5) and (4) carrying out collision avoidance on the collision resolution control sequence solved in the step (4).
2. The multi-rotor unmanned aerial vehicle formation anti-collision control method according to claim 1, wherein the step (2) is realized in a specific manner as follows:
(201) establishing a navigation coordinate system of the machine; the navigation coordinate system adopts a northeast coordinate system, the carrier coordinate system point is positioned at the center of gravity of the machine body, the X axis points to the right side of the machine body, the Y axis points to the advancing direction of the machine head, and the Z axis points to the upper part of the machine body; the particle description equation of the unmanned plane in the three-dimensional space is as follows:
wherein v is the speed of the unmanned aerial vehicle, psi is the course angle of the unmanned aerial vehicle, and gamma is the track inclination angle of the unmanned aerial vehicle;
modeling the unmanned aerial vehicle collision avoidance process by adopting a first-order linear system:
wherein, tauv、τψAnd τγIs the time constant of the first order response, [ v ]c ψc γc]TRepresenting a desired speed, a desired heading angle, and a desired track inclination of the own ship;
combined vertical type (1), formula (2), separating control vector [ vc ψc γc]TAnd obtaining a second derivative model of the unmanned aerial vehicle collision avoidance process:
3. the multi-rotor unmanned aerial vehicle formation anti-collision control method according to claim 2, wherein the specific manner of the step (3) is as follows:
defining an unmanned aerial vehicle safety protection area, wherein the unmanned aerial vehicle safety protection area has a radius of dmMinimum safety interval ball P:
P＝{x|||x-ra||＜dm} (9)
wherein x ∈ R2And | | · | | represents the euclidean distance, raIs the current location of the conflict target; definition of dsMinimum vertical distance between the intruding vehicle and the unmanned aerial vehicle track when ds≤dmAnd (4) considering collision conflict, and establishing a conflict aircraft set.
4. The multi-rotor unmanned aerial vehicle formation anti-collision control method according to claim 3, wherein the specific manner of the step (4) is as follows:
discretizing the unmanned plane kinematic equation shown in the formula (1) and expressing the unmanned plane kinematic equation as an NMPC system equation:
where x (k) is a state of the drone at time k, Δ t is a sampling period, and the control amount is u (k) ═ ψc(k) γc(k)]T；
The collision avoidance control system needs to enable the drone to achieve collision resolution at minimum cost while flying towards the target point. Order to
Defining a cost function:
wherein phi isTR phi is the energy cost of the collision avoidance process, R is the energy cost weighting coefficient, phi (t)f) Is tfConstantly unmanned aerial vehicle and target point's distance specifically does:
Φ(tf)＝[Xu(tf)-Xc]TQ[Xu(tf)-Xc]
wherein, tfThe position vector of the unmanned aerial vehicle at the moment is Xu(tf)＝[xu(tf) yu(tf) zu(tf)]TThe target point position vector is Xc(tf)＝[xc(tf) yc(tf) zc(tf)]TQ is a distance cost coefficient;
g (t) is the collision cost, defined as:
wherein λ is the weighting coefficient of the cost function, B (t) is the set of intruding drones, ds(xu(t),xj(t))]Minimum vertical distance, omega, for unmanned aerial vehicle and intruding unmanned aerial vehiclejRepresenting the degree of emergency of collision of the unmanned aerial vehicle with different invasive aircrafts;
establishing an unmanned aerial vehicle anti-collision nonlinear discrete model predictive control optimization model:
wherein, N represents a prediction time domain,
is a system in
A predicted state under control output; solving the formula (7) to obtain a control sequence in a prediction time domain N;
the control quantity output at the time k is as follows:
according to the definition of the optimization model in the formula (7), the optimization target value is J when the k moment is recorded and the prediction time domain is Nk(N) J is an optimization target for predicting that the time domain is { τ ═ 1,2, …, N-1} under the same control sequencek(τ), the drone is also flying towards the target point during collision avoidance, which can be:
Jk(τ)＜Jk(N)+Φ(x(k|k)) (10)
where, the cost of Φ (x (k | k)) at x (k | k) is expressed as:
Φ(x(k|k))＝[x(k|k)-xc]TQ[x(k|k)-xc] (11)
according to equation (8), the candidate control sequence at time k +1 is defined as:
the first N-1 elements of the candidate control sequence are the last N-1 elements of the optimal solution at the moment k, and the first N-1 elements and the last N-1 elements meet the control constraint; calculating candidate state sequence of unmanned aerial vehicle according to state equation of unmanned aerial vehicle
Judging collision conflict by combining the state of the invading unmanned aerial vehicle and the candidate state sequence of the unmanned aerial vehicle, establishing a set of conflict unmanned aerial vehicles, and calculating a candidate optimization target value according to the formula (7)
And cost Φ (x (k | k)), traverse the drone maneuver strategy tree to candidate sequences
As the current optimal control sequence U*(k) The first term u*And (k | k) is used as unmanned plane control input, unmanned plane state feedback x (k +1| k +1) is obtained as an initial state of the next moment, and finally k is made to be k +1, and the process is rolled to the step (3).
5. The multi-rotor unmanned aerial vehicle formation anti-collision control method according to claim 4, wherein the step (5) is realized in a specific manner as follows:
(501) calculating a candidate optimization target value according to equation (7)
And a cost Φ (x (k | k));
(502) constructing an optional maneuvering strategy tree of the unmanned aerial vehicle, wherein the maneuvering strategy tree comprises five maneuvering strategies: the method comprises the following steps of ascending the maximum climbing rate, descending the maximum climbing rate, turning left the maximum turning rate, turning right the maximum turning rate, and keeping the turning rate and the climbing rate at zero, wherein the current flight state is kept when the turning rate and the climbing rate are zero;
(503) traversing the maneuver strategy tree according to the depth-first mode, and calculating Jk(τ), τ ═ 1,2, …, N, if Jk(τ)≥Jk(N) + Φ (x (k | k)), terminating the current branch search, otherwise continuing the search; after the traversal is completed, the candidate sequence
As the current optimal control sequence U*(k) Output the first term u*(k | k) is used as the drone control input, and drone state feedback x (k +1| k +1) is obtained as the initial state at the next time.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111441054.8A CN114265425A (en) | 2021-11-30 | 2021-11-30 | Multi-rotor unmanned aerial vehicle formation anti-collision control method |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111441054.8A CN114265425A (en) | 2021-11-30 | 2021-11-30 | Multi-rotor unmanned aerial vehicle formation anti-collision control method |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN114265425A true CN114265425A (en) | 2022-04-01 |
Family
ID=80825888
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202111441054.8A Pending CN114265425A (en) | 2021-11-30 | 2021-11-30 | Multi-rotor unmanned aerial vehicle formation anti-collision control method |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114265425A (en) |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115394125A (en) * | 2022-08-24 | 2022-11-25 | 陕西凌云电器集团有限公司 | Air collision avoidance method of aircraft based on ADS-B |
| CN117647997A (en) * | 2024-01-29 | 2024-03-05 | 中国人民解放军战略支援部队航天工程大学 | Knowledge bidirectional migration unmanned aerial vehicle collaborative track local re-planning method and system |
Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| GB0812496D0 (en) * | 2007-07-09 | 2008-08-13 | Eads Deutschland Gmbh | Collision and conflict avoidance system for autonomous unmanned air vechicles(uavs) |
| CN112148024A (en) * | 2020-08-20 | 2020-12-29 | 中国人民解放军海军航空大学 | Unmanned aerial vehicle real-time online flight path planning method based on self-adaptive pseudo-spectral method |
-
2021
- 2021-11-30 CN CN202111441054.8A patent/CN114265425A/en active Pending
Patent Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| GB0812496D0 (en) * | 2007-07-09 | 2008-08-13 | Eads Deutschland Gmbh | Collision and conflict avoidance system for autonomous unmanned air vechicles(uavs) |
| CN112148024A (en) * | 2020-08-20 | 2020-12-29 | 中国人民解放军海军航空大学 | Unmanned aerial vehicle real-time online flight path planning method based on self-adaptive pseudo-spectral method |
Non-Patent Citations (3)
| Title |
|---|
| 代进进 等: "基于模型预测控制的无人机避障路径规划方法", 火力与指挥控制, vol. 45, no. 01, pages 114 - 119 * |
| 宋敏 等: "基于NMPC的无人机自主防撞控制方法", 系统工程与电子技术, vol. 41, no. 09, pages 2092 - 2099 * |
| 李季 等: "基于多步寻优搜索的无人机航迹重规划算法", 系统工程与电子技术, vol. 31, no. 10, pages 2510 - 2512 * |
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115394125A (en) * | 2022-08-24 | 2022-11-25 | 陕西凌云电器集团有限公司 | Air collision avoidance method of aircraft based on ADS-B |
| CN117647997A (en) * | 2024-01-29 | 2024-03-05 | 中国人民解放军战略支援部队航天工程大学 | Knowledge bidirectional migration unmanned aerial vehicle collaborative track local re-planning method and system |
| CN117647997B (en) * | 2024-01-29 | 2024-04-16 | 中国人民解放军战略支援部队航天工程大学 | Knowledge bidirectional migration unmanned aerial vehicle collaborative track local re-planning method and system |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109871032B (en) | Multi-unmanned aerial vehicle formation cooperative control method based on model predictive control | |
| Wu | Coordinated path planning for an unmanned aerial-aquatic vehicle (UAAV) and an autonomous underwater vehicle (AUV) in an underwater target strike mission | |
| Cui et al. | Autonomous navigation of UAV in foliage environment | |
| CN102915652B (en) | aircraft traffic separation system and method | |
| CN105759829A (en) | Laser radar-based mini-sized unmanned plane control method and system | |
| CN114265425A (en) | Multi-rotor unmanned aerial vehicle formation anti-collision control method | |
| CN110703804A (en) | Layering anti-collision control method for fixed-wing unmanned aerial vehicle cluster | |
| CN110262555B (en) | Real-time obstacle avoidance control method for unmanned aerial vehicle in continuous obstacle environment | |
| CN111881518A (en) | Intelligent reentry maneuver guidance method and system for hypersonic aircraft | |
| CN112198901B (en) | Unmanned aerial vehicle autonomous collision avoidance decision method based on three-dimensional dynamic collision area | |
| Spurny et al. | Cooperative transport of large objects by a pair of unmanned aerial systems using sampling-based motion planning | |
| Zhao et al. | Curved path following control for fixed-wing unmanned aerial vehicles with control constraint | |
| CN116627154B (en) | Unmanned aerial vehicle guiding landing method based on pose prediction and track optimization and unmanned aerial vehicle | |
| Fu et al. | An advanced sense and collision avoidance strategy for unmanned aerial vehicles in landing phase | |
| CN113848974A (en) | Aircraft trajectory planning method and system based on deep reinforcement learning | |
| CN110297500A (en) | A kind of continuous path planning method giving unmanned plane under more way points | |
| Vijayakumari et al. | Receding-horizon trajectory planning for multiple uavs using particle swarm optimization | |
| CN114047744A (en) | Self-adaptive backstepping sliding mode multi-unmanned ship formation control method based on sampling communication | |
| Duoxiu et al. | Proximal policy optimization for multi-rotor uav autonomous guidance, tracking and obstacle avoidance | |
| CN108459614B (en) | UUV real-time collision avoidance planning method based on CW-RNN network | |
| Liu et al. | An Improved Four‐Rotor UAV Autonomous Navigation Multisensor Fusion Depth Learning | |
| CN115903882A (en) | Coordinated target tracking control method for cluster unmanned aerial vehicle under field angle constraint | |
| CN114740873A (en) | Path planning method of autonomous underwater robot based on multi-target improved particle swarm algorithm | |
| Bertoncini et al. | Fixed-Wing UAV Path Planning and Collision Avoidance using Nonlinear Model Predictive Control and Sensor-based Cloud Detection | |
| Sahawneh et al. | Path planning in the local-level frame for small unmanned aircraft systems |
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 |