CN112925342A - Unmanned aerial vehicle dynamic obstacle avoidance method based on improved mutual velocity obstacle method - Google Patents
Unmanned aerial vehicle dynamic obstacle avoidance method based on improved mutual velocity obstacle method Download PDFInfo
- Publication number
- CN112925342A CN112925342A CN202110078490.7A CN202110078490A CN112925342A CN 112925342 A CN112925342 A CN 112925342A CN 202110078490 A CN202110078490 A CN 202110078490A CN 112925342 A CN112925342 A CN 112925342A
- Authority
- CN
- China
- Prior art keywords
- unmanned aerial
- aerial vehicle
- obstacle
- speed
- alpha
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 82
- 230000004888 barrier function Effects 0.000 claims abstract description 31
- 230000008569 process Effects 0.000 claims description 17
- 238000010586 diagram Methods 0.000 description 9
- 238000005516 engineering process Methods 0.000 description 3
- 230000003068 static effect Effects 0.000 description 2
- RZVHIXYEVGDQDX-UHFFFAOYSA-N 9,10-anthraquinone Chemical compound C1=CC=C2C(=O)C3=CC=CC=C3C(=O)C2=C1 RZVHIXYEVGDQDX-UHFFFAOYSA-N 0.000 description 1
- 230000001133 acceleration Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 230000002068 genetic effect Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000005457 optimization Methods 0.000 description 1
- 230000010355 oscillation Effects 0.000 description 1
- 239000002245 particle Substances 0.000 description 1
- 230000035484 reaction time Effects 0.000 description 1
- 238000010845 search algorithm Methods 0.000 description 1
- 238000004904 shortening Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 230000004083 survival effect Effects 0.000 description 1
- 230000000007 visual effect Effects 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/10—Simultaneous control of position or course in three dimensions
- G05D1/101—Simultaneous control of position or course in three dimensions specially adapted for aircraft
Landscapes
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The invention discloses an unmanned aerial vehicle dynamic obstacle avoidance method based on an improved mutual velocity obstacle method, which is characterized in that the mutual velocity obstacle method is improved, a new flight velocity direction is reselected after collision between an unmanned aerial vehicle and an obstacle is judged, and the optimal flight velocity and flight direction are reselected according to the positions of the current unmanned aerial vehicle and a target point after obstacle avoidance is finished. The method comprehensively considers the time, the place, the distance and other factors of the collision of the unmanned aerial vehicle and the dynamic barrier, particularly, the speed selection is carried out after the collision is judged, the smooth track and the collision-free flight path of the unmanned aerial vehicle are ensured, so that the unmanned aerial vehicle can select the speed vector according to the size of the unmanned aerial vehicle, the radius and the distance of the dynamic barrier, unnecessary jitter is reduced, and the collision avoidance time is shortened.
Description
Technical Field
The invention belongs to the technical field of flight control of unmanned aerial vehicles, relates to an obstacle avoidance technology of an unmanned aerial vehicle in a dynamic complex environment, and particularly relates to an unmanned aerial vehicle dynamic obstacle avoidance method based on an improved mutual velocity obstacle method.
Background
In recent years, unmanned aerial vehicles play more and more important roles in military and civil fields, development potential is gradually excavated, and unmanned aerial vehicles with autonomous mission execution capability are an inevitable trend of future development. The path planning is a key technology for ensuring the autonomous flight of the unmanned aerial vehicle and improving the survival ability and the safety index. As one of key technologies reflecting autonomous control capability of unmanned aerial vehicles, obstacle avoidance airway planning is receiving wide attention. The obstacle avoidance path planning problem is how to plan a path which can safely bypass all obstacles without collision for the unmanned aerial vehicle in an environment with the obstacles. The flight path should satisfy the physical constraint condition of the unmanned aerial vehicle, and should be capable of safely avoiding obstacles and threats. Obstacle avoidance airway planning is the basis and important component of unmanned aerial vehicle mission planning. Obstacles generally refer to objects that need to be avoided by a drone in a flight environment, which may be moving or stationary, and it is very important to deal with collision of the drone with obstacles, especially with dynamic obstacles, and is a difficult problem. The online planning under the non-structural environment should include avoidance of unknown risks, so that collision threats are predicted, and effective collision avoidance is planned again. The detection information mainly comprises the distance, the direction, the visual angle, the relative speed and the acceleration of the unmanned aerial vehicle and the obstacle. At present, there are two main obstacle avoidance methods: one method is to add the position information and the obstacle information of the unmanned aerial vehicle into the trajectory planning as constraint conditions, such as an A-star search algorithm, a fast-expanding random tree, a genetic algorithm, a particle swarm optimization algorithm and an artificial potential field method, and the method solves the problem that the motion trajectory of the unmanned aerial vehicle does not meet obstacle avoidance constraint. And designing an obstacle avoidance guidance law according to the direction and the position. The first method is mainly used for static obstacle avoidance. With the increasing range of unmanned aerial vehicles, the requirement of practical problems cannot be met only by adopting static obstacle avoidance, and with the complexity of working environment, dynamic obstacle avoidance becomes more important. For dynamic planning, a second method is generally adopted to meet the requirements of reaction time and quick maneuvering, and the motion information and the relative motion trend of the unmanned aerial vehicle and the obstacle can be fully utilized.
A Velocity Obstacle (VO) method belongs to a method for avoiding dynamic Obstacle collision based on a Velocity space, has the advantages of simplicity, intuition and good real-time performance, and can meet the requirement of real-time Obstacle avoidance. In recent years, the device is widely applied to the problems of obstacle avoidance and detachment of unmanned planes and robots. On the basis of the Velocity barrier, the existing improved method adopts a mutual Velocity barrier method (RVO). The velocity barrier method is to select any one of the velocities except the set of absolute velocities, and the mutual velocity barrier method is to select a velocity satisfying the following condition:
compared with a speed obstacle method, although the mutual speed obstacle method reduces the speed selection range during obstacle avoidance, the collision and the oscillation cannot occur in the collision avoidance process, the problem that the turning angle is too large can be caused by the speed adopted by the mutual speed obstacle method, the flight performance of the unmanned aerial vehicle cannot be met, the flight speed is further influenced, and the speed cannot be reached.
Disclosure of Invention
In order to overcome the defects in the prior art, the invention provides a dynamic obstacle avoidance method for an unmanned aerial vehicle with an improved mutual velocity obstacle method, which solves the problem that when the unmanned aerial vehicle flies towards a target point by using the velocity reselected by the mutual velocity obstacle method in a velocity space, the turning angle is too large. The improved mutual velocity obstacle method improves the existing mutual velocity obstacle method, reduces the turning angle of the unmanned aerial vehicle by reselecting a new obstacle avoidance speed, and ensures a smooth path. The method is suitable for dynamic obstacle avoidance of the single unmanned aerial vehicle, and can avoid the problem of unsmooth path caused by overlarge turning angle of the unmanned aerial vehicle. For convenience of description, the following term names and parameters are defined:
alpha: the angle formed by the connecting line of the obstacle coordinate and the unmanned aerial vehicle coordinate and the X positive half shaft;
beta: define Beta ═ asin ((r _ a + r _ B)/d); r _ a is the drone radius; r _ B is the obstacle radius;
d, the distance between the obstacle and the unmanned aerial vehicle;
m, N: m, N are two rays from the drone that are perpendicular to the radius of the obstacle.
Gamma is the angle between the line connecting the optimal speed direction V _ Abest of the unmanned plane and the speed V _ B of the obstacle and the positive half axis of X.
The technical scheme of the invention is as follows:
a dynamic obstacle avoidance method of an unmanned aerial vehicle based on an improved mutual velocity obstacle method is characterized in that the velocity obstacle method is improved, after collision of the unmanned aerial vehicle and an obstacle is judged, a new flight velocity direction is reselected, and after obstacle avoidance is finished, the optimal flight velocity and flight direction are reselected according to the positions of the current unmanned aerial vehicle and a target point. The method comprises the following steps:
1) acquiring the area size of a task area, the initial coordinate, speed and radius of the unmanned aerial vehicle and the initial coordinate, speed, radius and direction of an obstacle; the coordinates, radius and speed of the unmanned aerial vehicle are respectively marked as Pos _ a (x), Pos _ a (y)), r _ A, V _ a, and the coordinates, radius and speed of the obstacle are respectively marked as Pos _ B (x), Pos _ B (y)), r _ B, V _ B; defining the coordinates of a point, and if the coordinates of the point P are marked as P (x), P (y));
2) calculating to obtain the distance condition of whether the unmanned aerial vehicle initial speed direction, the unmanned aerial vehicle and the barrier collide or not and the unmanned aerial vehicle and the target point.
21) And (3) calculating to obtain the optimal speed direction of the unmanned aerial vehicle by adopting the formula (1) according to the initial coordinate information and the target point coordinate information of the unmanned aerial vehicle.
ang_A＝atan2(Goal_A(y)-Pos_A(y),Goal_A(x)-Pos_A(x))
V _ Abest ═ (V _ Abest (x), V _ Abest (y)) (1)
Wherein: v _ Abest (X) ═ V _ a × cos (ang _ a), V _ Abest (Y) ═ V _ a × sin (ang _ a), V _ Abest (X), V _ Abest (Y), respectively, are distances that the obstacle optimal speed direction V _ Abest moves on the X axis and the Y axis. and ang _ A is an included angle between a connecting line of the target point coordinate and the unmanned aerial vehicle initial coordinate and the X positive half shaft, and the unit is expressed by radian. Goal _ A (X), Goal _ A (Y) are X-axis and Y-axis coordinates of the target point, respectively, and Pos _ A (X), Pos _ A (Y) are X-axis and Y-axis coordinates of the unmanned aerial vehicle position, respectively.
22) According to the information of the optimal speed direction V _ absest and the obstacle speed V _ B of the unmanned aerial vehicle, the position Pos _ A of the unmanned aerial vehicle and the position Pos _ B of the obstacle obtained by the formula (1), whether the unmanned aerial vehicle and the obstacle collide is judged.
Firstly, obtaining the value of the included angle Gamma between the connecting line of the optimal speed direction V _ Abest of the unmanned aerial vehicle and the barrier speed V _ B and the positive half axis X by using the formula (2). Then, obtaining the value of an included angle Alpha between a connecting line of the coordinates of the obstacle and the coordinates of the unmanned aerial vehicle and the positive X half axis by using the formula (3):
Gamma-Gamma (atan2(V _ absest (y) -V _ b (y), V _ absest (x) -V _ b (x))) formula (2)
Alpha-atan 2(Pos _ b (y) -Pos _ a (y), Pos _ b (x) -Pos _ a (x)) (3)
Where both Gamma and Alpha units are expressed in radians.
The formula of the distance d between the obstacle and the unmanned aerial vehicle is shown in formula 4:
and then, judging whether the unmanned aerial vehicle and the obstacle collide with each other or not according to the distance d between the obstacle and the unmanned aerial vehicle and the value of r _ B x 2. Two cases are included:
case 2. when d is not equal to r _ B × 2, let Beta ═ asin ((r _ a + r _ B)/d), the range of values of the asin function is [ -pi/2, pi/2 ] by the nature of the arcsine asin function, thus including three cases: B1) Alpha-Beta is less than 0, and when Gamma is between 0 and Alpha + Beta or between Alpha-Beta +2 pi and 2 pi, no collision occurs, i.e., result is 1. Otherwise, collision occurs when result is 0; B2) alpha + Beta is greater than 2 pi, and when Gamma is from 0 to Alpha + Beta-2 pi or from Alpha-Beta to 2 pi, no collision occurs, i.e., result is 1. Otherwise, collision occurs when result is 0; B3) except B1), B2), no collision occurs when Gamma is between 0 and Alpha + Beta-2 pi or between Alpha-Beta and 2 pi, i.e., result is 1. Otherwise, collision occurs when result is 0;
23) and identifying whether the unmanned aerial vehicle reaches the target point or not according to the distance between the current position of the unmanned aerial vehicle and the target point.
If the distance between the current position of the drone and the target point is between 0 and r _ a, the getgoal variable (representing whether the drone reaches the target point) is set equal to 1, otherwise it is set to 0. 1 represents that the drone has arrived at the target point, and 0 represents that the drone has not arrived at the target point.
3) Path planning in the absence of collision;
if no collision of obstacles occurs during the flight, that is, if result is 0, the drone flies to the target point along the direction of V _ Abest, and when the value of getgoal changes from 0 to 1, the algorithm ends.
4) Path planning in the event of a collision;
if it is calculated that a collision will occur during flight, then result has a value of 1. The unmanned aerial vehicle flies along the direction of V _ Abest firstly, and when and only when the distance between the unmanned aerial vehicle and the obstacle in the last second is more than 3 (r _ A + r _ B) and the distance between the unmanned aerial vehicle and the obstacle in the next second is less than or equal to 3 (r _ A + r _ B), namely the count value is 1, the unmanned aerial vehicle retreats to the position in the last second, and the speed direction of the unmanned aerial vehicle is changed to fly.
5) Selecting the speed when collision occurs;
the speed selection includes two cases:
1. if the coordinate of the unmanned aerial vehicle is above the extension line of the speed of the obstacle or on the extension line of the speed of the obstacle, namely at the abscissa of the unmanned aerial vehicle, and the ordinate of the unmanned aerial vehicle is greater than or equal to the ordinate of the equation where the extension line of the speed of the obstacle is located, the unmanned aerial vehicle selects the speed of the obstacle in the upper direction.
2. If the coordinate of the unmanned aerial vehicle is below the extension line of the speed of the obstacle, namely at the abscissa of the unmanned aerial vehicle, and the ordinate of the unmanned aerial vehicle is smaller than the ordinate of the equation where the extension line of the speed of the obstacle is located, the unmanned aerial vehicle selects the speed of the obstacle in the direction below the obstacle.
6) Speed direction in the event of a collision;
two cases are included:
1. when the unmanned aerial vehicle coordinate is in the top of barrier speed extension line or when on barrier speed extension line, in order to guarantee unmanned aerial vehicle's safety, unmanned aerial vehicle next second's speed: unmanned aerial vehicle's turn angle is (Beta + Alpha), and the turn size is:
2. when the coordinates of the unmanned aerial vehicle are below the extension line of the speed of the obstacle, the speed of the unmanned aerial vehicle for the next second: unmanned aerial vehicle's turn angle is (Alpha-Beta), and the turn size is:
7) updating the flight speed direction after obstacle avoidance to be a V _ Absest direction;
in order to reduce the turning angle of the unmanned aerial vehicle, after the turning angle avoids conflict, the unmanned aerial vehicle does not return to the original speed direction, and the optimal speed of the unmanned aerial vehicle is recalculated according to the current position of the unmanned aerial vehicle and the position of the target point. Then, the unmanned aerial vehicle flies according to the updated V _ Abest direction to avoid the problems that the original flight path is recovered, the turning times are increased, and the path length is increased, so that the unmanned aerial vehicle flies along the target point after the flight angle is changed to avoid the obstacle.
Through the steps, the unmanned aerial vehicle dynamic obstacle avoidance based on the improved mutual velocity obstacle method in the flight process of the unmanned aerial vehicle is realized.
Compared with the prior art, the invention has the beneficial effects that:
the invention provides an improved mutual velocity barrier method, which comprehensively considers the time, the place, the distance and other factors of collision of an unmanned aerial vehicle and a dynamic barrier, particularly judges the speed selection problem of collision, ensures a smooth track and a collision-free path, and enables the unmanned aerial vehicle to select a velocity vector according to the size of the unmanned aerial vehicle, the radius and the distance of the dynamic barrier, thereby reducing unnecessary jitter and shortening collision avoidance time. The invention has the following technical advantages:
the turning angle of the unmanned aerial vehicle in the flying process is reduced, and the smoothness of a path and the maneuvering frequency of the unmanned aerial vehicle are ensured;
and (II) after obstacle avoidance, the aircraft no longer returns to the original flight track, but flies by adopting a new V _ Abest, so that the turning times are effectively reduced.
By comparing three factors of flight time, turning angle and total path length of two algorithms of a mutual velocity obstacle method and the improved mutual velocity obstacle method provided by the invention, the algorithm can effectively reduce the turning angle in the flight process, and is suitable for effectively avoiding obstacles in a single unmanned aerial vehicle dynamic environment.
Drawings
Fig. 1 is a schematic diagram of speed selection during flight of an unmanned aerial vehicle.
Fig. 2 is a schematic diagram of speed and direction selection when a collision occurs during the flight of the unmanned aerial vehicle.
Fig. 3 is a schematic diagram of the speed direction after obstacle avoidance in the flight process of the unmanned aerial vehicle.
Fig. 4 is a flow chart diagram of a dynamic obstacle avoidance method based on an improved mutual velocity obstacle method provided by the invention.
Fig. 5 is a schematic view of a flight path of a drone in an embodiment of the present invention;
in the diagram o represents an obstacle,. o represents a drone and. target point.
Detailed Description
The invention will be further described by way of examples, without in any way limiting the scope of the invention, with reference to the accompanying drawings.
The invention provides an improved mutual velocity barrier method, which is used for solving the problem that when an unmanned aerial vehicle flies towards a target point by using the velocity reselected by the mutual velocity barrier method in a velocity space, the turning angle is too large. The unmanned aerial vehicle dynamic obstacle avoidance device is suitable for the problem of dynamic obstacle avoidance of the unmanned aerial vehicle, and avoids the problem that the unmanned aerial vehicle is maneuvering for many times due to overlarge turning angle.
Fig. 4 shows a flow of a dynamic obstacle avoidance method based on the improved mutual velocity obstacle method provided by the present invention. In specific implementation, the method specifically comprises the following steps:
1) acquiring the area size of a task area, initial coordinates, speed and radius of an unmanned aerial vehicle and an obstacle and the speed direction of the obstacle, wherein the coordinates, the radius and the speed of the unmanned aerial vehicle are Pos _ A, r _ A, V _ A respectively, and the coordinates, the radius and the speed of the obstacle are Pos _ B, r _ B, V _ B respectively;
2) calculating to obtain the distance condition of whether the unmanned aerial vehicle initial speed direction, the unmanned aerial vehicle and the barrier collide or not and the unmanned aerial vehicle and the target point.
21) And (3) calculating to obtain the optimal speed direction of the unmanned aerial vehicle by adopting the formula (1) according to the initial coordinate information and the target point coordinate information of the unmanned aerial vehicle.
ang_A＝atan2(Goal_A(y)-Pos_A(y),Goal_A(x)-Pos_A(x))
V _ Abest ═ (V _ Abest (x), V _ Abest (y)) (1)
Since the value of ang _ a should range from 0 to 2 pi and should not be a negative number, when ang _ a is less than 0, 2 pi is added to ang _ a.
Wherein: v _ abest (X) ═ V _ a cos (ang _ a), V _ abest (y) ═ V _ a sin (ang _ a), ang _ a is the angle between the line connecting the coordinates of the starting point and the coordinates of the target point and the positive half axis X, and the unit is expressed by radian.
22) And judging whether the unmanned aerial vehicle and the obstacle collide with each other or not according to the information of V _ Absest and V _ B, Pos _ A, Pos _ B obtained by the formula 1.
Gamma-Gamma (atan2(V _ absest (y) -V _ b (y), V _ absest (x) -V _ b (x))) formula (2)
Alpha-atan 2(Pos _ b (y) -Pos _ a (y), Pos _ b (x) -Pos _ a (x)) (3)
First, a value of Gamma is obtained by equation (2). Similarly, the value range of Gamma should be between 0 and 2 pi, and should not be a negative number. Therefore, when the value of Gamma is less than 0, 2 π is added to the Gamma. Then, the value of Alpha is obtained using equation 3, and if Alpha is less than 0, Alpha +2 pi. The formula of the distance d between the obstacle and the unmanned aerial vehicle is shown in formula 4:
then, judge whether unmanned aerial vehicle and barrier can collide. The judgment basis is divided into two conditions according to the distance d between the obstacle and the unmanned aerial vehicle and the value of r _ B x 2:
when d is equal to r _ B × 2, the method is divided into three cases: (1) alpha is the first quadrant angle, and only when Gamma is from 0 to Alpha + pi/2 or from Alpha +3 pi/2 to 2 pi, the unmanned aerial vehicle can not collide in the whole flight process, namely result is 1. Otherwise, collision occurs when result is 0; (2) alpha is the second and third quadrant angle, and no collision occurs, i.e. result is 1, only Gamma is between Alpha-pi/2 and Alpha + pi/2. Otherwise, collision occurs when result is 0; (3) alpha is the fourth quadrant angle, and only if Gamma is in Alpha-pi/2 to 2 pi or 0 to Alpha-3 x pi/2, no collision occurs, i.e., result is 1. Otherwise, collision occurs when result is 0;
when d is not equal to r _ B × 2, let Beta ═ asin ((r _ a + r _ B)/d), as can be seen from the property values of the arcsine asin function, the range of the asin function is [ -pi/2, pi/2 ], so the method is divided into three cases: (1) Alpha-Beta is less than 0, and when Gamma is between 0 and Alpha + Beta or between Alpha-Beta +2 pi and 2 pi, no collision occurs, i.e., result is 1. Otherwise, collision occurs when result is 0; (2) alpha + Beta is greater than 2 pi, and when Gamma is from 0 to Alpha + Beta-2 pi or from Alpha-Beta to 2 pi, no collision occurs, i.e., result is 1. Otherwise, collision occurs when result is 0; (3) with the exception of (1) (2), when Gamma is between 0 and Alpha + Beta-2 pi or between Alpha-Beta and 2 pi, no collision occurs, i.e., result is 1. Otherwise, collision occurs when result is 0;
23) and identifying whether the unmanned aerial vehicle reaches the target point according to the distance between the current position of the unmanned aerial vehicle and the target point.
The getgoal variable is set equal to 1 if the drone is at a distance from the target point between 0 and r _ a, and 0 otherwise. 1 represents that the drone has arrived at the target point, and 0 represents that the drone has not arrived at the target point.
3) Path planning in the absence of collision;
if no collision of obstacles occurs during the flight, that is, if result is 0, the drone flies to the target point along the direction of V _ Abest, and when the value of getgoal changes from 0 to 1, the algorithm ends.
4) Path planning in the event of a collision;
if it is calculated that collision must occur during flight, the result value is 1. The unmanned aerial vehicle flies along the direction of V _ Abest firstly, and when and only when the Euclidean distance between the unmanned aerial vehicle and the obstacle in the previous second is more than 3 (r _ A + r _ B) and the Euclidean distance between the unmanned aerial vehicle and the obstacle in the next second is less than or equal to 3 (r _ A + r _ B), the count value is 1. And the unmanned aerial vehicle retreats to the position of the last second, and the speed direction of the unmanned aerial vehicle is changed to fly.
5) Selecting the speed when collision occurs;
a speed selection diagram is shown in fig. 1. Speed selection can be divided into two cases:
1. if the coordinate of the unmanned aerial vehicle is above the extension line of the barrier speed or on the extension line of the barrier speed, namely at the abscissa of the unmanned aerial vehicle, the ordinate of the unmanned aerial vehicle is greater than or equal to the ordinate of the equation where the extension line of the barrier speed is located. The drone selects the speed in the direction above the obstacle.
2. If the coordinate of the unmanned aerial vehicle is below the extension line of the barrier speed, namely at the abscissa of the unmanned aerial vehicle, the ordinate of the unmanned aerial vehicle is smaller than the ordinate of the equation where the extension line of the barrier speed is located. The drone selects the speed in the direction below the obstacle.
6) Speed direction in the event of a collision;
1. when the coordinates of the drone are above or on the extension of the speed of the obstacle, the speed direction selection is as shown in figure 2. In fig. 2, Alpha: an angle formed by a connecting line of the coordinates of the obstacle and the coordinates of the unmanned aerial vehicle and the X positive half shaft; beta: beta is asin ((r _ A + r _ B)/d), d is the distance between the obstacle and the unmanned aerial vehicle. M, N: m, N are two rays from the drone that are perpendicular to the radius of the obstacle. In order to guarantee the safety of the unmanned aerial vehicle, the speed direction of the unmanned aerial vehicle in the next second is as follows: unmanned aerial vehicle's turn angle is (Beta + Alpha), and the turn size is:
2. when the unmanned aerial vehicle coordinate is below the extension line of the barrier speed, the unmanned aerial vehicle speed direction of the next second: unmanned aerial vehicle's turn angle is (Alpha-Beta), and the turn size is:
7) updating the speed direction after obstacle avoidance to be flying in a V _ Absest direction;
in order to reduce the turning angle of the unmanned aerial vehicle, after the turning angle avoids conflict, the unmanned aerial vehicle does not return to the original speed direction, and the optimal speed of the unmanned aerial vehicle is recalculated according to the current position of the unmanned aerial vehicle and the position of the target point. Then, the drone flies in the new V Abest direction. The problem that the number of turns and the path length are increased due to the fact that the original flight path is recovered is avoided, the unmanned aerial vehicle flies along the target point after the flight angle is changed to avoid the obstacle, and the schematic diagram is shown in fig. 3.
The task area of the unmanned aerial vehicle is 150 multiplied by 200, the unmanned aerial vehicle starts from a point (50, 120) and makes uniform linear motion at the speed of 30cm/s, and the flight angle points to a target point from a starting point; the dynamic barrier starts from a point (25,60) and invades at an angle of pi/4, and when the dynamic barrier and the dynamic barrier are calculated to collide in the flight process, the method of the invention is used for completing new speed selection and realizing collision avoidance. If no conflict occurs, the vehicle flies to the target point along the direction of V _ Abest all the time. Fig. 4 is a block diagram of a dynamic obstacle avoidance process of the method for improving mutual velocity obstacle provided by the present invention, and fig. 5 is a schematic route diagram in an embodiment of the present invention.
It is noted that the disclosed embodiments are intended to aid in further understanding of the invention, but those skilled in the art will appreciate that: various substitutions and modifications are possible without departing from the spirit and scope of the invention and appended claims. Therefore, the invention should not be limited to the embodiments disclosed, but the scope of the invention is defined by the appended claims.
Claims (3)
1. A dynamic obstacle avoidance method of an unmanned aerial vehicle based on an improved mutual velocity obstacle method is characterized in that the mutual velocity obstacle avoidance method is improved, after collision between the unmanned aerial vehicle and an obstacle is judged, a new flight velocity direction is reselected, and after obstacle avoidance is finished, the optimal flight velocity and flight direction are reselected according to the positions of the current unmanned aerial vehicle and a target point; the method comprises the following steps:
1) acquiring the area of a task area; acquiring initial coordinates, radius and speed of the unmanned aerial vehicle, and recording as Pos _ A (x), Pos _ A (y)) and r _ A, V _ A respectively; acquiring initial coordinates, radius and speed of the dynamic barrier, and respectively recording the initial coordinates, the radius and the speed as Pos _ B (x), Pos _ B (y) and r _ B, V _ B; acquiring the direction of a dynamic barrier;
2) calculating to obtain the initial speed direction of the unmanned aerial vehicle, whether the unmanned aerial vehicle collides with the barrier or not and the distance between the unmanned aerial vehicle and the target point; the method specifically comprises the following steps:
21) calculating the optimal speed direction of the unmanned aerial vehicle by adopting a formula (1) according to the initial coordinate information and the target point coordinate information of the unmanned aerial vehicle;
ang_A＝atan2(Goal_A(y)-Pos_A(y),Goal_A(x)-Pos_A(x))
v _ Abest ═ (V _ Abest (x), V _ Abest (y)) (1)
Wherein: ang _ A is an included angle between a connecting line of the target point coordinate and the unmanned aerial vehicle initial coordinate and an X positive half shaft, and the unit is expressed by radian; v _ Abest (X) ═ V _ a × cos (ang _ a), V _ Abest (Y) ═ V _ a × sin (ang _ a), V _ Abest (X), V _ Abest (Y) are distances that the V _ Abest moves in the obstacle optimal velocity direction on the X axis and the Y axis, respectively; goal _ A (X) and Goal _ A (Y) are X-axis and Y-axis coordinates of the target point respectively; pos _ a (X), Pos _ a (Y) are X-axis and Y-axis coordinates of the drone position, respectively;
22) judging whether the unmanned aerial vehicle collides with the obstacle or not according to the obtained information of the optimal speed direction V _ Abest of the unmanned aerial vehicle, the obstacle speed V _ B, the position Pos _ A of the unmanned aerial vehicle and the position Pos _ B of the obstacle; the method comprises the following steps:
firstly, obtaining the value of an included angle Gamma between a connecting line of an optimal speed direction V _ Abest of the unmanned aerial vehicle and a barrier speed V _ B and an X positive semi-axis by using a formula (2):
Gamma-Gamma (atan2(V _ absest (y) -V _ b (y), V _ absest (x) -V _ b (x))) formula (2)
Then, obtaining the value of an included angle Alpha between a connecting line of the coordinates of the obstacle and the coordinates of the unmanned aerial vehicle and the positive X half axis by using the formula (3):
alpha-atan 2(Pos _ b (y) -Pos _ a (y), Pos _ b (x) -Pos _ a (x)) (3)
Wherein, both Gamma and Alpha units are expressed in radians;
the formula of the distance d between the obstacle and the unmanned aerial vehicle is calculated by the formula 4:
judging whether the unmanned aerial vehicle collides with the obstacle or not according to the distance d between the obstacle and the unmanned aerial vehicle and the value of r _ B x 2;
23) identifying whether the unmanned aerial vehicle reaches a target point according to the distance between the current position of the unmanned aerial vehicle and the target point:
if the distance between the current position of the unmanned aerial vehicle and the target point is between 0 and r _ A, setting a getgoal variable for indicating whether the unmanned aerial vehicle reaches the target point; setting getgoal equal to 1 if the unmanned aerial vehicle reaches the target point, and otherwise setting to 0;
3) planning the unmanned aerial vehicle path without collision;
if no collision of an obstacle occurs during the flight, the unmanned aerial vehicle flies to the target point in the direction of V _ Abest, and the unmanned aerial vehicle finishes when the value of getgoal is changed from 0 to 1;
4) planning the unmanned aerial vehicle path when collision occurs:
if it is calculated that collision occurs in the flying process, the unmanned aerial vehicle flies along the direction of V _ Abest firstly, and when the distance between the unmanned aerial vehicle and the obstacle in the last second is larger than a set value and the distance between the unmanned aerial vehicle and the obstacle in the next second is smaller than or equal to the set value, the unmanned aerial vehicle retreats to the position in the last second and changes the speed direction of the unmanned aerial vehicle to fly.
5) Selecting the speed of the unmanned aerial vehicle when collision occurs; the method comprises the following steps:
the method comprises the following steps that 1, if the coordinate of the unmanned aerial vehicle is above the extension line of the speed of the obstacle or on the extension line of the speed of the obstacle, namely at the abscissa of the unmanned aerial vehicle, the ordinate of the unmanned aerial vehicle is greater than or equal to the ordinate of the equation where the extension line of the speed of the obstacle is located, the speed of the unmanned aerial vehicle in the direction above the obstacle is selected;
case 2. if the coordinate of the unmanned aerial vehicle is below the extension line of the speed of the obstacle, namely at the abscissa of the unmanned aerial vehicle, and the ordinate of the unmanned aerial vehicle is smaller than the ordinate of the equation where the extension line of the speed of the obstacle is located, selecting the speed of the obstacle in the lower direction;
6) the speed direction of the unmanned aerial vehicle when collision occurs; the method comprises the following steps:
case 1. when the coordinates of the drone are above or on the extension of the speed of the obstacle, the speed of the drone for the next second is: the unmanned aerial vehicle has the turning angle of (Beta + Alpha) and the turning size of
2. When the unmanned aerial vehicle coordinate is in the below of barrier speed extension line, the unmanned aerial vehicle next second's speed is: unmanned aerial vehicle's turn angle is (Alpha-Beta), and the turn size is:
7) updating the flight speed direction of the unmanned aerial vehicle after obstacle avoidance to be a V _ Absest direction;
recalculating the optimal speed of the unmanned aerial vehicle according to the current position of the unmanned aerial vehicle and the position of the target point;
the unmanned aerial vehicle flies according to the updated V _ Abest direction, so that the unmanned aerial vehicle flies along a target point after the flying angle is changed to avoid an obstacle;
through the steps, the unmanned aerial vehicle dynamic obstacle avoidance based on the improved mutual velocity obstacle method in the flight process of the unmanned aerial vehicle is realized.
2. The unmanned aerial vehicle dynamic obstacle avoidance method based on the improved mutual velocity obstacle method as claimed in claim 1, wherein, step 22) judges whether the unmanned aerial vehicle collides with the obstacle according to the distance d between the obstacle and the unmanned aerial vehicle and the value of r _ B x 2; the method specifically comprises the following steps:
case 1. when d is equal to r _ B × 2, three cases are included:
A1) when the Alpha is a first quadrant angle and only the Gamma is between 0 and Alpha + pi/2 or between Alpha +3 x pi/2 and 2 pi, the unmanned aerial vehicle cannot collide in the flying process; otherwise, collision will occur;
A2) when the Alpha is a second quadrant angle and a third quadrant angle and only the Gamma is between the Alpha-pi/2 and the Alpha + pi/2, the unmanned aerial vehicle cannot collide in the flying process; otherwise, collision will occur;
A3) when the Alpha is a fourth quadrant angle and only the Gamma is between Alpha-pi/2 and 2 pi or between 0 and Alpha-3 x pi/2, the unmanned aerial vehicle cannot collide in the flying process; otherwise, collision will occur;
case 2. when d is not equal to r _ B × 2, let Beta ═ asin ((r _ a + r _ B)/d), and the range of values of the asin function is [ -pi/2, pi/2 ], including three cases:
B1) the Alpha-Beta is less than 0, and when the Gamma is between 0 and Alpha + Beta or between Alpha-Beta +2 pi and 2 pi, the unmanned aerial vehicle cannot collide in the flying process; otherwise, collision will occur;
B2) the Alpha + Beta is larger than 2 pi, and when the Gamma is between 0 and Alpha + Beta-2 pi or between Alpha-Beta and 2 pi, the unmanned aerial vehicle cannot collide in the flight process; otherwise, collision will occur;
B3) except for B1), B2), when Gamma is between 0 and Alpha + Beta-2 pi or between Alpha-Beta and 2 pi, the unmanned aerial vehicle can not collide in the flight process; otherwise, a collision may occur.
3. The unmanned aerial vehicle dynamic obstacle avoidance method based on the improved mutual velocity obstacle method as claimed in claim 1, wherein, when the path planning in the collision in step 4) is performed, the set value is 3 x (r _ a + r _ B).
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110078490.7A CN112925342B (en) | 2021-01-20 | 2021-01-20 | Unmanned aerial vehicle dynamic obstacle avoidance method based on improved mutual velocity obstacle method |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110078490.7A CN112925342B (en) | 2021-01-20 | 2021-01-20 | Unmanned aerial vehicle dynamic obstacle avoidance method based on improved mutual velocity obstacle method |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN112925342A true CN112925342A (en) | 2021-06-08 |
| CN112925342B CN112925342B (en) | 2022-07-01 |
Family
ID=76165313
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110078490.7A Active CN112925342B (en) | 2021-01-20 | 2021-01-20 | Unmanned aerial vehicle dynamic obstacle avoidance method based on improved mutual velocity obstacle method |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112925342B (en) |
Cited By (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113625762A (en) * | 2021-08-30 | 2021-11-09 | 吉林大学 | Unmanned aerial vehicle obstacle avoidance method and system, and unmanned aerial vehicle cluster obstacle avoidance method and system |
| CN113741476A (en) * | 2021-09-14 | 2021-12-03 | 深圳市优必选科技股份有限公司 | Robot smooth motion control method and device and robot |
| CN113759985A (en) * | 2021-08-03 | 2021-12-07 | 华南理工大学 | Unmanned aerial vehicle flight control method, system, device and storage medium |
| CN115033027A (en) * | 2022-08-15 | 2022-09-09 | 中国民航大学 | Dynamic obstacle avoidance prediction management method for fixed-wing unmanned aerial vehicle |
Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103365299A (en) * | 2013-08-02 | 2013-10-23 | 中国科学院自动化研究所 | Method and device for avoiding obstacle of unmanned aerial vehicle |
| US20150284010A1 (en) * | 2013-09-16 | 2015-10-08 | Disney Enterprises, Inc. | Shared control of semi-autonomous vehicles including collision avoidance in multi-agent scenarios |
| CN106292712A (en) * | 2016-10-25 | 2017-01-04 | 中国人民解放军海军航空工程学院 | A kind of unmanned vehicle barrier-avoiding method based on Speed Obstacles arc method |
| WO2018058442A1 (en) * | 2016-09-29 | 2018-04-05 | 深圳市大疆创新科技有限公司 | Method and device for panning route, flight control system, omnidirectional obstacle avoidance system, and unmanned aerial vehicle |
| CN108958289A (en) * | 2018-07-28 | 2018-12-07 | 天津大学 | Cluster unmanned plane collision prevention method based on relative velocity obstacle |
| CN110427044A (en) * | 2019-07-17 | 2019-11-08 | 河北科技大学 | Based on the unmanned plane conflict probe and conflict Resolution method for improving Speed Obstacles method |
| CN110825108A (en) * | 2019-11-11 | 2020-02-21 | 浙江理工大学 | Cooperative anti-collision method for multiple tracking unmanned aerial vehicles in same airspace |
| KR20200081322A (en) * | 2018-12-27 | 2020-07-07 | (주)자이언트드론 | The device for managing obstacle collision of drone |
| CN111781948A (en) * | 2020-06-18 | 2020-10-16 | 南京非空航空科技有限公司 | Unmanned aerial vehicle formation shape transformation and online dynamic obstacle avoidance method |
-
2021
- 2021-01-20 CN CN202110078490.7A patent/CN112925342B/en active Active
Patent Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103365299A (en) * | 2013-08-02 | 2013-10-23 | 中国科学院自动化研究所 | Method and device for avoiding obstacle of unmanned aerial vehicle |
| US20150284010A1 (en) * | 2013-09-16 | 2015-10-08 | Disney Enterprises, Inc. | Shared control of semi-autonomous vehicles including collision avoidance in multi-agent scenarios |
| WO2018058442A1 (en) * | 2016-09-29 | 2018-04-05 | 深圳市大疆创新科技有限公司 | Method and device for panning route, flight control system, omnidirectional obstacle avoidance system, and unmanned aerial vehicle |
| CN106292712A (en) * | 2016-10-25 | 2017-01-04 | 中国人民解放军海军航空工程学院 | A kind of unmanned vehicle barrier-avoiding method based on Speed Obstacles arc method |
| CN108958289A (en) * | 2018-07-28 | 2018-12-07 | 天津大学 | Cluster unmanned plane collision prevention method based on relative velocity obstacle |
| KR20200081322A (en) * | 2018-12-27 | 2020-07-07 | (주)자이언트드론 | The device for managing obstacle collision of drone |
| CN110427044A (en) * | 2019-07-17 | 2019-11-08 | 河北科技大学 | Based on the unmanned plane conflict probe and conflict Resolution method for improving Speed Obstacles method |
| CN110825108A (en) * | 2019-11-11 | 2020-02-21 | 浙江理工大学 | Cooperative anti-collision method for multiple tracking unmanned aerial vehicles in same airspace |
| CN111781948A (en) * | 2020-06-18 | 2020-10-16 | 南京非空航空科技有限公司 | Unmanned aerial vehicle formation shape transformation and online dynamic obstacle avoidance method |
Non-Patent Citations (8)
| Title |
|---|
| XUEPING REN ET AL.: "Multi-target UAV path planning based on improved RRT algorithm", 《11TH ASIA CONFERENCE ON MECHANICAL AND AEROSPACE ENGINEERING ACMAE (2020)》 * |
| XUEPING REN ET AL.: "Multi-target UAV path planning based on improved RRT algorithm", 《11TH ASIA CONFERENCE ON MECHANICAL AND AEROSPACE ENGINEERING ACMAE (2020)》, 27 December 2020 (2020-12-27), pages 1 - 5 * |
| 徐鹏 等: "改进互动速度障碍的多机器人协同避障", 《自动化与仪表》 * |
| 徐鹏 等: "改进互动速度障碍的多机器人协同避障", 《自动化与仪表》, no. 09, 30 September 2020 (2020-09-30), pages 35 - 39 * |
| 杨秀霞 等: "基于比例导引角度线性化的无人机避障研究", 《导航定位与授时》 * |
| 杨秀霞 等: "基于比例导引角度线性化的无人机避障研究", 《导航定位与授时》, no. 03, 31 December 2014 (2014-12-31), pages 1 - 5 * |
| 谭励 等: "三维空间动态目标追踪自部署算法", 《传感器与微系统》 * |
| 谭励 等: "三维空间动态目标追踪自部署算法", 《传感器与微系统》, 29 February 2020 (2020-02-29), pages 101 - 108 * |
Cited By (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113759985A (en) * | 2021-08-03 | 2021-12-07 | 华南理工大学 | Unmanned aerial vehicle flight control method, system, device and storage medium |
| CN113625762A (en) * | 2021-08-30 | 2021-11-09 | 吉林大学 | Unmanned aerial vehicle obstacle avoidance method and system, and unmanned aerial vehicle cluster obstacle avoidance method and system |
| CN113625762B (en) * | 2021-08-30 | 2023-07-25 | 吉林大学 | Unmanned aerial vehicle obstacle avoidance method and system, and unmanned aerial vehicle cluster obstacle avoidance method and system |
| CN113741476A (en) * | 2021-09-14 | 2021-12-03 | 深圳市优必选科技股份有限公司 | Robot smooth motion control method and device and robot |
| CN115033027A (en) * | 2022-08-15 | 2022-09-09 | 中国民航大学 | Dynamic obstacle avoidance prediction management method for fixed-wing unmanned aerial vehicle |
Also Published As
| Publication number | Publication date |
|---|---|
| CN112925342B (en) | 2022-07-01 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN112925342B (en) | Unmanned aerial vehicle dynamic obstacle avoidance method based on improved mutual velocity obstacle method | |
| CN112378408B (en) | Path planning method for realizing real-time obstacle avoidance of wheeled mobile robot | |
| CN109871031B (en) | Trajectory planning method for fixed-wing unmanned aerial vehicle | |
| Zhen et al. | Rotary unmanned aerial vehicles path planning in rough terrain based on multi-objective particle swarm optimization | |
| CN107643764B (en) | Obstacle avoidance method of unmanned aerial vehicle based on double-rotation Lyapunov vector field | |
| CN111552296B (en) | Local smooth track planning method based on curved cylindrical coordinate system | |
| CN112162567B (en) | Avoidance guidance method suitable for online no-fly zone of aircraft | |
| CN111024085A (en) | Unmanned aerial vehicle track planning method with end point direction and time constraints | |
| CN114721427A (en) | Multi-unmanned aerial vehicle collaborative search and rescue reconnaissance planning method in dynamic environment | |
| CN110262473B (en) | Unmanned ship automatic collision avoidance method based on improved Bi-RRT algorithm | |
| CN110793522B (en) | Flight path planning method based on ant colony algorithm | |
| CN114428499A (en) | Astar and DWA algorithm fused mobile trolley path planning method | |
| CN114138022A (en) | Distributed formation control method for unmanned aerial vehicle cluster based on elite pigeon swarm intelligence | |
| Jilkov et al. | An efficient algorithm for aircraft conflict detection and resolution using list viterbi algorithm | |
| CN110986948B (en) | Multi-unmanned aerial vehicle grouping collaborative judgment method based on reward function optimization | |
| CN111121804A (en) | Intelligent vehicle path planning method and system with safety constraint | |
| Gao et al. | UAV dynamic obstacle avoidance based on improved reciprocal velocity obstacle | |
| CN115808933A (en) | Unmanned aerial vehicle path planning method based on quantum seagull algorithm | |
| CN115686068A (en) | Collaborative path planning method in multi-threat environment | |
| CN112161626B (en) | High-flyability route planning method based on route tracking mapping network | |
| CN114815899A (en) | Unmanned aerial vehicle three-dimensional space path planning method based on 3D laser radar sensor | |
| Wang et al. | Entry guidance command generation for hypersonic glide vehicles under threats and multiple constraints | |
| CN113342045B (en) | Unmanned aerial vehicle autonomous avoidance navigation control method for any no-fly zone | |
| CN114911263B (en) | Synchronous arrival track planning method, storage medium and equipment for multiple unmanned aerial vehicles based on homotopy | |
| Yang et al. | Fast on-ship route planning using improved sparse A-star algorithm for UAVs |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| CB03 | Change of inventor or designer information |
Inventor after: Gao Jing Inventor after: Ren Xueping Inventor after: Tan Li Inventor after: Lian Xiaofeng Inventor after: Xu Tianying Inventor before: Ren Xueping Inventor before: Gao Jing Inventor before: Tan Li Inventor before: Lian Xiaofeng Inventor before: Xu Tianying |
|
| CB03 | Change of inventor or designer information | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |