Anti-collision control method and device between unmanned aerial vehicles in formation flight of unmanned aerial vehicles

Abstract

The invention provides an inter-aircraft anti-collision control method and device in formation flight of unmanned aerial vehicles. The method comprises the following steps: acquiring the position and speed of another aircraft relative to the course of a local aircraft; according to the forward position Px, the lateral position Py and the vertical position Pz of the robot relative to the local machine, virtual resultant repulsive force is generated; performing normalization processing on the virtual resultant repulsive force; projecting the virtual repulsive force F on an X axis, a Y axis and a Z axis to obtain a forward anti-collision repulsive force Fx, a lateral anti-collision repulsive force Fy and a vertical anti-collision repulsive force Fz; calculating a three-axis component force normalization coefficient; setting a three-axis air route tracking adjustment coefficient; setting a three-axis anti-collision speed adjusting coefficient; according to the three-axis component force normalization coefficient, the three-axis air route tracking adjustment coefficient and the three-axis anti-collision speed adjustment coefficient, a control mode adopted by the unmanned aerial vehicle during anti-collision control is adopted, and a three-axis anti-collision control instruction is generated; and limiting the amplitude of each control instruction, then outputting the control instructions, and superposing the control instructions with the control law instructions of the corresponding modes to carry out anti-collision control of the unmanned aerial vehicle.

Description

Technical Field

[0001] The present invention belongs to the field of UAV cluster flight control technology, and in particular relates to a method and device for preventing collisions between UAVs in formation flight. Background Art

[0002] Multi-UAV formation collaborative control technology is a hot research issue in the field of swarm intelligence. UAVs need to maintain a certain distance during formation flight, and the minimum safe distance is an insurmountable red line.

[0003] The setting of anti-collision instructions between drones will affect the control effect of anti-collision. Excessive anti-collision instructions may cause the flight trajectory of the drone to oscillate, or even cause the drone to deviate too far from the original flight route during anti-collision. Insufficient anti-collision instructions will result in the failure to achieve the anti-collision effect, leading to the risk of collision between drones. The current common anti-collision instruction generation method between drones is mainly designed based on the relative position between drones, without considering the relative motion trend between drones, resulting in inappropriate anti-collision instructions and affecting the anti-collision control effect between drones. Summary of the invention

[0004] The present invention proposes an inter-drone anti-collision control method and device in UAV formation flight, aiming to solve the problem of inter-drone collision prediction based only on position and speed information and the problem of anti-collision overshoot control.

[0005] In a first aspect, the present application provides a method for preventing collisions between drones in formation flight, the method comprising:

[0006] S10, obtaining the collision flag CA_Flag of other drones relative to the host through the collision prediction module, when CA_Flag=1, it indicates that there is a collision conflict between the other drone and the host, and when CA_Flag≠1, it indicates that there is no collision conflict or collision between the other drone and the host;

[0007] S20, obtaining the forward position P of the other aircraft relative to the heading of the aircraft x , lateral position P y , vertical position P z , forward speed V x , lateral speed V y , vertical speed V z ;

[0008] S30, according to his forward position P relative to the aircraft x , lateral position P y and vertical position P z , generating a virtual combined repulsive force;

[0009] S40, normalizing the virtual combined repulsive force;

[0010] S50, based on the forward position P of the other aircraft relative to the heading of the aircraft x , lateral position P y , vertical position P z , calculate and construct the pitch azimuth angle θ and yaw azimuth angle ψ of the other aircraft in the heading coordinate system of this aircraft:

[0011] S60, project the virtual repulsive force F onto the X-axis, Y-axis, and Z-axis to obtain the forward anti-collision repulsive force Fx, the lateral anti-collision repulsive force Fy, and the vertical anti-collision repulsive force Fz:

[0012]

[0013] S70, according to the forward maximum collision avoidance boundary distance X max , maximum lateral collision avoidance boundary distance Y max , vertical maximum collision avoidance boundary distance Z max , calculate the normalized coefficients of the three-axis force components:

[0014]

[0015] S80, set the three-axis route tracking adjustment coefficient;

[0016] S90, set the three-axis collision avoidance speed adjustment coefficient:

[0017] S10, according to the three-axis force normalization coefficient, the three-axis route tracking adjustment coefficient and the three-axis collision avoidance speed adjustment coefficient, the control mode adopted by the UAV during collision avoidance control is adopted to generate a forward collision avoidance control instruction ΔX c , lateral collision avoidance control command ΔY c and vertical collision avoidance control command ΔZ c ;

[0018] S11, forward collision avoidance control command ΔX c , lateral collision avoidance control command ΔY c and vertical collision avoidance control command ΔZ c , after being limited according to the corresponding control command limit, the output is superimposed with the control law command of the corresponding mode to perform UAV collision avoidance control.

[0019] Furthermore, S20 includes:

