An unmanned aerial vehicle inter-vehicle collision avoidance control method and device in formation flight

Abstract

The application provides a method and device for anti-collision control between unmanned aerial vehicles in formation flight, the method comprising: obtaining the position and speed of other vehicles relative to the own vehicle heading; generating a virtual repulsive force according to the front position P x , side position P y and vertical position P z of other vehicles relative to the own vehicle; performing normalization processing on the virtual repulsive force; projecting the virtual repulsive force F on the X, Y and Z axes to obtain front anti-collision repulsive force Fx, side anti-collision repulsive force Fy and vertical anti-collision repulsive force Fz; calculating three-axis force normalization coefficients; setting three-axis route tracking adjustment coefficients; setting three-axis anti-collision speed adjustment coefficients; generating three-axis anti-collision control instructions according to the three-axis force normalization coefficients, three-axis route tracking adjustment coefficients and three-axis anti-collision speed adjustment coefficients, and the control mode adopted by the unmanned aerial vehicle when adopting anti-collision control; and outputting the control instructions after limiting the amplitude, and superimposing the control law instructions of the corresponding mode to perform unmanned aerial vehicle anti-collision control.

Description

TECHNICAL FIELD

[0001] The present application belongs to the technical field of unmanned aerial vehicle cluster flight control, and particularly relates to a method and device for preventing collision between unmanned aerial vehicles in formation flight. BACKGROUND

[0002] Multi-unmanned aerial vehicle formation cooperative control technology is a hot research issue in the field of cluster intelligence. Unmanned aerial vehicles need to maintain a certain distance during formation flight, and the minimum safety distance is an inviolable red line.

[0003] The setting of the anti-collision instruction between unmanned aerial vehicles will affect the control effect of anti-collision. An excessively large anti-collision instruction may cause oscillation of the flight trajectory of the unmanned aerial vehicle, and even make the unmanned aerial vehicle deviate from the original flight route too far during anti-collision. An insufficient anti-collision instruction will lead to the failure to achieve the effect of anti-collision, thereby causing the risk of collision between unmanned aerial vehicles. The common method for generating anti-collision instructions between unmanned aerial vehicles at present is mainly based on the relative position between unmanned aerial vehicles, without considering the relative motion trend between unmanned aerial vehicles, thereby leading to unsuitable anti-collision instructions and affecting the control effect of anti-collision between unmanned aerial vehicles. SUMMARY

[0004] The present application provides a method and device for preventing collision between unmanned aerial vehicles in formation flight, aiming to solve the problem of collision prediction between unmanned aerial vehicles based only on position and speed information and the problem of anti-collision overshoot control.

[0005] In the first aspect, the present application provides a method for preventing collision between unmanned aerial vehicles in formation flight, which comprises the following steps:

[0006] S10, obtaining the collision flag CA_Flag of other unmanned aerial vehicles relative to the unmanned aerial vehicle by a collision prediction module. When CA_Flag = 1, it indicates that the other unmanned aerial vehicle has a collision conflict with the unmanned aerial vehicle. When CA_Flag ≠ 1, it indicates that the other unmanned aerial vehicle has no collision conflict and collision with the unmanned aerial vehicle.

[0007] S20, obtaining the forward position P x , the lateral position P y , and the vertical position P z of the other unmanned aerial vehicle relative to the unmanned aerial vehicle; x , the lateral position P y , and the vertical position P z of the other unmanned aerial vehicle relative to the unmanned aerial vehicle; x , the lateral speed V y , and the vertical speed V z of the other unmanned aerial vehicle relative to the unmanned aerial vehicle;

[0008] S30, generating a virtual attractive and repulsive force according to the forward position P x , the lateral position P y , and the vertical position P z of the other unmanned aerial vehicle relative to the unmanned aerial vehicle;

[0009] S40, performing normalization processing on the virtual attractive and repulsive force;

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

[0011] S60, project the virtual repulsive force F on the X axis, Y axis and Z axis respectively to obtain the front 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 front maximum anti-collision boundary distance X max , the lateral maximum anti-collision boundary distance Y max , the vertical maximum anti-collision boundary distance Z max , calculate the three-axis force normalization coefficient:

[0014]

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

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

