Unmanned aerial vehicle obstacle avoidance control method for improving VFH based on obstacle field angle change

Abstract

The invention discloses an unmanned aerial vehicle obstacle avoidance control method for improving VFH based on obstacle field angle change, and relates to the field of unmanned aerial vehicle control. A candidate direction grid in a three-dimensional space is constructed, threats of candidate directions are evaluated according to the field angle of an obstacle, and the application range of a traditional VFH method is expanded to the three-dimensional space; meanwhile, when threats in the candidate directions are evaluated, the influence of different speeds of the obstacle on the threat degree is evaluated through the change rate of the field angle of the obstacle, and the obstacle avoidance performance of the unmanned aerial vehicle facing the moving obstacle is effectively improved; besides, when the obstacle avoidance direction is selected, the safety degree of each candidate direction is calculated based on the threat of each candidate direction and in combination with a pitch retention factor and a course retention factor, and the defect that a threshold value needs to be set in a traditional VFH method is overcome; according to the invention, the obstacle avoidance performance of the unmanned aerial vehicle for different types of obstacles in a three-dimensional space is effectively improved.

Description

Technical Field

[0001] The present invention relates to the field of unmanned aerial vehicle control, and in particular to an unmanned aerial vehicle obstacle avoidance control method based on improving VFH based on the change of obstacle field of view angle. Technical Background

[0002] When UAVs are performing tasks, there are usually many obstacles in the environment, such as stationary mountains, buildings, moving flocks of birds, non-cooperative aircraft, etc. These obstacles pose a serious threat to the flight safety of UAVs. Therefore, obstacle avoidance control is a safety prerequisite for UAVs 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 obstacle avoidance control of UAVs. However, most VFH methods only consider obstacle avoidance on a two-dimensional plane and are difficult to be directly used in three-dimensional space; when constructing a threat histogram, most existing VFH methods are based only on the static density distribution characteristics of obstacles, and rarely consider how to adjust the histogram when the obstacle moves, which makes it difficult to cope with dynamic environments; existing VFH methods select the advancing direction based on a threshold, and the size of the threshold significantly affects the choice of obstacle avoidance direction. It is difficult to determine the optimal threshold in practical applications.

[0003] In order to solve the defects of the existing VFH method, such as limited applicable space, insufficient consideration of motion obstacles, and obstacle avoidance performance being sensitive to threshold settings, the present invention proposes a UAV obstacle avoidance control method based on improved VFH based on the change of obstacle field of view angle. Summary of the invention

[0004] In order to solve the above technical problems, the present invention proposes a drone obstacle avoidance control method based on improved VFH based on the change of obstacle field of view angle. By constructing a candidate direction grid in three-dimensional space and evaluating the threat of the candidate direction according to the field of view angle of the obstacle, the application scope of the traditional VFH method is expanded to three-dimensional space; at the same time, when evaluating the threat of the candidate direction, the present invention evaluates the influence of different speeds of the obstacle on the degree of threat by the rate of change of the obstacle field of view angle, which effectively improves the obstacle avoidance performance of the drone when facing moving obstacles; in addition, when selecting the obstacle avoidance direction, the present invention calculates the safety of each candidate direction based on the threat of each candidate direction, combined with the pitch holding factor and the heading holding factor, which overcomes the defect of the traditional VFH method that a threshold needs to be set; the present invention effectively improves the obstacle avoidance performance of the drone against different types of obstacles in three-dimensional space.

[0005] The technical solution of the present invention comprises the following steps:

[0006] Step 1, establish a candidate direction grid of the UAV in a three-dimensional environment;

[0007] Step 2, evaluating the threat in each candidate direction based on the change in the obstacle field of view angle;

[0008] Step 3, calculate the safety of each candidate direction;

[0009] Step 4, select obstacle avoidance direction;

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

