Dynamic obstacle avoidance method for unmanned aerial vehicle

Abstract

The invention belongs to the technical field of unmanned aerial vehicle obstacle avoidance, and discloses a dynamic obstacle avoidance method for an unmanned aerial vehicle, and the specific technical scheme is as follows: according to the distance and speed of an obstacle relative to the unmanned aerial vehicle, dynamically adjusting and calculating a time step length through a fuzzy mathematical algorithm; the method comprises the following steps: firstly, integrating sensor information of an unmanned aerial vehicle to obtain the position, linear velocity and angular velocity of an obstacle, predicting the movement trend of the obstacle in a short time in the future, and taking the movement trend as input of a speed obstacle method to obtain an expanded speed obstacle area; selecting a speed which does not generate a collision risk with the obstacle in a short time as an optional obstacle avoidance speed; the optimal direction-adjusting obstacle-avoiding speed is preferentially selected; the method comprehensively considers the movement trend of the obstacle, is more predictable, obtains a safer and more reliable obstacle avoidance path, and reduces the cost of path, time and calculation resources caused by obstacle avoidance as much as possible by adopting adaptive calculation of time step length and improvement of a cost function.

Description

Technical Field

[0001] The invention belongs to the technical field of unmanned aerial vehicle obstacle avoidance, and in particular relates to an efficient unmanned aerial vehicle obstacle avoidance method in a dynamic environment. Background Art

[0002] Over the past decade, unmanned aerial vehicles (UAVs) have made significant progress in scientific research and real-world applications. In addition, various sensors have been installed on UAVs for applications such as autonomous formation flying, surveillance, target tracking, reconnaissance, and disaster relief. In order to fully utilize UAVs in the real world, flight safety with close encounters with obstacles should be guaranteed. In actual situations, obstacle information is unknown before deployment and should be detected by sensors installed on the UAV during the mission. The UAV must be able to use the information collected online to determine whether there is a risk of collision with the detected obstacle and to take corresponding evasive maneuvers. In other words, obstacle conflict detection and avoidance is one of the most important capabilities for the success of UAV applications.

[0003] In the process of local obstacle avoidance, the traditional speed obstacle method has some defects. First, it usually assumes that the obstacle moves in a straight line with uniform speed or uniform acceleration, and considers the angular velocity of the obstacle to be 0, and only considers the current state of the obstacle, without considering the speed and angle changes and movement trend of the obstacle. In addition, the interval time (calculation time step) for calculating whether there is a collision risk within the detection range is usually fixed, without considering the position, speed and other states of the obstacle, which may lead to low calculation efficiency or poor collision avoidance effect in some cases. Finally, when calculating the obstacle avoidance speed, the traditional speed obstacle method gives the target point strategy, the maximum obstacle avoidance speed strategy, etc., and when used for local path planning, the existing commonly used speed obstacle method is usually based on the minimum speed change, the closest straight line speed (the fastest speed from the current position to the sub-target point) and the minimum speed change, etc., which is independent of the state of the obstacle. Then, when the obstacle avoidance speed is consistent with the obstacle movement trend, it may cause the drone to chase the obstacle, making the obstacle avoidance process too long or even causing a collision. Summary of the invention

[0004] In order to solve the technical problems existing in the prior art, the present invention provides an obstacle avoidance algorithm for UAV in a dynamic environment based on an improved speed obstacle method. Compared with the traditional speed obstacle method, the movement trend of obstacles is comprehensively considered, so that when avoiding dynamic obstacles, it is more predictable and a safer and more reliable obstacle avoidance path is obtained. In addition, the cost of the path, time, etc. caused by obstacle avoidance is reduced as much as possible by adopting an adaptive calculation time step and improving the cost function.

[0005] To achieve the above purpose, the technical solution adopted by the present invention is: a dynamic obstacle avoidance method for a drone, the specific steps are as follows:

[0006] Step 1: Initialize the position, speed, and radius of the drone, the position, linear speed, angular speed, and radius of the obstacle, and the starting point and sub-target point of the drone;

[0007] Step 2: Determine whether the drone has reached the target point. If it is determined that the drone has reached the target point, the algorithm ends. If it is determined that the drone has not reached the target point, execute step 3.

[0008] Step 3: Check whether there is an obstacle within the detection range. If the result of the judgment is that there is no obstacle, the speed of the drone is set to the straight-line speed at the next moment, and step 6 is executed. If the result of the judgment is that there is an obstacle, it is determined whether the speed of the drone needs to be changed according to steps 4 and 5;

[0009] Step 4: When there is an obstacle within the detection range, predict the obstacle information and determine whether there is a risk of collision with the UAV flying in a straight line. If it is determined that there is no risk of collision, change the speed of the UAV at the next moment to the straight line speed and execute step 6; if it is determined that there is a risk of collision, execute step 5;

[0010] First, the position, linear velocity and angular velocity of the obstacle are obtained by integrating the sensor information of the drone, and it is assumed that the state of the obstacle remains unchanged in a short time. Based on this, the motion formula of the quadratic motion model - the constant turning rate and speed amplitude model can be used to predict the movement trend of the obstacle in the short future time, and it can be used as the input of the speed obstacle method to expand the speed obstacle area.

[0011] Let the current state of the obstacle be X o,c =(P o,x ,P o,y ,V,φ,ω), where (P o,x ,P o,y ) is the component of the obstacle position on the coordinate axis, V is the linear velocity, φ is the obstacle direction, and ω is the angular velocity. The specific formula for predicting the state of the obstacle at the i-th moment in the future is shown in formula (1):

[0012]

[0013] At the same time, the corresponding future position of the UAV X u,c =(P u,x ,P u,y ) prediction is shown in formula (2):

[0014] X u,c+i =X u,c +i*vΔt(2)

[0015] Here, v is the current velocity of the drone. The acceleration vector is not used because the velocity changes caused by the algorithm may cause poor predictions.

[0016] Then, it is determined whether there is a collision risk between the linear speed and the predicted obstacle state information at the next k moments. The collision cones are constructed for the drone state and the obstacle at the next k moments, and the k collision cones are merged to obtain the extended speed obstacle area. When the linear speed is within the speed obstacle area, it is considered that there is a collision risk. Because the above trajectory prediction formula is suitable for short-term prediction, the prediction range k is related to the radius and speed of the obstacle. Considering the obstacle radius and speed range, the prediction range is 3 calculation time steps.

[0017] According to the principle of the classic speed obstacle method, when the relative speed vector v between the drone and the obstacle ou When it is within the collision cone, it means that there is a risk of collision between the drone and the obstacle at the current speed.

