CN113885562A - A collaborative collision avoidance method for multi-UAVs under perception constraints based on speed obstacles - Google Patents

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

A multi-UAV cooperative collision avoidance method under the perception constraint based on speed obstacle

technical field

The invention relates to a collaborative collision avoidance method for multiple UAVs under the perception constraint based on speed obstacles, and belongs to the technical field of UAV planning.

Background technique

Due to the characteristics of high maneuverability, low operating power consumption, and easy expansion and development of UAVs, the research on UAV systems, especially multi-UAV systems, has received extensive attention from academia and industry in recent years. Multi-UAV systems have a large number of practical applications in search and rescue, collaborative exploration, and entertainment performances. When performing these complex tasks, the multi-UAV cooperative collision avoidance technology is a basic task, which directly determines the following performances of the multi-UAV system:

Safety: When the multi-UAV system performs tasks, such as formation change, collaborative exploration, etc., the ability of UAVs to avoid collisions with each other;

Robustness: The ability of the multi-UAV system to face external disturbances, such as wind disturbance, communication disturbance, etc., the entire system does not collide.

For the collaborative collision avoidance problem, there are the following main solutions:

Scheme 1: Literature (Y. Xu, S. Lai, J. Li, D. Luo, and Y. You, "Concurrent optimaltrajectory planning for indoor quadrotor formation switching," Journal of Intelligent&Robotic Systems, vol. 94, no. 2, pp .503–520, 2019.) and literature (S.H.Arul and D.Manocha, “Dcad: Decentralized collision avoidance with dynamics constraints for agile quadrotor swarms,” IEEE Robotics and Automation Letters, vol.5, no.2, pp.1191–1198 , 2020.) based on two improved speed obstacle methods to achieve multi-UAV collision avoidance. But these methods do not consider perceptual constraints, and in perceptually constrained environments, these two methods are not applicable. The literature (P.Conroy, D.Bareiss, M.Beall, and J.v.d.Berg, "3-d reciprocal collision avoidance onphysical quadrotor helicopters with on-board sensing for relativepositioning," arXiv preprint arXiv:1411.3794, 2014.) achieves consideration of sensing Constrained multi-UAV collision avoidance, but the proposed method is only suitable for the case of fewer UAVs, and ignores the one-way observation problem caused by the visual field constraint.

Scenario 2: Literature (J. Alonso-Mora, T. Naegeli, R. Siegwart, and P. Beardsley, "Collision avoidance for aerial vehicles in multi-agent scenarios," Autonomous Robots, vol. 39, no. 1, pp. 101 –121, 2015.) proposed a collision avoidance method based on speed obstacles and artificial potential fields, but this method needs to rely on a global positioning system, and in a distributed UAV system, this method cannot guarantee the stability of the system.

Scheme 3: Literature (S. Roelofsen, A. Martinoli, and D. Gillet, "3d collisionavoidance algorithm for unmanned aerial vehicles with limited field of viewconstraints," in 2016 IEEE 55th Conference on Decision and Control (CDC). IEEE, 2016, pp.2555-2560.) and literature (S. Roelofsen, D. Gillet, and A. Martinoli, "Collisionavoidance with limited field of view sensing: A velocity obstacle approach," in2017 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2017, pp.1922-1927.) based on a speed selection-limited method to achieve multi-UAV collision avoidance with perception constraints, but the designed method has great restrictions on the flexibility of the UAV platform , and make the scalability of the system worse.

SUMMARY OF THE INVENTION

The invention proposes a distributed multi-UAV system collaborative collision avoidance method based on speed obstacles. Each UAV only obtains external information based on local observations, and an improved speed obstacle algorithm is introduced, so that the UAV is constrained by perception. Under the condition of , cooperative collision avoidance can be realized only based on local observation information.

A distributed multi-UAV system cooperative collision avoidance method based on speed obstacles, comprising:.

Step 1. For each UAV in the UAV system, generate an initial motion trajectory;

Step 2. Real-time collaborative collision avoidance based on observation information, including:

Step 21. Assume that there are UAV i and UAV j, the body radius is r, and the field of view is FOV; the flying speed of UAV _i is vi, and the UAV j is observed during the flight. Position information p _j , speed information v _j and nose orientation information ψ _j ; based on the observation information, the cooperative mode function is defined as:

When g(p, ψ) ≥ 0, the two UAVs are in mutual observation mode; when g(p, ψ) < 0, UAV i observes UAV j unilaterally, that is, unidirectional observation mode ;

