An unmanned aerial vehicle trajectory obstacle avoidance method based on prior artificial potential field

Abstract

The application discloses a kind of unmanned aerial vehicle trajectory obstacle avoidance methods based on prior artificial potential field, comprising the following steps: S1, constructs virtual force field, sets potential field model;S2, according to potential force field model, predict the path in future period of time, obtain the attitude angle change rate in future period of time, according to attitude angle change rate obtains angle offset;S3, angle offset is superimposed to unmanned aerial vehicle current state, according to potential field model, obtain the unmanned aerial vehicle position of next time, and move to the position;S4, repeat S2-S3, until unmanned aerial vehicle reaches final target point.The unmanned aerial vehicle trajectory obstacle avoidance method based on prior artificial potential field disclosed in the application solves the problem that the attitude angle changes sharply when unmanned aerial vehicle flies, and the flight stability is low, improves the flight trajectory smoothness;Improve the problem that classical artificial potential field method is easy to fall into local minimum point, improve the complexity of unmanned aerial vehicle application scene.

Description

Technical Field

[0001] This invention relates to a method for obstacle avoidance of unmanned aerial vehicle (UAV) trajectories based on a priori artificial potential fields, belonging to the field of navigation and control technology. Background Technology

[0002] Artificial potential field (APF) is one of the commonly used obstacle avoidance methods for UAVs.

[0003] However, existing artificial potential field methods are prone to producing local minima, which can cause UAVs to lose their direction of travel and fail to find the target point during path planning because the net force is zero.

[0004] In addition, existing artificial potential field methods can lead to a large rate of change of attitude angle during drone flight in some cases, such as in complex environments like narrow corridors, which can easily cause flight hazards.

[0005] Therefore, it is necessary to further study existing drone trajectory obstacle avoidance methods in order to solve the above problems. Summary of the Invention

[0006] To overcome the above problems, the inventors conducted in-depth research and provided a UAV trajectory obstacle avoidance method based on a priori artificial potential field, comprising the following steps:

[0007] S1. Construct a virtual force field and set up a potential field model;

[0008] S2. Predict the path over a future period based on the force field model, obtain the attitude angle change rate over the future period, and obtain the angle offset based on the attitude angle change rate.

[0009] S3. Add the angle offset to the current state of the drone, obtain the drone's position at the next moment according to the power field model, and move to that position;

[0010] S4. Repeat S2-S3 until the drone reaches the final target point.

[0011] In a preferred embodiment, the potential field model includes a repulsive potential energy function, a repulsive function, a gravitational potential energy function, and a gravitational function.

[0012] In a preferred embodiment, the repulsive potential energy function is set as follows:

[0013]

[0014] The repulsion function is set as follows:

[0015]

[0016] Where, k r d is the repulsive potential energy coefficient. ao d represents the distance between the drone and the obstacle. r d represents the range of influence of the obstacle. s To maintain a safe distance when avoiding obstacles, d at This indicates the distance between the drone and the target point.

[0017] In a preferred embodiment, the gravitational potential energy function Set to:

[0018]

[0019] The gravitational function Set to:

[0020]

[0021] Where, k a d is the gravitational potential energy coefficient. at This indicates the distance between the drone and the target point.

[0022] In a preferred embodiment, the potential field model further includes a boundary repulsion potential field function and a boundary repulsion function.

[0023] In a preferred embodiment, the boundary repulsive potential field function Set to:

[0024]

[0025] The boundary repulsion function Set to:

[0026]

[0027] Where, k edge d is the boundary repulsive potential energy coefficient. ae d represents the distance between the drone and the boundary. e The range of influence of the boundary repulsive potential energy.

[0028] In a preferred embodiment, in S2, the position and attitude angle of the UAV at the next moment are obtained by calculating the resultant force of gravity and repulsion according to the force field model.

[0029] The obtained position and attitude angle of the UAV at the next moment are used as the actual position and attitude angle of the UAV at the next moment. The position and attitude angle of the UAV at the next moment are obtained again. The above process is repeated to obtain the attitude angle of the UAV at the Nth moment in the future. The difference between this attitude angle and the attitude angle of the UAV in the initial state is the rate of change of attitude angle between the initial moment and the Nth moment in the future.

[0030] In a preferred embodiment, an angle change threshold is set, and when the rate of change of the attitude angle is greater than the threshold, the angle offset is set to a preset offset Δθ.