[0017] S10, according to the three-axis force normalization coefficient, the three-axis route tracking adjustment coefficient and the three-axis anti-collision speed adjustment coefficient, adopt the control mode of the unmanned aerial vehicle when taking anti-collision control, generate the front anti-collision control instruction ΔX c , the lateral anti-collision control instruction ΔY c and the vertical anti-collision control instruction ΔZ c ;

[0018] S11, limit the front anti-collision control instruction ΔX c , the lateral anti-collision control instruction ΔY c and the vertical anti-collision control instruction ΔZ c according to the corresponding control instruction limit, and then output and superimpose the control law instruction of the corresponding mode to realize the anti-collision control of the unmanned aerial vehicle.

[0019] Further, S20 includes:

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

[0021] S202, establish the heading coordinate system of the aircraft, take the center of gravity of the aircraft as the coordinate origin, take the current speed direction of the aircraft as the x axis, take the vertical direction upward as the z axis, and determine the y axis according to the left-hand rule;

[0022] S203, the relative position and the relative speed of the other machine are converted to the local heading coordinate system through coordinate conversion to obtain the front position P of the other relative to the local x , the lateral position P y , the vertical position P z , the front speed V x , the lateral speed V y , the vertical speed V z .

[0023] Further, S30 includes:

[0024] S301, the formation unit longitudinal spacing distance Formation_Z, the formation unit front spacing distance Formation_X, and the formation unit lateral spacing distance Formation_Y are obtained from the formation performance library, and the formation unit spacing distance L is obtained through the following formula max :

[0025]

[0026] S302, when CA_Flag=0, the virtual attractive and repulsive force F is 0; when CA_Flag=1, the virtual attractive and repulsive force is generated according to the following formula:

[0027]

[0028] Wherein, K min is the set virtual attractive and repulsive force coefficient, and K is limited to [1L max ] in the calculation.

[0029] Further, S40 includes:

[0030] S401, the front maximum anti-collision boundary distance X max , the lateral maximum anti-collision boundary distance Y max , and the vertical maximum anti-collision boundary distance Z max are obtained from the collision prediction module.

[0031] S402, according to the front maximum anti-collision boundary distance X max , the lateral maximum anti-collision boundary distance Y max , and the vertical maximum anti-collision boundary distance Z max , the normalized parameter L min is calculated:

[0032]

[0033] S403, according to the normalized parameter L min , the normalized virtual attractive and repulsive force F'(i) is obtained according to the following formula:

[0034]

[0035] wherein K min is the virtual repulsive force gain set.

[0036] Further, S50 includes:

[0037] S501, calculate the relative position P x of the other aircraft to the host aircraft in the host aircraft's coordinate system: x ;

[0038] S502, calculate the yaw angle ψ of the other aircraft in the host aircraft's coordinate system according to the forward position P x , the lateral position P y , the vertical position P z , and P x :

[0039]

[0040] wherein p is the ratio of the circumference of a circle to its diameter;

[0041] S503, calculate the pitch angle θ of the other aircraft in the host aircraft's coordinate system according to the forward position P x , the lateral position P y , the vertical position P z :

[0042]

[0043] Further, in S70:

[0044] limit |Px| to be within [1, Formation_X] when calculating;

[0045] limit |Py| to be within [1, Formation_Y] when calculating;

[0046] limit |Pz| to be within [1, Formation_Z] when calculating.

[0047] Further, S80 includes:

[0048] S801, obtain the host position signal from the position sensor, and calculate the lateral deviation Dz and the height deviation dH by comparing with the host intended flight path:

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

[0050]

[0051] wherein K Fx0The forward repulsion base adjustment coefficient is set according to the selected forward control mode; K Fx_les K is the forward route adjustment coefficient. Fx_les The setting range is (0,1); ΔX is the allowable forward error range for the UAV in lateral formation;

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

[0053]

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

[0055] S804, Set the vertical flight path tracking adjustment coefficient K Fz :

[0056]

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

[0058] Furthermore, the S90 includes:

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

[0060]

[0061] Among them, K Vx_add K is the forward velocity approximation coefficient. Vx_les K is the forward velocity deviation coefficient. Vx_les The setting range is [0,1);

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