[0020] S201, obtaining the relative position and relative speed of the other machine from the inter-machine link;

[0021] S202, establish the heading coordinate system of the aircraft, with the center of gravity of the aircraft as the coordinate origin, the current speed direction of the aircraft as the x-axis, the vertical horizontal ground upward as the z-axis, and the y-axis determined by the left-hand rule;

[0022] S203, convert the relative position and relative speed of the other aircraft into the heading coordinate system of the own aircraft through coordinate conversion, and obtain the forward position P of the other aircraft relative to the own aircraft x , lateral position P y , vertical position P z , forward speed V x , lateral speed V y , vertical speed V z .

[0023] Furthermore, S30 includes:

[0024] S301, obtain the formation unit longitudinal separation distance Formation_Z, the formation unit forward separation distance Formation_X, and the formation unit lateral separation distance Formation_Y from the formation performance database, and obtain the formation unit separation distance L through the following formula max :

[0025]

[0026] S302, when CA_Flag = 0, the virtual closing repulsion F is generated to be 0; when CA_Flag = 1, the virtual closing repulsion is generated according to the following formula:

[0027]

[0028] Among them, K min is the virtual repulsion coefficient set. Limited to [1L max ]Inside.

[0029] Furthermore, S40 includes:

[0030] S401, obtain the forward maximum anti-collision boundary distance X from the collision prediction module max , maximum lateral collision avoidance boundary distance Y max , vertical maximum collision avoidance boundary distance Z max ;

[0031] S402, according to the forward maximum anti-collision boundary distance X max , maximum lateral collision avoidance boundary distance Y max , vertical maximum collision avoidance boundary distance Z max , calculate the normalization parameter L min :

[0032]

[0033] S403, according to the normalization parameter L min , the normalized virtual combined repulsion F'(i) is obtained as follows:

[0034]

[0035] Among them, K min The virtual repulsion gain is set.

[0036] Furthermore, S50 includes:

[0037] S501, P x After dividing by 0 protection, we get P x ';

[0038] S502, based on the forward position P of the other aircraft relative to the heading of the aircraft x , lateral position P y , vertical position P z and P x ', calculate the yaw azimuth angle ψ of the other aircraft in the heading coordinate system of this aircraft:

[0039]

[0040] Among them, pi is the ratio of the circumference of a circle;

[0041] S503, based on the forward position P of the other aircraft relative to the heading of the aircraft x , lateral position P y , vertical position P z , calculate the pitch azimuth angle θ of the other aircraft in the heading coordinate system of this aircraft:

[0042]

[0043] Furthermore, in S70:

[0044] Limit |Px| to [1,Formation_X] during calculation;

[0045] Limit |Py| to [1,Formation_Y] during calculation;

[0046] |Pz| is constrained to be within [1,Formation_Z] during calculation.

[0047] Furthermore, S80 includes:

[0048] S801, obtaining the own-aircraft position signal from the position sensor, and comparing it with the own-aircraft flight path to calculate the own-aircraft side offset Dz and altitude deviation dH;

[0049] S802, set forward route tracking adjustment coefficient K Fx :

[0050]

[0051] Among them, K Fx0K is the forward repulsion basic adjustment coefficient, which is set according to the selected forward control mode; Fx_les is the forward route adjustment coefficient, K Fx_les The setting range is (0,1); ΔX is the allowable forward error range of the UAV when in lateral formation;

[0052] S803, set lateral path tracking adjustment coefficient K Fy :

[0053]

[0054] Among them, K Fy0 K is the lateral basic adjustment coefficient, which is set according to the selected lateral control mode; Fy_les is the lateral route adjustment coefficient, K Fy0_les The setting range is (0,1); ΔDz is the side offset control error range;

[0055] S804, set the vertical route tracking adjustment coefficient K Fz :

[0056]

[0057] Among them, K Fz0 K is the vertical foundation adjustment coefficient, which is set according to the selected vertical control mode; Fz_les is the vertical route adjustment coefficient, K Fz0_les The setting range is (0,1); ΔH is the height control error range.

[0058] Furthermore, S90 includes:

[0059] S901, set forward collision avoidance speed adjustment coefficient K Vx :

[0060]

[0061] Among them, K Vx_add is the forward velocity approach coefficient, K Vx_les is the forward velocity away coefficient, K Vx_les The setting range of is [0,1);

[0062] S902, set the side collision avoidance speed adjustment coefficient K Vy :

[0063]

[0064] Among them, K Vy_add is the lateral velocity approach coefficient, K Vy_les is the lateral velocity away coefficient, K Vy_lesThe setting range of is [0,1);

[0065] S903. Set vertical collision avoidance speed adjustment coefficient K Vz :

[0066]

[0067] Among them, K Vz_add is the vertical velocity approach coefficient, K Vz_les is the vertical velocity away coefficient, K Vz_les The setting range is [0,1).

[0068] Furthermore, S10 includes:

[0069] S1002: Generate forward collision avoidance control instruction ΔX c :ΔX c =F x ·K x1 ·K Fx ·K Vx ;

[0070] S1003: Generate lateral collision avoidance control instruction ΔY c :ΔY c =F y ·K y1 ·K Fy ·K Vy ;

[0071] S1004: Generate vertical collision avoidance control instruction ΔZ c :ΔZ c =F z ·K z1 ·K Fz ·K Vz .

[0072] In a second aspect, the present application provides an inter-aircraft anti-collision control device in UAV formation flight, wherein the inter-aircraft anti-collision control device is used to implement the above-mentioned inter-aircraft anti-collision control method in UAV formation flight.

[0073] In summary, the present application provides a method and device for anti-collision control between drones in formation flight of drones. By generating a normalized anti-collision instruction for three axes of the drone including a route tracking adjustment coefficient and an anti-collision speed adjustment coefficient, the anti-collision control effect between drones is improved. The relative position, relative speed, and target route between the instructions change dynamically. The generated anti-collision control instructions can achieve anti-collision control between drones without affecting the normal route flight of the drones. BRIEF DESCRIPTION OF THE DRAWINGS

[0074] Figure 1 A schematic diagram of the inter-machine anti-collision control effect provided in Example 1;

[0075] Figure 2 This is a schematic diagram of the inter-machine anti-collision control effect provided in Example 2. DETAILED DESCRIPTION

[0076] In conjunction with the accompanying drawings, the present invention provides a method for preventing collisions between drones during formation flight, comprising:

[0077] S10. Obtain the collision flag CA_Flag of other drones relative to the local machine through the collision prediction module. When CA_Flag=1, it indicates that there is a collision conflict between the other drone and the local machine. When CA_Flag≠1, it indicates that there is no collision conflict or collision between the other drone and the local machine.

[0078] S20, obtaining the forward position P of the other aircraft relative to the heading of the aircraft x , lateral position P y , vertical position P z , forward speed V x , lateral speed V y , vertical speed V z ;

[0079] Specifically, S20 includes:

[0080] S201. Obtain the relative position and relative speed of another machine from the inter-machine link.

[0081] S202, establish the heading coordinate system of the aircraft, with the center of gravity of the aircraft as the coordinate origin, the current speed direction of the aircraft as the x-axis, the vertical horizontal ground upward as the z-axis, and the y-axis determined by the left-hand rule.

[0082] S203, convert the relative position and relative speed of the other aircraft into the heading coordinate system of the own aircraft through coordinate conversion, and obtain the forward position P of the other aircraft relative to the own aircraft x , lateral position P y , vertical position P z , forward speed V x , lateral speed V y , vertical speed V z .

[0083] S30, according to his forward position P relative to the aircraft x , lateral position P y and vertical position P z , generating a virtual combined repulsive force;

[0084] Specifically, S30 includes:

[0085] S301, obtain the formation unit longitudinal separation distance Formation_Z, the formation unit forward separation distance Formation_X, and the formation unit lateral separation distance Formation_Y from the formation performance database, and obtain the formation unit separation distance L through the following formula max :

[0086]

[0087] S302, when CA_Flag = 0, the virtual closing repulsion F is generated to be 0; when CA_Flag = 1, the virtual closing repulsion is generated according to the following formula:

[0088]

[0089] Among them, K min is the virtual repulsion coefficient set. Limited to [1L max ]Inside;

[0090] S40, normalizing the virtual combined repulsive force;

[0091] Specifically, S40 includes:

[0092] S401, obtain the forward maximum anti-collision boundary distance X from the collision prediction module max , maximum lateral collision avoidance boundary distance Y max , vertical maximum collision avoidance boundary distance Z max ;

[0093] S402, according to the forward maximum anti-collision boundary distance X max , maximum lateral collision avoidance boundary distance Y max , vertical maximum collision avoidance boundary distance Z max , calculate the normalization parameter L min :

[0094]

[0095] S403, according to the normalization parameter L min , the normalized virtual combined repulsion F'(i) is obtained as follows:

[0096]

[0097] Among them, K min The virtual repulsion gain is set.

[0098] S50, based on the forward position P of the other aircraft relative to the heading of the aircraft x , lateral position P y , vertical position P z, calculate and construct the pitch azimuth angle θ and yaw azimuth angle ψ of the other aircraft in the heading coordinate system of this aircraft:

[0099] Specifically, S50 includes:

[0100] S501, P x After dividing by 0 protection, we get P x ';

[0101] S502, based on the forward position P of the other aircraft relative to the heading of the aircraft x , lateral position P y , vertical position P z and P x ', calculate the yaw azimuth angle ψ of the other aircraft in the heading coordinate system of this aircraft:

[0102]

[0103] Among them, pi is the circumference of a circle.

