Unmanned aerial vehicle formation obstacle avoidance control algorithm based on improved artificial potential field method

Abstract

The invention discloses an unmanned aerial vehicle group formation obstacle avoidance control method based on an improved artificial potential field method. The method comprises the following steps: step 1, performing formation control on an unmanned aerial vehicle group based on a virtual structure method; 2, for the ith unmanned aerial vehicle, judging whether the distance between the ith unmanned aerial vehicle and the obstacle is greater than a safe distance or not through an artificial potential field method in the process of advancing to the target point, and if the distance between the ith unmanned aerial vehicle and the obstacle is greater than the safe distance, continuing to advance to the target point; if the distance between the ith unmanned aerial vehicle and the obstacle is smaller than the safe distance, an improved artificial potential field method is introduced to avoid the obstacle until the target point is reached; the improved artificial potential field method obstacle avoidance is specifically as follows: Fiob and Fiatt are kept orthogonal, and when an included angle # imgabs0 # is formed between the Fiatt and the Fiob, only a component Fiobrep orthogonal to the Fiatt in the Fiob is reserved, and a component parallel to the Fiatt is removed, so that attraction is dominant in a total motion trend, and the efficiency of the unmanned aerial vehicle reaching a target point is improved.

Description

Technical Field

[0001] The present invention relates to the technical field of unmanned aerial vehicle (UAV) group coordination, and in particular to an unmanned aerial vehicle (UAV) formation obstacle avoidance control algorithm based on an improved artificial potential field method. Background Art

[0002] With the further development of UAV technology, the research on multi-UAV formation flight has also received more and more attention. UAVs cooperate with each other to form a formation group, which can give full play to the advantages that a single UAV does not have, and is more competent for work in complex and multi-task scenarios. UAV swarm formation control and obstacle avoidance is a typical problem in the research of UAV swarm coordination technology. It can be applied to the process of UAV swarms performing various mission scenarios to effectively avoid obstacles and avoid self-collision, and reduce unnecessary losses while completing tasks efficiently. UAV swarm coordination formation control requires the interconnection between swarms to be guaranteed on the basis of good communication, and the sharing of resources in different scenarios is realized by relying on the communication interaction between swarms. Obstacles in complex combat environments will pose a threat to UAV flight, and in severe cases may even lead to the failure of UAV combat missions. Therefore, having efficient obstacle avoidance capability is an important guarantee for UAVs to complete flight missions safely, and it has certain research significance for improving the accuracy, success rate and timeliness of swarm coordination in completing various tasks.

[0003] The leader-follower method is an earlier proposed method in multi-robot formation control. Its basic idea is to set a "leader" robot, and the other robots follow the position and speed of the leader according to certain rules to achieve the effect of formation control. The leader is usually responsible for setting the target, and the other followers maintain the formation through local control strategies. The control is relatively simple, usually only needing to calculate the relative position with the leader. However, the formation has poor stability. If the leader fails, the entire formation may collapse. It is not suitable for leader failure or dynamic changes in tasks.

[0004] The artificial potential field method is a path planning algorithm based on the concept of force field. It guides the drone or robot to avoid obstacles and move toward the target location by applying attractive and repulsive forces to the target and obstacles. The attraction (gravitational force) of the target point guides the robot toward the target, and the repulsive force of the obstacle prevents the robot from colliding with the obstacle. These forces are combined through a mathematical model to determine the direction of the robot's movement. The vector superposition of repulsive and attractive forces may result in a total force of zero at certain positions. The robot may reach equilibrium at a non-target position, that is, the gradient is zero, causing the robot to be unable to move forward. This is the local optimal problem. Summary of the invention

[0005] The purpose of the present invention is to provide a UAV formation obstacle avoidance control algorithm based on an improved artificial potential field method in order to solve the UAV local optimal problem existing in the artificial potential field method in the prior art.

[0006] The technical solution adopted to achieve the purpose of the present invention is:

[0007] A UAV formation obstacle avoidance control method based on an improved artificial potential field method comprises the following steps:

[0008] Step 1: Based on the virtual structure method, the drone swarm forms the desired formation within the specified time and determines the forward direction according to the current position and the target position;

