A formation control method based on path compensation to eliminate forward deviation

Abstract

The present invention provides a formation control method based on route compensation to eliminate forward deviation, which mainly solves the problem that a stable formation cannot be formed quickly when the initial spatial position difference between the slave aircraft and the leading aircraft is large or the speed regulation capability of the slave aircraft is insufficient. The method calculates the minimum turning radius according to the current speed of the slave aircraft, and performs route planning by combining the turning radius with the forward deviation and the original route. The planned new route can effectively eliminate the forward deviation between the slave aircraft and the leading aircraft, so that the slave aircraft can be exactly in the expected spatial position in its formation configuration after completing the circumnavigation along the new route, meeting the requirement of quickly realizing the formation configuration.

Description

Technical Field

[0001] The present invention belongs to the field of unmanned aerial vehicle swarm systems, and in particular relates to a formation control method for a swarm system based on route compensation to eliminate forward deviations. Background Art

[0002] The design concept of drone swarm is to use a large number of drones to integrate under an open system architecture, take the communication between different drones as the basis, and use the power of cluster collaboration to complete specific tasks, and finally achieve the effect of one plus one is greater than two. When performing tasks, drone swarms often need to form different specific formations for different tasks to improve the efficiency of task completion and ensure the final result of task execution. The traditional formation control method uses the information of the leader in the formation as a benchmark. The leader broadcasts its own position, speed and other information in the cluster. After receiving the information, the slave adjusts its own speed and heading according to the information to finally form a stable formation. The traditional formation control method relies too much on the speed regulation ability of the slave itself. When the initial spatial position difference between the slave and the leader is large or the speed regulation ability of the slave is insufficient, a stable formation cannot be formed quickly. Therefore, it is urgent to design a method to re-plan the route of the slave, and solve the problem of rapid formation formation by flying around. It has broad application prospects in drone swarm systems. Summary of the invention

[0003] In order to overcome the shortcomings of the prior art and solve the problem that a stable formation cannot be formed quickly when the initial spatial position difference between the follower aircraft and the leader aircraft is large or the speed regulation capability of the follower aircraft is insufficient, the present invention provides a formation control method based on path compensation to eliminate forward deviation.

[0004] The present invention provides a formation control method for eliminating forward deviation based on route compensation. When the leading aircraft is flying at a fixed airspeed, the following aircraft calculates its expected plane position according to the plane position of the leading aircraft combined with the current route and the expected formation of the formation, and calculates the forward deviation based on its current plane position and the expected position. After that, the following aircraft will judge how to fly around to eliminate the forward deviation based on the size of the forward deviation, its current flight speed and other information, thereby autonomously planning different flying routes. When the following aircraft flies along the planned route and completes the flying around, the additional flight distance generated by compensating the route will just offset the forward deviation between the following aircraft and the expected position, thereby achieving fast and efficient formation formation.

[0005] A formation control method for eliminating forward deviation based on route compensation comprises the following steps:

[0006] Step 1: Define the north-east coordinate system, obtain the expected spatial position and forward deviation of the slave aircraft, and calculate the turning radius of the slave aircraft;

[0007] Define the North-East coordinate system: the origin o is the projection of the slave on the ground, the ox0 axis points to the north as positive; the oy0 axis is in the plumb plane containing the ox0 axis, perpendicular to the ox0 axis, and upward as positive; the oz0 axis is perpendicular to the plane formed by ox0y0, forming a right-handed coordinate system;

[0008] The slave calculates its expected spatial position and the forward deviation x of the slave according to the position of the leader. dis ;

[0009] Calculate the turning radius and forward deviation of the slave aircraft;

[0010] According to the current flight speed V and 30° roll angle of the slave aircraft, calculate the corresponding turning radius R of the slave aircraft 30 :

[0011]

[0012] Step 2: If the judgment conditions of the large circle mode are met at the same time, enter the large circle mode; if any one of the judgment conditions of the large circle mode is not met, enter step 3;

[0013] The judgment conditions for the big circle mode are:

[0014] 2.A: If the forward deviation is greater than the minimum turning radius R 30 The flight length L when circling a circle;

[0015] 2.B: The distance HLD between the current plane position of the slave and the next waypoint dis Greater than or equal to the minimum turning radius R 30 ;

[0016] Where L is the minimum turning radius R 30 The flight length when flying around a circle is the circumference of the circle when flying at a 30° roll angle throughout the flight. The value of L is 2*π*R 30 ;

[0017] The method of going around the big circle mode is:

[0018] When the slave aircraft determines that the conditions for the large circle mode are met, it will add four waypoints on the route based on the current plane position and the current route. After the slave aircraft flies around along the newly added waypoints, the track will form a large circle route track. After the slave aircraft finishes flying around, it will be at the position when it started flying around. Therefore, the newly added route length will be completely used to eliminate the forward deviation, so as to achieve the purpose of quickly eliminating the large forward deviation and forming a stable formation.