[0104] S503, based on the forward position P of the other aircraft relative to the heading of the aircraft x , lateral position P y , vertical position P z , calculate the pitch azimuth angle θ of the other aircraft in the heading coordinate system of this aircraft:

[0105]

[0106] S60, project the virtual repulsive force F onto the X-axis, Y-axis, and Z-axis to obtain the forward anti-collision repulsive force Fx, the lateral anti-collision repulsive force Fy, and the vertical anti-collision repulsive force Fz:

[0107]

[0108] S70, according to the maximum forward collision avoidance boundary distance X max , maximum lateral collision avoidance boundary distance Y max , vertical maximum collision avoidance boundary distance Z max , calculate the normalized coefficients of the three-axis force components:

[0109]

[0110] In the calculation, |Px| is restricted to [1,Formation_X].

[0111] Limit |Py| to the range [1,Formation_Y] during calculation.

[0112] |Pz| is constrained to be within [1,Formation_Z] during calculation.

[0113] S80, set the three-axis route tracking adjustment coefficient;

[0114] Specifically, S80 includes:

[0115] S801, obtaining the own-aircraft position signal from the position sensor, and comparing it with the own-aircraft flight path to calculate the own-aircraft side offset Dz and altitude deviation dH;

[0116] S802, set forward route tracking adjustment coefficient K Fx :

[0117]

[0118] Among them, K Fx0 K is the forward repulsion basic adjustment coefficient, which is set according to the selected forward control mode; Fx_les is the forward route adjustment coefficient, K Fx_les The setting range is (0,1); ΔX is the allowable forward error range of the UAVs when they are in lateral formation.

[0119] S803, set lateral path tracking adjustment coefficient K Fy :

[0120]

[0121] Among them, K Fy0 K is the lateral basic adjustment coefficient, which is set according to the selected lateral control mode; Fy_les is the lateral route adjustment coefficient, K Fy0_les The setting range is (0,1); ΔDz is the side offset control error range.

[0122] S804, set the vertical route tracking adjustment coefficient K Fz :

[0123]

[0124] Among them, K Fz0 K is the vertical foundation adjustment coefficient, which is set according to the selected vertical control mode; Fz_les is the vertical route adjustment coefficient, K Fz0_les The setting range is (0,1); ΔH is the height control error range.

[0125] S90, set the three-axis collision avoidance speed adjustment coefficient:

[0126] Specifically, S90 includes:

[0127] S901, set forward collision avoidance speed adjustment coefficient K Vx :

[0128]

[0129] Among them, K Vx_add is the forward velocity approach coefficient, K Vx_les is the forward velocity away coefficient, K Vx_les The setting range is [0,1);

[0130] S902, set the side collision avoidance speed adjustment coefficient K Vy :

[0131]

[0132] Among them, K Vy_add is the lateral velocity approach coefficient, K Vy_les is the lateral velocity away coefficient, K Vy_les The setting range of is [0,1);

[0133] S903. Set vertical collision avoidance speed adjustment coefficient K Vz :

[0134]

[0135] Among them, K Vz_add is the vertical velocity approach coefficient, K Vz_les is the vertical velocity away coefficient, K Vz_les The setting range is [0,1).

[0136] S10, according to the three-axis force normalization coefficient, the three-axis route tracking adjustment coefficient and the three-axis collision avoidance speed adjustment coefficient, the control mode adopted by the UAV during collision avoidance control is adopted to generate a forward collision avoidance control instruction ΔX c , lateral collision avoidance control command ΔY c and vertical collision avoidance control command ΔZ c ;

[0137] Specifically, S10 includes:

[0138] S1002: Generate forward collision avoidance control instruction ΔX c :ΔX c =F x ·K x1 ·K Fx ·K Vx ;

[0139] S1003: Generate lateral collision avoidance control instruction ΔY c :ΔY c =F y ·K y1 ·K Fy ·K Vy ;

[0140] S1004: Generate vertical collision avoidance control instruction ΔZ c :ΔZ c =F z ·K z1 ·K Fz ·K Vz .

[0141] S11, forward collision avoidance control command ΔX c , lateral collision avoidance control command ΔY c and vertical collision avoidance control command ΔZ c , after being limited according to the corresponding control command limit, the output is superimposed with the control law command of the corresponding mode to perform UAV collision avoidance control.

[0142] Example 1

[0143] S101. Obtain the collision flag CA_Flag of other drones relative to the local machine through the collision prediction module. When CA_Flag=1, it indicates that there is a collision conflict between the other drone and the local machine. When CA_Flag≠1, it indicates that there is no collision conflict or collision between the other drone and the local machine.

[0144] S102, obtaining the position and speed of the other aircraft relative to the heading of the own aircraft:

[0145] S102A, obtaining the relative position and relative speed of the other machine from the inter-machine link.

