A platoon control method fusing improved artificial potential field method and navigation method

Abstract

The application discloses a kind of formation control methods of fusing improved artificial potential field method and navigation method, belong to unmanned aerial vehicle technical field, specifically: first, for each unmanned aerial vehicle to be formed, establish dynamic potential field model;The position of the given target point of long aircraft is obtained, and the position and speed of each unmanned aerial vehicle;Long aircraft calculates the target point position of each wing aircraft through its own position and heading;Then, each wing aircraft calculates the attraction of itself pointing to target point, and the repulsion of the remaining wing aircraft in its safety distance;Thus, the resultant force of each wing aircraft is obtained, and the heading is adjusted according to the direction of resultant force;When the attraction and repulsion of wing aircraft are close, decompose the resultant force, get the component force perpendicular to the direction of resultant force, transform the component force to get the new resultant force direction, and the wing aircraft obtains the new heading according to the new resultant force direction and angular velocity transformation limit;And adjust its speed, fly along the new heading.The application avoids the problems such as target unreachable in traditional artificial potential field method, and has strong real-time performance.

Description

Technical Field

[0001] This invention belongs to the field of unmanned aerial vehicle (UAV) technology, specifically relating to a formation control method that integrates an improved artificial potential field method and a navigation method. Background Technology

[0002] In the field of drones, formation control technology is the key to achieving group collaboration and mission execution. It not only requires individual drones to have efficient autonomous navigation capabilities, but also requires the entire group to maintain a stable formation structure in various environments.

[0003] The leader-follower method is commonly used in multi-robot systems to maintain formation, where one or more drones act as leaders and the others as followers. The main challenge of this method is how to quickly and effectively adjust the entire formation when the leader encounters unforeseen circumstances, while maintaining the integrity of the formation and the continuity of mission execution.

[0004] Artificial potential field (APF) is a popular path planning and obstacle avoidance strategy that guides drones around obstacles and toward a target by constructing attractive and repulsive force fields; however, this method is susceptible to local minima.

[0005] Considering the limitations of a single method in practical applications, it is particularly important to develop a new formation control strategy that combines the dynamic obstacle avoidance capability of the artificial potential field method with the formation control advantage of the navigation method. Summary of the Invention

[0006] To address the collision issues that arise during UAV formation flight and formation changes using the navigation method, this invention provides a formation control method that integrates an improved artificial potential field method and the navigation method. This method enhances the flexibility and adaptability of formation control in changing environments and complex mission execution.

[0007] The formation control method that integrates the improved artificial potential field method and the navigation method includes the following steps:

[0008] Step 1: Establish a dynamic potential field model for the lead aircraft and each wingman drone to be formed;

[0009] Specifically, each drone has its own target point. A repulsive potential field is set for each drone, and a gravitational potential field is set for each target point. The direction of the repulsive potential field is from the other drones to itself, and the direction of the gravitational potential field is from itself to the target point.

[0010] Step 2: The lead drone is given a target point, and the position of that target point, as well as the position and speed of each drone, are obtained;

[0011] The coordinates of each unmanned aerial vehicle are converted from latitude and longitude to rectangular coordinate system, and the coordinate of the target point of the long machine is E1(x, y);

[0012] The coordinate of the a-th unmanned aerial vehicle is q a (x, y), a∈[1, n], n is the total number of unmanned aerial vehicles in the formation; the heading is yaw a , and the speed is v a , wherein yaw a ∈[0, 2π];

[0013] Step three, the long machine calculates the target point position of each wingman through its own position and heading;

[0014] The target point position of the a-th wingman is E a (x, y), a∈[2, n]; the specific calculation process is as follows:

[0015] Firstly, the initial vertical distance between each wingman and the long machine is dy, and the initial horizontal distance is dx;

[0016] When the wingman is initially arranged in formation, the flight heading of the long machine is along the y-axis direction;