[0019] The large circle route track includes a rectangle and two arcs; the arcs are arranged outside the short sides of the rectangle; the midpoints of the two short sides of the rectangle are the centers of the arcs; the central angles of the two arcs are 180 degrees; the length of the short side of the rectangle is equal to the diameter of the arc; the closed track formed by the two long sides of the rectangle and the arc is the large circle route track;

[0020] Step 3: If all the judgment conditions of the circling mode are met, enter the circling mode; if any of the judgment conditions of the circling mode is not met, enter step 4;

[0021] The judgment conditions for the circle mode are:

[0022] 3.A: The forward deviation of the slave is greater than L-4×R 30 ;

[0023] 3.B: The distance HLD between the current plane position of the slave and the next waypoint dis Greater than 4×R 30 ;

[0024] The method of the mid-circle mode is as follows: the mid-circle flight will move forward along the route direction to form a mid-circle route trajectory; since there is a forward distance along the route, a small forward deviation of less than four times the turning radius can be eliminated;

[0025] The orbit trajectory of the center circle includes a rectangle, a semicircular arc, a first circular arc, and a second circular arc; the end of one long side of the rectangle closest to the original track is connected to one end of the first circular arc; the end of another long side of the rectangle closest to the original track is connected to one end of the second circular arc; the central angles corresponding to the first circular arc and the second circular arc are both 90 degrees; the centers of the first circular arc and the second circular arc are located on the extension line of the short side of the rectangle closest to the track; the ends of the two long sides of the rectangle farthest from the track are respectively connected to the two ends of the semicircular arc; the radii of the semicircular arc, the first circular arc, and the second circular arc are the same and are half the length of the short side of the rectangle;

[0026] Step 4: If all the judgment conditions of the circling small circle mode are met at the same time, the circling small circle mode is entered; if any of the judgment conditions of the circling small circle mode is not met, the circling process is not performed;

[0027] The judgment conditions for the small circle mode are:

[0028] 4.A: If the forward deviation of the slave is less than L-4*R 30 ;

[0029] 4.B: If the forward deviation of the slave is greater than 100;

[0030] 4.C: If the distance between the current slave plane position and the next waypoint is HLDdis Greater than 5×R 30 ;

[0031] The method of going around the small circle mode is:

[0032] The small circle route trajectory includes a first-stage arc, a second-stage arc and a third-stage arc; the arcs of the first-stage arc and the third-stage arc are equal, and the arc of the second-stage arc is twice the arc of the first-stage arc; the centers of the first-stage arc and the third-stage arc are on one side of the original route of the slave aircraft; the center of the second-stage arc is on the other side of the original route of the slave aircraft;

[0033] Furthermore, after compensating the turning radius of the slave, the corresponding turning radius R 30 for

[0034]

[0035] Furthermore, the specific steps of the large circle mode are:

[0036] First, calculate the length of the long side of the large circle trajectory

[0037]

[0038] Here, the division by 2 is because the route has two long sides;

[0039] The coordinates of the four waypoints in the large circle mode are calculated based on the turning radius and the length of the long side of the large circle route track. The coordinates of the four waypoints in the large circle mode are the coordinates of the first waypoint (xHLD add1 ,zHLD add1 ), the second waypoint coordinates (xHLD add2 ,zHLD add2 ), the third waypoint coordinates (xHLD add3 ,zHLD add3 ) and the fourth waypoint coordinates (xHLD add4 ,zHLD add4 );

[0040] The coordinates of the first waypoint are (xHLD add1 ,zHLD add1 ), which is the distance of a turning radius from the current plane position along the route:

[0041] xHH add1 =x+R 30 *cos(ρ0), zHLD add1 =zR 30 *sin(ρ0);

[0042] Among them, x and z represent the plane position of the current slave in the north-east coordinate system, and ρ0 represents the angle between the current slave flight direction and the north direction, with a value range of (-180, 180], with north-west being positive;

[0043] The coordinates of the second waypoint are based on the calculated coordinates of the first waypoint, and are extended to the outside of the route by twice the turning radius and lengthening the side length; first calculate the initial value ρ' of the heading between the first waypoint and the second waypoint 01 :

[0044] ρ' 01 =ρ0+90;

[0045] According to ρ' 01 range, calculate the current route direction ρ 01 ;

[0046]

[0047] Then calculate the coordinates of the second waypoint (xHLD add2 ,zHLD add2 ):

[0048]

[0049] The coordinates of the third waypoint are based on the coordinates of the second waypoint, and are extended in the opposite direction of the basic route by a distance of twice the turning radius;

[0050] Calculate the initial heading value ρ1'2 between the second waypoint and the third waypoint and the coordinates of the third waypoint (xHLD add3 ,zHLD add3 ):

[0051] ρ' 12 =ρ 01 +90

[0052] According to the initial heading value ρ' 12 , calculate the heading ρ between the second waypoint and the third waypoint 12 :

[0053]

[0054] xHH add3 =xHLD add2 +2*R 30 *cos(ρ 12 )