[0031] When the rate of change of attitude angle is less than the threshold, the angle offset is 0.

[0032] The beneficial effects of this invention include:

[0033] (1) It solves the problem of low flight stability caused by rapid changes in attitude angle during UAV flight and improves the smoothness of flight trajectory;

[0034] (2) It improves the problem that the classical artificial potential field method is prone to getting trapped in local minima and increases the complexity of the application scenarios of UAVs;

[0035] (3) Improved safety during flight. Attached Figure Description

[0036] Figure 1 A schematic flowchart of a UAV trajectory obstacle avoidance method based on a priori artificial potential field according to a preferred embodiment of the present invention is shown.

[0037] Figure 2 The diagram shows the S2-S4 flow chart of a UAV trajectory obstacle avoidance method based on a priori artificial potential field according to a preferred embodiment of the present invention.

[0038] Figure 3 The drone trajectory obtained in Example 1 is shown;

[0039] Figure 4 The drone trajectory obtained in Comparative Example 1 is shown.

[0040] Figure 5 The diagram shows a comparison of the distances between the drone and the obstacle in Comparative Example 1 and Comparative Example 1. Detailed Implementation

[0041] The present invention will be further described in detail below with reference to the accompanying drawings and embodiments. Through these descriptions, the features and advantages of the present invention will become clearer and more apparent.

[0042] The term “exemplary” as used herein means “serving as an example, embodiment, or illustration.” Any embodiment illustrated herein as “exemplary” is not necessarily to be construed as superior to or better than other embodiments. Although various aspects of embodiments are shown in the accompanying drawings, the drawings are not necessarily drawn to scale unless specifically indicated otherwise.

[0043] According to the present invention, a method for obstacle avoidance of unmanned aerial vehicle (UAV) trajectory based on a priori artificial potential field is provided, such as... Figure 1 As shown, it includes the following steps:

[0044] S1. Construct a virtual force field and set up a potential field model;

[0045] S2. Predict the path over a future period based on the force field model, obtain the attitude angle change rate over the future period, and obtain the angle offset based on the attitude angle change rate.

[0046] S3. Add the angle offset to the current state of the drone, obtain the drone's position at the next moment according to the power field model, and move to that position;

[0047] S4. Repeat S2-S3 until the drone reaches the final target point.

[0048] Virtual force fields are the foundation of artificial potential field methods. Path planning using artificial potential field methods is a virtual force method proposed by Khatib (Oussama Khatib, Real-Time Obstacle Avoidance for Manipulators and Mobile Robots. Proc of The 1985 IEEE). Its basic idea is to design the movement of a drone in its surrounding environment as movement within an abstract artificial gravitational field. The target point exerts an "attractive force" on the drone, while obstacles exert a "repulsive force." Finally, the drone's movement is controlled by calculating the resultant force. In other words, a virtual force field is constructed for the drone's flight environment, and the drone plans an obstacle avoidance path under the influence of this virtual force field. The virtual force field mainly consists of two parts: first, the repulsive potential field exerted on the drone by obstacles and boundaries, with directions along the tangent to the obstacle's area and perpendicular to the boundary, respectively; second, the gravitational potential field exerted on the drone by the target point, directed from the drone towards the target point, causing the drone to move towards a predetermined position. The repulsive potential field and the gravitational potential field work together to form an artificial potential field. The UAV searches for a collision-free path by looking for the direction in which the potential field function decreases, that is, the direction in which the net force on it decreases.

[0049] In this invention, the potential field model in the traditional artificial potential field method is improved to adapt to the complex scenarios in actual UAV flight.

[0050] Specifically, the potential field model includes a repulsive potential energy function, a repulsive function, a gravitational potential energy function, and a gravitational function.

[0051] Preferably, the repulsive potential energy function Set to:

[0052]

[0053] The repulsion function Set to:

[0054]

[0055] Where, k r d is the repulsive potential energy coefficient. ao d represents the distance between the drone and the obstacle. r d represents the range of influence of the obstacle. s To maintain a safe distance when avoiding obstacles, d at This indicates the distance between the drone and the target point.

[0056] Compared to the repulsive potential energy function and repulsive force function in the traditional artificial potential field method, the repulsive potential energy function and repulsive force function set in this invention enable the drone to accurately stop at the target point, and there is no situation where the drone can only wander near the destination due to the target position being within the range of the repulsive force of the obstacle.

