An unmanned aerial vehicle obstacle avoidance control method based on improved VFH with change of obstacle field of view angle

Abstract

The application relates to an unmanned aerial vehicle obstacle avoidance control method based on obstacle field of view angle change and improved VFH, and relates to the field of unmanned aerial vehicle control. The application expands the application range of the traditional VFH method to three-dimensional space by constructing a three-dimensional space candidate direction grid and evaluating the threat of the candidate direction according to the field of view angle of the obstacle. Meanwhile, when evaluating the threat of the candidate direction, the application evaluates the influence of different speeds of the obstacle on the threat degree through the change rate of the field of view angle of the obstacle, effectively improving the obstacle avoidance performance of the unmanned aerial vehicle when facing a moving obstacle. In addition, when selecting an obstacle avoidance direction, the application calculates the safety degree of each candidate direction based on the threat of each candidate direction, combining a pitch retention factor and a heading retention factor, overcoming the defect that the traditional VFH method needs to set a threshold. The application effectively improves the obstacle avoidance performance of the unmanned aerial vehicle in three-dimensional space for different types of obstacles.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of unmanned aerial vehicle control, in particular to an unmanned aerial vehicle obstacle avoidance control method based on change of field of view angle of obstacles. BACKGROUND

[0002] During the execution of tasks by unmanned aerial vehicles, there are usually various obstacles in the environment, such as stationary mountains, buildings, etc., moving bird flocks, non-cooperative aircraft, etc. These obstacles pose a serious threat to the flight safety of unmanned aerial vehicles. Therefore, obstacle avoidance control is a safety prerequisite for unmanned aerial vehicles to successfully complete tasks in complex environments. Due to its simple principle and fast calculation speed, the vector field histogram (VFH) method has been widely used in recent years to realize unmanned aerial vehicle obstacle avoidance control. However, most VFH methods only consider obstacle avoidance in two-dimensional planes, and are difficult to be directly used in three-dimensional space; when constructing threat histograms, most existing VFH methods only rely on the static density distribution characteristics of obstacles, rarely consider how to adjust the histogram when obstacles are moving, and are difficult to cope with dynamic environments; the existing VFH method selects the forward direction based on a threshold value, and the size of the threshold value significantly affects the selection of the obstacle avoidance direction, making it difficult to determine the optimal threshold value in actual applications.

[0003] To solve the defects of the existing VFH method, such as limited applicable space, insufficient consideration of moving obstacles, and sensitive obstacle avoidance performance to threshold setting, the present application proposes an unmanned aerial vehicle obstacle avoidance control method based on improved VFH of change of field of view angle of obstacles. SUMMARY

[0004] To solve the above technical problems, the present application proposes an unmanned aerial vehicle obstacle avoidance control method based on improved VFH of change of field of view angle of obstacles. By constructing a candidate direction grid in three-dimensional space, the threat of each candidate direction is evaluated according to the field of view angle of obstacles, and the application range of the traditional VFH method is expanded to three-dimensional space; at the same time, when evaluating the threat of each candidate direction, the rate of change of the field of view angle of obstacles is used to evaluate the influence of different speeds of obstacles on the threat level, effectively improving the obstacle avoidance performance of unmanned aerial vehicles when facing moving obstacles; in addition, when selecting the obstacle avoidance direction, the safety degree of each candidate direction is calculated based on the threat of each candidate direction, combined with the pitch retention factor and the heading retention factor, overcoming the defect of the traditional VFH method that requires setting a threshold value; the present application effectively improves the obstacle avoidance performance of unmanned aerial vehicles in three-dimensional space for different types of obstacles.

[0005] The technical scheme of the present application includes the following steps:

[0006] Step 1: Establishing a candidate direction grid for unmanned aerial vehicles in a three-dimensional environment;

[0007] Step 2: Evaluating the threat of each candidate direction based on the change of the field of view angle of obstacles;

[0008] Step 3: Calculate the safety of each candidate direction;

[0009] Step 4: Select the obstacle avoidance direction;

[0010] Step 5. Repeat steps 1 to 4 until the drone is away from the obstacle.

[0011] Furthermore, in step 1, the method for establishing the candidate direction grid of the drone is as follows:

[0012] Assume that there are N o obstacles, among which the kth (k=1,2,…,N o ) The radius of the obstacle is r k At the moment of obstacle avoidance, the position vector of the center of the kth obstacle in the geographic coordinate system is p k , the velocity vector is v k Assume that the UAV is a point mass model. At the moment of obstacle avoidance, the position vector of the UAV in the geographic coordinate system is p, the pitch angle is γ, the heading angle is ψ, and the velocity is V. Then the velocity vector of the UAV in the geographic coordinate system at the moment of obstacle avoidance is v = [Vcosγcosψ, Vcosγsinψ, Vsinγ] T , [] T is the transpose; the distance from the UAV to the center of the kth obstacle at the time of obstacle avoidance is recorded as ||p k -p||;