[0017] Then, according to the real-time position of the target point of the long machine E1(x, y) and the heading of the long machine yaw1, dyaw1 perpendicular to the heading of the long machine is obtained; thus the target point position of each wingman is obtained;

[0018] The target point position of the a-th wingman is calculated as follows:

[0019] E a (x)=dx*[cos(yaw1)+cos(dyaw1)]+E1(x)

[0020] E a (y)=dy*[sin(yaw1)+sin(dyaw1)]+E1(y)

[0021] Step four, each wingman calculates the gravity vector of each wingman pointing to the target point according to the target point position

[0022] The length of the vector is the size of the gravity; for the a-th wingman, the gravity vector is:

[0023]

[0024] Where p(E a , q a ) is the Euclidean distance from the coordinate position of the a-th wingman to the target point position thereof;

[0025] Step five, each wingman judges whether there is the rest of the wingman in its own safety distance, if yes, calculates the repulsion of the rest of the wingman to itself; otherwise, records the repulsion value of the wingman as 0

[0026] The total repulsion formula accepted by the a-th wingman is:

[0027]

[0028] Wherein, q b (x, y) is the coordinates of the unmanned aerial vehicle b in the safety range of the wingman; p(q a , q b ) is the Euclidean distance between the a-th wingman and the unmanned aerial vehicle b in the safety range; d is the safety range threshold of the wingman;

[0029] The resultant force of the repulsion of the a-th wingman is scaled down by a certain proportion to obtain a repulsion with a module length of w, wherein w ∈ (0, 1), and the updated repulsion formula is:

[0030]

[0031] Step six, each wingman adds the attraction between itself and the target point and the repulsion in the safety range to obtain the resultant force, adjusts the heading according to the direction of the resultant force, and finally flies along the direction of the resultant force, so that the unmanned aerial vehicle formation can maintain a fixed formation;

[0032] The resultant force The calculation formula is:

[0033]

[0034] Step seven, when the attraction and the repulsion of the a-th wingman are close, the resultant force is decomposed to obtain a component perpendicular to the direction of the resultant force, and the new resultant force direction is obtained by transforming the component;

[0035] The resultant force is decomposed into two vectors along the direction of the attraction and perpendicular to the direction of the attraction, which are:

[0036]

[0037]

[0038]

[0039] Wherein is the resultant force pointing to the direction of the attraction, is the resultant force perpendicular to the direction of the attraction;

[0040] When the module length of the resultant force perpendicular to the direction of the attraction is less than a set value c, the resultant force is scaled up to a resultant force component with a module length of c New resultant force is obtained at this time

[0041]

[0042]

[0043] Step eight, the heading of the a-th subordinate aircraft is obtained according to the new resultant force direction and the angular velocity change limit to obtain a new heading;

[0044] New resultant force received by the a-th subordinate aircraft The angle between the vector and the positive direction of the x-axis is γ, and the angular velocity change rate is δ at this time;

[0045] Determine whether |γ-yaw a |<δ, if yes, the new heading of the unmanned aerial vehicle is nyaw a =γ; otherwise, Wherein, nyaw a ∈[0, 2π];

[0046] Step nine, the leader and each subordinate aircraft adjust their own speed and fly according to the new heading obtained respectively.

[0047] The advantages of the present application are:

[0048] 1) A formation control method combining and improving the artificial potential field method and the navigation method, which solves the problems of maintaining the formation and avoiding obstacles in the formation, and the unmanned aerial vehicle can effectively fly towards the target point and avoid the remaining unmanned aerial vehicles when turning and changing the formation, thereby avoiding the problems such as unattainable target in the traditional artificial potential field method;

[0049] 2) A formation control method combining and improving the artificial potential field method and the navigation method, the unmanned aerial vehicle can quickly calculate the resultant force received according to the target point and the positions of the remaining unmanned aerial vehicles, so as to calculate the motion speed and position of the next step, and the real-time performance is strong. DETAILED DESCRIPTION

[0050] Figure 1 It is a flow chart of the formation control method combining and improving the artificial potential field method and the navigation method of the present application;