[0146] S102B, establish the heading coordinate system of the aircraft, with the center of gravity of the aircraft as the coordinate origin, the current speed direction of the aircraft as the x-axis, the vertical horizontal ground upward as the z-axis, and the y-axis determined by the left-hand rule.

[0147] S102C, convert the relative position and relative speed of the other aircraft into the heading coordinate system of the own aircraft through coordinate conversion, and obtain the forward position P of the other aircraft relative to the own aircraft x , lateral position P y , vertical position P z , forward speed V x , lateral speed V y , vertical speed V z .

[0148] S103, generating virtual repulsive force:

[0149] S103A, obtain the formation unit longitudinal separation distance Formation_Z=8m, the formation unit forward separation distance Formation_X=20m, and the formation unit lateral separation distance Formation_Y=15m from the formation performance database, and obtain the formation unit separation distance L through the following formula max :

[0150]

[0151] S103B, when CA_Flag = 0, the virtual closing repulsion F is generated to be 0; when CA_Flag = 1, the virtual closing repulsion is generated according to the following formula:

[0152]

[0153] Among them, the virtual repulsion coefficient K min Set to 81, and calculate Limited to [1L max ]Inside;

[0154] S104, normalizing the virtual combined repulsive force:

[0155] S104A, obtain the maximum collision avoidance boundary distances in the forward, lateral and vertical directions from the collision prediction module as X respectively. max =5.6m, Y max =5.6m, Z max =3m;

[0156] S104B, calculate normalization parameter L min :

[0157]

[0158] S104C, obtain the normalized virtual combined repulsive force F'(i) according to the following formula:

[0159]

[0160] Among them, K min The virtual repulsion gain is set.

[0161] S105, calculate and construct θ and ψ:

[0162] S105A, P x After dividing by 0 protection, we get P x ';

[0163] S105B. Calculate the yaw azimuth angle ψ of the other aircraft in the heading coordinate system of this aircraft:

[0164]

[0165] Among them, pi is the circumference of a circle.

[0166] S105C. Calculate the pitch azimuth angle θ of the other aircraft in the heading coordinate system of this aircraft:

[0167]

[0168] S106. Project the virtual repulsive force F onto the X-axis, Y-axis, and Z-axis to obtain the forward anti-collision repulsive force Fx, the lateral anti-collision repulsive force Fy, and the vertical anti-collision repulsive force Fz:

[0169]

[0170] S107. Calculate the normalization coefficient of the three-axis force components:

[0171]

[0172] |Px| is constrained to be within [1,Formation_X] during calculation. |Py| is constrained to be within [1,Formation_Y] during calculation. |Pz| is constrained to be within [1,Formation_Z] during calculation.

[0173] S108, set the three-axis route tracking adjustment coefficient:

[0174] S108A, obtain the own-aircraft position signal from the position sensor, compare it with the own-aircraft flight path, and calculate the own-aircraft side offset Dz and altitude deviation dH;

[0175] S108B, set forward route tracking adjustment coefficient K Fx :

[0176]

[0177] Among them, the forward repulsion basic adjustment coefficient K is set Fx0 is 0.74; forward route adjustment coefficient K Fx_les is 0.25; the allowable forward error range ΔX of the UAV when in lateral formation is 3m.

[0178] S108C, set lateral path tracking adjustment coefficient K Fy :

[0179]

[0180] Among them, set the lateral foundation adjustment coefficient K Fy0 is 24; lateral route adjustment coefficient K Fy_les is 0.75; the side deviation control error range ΔDz is 3m.

[0181] S108D, set the vertical route tracking adjustment coefficient K Fz :

[0182]

[0183] Among them, the vertical foundation adjustment coefficient K is set Fz0 is 12.2, the vertical route adjustment coefficient K Fz_lesis 0.25, and the height control error range ΔH is 3m.

[0184] S109, set the three-axis anti-collision speed adjustment coefficient:

[0185] S109A, set forward collision avoidance speed adjustment coefficient K Vx :

[0186]

[0187] Among them, set the forward speed approach coefficient K Vx_add is 4, the forward speed is far away from the coefficient K Vx_les is 0.1;

[0188] S109B, set the side collision avoidance speed adjustment coefficient K Vy :

[0189]

[0190] Among them, set the lateral speed approach coefficient K Vy_add is 1, the lateral speed is far from the coefficient K Vy_les is 0.01;

[0191] S109C, set the vertical collision avoidance speed adjustment coefficient K Vz :

[0192]

[0193] Among them, set the vertical speed approach coefficient K Vz_add is 10, the vertical velocity is away from the coefficient K Vz_les is 0.1.

[0194] S110, generating three-axis collision avoidance compensation control instructions:

[0195] S110A, according to the control modes adopted by the UAV when taking anti-collision control, namely speed control, altitude control, and tilt angle control, respectively generate anti-collision control compensation instructions ΔX for each control mode c , ΔY c , ΔZ c ;