[0009] Step 2: for the i-th UAV, in the process of moving towards the target point, determine whether the distance between the i-th UAV and the obstacle is greater than the safety distance. If the distance between the i-th UAV and the obstacle is greater than the safety distance, continue to move towards the target point; if the distance between the i-th UAV and the obstacle is less than the safety distance, introduce the improved artificial potential field method to avoid obstacles until reaching the target point;

[0010] Improved artificial potential field method to avoid obstacles is as follows:

[0011] The i-th UAV is subjected to the artificial potential force F when it moves towards the target point. i 'Including the attraction F of the target point to the i-th drone i att , the resultant force of the artificial potential field repulsion between the i-th UAV is F i rep and the artificial potential field repulsion F of the obstacle on the i-th UAV i ob , by improving the artificial potential field method i ob Make corrections so that the artificial potential field repulsion F of the obstacle on the i-th UAV i ob The attraction F of the target point to the i-th UAV i att Maintaining orthogonality, when F i att With F i ob The angle between When only F is retained i ob Medium and F i att Orthogonal components F i obrep , remove the i att Parallel components.

[0012] In the above technical solution, in step 2, the i-th UAV is attracted by the target point F i att The calculation formula is:

[0013]

[0014] In the formula, F i att is the attraction of the target point to the i-th UAV, U i att is the gravitational potential field.

[0015] In the above technical solution, the gravitational potential field U i att The function is expressed as:

[0016]

[0017] Then the i-th UAV is attracted by the target point F i att The calculation formula is:

[0018]

[0019] Where k is the integral proportional gain coefficient, |p i -p gi | is the position p of the i-th UAV i and the position of the target point p gi The Euclidean distance between .

[0020] In the above technical solution, the resultant force F of the repulsive force of the artificial potential field between the i-th drone in step 2 is i rep The calculation formula is:

[0021]

[0022] In the formula, F i rep is the resultant repulsive force of the artificial potential field between the i-th drones, is the artificial potential field repulsion between the i-th UAV and the j-th UAV, p ij =p i -p j is the displacement vector between the i-th UAV and the j-th UAV, and N is the number of UAVs.

[0023] In the above technical solution, the artificial potential field repulsion between the i-th UAV and the j-th UAV The calculation formula is:

[0024]

[0025] In the formula, is the repulsive potential field between the i-th UAV and the j-th UAV.

[0026] In the above technical solution, the repulsive potential field between the i-th UAV and the j-th UAV is The function is expressed as:

[0027]

[0028] Where η is the repulsive force gain, d ij is the relative safety distance between the i-th UAV and the j-th UAV;

[0029] Assume that the maximum range of repulsion is D max , then the repulsive force between the i-th UAV and the j-th UAV is It is expressed as:

[0030]

[0031] Where D max It is the maximum range within which the repulsive force can act.

[0032] In the above technical solution, in step 2, F i ob Medium and F i att Orthogonal components F i obrep The calculation formula is as follows:

[0033]

[0034] In the formula, F i ob is the artificial potential field repulsion of the obstacle on the i-th UAV, F i att With F i ob The angle between To improve F i ob The coefficient of is the identity matrix, γ i is the adjustment factor, F i att is the attraction of the target point to the i-th UAV, is the matrix transpose of the attraction of the target point to the i-th UAV.

[0035] In the above technical solution, the artificial potential field repulsion F of the obstacle on the i-th UAV is i ob The calculation formula is as follows:

[0036]

[0037] Where Ui ob is the repulsive potential field between the obstacle and the i-th UAV.

[0038] In the above technical solution, the repulsive potential field U between the obstacle and the i-th UAV is i ob The calculation formula is:

[0039]

[0040] Then the artificial potential field repulsion F of the obstacle on the i-th UAV is i ob The calculation formula is:

[0041]

[0042] Where η is the repulsive force gain, d i is the distance between the ith UAV and the obstacle, p0 = [x0, y0, z0] is the coordinate of the obstacle, p i is the coordinate position of the i-th UAV.

[0043] In the above technical solution, the i-th UAV is subjected to the artificial potential force F i The calculation formula of ' is:

[0044] F i '=F i att +F i rep +F i obrep

[0045] In the formula, F i ' is the force of artificial potential on the i-th UAV, F i att is the attraction of the target point to the i-th UAV, F i rep is the resultant force of the artificial potential field repulsion between the i-th UAV, F i ob Medium and F i att Orthogonal components F i obrep .