[0063]

[0064] Among them, K Vy_add K is the lateral velocity approximation coefficient. Vy_les K is the lateral velocity deviation coefficient. Vy_lesthe setting range of K is [0, 1) ;

[0065] S903, setting a vertical anti-collision speed adjustment coefficient K Vz :

[0066]

[0067] wherein, K Vz_add is a vertical speed approaching coefficient, K Vz_les is a vertical speed away coefficient, K Vz_les the setting range of K is [0, 1).

[0068] Further, S10 comprises:

[0069] S1002, generating a front anti-collision control instruction ΔX c : ΔX c = F x · K x1 · K Fx · K Vx ;

[0070] S1003, generating a lateral anti-collision control instruction ΔY c : ΔY c = F y · K y1 · K Fy · K Vy ;

[0071] S1004, generating a vertical anti-collision control instruction ΔZ c : ΔZ c = F z · K z1 · K Fz · K Vz .

[0072] In a second aspect, the application provides an inter-aircraft anti-collision control device in the formation flight of unmanned aerial vehicles, which is used to implement the above-mentioned inter-aircraft anti-collision control method in the formation flight of unmanned aerial vehicles.

[0073] In summary, the application provides an inter-aircraft anti-collision control method and device in the formation flight of unmanned aerial vehicles, which generates a three-axis normalized anti-collision instruction of unmanned aerial vehicles containing a flight path tracking adjustment coefficient and an anti-collision speed adjustment coefficient, so as to improve the inter-aircraft anti-collision control effect of unmanned aerial vehicles. The instruction randomly changes the relative position, relative speed and target flight path, and the generated anti-collision control instruction can realize inter-aircraft anti-collision control without affecting the normal flight path of unmanned aerial vehicles. BRIEF DESCRIPTION OF DRAWINGS

[0074] Figure 1 The inter-aircraft anti-collision control effect diagram provided for Example 1;

[0075] Figure 2 The schematic diagram of the inter-vehicle collision avoidance control effect provided for example 2. DETAILED DESCRIPTION

[0076] With reference to the drawings, the present application provides an inter-vehicle collision avoidance control method in unmanned vehicle formation flight, comprising:

[0077] S10, obtaining a collision flag CA_Flag of other unmanned vehicles relative to the vehicle through a collision prediction module, when CA_Flag=1, indicating that the other vehicles and the vehicle exist collision conflict, when CA_Flag≠1, indicating that the other vehicles and the vehicle do not exist collision conflict and collision.

[0078] S20, obtaining a forward position P x , a lateral position P y , a vertical position P z , a forward speed V x , a lateral speed V y , and a vertical speed V z of the other vehicles relative to the vehicle.

[0079] Specifically, S20 comprises:

[0080] S201, obtaining the relative position and relative speed of the other vehicles from the inter-vehicle link.

[0081] S202, establishing a vehicle heading coordinate system, taking the center of gravity of the vehicle as the coordinate origin, taking the current speed direction of the vehicle as the x axis, taking the vertical direction of the ground as the z axis, and determining the y axis according to the left-hand rule.

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

[0083] S30, generating a virtual repulsive force according to the forward position P x , the lateral position P y , and the vertical position P z of the other vehicles relative to the vehicle.

[0084] Specifically, S30 comprises:

[0085] S301, obtain a formation unit longitudinal spacing distance Formation_Z, a formation unit forward spacing distance Formation_X, and a formation unit lateral spacing distance Formation_Y from a formation performance library, and obtain a formation unit spacing distance L by the following formula max :

[0086]

[0087] S302, when CA_Flag=0, generate a virtual attractive force F=0; when CA_Flag=1, generate a virtual attractive force according to the following formula:

[0088]

[0089] wherein K min is a set virtual attractive force coefficient, and in calculation, K is limited in [1L max ];

[0090] S40, normalize the virtual attractive force;

[0091] Specifically, S40 includes:

[0092] S401, obtain a forward maximum anti-collision boundary distance X max , a lateral maximum anti-collision boundary distance Y max , and a vertical maximum anti-collision boundary distance Z max from a collision prediction module.

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

[0094]