[0018] p u 、p o are the positions of the drone and the obstacle, respectively, ou =p o -p u is the relative position vector between the UAV and the obstacle, r u 、r o are the radius of the drone and the obstacle, r s is the additional safety radius, R c =r u +r o +r s is the collision radius of the obstacle, and the collision area RRC ou It is p o is the center of the circle, R c is a circle with radius v ou It represents the velocity vector of the UAV relative to the obstacle. LT and RT are respectively the velocity vectors of the UAV with p as the origin. o is the center of the circle, R c is the left and right tangent of the circle with radius, β is the relative velocity vector v ou and the relative position vector p ou The angle, δ c is the distance between the UAV and the closest point to the obstacle.

[0019] According to the geometric relationship of the collision cone, if the position of the drone p u is the vertex, along the relative velocity vector v ou The ray in the direction passes through the collision area RRC uo, there is a potential collision risk between the drone and the obstacle. When there are multiple obstacles, there will be multiple corresponding collision cones. In order to facilitate the unified description of multiple collision cones in the absolute velocity space of the drone, the collision cone RRC ou and the obstacle velocity vector v o Perform Minkowski vector sum operation to obtain the speed obstacle area VO surrounded by LT' and RT' ou , as shown in formula (3), corresponds to the area surrounded by LT' and RT', where RV (Reachable Velocity) is the reachable velocity area of ​​the UAV.

[0020]

[0021] The predicted obstacle state information at the next k moments is used to construct collision cones and collision areas with the drone, and then compared with the obstacle velocity vector v o Perform Minkowski vector and operation to obtain the speed obstacle area VO′ ou , VO″ ou , VO″′ ou , the three predicted speed obstacle areas are compared with the current speed obstacle area VO ou The area surrounded by the tangent lines LT' and RT'3 is the extended speed obstacle area VO. ou , when the straight-line speed is in the extended speed obstacle area VO ou If the vehicle is within the specified range, it is considered that there is a risk of collision.

[0022] Since the UAV speed is discretized later, for the convenience of calculation, formula (4) is used as the potential collision detection condition at each moment:

[0023] p ou ·v ou ≥0,δ C <R c (4)

[0024] Formula (4) means that only when the relative position vector p ou With the relative velocity vector v ou The obstacle and the UAV are considered to have a potential collision only when the angle β∈(-π / 2, +π / 2) and the distance between the UAV and the closest point is less than the collision radius.

[0025] Step 5: When there is an obstacle within the detection range and there is a risk of collision between the drone and the obstacle when the drone is running at a straight-line speed, determine whether there is a risk of collision between the drone and the obstacle at the current speed. If there is no risk of collision, there is no need to avoid the obstacle and the drone maintains the current speed. If there is a risk of collision, it is necessary to avoid the obstacle and the optimal obstacle avoidance speed at the next moment is selected.

[0026] The first step is to obtain a discretized achievable speed set based on the performance constraints and safety considerations of the drone. Based on the performance constraints and safety considerations of the drone, the speed adjustment and heading adjustment of the drone will have a range limit, that is:

[0027] V vary ∈[v min , v max ] (5)

[0028] Δθ∈[0,Δθ max ](6)

[0029] Assume v c and θ c are the current speed and heading of the UAV respectively, and the expression of the achievable speed set (RV) of the UAV can be obtained as follows:

[0030] RV={(v,θ)|v∈[v c -v min , v c +v max ],θ∈[θ c -Δθ max ,θc+Δθ max ,]} (7)

[0031] Then, RV is discretized, the continuous θ in RV is discretized into a set of N angles at equal intervals, and the continuous v is discretized into a set of M velocities at equal intervals, thereby obtaining M×N velocity vectors (v i ,θ j ), where v i is the magnitude of the velocity vector, θ j For the heading.

[0032] In order to ensure the reliability of collision avoidance performance, the continuous change of the UAV state should be avoided as much as possible when selecting the optimal obstacle avoidance speed, so as to achieve the collision avoidance and mission execution functions on the basis of minimum maneuvers. Therefore, in the reachable speed set RV, a speed subset (RVA) that only changes the direction of N speeds and a speed subset (RVN) that only changes the modulus of M speeds are selected as the preferred obstacle avoidance speed set.

[0033] In the second step, based on the potential collision detection condition formula (4), a feasible collision-free speed set (RAV) with no collision risk is obtained, and the optimal obstacle avoidance speed is selected according to the evaluation function.

[0034] First, the velocity subset RVA of the N velocities that only change direction is processed. According to the potential collision detection condition formula (4), the velocity vectors that have a collision risk with the obstacle state information predicted in the future k moments are removed. Among the remaining non-collision velocity vectors, the evaluation function G is used to select the velocity vectors with the potential collision risk.ij (8) Select the optimal steering and obstacle avoidance speed V angle .

[0035]

[0036] In the formula, (v c ,θ c ) is the current velocity vector of the drone, (v i ,θ j ) is the candidate obstacle avoidance velocity vector in RVA, (v g ,θ g ) is the linear speed (the fastest speed from the current position to the sub-target point), (VO Lθ , VO Rθ ) are speed obstacle areas VO ou The left and right sides of the object are the angles between the x-axis and the object’s left and right sides. γ is the angle between the line connecting the object and the obstacle at the current moment and the x-axis. max =max(γ t ), γ min =min(γt), t=1,2,3, γt is the angle between the predicted obstacle position at the tth moment and the line connecting the current position of the drone and the x-axis, m1, m2 and m3 are weight coefficients. To avoid the path being too long due to obstacle avoidance, the value of m1 should be the largest. Therefore, let m2=0.6, m1=0.4, m3=0.4, and take the collision avoidance cost G ij The minimum speed is taken as the optimal steering and obstacle avoidance speed V angle .

[0037] Evaluation function G ij The first part is variance(v, θ), which mainly considers the deviation cost between the obstacle avoidance speed and the current speed. The larger the cost, the worse the smoothness of the track, and this part of the cost value is normalized.

[0038] Evaluation function G ij The second part is goal(v, θ), which mainly considers the deviation cost between the obstacle avoidance speed direction and the target direction. The larger the cost, the longer the distance required to reach the target, and this part of the cost value is normalized.

[0039] Evaluation function G ij The third part is danger(v, θ), which mainly considers the movement trend of the obstacle. If the obstacle avoidance speed relative to the speed obstacle area is consistent with the obstacle movement direction, an additional obstacle avoidance danger cost is added to the speed as a penalty.

[0040] In formula (8e), if γt in the three predicted states of the obstacle is larger than the angle γ at the current moment, it is considered that the obstacle has a tendency to move to the left of the UAV, so a penalty cost is added to the speed vector on the left side of the speed obstacle area in the RV area; similarly, in formula (8f), if γt is smaller than the angle γ at the current moment, it is considered that the obstacle has a tendency to move to the right of the UAV, so a penalty cost is added to the speed vector on the right side of the speed obstacle area in the RV area. Based on the prediction of the future position of the obstacle, p' is obtained. o ,p" o 、p''' o Respectively with p u The angle γ between the connecting line and the x-axis t , compared with γ, γ max >γ,γ<γ min , so it is believed that the obstacle has a tendency to move to the left side of the drone. Therefore, a penalty cost is added to the left area RVL of the speed obstacle area in the RV area.

