CN113885562A - Multi-unmanned aerial vehicle cooperative collision avoidance method under perception constraint based on speed obstacle - Google Patents

Multi-unmanned aerial vehicle cooperative collision avoidance method under perception constraint based on speed obstacle Download PDF

Info

Publication number
CN113885562A
CN113885562A CN202111172463.2A CN202111172463A CN113885562A CN 113885562 A CN113885562 A CN 113885562A CN 202111172463 A CN202111172463 A CN 202111172463A CN 113885562 A CN113885562 A CN 113885562A
Authority
CN
China
Prior art keywords
unmanned aerial
aerial vehicle
speed
collision avoidance
observation
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.)
Granted
Application number
CN202111172463.2A
Other languages
Chinese (zh)
Other versions
CN113885562B (en
Inventor
杨庆凯
李若成
赵维鹏
赵欣悦
方浩
陈杰
辛斌
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Beijing Institute of Technology BIT
Original Assignee
Beijing Institute of Technology BIT
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Beijing Institute of Technology BIT filed Critical Beijing Institute of Technology BIT
Priority to CN202111172463.2A priority Critical patent/CN113885562B/en
Publication of CN113885562A publication Critical patent/CN113885562A/en
Application granted granted Critical
Publication of CN113885562B publication Critical patent/CN113885562B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/10—Simultaneous control of position or course in three dimensions
    • G05D1/101—Simultaneous control of position or course in three dimensions specially adapted for aircraft

Landscapes

  • Engineering & Computer Science (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

The invention provides a multi-unmanned aerial vehicle cooperative collision avoidance method under perception constraint based on speed obstacle, wherein a cooperative collision avoidance algorithm is designed in a distributed multi-unmanned aerial vehicle system consisting of a plurality of rotor unmanned aerial vehicles, so that the unmanned aerial vehicles realize interactive collision avoidance based on observed state information (position and speed), and the collision avoidance is realized only based on the observed information, so that the multi-unmanned aerial vehicle system is more flexible, the collision avoidance algorithm established based on the observed information can improve the safety and robustness of the system, the dependence of the system on communication is reduced, and the multi-unmanned aerial vehicle system has better adaptability in a complex environment.

Description

Multi-unmanned aerial vehicle cooperative collision avoidance method under perception constraint based on speed obstacle
Technical Field
The invention relates to a multi-unmanned aerial vehicle cooperative collision avoidance method under perception constraint based on speed obstacle, and belongs to the technical field of unmanned aerial vehicle planning.
Background
Because unmanned aerial vehicle has high mobility, low running power consumption and easily expand characteristics such as development, to unmanned aerial vehicle system, especially many unmanned aerial vehicle system's research has received the extensive attention of academic and industrial world in recent years. The multi-unmanned aerial vehicle system has a large amount of practical applications in the aspects of search and rescue, collaborative exploration, entertainment performance and the like. In executing these complex tasks, the cooperative collision avoidance technique of multiple drones is a basic task, which directly determines the following performance of the multiple drone system:
safety: the capability of preventing unmanned aerial vehicles from colliding with each other when a multi-unmanned aerial vehicle system executes tasks, such as formation transformation, collaborative exploration and the like;
robustness: the multi-unmanned aerial vehicle system has the capability of avoiding collision of the whole system when facing external disturbance such as wind disturbance and communication disturbance.
For the cooperative collision avoidance problem, there are several main solutions:
scheme 1: the literature (Y.Xu, S.Lai, J.Li, D.Luo, and Y.you, "Current optimal trajectory planning for inductor resolver switching," Journal of Intelligent & robot Systems, vol.94, No.2, pp.503-520,2019 ") and the literature (S.H.Arul and D.Manocha," ad: Decentralized coordinated visibility with dynamic trajectories comparators for unmanned plane avoidance, "IEEE robot and Automation Letters, vol.5, No.2, pp.1191-1198,2020") achieve multiple unmanned plane avoidance based on two improved velocity barrier methods, respectively. However, these methods do not take into account the perceptual constraints, and both methods are not applicable in perceptually limited environments. The literature (P.Conroy, D.Bareiss, M.Beall, and J.v.d.berg, "3-d-directional coherence attitude on physical liquid dynamics with on-board sensing for relative positioning," arXiv prediction arXiv:1411.3794,2014.) realizes the collision avoidance of multiple unmanned planes considering the perception constraint, but the proposed method is only applicable to the case of fewer unmanned planes and ignores the one-way observation problem caused by the field constraint.
Scheme 2: collision avoidance methods based on velocity barriers and artificial potential fields are proposed in the literature (j.alonso-Mora, t.naegeli, r.siegwart, and p.beardsley, "precision available for aviation vehicles in multi-agent vehicles," Autonomous Robots, vol.39, No.1, pp.101-121,2015.), but such methods require the reliance on global positioning systems and, in distributed unmanned aerial vehicle systems, do not guarantee the stability of the system.
Scheme 3: documents (s.roelofsen, a.martinili, and d.gillet, "3 d compliance amplitude algorithm for unmanned aerial vehicle aspects with limited field of view constraints," in 2016 IEEE 55th Conference on Decision and Control (CDC) IEEE, 2016, pp.2555-2560.) and documents (s.roelofsen, d.gillet, and a.martinili, "condensation amplitude with limited field of view sensing: a. velocity object approach," in 2017 IEEE International Conference on Robotics and Automation (ICRA), IEEE 2017, pp.1922-1927.) realize a limited approach based on one speed selection method, but with a large limited flexibility of unmanned aerial vehicle design, and a large degree of unmanned aerial vehicle scalability system design.
Disclosure of Invention
The invention provides a distributed multi-unmanned aerial vehicle system cooperative collision avoidance method based on speed barriers.
A distributed multi-unmanned aerial vehicle system cooperative collision avoidance method based on speed barriers comprises the following steps: .
Step 1, generating an initial motion track for each unmanned aerial vehicle in an unmanned aerial vehicle system;
step 2, real-time cooperative collision avoidance based on observation information, specifically comprising:
step 21, assuming that an unmanned plane i and an unmanned plane j exist, the radius of the plane is r, and the size of the view angle is FOV; unmanned plane i self flying speed viAnd observe the position information p of the unmanned plane j in the flight processjVelocity information vjAnd handpiece orientation information psij(ii) a Based on the observation information, the collaborative mode function is defined as:
Figure BDA0003293890380000021
when g (p, psi) is more than or equal to 0, the two unmanned aerial vehicles are in a mutual observation mode; when g (p, psi) < 0, the unmanned aerial vehicle i carries out unilateral observation on the unmanned aerial vehicle j, namely, in a unidirectional observation mode;
step 22, according to the observation information, the view vector field function is defined as:
Figure BDA0003293890380000022
wherein,
Figure BDA0003293890380000031
is a unit vector of the unmanned plane i in the x-axis direction in the fuselage coordinate system,
Figure BDA0003293890380000032
is the observed velocity vjThe transpose of (a) is performed,
Figure BDA0003293890380000033
is the location information p of the drone jjComponent on the x-axis of the unmanned plane i-body coordinate system, h (p)j)，c(pj) Is a scalar function that is always greater than 0; when the function value returned by the VVF is larger than 0, the unmanned aerial vehicle i and the unmanned aerial vehicle j have potential collision risks; when the function value is less than or equal to 0, the two unmanned aerial vehicles have no collision risk;
step 23, after judging that there is a collision risk, realizing cooperative collision avoidance by the following steps:
231, generating a relative velocity obstacle set based on the observation information, and when the observation mode is bidirectional observation, generating the relative velocity obstacle set
Figure BDA0003293890380000034
The following expression is calculated:
Figure BDA0003293890380000035
Figure BDA0003293890380000036
wherein u is a control quantity output to the unmanned aerial vehicle, namely a speed change quantity; argmin represents the minimum of the function taken,
Figure BDA0003293890380000037
is the boundary of the infeasible speed set, and represents the modular operation, | | | - |; set of relative velocity obstacles
Figure BDA0003293890380000038
A collision-free speed set constructed based on the control quantity u, wherein the geometry of the collision-free speed set is a semi-plane; n is a normal vector perpendicular to the half-plane; τ is the execution time of the planner;
when the observation mode is one-way observation, if VVF returns a value of c (p)j) Set of relative velocity disorders
Figure BDA0003293890380000039
The following expression is calculated:
vi＝vi-λ1vi-λ2vj
Figure BDA00032938903800000310
Figure BDA00032938903800000311
if VVF returns a value of
Figure BDA00032938903800000312
Set of relative velocity obstacles
Figure BDA00032938903800000313
The following expression is calculated:
u＝-λ3vi
Figure BDA00032938903800000314
wherein λ1，λ2，λ3Is a positive real number;
step 232, generate a safe speed set
Figure BDA00032938903800000315
Wherein v ismaxFor the maximum allowable speed, n represents taking the intersection;
D(0，vmax)＝{p|||p-0||＜vmax}；
p represents all velocity vectors that meet the requirements;
step 233, selecting an optimal speed v within the generated safe speed setoptDefined as:
Figure BDA0003293890380000041
step 234, generating two continuous smooth track sequences according to the selected optimal speed
Figure BDA0003293890380000042
Is defined as:
Figure BDA0003293890380000043
Figure BDA0003293890380000044
wherein delta1，δ2Is to satisfy delta1+δ2A constant of < 1;
Figure BDA0003293890380000045
the smooth trajectory sequence of (2) is calculated as follows:
Figure BDA0003293890380000046
s.t.ζ(0)＝pi
Figure BDA0003293890380000047
Figure BDA0003293890380000048
Figure BDA0003293890380000049
where, ζ represents a track generated in real time,
Figure BDA00032938903800000410
is the second derivative of the trajectory and,
Figure BDA00032938903800000411
is the third derivative of the trajectory,
Figure BDA00032938903800000412
indicating the position and velocity at the initial time,
Figure BDA00032938903800000413
represents delta1Speed at time τ, amaxRepresenting the maximum acceleration of the aircraft;
Figure BDA00032938903800000414
the smoothed sequence of (c) is calculated as follows:
Figure BDA00032938903800000415
Figure BDA00032938903800000416
Figure BDA00032938903800000417
Figure BDA00032938903800000418
Figure BDA00032938903800000419
wherein ζnewFor the newly generated sequence of smooth tracks,
Figure BDA00032938903800000420
for the last track sequence
Figure BDA00032938903800000421
The last position of (a);
and 3, after two continuous tracks are executed, returning the unmanned aerial vehicle to the initial motion track.
Preferably, λ1，λ2，λ3The calculation process is as follows:
first order
Figure BDA0003293890380000051
Wherein,
Figure BDA0003293890380000052
is the location information p of the drone jjThe component on the x-axis of the unmanned plane i body coordinate system,
Figure BDA0003293890380000053
is the location information p of the drone jjThe component on the y-axis of the i-body coordinate system of the unmanned aerial vehicle,
Figure BDA0003293890380000054
the numerical values of the positions of the coordinate systems of the bodies of the unmanned aerial vehicles i are all 0;
from k to kijObtaining:
Figure BDA0003293890380000055
wherein x is1Component of relative velocity of unmanned aerial vehicle i, j on x-axis of body, x2Is the projection of the position of drone j on the x-axis.
Figure BDA0003293890380000056
The components of the speeds of the unmanned aerial vehicles i and j on the x and y axes respectively;
order to
Figure BDA0003293890380000057
Figure BDA0003293890380000058
Wherein
Figure BDA0003293890380000059
Coordinates of x and y axes of the midpoint of the I, j position connecting line of the unmanned aerial vehicle;
obtaining:
Figure BDA00032938903800000510
wherein y is1Is the projection of the position of drone j on the y-axis, y2Projections of intersection points of two common tangent lines on one sides of the profiles of the unmanned aerial vehicle i and the unmanned aerial vehicle j close to the two fuselage on the y axis;
from the value of m, lambda is obtained1Maximum value of (d):
Figure BDA0003293890380000061
wherein alpha-theta is an included angle between a common tangent and an x axis;
based on this, λ is obtained1The range of (A) is as follows:
Figure BDA0003293890380000062
from the geometric relationship, λ is calculated3：
Figure BDA0003293890380000063
Preferably, the step 3 specifically includes the following steps:
step 31, eliminating the path points which the unmanned aerial vehicle has passed through and the path points which are not passed through in the execution process but are smaller than the distance threshold from the current path point matrix;
step 32, redistributing the residual time to generate a new time point matrix matched with the path point matrix;
and step 33, solving the initial track generation frame to obtain a new track passing through the residual path points.
Preferably, a path point sequence and a time sequence corresponding to the path point are generated based on the initial task of each unmanned aerial vehicle, the task is mapped into a motion track, and a 3-order B-spline curve is adopted to generate the motion track.
Preferably, in the step 1, the motion trajectory generation is realized by using an MATLAB optimization solver quadrprog.
The invention has the following beneficial effects:
the invention provides a multi-unmanned aerial vehicle cooperative collision avoidance method under perception constraint based on speed obstacle, wherein a cooperative collision avoidance algorithm is designed in a distributed multi-unmanned aerial vehicle system consisting of a plurality of rotor unmanned aerial vehicles, so that the unmanned aerial vehicles realize interactive collision avoidance based on observed state information (position and speed), and the collision avoidance is realized only based on the observed information, so that the multi-unmanned aerial vehicle system is more flexible, the collision avoidance algorithm established based on the observed information can improve the safety and robustness of the system, the dependence of the system on communication is reduced, and the multi-unmanned aerial vehicle system has better adaptability in a complex environment.
The method solves the actual problems in the engineering, namely, the visual field constraint is considered, the method can realize the interactive collision avoidance of the multi-unmanned aerial vehicle system only by one camera sensor, and the effect of saving the hardware cost can be achieved;
planning is carried out based on a rolling optimization framework, and information required by planning is obtained through real-time observation, so that the robustness of the system can be improved; the improved collision avoidance method may determine potential collision risk based on speed direction, which may improve planning efficiency of the system.
The method balances two indexes of the task and the safety, and can ensure that the system can complete the established task on the premise of ensuring the safety.
Drawings
Fig. 1 shows a schematic view of two unmanned aerial vehicle interaction collision avoidance;
FIG. 2 shows a field of view vector field established based on a perception range;
FIG. 3 shows a modified set of relative velocity barriers;
FIG. 4 is a schematic diagram of an improved cooperative collision avoidance algorithm;
FIG. 5 shows an improved flight simulation experimental plot based on a set of relative velocity obstacles.
Detailed Description
The invention is further illustrated by the following figures and examples:
step 1, generating a motion trail facing to a subtask
Consider a multi-drone system consisting of n (n ≧ 6) drones. For each drone in the system
Figure BDA0003293890380000071
Figure BDA0003293890380000072
With an initial task of a given sequence of path points
Figure BDA0003293890380000073
And time series corresponding to the path points
Figure BDA0003293890380000074
Mapping the task to a motion trajectory and generating the motion trajectory by using a 3-order B-spline curve, i.e.
Figure BDA0003293890380000075
Wherein c isj∈C＝[c0，c1，...，cM-1]TIs the control point of the B-spline,
Figure BDA0003293890380000076
is the basis function of a 3 rd order B-spline. The task initial goal is to generate a track passing through all given path points, and simultaneously, the track is required to be smooth, and the optimization framework is defined as:
min JS+JW
Figure BDA0003293890380000077
Figure BDA0003293890380000078
wherein JSIs a cost function that smoothes the trajectory, JWIs a cost function for a trajectory through a given path point,
Figure BDA0003293890380000079
are constraints of the initial and the end state,
Figure BDA00032938903800000710
is a kinematic constraint that satisfies the performance of the unmanned aerial vehicle,
Figure BDA00032938903800000711
is a vector form of the control point matrix C. Wherein, JSDerived by penalising the third derivative of the trajectory, i.e.
Figure BDA0003293890380000081
Where a is a constant number of times,
Figure BDA0003293890380000082
Figure BDA0003293890380000083
is a semi-positive definite matrix.
Given a as s/t, JWIs defined as:
Figure BDA0003293890380000084
where matrix H is defined as
Figure BDA0003293890380000085
The constraint term is defined as:
Figure BDA0003293890380000086
Figure BDA0003293890380000087
Figure BDA0003293890380000088
Figure BDA0003293890380000089
Figure BDA00032938903800000810
wherein Λ, An，ΓnIs a mapping matrix.
The above problem can be realized by MATLAB's optimization solver quadrprog.
Step 2, real-time cooperative collision avoidance based on observation information
And step 21, assuming that the unmanned plane i and the other unmanned plane j exist, the radius of the plane is r, and the view angle is FOV. Unmanned plane i self flying speed viAnd the position information p of other unmanned aerial vehicles in the visual field range can be observed in the flight processjVelocity information vjAnd handpiece orientation information psij. Based on the observation information, the collaborative mode function is defined as
Figure BDA00032938903800000811
When g (p, psi) is more than or equal to 0, the two unmanned aerial vehicles are in a mutual observation mode; when g (p, ψ) < 0, drone i observes drone j unilaterally, i.e. unidirectional observation mode.
Step 22, defining the view vector field function as
Figure BDA0003293890380000091
Wherein
Figure BDA0003293890380000092
Is a unit vector of the unmanned plane i in the x-axis direction of the fuselage,
Figure BDA0003293890380000093
is the observed velocity vjThe transpose of (a) is performed,
Figure BDA0003293890380000094
is the location information p of the drone jjComponent on the x-axis of the unmanned plane i-body coordinate system, h (p)j)，c(pj) Is a scalar function that is always greater than 0. When the function value returned by the VVF is larger than 0, observing that the potential collision risk exists between the unmanned aerial vehicle and the target unmanned aerial vehicle; when the function value is less than or equal to 0, the two unmanned aerial vehicles have no collision risk.
Step 23, after judging that there is a collision risk, realizing cooperative collision avoidance by the following steps:
step 231, generating a relative velocity obstacle set Mod _ ORCA (p) based on the observation informationj，vi，vjMode), where mode is an observation mode, the values of which are given by g (p, ψ). When the observation mode is bidirectional observation, the following expression is calculated:
Figure BDA0003293890380000095
Figure BDA0003293890380000096
and u is a speed change amount, and the calculation method is to solve the minimum value from the relative speed of the unmanned aerial vehicle i, j to the boundary of the set of the infeasible speeds. The minimum amount of change from the current speed to the desired speed can be obtained by solving for u. Wherein argmin represents the minimum of the function,
Figure BDA0003293890380000097
being the boundary of the infeasible velocity set, | | | · | |, represents the modulo operation.
Figure BDA0003293890380000098
The geometry of the collision-free velocity set constructed based on the control quantity u is a half plane.
Figure BDA0003293890380000099
The method of calculation of (a) is to find all velocities that have an inner product with u greater than or equal to 0, so that the velocities in the set are all safe and collision-free. Wherein n is a normal vector perpendicular to the half-plane, the calculation method is to take a unit vector of u, and τ is the execution time of the planner.
When the observation mode is one-way observation, if VVF returns a value of c (p)j) The following expression is calculated:
vi＝vi-λ1vi-λ2vj
Figure BDA00032938903800000910
Figure BDA00032938903800000911
if VVF returns a value of
Figure BDA0003293890380000101
Then
u＝-λ3vi
Figure BDA0003293890380000102
Wherein λ1，λ2，λ3The method is a positive real number, and is combined with an improved collaborative collision avoidance algorithm schematic diagram (fig. 4), and the numerical calculation process is as follows:
first order
Figure BDA0003293890380000103
Wherein,
Figure BDA0003293890380000104
is the location information p of the drone jjThe component on the x-axis of the unmanned plane i body coordinate system,
Figure BDA0003293890380000105
is the location information p of the drone jjThe component on the y-axis of the i-body coordinate system of the unmanned aerial vehicle,
Figure BDA0003293890380000106
the numerical value of the position of the coordinate system of the body of the unmanned aerial vehicle i is always 0.
From k to kijCan obtain
Figure BDA0003293890380000107
Wherein x is1Component of relative velocity of unmanned aerial vehicle i, j on x-axis of body, x2Is the projection of the position of drone j on the x-axis.
Figure BDA0003293890380000108
The components of the velocity of drone i, j on the x, y axes, respectively.
Order to
Figure BDA0003293890380000109
Figure BDA00032938903800001010
Wherein
Figure BDA00032938903800001011
And the coordinates of the x axis and the y axis of the midpoint of the connecting line of the i position and the j position of the unmanned aerial vehicle.
Can obtain
Figure BDA00032938903800001012
Wherein y is1Is the projection of the position of drone j on the y-axis, y2Projections of intersection points of two common tangent lines on the side close to the profile (circular shape) of two bodies of the unmanned aerial vehicle i and the unmanned aerial vehicle j on the y axis;
from the value of m, λ can be obtained1Maximum value of
Figure BDA0003293890380000111
Wherein alpha-theta is the angle between the common tangent and the x-axis.
Based on this, λ can be obtained1In the range of
Figure BDA0003293890380000112
By calculating λ from geometric relationships3Can obtain
Figure BDA0003293890380000113
Step 232, generating a safe speed set ORCAτ＝D(0，vmax) Andgate Mod _ ORCA, wherein vmaxFor the maximum allowable speed, n represents taking the intersection;
D(0，vmax)＝{p|||p-0||＜vmax}。
wherein p represents all velocity vectors that meet the requirements; d (0, v)max) A circular set is constructed which is calculated by finding all the modulus values less than vmaxRepresents the velocity space allowed by the drone dynamical model. By solving for D (0, v)max) The maximum value and the minimum value which can be reached by the speed of the unmanned aerial vehicle in flight can be obtained, and the ORCA is solvedτA safe collision-free velocity set can be obtained that meets the dynamics requirements when solving the trajectory.
Step 233, selecting an optimal speed v within the generated safe speed setoptDefined as:
Figure BDA0003293890380000114
step 234, generating two continuous smooth track sequences according to the selected optimal speed
Figure BDA0003293890380000115
Is defined as
Figure BDA0003293890380000116
Figure BDA0003293890380000117
Wherein delta1，δ2Is to satisfy delta1+δ2A constant of < 1;
Figure BDA0003293890380000118
the smooth trajectory sequence of (2) is calculated as follows:
Figure BDA0003293890380000121
s.t.ζ(0)＝pi
Figure BDA0003293890380000122
Figure BDA0003293890380000123
Figure BDA0003293890380000124
where ζ represents a real-time generated trackThe trace is a trace of the data to be written,
Figure BDA0003293890380000125
is the second derivative of the trajectory and,
Figure BDA0003293890380000126
is the third derivative of the trajectory,
Figure BDA0003293890380000127
indicating the position and velocity at the initial time,
Figure BDA0003293890380000128
represents delta1Speed at time τ, amaxRepresenting the maximum acceleration of the aircraft;
Figure BDA0003293890380000129
the smoothed sequence of (c) is calculated as follows:
Figure BDA00032938903800001210
Figure BDA00032938903800001211
Figure BDA00032938903800001212
Figure BDA00032938903800001213
Figure BDA00032938903800001214
wherein ζnewFor the newly generated sequence of smooth tracks,
Figure BDA00032938903800001215
for the last track sequence
Figure BDA00032938903800001216
The last position of (a);
and 3, after the steps are implemented, the unmanned aerial vehicle can avoid collision in the current interaction process, namely within the time tau. After that, the following steps are implemented to make the unmanned aerial vehicle return to the initial track:
step 31, executing the waypoint update function Waypoints _ update (p). The function eliminates the path points which the unmanned aerial vehicle has passed through and the path points which are not passed through but are smaller than the distance threshold value in the execution process from the current path point matrix;
step 32, the Time update function Time _ response (t) is executed. The function redistributes the remaining time to generate a new time point matrix matching the path point matrix. If the feasible track can not be generated only by depending on the remaining time, the iteration is executed until the generated track is feasible.
And step 33, solving the initial track generation frame to obtain a new track passing through the residual path points. In the solving process, a quadrprog function of MATLAB is used as a solver.
Through the above steps, the drone may generate a trajectory back to the initial task. And (4) repeating the collision avoiding step and the regression step through online rolling optimization until each unmanned aerial vehicle reaches the own termination position.
Fig. 1 shows the overall process of cooperative collision avoidance by two drones. Under the condition that an initial task is given, two unmanned aerial vehicles plan and generate collision-prevention tracks through real-time observation information; and generating a track for returning to the current task after collision avoidance is finished.
Fig. 2 shows the constructed field-of-view vector field. And (4) observing that the unmanned aerial vehicle constructs a visual field vector field based on a body coordinate system of the unmanned aerial vehicle, and filtering the unmanned aerial vehicle without potential collision risk through a field function.
Fig. 3-4 illustrate an improved set of speed barriers and a specific improved calculation method.
Then, the present invention performs simulation and physical experiment on the proposed control method. The invention carries out two types of simulation experiments: one is experimental verification of the improved collision avoidance algorithm. In the simulation, a system consisting of two unmanned aerial vehicles is generated, the two unmanned aerial vehicles are always in a one-way observation state in the experimental process, and potential collision possibility exists between the two unmanned aerial vehicles. By executing the cooperative collision avoidance algorithm, the observation unmanned aerial vehicle slightly changes the current track to avoid the obstacle unmanned aerial vehicle. Compared with the traditional method, the method is more intelligent and has better collision prevention effect; the other type is that a multi-unmanned aerial vehicle system consisting of six unmanned aerial vehicles carries out the experiment of the cooperative collision avoidance simulation. In the simulation, an initial task track of each unmanned aerial vehicle is given, and the unmanned aerial vehicle is in cooperative collision avoidance with other unmanned aerial vehicles through observation information in the motion process and finally reaches a target point. The invention provides a position and speed curve in the whole motion process to prove the effectiveness of the algorithm. In a physical experiment, the invention establishes a multi-unmanned aerial vehicle system consisting of six unmanned aerial vehicles, and carries out experimental verification based on the conditions of the second type of simulation. In the experimental process, matrix operation needs to be carried out by using an origin library of C + +, and an optimization problem is solved by using an OOQP library.
Fig. 5 shows the experimental effect of the first type of simulation, where the path points of two drones are P respectively1＝[0，0；4，0]，P2＝[2，-1.7；2，1.7]。
The path points of the six unmanned planes are respectively:
P1＝[3.97，-0.05；0.04，-0.05]，
P2＝[0.02，0.05；3.90，0.05]，
P3＝[1.8，1.7；1.8，-0.05；2.1，-1.7]，
P4＝[2.0，-1.7；2.0，0.05；1.7，1.7]，
P5＝[0.04，1.2；2.20，-1.00；3.47，-1.2]，
P6＝[3.47，1.6；2.80，0；0.04，-1]。
six unmanned aerial vehicles satisfy safe distance's restraint between two liang at same moment.
And respectively obtaining initial task tracks by the six unmanned aerial vehicles at the initial positions. If potential collision risks are found in the process of executing the tasks, executing the cooperative collision avoidance, replanning the task track after determining the safety, continuing to execute the established tasks, and finally reaching the target position.
Through simulation and experimental verification, the cooperative collision avoidance method of the multiple unmanned aerial vehicles under the perception constraint based on the speed obstacle can realize cooperative collision avoidance of a multiple unmanned aerial vehicle system under the condition of only local observation information, and each unmanned aerial vehicle can return to an initial track after the collision avoidance is finished, so that the task is completed on the premise of ensuring safety. In addition, all optimization problems in the online rolling optimization process are standard quadratic programming problems, and a feasible track can be generated by adopting an optimization solver, so that the feasibility of the algorithm is ensured.
The present invention is not limited to the above-described embodiments, and various modifications, equivalent substitutions, improvements and the like made within the spirit and principle of the present invention are included in the scope of the present invention.