[0055] ZlUT add3 =zHLD add2 -2*R 30 *sin(ρ 12)

[0056] The coordinates of the fourth waypoint are based on the coordinates of the third waypoint, and the turning radius is doubled and the side length is lengthened toward the basic route; the initial heading ρ' between the third waypoint and the fourth waypoint is calculated 23 With the fourth waypoint coordinates (xHLD add4 ,zHLD add4 ):

[0057] ρ' 23 =ρ 12 +90

[0058] According to ρ' 23 Calculate the heading ρ between the third waypoint and the fourth waypoint 23 :

[0059]

[0060] At this point, the waypoints for route compensation have been calculated, and the slave aircraft is controlled to fly around the route once and then return to the original route, so that the forward deviation before the detour can be quickly eliminated.

[0061] Furthermore, the specific steps of the winding circle mode are:

[0062] First, calculate the additional distance required to fly around the center circle route, excluding four times the turning radius and the circumference of the flying circle:

[0063]

[0064] The coordinates of the four waypoints of the middle circle mode are calculated according to the turning radius and the length of the long side. The four waypoints of the middle circle mode are the coordinates of the first waypoint of the middle circle, the coordinates of the second waypoint of the middle circle, the coordinates of the third waypoint of the middle circle and the coordinates of the fourth waypoint of the middle circle.

[0065] Coordinates of the first waypoint in the middle circle The distance of a turning radius forward along the route from the aircraft's current plane position:

[0066]

[0067] in, Represents the angle between the current route direction and the north direction;

[0068] Coordinates of the second waypoint in the middle circle Based on the calculated first waypoint of the middle circle, expand the route by twice the turning radius plus the extended length; calculate the initial heading between the first waypoint of the middle circle and the second waypoint of the middle circle

[0069]

[0070] Calculate the heading between the first waypoint in the middle circle and the second waypoint in the middle circle

[0071]

[0072] Calculate the coordinates of the second waypoint in the middle circle

[0073]

[0074] The third waypoint in the middle circle is based on the second waypoint in the middle circle, and is extended along the basic route by twice the turning radius; the initial heading between the second waypoint in the middle circle and the third waypoint in the middle circle is calculated.

[0075]

[0076] Calculate the heading between the second waypoint in the middle circle and the third waypoint in the middle circle

[0077]

[0078] Calculate the coordinates of the third waypoint in the middle circle

[0079]

[0080] The fourth waypoint in the middle circle is based on the third waypoint in the middle circle, and the turning radius is doubled and the side length is lengthened toward the basic route; the initial heading between the third waypoint in the middle circle and the fourth waypoint in the middle circle is calculated.

[0081]

[0082] Calculate the heading between the third waypoint in the middle circle and the fourth waypoint in the middle circle

[0083]

[0084] Calculate the coordinates of the fourth waypoint in the middle circle

[0085]

[0086] At this point, the waypoints for route compensation have been calculated. By controlling the slave aircraft to fly around the route once and then return to the original route, the forward deviation before the detour can be quickly eliminated.

[0087] Furthermore, the specific steps of the small circle mode are:

[0088] Calculate the radius R corresponding to the first stage arc and the third stage arc:

[0089]

[0090] Among them, α is the central angle corresponding to the arc in the first stage;

[0091] The coordinates of the arc center (x o ,z o ):

[0092] dis=R 30 *sin(α)+R*sin(α);

[0093]

[0094] x o =x temp +R temp *cos(ρ temp )*cos(α);

[0095] z o =z temp -R temp *sin(ρ temp )*cos(α);

[0096] in x is the direction of the slave's current route; temp , R temp 、z temp , temp , ρ' temp All are calculated intermediate variables;

[0097] According to the coordinates of the center of the circle (x o ,z o ) calculates the corresponding control instructions to make the slave aircraft fly along the arc and eliminate the forward deviation of up to 200 meters after completing the entire three stages.

[0098] Furthermore, when the central angles corresponding to the first-stage arc and the third-stage arc are 50 degrees, the forward deviation eliminated by the small-circle route trajectory at one time is no more than 200 meters.

[0099] Furthermore, the first-stage arc and the third-stage arc are fixed roll angles, and the fixed roll angle is 30 degrees.

[0100] The beneficial effects of the present invention are:

[0101] Compared with the existing technology, this method realizes autonomous route planning of different gears by calculating the forward deviation between the slave aircraft and its expected position, and meets the demand of quickly realizing the formation configuration by flying around along the route.

[0102] Compared with the formation control method without using route compensation, the present invention can significantly shorten the formation formation time: for an initial forward deviation of 2000 meters, it takes a total of 180 seconds to form a formation using the present invention, while it takes a total of 663 seconds to form a formation without using the present invention, and the formation formation time is shortened by more than 3.6 times; for an initial forward deviation of 1200 meters, it takes a total of 198 seconds to form a formation using the present invention, while it takes a total of 459 seconds to form a formation without using the present invention, and the formation formation time is shortened by more than 2.3 times; for an initial forward deviation of 350 meters, it takes a total of 174 seconds to form a formation using the present invention, while it takes a total of 213 seconds to form a formation without using the present invention, and the formation formation time is shortened by more than 1.2 times. BRIEF DESCRIPTION OF THE DRAWINGS