[0041] When the speed subset RVA of N speeds that only change direction removes the speed vectors that have a collision risk with the obstacle state information predicted at the next k moments, and the collision-free speed vector set is empty, the speed subset RVN of M speeds that only change the modulus is processed. According to the potential collision detection condition formula (4), the speed vectors that have a collision risk with the obstacle state information predicted at the next k moments are removed, and the optimal speed control obstacle avoidance speed V is selected from the remaining collision-free speed vectors according to the evaluation function (8). norm .

[0042] In short, according to the principle of convenience in the execution process of the drone, the optimal steering obstacle avoidance speed is given priority. If it does not exist, the optimal speed adjustment obstacle avoidance speed is selected. When both are not feasible, the obstacle avoidance speed is selected from the speed reachable set. That is, the final optimal obstacle avoidance speed V is determined by the following steps:

[0043] If V angle exists, then V=V angle ;

[0044] If V angle Does not exist, V norm exists, then V=V norm ;

[0045] If V norm Does not exist, V angle If it does not exist, the achievable speed set RV of the UAV is processed. According to the potential collision detection condition formula (4), the speed vectors with collision risk with the predicted obstacle state information at the next k moments are removed, and the optimal obstacle avoidance speed V is selected from the remaining collision-free speed vectors according to the evaluation function (8).

[0046] Step 6: Use fuzzy mathematics method to determine the adaptive calculation time step dt, use the calculation time step dt and the determined UAV speed to update the UAV and obstacle status at the next moment, and return to step 2.

[0047] In the traditional speed obstacle method, a fixed time interval is used to calculate whether there is a new collision risk within the detection range. However, when the interval is small and the obstacle is still far away, multiple calculations in a short period of time are a waste of computing resources, energy, and time. When the interval is long and the obstacle is close, the failure to respond to the state change of the obstacle in time greatly increases the risk of collision. Therefore, according to the distance Dist between the drone and the tangent point of the obstacle circle and the size of the collision risk probability CP, the size of the calculation time step can be adaptively adjusted to effectively improve the utilization of time.

[0048] The definition of collision risk probability CP is shown in formula (9):

[0049]

[0050] Among them, CP∈[0,4] indicates the multiple of the distance between the UAV and the obstacle's closest point in the current state with respect to the radius of the obstacle circle. 0-1 times indicates that there is a collision risk between the two, 1-4 times indicates that there is no collision risk for the time being, but the possibility of collision risk in the future is high, and more than 4 times indicates that the probability of collision risk is low, and its value is regarded as 4 times. Correspondingly, the fuzzy domain is divided into four levels: VB, B, M, S, representing very large, large, general, and small, respectively.

[0051] The range of the distance Dist between the drone and the tangent point of the obstacle circle is determined by the detection distance of the onboard sensor. The fuzzy domain of the distance Dist is divided into three levels: N, M, and F, representing near, medium, and far, respectively; correspondingly, the calculation time step dt is divided into four fuzzy domains: VS, S, M, and L, representing very short, short, general, and long, respectively.

[0052] According to the principle that the greater the obstacle collision risk, the shorter the calculation time step, that is, the smaller Dist is and the larger CP is, the larger dt is.

[0053] The present invention proposes a dynamic obstacle avoidance algorithm for unmanned aerial vehicles based on an improved speed obstacle method. First, in the detection stage, the calculation time step is dynamically adjusted by a fuzzy mathematical algorithm according to the distance and speed of the obstacle relative to the unmanned aerial vehicle, so as to reduce the waste of computing resources of the unmanned aerial vehicle under the influence of long-distance obstacles. Secondly, in the obstacle avoidance stage, the position, linear velocity and angular velocity of the obstacle are first obtained by integrating the sensor information of the unmanned aerial vehicle, and it is assumed that the state of the obstacle remains unchanged in a short time. Based on this, the motion formula of the quadratic motion model-constant turning rate and speed amplitude model can be used to predict the movement trend of the obstacle in the short future time, and it is used as the input of the speed obstacle method to obtain an extended speed obstacle area. In the discretized reachable speed set of the unmanned aerial vehicle limited by its own motion performance, the speed that does not produce a collision risk with the obstacle in a short time is selected as the optional obstacle avoidance speed. In order to ensure the reliability of the collision avoidance performance, when selecting the optimal obstacle avoidance speed, the continuous change of the state of the unmanned aerial vehicle should be avoided as much as possible, so as to achieve the collision avoidance and mission execution functions on the basis of minimum maneuvering. In addition, according to the principle of convenience in the UAV execution process, the optimal steering obstacle avoidance speed is given priority; if it does not exist, the optimal speed adjustment obstacle avoidance speed is selected; when both are not feasible, the obstacle avoidance speed is selected from the speed reachable set.

[0054] Therefore, first, a speed subset that only changes the direction is selected from the achievable speed set, and the speed vectors existing in the extended speed obstacle area are removed. For the remaining speed vectors, the optimal steering obstacle avoidance speed is selected as the optimal obstacle avoidance speed according to the evaluation function; when the optimal steering obstacle avoidance speed does not exist, a speed subset that only changes the modulus is selected from the achievable speed set, and the speed vectors existing in the extended speed obstacle area are removed. For the remaining speed vectors, the optimal speed regulation obstacle avoidance speed is selected as the optimal obstacle avoidance speed according to the evaluation function; when the optimal speed regulation obstacle avoidance speed does not exist, the speed vectors existing in the extended speed obstacle area are removed from the achievable speed set, and the optimal obstacle avoidance speed is selected for the remaining speed vectors according to the evaluation function, so that the speed is optimized in terms of rapid arrival at the sub-target point, speed change, obstacle movement trend, etc.

[0055] The present invention detects the risk of collision in advance and responds to it according to the expanded speed obstacle area, so that less time is consumed, the angle changes more gradually, the number of maneuvers is reduced, and it is safer when selecting a return point.

[0056] The present invention improves the evaluation function of the obstacle avoidance speed according to the movement trend of the obstacle, so that the algorithm selects the speed vector in the direction of the obstacle as much as possible, making the speed selection more sensible.

[0057] The present invention determines the calculation time step according to the current relative state between the UAV and the obstacle, thereby avoiding untimely response due to long step length and waste of calculation resources due to short step length. BRIEF DESCRIPTION OF THE DRAWINGS