[0095] S403, obtain a normalized virtual attractive force F'(i) according to the normalization parameter L min , according to the following formula:

[0096]

[0097] wherein K min is a set virtual attractive force gain.

[0098] S50, obtain a forward position P x , a lateral position P y , and a vertical position P z of a relative heading of another aircraft relative to a host aircraft., calculate the pitch angle θ and the yaw angle ψ of the other aircraft in the local heading coordinate system:

[0099] Specifically, S50 includes:

[0100] S501, P x After the protection of 0, P x ';

[0101] S502, according to the front position P x , the lateral position P y , the vertical position P z and P x of the other aircraft relative to the local heading, calculate the yaw angle ψ of the other aircraft in the local heading coordinate system:

[0102]

[0103] Where, pi is the circular ratio.

[0104] S503, according to the front position P x , the lateral position P y , the vertical position P z of the other aircraft relative to the local heading, calculate the pitch angle θ of the other aircraft in the local heading coordinate system:

[0105]

[0106] S60, project the virtual repulsive force F on the X axis, Y axis and Z axis respectively to get the front 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 front anti-collision boundary distance X max , the maximum lateral anti-collision boundary distance Y max , the maximum vertical anti-collision boundary distance Z max , calculate the three-axis force normalization coefficient:

[0109]

[0110] Where, |Px| is limited to [1, Formation_X] when calculating.

[0111] |Py| is limited to [1, Formation_Y] when calculating.

[0112] |Pz| is limited to [1, Formation_Z] when calculating.

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

[0114] Specifically, S80 includes:

[0115] S801, obtaining the local position signal from the position sensor, and calculating the lateral deviation distance Dz and the height deviation dH by comparing the local flight path;

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

[0117]

[0118] wherein K Fx0 is the set forward repulsion basic adjustment coefficient, which is set according to the selected forward control mode; K Fx_les is the forward route adjustment coefficient, and the setting range of K Fx_les is (0, 1); ΔX is the allowed forward error range of the unmanned aerial vehicle when in lateral formation.

[0119] S803, setting the lateral route tracking adjustment coefficient K Fy :

[0120]

[0121] wherein K Fy0 is the set lateral basic adjustment coefficient, which is set according to the selected lateral control mode; K Fy_les is the lateral route adjustment coefficient, and the setting range of K Fy0_les is (0, 1); ΔDz is the lateral deviation distance control error range.

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

[0123]

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

[0125] S90, setting the three-axis anti-collision speed adjustment coefficient:

[0126] Specifically, S90 includes:

[0127] S901, setting the forward anti-collision speed adjustment coefficient K Vx :

[0128]

[0129] wherein, K Vx_add is a forward speed approaching coefficient, K Vx_les is a forward speed distancing coefficient, K Vx_les is set in the range of [0, 1);

[0130] S902, set a lateral anti-collision speed adjustment coefficient K Vy :

[0131]

[0132] wherein, K Vy_add is a lateral speed approaching coefficient, K Vy_les is a lateral speed distancing coefficient, K Vy_les is set in the range of [0, 1);

[0133] S903, set a vertical anti-collision speed adjustment coefficient K Vz :

[0134]

[0135] wherein, K Vz_add is a vertical speed approaching coefficient, K Vz_les is a vertical speed distancing coefficient, K Vz_les is set in the range of [0, 1).

[0136] S10, according to the three-axis force normalization coefficient, the three-axis flight path tracking adjustment coefficient and the three-axis anti-collision speed adjustment coefficient, the control mode adopted by the unmanned aerial vehicle when the anti-collision control is adopted, a forward anti-collision control instruction ΔX c , a lateral anti-collision control instruction ΔY c and a vertical anti-collision control instruction ΔZ c are generated.

[0137] Specifically, S10 includes:

[0138] S1002, generate a forward anti-collision control instruction ΔX c : ΔX c = F x · K x1 · K Fx · K Vx ;

[0139] S1003, generate a lateral anti-collision control instruction ΔY c : ΔY c = F y · K y1 · K Fy · K Vy ;

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

[0141] S11, limit the front collision avoidance control command ΔX c , the lateral collision avoidance control command ΔY c and the vertical collision avoidance control command ΔZ c , after limiting according to the corresponding control command limit, output, and superimpose the control law command of the corresponding mode to carry out unmanned aerial vehicle collision avoidance control.