[0103] Figure 1 It is the overall flow chart of the algorithm;

[0104] Figure 2 This is a schematic diagram of the large circle flying plan;

[0105] Figure 3 This is a schematic diagram of the mid-circle flyby plan;

[0106] Figure 4 This is a schematic diagram of the small circle flying plan;

[0107] Figure 5 The following is a simulation result diagram of a large circle flyby; (a) is the plane position at the beginning of the flyby; (b) is the plane position at the end of the flyby;

[0108] Figure 6 The following is a simulation result diagram of the mid-circle flyby; (a) is the plane position at the beginning of the flyby; (b) is the plane position at the end of the flyby;

[0109] Figure 7 The following is a simulation result diagram of a small circle flyby; (a) is the plane position at the beginning of the flyby; (b) is the plane position at the end of the flyby;

[0110] Figure 8 This is a comparison chart of algorithms in the case of large circle flying; (a) initial plane position and time without route compensation; (b) after formation formation without route compensation; (c) initial plane position and time with route compensation; (d) plane position and time after formation formation with route compensation;

[0111] Fig. 9 This is a comparison chart of the algorithms in the case of mid-circle circling; (a) Initial plane position and time without using route compensation;

[0112] (b) Plane position and time after formation formation without route compensation; (c) Initial plane position and time with route compensation; (d) After formation formation with route compensation;

[0113] Fig.10 This is a comparison chart of algorithms in the case of small circle flying; (a) initial plane position and time without route compensation (b) plane position and time after formation formation without route compensation; (c) initial plane position and time with route compensation; (d) plane position and time after formation formation with route compensation;

[0114] In the figure, R represents the turning radius, HLD represents the original route, C represents the long side, o represents the center of the circle, and r represents the radius of the arc. The black dot represents the follower aircraft, and the red dot represents the lead aircraft. DETAILED DESCRIPTION

[0115] A formation control method for eliminating forward deviation based on route compensation comprises the following steps:

[0116] Step 1: Define the north-east coordinate system, obtain the expected spatial position and forward deviation of the slave aircraft, and calculate the turning radius of the slave aircraft;

[0117] Define the North-East coordinate system: the origin o is the projection of the slave on the ground, the ox0 axis points to the north as positive; the oy0 axis is in the plumb plane containing the ox0 axis, perpendicular to the ox0 axis, and upward as positive; the oz0 axis is perpendicular to the plane formed by ox0y0, forming a right-handed coordinate system;

[0118] The slave calculates its own expected spatial position and the forward deviation of the slave based on the position of the leader;

[0119] Calculate the turning radius and forward deviation of the slave aircraft;

[0120] According to the current flight speed V of the slave aircraft and the expected 30° roll angle turn, calculate the corresponding turning radius R of the slave aircraft 30 :

[0121]

[0122] Compensate the turning radius of the slave aircraft according to its turning capability:

[0123] Calculate the forward deviation x of the slave d i s ;

[0124] Step 2: If the judgment conditions of the large circle mode are met at the same time, enter the large circle mode; if any one of the judgment conditions of the large circle mode is not met, enter step 3;

[0125] The judgment conditions for the big circle mode are:

[0126] 2.A: If the forward deviation is greater than the minimum turning radius R 30 The flight length L when circling a circle;

[0127] 2.B: The distance HLD between the current plane position of the slave and the next waypoint dis Greater than or equal to the minimum turning radius R 30 ;

[0128] Where L is the minimum turning radius R 30 The flight length when flying around a circle is the circumference of the circle when flying at a 30° roll angle throughout the flight. The value of L is 2*π*R 30 ;

[0129] The method of going around the big circle mode is:

[0130] When the slave aircraft determines that the conditions for the large circle mode are met, it will add four waypoints on the route based on the current plane position and the current route. After the slave aircraft flies around along the newly added waypoints, the track will form a large circle route track. After the slave aircraft finishes flying around, it will be at the position when it started flying around. Therefore, the newly added route length will be completely used to eliminate the forward deviation, so as to achieve the purpose of quickly eliminating the large forward deviation and forming a stable formation.

[0131] The large circle flight path includes a rectangle and two arcs; the arcs are arranged outside the short sides of the rectangle; the midpoints of the two short sides of the rectangle are the centers of the arcs; the central angles of the two arcs are 180 degrees; the length of the short side of the rectangle is equal to the diameter of the arc; the closed track formed by the two long sides of the rectangle and the arc is the large circle flight path; the large circle flight path is shown in the figure Figure 2 As shown;