Step 22. According to the observation information, the field of view vector field function is defined as:

为无人机i在机身坐标系中x轴方向的单位向量，

是观测速度v_j的转置，

是无人机j的位置信息p_j在无人机i机身坐标系x轴上的分量，h(p_j)，c(p_j)是始终大于0的标量函数；当VVF返回的函数值大于0时，无人机i和无人机j存在潜在的碰撞风险；当函数值小于或等于0时，两个无人机无碰撞风险；in,

is the unit vector of the drone i in the x-axis direction of the fuselage coordinate system,

is the transpose of the observed velocity v _j ,

is the component of the position information p _j of UAV j on the x-axis of the coordinate system of UAV i fuselage, h(p _j ), c(p _j ) are scalar functions that are always greater than 0; when the function value returned by VVF When it is greater than 0, there is a potential collision risk between UAV i and UAV j; when the function value is less than or equal to 0, there is no collision risk between the two UAVs;

Step 23: After judging that there may be a collision risk, implement collaborative collision avoidance through the following steps:

计算如下表达式：Step 231: Generate a relative velocity obstacle set based on the observation information. When the observation mode is bidirectional observation, the relative velocity obstacle set is

Evaluate the following expression:

为不可行速度集合的边界，||·||表示取模运算；相对速度障碍集

为基于控制量u构造的无碰撞速度集合，其几何形状为一个半平面；n为垂直于该半平面的法向量；τ为规划器的执行时间；Among them, u is the control amount output to the UAV, that is, the speed change amount; argmin indicates the minimum value of the function,

is the boundary of the infeasible velocity set, ||·|| represents the modulo operation; the relative velocity obstacle set

is the collision-free velocity set constructed based on the control quantity u, and its geometric shape is a half-plane; n is the normal vector perpendicular to the half-plane; τ is the execution time of the planner;

计算如下表达式为：When the observation mode is one-way observation, if the return value of VVF is c(p _j ), the relative velocity obstacle set

Calculate the following expression as:

v _{i =} vi -λ ₁ v _i -λ ₂ v _j

则相对速度障碍集

计算如下表达式为：If the VVF return value is

then the relative velocity obstacle set

Calculate the following expression as:

u=-λ ₃ v _i

where λ ₁ , λ ₂ , λ ₃ are positive real numbers;

其中v_max为最大容许速度，∩表示取交集；Step 232, generate a safe speed set

Where v _max is the maximum allowable speed, ∩ represents the intersection;

D(0, v _max )={p|||p-0||<v _max };

p represents all velocity vectors that meet the requirements;

Step 233: Select the optimal speed v ^opt in the generated safe speed set, which is defined as:

定义为：Step 234: Generate two consecutive smooth trajectory sequences according to the selected optimal speed

defined as:

Wherein δ ₁ , δ ₂ are constants satisfying δ ₁ +δ ₂ <1;

的光滑轨迹序列计算如下：

The smooth trajectory sequence of is calculated as follows:

_stζ (0)=pi

是轨迹的二阶导数，

是轨迹的三阶导数，

表示初始时刻的位置和速度，

表示δ₁τ时刻的速度，a_max表示飞机的最大加速度；where ζ represents the real-time generated trajectory,

is the second derivative of the trajectory,

is the third derivative of the trajectory,

represents the position and velocity at the initial moment,

Represents the speed at time δ ₁ τ, a _max represents the maximum acceleration of the aircraft;

的光滑序列计算如下：

The smooth sequence of is calculated as follows:

为上一段轨迹序列

的末位置；where ζ ^new is the newly generated smooth trajectory sequence,

is the previous track sequence

the end position of ;

Step 3. After executing two consecutive two segments of trajectory, make the UAV return to the initial motion trajectory.

Preferably, the calculation process of λ ₁ , λ ₂ , λ ₃ is as follows:

first order

是无人机j的位置信息p_j在无人机i机身坐标系x轴上的分量，

是无人机j的位置信息p_j在无人机i机身坐标系y轴上的分量，

为无人机i自身机体坐标系的位置，其数值都始终为0；in,

is the component of the position information p _j of UAV j on the x-axis of UAV i body coordinate system,

is the component of position information p _j of UAV j on the y-axis of UAV i body coordinate system,

is the position of the coordinate system of the drone i itself, and its value is always 0;

Obtained from k _ij :

分别为无人机i，j的速度在x，y轴上的分量；Among them, x ₁ is the component of the relative velocity of UAV i, j on the x-axis of the body, and x ₂ is the projection of the position of UAV j on the x-axis.