[0142] Embodiment 1

[0143] S101, obtain the collision flag CA_Flag of other unmanned aerial vehicles relative to the host machine through the collision prediction module, when CA_Flag = 1, it indicates that the other machine and the host machine exist collision conflict, when CA_Flag ≠ 1, it indicates that the other machine and the host machine do not exist collision conflict and collision.

[0144] S102, get the position and speed of the other machine relative to the host machine heading:

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

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

[0147] S102C, convert the relative position and relative speed of the other machine to the host machine heading coordinate system through coordinate conversion, to get the front position P x , the lateral position P y , the vertical position P z , the front speed V x , the lateral speed V y , the vertical speed V z of the other machine relative to the host machine.

[0148] S103, generate virtual attractive and repulsive force:

[0149] S103A, obtain the formation unit longitudinal spacing distance Formation_Z = 8m, the formation unit front spacing distance Formation_X = 20m, and the formation unit lateral spacing distance Formation_Y = 15m from the formation performance library, and get the formation unit spacing distance L max :

[0150]

[0151] S103B, when CA_Flag=0, the virtual attractive force F is generated as 0; when CA_Flag=1, the virtual attractive force is generated according to the following formula:

[0152]

[0153] wherein, the virtual attractive force coefficient K min is set as 81, and in calculation, is limited in [1L max ];

[0154] S104, the virtual attractive force is normalized:

[0155] S104A, the maximum anti-collision boundary distance of the front, side and vertical is obtained from the collision prediction module as X max =5.6m, Y max =5.6m, Z max =3m respectively;

[0156] S104B, the normalization parameter L min is calculated:

[0157]

[0158] S104C, the normalized virtual attractive force F'(i) is obtained according to the following formula:

[0159]

[0160] wherein, K min is the set virtual attractive force gain.

[0161] S105, the construction θ, ψ is calculated:

[0162] S105A, P x is divided by 0 protection to obtain P x ';

[0163] S105B, the yawing azimuth angle ψ of the other machine in the local heading coordinate system is calculated:

[0164]

[0165] wherein, pi is the circular constant.

[0166] S105C, the pitching azimuth angle θ of the other machine in the local heading coordinate system is calculated:

[0167]

[0168] S106, project the virtual repulsive force F on the X axis, Y axis and Z axis respectively to obtain the front 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 three-axis component normalization coefficient:

[0171]

[0172] Limit |Px| to [1, Formation_X] when calculating. Limit |Py| to [1, Formation_Y] when calculating. Limit |Pz| to [1, Formation_Z] when calculating.

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

[0174] S108A, obtain the local position signal from the position sensor, and calculate the local side deviation Dz and the height deviation dH by comparing with the local flight route;

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

[0176]

[0177] Wherein, the set front repulsive force basic adjustment coefficient K Fx0 is 0.74; the front route adjustment coefficient K Fx_les is 0.25; the allowable front error range ΔX of the unmanned aerial vehicle in the lateral formation is 3m.

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

[0179]

[0180] Wherein, the set lateral basic adjustment coefficient K Fy0 is 24; the 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] Wherein, the set vertical basic adjustment coefficient K Fz0 is 12.2, and the vertical route adjustment coefficient K Fz_lesis 0.25, and the height control error range AH is 3 m.

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

[0185] S109A, set the forward anti-collision speed adjustment coefficient K Vx :

[0186]

[0187] wherein the forward speed approaching coefficient K Vx_add is 4, the forward speed away coefficient K Vx_les is 0.1;

[0188] S109B, set the lateral anti-collision speed adjustment coefficient K Vy :

[0189]

[0190] wherein the lateral speed approaching coefficient K Vy_add is 1, the lateral speed away coefficient K Vy_les is 0.01;

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

[0192]

[0193] wherein the vertical speed approaching coefficient K Vz_add is 10, the vertical speed away coefficient K Vz_les is 0.1.

[0194] S110, generate a three-axis anti-collision compensation control instruction:

[0195] S110A, according to the control mode adopted by the unmanned aerial vehicle when the anti-collision control is taken, which is speed control, height control, and tilt angle control, respectively, generate anti-collision control compensation instructions AX c , AY c , and AZ c , respectively.