[0132] The length of the two short sides of the large circle trajectory is 2*R 30 , which is the length produced when the slave turns 180° with the minimum turning radius; the two long sides of the large circle route track are used to eliminate the forward deviation, which is the forward deviation value that needs to be eliminated after removing the necessary four turning radii; the large circle flyby is used to eliminate large forward deviations greater than four times the turning radius;

[0133] The specific steps of the big circle mode are:

[0134] First, calculate the length of the long side of the large circle trajectory

[0135]

[0136] Here, the division by 2 is because the route has two long sides;

[0137] After that, the coordinates of the four waypoints in the large circle mode are calculated based on the turning radius and the length of the long side of the large circle route track. The coordinates of the four waypoints in the large circle mode are the coordinates of the first waypoint (xHLD add1 ,zHLD add1 ), the second waypoint coordinates (xHLD add2 ,zHLD add2 ), the third waypoint coordinates (xHLD add3 ,zHLD add3 ) and the fourth waypoint coordinates (xHLD add4 ,zHLD add4 );

[0138] The coordinates of the first waypoint are (xHLD add1 ,zHLD add1 ), which is the distance of a turning radius from the current plane position along the route:

[0139] xHH add1 =x+R 30 *cos(ρ0), zHLD add1 =zR 30 *sin(ρ0);

[0140] Among them, x and z represent the plane position of the current slave in the north-east coordinate system, and ρ0 represents the angle between the current slave flight direction and the north direction, with a value range of (-180, 180], with north-west being positive;

[0141] The coordinates of the second waypoint are based on the calculated coordinates of the first waypoint, and are extended to the outside of the route by twice the turning radius and lengthening the side length; first calculate the initial value ρ' of the heading between the first waypoint and the second waypoint 01 :

[0142] ρ' 01 =ρ0+90;

[0143] According to ρ' 01 range, calculate the current route direction ρ 01 ;

[0144]

[0145] Then calculate the coordinates of the second waypoint (xHLD add2 ,zHLD add2 ):

[0146]

[0147] The coordinates of the third waypoint are based on the coordinates of the second waypoint, and are extended in the opposite direction of the basic route by a distance of twice the turning radius;

[0148] Calculate the initial heading value ρ1'2 between the second waypoint and the third waypoint and the coordinates of the third waypoint (xHLD add3 ,zHLD add3 ):

[0149] ρ' 12 =ρ 01 +90

[0150] According to the initial heading value ρ' 12 , calculate the heading ρ between the second waypoint and the third waypoint 12 :

[0151]

[0152] xHH add3 =xHLD add2 +2*R 30 *cos(ρ 12 )

[0153] ZlUT add3 =zHLD add2 -2*R 30 *sin(ρ 12 )

[0154] The coordinates of the fourth waypoint are based on the coordinates of the third waypoint, and the turning radius is doubled and the side length is lengthened toward the basic route; the initial heading ρ' between the third waypoint and the fourth waypoint is calculated 23 With the fourth waypoint coordinates (xHLD add4 ,zHLD add4 ):

[0155] ρ' 23 =ρ 12 +90

[0156] According to ρ' 23 Calculate the heading ρ between the third waypoint and the fourth waypoint 23 :

[0157]

[0158] At this point, the waypoints for route compensation have been calculated, and the slave aircraft is controlled to fly around the route once and then return to the original route, which can quickly eliminate the forward deviation before the flight.

[0159] Step 3: If all the judgment conditions of the circling mode are met, the circling mode is entered; if any of the judgment conditions of the circling mode is not met, step 4 is entered;

[0160] The judgment conditions for the circle mode are:

[0161] 3.A: The forward deviation of the slave is greater than L-4×R 30 ,

[0162] 3.B: The distance HLD between the current plane position of the slave and the next waypoint dis Greater than 4×R 30 ;

[0163] The method of the mid-circle mode is as follows: the mid-circle flight will move forward along the route direction, forming a protrusion of an "Ω" configuration, that is, the mid-circle route trajectory; since there is a forward distance along the route, a small forward deviation of less than four times the turning radius can be eliminated;

[0164] The orbit trajectory of the center circle includes a rectangle, a semicircular arc, a first circular arc, and a second circular arc; the end of one long side of the rectangle closest to the original track is connected to one end of the first circular arc; the end of another long side of the rectangle closest to the original track is connected to one end of the second circular arc; the central angles corresponding to the first circular arc and the second circular arc are both 90 degrees; the centers of the first circular arc and the second circular arc are located on the extension line of the short side of the rectangle closest to the track; the ends of the two long sides of the rectangle farthest from the track are respectively connected to the two ends of the semicircular arc; the radii of the semicircular arc, the first circular arc, and the second circular arc are the same and are half the length of the short side of the rectangle;

[0165] The specific steps of the circle mode are:

[0166] First, calculate the additional distance required to fly around the center circle route, excluding four times the turning radius and the circumference of the flying circle:

[0167]

[0168] Afterwards, the coordinates of the four waypoints of the middle circle mode are calculated according to the turning radius and the length of the long side. The four waypoints of the middle circle mode are the coordinates of the first waypoint of the middle circle, the coordinates of the second waypoint of the middle circle, the coordinates of the third waypoint of the middle circle and the coordinates of the fourth waypoint of the middle circle;