[0051] Figure 2 It is a heading transformation diagram in the embodiment of the present application;

[0052] Figure 3 It is a force decomposition diagram in the embodiment of the present application;

[0053] Figure 4 It is a force transformation diagram in the embodiment of the present application. DETAILED DESCRIPTION

[0054] For the convenience of those skilled in the art to understand and implement the present application, the present application is further described in detail below in combination with the drawings and examples. Obviously, the described examples are only partial examples of the present application, but not all examples, and all other examples obtained by those skilled in the art without creative labor based on the examples in the present application shall belong to the scope of protection of the present application.

[0055] The present application is a kind of formation control method combining improved artificial potential field method and navigation method, as shown in Figure 1 The steps are as follows:

[0056] Step one, for the long aircraft and each wingman unmanned aerial vehicle to be formed, a dynamic potential field model is established;

[0057] Specifically, each unmanned aerial vehicle has its own target point, and is repelled by the remaining unmanned aerial vehicles within the safety distance; each unmanned aerial vehicle is provided with a repulsive potential field, and each target point is provided with an attractive potential field, the direction of the repulsive potential field is pointed to itself by the remaining unmanned aerial vehicles, and the direction of the attractive potential field is pointed to the target point by itself;

[0058] Step two, the long aircraft gives a target point, and the position of the target point and the position and speed of each unmanned aerial vehicle in the unmanned aerial vehicle formation are obtained;

[0059] The coordinates of each unmanned aerial vehicle are converted from latitude and longitude to rectangular coordinate system, and the coordinates of the long aircraft target point are E1(x, y);

[0060] The coordinates of the a unmanned aerial vehicle are q a (x, y), a∈[1, n], n is the total number of unmanned aerial vehicles in the formation; the heading of the a unmanned aerial vehicle is yaw a , and the speed is v a , wherein yaw a ∈[0, 2π];

[0061] Step three, according to the navigation method, the long aircraft calculates the target point position of each wingman through its own position and heading and the predetermined formation shape;

[0062] The target point of the unmanned aerial vehicle wingman is the point expected to be reached by each wingman calculated from the long aircraft position and heading and the expected unmanned aerial vehicle formation. When all unmanned aerial vehicles reach the target point, the unmanned aerial vehicle formation maintains the predetermined formation shape;

[0063] The target point position of the a wingman is E a (x, y), a∈[2, n]; the specific calculation process is as follows:

[0064] First, according to the initial formation shape, the positional relationship between each wingman and the long aircraft is obtained, i.e. the vertical distance between the wingman and the long aircraft is dy and the horizontal distance is dx;

[0065] The flight heading of the lead aircraft is along the y-axis direction when the initial formation of the wingman is set;

[0066] Then, according to the real-time position E1(x, y) of the target point of the lead aircraft and the heading yaw1 of the lead aircraft, dyaw1 perpendicular to the heading of the lead aircraft is obtained; thus the target point position of each wingman is obtained;

[0067] The target point position of the a-th wingman is calculated as:

[0068] E a (x) = dx*[cos(yaw1) + cos(dyaw1)] + E1(x)

[0069] E a (y) = dy*[sin(yaw1) + sin(dyaw1)] + E1(y)

[0070] Step four, each unmanned aircraft calculates the gravitational vector of each unmanned aircraft itself pointing to the target point according to the target point position

[0071] The wingman obtains its expected position according to the expected formation, and thus obtains the gravity, at this time the gravity is a constant value;

[0072] The modulus of the vector is the size of the gravity; the gravity modulus value is set as a constant 1, providing a consistent guiding force; for the a-th unmanned aircraft, the gravity vector is:

[0073]

[0074] Where p(E a , q a ) is the Euclidean distance from the coordinate position of the a-th wingman to the target point position of the a-th wingman;

[0075] Step five, each wingman judges whether there is any other wingman within its safety distance, if yes, calculates the repulsion of the other wingman to itself; otherwise, records the repulsion value of the wingman as 0