[0058] Figure 1 For the comparison between the traditional speed barrier method and the improved speed barrier method in this paper, Figure 1 (a) is a schematic diagram of the speed barrier method. Figure 1 (b) Schematic diagram of the speed obstacle method combined with trajectory prediction.

[0059] Figure 2 Added diagram for penalty cost.

[0060] Figure 3 This is the overall flow chart of the local obstacle avoidance algorithm based on the improved speed obstacle method.

[0061] Figure 4 is a schematic diagram of the input-output membership function. Figure 4 (a) is a schematic diagram of the membership function of Dist. Figure 4 (b) is a schematic diagram of the membership function of CP. Figure 4 (c) is a schematic diagram of the membership function of dt.

[0062] Figure 5 Schematic diagram for comparison of obstacle avoidance paths at different step lengths.

[0063] Figure 6 is the drone-related variable record in the prediction instance (1), Figure 6 (a) is a comparison of the obstacle avoidance trajectories of the drone. The blue line is the curve with prediction added, and the yellow line is the curve without prediction added. '+' is the number of iterations of the calculation time step when the collision risk is detected for the first time. Figure 6 (b) is the relative distance diagram between the drone and the obstacle. Figure 6 (c) is a comparison chart of the change in the speed modulus of the drone's obstacle avoidance. Figure 6 (d) is a comparison diagram of the speed angle changes of the drone's obstacle avoidance.

[0064] Figure 7 is the drone-related variable record in the prediction instance (2), Figure 7 (a) is a comparison of the obstacle avoidance trajectories of the drone. The blue line is the curve with prediction added, and the yellow line is the curve without prediction added. Figure 7 (b) is the relative distance diagram between the drone and the obstacle. Figure 7 (c) is a comparison chart of the change in the speed modulus of the drone's obstacle avoidance. Figure 7 (d) is a comparison diagram of the speed angle changes of the drone's obstacle avoidance.

[0065] Figure 8 Schematic diagram of obstacle characteristics used in generating Monte Carlo simulations.

[0066] Fig. 9 Schematic diagram of the minimum distance between the drone and obstacles. DETAILED DESCRIPTION

[0067] In order to make the technical problems, technical solutions and beneficial effects to be solved by the present invention more clearly understood, the present invention is further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not intended to limit the present invention.

[0068] A dynamic obstacle avoidance method for a UAV, the specific steps are as follows:

[0069] Step 1: Initialize the position, speed, and radius of the drone, the position, linear speed, angular speed, and radius of the obstacle, and the starting point and sub-target point of the drone;

[0070] Step 2: Determine whether the drone has reached the target point. If it is determined that the drone has reached the target point, the algorithm ends. If it is determined that the drone has not reached the target point, execute step 3.

[0071] Step 3: Check whether there is an obstacle within the detection range. If the result of the judgment is that there is no obstacle, the speed of the drone is set to the straight-line speed at the next moment, and step 6 is executed. If the result of the judgment is that there is an obstacle, it is determined whether the speed of the drone needs to be changed according to steps 4 and 5;

[0072] Step 4: When there is an obstacle within the detection range, predict the obstacle information and determine whether there is a risk of collision with the drone flying in a straight line. If it is determined that there is no obstacle, change the speed of the drone at the next moment to the straight line speed and execute step 6; if it is determined that there is an obstacle, execute step 5;

[0073] First, the position, linear velocity and angular velocity of the obstacle are obtained by integrating the sensor information of the drone, and it is assumed that the state of the obstacle remains unchanged in a short time. Based on this, the motion formula of the quadratic motion model - the constant turning rate and speed amplitude model can be used to predict the movement trend of the obstacle in the short future time, and it can be used as the input of the speed obstacle method to expand the speed obstacle area.

[0074] Let the current state of the obstacle be X o,c =(P o,x ,P o,y ,V,φ,ω), where (P o,x ,P o,y ) is the component of the obstacle position on the coordinate axis, V is the linear velocity, φ is the obstacle direction, and ω is the angular velocity. The specific formula for predicting the state of the obstacle at the i-th moment in the future is shown in formula (1):

[0075]

[0076] At the same time, the corresponding future position of the UAV X u,c =(Pu,x ,P u,y ) prediction is shown in formula (2):

[0077] X u,c+i =X u,c +i*vΔt(2)

[0078] Here, v is the current velocity of the drone. The acceleration vector is not used because the velocity changes caused by the algorithm may cause poor predictions.

[0079] Then, it is determined whether there is a collision risk between the linear speed and the predicted obstacle state information at the next k moments. The collision cones are constructed for the drone state and the obstacle at the next k moments, and the k collision cones are merged to obtain the extended speed obstacle area. When the linear speed is within the speed obstacle area, it is considered that there is a collision risk. Because the above trajectory prediction formula is suitable for short-term prediction, the prediction range k is related to the radius and speed of the obstacle. Considering the obstacle radius and speed range, the prediction range is 3 calculation time steps.

[0080] According to the principle of the classic speed obstacle method, when the relative speed vector ν between the drone and the obstacle ou When the drone is within the collision cone, it means that there is a risk of collision between the drone and the obstacle at the current speed. Figure 1 As shown in (a), the principle of the speed barrier method combined with the trajectory prediction result is as follows Figure 1 (b) as shown.

[0081] exist Figure 1 In (a), p u 、p o are the positions of the drone and the obstacle, respectively, ou =p o -p u is the relative position vector between the UAV and the obstacle, r u 、r o are the radius of the drone and the obstacle, r s is the additional safety radius, R c =r u +r o +r s is the collision radius of the obstacle, and the collision area RRC ou It is p o is the center of the circle, R c is a circle with radius v ou It represents the velocity vector of the drone relative to the obstacle. LT is the velocity vector with the drone as the origin and p o is the center of the circle, R c The left tangent of the circle with radius RT is the line with the drone as the origin and p ois the center of the circle, R c is the right tangent of the circle with radius, β is the relative velocity vector v ou and the relative position vector p ou The angle, δ c is the distance between the UAV and the closest point to the obstacle.

[0082] According to the geometric relationship of the collision cone, if the position of the drone p u As the vertex, along the relative velocity vector ν ou The ray in the direction passes through the collision area RRC uo , then there is a potential collision risk between the drone and the obstacle. When there are multiple obstacles, there will be multiple corresponding collision cones. In order to facilitate the unified description of multiple collision cones in the absolute velocity space of the drone, the collision cone RRC can be ou and the obstacle velocity vector v o Perform Minkowski vector sum operation to obtain the speed obstacle area VO surrounded by LT' and RT' ou , as shown in formula (3), corresponds to Figure 1 (b) The area enclosed by LT’ and RT’, where RV is the achievable speed area of ​​the UAV.

[0083]