[0196] S110B, generate forward collision avoidance control instruction ΔX c :

[0197] ΔX c =F x ·K x1 ·K Fx ·K Vx

[0198] S110B, generate lateral collision avoidance control instruction ΔYc :

[0199] ΔY c =F y ·K y1 ·K Fy ·K Vy

[0200] S110C, generate vertical collision avoidance control command ΔZ c :

[0201] ΔZ c =F z ·K z1 ·K Fz ·K Vz

[0202] S111, ΔX c The speed compensation control command ΔV is limited to [-5m / s, 10m / s] c ; ΔY c The tilt angle compensation control command ΔPhi is limited to [-30o, 30o] c , ΔZ c The height control compensation command ΔH is obtained within the range of [-4m, 4m] c , ΔV c , ΔPhi c , ΔH c The output is superimposed with the control law command of the corresponding mode to perform UAV collision avoidance control.

[0203] Example 2

[0204] S101. Obtain the collision flag CA_Flag of other drones relative to the local machine through the collision prediction module. When CA_Flag=1, it indicates that there is a collision conflict between the other drone and the local machine. When CA_Flag≠1, it indicates that there is no collision conflict or collision between the other drone and the local machine.

[0205] S102, obtaining the position and speed of the other aircraft relative to the heading of the own aircraft:

[0206] S102A, obtaining the relative position and relative speed of the other machine from the inter-machine link.

[0207] S102B, establish the heading coordinate system of the aircraft, with the center of gravity of the aircraft as the coordinate origin, the current speed direction of the aircraft as the x-axis, the vertical horizontal ground upward as the z-axis, and the y-axis determined by the left-hand rule.

[0208] S102C, convert the relative position and relative speed of the other aircraft into the heading coordinate system of the own aircraft through coordinate conversion, and obtain the forward position P of the other aircraft relative to the own aircraft x , lateral position P y , vertical position Pz , forward speed V x , lateral speed V y , vertical speed V z .

[0209] S103, generating virtual repulsive force:

[0210] S103A, obtain the formation unit longitudinal separation distance Formation_Z=10m, the formation unit forward separation distance Formation_X=30m, and the formation unit lateral separation distance Formation_Y=15m from the formation performance database, and obtain the formation unit separation distance L through the following formula max :

[0211]

[0212] S103B, when CA_Flag = 0, the virtual closing repulsion F is generated to be 0; when CA_Flag = 1, the virtual closing repulsion is generated according to the following formula:

[0213]

[0214] Among them, the virtual repulsion coefficient K min Set to 81, and calculate Limited to [1L max ]Inside;

[0215] S104, normalizing the virtual combined repulsive force:

[0216] S104A, obtain the maximum collision avoidance boundary distances in the forward, lateral and vertical directions from the collision prediction module as X respectively. max =5.6m, Y max =5.6m, Z max =3m;

[0217] S104B, calculate normalization parameter L min :

[0218]

[0219] S104C, obtain the normalized virtual combined repulsive force F'(i) according to the following formula:

[0220]

[0221] Among them, K min The virtual repulsion gain is set.

[0222] S105, calculate and construct θ and ψ:

[0223] S105A, P xAfter dividing by 0 protection, we get P x ';

[0224] S105B. Calculate the yaw azimuth angle ψ of the other aircraft in the heading coordinate system of this aircraft:

[0225]

[0226] Among them, pi is the circumference of a circle.

[0227] S105C. Calculate the pitch azimuth angle θ of the other aircraft in the heading coordinate system of this aircraft:

[0228]

[0229] S106. Project the virtual repulsive force F onto the X-axis, Y-axis, and Z-axis to obtain the forward anti-collision repulsive force Fx, the lateral anti-collision repulsive force Fy, and the vertical anti-collision repulsive force Fz:

[0230]

[0231] S107. Calculate the normalization coefficient of the three-axis force components:

[0232]

[0233] |Px| is constrained to be within [1,Formation_X] during calculation. |Py| is constrained to be within [1,Formation_Y] during calculation. |Pz| is constrained to be within [1,Formation_Z] during calculation.

[0234] S108, set the three-axis route tracking adjustment coefficient:

[0235] S108A, obtain the own-aircraft position signal from the position sensor, compare it with the own-aircraft flight path, and calculate the own-aircraft side offset Dz and altitude deviation dH;

[0236] S108B, set forward route tracking adjustment coefficient K Fx :

[0237]

[0238] Among them, the forward repulsion basic adjustment coefficient K is set Fx0 is 0.9; forward route adjustment coefficient K Fx_les is 0.2; the allowable forward error range ΔX of the UAV when in lateral formation is 3m.

[0239] S108C, set lateral path tracking adjustment coefficient K Fy :

[0240]

[0241] Among them, set the lateral foundation adjustment coefficient K Fy0 is 22; lateral route adjustment coefficient K Fy_les is 0.6; the side deviation control error range ΔDz is 3m.