[0076] Set a safety range d for each wingman, when there is any other unmanned aircraft within the safety range of the wingman, each other unmanned aircraft within the range gives the wingman a repulsion, the direction of which is from the other unmanned aircraft to the wingman, and the repulsion is added to obtain the total repulsion; wherein the repulsion given by each unmanned aircraft to the wingman is inversely proportional to the distance between them, and the total repulsion received by the a-th wingman is calculated by the formula:

[0077]

[0078] Where, q b (x, y) is the coordinate of the unmanned aircraft b within the safety range of the wingman; p(q a , qb ) is the Euclidean distance between the a-th drone and the safety range of the drone b; d is the safety range threshold of the drone;

[0079] The repulsive force of the a-th drone is scaled down to get a repulsive force with a module w, where w∈(0, 1), and the updated repulsive force formula is:

[0080]

[0081] Step six, each drone adds the attractive force between itself and the target point and the repulsive force within the safety range to get the resultant force, adjusts the heading according to the direction of the resultant force, and finally flies along the direction of the resultant force, so that the UAV formation can maintain a fixed formation;

[0082] Under the action of the attractive force provided by the navigation method, each drone flies to the target point, and the UAV formation can maintain the formation, and under the action of the repulsive force, each UAV can prevent collision, as shown in Figure 2 , finally the UAV flies to the target point under the action of the resultant force of the attractive force and the repulsive force without collision, so that the UAV formation can maintain a certain formation for flight;

[0083] The resultant force The calculation formula is:

[0084]

[0085] Step seven, when the attractive force and the repulsive force of the a-th drone are close, the resultant force is decomposed to get a component perpendicular to the direction of the resultant force, and the new resultant force direction is obtained by transforming the component;

[0086] In order to prevent collision, the following method is used to improve the resultant force function: the resultant force is decomposed along the direction of the attractive force and perpendicular to the direction of the attractive force to get two vectors, as shown in Figure 3 , specifically:

[0087]

[0088]

[0089]

[0090] wherein is the resultant force pointing to the direction of the attractive force, is the resultant force perpendicular to the direction of the attractive force;

[0091] When the module of the resultant force of the UAV perpendicular to the direction of the attractive force is less than the set value c, the UAVs may collide, so the resultant force is scaled up to the resultant force component with a module of c, as shown in Figure 4As shown, a new resultant force is obtained at this time.

[0092]

[0093]

[0094] Step 8: Obtain a new heading from the heading of the a-th wingman based on the new resultant force direction and angular velocity change constraints;

[0095] The new combined force received by wingman a The vector makes an angle of γ with the positive x-axis, and the rate of change of angular velocity is δ.

[0096] Determine if |γ-yaw is satisfied a |<δ, if so, the drone's new heading is nyaw a =γ; otherwise, the new course of the drone is: Among them nyaw a ∈[0, 2π];

[0097] Step 9: The lead aircraft and each wingman adjust their speed and fly according to their newly acquired heading.

[0098] Example:

[0099] Step 1: Set up a repulsive potential field for each drone and a gravitational potential field for each drone's target point. The purpose is to obtain a resultant force that allows the drone to avoid the combined force of the other drones when flying toward the target point.

[0100] Step 2: Obtain the coordinates q of each drone a The coordinates of the target point E1(x, y) and the target point of the lead aircraft are obtained, and the heading yaw of each UAV is also obtained. a and speed magnitude v a , where a∈[1,n], and n is the total number of drones in the formation;

[0101] Step 3: Calculate the target position E of the drone wingman based on the lead aircraft's position and heading, as well as the predetermined formation. a (x, y), a∈[2, n];

[0102] Set the initial formation to obtain the positional relationship between each wingman and the lead aircraft, namely the vertical distance dy and the horizontal distance dx between the wingman and the lead aircraft. When setting the initial formation, the lead aircraft's flight heading is along the y-axis.