[0013] Assume that the resolution of the pitch angle change of the UAV during obstacle avoidance is σ γ , the resolution of the heading angle change is σ ψ , and σ γ , σ ψ can be divided by 180; the total pitch angles that the drone can choose when avoiding obstacles are There are a total of 4 optional heading angles. The pitch angle and heading angle are combined into N cγ ×N cψ candidate directions; construct an N cγ Row, N cψ The candidate direction grid of the column, such as Figure 1 As shown, the mth (m=1,2,…,N cγ ) row corresponding to the pitch angle change is Δγ m =90-(m-1)σ γ , nth (n=1,2,…,N cψ ) column corresponds to the heading angle change of all cells in the column is Δψ n =180-(n-1)σ ψThe candidate direction corresponding to the cell in the mth row and the nth column in the candidate direction grid is denoted as φ cmn .

[0014] Further, in step 2, the candidate direction threat evaluation method is as follows:

[0015] S2.1, the candidate direction φ cmn The corresponding candidate pitch angle is γ cm = γ + Δγ m The candidate heading angle is ψ cn = ψ + Δψ n , [ ] T is transposed, so the candidate direction φ cmn The candidate obstacle avoidance speed vector v cmn is:

[0016] v cmn = [Vcosγ cm cosψ cn , Vcosγ cm sinψ cn , Vsinγ cm ] T

[0017] S2.2, the threat value ρ cmn of the candidate obstacle avoidance speed vector v kmn is calculated:

[0018] If the kth obstacle is within the detection range of the UAV, the field of view angle δ k of the kth obstacle relative to the UAV is calculated as:

[0019]

[0020] Then, the field of view angle change rate δ′ cmn of the kth obstacle in the candidate direction φ kmn is calculated:

[0021]

[0022] Finally, the threat value ρ cmn of the kth obstacle to the candidate direction φ kmn is calculated:

[0023]

[0024] In the formula, λ1 is a scaling coefficient of the field of view angle change rate, λ2 is a scaling coefficient of the field of view angle, and λ1 and λ2 are both positive constants;

[0025] If the kth obstacle is outside the detection range of the UAV, the threat value ρ kmnis 0;

[0026] S2.3, repeat step 2.2 until N o obstacle pairs candidate directions φ cmn The threat values ​​of are calculated; the maximum value is ρ mn ,Right now:

[0027]

[0028] In order to make the UAV avoid the ground as much as possible, the candidate direction φ cmn Introducing the ground avoidance factor 1+max{0,-sinγ cm}, candidate direction φ cmn The threat value h mn Calculated as:

[0029] h mn =(1+max{0,-sinγ cm})ρ mn

[0030] S2.4, repeat steps S2.1 to S2.3 until the threat values ​​of all candidate directions are calculated, and the threat values ​​of all candidate directions form a threat histogram.

[0031] Furthermore, in step 3, the safety degree of the candidate direction is calculated as follows:

[0032] S3.1, in order to avoid large-scale maneuvers of the UAV during obstacle avoidance and reduce energy consumption during obstacle avoidance, the candidate direction φ cmn Introducing the pitch hold factor and heading keeping factor Among them, μ1 is the pitch angle maintenance gradient coefficient, μ2 is the heading angle maintenance gradient coefficient, and both μ1 and μ2 are positive constants. Considering the dynamic limitations of the UAV itself, such as speed and control amount, if the UAV can turn to the candidate direction φ under the current state and input limitations, cmn , then calculate the candidate direction φ cmn Safety mn for:

[0033]

[0034] Where, μ3 is the threat factor, μ4 is the safety factor, and μ3 and μ4 are positive constants;

[0035] If φ cmn If the candidate direction φ is beyond the available range of the UAV, cmn Safety mn is 0;

[0036] S3.2, repeat step 3.1 until Ncγ ×N cψ The safety degrees of all candidate directions are calculated, and the safety degrees of all candidate directions form a safety histogram.

[0037] Further, in step 4, the selection of the obstacle avoidance direction is as follows:

[0038] In N cγ ×N cψ candidate directions, the candidate direction with the highest safety degree is selected as the obstacle avoidance direction, and the candidate direction with the highest safety degree is selected as the motion direction of the UAV.

[0039] An electronic device, comprising one or more processors; and a memory; one or more programs, characterized in that the one or more programs are stored in the memory and configured to be executed by the one or more processors, and the one or more programs are configured to execute the UAV obstacle avoidance control method described above.

[0040] A computer readable storage medium, the computer readable storage medium stores program code, the program code can be called and executed by a processor to execute the UAV obstacle avoidance control method as described above.