[0242] S108D, set the vertical path tracking adjustment coefficient K Fz :

[0243]

[0244] Among them, the vertical foundation adjustment coefficient K is set Fz0 is 14, the vertical route adjustment coefficient K Fz_les is 0.2, and the height control error range ΔH is 3m.

[0245] S109, set the three-axis anti-collision speed adjustment coefficient:

[0246] S109A, set forward collision avoidance speed adjustment coefficient K Vx :

[0247]

[0248] Among them, set the forward speed approach coefficient K Vx_add is 2, the forward speed is far away from the coefficient K Vx_les is 0.01;

[0249] S109B, set the side collision avoidance speed adjustment coefficient K Vy :

[0250]

[0251] Among them, set the lateral speed approach coefficient K Vy_add is 1.5, the lateral speed is far from the coefficient K Vy_les is 0.1;

[0252] S109C, set the vertical collision avoidance speed adjustment coefficient K Vz :

[0253]

[0254] Among them, set the vertical speed approach coefficient K Vz_add is 8, the vertical velocity is away from the coefficient K Vz_les is 0.3.

[0255] S110, generating three-axis collision avoidance compensation control instructions:

[0256] S110A, according to the control modes adopted by the UAV when taking anti-collision control, namely speed control, altitude control, and tilt angle control, respectively generate anti-collision control compensation instructions ΔX for each control mode c , ΔY c , ΔZ c ;

[0257] S110B, generate forward collision avoidance control instruction ΔX c :

[0258] ΔX c =F x ·K x1 ·K Fx ·K Vx

[0259] S110B, generate lateral collision avoidance control instruction ΔY c :

[0260] ΔY c =F y ·K y1 ·K Fy ·K Vy

[0261] S110C, generate vertical collision avoidance control command ΔZ c :

[0262] ΔZ c =F z ·K z1 ·K Fz ·K Vz

[0263] S111, ΔX c The speed compensation control command ΔV is limited to [-3m / s, 8m / s] c ; ΔY c The tilt angle compensation control command ΔPhi is limited to [-25°, 25°] c , ΔZ c The height control compensation command ΔH is obtained within the range of [-4m, 4m] c , ΔV c , ΔPhi c , ΔH c The output is superimposed with the control law command of the corresponding mode to perform UAV collision avoidance control.

Claims

1. A method for preventing collisions between drones in formation flight, characterized in that: include: S10, obtaining the collision flag CA_Flag of other drones relative to the host through the collision prediction module, when CA_Flag=1, it indicates that there is a collision conflict between the other drone and the host, and when CA_Flag≠1, it indicates that there is no collision conflict or collision between the other drone and the host; S20, obtaining the forward position P of the other aircraft relative to the heading of the aircraft x , lateral position P y , vertical position P z , forward speed V x , lateral speed V y , vertical speed V z ; S30, according to his forward position P relative to the aircraft x , lateral position P y and vertical position P z , generating a virtual combined repulsive force; S40, normalizing the virtual combined repulsive force; S50, based on the forward position P of the other aircraft relative to the heading of the aircraft x , lateral position P y , vertical position P z , calculate and construct the pitch azimuth angle θ and yaw azimuth angle ψ of the other aircraft in the heading coordinate system of this aircraft; S60, project the virtual repulsive force F onto the X-axis, Y-axis, and Z-axis to obtain the forward anti-collision repulsive force Fx, the lateral anti-collision repulsive force Fy, and the vertical anti-collision repulsive force Fz: S70, according to the maximum forward collision avoidance boundary distance X max , maximum lateral collision avoidance boundary distance Y max , vertical maximum collision avoidance boundary distance Z max , calculate the normalized coefficients of the three-axis force components: S80, set the three-axis route tracking adjustment coefficient; S90, set the three-axis anti-collision speed adjustment coefficient; S10, according to the three-axis force normalization coefficient, the three-axis route tracking adjustment coefficient and the three-axis collision avoidance speed adjustment coefficient, the control mode adopted by the UAV during collision avoidance control is adopted to generate a forward collision avoidance control instruction ΔX c , lateral collision avoidance control command ΔY c and vertical collision avoidance control command ΔZ c ; S11, forward collision avoidance control command ΔX c , lateral collision avoidance control command ΔY c and vertical collision avoidance control command ΔZ c , after being limited according to the corresponding control command limit, the output is superimposed with the control law command of the corresponding mode to perform UAV collision avoidance control.

2. The machine-to-machine collision avoidance control method according to claim 1, characterized in that: The S20 includes: S201, obtaining the relative position and relative speed of the other machine from the inter-machine link; S202, establish the heading coordinate system of the aircraft, with the center of gravity of the aircraft as the coordinate origin, the current speed direction of the aircraft as the x-axis, the vertical horizontal ground upward as the z-axis, and the y-axis determined by the left-hand rule; S203, convert the relative position and relative speed of the other aircraft into the heading coordinate system of the own aircraft through coordinate conversion, and obtain the forward position P of the other aircraft relative to the own aircraft x , lateral position P y , vertical position P z , forward speed V x , lateral speed V y , vertical speed V z .