[0103] Then, based on the lead aircraft's real-time position E1(x, y) and its own heading yaw1, the dyaw1 perpendicular to the lead aircraft's heading can be obtained. At this point, the target position of each wingman can be obtained:

[0104] E a(x) = dx * [cos(yawl) + cos(dyawl)] + El(x)

[0105] E a (y) = dy * [sin(yawl) + sin(dyawl)] + El(y)

[0106] Step 4: Each drone gets the attractive force vector from itself to the target point according to its coordinate q a (x, y) and the target point coordinate E a (x, y) by itself. Where the length of the vector is the size of the attractive force.

[0107] In the traditional artificial potential field method, the size of the attractive force is proportional to the distance between itself and the target point. However, when the distance is too large, the attractive force is too large and the repulsive force may be ignored. When the distance is too small, the attractive force may be too small and the target point may be unreachable. Therefore, the modulus of the attractive force is set to a constant 1 to provide a consistent guiding force. At this time, the attractive force vector is:

[0108]

[0109] Where p(E a , q a ) is the Euclidean distance from the drone's own position q a (x, y) to the target point E a (x, y), and a is the drone number.

[0110] Step 5: Set a safety range d for each drone. When there are other drones within the safety range of the drone, each of the remaining drones within the range will give the drone a repulsive force, with the direction from the remaining drones to the drone. The repulsive forces are added to get the total repulsive force.

[0111] Where each drone gives the drone a repulsive force inversely proportional to the distance between them. The repulsive force formula is:

[0112]

[0113] Where p(q a , q b ) is the Euclidean distance from the drone's own position q a (x, y) to the remaining drones q b (x, y) within the safety range, a is the number of drones subjected to repulsive force, and b is the number of the remaining drones within the safety range of drone a.

[0114] However, to ensure the guiding effect of the drone's gravity and prevent the wingman from entering a state of force equilibrium where the target point is unreachable under the action of repulsive force, the sum of the drone's repulsive forces is proportionally reduced to obtain a repulsive force with a modulus of w, where w∈(0,1). The new repulsive force formula is:

[0115]

[0116] Step 6: Add the gravitational vector and the new repulsive force vector to obtain the resultant force. At this point, because the gravitational modulus of the drone is greater than the repulsive force, the drone can still move toward the target point even under the action of the repulsive force, and will not get stuck in the situation where the target point is unreachable.

[0117] Step 7: However, when the gravitational force of the drone is always greater than the repulsive force, the obstacle avoidance effect will be greatly weakened, and collisions may occur between drones. Therefore, in order to prevent collisions, the following method will be used to improve the resultant force function:

[0118] The resultant force can be decomposed into two vectors along and perpendicular to the gravitational direction:

[0119]

[0120]

[0121]

[0122] in The resultant force is pointing in the direction of gravity. The resultant force is perpendicular to the direction of gravity; 'a' is the UAV number.

[0123] When the drone's net force is perpendicular to the direction of gravity If the module length is too small, collisions may occur between drones, therefore when When the modulus is less than a certain size b, The resultant force component is proportionally expanded to a modulus of b. At this moment, a new synergy is obtained. The formula for the resultant force is as follows:

[0124]

[0125]

[0126] Step 8: The current UAV numbered 'a' is heading 'yaw'. a The magnitude of the net force is The angle between this vector and the positive x-axis is γ, and the rate of change of angular velocity is δ; when |γ-yaw a |<δ time, new nyaw course for dronesa = γ; otherwise where nyaw a ∈ [0, 2π];

[0127] Step 9: The leader and wingman obtain their own target flight lines and adjust their own speeds to fly according to the flight lines.

Claims