Claims (5)

1. A distributed multi-unmanned aerial vehicle system cooperative collision avoidance method based on speed obstacle is characterized by comprising the following steps: .
Step 1, generating an initial motion track for each unmanned aerial vehicle in an unmanned aerial vehicle system;
step 2, real-time cooperative collision avoidance based on observation information, specifically comprising:
step 21, assuming that an unmanned plane i and an unmanned plane j exist, the radius of the plane is r, and the size of the view angle is FOV; unmanned plane i self flying speed viAnd observe the position information p of the unmanned plane j in the flight processjVelocity information vjAnd handpiece orientation information psij(ii) a Based on the observation information, the collaborative mode function is defined as:
Figure FDA0003293890370000011
when g (p, psi) is more than or equal to 0, the two unmanned aerial vehicles are in a mutual observation mode; when g (p, psi) < 0, the unmanned aerial vehicle i carries out unilateral observation on the unmanned aerial vehicle j, namely, in a unidirectional observation mode;
step 22, according to the observation information, the view vector field function is defined as:
Figure FDA0003293890370000012
wherein,
Figure FDA0003293890370000013
is a unit vector of the unmanned plane i in the x-axis direction in the fuselage coordinate system,
Figure FDA0003293890370000014
is the observed velocity vjThe transpose of (a) is performed,
Figure FDA0003293890370000015
is the location information p of the drone jjComponent on the x-axis of the unmanned plane i-body coordinate system, h (p)j)，c(pj) Is a scalar function that is always greater than 0; when the function value returned by the VVF is larger than 0, the unmanned aerial vehicle i and the unmanned aerial vehicle j have potential collision risks; when the function value is less than or equal to 0, the two unmanned aerial vehicles have no collision risk;
step 23, after judging that there is a collision risk, realizing cooperative collision avoidance by the following steps:
231, generating a relative velocity obstacle set based on the observation information, and when the observation mode is bidirectional observation, generating the relative velocity obstacle set
Figure FDA0003293890370000016
The following expression is calculated:
Figure FDA0003293890370000017
Figure FDA0003293890370000018
wherein u is a control quantity output to the unmanned aerial vehicle, namely a speed change quantity; argmin represents the minimum of the function taken,
Figure FDA0003293890370000019
is the boundary of the infeasible speed set, and represents the modular operation, | | | - |; set of relative velocity obstacles
Figure FDA00032938903700000110
A collision-free speed set constructed based on the control quantity u, wherein the geometry of the collision-free speed set is a semi-plane; n is a normal vector perpendicular to the half-plane; τ is the execution time of the planner;
when the observation mode is one-way observation, if VVF returns a value of c (p)j) Set of relative velocity disorders
Figure FDA00032938903700000111
The following expression is calculated:
vi＝vi-λ1vi-λ2vj
Figure FDA0003293890370000021
Figure FDA0003293890370000022
if VVF returns a value of
Figure FDA0003293890370000023
Set of relative velocity obstacles
Figure FDA0003293890370000024
The following expression is calculated:
u＝-λ3vi
Figure FDA0003293890370000025
wherein λ1，λ2，λ3Is a positive real number;
step 232, generate a safe speed set
Figure FDA0003293890370000026
Wherein v ismaxFor the maximum allowable speed, n represents taking the intersection;
D(0，vmax)＝{p|||p-0||＜vmax}；
p represents all velocity vectors that meet the requirements;
step 233, selecting an optimal speed v within the generated safe speed setoptDefined as:
Figure FDA0003293890370000027
step 234, generating two continuous smooth track sequences according to the selected optimal speed
Figure FDA0003293890370000028
Is defined as:
Figure FDA0003293890370000029
Figure FDA00032938903700000210
wherein delta1，δ2Is to satisfy delta1+δ2A constant of < 1;
Figure FDA00032938903700000211
the smooth trajectory sequence of (2) is calculated as follows:
Figure FDA00032938903700000212
s.t.ζ(0)＝pi
Figure FDA00032938903700000213
Figure FDA00032938903700000214
Figure FDA00032938903700000215
where, ζ represents a track generated in real time,
Figure FDA00032938903700000216
is the second derivative of the trajectory and,
Figure FDA00032938903700000217
is the third derivative of the trace, ζ (0),
Figure FDA00032938903700000218
indicating the position and velocity at the initial time,
Figure FDA00032938903700000219
represents delta1Speed at time τ, amaxRepresenting the maximum acceleration of the aircraft;
Figure FDA0003293890370000031
the smoothed sequence of (c) is calculated as follows:
Figure FDA0003293890370000032
Figure FDA0003293890370000033
Figure FDA0003293890370000034
Figure FDA0003293890370000035
Figure FDA0003293890370000036
wherein ζnewFor the newly generated sequence of smooth tracks,
Figure FDA0003293890370000037
for the last track sequence
Figure FDA0003293890370000038
The last position of (a);
and 3, after two continuous tracks are executed, returning the unmanned aerial vehicle to the initial motion track.
2. The cooperative collision avoidance method for distributed multi-unmanned aerial vehicle system based on speed obstacle as claimed in claim 1, wherein λ is λ1，λ2，λ3The calculation process is as follows:
first order
Figure FDA0003293890370000039
Wherein,
Figure FDA00032938903700000310
is the location information p of the drone jjThe component on the x-axis of the unmanned plane i body coordinate system,
Figure FDA00032938903700000311
is the location information p of the drone jjThe component on the y-axis of the i-body coordinate system of the unmanned aerial vehicle,
Figure FDA00032938903700000312
the numerical values of the positions of the coordinate systems of the bodies of the unmanned aerial vehicles i are all 0;
from k to kijObtaining:
Figure FDA00032938903700000313
wherein x is1Component of relative velocity of unmanned aerial vehicle i, j on x-axis of body, x2Is the projection of the position of drone j on the x-axis.
Figure FDA00032938903700000314
The components of the speeds of the unmanned aerial vehicles i and j on the x and y axes respectively;
order to
Figure FDA00032938903700000315
Figure FDA0003293890370000041
Wherein
Figure FDA0003293890370000042
Coordinates of x and y axes of the midpoint of the I, j position connecting line of the unmanned aerial vehicle;
obtaining:
Figure FDA0003293890370000043
wherein y is1Is the projection of the position of drone j on the y-axis, y2Projections of intersection points of two common tangent lines on one sides of the profiles of the unmanned aerial vehicle i and the unmanned aerial vehicle j close to the two fuselage on the y axis;
from the value of m, lambda is obtained1Maximum value of (d):
Figure FDA0003293890370000044
wherein alpha-theta is an included angle between a common tangent and an x axis;
based on this, λ is obtained1The range of (A) is as follows:
Figure FDA0003293890370000045
from the geometric relationship, λ is calculated3：
Figure FDA0003293890370000046
3. The cooperative collision avoidance method of the distributed multi-unmanned aerial vehicle system based on the speed obstacle as claimed in claim 1, wherein the step 3 specifically comprises the steps of:
step 31, eliminating the path points which the unmanned aerial vehicle has passed through and the path points which are not passed through in the execution process but are smaller than the distance threshold from the current path point matrix;
step 32, redistributing the residual time to generate a new time point matrix matched with the path point matrix;
and step 33, solving the initial track generation frame to obtain a new track passing through the residual path points.
4. The cooperative collision avoidance method of the distributed multi-unmanned aerial vehicle system based on the speed obstacle as claimed in claim 2 or 3, wherein based on the initial task of each unmanned aerial vehicle, a path point sequence and a time sequence corresponding to the path point are generated, the task is mapped to a motion trajectory, and the motion trajectory is generated by adopting a 3-order B spline curve.
5. The cooperative collision avoidance method for the distributed multi-unmanned aerial vehicle system based on the speed obstacle as claimed in claim 2 or 3, wherein in the step 1, the generation of the motion trail is realized by using an optimization solver quadrprog of MATLAB.
CN202111172463.2A 2021-10-08 2021-10-08 Multi-unmanned aerial vehicle cooperative collision avoidance method under perception constraint based on speed obstacle Active CN113885562B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111172463.2A CN113885562B (en) 2021-10-08 2021-10-08 Multi-unmanned aerial vehicle cooperative collision avoidance method under perception constraint based on speed obstacle

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111172463.2A CN113885562B (en) 2021-10-08 2021-10-08 Multi-unmanned aerial vehicle cooperative collision avoidance method under perception constraint based on speed obstacle