[0084] The predicted obstacle state information at the next k moments is used to construct collision cones and collision areas with the drone, and then compared with the obstacle velocity vector v o Perform Minkowski vector and operation to obtain the speed obstacle area VO' ou , VO' o ' u , VO' o ” u , the three predicted speed obstacle areas are compared with the current speed obstacle area VO ou Merged by Figure 1 The area surrounded by the tangent lines LT' and RT'3 of (b), namely, the extended speed obstacle area VO ou , when the straight-line speed is in the extended speed obstacle area VO ou If the vehicle is within the specified range, it is considered that there is a risk of collision.

[0085] Since the UAV speed is discretized later, for the convenience of calculation, formula (4) is used as the potential collision detection condition at each moment:

[0086] p ou ·v ou ≥0,δ C <R c (4)

[0087] Formula (4) means that only when the relative position vector p ou With the relative velocity vector ν ou The obstacle and the drone are considered to have a potential collision only when the angle β∈(-π / 2,+π / 2) and the distance between the drone and the closest point is less than the collision radius.

[0088] Step 5: Figure 4 As shown in the figure, when there is an obstacle within the detection range and there is a risk of collision between the UAV running at a straight-line speed and the obstacle, it is determined whether there is a risk of collision between the UAV and the obstacle at the current speed. If it is determined that there is no collision risk, there is no need to avoid the obstacle, and the UAV maintains the current speed; if it is determined that there is a collision risk, obstacle avoidance is required, and the optimal obstacle avoidance speed at the next moment is selected.

[0089] The first step is to obtain a discretized achievable speed set based on the performance constraints and safety considerations of the drone. Based on the performance constraints and safety considerations of the drone, the speed adjustment and heading adjustment of the drone will have a range limit, that is:

[0090] ν vary ∈v min ,v max (5)

[0091] Δθ∈0,Δθ max (6)

[0092] Assume that c and θ c are the current speed and heading of the UAV respectively, and the expression of the achievable speed set (RV) of the UAV is obtained as follows:

[0093] RV=(v,θ)|ν∈ν c -v min ,ν c +v max ,θ∈[θ c -Δθ max ,θ c +Δθ max ,](7)

[0094] Then, RV is discretized, the continuous θ in RV is discretized into a set of N angles at equal intervals, and the continuous v is discretized into a set of M velocities at equal intervals, thereby obtaining M×N velocity vectors ν i ,θ j , where ν i is the magnitude of the velocity vector, θ j For the heading.

[0095] In order to ensure the reliability of collision avoidance performance, the continuous change of the UAV state should be avoided as much as possible when selecting the optimal obstacle avoidance speed, so as to achieve the collision avoidance and mission execution functions on the basis of minimum maneuvers. Therefore, in the reachable speed set RV, a speed subset (RVA) that only changes the direction of N speeds and a speed subset (RVN) that only changes the modulus of M speeds are selected as the preferred obstacle avoidance speed set.

[0096] In the second step, based on the potential collision detection condition formula (4), a feasible collision-free speed set (RAV) with no collision risk is obtained, and the optimal obstacle avoidance speed is selected according to the evaluation function.

[0097] First, the velocity subset RVA of the N velocities that only change direction is processed. According to the potential collision detection condition formula (4), the velocity vectors that have a collision risk with the obstacle state information predicted in the future k moments are removed. Among the remaining non-collision velocity vectors, the evaluation function G is used to select the velocity vectors with the potential collision risk. ij (8) Select the optimal steering and obstacle avoidance speed V angle .

[0098] G ij =m1·variance(v,θ)+m2·goal(v,θ)+m3·danger(v,θ)(a)

[0099]

[0100] danger(v,θ)=danger l (v,θ)+danger r (v,θ)(d)

[0101]

[0102] Where, ν c ,θ c is the current velocity vector of the drone, ν i ,θ j is the candidate obstacle avoidance velocity vector in RVA, ν g ,θ g is the linear speed (the fastest speed from the current position to the sub-target point), VO Lθ Speed ​​barrier area VO ou The angle between the left side of and the x-axis, VO Rθ Speed ​​barrier area VO ou is the angle between the right side of the object and the x-axis, γ is the angle between the line connecting the drone and the obstacle and the x-axis at the current moment, max =max(γ t ), γ min =min(γ t), t=1,2,3,γ t is the angle between the predicted obstacle position at the tth moment and the line connecting the current position of the drone and the x-axis. m1, m2 and m3 are weight coefficients. To avoid the path being too long due to obstacle avoidance, the value of m1 should be the largest. Therefore, let m2 = 0.6, m1 = 0.4, m3 = 0.4, and take the collision avoidance cost G ij The minimum speed is taken as the optimal steering and obstacle avoidance speed V angle .

[0103] Evaluation function G ij The first part is variance(v,θ), which mainly considers the deviation cost between the obstacle avoidance speed and the current speed. The larger the cost, the worse the smoothness of the track, and this part of the cost value is normalized.

[0104] Evaluation function G ij The second part is goal(v,θ), which mainly considers the deviation cost between the obstacle avoidance speed direction and the target direction. The larger the cost, the longer the distance required to reach the target, and this part of the cost value is normalized.

[0105] Evaluation function G ij The third part is danger(v,θ), which mainly considers the movement trend of the obstacle. If the obstacle avoidance speed relative to the speed obstacle area is consistent with the obstacle movement direction, an additional obstacle avoidance danger cost is added to the speed as a penalty.

[0106] Formula (8e) is for the γ in the three predicted states of the obstacle t If there is an angle greater than the current angle γ, it is considered that the obstacle has a tendency to move to the left of the UAV, so a penalty cost is added to the speed vector on the left side of the speed obstacle area in the RV area; similarly, in formula (8f), when γ t If there is an angle smaller than the current angle γ, it is considered that the obstacle has a tendency to move to the right of the drone, so a penalty cost is added to the speed vector on the right side of the speed obstacle area in the RV area, such as Figure 2 As shown, based on the prediction of the future position of the obstacle, p' is obtained o 、p' o '、p' o ''respectively with p u The angle γ between the connecting line and the x-axis t , compared with γ, γ max >γ,γ<γ min , so it is believed that the obstacle has a tendency to move to the left side of the drone. Therefore, a penalty cost is added to the left area RVL of the speed obstacle area in the RV area.

[0107] When the speed subset RVA of N speeds that only change direction removes the speed vectors that have a collision risk with the obstacle state information predicted at the next k moments, and the collision-free speed vector set is empty, the speed subset RVN of M speeds that only change the modulus is processed. According to the potential collision detection condition formula (4), the speed vectors that have a collision risk with the obstacle state information predicted at the next k moments are removed, and the optimal speed control obstacle avoidance speed V is selected from the remaining collision-free speed vectors according to the evaluation function (8). norm .