1. A platoon control method that fuses an improved artificial potential field method and a navigation method, characterized by, The specific steps are as follows: Step one, for the long machine and each wingman unmanned aerial vehicle to be formed, a dynamic potential field model is established; Step two, the long machine gives a target point, and the position of the target point and the position and speed of each unmanned aerial vehicle are obtained; Step three, the long machine calculates the target point position of each wingman through its own position and heading; Target point position E of the a-th subordinate machine a (x, y), a e [2, n]; the specific calculation process is as follows: Firstly, the initial vertical distance of each wingman from the long machine is dy and the initial horizontal distance is dx; The flight heading of the long machine is along the y-axis direction when the wingman initial formation is set; Then, according to the real-time position E1(x,y) of the target point of the long machine and the heading yaw1 of the long machine, dyaw1 perpendicular to the heading of the long machine is obtained; thus the target point position of each wingman is obtained; The target point position of the a-th wingman is calculated as: E a (x) = dx * [cos(yaw1) + cos(dyaw1)] + E1(x) E a (y) = dy * [sin(yaw1) + sin(dyaw1)] + E1(y) Step four, each wingman calculates its own gravity vector pointing to the target point according to the target point position The modulus of the vector is the size of the gravity; for the a-th wingman, the gravity vector is: where p(E a , q a ) is the Euclidean distance from the coordinate position of the a-th UAV to its target point position; q a (x, y) is the coordinate of the a-th UAV, a ∈ [1, n], and n is the total number of UAVs in the formation. Step five, each wingman judges whether there is any other wingman in its safe distance, if yes, calculates the repulsion of the other wingman to itself; otherwise, records the repulsion value of the wingman as 0; The total repulsion received by the a-th wingman is calculated as: wherein q b (x,y) is the coordinates of the UAV b within the safety range of the wingman; p(q a b ) is the Euclidean distance between the a-th wingman and the UAV b within the safety range; d is the safety range threshold of the wingman.​ The repulsion force of the a-th wingman is scaled down by a certain proportion to obtain the repulsion with a modulus of w, wherein w∈(0,1), and the updated repulsion formula is: Step six, each wingman adds the gravity between itself and the target point and the repulsion received in the safe range to obtain the resultant force, adjusts the heading according to the direction of the resultant force, and finally flies along the direction of the resultant force, so that the unmanned aerial vehicle formation can maintain a fixed formation; resultant force The calculation formula is: Step seven, when the gravity and the repulsion of the a-th wingman are close, the resultant force is decomposed to obtain a component perpendicular to the direction of the resultant force, and the new resultant force direction is obtained by transforming the component; The resultant force is decomposed along the direction of the gravity and the direction perpendicular to the direction of the gravity to obtain two vectors, which are: wherein is the resultant force directed towards the direction of gravity, is the resultant force perpendicular to the direction of gravity; When the drone's net force is perpendicular to the direction of gravity When the modulus is less than the set value c, the resultant force will be... The resultant force component is proportionally expanded to a modulus of length c. At this moment, a new synergy is achieved. Step eight, the heading of the a-th wingman is obtained according to the new resultant force direction and the angular velocity transformation limit; The new resultant force on the a-th subordinate machine The angle between the vector and the positive direction of the x-axis is γ, and the rate of change of the angular velocity is δ at this time. determine whether |γ-yaw a |δ, if yes, new yaw direction n yaw for the drone a = γ; otherwise, where n yaw a ∈ [0, 2π] Step nine, the long machine and each wingman adjusts its own speed and flies according to the new heading obtained by itself.

2. The method of claim 1, wherein the improved artificial potential field method and the navigation method are fused. The step one is specifically: each unmanned aerial vehicle has its own target point, the repulsion potential field is set for each unmanned aerial vehicle, and the gravity potential field is set for each target point. The direction of the repulsion potential field is pointed to itself by the remaining unmanned aerial vehicles, and the direction of the gravity potential field is pointed to the target point by itself.

3. The method of claim 1, wherein the method is characterized by, In the second step, the coordinates of each drone are converted from latitude and longitude to rectangular coordinates, and the target point coordinates of the lead drone are E1(x, y); the heading is yaw a , and the speed is v a , where yaw a ∈[0, 2π].

Images