Formation control method integrating improved artificial potential field method and piloting method

Abstract

The invention discloses a formation control method fusing and improving an artificial potential field method and a piloting method, and belongs to the technical field of unmanned aerial vehicles, and the method specifically comprises the steps: firstly, building a dynamic potential field model for each to-be-formed unmanned aerial vehicle; obtaining the position of a given target point of the long aircraft and the position and speed of each unmanned aerial vehicle; the lead aircraft calculates the target point position of each wing aircraft according to the position and the course of the lead aircraft; then, each wing aircraft calculates the gravity pointing to the target point and the repulsive force of the other wing aircrafts within the safety distance of the wing aircrafts; the heading is adjusted according to the resultant force direction; when the gravitational force and the repulsive force of the wing aircraft approach each other, the resultant force is decomposed to obtain component force perpendicular to the resultant force direction, the component force is converted to obtain a new resultant force direction, and the wing aircraft obtains a new course according to the new resultant force direction and angular velocity conversion limitation; the self speed is adjusted, and the aircraft flies in the new flight direction. The problem that a target cannot be reached in a traditional artificial potential field method is solved, and the real-time performance is high.

Description

Technical Field

[0001] The invention belongs to the technical field of unmanned aerial vehicles, and in particular relates to a formation control method integrating an improved artificial potential field method and a piloting method. Background Art

[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 be able to maintain a stable formation structure in various environments.

[0003] The leader-follower method is often used to maintain the formation structure in multi-robot systems, where one or several drones act as leaders and the other drones act as followers. The main challenge of this method is how to quickly and effectively adjust the entire formation when the leader encounters an emergency, while maintaining the integrity of the formation and the continuity of task execution.

[0004] Artificial potential field (APF) is a popular path planning and obstacle avoidance strategy that guides drones around obstacles and toward their targets 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 and the formation control advantages of the pilot method. Summary of the invention

[0006] In order to solve the collision problem that occurs when a UAV formation flies and changes its formation using the piloting method, the present invention provides a formation control method that integrates the improved artificial potential field method and the piloting method, which can improve the flexibility and adaptability of formation control in a changing environment and during complex mission execution.

[0007] The formation control method integrating the improved artificial potential field method and the pilot method comprises the following steps:

[0008] Step 1: Establish a dynamic potential field model for the leader and wingman drones 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 leader gives a target point and obtains the position of the target point, as well as the position and speed of each drone;

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

[0012] The coordinates of the ath drone are q a (x, y), a∈[1, n], n is the total number of drones in the formation; the heading is yaw a , the velocity is v a , where yaw a ∈[0, 2π];

[0013] Step 3: The leader aircraft calculates the target point position of each wingman aircraft based on its own position and heading;

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

[0015] First, the initial vertical distance between each wingman and the leader is dy and the initial horizontal distance is dx;

[0016] When the wingman's initial formation is set, the leader's flight direction is along the y-axis;

[0017] 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; thereby, the target point position of each wingman is obtained;

[0018] The target point position of the ath wingman is calculated as:

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

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

[0021] Step 4: Each wingman calculates its own gravity vector pointing to the target point based on the position of the target point

[0022] The modulus of the vector is the magnitude of gravity; for the ath wingman, the gravity vector is:

[0023]

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

[0025] Step 5: Each wingman determines whether there are other wingmen within its safe distance. If so, calculate the repulsion of the other wingmen on itself; otherwise, the repulsion value of the wingman is recorded as 0

[0026] The total repulsion force received by the ath wingman is calculated as:

[0027]

[0028] Among them, q b (x, y) is the coordinate of drone b within the safety range of the wingman; p(q a ,q b ) is the Euclidean distance between the ath wingman and the drone b within the safety range; d is the safety range threshold of the wingman;

[0029] The repulsive force of the ath wingman is proportionally reduced to obtain a repulsive force with a modulus of w, where w∈(0,1). The updated repulsive force formula is:

[0030]

[0031] Step 6: Each wingman adds the gravitational force between itself and the target point and the repulsive force within the safe range to obtain a resultant force, adjusts its heading according to the direction of the resultant force, and finally flies in the direction of the resultant force, so that the UAV formation can maintain a fixed formation;

[0032] Join forces The calculation formula is:

[0033]

[0034] Step 7: When the attraction and repulsion of the ath wingman are close, the resultant force is decomposed to obtain a component force perpendicular to the direction of the resultant force, and the component force is transformed to obtain a new direction of the resultant force;

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

[0036]

[0037]

[0038]

[0039] in is the resultant force pointing in the direction of gravity, is the resultant force perpendicular to the direction of gravity;

[0040] When the drone is perpendicular to the direction of gravity, the net force When the modulus length is less than the set value c, the resultant force The resultant force component is proportionally expanded to a modulus of c Get a new force at this time

[0041]

[0042]

[0043] Step 8: Obtain a new heading of the ath wingman according to the new resultant force direction and angular velocity transformation limit;

[0044] The new combined force on the wingman The angle between this vector and the positive direction of the x-axis is γ, and the rate of change of the angular velocity is δ;

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

[0046] Step 9: The leader and each wingman adjust their own speed and fly according to the new heading they have obtained.

[0047] The advantages of the present invention are:

[0048] 1) A formation control method that integrates the improved artificial potential field method and the pilot method solves the problems of maintaining formation and avoiding obstacles in the formation. When turning and changing formation, the drone can effectively fly toward the target point and avoid other drones, avoiding the problem of unreachable targets in the traditional artificial potential field method;