[0196] S110B, generate a forward anti-collision control instruction AX c :

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

[0198] S110B, generate a lateral anti-collision control instruction AYc :

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

[0200] S110C, generating vertical collision avoidance control instruction ΔZ c :

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

[0202] S111, limiting ΔX c in [-5m / s, 10m / s] to obtain speed compensation control instruction ΔV c ; limiting ΔY c in [-30o, 30o] to obtain tilt angle compensation control instruction ΔPhi c , limiting ΔZ c in [-4m, 4m] to obtain height control compensation instruction ΔH c , ΔV c , ΔPhi c , ΔH c Output and superimpose the control law instruction of the corresponding mode to carry out unmanned aerial vehicle collision avoidance control.

[0203] Embodiment 2

[0204] S101, obtaining the collision flag CA_Flag of other unmanned aerial vehicles relative to the host machine through the collision prediction module, when CA_Flag = 1, indicating that the other machine has collision conflict with the host machine, when CA_Flag ≠ 1, indicating that the other machine has no collision conflict and collision with the host machine.

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

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

[0207] S102B, establishing the host machine heading coordinate system, taking the center of gravity of the host machine as the coordinate origin, taking the current speed direction of the host machine as the x axis, taking the vertical to the horizontal ground as the z axis, and determining the y axis by the left hand rule.

[0208] S102C, converting the relative position and relative speed of the other machine to the host machine heading coordinate system through coordinate transformation to obtain the front position P x , the lateral position P y , and the vertical position Pz forward speed V x lateral speed V y vertical speed V z .

[0209] S103, generating virtual repulsive force:

[0210] S103A, obtaining formation unit longitudinal spacing distance Formation_Z = 10m, formation unit forward spacing distance Formation_X = 30m, and formation unit lateral spacing distance Formation_Y = 15m from the formation performance library, and obtaining the formation unit spacing distance L by the following formula: max

[0211]

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

[0213]

[0214] wherein the virtual repulsive force coefficient K min is set to 81, and in the calculation, is limited to [1L max ];

[0215] S104, normalizing the virtual repulsive force:

[0216] S104A, obtaining the maximum anti-collision boundary distance in the forward, lateral and vertical directions from the collision prediction module as X max = 5.6m, Y max = 5.6m, and Z max = 3m;

[0217] S104B, calculating the normalization parameter L min :

[0218]

[0219] S104C, obtaining the virtual repulsive force F'(i) after normalization according to the following formula:

[0220]

[0221] wherein K min is the virtual repulsive force gain set.

[0222] S105, calculating the construction θ, ψ:

[0223] S105A, setting P x ​After removing the 0 protection, P is obtained x ;

[0224] S105B, calculate the yaw angle ψ of the other machine in the local heading coordinate system:

[0225]

[0226] Where, pi is the circular constant.

[0227] S105C, calculate the pitch angle θ of the other machine in the local heading coordinate system:

[0228]

[0229] S106, project the virtual repulsive force F on the X axis, Y axis and Z axis respectively to obtain the front 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 three-axis force normalization coefficient:

[0232]

[0233] When calculating, limit |Px| to [1, Formation_X]. When calculating, limit |Py| to [1, Formation_Y]. When calculating, limit |Pz| to [1, Formation_Z].

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

[0235] S108A, obtain the local position signal from the position sensor, compare it with the local flight route, and calculate the lateral deviation Dz and the height deviation dH;

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

[0237]

[0238] Where, the set front repulsive force basic adjustment coefficient K Fx0 is 0.9; the front route adjustment coefficient K Fx_les is 0.2; the allowable front error range ΔX of the unmanned aerial vehicle in the lateral formation is 3m.

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

[0240]

[0241] wherein a lateral basic adjustment coefficient K Fy0 is set to 22; a lateral path adjustment coefficient K Fy_les is set to 0.6; a side slip distance control error range ΔDz is 3 m.

[0242] S108D, a vertical path tracking adjustment coefficient K Fz is set.

[0243]