[0041] The beneficial effects of the present application are as follows:

[0042] 1. The present application considers the construction of the candidate direction grid in three-dimensional space, and evaluates the threat of each candidate direction according to the field of view angle of the obstacle, and the application range of the traditional VFH method is extended to three-dimensional space;

[0043] 2. When evaluating the threat of the candidate direction, the present application considers the influence of different speeds of the obstacle on the threat degree through the change rate of the field of view angle of the obstacle, effectively improving the obstacle avoidance performance of the UAV when facing moving obstacles;

[0044] 3. When selecting the obstacle avoidance direction, the present application calculates the safety degree of each candidate direction based on the threat of each candidate direction, combines the pitch holding factor and the heading holding factor, and overcomes the defect that the traditional VFH method needs to set a threshold;

[0045] 4. The present application can effectively improve the obstacle avoidance performance of the UAV in three-dimensional space for different types of obstacles. BRIEF DESCRIPTION OF DRAWINGS

[0046] Figure 1 A schematic diagram for constructing a candidate direction grid;

[0047] Figure 2 A threat histogram at time 14s in the embodiment of the present application;

[0048] Figure 3The safety histogram when the time is 14s in the embodiment of the application;

[0049] Figure 4 The UAV obstacle avoidance path map in the embodiment of the application. DETAILED DESCRIPTION

[0050] To verify the obstacle avoidance performance of the application in three-dimensional space for different types of obstacles, simulation experiments are set up for verification in this embodiment. In this embodiment, a fixed-wing UAV flies according to a pre-set flight path. The number of obstacles in the flight space is N o =7. Among the 7 obstacles, the first, second, third, fourth and fifth obstacles are static obstacles, the sixth obstacle is a moving obstacle moving along a straight line with a constant velocity vector of [-21, 21, 3] T m / s, and the seventh obstacle is a moving obstacle moving along a curve. The radii of the 7 obstacles are 130m, 100m, 180m, 200m, 230m, 120m and 80m respectively; the position vector and velocity vector of each obstacle in the geographic coordinate system when the time is 14s are shown in Table 1.

[0051] Table 1 Position vector and velocity vector of obstacles when the time is 14s

[0052]

[0053] When the time is 14s, the position vector p of the UAV in the geographic coordinate system is [654, 342, 146] T m, the pitch angle γ is 27°, the heading angle ψ is 53°, and the speed V is 30m / s. The velocity vector v of the UAV in the geographic coordinate system is [15.9, 21.5, 13.8] T m / s. At this time, the UAV perceives the first obstacle and performs obstacle avoidance according to the method proposed in the application, and the specific steps are as follows:

[0054] (1) Set the resolution σ γ of the change of the pitch angle of the UAV to 5°, the resolution σ ψ of the change of the heading angle to 5°, N cγ =37, N cψ =73, and establish a candidate direction grid.

[0055] (2) Set λ1=10 and λ2=2, and calculate the threat values of each candidate direction. The threat histogram composed of the threat values of all candidate directions when the time is 14s is shown in Figure 2 .

[0056] (3) Set μ1 = 100, μ2 = 100, μ3 = 1.5, μ4 = 1.5, and calculate the safety of each candidate direction. When the time is 14 seconds, the safety histogram composed of the safety of all candidate directions is as follows: Figure 3 shown.

[0057] (4) Select the direction with the highest safety as the obstacle avoidance direction.

[0058] (5) At each other moment when obstacle avoidance is required, repeat the above steps (1) to (4) to avoid obstacles until the drone is away from the obstacle.

[0059] The final trajectory of the drone is as follows Figure 4 During the entire flight, the minimum distance between the drone and the boundary of each obstacle was 23.8m, and the drone successfully avoided collision with obstacles.

Claims