[0057] In a preferred embodiment, the gravitational potential energy function Set to:

[0058]

[0059] The gravitational function Set to:

[0060]

[0061] Where, k a d is the gravitational potential energy coefficient. at This indicates the distance between the drone and the target point.

[0062] Preferably, in this invention, the potential field model further includes a boundary repulsive potential field function and a boundary repulsive function. By setting the boundary repulsive potential field function and the boundary repulsive function, the UAV can adapt to scenarios with narrow flight paths.

[0063] In a preferred embodiment, the boundary repulsive potential field function Set to:

[0064]

[0065] The boundary repulsion function Set to:

[0066]

[0067] Where, k edge d is the boundary repulsive potential energy coefficient. ae d represents the distance between the drone and the boundary. e The range of influence of the boundary repulsive potential energy.

[0068] In this invention, by specifically designing the repulsive boundary potential field function and the boundary repulsive force function, the problem of the classical artificial potential field method easily getting trapped in local minima is improved, the actual flight distance of the UAV is reduced, and the UAV is protected.

[0069] Furthermore, according to the principle of potential field superposition, the total potential field function of the UAV in the motion space can be obtained:

[0070]

[0071] The resultant force of the drone in the space of motion is:

[0072]

[0073] In the traditional artificial potential field method, the position of the UAV at the next moment is directly obtained based on the potential field model, and then the UAV moves to that position, repeating this process until the UAV reaches the final target point. However, in the narrow environment of a connecting corridor, this method can cause the UAV's attitude angle change rate to be too large during actual flight, thus creating flight hazards.

[0074] In this invention, the path is predicted over a future period of time, the rate of change of attitude angles is obtained over the future period of time, the angle offset is obtained based on the rate of change of attitude angles, and then the current state of the UAV is corrected based on the angle offset. The position of the UAV at the next moment is obtained based on the corrected state, thereby slowing down the change of attitude angles of the UAV in flight, avoiding abrupt changes in attitude angles, and thus improving flight safety.

[0075] According to the present invention, in S2, the position and attitude angle of the UAV at the next moment can be obtained by calculating the resultant force of gravity and repulsion based on the force field model;

[0076] The calculation methods for the specific UAV position and attitude angle are the same as those in the traditional artificial potential field method, and will not be described in detail in this invention.

[0077] Furthermore, such as Figure 2 As shown, the position and attitude angle of the UAV at the next moment are used as the actual position and attitude angle of the UAV at the next moment. The position and attitude angle of the UAV at the next moment are then obtained. The above process is repeated to obtain the attitude angle of the UAV at the Nth moment in the future. The difference between this attitude angle and the attitude angle of the UAV in the initial state is the rate of change of attitude angle between the initial moment and the Nth moment in the future.

[0078] In a preferred embodiment, an angle change threshold θ is also provided. max When the rate of change of attitude angle is greater than the preset threshold, the angle offset is set to the preset offset Δθ; when the rate of change of attitude angle is less than the preset threshold, the angle offset is 0.

[0079] In this invention, by setting an angle change threshold, severe flight jitter caused by excessive attitude angle change rate is prevented. Specifically, when the predicted attitude angle change rate is large, the UAV state is corrected in advance, and the UAV position at the next moment is obtained again through the force field model, so as to prevent the flight trajectory from changing drastically due to excessive attitude angle change rate, resulting in flight jitter and making subsequent trajectory tracking difficult to control.

[0080] In this invention, by performing angle correction in advance, the system can adapt to attitude changes in a timely manner during flight and maintain stable flight, thereby improving the robustness and reliability of the entire control system.

[0081] In a preferred embodiment, based on practical experience, the angle change threshold is set to θ. max =10°, and set the preset angle bias to Δθ = 1°. This value setting has consistently yielded good control results through numerous practical engineering experiments. Compared to other parameters, the above value setting results in a smoother trajectory for the UAV.

[0082] In a preferred embodiment, the specific setting value of the future Nth time can be freely set by those skilled in the art according to actual needs. Preferably, it is set according to the sampling frequency of the UAV's flight status. More preferably, the time when the UAV performs the 10th sampling is taken as the future Nth time.