[0244] wherein a vertical basic adjustment coefficient K Fz0 is set to 14; a vertical path adjustment coefficient K Fz_les is set to 0.2; a height control error range ΔH is 3 m.

[0245] S109, a three-axis anti-collision speed adjustment coefficient K

[0246] S109A, a front anti-collision speed adjustment coefficient K Vx is set.

[0247]

[0248] wherein a front speed approaching coefficient K Vx_add is set to 2; a front speed away coefficient K Vx_les is set to 0.01.

[0249] S109B, a lateral anti-collision speed adjustment coefficient K Vy is set.

[0250]

[0251] wherein a lateral speed approaching coefficient K Vy_add is set to 1.5; a lateral speed away coefficient K Vy_les is set to 0.1.

[0252] S109C, a vertical anti-collision speed adjustment coefficient K Vz is set.

[0253]

[0254] wherein a vertical speed approaching coefficient K Vz_add is set to 8; a vertical speed away coefficient K Vz_les is set to 0.3.

[0255] S110, a three-axis anti-collision compensation control instruction is generated.

[0256] S110A, the control modes adopted by the unmanned aerial vehicle when taking collision avoidance control are speed control, height control, and tilt angle control, respectively, and collision avoidance control compensation instructions ΔX c , ΔY c , and ΔZ c are generated for each control mode, respectively.

[0257] S110B, forward collision avoidance control instruction ΔX c is generated.

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

[0259] S110B, side collision avoidance control instruction ΔY c is generated.

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

[0261] S110C, vertical collision avoidance control instruction ΔZ c is generated.

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

[0263] S111, ΔX c is limited to [-3m / s, 8m / s] to obtain speed compensation control instruction ΔV c ; ΔY c is limited to [-25o, 25o] to obtain tilt angle compensation control instruction ΔPhi c ; ΔZ c is limited to [-4m, 4m] to obtain height control compensation instruction ΔH c ; ΔV c , ΔPhi c , and ΔH c are output and superimposed with the control law instructions of the corresponding modes to perform unmanned aerial vehicle collision avoidance control.

Claims

1. A method for inter-drone collision avoidance control during UAV formation flight, characterized in that the method... Comprise: S10, obtain the collision flag CA_Flag of other unmanned aerial vehicles relative to the local machine through the collision prediction module, when CA_Flag=1, it indicates that the other machine and the local machine exist collision conflict, when CA_Flag 1, it indicates that the other machine and the local machine do not exist collision conflict and collision; S20, obtaining a forward position of the other aircraft relative to the heading of the own aircraft , a lateral position , a vertical position , a forward speed , a lateral speed , a vertical speed ; S30, generating a virtual repulsive force based on his relative forward position to the native lateral position and vertical position ​ S40, normalize the virtual repulsive force; S50, forward position of the other aircraft relative to the ownship heading , lateral position , vertical position , calculate the pitch azimuth of the other aircraft in the ownship heading coordinate system , yaw azimuth ; S60, the virtual repulsive force respectively projected on the X-axis, Y-axis, Z-axis to obtain forward anti-collision repulsive force , side anti-collision repulsive force , vertical anti-collision repulsive force : ； S70、according to the forward maximum anticollision boundary distance , the lateral maximum anticollision boundary distance , the vertical maximum anticollision boundary distance , calculating a three-axis component normalization coefficient: S80, set three-axis route tracking adjustment coefficient; S90, set three-axis anti-collision speed adjustment coefficient; S11. Based on the three-axis force normalization coefficient, the three-axis path tracking adjustment coefficient, and the three-axis collision avoidance speed adjustment coefficient, and considering the control mode adopted by the UAV during collision avoidance control, generate a forward collision avoidance control command. Lateral collision avoidance control command and vertical collision avoidance control commands ; S12, the forward collision control instruction , the lateral collision control instruction and the vertical collision control instruction , the corresponding control instruction limit is limited, and the control law instruction of the corresponding mode is superimposed to control the unmanned aerial vehicle collision.