[0169] Coordinates of the first waypoint in the middle circle The distance of a turning radius forward along the route from the aircraft's current plane position:

[0170]

[0171] in, Represents the angle between the current route direction and the north direction;

[0172] Coordinates of the second waypoint in the middle circle Based on the calculated first waypoint of the middle circle, expand the route by twice the turning radius plus the extended length; calculate the initial heading between the first waypoint of the middle circle and the second waypoint of the middle circle

[0173]

[0174] Calculate the heading between the first waypoint in the middle circle and the second waypoint in the middle circle

[0175]

[0176] Calculate the coordinates of the second waypoint in the middle circle

[0177]

[0178] The third waypoint in the middle circle is based on the second waypoint in the middle circle, and is extended along the basic route by twice the turning radius; the initial heading between the second waypoint in the middle circle and the third waypoint in the middle circle is calculated.

[0179]

[0180] Calculate the heading between the second waypoint in the middle circle and the third waypoint in the middle circle

[0181]

[0182] Calculate the coordinates of the third waypoint in the middle circle

[0183]

[0184] The fourth waypoint in the middle circle is based on the third waypoint in the middle circle, and the turning radius is doubled and the side length is lengthened toward the basic route; the initial heading between the third waypoint in the middle circle and the fourth waypoint in the middle circle is calculated.

[0185]

[0186] Calculate the heading between the third waypoint in the middle circle and the fourth waypoint in the middle circle

[0187]

[0188] Calculate the coordinates of the fourth waypoint in the middle circle

[0189]

[0190] At this point, the waypoints for route compensation have been calculated. The forward deviation before the detour can be quickly eliminated by controlling the slave aircraft to fly around the route and then return to the original route.

[0191] Step 4: If all the judgment conditions of the circling small circle mode are met at the same time, the circling small circle mode is entered; if any of the judgment conditions of the circling small circle mode is not met, the circling process is not performed;

[0192] The judgment conditions for the small circle mode are:

[0193] 4.A: If the forward deviation of the slave is less than L-4*R 30 ;

[0194] 4.B: If the forward deviation of the slave is greater than 100;

[0195] 4.C: If the distance between the current slave plane position and the next waypoint is HLD dis Greater than 5×R 30 ;

[0196] The method of going around the small circle mode is:

[0197] The small circle route trajectory includes the first stage arc, the second stage arc and the third stage arc; the arcs of the first stage arc and the third stage arc are equal, and the arc of the second stage arc is twice the arc of the first stage arc; the centers of the first stage arc and the third stage arc are on one side of the original route of the slave aircraft; the center of the second stage arc is on the other side of the original route of the slave aircraft; when the center angles of the first stage arc and the third stage arc are 50 degrees, the forward deviation eliminated by the small circle route trajectory at one time is no more than 200 meters;

[0198] The first stage arc and the third stage arc are fixed roll angle, and the fixed roll angle is 30 degrees;

[0199] At this time, the first stage of the follower's circling flight will first control the follower to turn outside the route according to a 30° roll; the second stage will calculate the radius of the second stage arc according to the forward deviation to be eliminated, and calculate the roll control amount required to form the arc according to the radius; the third stage will complete the small circle circling process by turning to the original route with a 30° roll control after the large arc circling flight is completed, and the flight distance is consistent with the first stage; due to the limitation of the route length, the forward deviation that can be eliminated in this process is limited, currently set to 200 meters, and the curvature of the arcs generated by the first and third stage turns is set to 50°;

[0200] The specific steps of the small circle mode are:

[0201] Calculate the radius R corresponding to the first stage arc and the third stage arc:

[0202]

[0203] Among them, α is the center angle of the arc corresponding to the first stage; α = 50;

[0204] The coordinates of the arc center (x o ,z o ):

[0205] dis=R 30 *sin(α)+R*sin(α);

[0206]

[0207] x o =x temp +R temp *cos(ρ temp )*cos(α);

[0208] z o =z temp -R temp *sin(ρ temp )*cos(α);

[0209] in '

[0210] x is the direction of the slave's current route; temp , R temp 、z temp , temp , temp All are calculated intermediate variables;

[0211] According to the coordinates of the center of the circle (x o ,z o ) calculates the corresponding control instructions to make the slave aircraft fly along the arc and eliminate the forward deviation of up to 200 meters after completing the entire three stages.

Claims