1. A UAV obstacle avoidance control method based on improved VFH based on the change of obstacle field of view angle, characterized in that: The specific steps of the control method are as follows: Step 1: Establish a candidate direction grid of the UAV in a three-dimensional environment; The candidate direction grid of the drone is established as follows: Assume that there are N o obstacles, where the radius of the kth obstacle is r k , where k = 1, 2, …, N o ; At the moment of obstacle avoidance, the position vector of the center of the kth obstacle in the geographic coordinate system is p k , the velocity vector is v k Assume that the UAV is a point mass model. At the moment of obstacle avoidance, the position vector of the UAV in the geographic coordinate system is p, the pitch angle is γ, the heading angle is ψ, and the velocity is V. Then the velocity vector of the UAV in the geographic coordinate system at the moment of obstacle avoidance is v = [Vcosγcosψ, Vcosγsinψ, Vsinγ] T ; The distance from the UAV to the center of the kth obstacle at the moment of obstacle avoidance is recorded as ||p k -p||; Assume that the resolution of the pitch angle change of the UAV during obstacle avoidance is σ γ , the resolution of the heading angle change is σ ψ , and σ γ , σ ψ can be divided by 180; the total pitch angles that the drone can choose when avoiding obstacles are There are a total of 4 optional heading angles. The pitch angle and heading angle are combined into N cγ ×N cψ candidate directions; construct an N cγ Row, N cψ The candidate direction grid of the column, where the pitch angle change corresponding to all cells in the mth row is Δγ m =90-(m-1)σ γ , where m=1,2,…,N cγ ; The heading angle change corresponding to all cells in the nth column is Δψ n =180-(n-1)σ ψ , where n = 1, 2, ..., N cψ ; The candidate direction corresponding to the cell in the mth row and nth column in the candidate direction grid is recorded as φ cmn ; Step 2: Evaluate the threat in each candidate direction based on the change in the obstacle field of view angle; The evaluation process of the candidate direction threat is as follows: S2.1, candidate direction φ cmn The corresponding candidate pitch angle is γ cm =γ+Δγ m , the candidate heading angle is ψ cn =ψ+Δψ n , therefore, the candidate direction φ cmn Candidate obstacle avoidance velocity vector v cmn for: v cmn =[Vcosγ cm cosψ cn ,Vcosγ cm sinψ cn ,Vsinγ cm ] T S2.2, calculate the candidate obstacle avoidance velocity vector v cmn The threat value ρ kmn If the kth obstacle is within the detection range of the UAV, calculate the field of view angle δ of the kth obstacle relative to the UAV k for: Then, calculate the k-th obstacle in the candidate direction φ cmn The rate of change of the field of view angle δ′ under kmn : Finally, calculate the k-th obstacle pair candidate direction φ cmn The threat value ρ kmn : Where λ1 is the scaling factor of the field of view angle change rate, λ2 is the scaling factor of the field of view angle, and both λ1 and λ2 are positive constants; If the kth obstacle is outside the detection range of the drone, the threat value of the kth obstacle is ρ kmn is 0; S2.3, repeat step 2.2 until N o obstacle pairs candidate directions φ cmn The threat values ​​of are calculated; the maximum value is ρ mn ,Right now: In order to make the UAV avoid the ground as much as possible, the candidate direction φ cmn Introducing the ground avoidance factor 1+max{0,-sinγ cm }, candidate direction φ cmn The threat value h mn Calculated as: h mn =(1+max{0,-sinγ cm })r mn S2.4, repeat steps S2.1 to S2.3 until the threat values ​​of all candidate directions are calculated and the threat values ​​of all candidate directions form a threat histogram; Step 3: Calculate the safety of each candidate direction; Step 4: Select the obstacle avoidance direction; Step 5. Repeat steps 1 to 4 until the drone is away from the obstacle.

2. The method for controlling an unmanned aerial vehicle obstacle avoidance based on improving VFH based on the change of the obstacle field of view angle according to claim 1 is characterized in that: The calculation method of the candidate direction safety in step 3 is as follows: S3.1, in order to avoid large-scale maneuvers of the UAV during obstacle avoidance and reduce energy consumption during obstacle avoidance, the candidate direction φ cmn Introducing the pitch hold factor and heading keeping factor Among them, μ1 is the pitch angle maintenance gradient coefficient, μ2 is the heading angle maintenance gradient coefficient, and both μ1 and μ2 are positive constants. Considering the dynamic limitations of the UAV itself, such as speed and control amount, if the UAV can turn to the candidate direction φ under the current state and input limitations, cmn , then calculate the candidate direction φ cmn Safety mn for: Where, μ3 is the threat factor, μ4 is the safety factor, and μ3 and μ4 are positive constants; If φ cmn If the candidate direction φ is beyond the available range of the UAV, cmn Safety mn is 0; S3.2, repeat step 3.1 until N cγ ×N cψ The safety of each candidate direction is calculated, and the safety of all candidate directions constitutes a safety histogram.

3. The method for controlling an unmanned aerial vehicle obstacle avoidance based on improving VFH based on the change of the obstacle field of view angle according to claim 1, characterized in that: The method for selecting the obstacle avoidance direction in step 4 is: In N cγ ×N cψ Among the candidate directions, the candidate direction with the highest safety is selected as the obstacle avoidance direction, and the candidate direction with the highest safety is used as the movement direction of the UAV.

4. An electronic device comprising one or more processors; and a memory; and one or more programs, characterized in that: The one or more programs are stored in the memory and configured to be executed by the one or more processors, and the one or more programs are configured to execute the drone obstacle avoidance control method according to any one of claims 1 to 3.

5. A computer-readable storage medium, characterized in that The computer-readable storage medium stores program code, and the program code can be called by a processor to execute the drone obstacle avoidance control method according to any one of claims 1 to 3.

Images