[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 radius of the kth (k=1,2,…,No)th 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 drone is a point model, the position vector of the drone in the geographic coordinate system at the time of obstacle avoidance is p, the pitch angle is γ, the heading angle is ψ, and the speed is V; then the speed vector of the drone in the geographic coordinate system at the time of obstacle avoidance is v = [Vcosγcosψ, Vcosγsinψ, Vsinγ] T ; The distance from the drone to the center of the kth obstacle at the moment of obstacle avoidance is recorded as ||p k -p||;

[0013] Assume that the resolution of the pitch angle change of the drone 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 selectable heading angles. The pitch angle and heading angle are combined to form N cγ ×N cψ candidate directions; construct an N cγ Row, N cψ The candidate direction grid of the column is Figure 1 As shown, the mth (m=1,2,…,N cγ ) row corresponds to a pitch angle change of Δγ m =90-(m-1)σ γ , the nth (n=1,2,…,N cψ ) column corresponds to the heading angle change Δψ n =180-(n-1)σ ψ ; The candidate direction corresponding to the cell in the mth row and nth column in the candidate direction grid is recorded as φ cmn .

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

[0015] 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 The candidate obstacle avoidance velocity vector v cmn for:

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

[0017] S2.2, calculate the candidate obstacle avoidance velocity vector v cmn The threat value ρ kmn

[0018] If the kth obstacle is within the detection range of the drone, calculate the field of view angle δ of the kth obstacle relative to the drone k for:

[0019]

[0020] Then, calculate the kth obstacle in the candidate direction φ cmn The rate of change of the field of view angle under k ' mn :

[0021]

[0022] Finally, calculate the candidate direction φ of the kth obstacle pair cmn The threat value ρ kmn :

[0023]

[0024] Wherein, λ1 is the scaling factor of the field angle change rate, λ2 is the scaling factor of the field angle, and both λ1 and λ2 are positive constants;

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

[0026] S2.3, repeat step 2.2 until No obstacle pairs for 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 Introduce 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 calculation method of the candidate direction safety is as follows:

[0032] S3.1, in order to avoid large 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 constraints 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 constraints, cmn , then calculate the candidate direction φ cmn Safety mn for:

[0033]

[0034] In the formula, μ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 drone, cmn Safety mn is 0;

[0036] 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.

[0037] Furthermore, in step 4, the obstacle avoidance direction is selected:

[0038] 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.

[0039] An electronic device comprises 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 are 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 described above.

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

[0041] Beneficial effects of the present invention:

[0042] 1. The present invention considers the construction of candidate direction grids in three-dimensional space, and evaluates the threat of each candidate direction based on the field of view angle of the obstacle, thus expanding the application scope of the traditional VFH method to three-dimensional space;

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

[0044] 3. When selecting the obstacle avoidance direction, the present invention calculates the safety of each candidate direction based on the threat of each candidate direction and in combination with the pitch holding factor and the heading holding factor, thus overcoming the defect that the traditional VFH method needs to set a threshold;

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

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

[0047] Figure 2 : is a threat histogram when the time is 14s in an embodiment of the present invention;

[0048] Figure 3 It is a safety histogram when the time is 14s in the embodiment of the present invention;

[0049] Figure 4 This is the obstacle avoidance track diagram of the UAV in the embodiment of the present invention. DETAILED DESCRIPTION

[0050] In order to verify the obstacle avoidance performance of the present invention for different types of obstacles in three-dimensional space, a simulation experiment is set up in this embodiment. In this embodiment, a fixed-wing UAV flies according to a pre-set route. The number of obstacles in the flight space is N o =7. Among these 7 obstacles, the 1st, 2nd, 3rd, 4th and 5th obstacles are stationary obstacles, the 6th obstacle is a moving obstacle moving along a straight line with a constant velocity vector of [-21, 21, 3]T m / s, and the 7th obstacle is a moving obstacle moving along a curve. The radii of these 7 obstacles are =130m, =100m, =180m, =200m, =230m, =120m, =80m respectively. When the time is 14s, the position vector and velocity vector of the center of each obstacle in the geographic coordinate system are shown in Table 1.

[0051] Table 1 The position vector and velocity vector of the obstacle at 14s

[0052]

[0053] At 14 seconds, the position vector p of the drone in the geographic coordinate system is [654, 342, 146] T m, pitch angle γ = 27°, heading angle ψ = 53°, speed V = 30m / s, and its velocity vector v in the geographic coordinate system is [15.9, 21.5, 13.8] T m / s. At this time, the drone senses the first obstacle and avoids it according to the method proposed by the present invention. The specific steps are as follows:

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

[0055] (2) Set λ1 = 10, λ2 = 2, and calculate the threat value of each candidate direction. When the time is 14 seconds, the threat histogram composed of the threat values ​​of all candidate directions is as follows: Figure 2 shown.

[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 far 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 drone obstacle avoidance control method based on improved VFH based on obstacle field of view angle change, 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; Step 2, evaluating the threat in each candidate direction based on the change in the obstacle field of view angle; 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 obstacles.

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 establishment of the candidate direction grid of the drone described in step 1 is 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 drone is a particle model. At the moment of obstacle avoidance, the drone's position vector in the geographic coordinate system is p, the pitch angle is γ, the heading angle is ψ, and the speed is V; then the drone's speed vector in the geographic coordinate system at the moment of obstacle avoidance is v = [Vcosγcosψ, Vcosγsinψ, Vsinγ]T; the distance from the drone 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 drone 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 selectable heading angles. The pitch angle and heading angle are combined to form 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 .

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 evaluation process of the candidate directional threats described in step 2 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 The 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 drone, calculate the field of view angle δ of the kth obstacle relative to the drone k for: Then, calculate the kth obstacle in the candidate direction φ cmn The rate of change of the field of view angle under k ' mn : Finally, calculate the candidate direction φ of the kth obstacle pair cmn The threat value ρ kmn : Wherein, λ1 is the scaling factor of the field angle change rate, λ2 is the scaling factor of the field 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 for 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 Introduce 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.

4. 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 calculation method of the safety degree of the candidate direction in step 3 is as follows: S3.1, in order to avoid large 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: In the formula, μ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 drone, 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.

5. The method for unmanned aerial vehicle obstacle avoidance control based on improved VFH based on obstacle field of view angle change 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.

6. 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 are 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 as described in any one of claims 1-5.

7. 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 as described in any one of claims 1 to 5.

Images