[0108] In short, according to the principle of convenience in the execution process of the drone, the optimal steering obstacle avoidance speed is given priority. If it does not exist, the optimal speed adjustment obstacle avoidance speed is selected. When both are not feasible, the obstacle avoidance speed is selected from the speed reachable set. That is, the final optimal obstacle avoidance speed V is determined by the following steps:

[0109] If V angle exists, then V=V angle ;

[0110] If V angle Does not exist, V norm exists, then V=V norm ;

[0111] If V norm Does not exist, V angle If it does not exist, the achievable speed set RV of the UAV is processed. According to the potential collision detection condition formula (4), the speed vectors with collision risk with the predicted obstacle state information at the next k moments are removed, and the optimal obstacle avoidance speed V is selected from the remaining collision-free speed vectors according to the evaluation function (8).

[0112] Step 6: Use fuzzy mathematics method to determine the adaptive calculation time step dt, use the calculation time step dt and the determined UAV speed to update the UAV and obstacle status at the next moment, and return to step 2.

[0113] In the traditional speed obstacle method, a fixed time interval is used to calculate whether there is a new collision risk within the detection range. However, when the interval is small and the obstacle is still far away, multiple calculations in a short period of time are a waste of computing resources, energy, and time. When the interval is long and the obstacle is close, the failure to respond to the state change of the obstacle in time greatly increases the risk of collision. Therefore, according to the distance Dist between the drone and the tangent point of the obstacle circle and the size of the collision risk probability CP, the size of the calculation time step can be adaptively adjusted to effectively improve the utilization of time.

[0114] according to Figure 1The geometric relationship shown in the figure, the definition of collision risk probability CP is shown in formula (9):

[0115]

[0116] Among them, CP∈[0,4] indicates the multiple of the distance between the UAV and the obstacle's closest point in the current state with respect to the radius of the obstacle circle. 0-1 times indicates that there is a collision risk between the two, 1-4 times indicates that there is no collision risk for the time being, but the possibility of collision risk in the future is high, and more than 4 times indicates that the probability of collision risk is low, and its value is regarded as 4 times. Correspondingly, the fuzzy domain is divided into four levels: VB, B, M, S, representing very large, large, general, and small, respectively.

[0117] The range of the distance Dist between the drone and the tangent point of the obstacle circle is determined by the detection distance of the onboard sensor. The fuzzy domain of the distance Dist is divided into three levels: N, M, and F, representing near, medium, and far, respectively; correspondingly, the calculation time step dt is divided into four fuzzy domains: VS, S, M, and L, representing very short, short, general, and long, respectively.

[0118] According to the principle that the greater the obstacle collision risk, the shorter the calculation time step, that is, the smaller Dist is and the larger CP is, the larger dt is, as shown in Table 1.

[0119] Table 1 Fuzzy rules

[0120]

[0121] In summary, the obstacle avoidance algorithm of the UAV in a dynamic environment based on the improved speed obstacle method is described in detail. The overall flow chart obtained according to the algorithm execution steps is as follows: Figure 3 shown.

[0122] 1. Adaptive calculation time step example

[0123] When the drone approaches an obstacle that must be avoided, the adaptive computation time step allows the drone to start correcting its path earlier than with a long computation time step, saving computational resources compared to a short computation time step.

[0124] The input of the fuzzy mathematical controller is the distance Dist∈[0, 10] between the drone and the tangent point of the obstacle circle and the collision risk probability CP∈[0, 4], and the output is the calculation time step dt∈[0.05, 0.25]. The membership functions of the distance Dist between the drone and the tangent point of the obstacle circle, the collision risk probability CP, and the calculation time step dt are as follows: Figure 4 (a) Figure 4 (b) Figure 4 (c) as shown.

[0125] Figure 5 is a comparison of the obstacle avoidance paths of the algorithm proposed in this invention under different step sizes. In order to reflect the effect of adaptive calculation time step, the obstacle speed is set to 0. Figure 5 As shown in the figure, as the drone moves around the side of the obstacle, it gradually shortens the distance to the obstacle to the minimum safe distance, and adaptively adjusts the calculation time step according to the collision risk of the obstacle, avoiding the untimely obstacle avoidance response caused by too long calculation time step (blue line) and the waste of computing resources caused by too short calculation time step (yellow line). The path length and flight time under the three step sizes are shown in Table 2. It can be seen that the adaptive calculation time step is 5.27% faster than the 0.1 second step in time, reducing the waste of computing resources. Compared with the 0.2 second step, the obstacle avoidance speed is updated faster in the face of rapidly changing obstacles, improving safety, and there is also a reduction of 0.222m on the path. The obstacle is a blue circle with coordinates (9,8), the safety radius is set to 0.3 meters, the starting point is (0, 0), the end point is (20, 20), and the drone detection distance is 8m.

[0126] Table 2 Path length and flight time under three step sizes

[0127] dt 0.2s Vary 0.1s Time(s) 7.708 8.625 9.105 Length(m) 29.999 29.777 29.499

[0128] 2. Example of forecasting trend:

[0129] (1) Detect collision risks in advance:

[0130] Considering the movement trend of obstacles in the future helps drones detect collision risks in advance and respond accordingly, improving the safety of obstacle avoidance and reducing the cost of obstacle avoidance. Considering obstacles as a constant turning rate and speed amplitude motion model in a short period of time not only takes into account the nonlinearity of obstacle motion trajectories in actual situations, but also reduces the computational complexity of predicting obstacle motion trends. Examples of detecting collision risks in advance include Figure 6 As shown, the starting point and the end point remain unchanged, the obstacle is a blue circle, the red curve is the obstacle trajectory, the initial position is set to (9,7), the safety radius is set to 0.5 meters, the initial speed is (vox,voy) = (2.1,-0.7) m / s, the angular velocity is 0.6 rad / s, the tangential acceleration of the drone is set to 5 m / s2, the radial acceleration is set to 5 rad / s2, the speed is changed by uniform acceleration, and the detection distance is 8m.

[0131] Embodiment (1): According to Figure 6It can be seen that when the function of predicting the movement trend of obstacles is not added (yellow line), the UAV finds that the UAV is in conflict with the obstacle at the 18th calculation time step iteration (about 1.8s), and after adding the function of predicting the movement trend of obstacles (blue line), the UAV finds that the UAV is in conflict with the obstacle at the 15th iteration (about 1.5s), and starts the obstacle avoidance maneuver in advance. The selected obstacle avoidance speed does not have directional jitter, and the two operation results are shown in Table 3. According to the operation results, it can be seen that the total time consumption of the algorithm proposed in the present invention is shorter, which is reduced by 10.34%, the cumulative amount of angle change is less, which is reduced by 31.06%, and the corresponding maneuvering time consumption is also less, which is reduced by 32.57%, and the number of maneuvers is also reduced. In general, after adding the prediction function, the collision risk can be detected in advance and the obstacle avoidance maneuver can be started, so that there is more time for obstacle avoidance maneuvers, the angle change is also more gentle, and the obstacle avoidance speed selected can be more predictable according to the predicted obstacle movement trend, reducing the directional jitter of the obstacle avoidance speed, and reducing the number of small maneuvers and reducing the maneuvering time.