[0046] Compared with the prior art, the present invention has the following beneficial effects:

[0047] 1. The present invention adopts a virtual structure method for formation control. Through the virtual structure, all drones are regarded as a whole to keep the relative position stable. All drones are controlled according to a predetermined virtual structure (rigid body or geometric shape). Each robot has an independent control strategy and does not rely on a single leader. This makes the entire control process more stable. All drones participate in formation control, reducing the risk of single point failure. The entire formation is more adaptable to dynamic environments and fault recovery, and has stronger adaptability.

[0048] 2. The present invention adopts the improved artificial potential field method in F i att With F i ob The angle between When the obstacle's repulsion and attraction on the i-th UAV are orthogonal, the repulsion and attraction of the obstacle on the i-th UAV are retained. i att Orthogonal components, delete the components related to F i att This improvement reduces the situation where the attraction is suppressed by the repulsive force, thereby reducing the occurrence of local minimum points, making the attraction dominate the overall motion trend and improving the efficiency of the drone in reaching the target point. BRIEF DESCRIPTION OF THE DRAWINGS

[0049] Figure 1 Shown is a flow chart of obstacle avoidance for a swarm of drones according to the control method of the present invention.

[0050] Figure 2 Shown is a schematic diagram of the virtual structure method in the control method of the present invention.

[0051] Figure 3 Shown is a schematic diagram of the formation under the relevant coordinate system and virtual structure in the control method of the present invention.

[0052] Figure 4 Shown is a schematic diagram of obstacle avoidance in the artificial potential field method improved by the control method of the present invention. DETAILED DESCRIPTION

[0053] The present invention is further described in detail below in conjunction with specific embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not used to limit the present invention.

[0054] Reference Figure 1 , a UAV group formation obstacle avoidance control method based on an improved artificial potential field method, comprising the following steps:

[0055] Step 1: Based on the virtual structure method, the drone swarm forms the desired formation within the specified time and determines the direction of travel according to the current position and target position.

[0056] Reference Figure 2The virtual structure method regards the entire drone swarm as a virtual rigid structure, and the position of each drone as a fixed point on the rigid structure. When the formation moves, the drone tracks the virtual structure particle to achieve the purpose of collaborative control. This method allows the entire drone swarm to form a relatively stable geometric structure. The main focus of the virtual structure method is: 1) clarifying the geometric formation of the formation; 2) the formation coordinates remain unchanged, and plan your own path in combination with the mission.

[0057] The establishment of virtual structure:

[0058] The core of the virtual structure method is to regard the entire UAV formation as a rigid whole (virtual structure). The movement of the UAV is determined by the trajectory of the virtual structure and the relative position of the UAV in the virtual structure. The actual position of each UAV is the superposition of the virtual structure trajectory and its offset in the formation. The offset is the relative position of each UAV in the virtual structure, and its function is to maintain the distance between UAVs and the shape of the formation. By adding different offsets on the basis of the virtual trajectory, it is ensured that the relative position of the UAV in the three-dimensional space meets the formation requirements. (x1, y1, z1,) are the coordinates of the virtual center point, then:

[0059] x=x1+offsetX

[0060] y=y1+offsetY

[0061] z=z1+offsetZ

[0062] Reference Figure 3 , the virtual structure method requires that the drones maintain a fixed relative position to ensure the stability and consistency of the formation. Assuming that the calculation of the offset satisfies the predetermined geometric relationship (such as rectangle, rhombus, etc.), the entire formation can remain rigid, and the offset (offsetX, offsetY, offsetZ) is used to ensure that the distance between the drones is constant. As time t changes, the trajectory of the center point of the virtual structure changes dynamically, and the actual trajectory of the drone formation is adjusted accordingly, but the relative position in the formation remains unchanged. Each drone performs independent movements under its own number, and the consistency of the formation is achieved through the offset and trajectory formula.

[0063] Step 2: for the i-th UAV, in the process of moving towards the target point, determine whether the distance between the i-th UAV and the obstacle is greater than the safety distance. If the distance between the i-th UAV and the obstacle is greater than the safety distance, continue to move towards the target point; if the distance between the i-th UAV and the obstacle is less than the safety distance, introduce the improved artificial potential field method to avoid obstacles until reaching the target point;