2. The inter-aircraft collision avoidance control method of claim 1, wherein S20 comprises: S201, obtain the relative position and relative speed of the other machine from the inter-machine link; S202, establish the local heading coordinate system, taking the local center of gravity as the coordinate origin, the current speed direction of the local machine as the x-axis, the vertical horizontal ground upward as the z-axis, and the left-hand rule to determine the y-axis; S203, convert the relative position and velocity of the other aircraft to the local heading coordinate system through coordinate transformation to obtain the forward position of the other aircraft relative to the local heading , lateral position , vertical position , forward velocity , lateral velocity , vertical velocity .

3. The inter-vehicle collision avoidance control method according to claim 1, characterized by, S30 comprises: S301, obtaining a platoon unit longitudinal spacing distance from the platoon performance library , a platoon unit forward spacing distance , a platoon unit lateral spacing distance , the platoon unit spacing distance is obtained by : S302, when CA_Flag=0, generate virtual repulsive force F as 0; when CA_Flag=1, generate virtual repulsive force according to the following formula: wherein, is the set virtual repulsive force coefficient, which is limited in the calculation to .​ 4. The inter-aircraft collision avoidance control method of claim 1, wherein S40 comprises: S401、get the forward maximum anti-collision boundary distance from the collision prediction module , the lateral maximum anti-collision boundary distance , the vertical maximum anti-collision boundary distance ; S402、According to the forward maximum anti-collision boundary distance , Lateral maximum anti-collision boundary distance , Vertical maximum anti-collision boundary distance , Calculate the normalization parameter : ； S403、According to the normalized parameter The normalized virtual repulsive force is obtained according to the following formula : wherein, is the virtual repulsive force gain set.

5. The inter-vehicle collision avoidance control method according to claim 1, wherein, S50 comprises: S501, will After performing division by zero protection, the result is obtained ; S502, calculate the forward position of the other machine relative to the local heading of the machine , the lateral position , the vertical position and , calculate the yaw angle of the other machine in the local heading coordinate system of the machine : wherein is the ratio of the circumference of a circle to its diameter; S503, calculate the forward position of the other machine relative to the heading of the local machine , lateral position , vertical position , calculate the pitch azimuth angle of the other machine in the heading coordinate system of the local machine : 。 6. The inter-vehicle collision avoidance control method according to claim 1, characterized by, In S70: At the time of calculation is limited to [1, ] At the time of calculation is limited to [1, ] In computing, the limit is restricted to [1, ].

7. The inter-vehicle collision avoidance control method according to claim 1, wherein S80 comprises: S801, obtaining the local position signal from the position sensor, and calculating the local lateral deviation distance by comparing the local position signal with the local intended flight path and height deviation ; S802, set forward track-while-look adjustment coefficient : wherein, is a set forward repulsion base adjustment coefficient, set according to the selected forward control mode; is a forward path adjustment coefficient, is set in the range of ; is an allowed forward error range for the UAV when in lateral formation. S803, set a lateral track keeping adjustment coefficient : wherein, is a set lateral base regulation coefficient, set according to the lateral control mode used; is a lateral track regulation coefficient, is set in the range ; is a side slip distance control error range; S804, set the vertical track-while-navigation adjustment coefficient : wherein, is a set vertical base adjustment factor, set according to the vertical control mode selected; is a vertical track adjustment factor, is set in the range ; is a height control error range.

8. The inter-vehicle collision avoidance control method according to claim 1, characterized by, S90 comprises: In step S901, the forward collision avoidance speed adjustment coefficient is set. : wherein, is a forward speed approach coefficient, is a forward speed departure coefficient, is set in the range of ; S902, set the lateral collision avoidance speed adjustment coefficient : wherein, is a lateral velocity approach coefficient, is a lateral velocity departure coefficient, is set in the range of ; In S903, the vertical anti-collision speed adjustment coefficient is set. : wherein, is a vertical velocity approach coefficient, is a vertical velocity departure coefficient, is set in the range .

9. The inter-vehicle collision avoidance control method according to claim 1, characterized by, S10 comprises: S1002, generate forward collision avoidance control instruction : ; S1003, generate lateral collision avoidance control command : ; S1004, generate vertical collision avoidance control instruction : .

10. An inter-vehicle collision avoidance control device in formation of unmanned aerial vehicles, characterized by, The inter-machine anti-collision control device is used to realize the inter-machine anti-collision control method of claim 1 in the formation flight of unmanned aerial vehicles.

Images