[0132] Table 3 Comparison of the results of the prediction trend function operation

[0133]

[0134] (2) Safer return point:

[0135] Considering the movement trend of obstacles in the future will help the drone improve the safety of selecting the time to resume the route to the sub-target point, that is, the return point. The traditional speed obstacle method only considers the current state of the drone and the obstacle. When it is judged that there is no collision risk when resuming the route to the sub-target point, it starts to return. However, the obstacle may suddenly change its speed, resulting in a collision risk between the obstacle state and the drone at the next moment. In this case, the drone needs to re-enter the obstacle avoidance program, causing the path to jitter and more obstacle avoidance costs.

[0136] Embodiment (2): Figure 7 As shown, the obstacle state is changed to the initial position (4,5), the safety radius is set to 0.5 meters, the initial speed is (vox,voy) = (0.6,2.1) m / s, the angular velocity is -0.2rad / s, the detection distance is 5m, and the rest of the settings are the same as (1). The results after running are that when the function of predicting the movement trend of obstacles is not added (yellow line), the cumulative angle change is 33.195 degrees, the number of angle maneuvers is 4 times, and the total maneuvering time is 0.942s; when the function of predicting the movement trend of obstacles is added (blue line), the cumulative angle change is 23.333 degrees, the number of maneuvers is 2 times, and the total maneuvering time is 0.569s. According to the running results Figure 7(d) It can be seen that the algorithm proposed in the present invention takes into account the movement trend of the obstacle when selecting the return point. The return is started only when the straight-line speed and the predicted short-term state of the obstacle do not produce a collision risk, thereby reducing the possibility of the UAV and the obstacle colliding again, so as to re-enter the obstacle avoidance step, resulting in increased obstacle avoidance cost, path jitter, and even collision in severe cases.

[0137] 3. Monte Carlo simulation:

[0138] The robustness of the proposed algorithm was investigated by Monte Carlo simulation, generating 500 different scenarios in which the following properties of the obstacles were assigned using a continuous uniform distribution: impact point, initial position, radius, linear velocity, and angular velocity. First, the impact point of the drone was randomly selected between the initial position of the drone (0,0) and the target point (20,20), such as Figure 8 shown.

[0139] Define this influence point as r p =(l p l p ), where l p ∈[0, 20], such as Figure 8 Then, the flight time of the drone to this point can be calculated as follows:

[0140]

[0141] Among them, v avg is the average speed of the drone. According to the obstacle avoidance strategy with turn priority, v avg is roughly chosen to be 5 m / s. In addition, considering the reasonable initial gap between the drone and the obstacle, the case of using tgo ≤ 1.5 s is excluded. Then, the range of the linear velocity and angular velocity of the obstacle at the impact point is set to [2, 6] and [-1, 1], respectively, and these are denoted as v0 and ω0, respectively. The angle between the linear velocity and the x-axis is Taking the value [-π, π], the initial state of the obstacle can be calculated as follows:

[0142]

[0143] In addition, the obstacle radius r o The value range is [1, 3], the safety radius r s Take it as 0.5m, then the Monte Carlo simulation results are as follows Fig. 9 As shown in Figure 1, this is the iterative sequence of time steps for calculating the minimum distance between the drone and the obstacle in each case. In any case, the drone reaches the target point without violating the safety requirement because the minimum distance is greater than the increased safety radius r. s,The Monte Carlo simulation results verify that the algorithm has good ,successful collision avoidance performance.

[0144] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions and improvements made within the spirit and principle of the present invention should be included in the scope of the present invention.

Claims

1. A dynamic obstacle avoidance method for an unmanned aerial vehicle, characterized in that: The specific steps are as follows: Step 1: Initialize the position, speed, and radius of the drone, the position, linear speed, angular speed, and radius of the obstacle, and the starting point and sub-target point of the drone; Step 2: Determine whether the drone has reached the target point. If it is determined that the drone has reached the target point, the algorithm ends. If it is determined that the drone has not reached the target point, execute step 3. Step 3: Check whether there is an obstacle within the detection range. If the result of the judgment is that there is no obstacle, the speed of the drone is set to the straight-line speed at the next moment, and step 6 is executed. If the result of the judgment is that there is an obstacle, it is determined whether the speed of the drone needs to be changed according to steps 4 and 5; Step 4: When there is an obstacle within the detection range, predict the obstacle information and determine whether there is a risk of collision with the drone flying in a straight line. If it is determined that there is no obstacle, change the speed of the drone at the next moment to the straight line speed and execute step 6; if it is determined that there is an obstacle, execute step 5; Step 5: When there is an obstacle within the detection range and there is a risk of collision between the drone and the obstacle when the drone is running at a straight-line speed, determine whether there is a risk of collision between the drone and the obstacle at the current speed. If there is no risk of collision, there is no need to avoid the obstacle and the drone maintains the current speed. If there is a risk of collision, it is necessary to avoid the obstacle and the optimal obstacle avoidance speed at the next moment is selected. Step 6: Use fuzzy mathematics method to determine the adaptive calculation time step dt, use the calculation time step dt and the determined UAV speed to update the UAV and obstacle status at the next moment, and return to step 2.