1. A formation control method for eliminating forward deviation based on route compensation, comprising the following steps: Step 1: Define the north-east coordinate system, obtain the expected spatial position and forward deviation of the slave aircraft, and calculate the turning radius of the slave aircraft; Define the North-East coordinate system: the origin o is the projection of the slave on the ground, the ox0 axis points to the north as positive; the oy0 axis is in the plumb plane containing the ox0 axis, perpendicular to the ox0 axis, and upward as positive; the oz0 axis is perpendicular to the plane formed by ox0y0, forming a right-handed coordinate system; The slave calculates its expected spatial position and the forward deviation x of the slave according to the position of the leader. dis ; Calculate the turning radius and forward deviation of the slave aircraft; According to the current flight speed V and 30° roll angle of the slave aircraft, calculate the corresponding turning radius R of the slave aircraft 30 : Step 2: If the judgment conditions of the large circle mode are met at the same time, enter the large circle mode; if any one of the judgment conditions of the large circle mode is not met, enter step 3; The judgment conditions for the big circle mode are: Condition 2.A: If the forward deviation is greater than the minimum turning radius R 30 The flight length L when circling a circle; Condition 2.B: The distance HLD between the current slave plane position and the next waypoint dis Greater than or equal to the minimum turning radius R 30 ; Where L is the minimum turning radius R 30 The flight length when flying around a circle is the circumference of the circle when flying at a 30° roll angle throughout the flight. The value of L is 2*π*R 30 ; The steps for the big circle mode are: When the slave aircraft determines that the conditions for the large circle mode are met, it will fly around the large circle route. After the slave aircraft finishes flying around, it will be at the position where it started flying around. The large circle route track includes a rectangle and two arcs; the arcs are arranged outside the short sides of the rectangle; the midpoints of the two short sides of the rectangle are the centers of the arcs; the central angles of the two arcs are 180 degrees; the length of the short side of the rectangle is equal to the diameter of the arc; the closed track formed by the two long sides of the rectangle and the arc is the large circle route track; Step 3: If all the judgment conditions of the circling mode are met, enter the circling mode; if any of the judgment conditions of the circling mode is not met, enter step 4; The judgment conditions for the circle mode are: Condition 3.A: The forward deviation of the slave is greater than L-4×R 30 ; Condition 3.B: The distance HLD between the current plane position of the slave and the next waypoint dis Greater than 4×R 30 ; The method of the mid-circle mode is as follows: the mid-circle flight will move along the route direction, forming a mid-circle route trajectory; The orbital trajectory of the center circle includes a rectangle, a semicircular arc, a first circular arc, and a second circular arc; the end of one long side of the rectangle closest to the original track is connected to one end of the first circular arc; the end of another long side of the rectangle closest to the original track is connected to one end of the second circular arc; the central angles corresponding to the first circular arc and the second circular arc are both 90 degrees; the centers of the first circular arc and the second circular arc are located on the extension line of the short side of the rectangle closest to the track; the ends of the two long sides of the rectangle farthest from the track are respectively connected to the two ends of the semicircular arc; the radii of the semicircular arc, the first circular arc, and the second circular arc are the same and are half the length of the short side of the rectangle; Step 4: If all the judgment conditions of the circling small circle mode are met at the same time, the circling small circle mode is entered; if any of the judgment conditions of the circling small circle mode is not met, the circling process is not performed; The judgment conditions for the small circle mode are: Condition 4.A: If the forward deviation of the slave is less than L-4*R 30 ; Condition 4.B: If the forward deviation of the slave is greater than 100; Condition 4.C: If the distance between the current slave plane position and the next waypoint is HLD dis Greater than 5×R 30 ; The method of going around the small circle mode is: The small circle route trajectory includes a first-stage arc, a second-stage arc and a third-stage arc; the arcs of the first-stage arc and the third-stage arc are equal, and the arc of the second-stage arc is twice that of the first-stage arc; the centers of the first-stage arc and the third-stage arc are on one side of the original route of the slave aircraft; the center of the second-stage arc is on the other side of the original route of the slave aircraft.