[0064] Improved artificial potential field method obstacle avoidance is as follows:

[0065] The i-th UAV is subjected to the artificial potential force F when it moves towards the target point. i 'Including the attraction F of the target point to the i-th UAV i att , the resultant force of the artificial potential field repulsion between the i-th UAV is F i rep and the artificial potential field repulsion F of the obstacle on the i-th UAV i ob , by improving the artificial potential field method i ob Make corrections so that the artificial potential field repulsion F of the obstacle on the i-th UAV i ob The attraction F of the target point to the i-th UAV i att Maintaining orthogonality, when F i att With F i ob The angle between When only F is retained i ob Medium and F i att Orthogonal components F i obrep , remove the i att Parallel components.

[0066] The gravitational potential field is mainly related to the distance between the drone and the target point. When the drone is far away from the target point, the gravitational potential field value of the drone is larger; when the drone is close to the target point, the gravitational potential field value of the drone is smaller. Therefore, the gravitational potential field U of the target point on the i-th drone is i att The function is expressed as:

[0067]

[0068] Where k is the integral proportional gain coefficient, |p i -p gi | is the robot position p i and the target point position p gi The Euclidean distance between ||p i -p gi ||, and its direction is from the position of the i-th UAV to the position of the target point. The corresponding negative gradient of the gravitational potential field is the attraction F of the target point to the i-th UAV i att It can be expressed as:

[0069]

[0070] In the formula, Fi att is the attraction of the target point to the i-th UAV, U i att is the gravitational potential field, k is the integral gain coefficient, |p i -p gi | is the position p of the i-th UAV i and the position of the target point p gi The Euclidean distance between .

[0071] The obstacle avoidance between drones adopts the traditional artificial potential field method, and then calculates the resultant force F of the artificial potential field repulsion between drones. i rep .

[0072] The artificial potential field method avoids conflicts among individuals in the formation by utilizing the repulsive potential between drones, and avoids external obstacles by utilizing the repulsive potential of obstacles on drones. In this method, the force generated by the artificial potential field method is converted into a desired position and vector-superimposed with the desired position calculated by the position of the drones in the swarm, thereby forming a new desired position, so that the drones in the formation are affected by the gravitational potential field. In space, the equilibrium point between the gravitational potential field and the repulsive potential field is the key condition for the drone formation to reach a stable state. In the dynamic trajectory planning process, the overall artificial potential field of the drone swarm is formed by the superposition of the artificial potential fields generated by all drones in the formation, where the artificial potential field U of the i-th drone source and the j-th drone source is the same as the gravitational potential field U of the j-th drone source. ij The function is expressed as:

[0073] U ij =U ij (|p ij |)

[0074] Where U ij is the artificial potential field generated by the jth UAV at the position of the i-th UAV; |p ij | is the position p of the i-th UAV i and the position p of the jth UAV j The displacement vector between them is defined as p ij =p i -p j .U ij The following conditions are met, when |p ij |→D min+ When U ij →+∞,U ij (|p ij |) in the interval (D min ,+∞) has a unique minimum value J ij (D) The total potential field value U of the i-th UAV i It can be expressed as the superposition result of the artificial potential field of multiple points on the i-th UAV, that is:

[0075]

[0076] Where U ij (p ij |) is the artificial potential field of the ith UAV at a certain point in the environment, and N is the number of UAVs.

[0077] In the UAV formation control based on the virtual structure method, the expected position calculated according to the position of the target point has a gravitational effect on the UAV, so the artificial potential field function between the drones no longer needs to consider gravitational force, only repulsive force needs to be considered, which can simplify the complexity of the method and improve the computing performance. The design is an inverse proportional function, that is:

[0078]

[0079] In the formula, is the repulsive potential field between the i-th UAV and the j-th UAV, η is the repulsive gain, d ij is the relative safety distance between the i-th UAV and the j-th UAV, |p ij | is the position p of the i-th UAV i and the position p of the jth UAV j The displacement vector between them is defined as p ij =p i -p j .

[0080] The repulsive force between the i-th UAV and the j-th UAV for:

[0081]