2. The dynamic obstacle avoidance method for a drone according to claim 1, characterized in that: In step 4, the position, linear velocity and angular velocity of the obstacle are first obtained by integrating the sensor information of the drone. Assuming that the state of the obstacle remains unchanged in a short period of time, the movement trend of the obstacle in the future is predicted and used as the input of the speed obstacle method to expand the speed obstacle area. Let the current state of the obstacle be X o,c for: X o,c =(P o,x ,P o,y ,V,φ,ω) Among them, (P o,x ,P o,y ) is the component of the obstacle position on the coordinate axis, V is the linear velocity, φ is the obstacle direction, and ω is the angular velocity; The specific formula for predicting the state of the obstacle at the i-th moment in the future is as shown in formula (1): The corresponding future position of the drone is X u,c =(P u,x ,P u,y ) prediction is shown in formula (2): X u,c+i =X u,c +i*vΔt (2) Among them, v is the speed of the drone at the current moment; Determine whether there is a collision risk between the linear speed and the predicted obstacle state information at the next k moments. Construct collision cones for the drone state and the obstacle at the next k moments. Merge the k collision cones to obtain the extended speed obstacle area. When the linear speed is within the speed obstacle area, it is considered that there is a collision risk. When the relative velocity vector ν between the drone and the obstacle ou When the drone is within the collision cone, it means that there is a risk of collision between the drone and the obstacle at the current speed. u is the position of the UAV, p o is the position of the obstacle, p ou =p o -p u is the relative position vector between the UAV and the obstacle, r u is the radius of the drone, r o is the radius of the obstacle, r s R is the additional safety radius. c =r u +r o +r s is the collision radius of the obstacle, and the collision area RRC ou It is p o is the center of the circle, R c is a circle with radius v ou It represents the velocity vector of the drone relative to the obstacle. LT is the velocity vector with the drone as the origin and p o is the center of the circle, R c The left tangent of the circle with radius RT is the line with the drone as the origin and p o is the center of the circle, R c is the right tangent of the circle with radius, β is the relative velocity vector v ou and the relative position vector p ou The angle, δ c is the distance between the UAV and the closest point to the obstacle; Collision cone RRC ou and the obstacle velocity vector v o Perform Minkowski vector and operation to obtain the speed obstacle area VO surrounded by LT' and RT' ou , as shown in formula (3): The predicted obstacle state information at the next k moments is used to construct collision cones with the drone, and the collision areas are constructed and compared with the obstacle velocity vector v o Perform Minkowski vector and operation to obtain the speed obstacle area VO' ou , VO” ou , VO”' ou , the three predicted speed obstacle areas are compared with the current speed obstacle area VO ou Merge into extended speed barrier area VO ou , when the straight-line speed is in the extended speed obstacle area VO ou If the value is within 100, it is considered that there is a collision risk; Formula (4) is used as the potential collision detection condition at each moment: p ou ·v ou ≥0.δ C <R c (4) Formula (4) means that only when the relative position vector p ou With the relative velocity vector ν ou When the angle β∈(-π / 2,+π / 2) and the distance between the UAV and the closest point is less than the collision radius, the obstacle and the UAV are considered to have a potential collision.

3. The dynamic obstacle avoidance method for a drone according to claim 2, characterized in that: In step 5, the drone speed adjustment amount ν vary There are limits on the range of the heading adjustment Δθ, namely: ν vary ∈[v min ,v max ] (5) Δθ∈[0,Δθ max ] (6) Assume that c is the current speed of the drone, θ c is the current heading of the UAV, and the expression of the achievable speed set RV of the UAV is obtained as follows: RV={(v,θ)|ν∈[ν c -v min ,n c +v max ],θ∈[θ c -Dth max ,i c +Δθ max ,]}(7) Then, RV is discretized, the continuous θ in RV is discretized into a set of N angles at equal intervals, and the continuous v is discretized into a set of M velocities at equal intervals, and M×N velocity vectors (ν i ,θ j ), where ν i is the magnitude of the velocity vector, θ j For the heading; A speed subset RVA of N speeds with only changed directions and a speed subset RVN of M speeds with only changed moduli are selected from the achievable speed set RV as the preferred obstacle avoidance speed set; Based on the potential collision detection condition (4), the feasible collision-free speed set RAV with no collision risk is obtained, and the optimal obstacle avoidance speed is selected according to the evaluation function; First, the velocity subset RVA of the N velocities that only change direction is processed. According to the potential collision detection condition (4), the velocity vectors that have a collision risk with the obstacle state information predicted in the future k moments are removed. Among the remaining non-collision velocity vectors, the evaluation function G is used to select the velocity vectors with the highest collision risk. ij (8) Select the optimal steering and obstacle avoidance speed V angle : G ij =m1·variance(v,θ)+m2·goal(v,θ)+m3·danger(v,θ) (a) danger(v,θ)=danger l (v,θ)+danger r (v,θ)(d) In the formula, (ν c ,θ c ) is the current velocity vector of the drone, (ν i ,θ j ) is the candidate obstacle avoidance velocity vector in RVA, (ν g ,θ g ) is the linear velocity, VO Lθ Speed ​​barrier area VO ou The angle between the left side of and the x-axis, VO Rθ Speed ​​barrier area VO ou is the angle between the right side of the object and the x-axis, γ is the angle between the line connecting the drone and the obstacle at the current moment and the x-axis, max =max(γ t ), γ min =min(γ t ), t=1,2,3,γ t is the angle between the predicted obstacle position at the tth moment and the line connecting the current position of the drone relative to the x-axis. m1, m2 and m3 are all weight coefficients. Let m2 = 0.6, m1 = 0.4, m3 = 0.4, and take the collision avoidance cost G ij The minimum speed is taken as the optimal steering and obstacle avoidance speed V angle ; Evaluation function G ij The first part is variance(v,θ), which is used to normalize the variance(v,θ) value. Evaluation function G ij The second part is goal(v,θ), which normalizes the value of goal(v,θ); Evaluation function G ij The third part is danger(v,θ), and formula (8e) is for γ in the three predicted states of the obstacle. t If there is an angle greater than the current angle γ, it is considered that the obstacle has a tendency to move to the left of the UAV, and a penalty cost is added to the speed vector on the left side of the speed obstacle area in the RV area; in formula (8f), when γ t If there is an angle smaller than the current angle γ, it is considered that the obstacle has a tendency to move to the right of the drone, and a penalty cost is added to the speed vector on the right side of the speed obstacle area in the RV area; According to the potential collision detection condition (4), the velocity vectors with collision risk with the predicted obstacle state information at the next k moments are removed. Among the remaining collision-free velocity vectors, the optimal speed control obstacle avoidance speed V is selected according to the evaluation function (8). norm ; The optimal obstacle avoidance speed V is determined by the following steps: If V angle exists, then V=V angle ; If V angle Does not exist, V norm exists, then V=V norm ; If V norm Does not exist, V angle If it does not exist, the achievable speed set RV of the UAV is processed, and the speed vectors with the risk of collision with the predicted obstacle state information at the next k moments are removed according to the potential collision detection condition formula (4). Among the remaining collision-free speed vectors, the optimal obstacle avoidance speed V is selected according to the evaluation function (8).

4. The dynamic obstacle avoidance method for a drone according to claim 3, characterized in that: In step 6, the definition of collision risk probability CP is as shown in formula (9): Among them, CP∈[0,4] indicates the distance between the UAV and the closest point to the obstacle in the current state in multiples of the radius of the obstacle circle, p ou is the relative position vector between the UAV and the obstacle, v ou is the relative velocity vector between the UAV and the obstacle, R C is the radius of the obstacle circle consisting of the UAV radius, collision radius and safety distance, δ c is the distance between the UAV and the closest point to the obstacle; The distance Dist between the drone and the obstacle circle tangent point is determined by the detection distance of the onboard sensor.

Images