2. A formation control method for eliminating forward deviation based on route compensation according to claim 1, characterized in that: After compensating the turning radius of the slave, the corresponding turning radius R of the slave 30 for 3. A formation control method for eliminating forward deviation based on route compensation according to claim 1, characterized in that: The specific steps of the large circle mode are: First, calculate the length of the long side of the large circle trajectory Here, the division by 2 is because the route has two long sides; The coordinates of the four waypoints in the large circle mode are calculated based on the turning radius and the length of the long side of the large circle route track. The coordinates of the four waypoints in the large circle mode are the coordinates of the first waypoint (xHLD add1 ,zHLD add1 ), the second waypoint coordinates (xHLD add2 ,zHLD add2 ), the third waypoint coordinates (xHLD add3 ,zHLD add3 ) and the fourth waypoint coordinates (xHLD add4 ,zHLD add4 ); The coordinates of the first waypoint are (xHLD add1 ,zHLD add1 ), which is the distance of a turning radius from the current plane position along the route: xHLD add1 =x+R 30 *cos(ρ0),zHLD add1 =z-R 30 *sin(ρ0); Among them, x and z represent the plane position of the current slave in the north-east coordinate system, and ρ0 represents the angle between the current slave flight direction and the north direction, with a value range of (-180, 180], with north-west being positive; The coordinates of the second waypoint are based on the calculated coordinates of the first waypoint, and are extended to the outside of the route by twice the turning radius and lengthening the side length; first calculate the initial value ρ' of the heading between the first waypoint and the second waypoint 01 : r' 01 =ρ0+90; According to ρ' 01 range, calculate the current route direction ρ 01 ; Then calculate the coordinates of the second waypoint (xHLD add2 ,zHLD add2 ): The coordinates of the third waypoint are based on the coordinates of the second waypoint, and are extended in the opposite direction of the basic route by a distance of twice the turning radius; Calculate the initial heading value ρ1'2 between the second waypoint and the third waypoint and the coordinates of the third waypoint (xHLD add3 ,zHLD add3 ): r' 12 =ρ 01 +90 According to the initial heading value ρ' 12 , calculate the heading ρ between the second waypoint and the third waypoint 12 : xHLD add3 =xHLD add2 +2*R 30 *cos(ρ 12 ) zHLD add3 =zHLD add2 -2*R 30 *sin(ρ 12 ) The coordinates of the fourth waypoint are based on the coordinates of the third waypoint, and the turning radius is doubled and the side length is lengthened toward the basic route; Calculate the initial heading ρ' between the third waypoint and the fourth waypoint 23 With the fourth waypoint coordinates (xHLD add4 ,zHLD add4 ): r' 23 =ρ 12 +90 According to ρ' 23 Calculate the heading ρ between the third waypoint and the fourth waypoint 23 : At this point, the waypoints for route compensation have been calculated, and the slave aircraft is controlled to fly around the route once and then return to the original route, so that the forward deviation before the detour can be quickly eliminated.

4. The formation control method based on route compensation to eliminate forward deviation according to claim 1, characterized in that: The specific steps of the winding circle mode are: First, calculate the additional distance required to fly around the center circle route, excluding four times the turning radius and the circumference of the flying circle: The coordinates of the four waypoints of the middle circle mode are calculated according to the turning radius and the length of the long side. The four waypoints of the middle circle mode are the coordinates of the first waypoint of the middle circle, the coordinates of the second waypoint of the middle circle, the coordinates of the third waypoint of the middle circle and the coordinates of the fourth waypoint of the middle circle; Coordinates of the first waypoint in the middle circle The distance of a turning radius forward along the route from the aircraft's current plane position: in, Represents the angle between the current route direction and the north direction; Coordinates of the second waypoint in the middle circle Based on the calculated first waypoint of the middle circle, expand the route by twice the turning radius plus the extended length; calculate the initial heading between the first waypoint of the middle circle and the second waypoint of the middle circle Calculate the heading between the first waypoint in the middle circle and the second waypoint in the middle circle Calculate the coordinates of the second waypoint in the middle circle The third waypoint in the middle circle is based on the second waypoint in the middle circle, and is extended along the basic route by twice the turning radius; the initial heading between the second waypoint in the middle circle and the third waypoint in the middle circle is calculated. Calculate the heading between the second waypoint in the middle circle and the third waypoint in the middle circle Calculate the coordinates of the third waypoint in the middle circle The fourth waypoint in the middle circle is based on the third waypoint in the middle circle, and the turning radius is doubled and the side length is lengthened toward the basic route; the initial heading between the third waypoint in the middle circle and the fourth waypoint in the middle circle is calculated. Calculate the heading between the third waypoint in the middle circle and the fourth waypoint in the middle circle Calculate the coordinates of the fourth waypoint in the middle circle At this point, the waypoints for route compensation have been calculated. By controlling the slave aircraft to fly around the route once and then return to the original route, the forward deviation before the detour can be quickly eliminated.

5. The formation control method based on route compensation to eliminate forward deviation according to claim 1, characterized in that: The specific steps of the small circle mode are: Calculate the radius R corresponding to the first stage arc and the third stage arc: Among them, α is the central angle corresponding to the arc in the first stage; The coordinates of the arc center (x o ,z o ): dis=R 30 *sin(a)+R*sin(a); x o =x temp +R temp *cos(ρ temp )*cos(α); z o =z temp -R temp *sin(r temp )*cos(α); in ' x is the direction of the slave's current route; temp , R temp 、z temp , temp , temp All are calculated intermediate variables; According to the coordinates of the center of the circle (x o ,z o ) calculates the corresponding control instructions to make the slave aircraft fly along the arc and eliminate the forward deviation of up to 200 meters after completing the entire three stages.

6. A formation control method for eliminating forward deviation based on route compensation according to claim 5, characterized in that: When the central angles corresponding to the first-stage arc and the third-stage arc are 50 degrees, the forward deviation eliminated by the small-circle route trajectory at one time is no more than 200 meters.

7. A formation control method for eliminating forward deviation based on route compensation according to claim 6, characterized in that: The first stage arc and the third stage arc are fixed roll angles, and the fixed roll angle is 30 degrees.

8. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores a computer program, and when the computer program is executed, the formation control method according to any one of claims 1 to 7 is implemented.

Images