Publications (2)

Publication Number Publication Date
CN113885562A true CN113885562A (en) 2022-01-04
CN113885562B CN113885562B (en) 2023-01-10

Family

ID=79005607

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111172463.2A Active CN113885562B (en) 2021-10-08 2021-10-08 Multi-unmanned aerial vehicle cooperative collision avoidance method under perception constraint based on speed obstacle

Country Status (1)

Country Link
CN (1) CN113885562B (en)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116048120A (en) * 2023-01-10 2023-05-02 中国建筑一局(集团)有限公司 Autonomous navigation system and method for small four-rotor unmanned aerial vehicle in unknown dynamic environment

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102759357A (en) * 2012-05-10 2012-10-31 西北工业大学 Cooperative real-time path planning method for multiple unmanned aerial vehicles (UAVs) in case of communication latency
CN106873628A (en) * 2017-04-12 2017-06-20 北京理工大学 A kind of multiple no-manned plane tracks the collaboration paths planning method of many maneuvering targets
CN108388270A (en) * 2018-03-21 2018-08-10 天津大学 Cluster unmanned plane track posture cooperative control method towards security domain
CN108958289A (en) * 2018-07-28 2018-12-07 天津大学 Cluster unmanned plane collision prevention method based on relative velocity obstacle
US20190253621A1 (en) * 2018-02-10 2019-08-15 Goodrich Corporation Distributed aperture systems for obstacle avoidance
CN110632941A (en) * 2019-09-25 2019-12-31 北京理工大学 Trajectory generation method for target tracking of unmanned aerial vehicle in complex environment
CN110825108A (en) * 2019-11-11 2020-02-21 浙江理工大学 Cooperative anti-collision method for multiple tracking unmanned aerial vehicles in same airspace

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102759357A (en) * 2012-05-10 2012-10-31 西北工业大学 Cooperative real-time path planning method for multiple unmanned aerial vehicles (UAVs) in case of communication latency
CN106873628A (en) * 2017-04-12 2017-06-20 北京理工大学 A kind of multiple no-manned plane tracks the collaboration paths planning method of many maneuvering targets
US20190253621A1 (en) * 2018-02-10 2019-08-15 Goodrich Corporation Distributed aperture systems for obstacle avoidance
CN108388270A (en) * 2018-03-21 2018-08-10 天津大学 Cluster unmanned plane track posture cooperative control method towards security domain
CN108958289A (en) * 2018-07-28 2018-12-07 天津大学 Cluster unmanned plane collision prevention method based on relative velocity obstacle
CN110632941A (en) * 2019-09-25 2019-12-31 北京理工大学 Trajectory generation method for target tracking of unmanned aerial vehicle in complex environment
CN110825108A (en) * 2019-11-11 2020-02-21 浙江理工大学 Cooperative anti-collision method for multiple tracking unmanned aerial vehicles in same airspace

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
李樾: "基于改进的速度障碍法的有人/无人机", 《西北工业大学学报》 *

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116048120A (en) * 2023-01-10 2023-05-02 中国建筑一局(集团)有限公司 Autonomous navigation system and method for small four-rotor unmanned aerial vehicle in unknown dynamic environment
CN116048120B (en) * 2023-01-10 2024-04-16 中国建筑一局(集团)有限公司 Autonomous navigation system and method for small four-rotor unmanned aerial vehicle in unknown dynamic environment