3. The machine-to-machine collision avoidance control method according to claim 1, characterized in that: S30 includes: S301, obtain the formation unit longitudinal separation distance Formation_Z, the formation unit forward separation distance Formation_X, and the formation unit lateral separation distance Formation_Y from the formation performance database, and obtain the formation unit separation distance L through the following formula max : S302, when CA_Flag = 0, the virtual closing repulsion F is generated to be 0; when CA_Flag = 1, the virtual closing repulsion is generated according to the following formula: Among them, K min is the virtual repulsion coefficient set. Limited to [1L max ]Inside.

4. The method for avoiding collision between machines according to claim 1, characterized in that: S40 includes: S401, obtain the forward maximum anti-collision boundary distance X from the collision prediction module max , maximum lateral collision avoidance boundary distance Y max , vertical maximum collision avoidance boundary distance Z max ; S402, according to the forward maximum anti-collision boundary distance X max , maximum lateral collision avoidance boundary distance Y max , vertical maximum collision avoidance boundary distance Z max , calculate the normalization parameter L min : S403, according to the normalization parameter L min , the normalized virtual combined repulsive force F'(i) is obtained as follows: Among them, K min The virtual repulsion gain is set.

5. The machine-to-machine collision avoidance control method according to claim 1, characterized in that: S50 includes: S501, P x After dividing by 0 protection, we get P x '; S502, based on the forward position P of the other aircraft relative to the heading of the aircraft x , lateral position P y , vertical position P z and P x ', calculate the yaw azimuth angle ψ of the other aircraft in the heading coordinate system of this aircraft: Among them, pi is the ratio of the circumference of a circle; S503, based on the forward position P of the other aircraft relative to the heading of the aircraft x , lateral position P y , vertical position P z , calculate the pitch azimuth angle θ of the other aircraft in the heading coordinate system of this aircraft:

6. The method for preventing collision between machines according to claim 1, characterized in that: In S70: Limit |Px| to [1,Formation_X] during calculation; Limit |Py| to [1,Formation_Y] during calculation; |Pz| is constrained to be within [1,Formation_Z] during calculation.

7. The machine-to-machine collision avoidance control method according to claim 1, characterized in that: S80 includes: S801, obtaining the own-aircraft position signal from the position sensor, and comparing it with the own-aircraft flight path to calculate the own-aircraft side offset Dz and the altitude deviation dH; S802, set forward route tracking adjustment coefficient K Fx : in, The forward repulsion basic adjustment coefficient is set according to the selected forward control mode; is the forward path adjustment coefficient, The setting range is (0,1); ΔX is the allowable forward error range of the UAV when in lateral formation; S803, set lateral path tracking adjustment coefficient K Fy : Among them, K Fy0 The lateral basic adjustment coefficient is set according to the selected lateral control mode; is the lateral route adjustment coefficient, The setting range is (0,1); ΔDz is the side offset control error range; S804, set the vertical route tracking adjustment coefficient K Fz : Among them, K Fz0 The vertical foundation adjustment coefficient is set according to the selected vertical control mode; is the vertical route adjustment coefficient, The setting range is (0,1); ΔH is the height control error range.

8. The machine-to-machine collision avoidance control method according to claim 1, characterized in that: S90 includes: S901, set forward collision avoidance speed adjustment coefficient Among them, K Vx_add is the forward velocity approach coefficient, K Vx_les is the forward velocity away coefficient, K Vx_les The setting range is [0,1); S902, set the side collision avoidance speed adjustment coefficient K Vy : Among them, K Vy_add is the lateral velocity approach coefficient, K Vy_les is the lateral velocity away coefficient, K Vy_les The setting range of is [0,1); S903. Set vertical collision avoidance speed adjustment coefficient K Vz : Among them, K Vz_add is the vertical velocity approach coefficient, K Vz_les is the vertical velocity away coefficient, K Vz_les The setting range is [0,1).

9. The machine-to-machine collision avoidance control method according to claim 1, characterized in that: S10 includes: S1002: Generate forward collision avoidance control instruction ΔX c :ΔX c =F x ·K x1 ·K Fx ·K Vx ; S1003: Generate lateral collision avoidance control instruction ΔY c :ΔY c =F y ·K y1 ·K Fy ·K Vy ; S1004: Generate vertical collision avoidance control instruction ΔZ c :ΔZ c =F z ·K z1 ·K Fz ·K Vz .

10. An anti-collision control device for drones in formation flight, characterized in that: The inter-aircraft collision avoidance control device is used to implement the inter-aircraft collision avoidance control method in UAV formation flight described in claim 1.

Images