are the components of the velocities of the drones i and j on the x and y axes, respectively;

make

为无人机i，j位置连线中点的x、y轴坐标；in

is the x and y-axis coordinates of the midpoint of the line connecting the positions of UAV i and j;

get:

Among them, y ₁ is the projection of the position of the drone j on the y-axis, and y ₂ is the projection on the y-axis of the intersection of the two common tangents of the two fuselage contours of the drone i and the drone j close to one side;

The maximum value of λ ₁ is obtained from the value of m:

where α-θ is the angle between the common tangent and the x-axis;

Based on this, the range of λ ₁ is obtained as:

According to the geometric relationship, calculate λ ₃ :

Preferably, the step 3 specifically includes the following steps:

Step 31. Eliminate from the current waypoint matrix the waypoints that the UAV has passed and the waypoints that have not been passed in the execution process but are smaller than the distance threshold;

Step 32, redistribute the remaining time to generate a new time point matrix that matches the waypoint matrix;

Step 33: Solve the initial trajectory generation framework to obtain a new trajectory passing through the remaining path points.

Preferably, based on the initial task of each UAV, a path point sequence and a time sequence corresponding to the path point are generated, the task is mapped into a motion trajectory, and a third-order B-spline curve is used to generate the motion trajectory.

Preferably, in the step 1, the motion trajectory is generated through Quadprog, an optimization solver of MATLAB.

The present invention has the following beneficial effects:

The invention proposes a collaborative collision avoidance method for multiple UAVs under the perception constraint based on speed obstacles. The observed state information (position, speed) realizes interactive collision avoidance, and the collision avoidance is only based on the observation information, which makes the multi-UAV system more flexible. The collision avoidance algorithm established based on the observation information can improve the safety and robustness of the system. It reduces the system's dependence on communication and makes the multi-UAV system have better adaptability in complex environments.

The method of the invention solves an actual problem in an engineering, that is, considering the visual field constraint, the method only needs one camera sensor to realize the interactive collision avoidance of the multi-UAV system, and can save the hardware cost;

Planning is carried out based on the rolling optimization framework, and the information required for planning is obtained through real-time observation, which can improve the robustness of the system; the improved collision avoidance method can judge potential collision risks based on the speed direction, which can improve the planning efficiency of the system.

The method balances the two indicators of task and safety, which can make the system complete the given task under the premise of ensuring safety.

Description of drawings

Figure 1 shows a schematic diagram of two UAVs interacting to avoid collision;

Fig. 2 shows the field of view vector field established based on the perception range;

Figure 3 represents an improved set of relative velocity obstacles;

Figure 4 shows a schematic diagram of an improved collaborative collision avoidance algorithm;

Figure 5 shows the improved flight simulation experiment diagram based on the relative velocity obstacle set.

Detailed ways

Below in conjunction with accompanying drawing and example, the present invention will be further described:

Step 1. Generate subtask-oriented motion trajectories

其初始任务为给定的路径点序列

和与路径点对应的时间序列

将任务映射为运动轨迹，并采用3阶B样条曲线生成运动轨迹，即Consider a multi-UAV system consisting of n (n ≥ 6) UAVs. For each drone in the system

Its initial task is a given sequence of waypoints

and the time series corresponding to the waypoints

The task is mapped to the motion trajectory, and the third-order B-spline curve is used to generate the motion trajectory, namely

是3阶B样条的基函数。任务初始目标是生成一条通过所有给定路径点的轨迹，同时要求轨迹要光滑，其优化框架定义为：where c _j ∈ C = [c ₀ , c ₁ , ..., c _M-1 ] ^T is the control point of the B-spline,

is the basis function of a B-spline of order 3. The initial goal of the task is to generate a trajectory that passes through all given path points, and the trajectory is required to be smooth. The optimization framework is defined as:

min J _S +J _W

是初始和终止状态的约束，

是满足无人机性能的运动学约束，

是控制点矩阵C的向量形式。其中，J_S通过惩罚轨迹的三阶导数得到，即where J _S is the cost function to make the trajectory smooth, J _W is the cost function to make the trajectory pass through a given waypoint,

are the constraints of the initial and final states,

is the kinematic constraint that satisfies the performance of the UAV,

is the vector form of the control point matrix C. where J _S is obtained by penalizing the third derivative of the trajectory, i.e.

where α is a constant,

is a positive semi-definite matrix.