[0083] In S3, the angle offset is superimposed on the attitude angle of the UAV, and the position and attitude of the UAV at the next moment are obtained again according to the artificial potential field method, and the UAV is controlled to move to that position.

[0084] In this invention, the trajectory of the UAV is optimized by predicting the flight state in the future time domain in advance and increasing the attitude angle in advance.

[0085] Example

[0086] Example 1

[0087] Conduct obstacle avoidance simulation tests on drones, setting the drone's takeoff position to [0m 0m 0m]. T The target point is located at [20m 20m 20m]. T A spherical obstacle with a radius of 0.7m and a narrow corridor are set on the line connecting the starting point and the target point. The obstacle information is shown in Table 1.

[0088] Table 1 Obstacle Information

[0089] 1 <![CDATA[[3 2.5 3] T ]]> 6 <![CDATA[[11 8 8] T ]]> 2 <![CDATA[[3.5 4.5 3.5] T ]]> 7 <![CDATA[[15 10.5 10] T ]]> 3 <![CDATA[[5.5 7 5.5] T ]]> 8 <![CDATA[[17 12.5 13.5] T ]]> 4 <![CDATA[[6.5 7 6.5] T ]]> 9 <![CDATA[[18 15 15.5] T ]]> 5 <![CDATA[[7.5 7 7.5] T ]]> 10 <![CDATA[[18.5 16 16] T ]]>

[0090] The following methods are used for drone trajectory obstacle avoidance:

[0091] S1. Construct a virtual force field and set up a potential field model;

[0092] S2. Predict the path over a future period based on the force field model, obtain the attitude angle change rate over the future period, and obtain the angle offset based on the attitude angle change rate.

[0093] S3. Add the angle offset to the current state of the drone and obtain the drone's position at the next moment based on the power field model;

[0094] S4. Repeat S2-S3 until the drone reaches the final target point.

[0095] The potential field model includes a repulsive potential energy function, a repulsive function, a gravitational potential energy function, a gravitational function, a boundary repulsive potential field function, and a boundary repulsive function.

[0096] The repulsive potential energy function is set as follows:

[0097]

[0098] The repulsion function is set as follows:

[0099]

[0100] The gravitational potential energy function Set to:

[0101]

[0102] The gravitational function Set to:

[0103]

[0104] The boundary repulsive potential field function Set to:

[0105]

[0106] The boundary repulsion function Set to:

[0107]

[0108] In S2, the position and attitude angle of the UAV at the next moment are obtained by calculating the resultant force of gravity and repulsion based on the force field model.

[0109] The obtained drone position and attitude angle are used as the actual position and attitude angle of the drone at the next moment. The position and attitude angle of the drone at the next moment are obtained again. The above process is repeated to obtain the attitude angle of the drone at the Nth future moment. The difference between this attitude angle and the attitude angle of the drone in the initial state is the rate of change of attitude angle between the initial moment and the Nth future moment.

[0110] The angle change threshold is set to 10°. When the rate of change of the attitude angle exceeds the threshold, the angle offset is set to a preset offset Δθ to correct the angle at the current sampling moment, preventing drastic fluctuations in the flight trajectory caused by excessively rapid changes in the attitude angle. When the rate of change of the attitude angle is less than the threshold, the angle offset is 0.

[0111] The preset offset Δθ = 1° is used, and the Nth time is the time when the UAV samples the flight status for the 10th time.

[0112] Comparative Example 1

[0113] The same experiment as in Example 1 was conducted, except that the traditional artificial potential field method was used. The traditional artificial potential field method can be found in the reference [Oussama Khatib, Real-Time Obstacle Avoidance for Manipulators and Mobile Robots. Proc of The 1985 IEEE.], and will not be described in detail in this invention.

[0114] The drone trajectory obtained in Example 1 is as follows Figure 3 As shown, the drone trajectory obtained in Comparative Example 1 is as follows: Figure 4 As shown in the figure, the curve represents the drone's trajectory, and the solid dots represent obstacles. It can be seen from the figure that both the methods in Example 1 and Comparative Example 1 can avoid obstacles and reach the designated target position. However, the drone trajectory in Comparative Example 1 is more tortuous, and its attitude angle is prone to significant abrupt changes, making control more difficult and more likely to fall into local extremes. In contrast, the method in Example 1 produces a significantly smoother trajectory without drone attachments, ensuring safety during flight and making it easier for the controller to track the drone.