[0082] It can be seen that if the safety distance d between the i-th UAV and the j-th UAV ij The larger the value, the greater the repulsion and the stronger the repulsion effect. In summary, considering that the addition of the artificial potential field method will change the expected position of the current UAV, thereby reducing the control accuracy of the formation, in order to reduce its greater impact on the accuracy, the range of repulsion is limited. When the UAV formation enters a steady state and the distance between the two UAVs participating in the formation is large, the formation control can ensure the control accuracy of the formation as much as possible. Assume that the maximum range of repulsion is D max , then the repulsive force between the i-th UAV and the j-th UAV is It is expressed as:

[0083]

[0084] Therefore, the resultant force F of the artificial potential field repulsion between drones on the i-th drone isi rep for:

[0085]

[0086] The effectiveness of the artificial potential field method depends on the design of the potential field. A reasonable potential field can avoid local minimum problems and improve the efficiency of path planning. By designing a suitable potential field function, the shape of the vector field can be controlled, thereby optimizing path planning and obstacle avoidance performance.

[0087] Unlike collision avoidance between drones, obstacles are fixed. When a drone avoids an obstacle, it can only unilaterally control the drone to change its flight path to avoid it, but cannot control the obstacle. Therefore, the artificial potential field between the drone and the obstacle is only generated by the obstacle. Suppose the coordinates of an obstacle in the airspace are p0 = [x0, y0, z0] T , the artificial potential field U between the obstacle and the i-th UAV i ob The function is expressed as:

[0088]

[0089] Where, d i is the distance between the ith UAV and the obstacle, then the artificial potential field repulsion F on the ith UAV is i ob for:

[0090]

[0091] Where U i ob is the repulsive potential field between the obstacle and the i-th UAV, η is the repulsive gain, d i is the distance between the ith UAV and the obstacle, is the coordinate of the obstacle, p i is the coordinate position of the i-th UAV.

[0092] This embodiment improves the traditional artificial potential field method (APF), and its core improvement is to correct the repulsive force F i ob , making it attractive to F i att Maintain an orthogonal relationship. i ob Attraction F i att In the case of inhibition, only F i ob Medium and F i att Orthogonal components Fi obrep , that is, when When the orthogonal component F is retained i obrep , and will be with F i att The parallel components are directly removed. This improvement effectively improves the computational efficiency of the algorithm. It reduces the interference between the attractive and repulsive forces and reduces the probability of the occurrence of local minima. Figure 4 , define an orthogonal projection matrix, from the repulsive force F i ob Remove and attract F i att Parallel components, only the orthogonal components F are retained i obrep . Ensure that the interference of repulsive force on attractive force is minimized and improve the efficiency of path planning. i ob Medium and F i att Orthogonal components F i obrep The calculation formula is as follows:

[0093]

[0094] In the formula, F i att With F i ob The angle between To improve F i ob The coefficient of is the identity matrix, γ i is the adjustment factor, F i att is the attraction of the target point to the i-th UAV, is the matrix transpose of the attraction of the target point to the i-th UAV.

[0095] Compared with the traditional artificial potential field method, the correction method proposed in this embodiment removes (I n -P ma,i )F i ob , it is F i ob The weight of F i att Obviously, this improved strategy makes F i att Taking up more space makes the robot more inclined to reach the target point.

[0096] In summary, the artificial potential force on the i-th UAV is:

[0097] F i '=F i att +F i rep +F i obrep

[0098] In the formula, F i ' is the force of artificial potential on the i-th UAV, F i att is the attraction of the target point to the i-th UAV, F i rep is the resultant force of the artificial potential field repulsion between the UAVs, F i obrep F i ob Medium and F i att Orthogonal components F i obrep .

[0099] The above is only a preferred embodiment of the present invention. It should be pointed out that, for ordinary technicians in this technical field, several improvements and modifications can be made without departing from the principle of the present invention. These improvements and modifications should also be regarded as the scope of protection of the present invention.

Claims