Given a = s/t, J _W is defined as:

Here the matrix H is defined as

The constraints are defined as:

where Λ, A _n , Γ _n are the mapping matrices.

The above problem can be implemented by Quadprog, the optimization solver of MATLAB.

Step 2. Real-time collaborative collision avoidance based on observation information

Step 21. Suppose that there are UAV i and other UAV j, the body radius is r, and the field of view angle is FOV. The flying speed of the drone i is v _i , and the position information p _j , the speed information v _j and the nose orientation information ψ _j of other drones in the field of view can be observed during the flight. Based on the observational information, the collaborative mode function is defined as

When g(p, ψ) ≥ 0, the two UAVs are in mutual observation mode; when g(p, ψ) < 0, UAV i observes UAV j unilaterally, that is, unidirectional observation mode .

Step 22. According to the observation information, the field of view vector field function is defined as

为无人机i在机身x轴方向的单位向量，

是观测速度v_j的转置，

是无人机j的位置信息p_j在无人机i机身坐标系x轴上的分量，h(p_j)，c(p_j)是始终大于0的标量函数。当VVF返回的函数值大于0时，观测无人机和目标无人机存在潜在的碰撞风险；当函数值小于或等于0时，两个无人机无碰撞风险。in

is the unit vector of the drone i in the x-axis direction of the fuselage,

is the transpose of the observed velocity v _j ,

is the component of position information p _j of UAV j on the x-axis of UAV i body coordinate system, h(p _j ), c(p _j ) are scalar functions that are always greater than 0. When the function value returned by VVF is greater than 0, there is a potential collision risk between the observation UAV and the target UAV; when the function value is less than or equal to 0, the two UAVs have no collision risk.

Step 23: After judging that there may be a collision risk, implement collaborative collision avoidance through the following steps:

Step 231: Generate a relative velocity obstacle set Mod_ORCA(p _j, v _i , v _j , mode) based on the observation information, where mode is the observation mode, and its value is given by g(p, ψ). When the observation mode is two-way observation, the following expression is calculated:

为不可行速度集合的边界，||·||表示取模运算。

为基于控制量u构造的无碰撞速度集合，其几何形状为一个半平面。

的计算方法为找到所有与u的内积大于或等于0的速度，使得集合内的速度都是安全无碰撞的。其中，n为垂直于该半平面的法向量，其计算方法为取u的单位向量，τ为规划器的执行时间。Among them, u is the speed change amount, and its calculation method is to solve the relative speed of the UAV i, j to the minimum value of the boundary of the infeasible speed set. The minimum change from the current speed to the desired speed can be obtained by solving for u. Among them, argmin means to take the minimum value of the function,

is the boundary of the infeasible velocity set, and ||·|| represents the modulo operation.

is a collision-free velocity set constructed based on the control quantity u, and its geometric shape is a half-plane.

The calculation method is to find all velocities whose inner product with u is greater than or equal to 0, so that the velocities in the set are safe and collision-free. Among them, n is the normal vector perpendicular to the half-plane, which is calculated by taking the unit vector of u, and τ is the execution time of the planner.

When the observation mode is one-way observation, if the return value of VVF is c(p _j ), the following expression is calculated:

v _{i =} vi -λ ₁ v _i -λ ₂ v _j

则If the VVF return value is

but

u=-λ ₃ v _i

Among them λ ₁ , λ ₂ , λ ₃ are positive real numbers, combined with the schematic diagram of the improved cooperative collision avoidance algorithm (Fig. 4), the numerical calculation process is as follows:

first order

是无人机j的位置信息p_j在无人机i机身坐标系x轴上的分量，

是无人机j的位置信息p_j在无人机i机身坐标系y轴上的分量，

为无人机i自身机体坐标系的位置，其数值都始终为0。in,

is the component of position information p _j of UAV j on the x-axis of UAV i fuselage coordinate system,

is the component of the position information p _j of UAV j on the y-axis of UAV i body coordinate system,

is the position of the drone i's own body coordinate system, and its value is always 0.

can be obtained from k _ij

分别为无人机i，j的速度在x，y轴上的分量。Among them, x ₁ is the component of the relative velocity of UAV i, j on the x-axis of the body, and x ₂ is the projection of the position of UAV j on the x-axis.

are the components of the velocities of the drones i and j on the x and y axes, respectively.

make

为无人机i，j位置连线中点的x、y轴坐标。in

is the x and y-axis coordinates of the midpoint of the line connecting the positions of UAV i and j.

can get