[0049] 2) A formation control method that integrates the improved artificial potential field method and the navigation method. The drone can quickly calculate the resultant force it receives based on the target point and the positions of other drones, thereby calculating the next movement speed and position with strong real-time performance. BRIEF DESCRIPTION OF THE DRAWINGS

[0050] Figure 1 This is a flow chart of a formation control method integrating an improved artificial potential field method and a pilot method according to the present invention;

[0051] Figure 2 is a heading change diagram in an embodiment of the present invention;

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

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

[0054] In order to facilitate those skilled in the art to understand and implement the present invention, the present invention is further described in detail below in conjunction with the accompanying drawings and embodiments. Obviously, the described embodiments are only partial embodiments of the present invention, rather than all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative work should fall within the scope of protection of the present invention.

[0055] The present invention provides a formation control method integrating an improved artificial potential field method and a pilot method, such as Figure 1 As shown, the following steps are included:

[0056] Step 1: Establish a dynamic potential field model for the leader and wingman drones to be formed;

[0057] Specifically, each drone has its own target point and is subject to repulsion from other drones within a safe distance. 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.

[0058] Step 2: The leader aircraft gives a target point and obtains the position of the target point, as well as the position and speed of each drone in the drone formation;

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

[0060] The coordinates of the ath drone are q a (x, y), a∈[1, n], n is the total number of drones in the formation; the heading of the ath drone is yaw a , the velocity is v a , where yaw a ∈[0, 2π];

[0061] Step 3: According to the piloting method, the lead aircraft calculates the target point position of each wingman through its own position and heading as well as the predetermined formation;

[0062] The target point of the drone wingman is the expected arrival point of each wingman calculated by the position and heading of the lead drone and the expected drone formation. When all drones reach the target point, the drone formation maintains the predetermined formation flight;

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

[0064] First, according to the initial formation, the position relationship between each wingman and the leader is obtained, that is, the vertical distance between the wingman and the leader is dy and the horizontal distance is dx;

[0065] When the wingman's initial formation is set, the leader's flight direction is along the y-axis;