[0115] Table 2 shows the path lengths of the UAVs in Example 1 and Comparative Example 1. As can be seen from the table, the actual trajectory length of the UAV in Example 1 is significantly reduced due to the smoother trajectory.

[0116] Table 2 Path Length

[0117] Example 1 195 Comparative Example 1 232

[0118] Comparing the distances between the drone and obstacles in Example 1 and Comparative Example 1, the results are as follows: Figure 5As shown in the figure, both Example 1 and Comparative Example 1 ensure that collisions with obstacles are prevented during flight. However, in Example 1, the distance difference between the drone and the obstacle is larger, the path is smoother, and it is easier to ensure safety during flight.

[0119] In the description of this invention, it should be noted that the terms "upper," "lower," "inner," "outer," "front," and "rear," etc., indicate the orientation or positional relationship based on the orientation or positional relationship in the working state of this invention, and are only for the convenience of describing this invention and simplifying the description, and do not indicate or imply that the device or element referred to must have a specific orientation, or be constructed and operated in a specific orientation, and therefore should not be construed as a limitation of this invention. Furthermore, the terms "first," "second," "third," and "fourth" are used for descriptive purposes only and should not be construed as indicating or implying relative importance.

[0120] In the description of this invention, it should be noted that, unless otherwise explicitly specified and limited, the terms "installation," "connection," and "linking" should be interpreted broadly. For example, they can refer to a fixed connection, a detachable connection, or an integral connection; they can refer to a mechanical connection or an electrical connection; they can refer to a direct connection or an indirect connection through an intermediate medium; and they can refer to the internal communication between two components. Those skilled in the art can understand the specific meaning of the above terms in this invention based on the specific circumstances.

[0121] The present invention has been described above with reference to preferred embodiments; however, these embodiments are merely exemplary and illustrative. Various substitutions and modifications can be made to the present invention based on these embodiments, all of which fall within the scope of protection of the present invention.

Claims

1. A method for obstacle avoidance of unmanned aerial vehicle (UAV) trajectories based on prior artificial potential fields, characterized in that, Includes the following steps: S1. Construct a virtual force field and set up a potential field model; S2. Predict the path over a future period based on the force field model, obtain the attitude angle change rate over the future period, and obtain the angle offset based on the attitude angle change rate. S3. Add the angle offset to the current state of the drone, obtain the drone's position at the next moment according to the power field model, and move to that position; S4. Repeat S2-S3 until the drone reaches the final target point. The potential field model includes a repulsive potential energy function, a repulsive function, a gravitational potential energy function, and a gravitational function. The repulsive potential energy function is set as follows: , The repulsion function is set as follows: , in, The repulsive potential energy coefficient is... The distance between the drone and the obstacle. The area affected by the obstacle. To maintain a safe distance when avoiding obstacles, Indicates the distance between the drone and the target point. The gravitational potential energy function Set to: , The gravitational function Set to: , in, The gravitational potential energy coefficient, Indicates the distance between the drone and the target point. In S2, the position and attitude angle of the UAV at the next moment are obtained by calculating the resultant force of gravity and repulsion based on the force field model. The obtained next-moment UAV position and attitude angle are used as the actual position and attitude angle of the UAV at the next moment. This process is repeated to obtain the position and attitude angle of the UAV at subsequent moments. The attitude angle of the drone at a given moment, and the difference between this attitude angle and the drone's attitude angle in the initial state, represents the difference between the initial moment and the future moment. The rate of change of attitude angle within a given time period.

2. The UAV trajectory obstacle avoidance method based on prior artificial potential field according to claim 1, characterized in that, The potential field model also includes a boundary repulsion potential field function and a boundary repulsion function.

3. The UAV trajectory obstacle avoidance method based on prior artificial potential field according to claim 2, characterized in that, The boundary repulsive potential field function Set to: , The boundary repulsion function Set to: , in, The boundary repulsive potential energy coefficient, The distance between the drone and the boundary. The range of influence of the boundary repulsive potential energy.

4. The UAV trajectory obstacle avoidance method based on prior artificial potential field according to claim 1, characterized in that, Set an angle change threshold. When the rate of change of the attitude angle exceeds the threshold, the angle offset is set to a preset offset. ; When the rate of change of attitude angle is less than the threshold, the angle offset is 0.

Images