where y ₁ is the projection of the position of drone j on the y-axis, and y ₂ is the intersection of the two common tangents of the two fuselage contours (circles) of drone i and drone j close to one side on the y-axis projection on;

The maximum value of λ ₁ can be obtained from the value of m

where α-θ is the angle between the common tangent and the x-axis.

Based on this, the range of λ ₁ can be obtained as

According to the geometric relationship, using the steps of calculating λ ₃ , we can get

Step 232: Generate a safe speed set ORCA ^τ =D(0, v _max )∩Mod_ORCA, where v _max is the maximum allowable speed, and ∩ represents the intersection;

D(0, v _max )={p|||p-0||<v _max }.

Among them, p represents all the speed vectors that meet the requirements; D(0, v _max ) constructs a circular set, the calculation method is to find all the speed sets whose modulus value is less than v _max , this set represents the allowable speed of the UAV dynamics model speed space. By solving D(0, v _max ), the maximum and minimum speed that the UAV can reach during flight can be obtained, and by solving ORCA ^τ , a safe and collision-free speed set that meets the dynamic requirements when solving the trajectory can be obtained.

Step 233: Select the optimal speed v ^opt in the generated safe speed set, which is defined as:

定义为Step 234: Generate two consecutive smooth trajectory sequences according to the selected optimal speed

defined as

Wherein δ ₁ , δ ₂ are constants satisfying δ ₁ +δ ₂ <1;

的光滑轨迹序列计算如下：

The smooth trajectory sequence of is calculated as follows:

_stζ (0)=pi

是轨迹的二阶导数，

是轨迹的三阶导数，

表示初始时刻的位置和速度，

表示δ₁τ时刻的速度，a_max表示飞机的最大加速度；where ζ represents the real-time generated trajectory,

is the second derivative of the trajectory,

is the third derivative of the trajectory,

represents the position and velocity at the initial moment,

Represents the speed at time δ ₁ τ, a _max represents the maximum acceleration of the aircraft;

的光滑序列计算如下：

The smooth sequence of is calculated as follows:

为上一段轨迹序列

的末位置；where ζ ^new is the newly generated smooth trajectory sequence,

is the previous track sequence

the end position of ;

Step 3. After the above steps are completed, the UAV can avoid collision during the current interaction process, that is, within the time τ. After this, perform the following steps to return the drone to the initial trajectory:

Step 31: Execute the waypoints update function Waypoints_Update(P). This function removes the waypoints that the UAV has passed and the waypoints that have not been passed during the execution process but are smaller than the distance threshold from the current waypoint matrix;

Step 32: Execute the time update function Time_Reallocation(T). This function redistributes the remaining time to generate a new matrix of time points that matches the matrix of waypoints. If a feasible trajectory cannot be generated only by the remaining time, T=T+ΔT is executed iteratively until the generated trajectory is feasible.

Step 33: Solve the initial trajectory generation framework to obtain a new trajectory passing through the remaining path points. During the solution process, the Quadprog function of MATLAB needs to be used as the solver.

After the above steps, the drone can generate a trajectory back to the initial mission. With online rolling optimization, the steps of collision avoidance and regression are repeated until each drone reaches its own end position.

Figure 1 shows the whole process of cooperative collision avoidance between two UAVs. When the initial mission is given, the two UAVs plan and generate collision avoidance trajectories through real-time observation information; after collision avoidance, a trajectory returning to the current mission is generated.

Figure 2 shows the constructed field of view vector field. The observation UAV builds a field of view vector field based on its own body coordinate system, and filters the UAVs without potential collision risk through the field function.

Figures 3 to 4 show the improved set of speed obstacles and the specific improved calculation method.

Next, the present invention conducts simulation and physical experiments on the proposed control method. Two types of simulation experiments are carried out in the present invention: one is the experimental verification of the improved collision avoidance algorithm. In this simulation, the present invention generates a system composed of two UAVs. During the experiment, the two UAVs always maintain a one-way observation state, and the two UAVs have a potential collision possibility. By executing the collaborative collision avoidance algorithm, the observation UAV slightly changes the current trajectory to avoid the obstacle UAV. Compared with the traditional method, this method is more intelligent and has better collision avoidance effect. In this simulation, given the initial mission trajectory of each UAV, the UAV cooperates with other UAVs to avoid collisions through observation information during the movement process, and finally reaches the target point. The present invention provides the position and velocity curve in the whole movement process to prove the validity of the algorithm. In the physical experiment, the present invention establishes a multi-unmanned aerial vehicle system composed of six unmanned aerial vehicles, and conducts experimental verification based on the conditions of the second type of simulation. During the experiment, the Eigen library of C++ needs to be used for matrix operations, and the OOQP library is used to solve the optimization problem.