[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; thereby, the target point position of each wingman is obtained;

[0067] The target point position of the ath 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 4: Each drone calculates its own gravity vector pointing to the target point based on the position of the target point

[0071] The wingman obtains its own desired position according to the desired formation, thereby obtaining gravity, which is a constant at this time;

[0072] The modulus of the vector is the magnitude of gravity; setting the modulus of gravity to a constant of 1 provides a consistent guiding force. For the ath drone, the gravity vector is:

[0073]

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

[0075] Step 5: Each wingman determines whether there are other wingmen within its safe distance. If so, calculate the repulsion of the other wingmen on itself; otherwise, the repulsion value of the wingman is recorded as 0

[0076] A safety range d is set for each wingman. When there are other drones within the safety range of the wingman, each of the other drones within the range will give the wingman a repulsive force, and the direction is from the other drones to the wingman. The repulsive forces are added together to get the total repulsive force. The repulsive force given to the wingman by each drone is inversely proportional to the distance between them. The total repulsive force received by the ath wingman is calculated as:

[0077]

[0078] Among them, q b (x, y) is the coordinate of drone b within the safety range of the wingman; p(q a ,qb ) is the Euclidean distance between the ath wingman and the drone b within the safety range; d is the safety range threshold of the wingman;

[0079] The repulsive force of the ath wingman is proportionally reduced to obtain a repulsive force with a modulus of w, where w∈(0,1). The updated repulsive force formula is:

[0080]

[0081] Step 6: Each wingman adds the gravitational force between itself and the target point and the repulsive force within the safe range to obtain a resultant force, adjusts its heading according to the direction of the resultant force, and finally flies in the direction of the resultant force, so that the UAV formation can maintain a fixed formation;

[0082] Under the gravitational force of the target point provided by the pilot method, each wingman flies to the target point, and the drone formation can maintain its formation. Under the repulsive force, each drone can avoid collision. Figure 2 As shown, the drones eventually fly towards the target point without collision under the combined force of gravity and repulsion, so that the drone formation can maintain a certain formation to fly;

[0083] Join forces The calculation formula is:

[0084]

[0085] Step 7: When the attraction and repulsion of the ath wingman are close, the resultant force is decomposed to obtain a component force perpendicular to the direction of the resultant force, and the component force is transformed to obtain a new direction of the resultant force;

[0086] In order to prevent collisions, the following method is used to improve the resultant force function: the resultant force is decomposed into two vectors along the gravitational direction and perpendicular to the gravitational direction, such as Figure 3 As shown, specifically:

[0087]

[0088]

[0089]

[0090] in is the resultant force pointing in the direction of gravity, is the resultant force perpendicular to the direction of gravity;

[0091] When the drone is perpendicular to the direction of gravity, the net force When the modulus length is less than the set value c, the drones may collide with each other, so the resultant force The resultant force component is proportionally expanded to a modulus of c like Figure 4As shown, the new resultant force is

[0092]

[0093]

[0094] Step 8: Obtain a new heading of the ath wingman according to the new resultant force direction and angular velocity transformation limit;

[0095] The new combined force on the wingman The angle between this vector and the positive direction of the x-axis is γ, and the rate of change of the angular velocity is δ;

[0096] Determine whether |γ-yaw is satisfied a |<δ, if yes, the new heading of the drone is nyaw a =γ; otherwise, the new heading of the drone is: Where nyaw a ∈[0, 2π];

[0097] Step 9: The leader and each wingman adjust their own speed and fly according to the new heading they have obtained.

[0098] Example:

[0099] Step 1: Set a repulsive potential field for each drone and a gravitational potential field for each drone’s target point, in order to obtain a combined force that enables the drone to avoid other drones when flying to the target point;

[0100] Step 2: Get the coordinates q of each drone a (x, y), and the coordinates of the target point of the lead aircraft E1 (x, y), and obtain the heading yaw of each drone a and velocity magnitude v a , where a∈[1,n], n is the total number of drones in the formation;

[0101] Step 3: Calculate the target position E of the wingman according to the position and heading of the lead aircraft and the predetermined formation. a (x, y), a∈[2, n];

[0102] Set the initial formation, obtain the positional relationship between each wingman and the leader, that is, the vertical distance dy and the horizontal distance dx between the wingman and the leader. When the initial formation is set, the flight direction of the leader is along the y-axis direction;

[0103] Then, according to the real-time position E1 (x, y) of the leader and its own heading yaw1, we can get dyaw1 which is perpendicular to the heading of the leader. At this time, we can get the target position of each wingman:

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

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

[0106] Step 4: Each drone is based on its own coordinates q a (x, y) and the target point coordinates E a (x, y) gets the gravity vector pointing from the drone itself to the target point The modulus of the vector is the magnitude of the gravitational force.

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

[0108]

[0109] Where p(E a ,q a ) is the position of the drone itself q a (x, y) to the target point E a The Euclidean distance of (x, y), where a is the drone number;

[0110] Step 5: Set a safety range d for each wingman. When there are other drones within the safety range of the wingman, each of the other drones within the range will give the wingman a repulsive force, with the direction from the other drones to the wingman, and the total repulsive force is obtained by adding up the repulsive forces.

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

[0112]

[0113] Where p(q a ,q b ) is the position of the drone itself q a (x, y) and other drones q within the safety range b The Euclidean distance of (x, y), a is the number of the drone that is subject to repulsion, and b is the number of the other drones within the safety range of drone a;

[0114] However, in order to ensure the guiding effect of the drone’s gravity and prevent the wingman from entering a force balance state where the target point is unreachable under the action of repulsion, the drone’s repulsion force obtained after addition is proportionally reduced to obtain a repulsion force with a modulus length of w, where w∈(0,1). At this time, the new repulsion formula is:

[0115]

[0116] Step 6: Add the gravitational force vector and the new repulsive force vector to get the resultant force At this time, since 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 fall into the situation where the target point is unreachable;

[0117] Step 7: However, when the gravity of the drone is always greater than the repulsion, 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] Decomposing the resultant force along the direction of gravity and perpendicular to the direction of gravity yields two vectors:

[0119]

[0120]

[0121]

[0122] in is the resultant force pointing in the direction of gravity, is the resultant force perpendicular to the direction of gravity, a is the drone number;

[0123] When the drone is perpendicular to the direction of gravity, the net force If the model length is too small, the drones may collide with each other. When the modulus length is less than a certain size b, The resultant force component is proportionally expanded to a modulus of b Get a new force at this time The formula for the resultant force is as follows:

[0124]

[0125]

[0126] Step 8: The current heading of the drone numbered a is yaw a , the magnitude of the resultant force is The angle between this vector and the positive direction of the x-axis is γ, and the rate of change of angular velocity is δ; when |γ-yaw a |<δ, the new heading of the UAV is nyawa =γ; otherwise Where nyaw a ∈[0, 2π];

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

Claims

1. A formation control method integrating improved artificial potential field method and pilot method, characterized in that: The specific steps are as follows: Step 1: Establish a dynamic potential field model for the leader and wingman drones to be formed; Step 2: The leader gives a target point and obtains the position of the target point, as well as the position and speed of each drone; Step 3: The leader aircraft calculates the target point position of each wingman aircraft based on its own position and heading; The target point position E of the ath wingman a (x, y), a∈[2,n]; the specific calculation process is: First, the initial vertical distance between each wingman and the leader is dy and the initial horizontal distance is dx; When the wingman's initial formation is set, the leader's flight direction is along the y-axis; 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; The target point position of the ath wingman is calculated as: E a (x)=dx*[cos(yaw1)+cos(dyaw1)]+E1(x) ITS a (y)=dy*[sin(yaw1)+sin(dyaw1)]+E1(y) Step 4: Each wingman calculates its own gravity vector pointing to the target point based on the position of the target point The modulus of the vector is the magnitude of gravity; for the ath wingman, the gravity vector is: Where p(E a ,q a ) is the Euclidean distance from the coordinate position of the ath wingman to its target point; q a (x, y) is the coordinate of the ath drone, a∈[1,n], n is the total number of drones in the formation; Step 5: Each wingman determines whether there are other wingmen within its safe distance. If so, calculate the repulsion of the other wingmen on itself; otherwise, the repulsion value of the wingman is recorded as 0; The total repulsion force received by the wingman a is calculated as: Among them, q b (x, y) is the coordinate of drone b within the safety range of the wingman; p(q a ,q b ) is the Euclidean distance between the ath wingman and the drone b within the safety range; d is the safety range threshold of the wingman; The repulsive force of the ath wingman is proportionally reduced to obtain a repulsive force with a modulus of w, where w∈(0,1). The updated repulsive force formula is: Step 6: Each wingman adds the gravitational force between itself and the target point and the repulsive force within the safe range to obtain a resultant force, adjusts its heading according to the direction of the resultant force, and finally flies in the direction of the resultant force, so that the UAV formation can maintain a fixed formation; Join forces The calculation formula is: Step 7: When the attraction and repulsion of the ath wingman are close, the resultant force is decomposed to obtain a component force perpendicular to the direction of the resultant force, and the component force is transformed to obtain a new direction of the resultant force; The resultant force is decomposed into two vectors along the direction of gravity and perpendicular to the direction of gravity, specifically: in is the resultant force pointing in the direction of gravity, is the resultant force perpendicular to the direction of gravity; When the drone is perpendicular to the direction of gravity, the net force When the modulus length is less than the set value c, the resultant force The resultant force component is proportionally expanded to a modulus of c Get a new force at this time Step 8: Obtain a new heading of the ath wingman according to the new resultant force direction and angular velocity transformation limit; The new combined force on the wingman The angle between this vector and the positive direction of the x-axis is γ, and the rate of change of the angular velocity is δ; Determine whether |γ-yaw is satisfied a |<δ, if yes, the new heading of the drone is nyaw a =γ; otherwise, Where nyaw a ∈[0, 2π]; Step 9: The leader and each wingman adjust their own speed and fly according to the new heading they have obtained.

2. A formation control method integrating the improved artificial potential field method and the piloting method as claimed in claim 1, characterized in that: The step 1 is specifically as follows: 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.

3. A formation control method integrating improved artificial potential field method and pilot method as claimed in claim 1, characterized in that: In step 2, the coordinates of each drone are converted to the rectangular coordinate system through longitude and latitude, and the coordinates of the target point of the long drone are E1 (x, y); the heading is yaw a , the velocity is v a , where yaw a ∈[0,2π].

Images