Also Published As

Publication number Publication date
CN113885562B (en) 2023-01-10

Similar Documents

Publication Publication Date Title
Hao et al. Formation flight of fixed-wing UAV swarms: A group-based hierarchical approach
Mellinger et al. Minimum snap trajectory generation and control for quadrotors
CN110471426B (en) Automatic collision avoidance method for unmanned intelligent vehicle based on quantum wolf cluster algorithm
Pan et al. Multi-robot obstacle avoidance based on the improved artificial potential field and PID adaptive tracking control algorithm
Zhou et al. Virtual rigid bodies for coordinated agile maneuvering of teams of micro aerial vehicles
Velasco et al. The use of intent information in conflict detection and resolution models based on dynamic velocity obstacles
Váňa et al. Minimal 3d dubins path with bounded curvature and pitch angle
Hou et al. Dynamic compound shape control of robot swarm
Singletary et al. Safety-critical rapid aerial exploration of unknown environments
Ali et al. Feature selection-based decision model for UAV path planning on rough terrains
Hua et al. A time-optimal trajectory planning strategy for an aircraft with a suspended payload via optimization and learning approaches
Parhi et al. Navigational path analysis of mobile robots using an adaptive neuro-fuzzy inference system controller in a dynamic environment
Dong et al. An optimal curvature smoothing method and the associated real-time interpolation for the trajectory generation of flying robots
CN113885562B (en) Multi-unmanned aerial vehicle cooperative collision avoidance method under perception constraint based on speed obstacle
Wang et al. Theory and experiments on enclosing control of multi-agent systems
Ma et al. A joint guidance and control framework for autonomous obstacle avoidance in quadrotor formations under model uncertainty
Park et al. Formation reconfiguration control with collision avoidance of nonholonomic mobile robots
Behroo et al. Near-optimal trajectory generation, using a compound B-spline interpolation and minimum distance criterion with dynamical feasibility correction
Deng et al. Multi-robot dynamic formation path planning with improved polyclonal artificial immune algorithm
Tartaglione et al. Obstacle avoidance via landmark clustering in a path-planning algorithm
Devitt et al. Implementation of the hybrid technology for quadcopter motion control in a complex non-deterministic environment
Iswanto et al. Path planning of decentralized multi-quadrotor based on fuzzy-cell decomposition algorithm
Jamieson et al. Trajectory generation using sub-riemannian curves for quadrotor UAVs
Nguyen et al. Real-time path generation for multicopters in environments with obstacles
CN113821028A (en) Under-actuated AUV formation trajectory tracking control method based on distributed model predictive control

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
GR01 Patent grant
GR01 Patent grant