Figure 5 shows the experimental results of the first type of simulation, where the path points of the two UAVs are P ₁ =[0, 0; 4, 0], P ₂ =[2, -1.7; 2, 1.7], respectively.

The waypoints for the six drones are:

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].

The six drones meet the safety distance constraints in pairs at the same time.

At the initial position, the six UAVs get the initial mission trajectory respectively. If a potential collision risk is found during the execution of the mission, collaborative collision avoidance will be performed, the mission trajectory will be re-planned after the safety is determined, and the mission will continue to be executed, and finally reach the target position.

Through simulation and experimental verification, it can be shown that the use of this multi-UAV cooperative collision avoidance method under the perception constraint based on speed obstacles can realize the cooperative collision avoidance of multi-UAV systems with only local observation information, and It can make each UAV return to the initial trajectory after collision avoidance, and complete the task under the premise of ensuring safety. In addition, all optimization problems in the online rolling optimization process are standard quadratic programming problems, and feasible trajectories can be generated by using the optimization solver, which ensures the feasibility of the algorithm.

The above are only the preferred embodiments of the present invention, and the present invention is not limited to the above-mentioned embodiments. All local changes, equivalent replacements, improvements, etc. made within the spirit and principles of the present invention should be included in the within the protection 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 v_iAnd observe the position information p of the unmanned plane j in the flight process_jVelocity information v_jAnd handpiece orientation information psi_j(ii) a Based on the observation information, the collaborative mode function is defined as:

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:

wherein,

is a unit vector of the unmanned plane i in the x-axis direction in the fuselage coordinate system,

is the observed velocity v_jThe transpose of (a) is performed,

is the location information p of the drone j_jComponent on the x-axis of the unmanned plane i-body coordinate system, h (p)_j)，c(p_j) 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

The following expression is calculated:

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,

is the boundary of the infeasible speed set, and represents the modular operation, | | | - |; set of relative velocity obstacles

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

The following expression is calculated:

v_i＝v_i-λ₁v_i-λ₂v_j

if VVF returns a value of

Set of relative velocity obstacles

The following expression is calculated:

u＝-λ₃v_i

wherein λ₁，λ₂，λ₃Is a positive real number;

step 232, generate a safe speed set

Wherein v is_maxFor the maximum allowable speed, n represents taking the intersection;

D(0，v_max)＝{p|||p-0||＜v_max}；

p represents all velocity vectors that meet the requirements;

step 233, selecting an optimal speed v within the generated safe speed set^optDefined as:

step 234, generating two continuous smooth track sequences according to the selected optimal speed

Is defined as:

wherein delta₁，δ₂Is to satisfy delta₁+δ₂A constant of < 1;

the smooth trajectory sequence of (2) is calculated as follows:

s.t.ζ(0)＝p_i

where, ζ represents a track generated in real time,

is the second derivative of the trajectory and,

is the third derivative of the trace, ζ (0),

indicating the position and velocity at the initial time,

represents delta₁Speed at time τ, a_maxRepresenting the maximum acceleration of the aircraft;

the smoothed sequence of (c) is calculated as follows:

wherein ζ^newFor the newly generated sequence of smooth tracks,

for the last track sequence

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 λ₁，λ₂，λ₃The calculation process is as follows:

first order

Wherein,

is the location information p of the drone j_jThe component on the x-axis of the unmanned plane i body coordinate system,

is the location information p of the drone j_jThe component on the y-axis of the i-body coordinate system of the unmanned aerial vehicle,

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 k_ijObtaining:

wherein x is₁Component of relative velocity of unmanned aerial vehicle i, j on x-axis of body, x₂Is the projection of the position of drone j on the x-axis.

The components of the speeds of the unmanned aerial vehicles i and j on the x and y axes respectively;

order to

Wherein

Coordinates of x and y axes of the midpoint of the I, j position connecting line of the unmanned aerial vehicle;

obtaining:

wherein y is₁Is the projection of the position of drone j on the y-axis, y₂Projections 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 obtained₁Maximum value of (d):

wherein alpha-theta is an included angle between a common tangent and an x axis;

based on this, λ is obtained₁The range of (A) is as follows:

from the geometric relationship, λ is calculated₃：

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.

Images