1. A UAV formation obstacle avoidance control method based on an improved artificial potential field method, characterized in that: The following steps are involved: Step 1: Based on the virtual structure method, the drone swarm forms the desired formation within the specified time and determines the forward direction according to the current position and the target position; Step 2: for the i-th UAV, in the process of moving towards the target point, determine whether the distance between the i-th UAV and the obstacle is greater than the safety distance. If the distance between the i-th UAV and the obstacle is greater than the safety distance, continue to move towards the target point; if the distance between the i-th UAV and the obstacle is less than the safety distance, introduce the improved artificial potential field method to avoid obstacles until reaching the target point; Improved artificial potential field method to avoid obstacles is as follows: The i-th UAV is subjected to the artificial potential force F when it moves towards the target point. i 'Including the attraction of the target point to the i-th drone The resultant force of the artificial potential field repulsion between the i-th UAV and the artificial potential field repulsion of obstacles on the i-th UAV By improving the artificial potential field method Make corrections so that the artificial potential field repulsion of the obstacle on the i-th UAV The attraction of the target point to the i-th UAV Maintaining orthogonality, when and The angle between When only Zhongyu Orthogonal components Remove and Parallel components.

2. The obstacle avoidance control method for UAV group formation based on the improved artificial potential field method according to claim 1, characterized in that: In step 2, the i-th drone is attracted by the target point The calculation formula is: In the formula, is the attraction of the target point to the i-th UAV, is the gravitational potential field.

3. The obstacle avoidance control method for UAV group formation based on the improved artificial potential field method as claimed in claim 2, characterized in that: Gravitational potential field The function is expressed as: Then the i-th UAV is attracted by the target point The calculation formula is: Where k is the integral proportional gain coefficient, |p i -p gi | is the position p of the i-th UAV i and the position of the target point p gi The Euclidean distance between .

4. The obstacle avoidance control method for UAV group formation based on improved artificial potential field method as claimed in claim 1, characterized in that: The resultant force of the artificial potential field repulsion between the i-th drone in step 2 The calculation formula is: In the formula, is the resultant repulsive force of the artificial potential field between the i-th drones, is the artificial potential field repulsion between the i-th UAV and the j-th UAV, p ij =p i -p j is the displacement vector between the i-th UAV and the j-th UAV, and N is the number of UAVs.

5. The obstacle avoidance control method for UAV group formation based on improved artificial potential field method as claimed in claim 1, characterized in that: Artificial potential field repulsion between the i-th UAV and the j-th UAV The calculation formula is: In the formula, is the repulsive potential field between the i-th UAV and the j-th UAV.

6. The obstacle avoidance control method for UAV group formation based on improved artificial potential field method as claimed in claim 5, characterized in that: The repulsive potential field between the i-th UAV and the j-th UAV The function is expressed as: Where η is the repulsive force gain, d ij is the relative safety distance between the i-th UAV and the j-th UAV; Assume that the maximum range of repulsion is D max , then the repulsive force between the i-th UAV and the j-th UAV is It is expressed as: Where D max It is the maximum range within which the repulsive force can act.

7. The obstacle avoidance control method for UAV group formation based on the improved artificial potential field method as claimed in claim 1, characterized in that In, In step 2 Zhongyu Orthogonal components The calculation formula is as follows: In the formula, is the artificial potential field repulsion of the obstacle on the i-th UAV, for and The angle between For improvement The coefficient of is the unit matrix, γi is the adjustment factor, is the attraction of the target point to the i-th UAV, is the matrix transpose of the attraction of the target point to the i-th UAV.

8. The obstacle avoidance control method for UAV group formation based on improved artificial potential field method as claimed in claim 7, characterized in that: The artificial potential field repulsion of obstacles on the i-th UAV The calculation formula is as follows: In the formula, is the repulsive potential field between the obstacle and the i-th UAV.

9. The obstacle avoidance control method for UAV swarm formation based on improved artificial potential field method as claimed in claim 7, characterized in that: The repulsive potential field between the obstacle and the i-th UAV The calculation formula is: The artificial potential field repulsion of the obstacle on the i-th UAV is The calculation formula is: Where η is the repulsion gain, di is the distance between the i-th UAV and the obstacle, p0 = [x0, y0, z0] is the coordinate of the obstacle, and pi is the coordinate position of the i-th UAV.

10. The obstacle avoidance control method for UAV group formation based on improved artificial potential field method according to claim 1, characterized in that: The calculation formula of the artificial potential field stress Fi' on the i-th UAV is: In the formula, F i ' is the force exerted by the artificial potential on the i-th UAV, is the attraction of the target point to the i-th UAV, is the resultant repulsive force of the artificial potential field between the i-th drones, Zhongyu Orthogonal components

Images