CN113433969B - Unmanned aerial vehicle cluster autonomous obstacle avoidance method and device based on combined vector field method - Google Patents
Unmanned aerial vehicle cluster autonomous obstacle avoidance method and device based on combined vector field method Download PDFInfo
- Publication number
- CN113433969B CN113433969B CN202110705077.9A CN202110705077A CN113433969B CN 113433969 B CN113433969 B CN 113433969B CN 202110705077 A CN202110705077 A CN 202110705077A CN 113433969 B CN113433969 B CN 113433969B
- Authority
- CN
- China
- Prior art keywords
- unmanned aerial
- obstacle
- aerial vehicle
- vector
- tangent
- 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.)
- Active
Links
- 239000013598 vector Substances 0.000 title claims abstract description 121
- 238000000034 method Methods 0.000 title claims abstract description 43
- 230000004888 barrier function Effects 0.000 claims abstract description 28
- 238000004364 calculation method Methods 0.000 claims description 8
- 230000009194 climbing Effects 0.000 claims description 8
- 238000010586 diagram Methods 0.000 description 8
- 230000008569 process Effects 0.000 description 7
- 230000008901 benefit Effects 0.000 description 3
- 230000003068 static effect Effects 0.000 description 3
- 239000000463 material Substances 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000005540 biological transmission Effects 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 238000004088 simulation Methods 0.000 description 1
- 239000007787 solid Substances 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/107—Simultaneous control of position or course in three dimensions specially adapted for missiles
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 cluster autonomous obstacle avoidance method and device based on a combined vector field method. An unmanned aerial vehicle cluster autonomous obstacle avoidance method based on a combined vector field method comprises the following steps: acquiring a connecting line between the current position of the unmanned aerial vehicle and the center of the barrier and a nearest intersection point with the surface of the barrier; determining a tangent plane of the surface of the barrier at the intersection point, and calculating a normal vector of the tangent plane and a tangent vector passing through the intersection point in the tangent plane; and designing a current guidance law for the unmanned aerial vehicle based on the normal vector and the tangent vector so that the unmanned aerial vehicle avoids the obstacle on the premise of flying towards the target. The invention provides the autonomous obstacle avoidance guidance law in a unified form and an analytic form aiming at various obstacles which may be encountered by an unmanned aerial vehicle cluster in a three-dimensional space, and can conveniently avoid the obstacles in various shapes.
Description
Technical Field
The invention relates to the technical field of information, in particular to an unmanned aerial vehicle cluster autonomous obstacle avoidance method and device based on a combined vector field method.
Background
In a real cluster mission, the threats faced by the drone include terrain (mountains, etc.), radar, firepower (antiaircraft gun, guided missiles, etc.), and the like. Terrain may be considered a static obstacle, while radar and fire are static or dynamic obstacles. In addition, for each member of the cluster, other members in the vicinity thereof pose a threat to their safe flight, and therefore, these members may also be considered dynamic obstacles.
In the related art, the obstacle avoidance technology of the unmanned aerial vehicle cluster can only process obstacles in a specific shape (such as a sphere, a cube and the like) in a three-dimensional space, and can not process three-dimensional obstacles in various shapes in a unified manner.
Disclosure of Invention
The embodiment of the invention provides an unmanned aerial vehicle cluster autonomous obstacle avoidance method and device based on a combined vector field method, which are used for solving the problem that an obstacle avoidance technology of an unmanned aerial vehicle cluster in the prior art can only process obstacles with specific shapes (such as spheres, cubes and the like) in a three-dimensional space and can not process three-dimensional obstacles with various shapes in a unified manner.
The unmanned aerial vehicle cluster autonomous obstacle avoidance method based on the combined vector field method comprises the following steps:
acquiring a connecting line between the current position of the unmanned aerial vehicle and the center of the barrier and a nearest intersection point with the surface of the barrier;
determining a tangent plane of the surface of the obstacle at the intersection point, and calculating a normal vector of the tangent plane and a tangent vector passing through the intersection point in the tangent plane;
and designing a current guidance law for the unmanned aerial vehicle based on the normal vector and the tangent vector so that the unmanned aerial vehicle avoids the barrier on the premise of flying towards a target.
According to some embodiments of the invention, the shape of the obstacle comprises a sphere, a cylinder, a cone, a parallelepiped, and a truncated cone.
According to some embodiments of the present invention, the obtaining a connection line between the current position of the drone and the center of the obstacle and a closest intersection point to the surface of the obstacle includes:
the obstacle is represented by equation 1:
wherein, (x, y, z) T Representing any position in space, q 0 ＝(x 0 ,y 0 ,z 0 ) T Represents the center of the obstacle, a i 、p i (i =1,2,3) are all real numbers, and when p is 1 ＝p 2 ＝p 3 ＝1、a 1 ＝a 2 ＝a 3 When the obstacle is a sphere; when p is 1 ＝p 2 ＝1、p 3 ＞1、a 1 ＝a 2 When the obstacle is a cylinder; when p is 1 ＝p 2 ＝1、0＜p 3 ＜1、a 1 ＝a 2 When the barrier is a cone; when p is 1 ,p 2 ,p 3 When the height is more than 1, the barrier is a parallelepiped; when p is 1 ＝p 2 ＝1、p 3 ＞1、
R 1 ＞R 2 When the height is more than 0, the barrier is a circular truncated cone.
According to some embodiments of the invention, said calculating a normal vector to said tangent plane and a tangent vector in said tangent plane passing through said intersection point comprises:
calculating the normal vector of the tangent plane according to formula 2:
wherein, F x (x i ,y i ,z i )、F y (x i ,y i ,z i )、F z (x i ,y i ,z i ) Respectively representing the partial derivatives of the function F (x, y, z) with respect to x, y, z at said intersection point q i ＝(x i ,y i ,z i ) T Taking the value of (1);
calculating a tangent vector passing through the intersection point in the tangent plane according to formula 3:
where x represents the outer product of the vectors, q 0 ＝(x 0 ,y 0 ,z 0 ) T Denotes the center of the obstacle, p u ＝(xu,y u ,z u ) T Representing the current location of the drone, (x) t ,y t ,z t ) T Indicating the target location.
According to some embodiments of the invention, designing a current guidance law for the drone based on the normal vector and the tangent vector comprises:
the combined vector field is determined according to equation 4:
where κ > 0 represents an adjustable parameter, F = F (x, y, z), s = ± 1 represents a parameter controlling the direction of rotation of the vector field, V nom A designable speed of the current position of the unmanned aerial vehicle is represented by more than 0;
calculating the desired course angle χ of the unmanned aerial vehicle according to formula 5 and formula 6 based on the combined vector field d And desired climbing angle gamma d ：
χ d ＝atan2(N dy ,N dx ) In the case of the formula 5,
wherein atan2 represents the quadrant tangent function.
The unmanned aerial vehicle cluster autonomous obstacle avoidance device based on the combined vector field method comprises the following steps:
the intersection point acquisition unit is used for acquiring a connecting line between the current position of the unmanned aerial vehicle and the center of the obstacle and an intersection point closest to the surface of the obstacle;
a vector calculation unit for determining a tangent plane of the obstacle surface at the intersection point, and calculating a normal vector of the tangent plane and a tangent vector passing through the intersection point in the tangent plane;
and the guidance law determining unit is used for designing a current guidance law for the unmanned aerial vehicle based on the normal vector and the tangent vector so that the unmanned aerial vehicle avoids the obstacle on the premise of flying towards a target.
According to some embodiments of the invention, the intersection acquisition unit is configured to:
the obstacle is represented by equation 1:
wherein, (x, y, z) T Representing any position in space, q 0 ＝(x 0 ,y 0 ,z 0 ) T Represents the center of the obstacle, a i 、p i (i =1,2,3) are all real numbers, and when p is 1 ＝p 2 ＝p 3 ＝1、a 1 ＝a 2 ＝a 3 When the obstacle is a sphere; when p is 1 ＝p 2 ＝1、p 3 ＞1、a 1 ＝a 2 When the obstacle is a cylinder; when p is 1 ＝p 2 ＝1、0＜p 3 ＜1、a 1 ＝a 2 When the barrier is a cone; when p is 1 ,p 2 ,p 3 When the height is more than 1, the barrier is a parallelepiped; when p is 1 ＝p 2 ＝1、p 3 ＞1、
R 1 ＞R 2 When the height is more than 0, the barrier is a circular truncated cone.
According to some embodiments of the invention, the vector calculation unit is to:
calculating the normal vector of the tangent plane according to formula 2:
wherein, F x (x i ,y i ,z i )、F y (x i ,y i ,z i )、F z (x i ,y i ,z i ) Respectively, the partial derivatives of the function F (x, y, z) with respect to x, y, z at said intersection point q i ＝(x i ,y i ,z i ) T Taking the value of (A);
calculating a tangent vector in the tangent plane passing through the intersection point according to formula 3:
where x represents the outer product of the vectors, q 0 ＝(x 0 ,y 0 ,z 0 ) T Represents the center of said obstacle, p u ＝(x u ,y u ,z u ) T Representing the current location of the drone, (x) t ,y t ,z t ) T Indicating the target location.
According to some embodiments of the invention, the guidance law determination unit is configured to:
the combined vector field is determined according to equation 4:
where κ > 0 represents an adjustable parameter, F = F (x, y, z), s = ± 1 represents a parameter controlling the vector field rotation direction, V nom > 0 represents a designable speed of the current position of the drone;
calculating the desired course angle χ of the unmanned aerial vehicle according to formula 5 and formula 6 based on the combined vector field d And desired climbing angle gamma d ：
χ d ＝atan2(N dy ,N dx ) In the case of the formula 5,
wherein atan2 represents the quadrant tangent function.
According to the computer readable storage medium of the embodiment of the present invention, the computer readable storage medium stores the implementation program of information transfer, and the program realizes the steps of the method as described above when being executed by a processor.
The embodiment of the invention provides the autonomous obstacle avoidance guidance law in a unified form and an analytic form aiming at various obstacles which may be encountered by an unmanned aerial vehicle cluster in a three-dimensional space, and can conveniently avoid the obstacles in various shapes.
The foregoing description is only an overview of the technical solutions of the present invention, and the embodiments of the present invention are described below in order to make the technical means of the present invention more clearly understood and to make the above and other objects, features, and advantages of the present invention more clearly understandable.
Drawings
Various other advantages and benefits will become apparent to those of ordinary skill in the art upon reading the following detailed description of the embodiments. The drawings are only for purposes of illustrating the preferred embodiments and are not to be construed as limiting the invention. In the drawings:
fig. 1 is a flowchart of an autonomous obstacle avoidance method for an unmanned aerial vehicle cluster based on a combined vector field method in the embodiment of the present invention;
FIG. 2 is a schematic diagram of a combined vector field of a sphere in an embodiment of the present invention;
FIG. 3 is a schematic diagram of autonomous obstacle avoidance for a sphere in an embodiment of the present invention;
FIG. 4 is a schematic view of a combined vector field of a cone in an embodiment of the present invention;
FIG. 5 is a schematic diagram of autonomous obstacle avoidance for a cone in an embodiment of the present invention;
FIG. 6 is a diagram illustrating a combined vector field of a cylinder according to an embodiment of the present invention;
FIG. 7 is a schematic diagram of autonomous obstacle avoidance for a cylinder according to an embodiment of the present invention;
FIG. 8 is a schematic view of a parallelepiped combined vector field in an embodiment of the present invention;
FIG. 9 is a schematic diagram of an autonomous obstacle avoidance system of a parallelepiped in an embodiment of the present invention;
FIG. 10 is a schematic diagram of a combined vector field of a circular truncated cone according to an embodiment of the present invention;
fig. 11 is a schematic diagram of autonomous obstacle avoidance in the circular truncated cone in the embodiment of the present invention.
Detailed Description
Exemplary embodiments of the present invention will be described in more detail below with reference to the accompanying drawings. While exemplary embodiments of the invention are shown in the drawings, it should be understood that the invention can be embodied in various forms and should not be limited to the embodiments set forth herein. Rather, these embodiments are provided so that this disclosure will be thorough and complete, and will fully convey the scope of the invention to those skilled in the art.
Both static and dynamic obstacles can be represented as standard convex entities of different shapes: the independent peak is equivalent to a cone or a round table; the mountain may be represented as a parallelepiped; the threat range of high shots can be regarded as a cylinder; the range of influence of guided missiles and radars may be represented by a hemisphere; threats from other members of the cluster may be represented by spheres.
The embodiment of the invention aims to design a unified guidance law for threats with different shapes, so that cluster unmanned aerial vehicles can avoid collision with the threats (namely, the cluster unmanned aerial vehicles always fly outside a threat body) while flying to a target point, and thus when the cluster unmanned aerial vehicles are far away from the threat body (or called an obstacle) or bypass the threat body, the unmanned aerial vehicles can fly to the target point. When the unmanned aerial vehicle is close to the threat body and does not bypass the threat body, the unmanned aerial vehicle can fly towards the direction far away from the threat body.
The embodiment of the invention is based on the following assumptions: setting the initial position of each unmanned aerial vehicle in the cluster to be far enough away from the threat body; it is assumed that the shape data of each threat body in the flight environment is known in advance.
An embodiment of the first aspect of the present invention provides an unmanned aerial vehicle cluster autonomous obstacle avoidance method based on a combined vector field method, as shown in fig. 1, including:
s1, acquiring a connecting line between the current position of the unmanned aerial vehicle and the center of an obstacle, and acquiring an intersection point nearest to the surface of the obstacle;
s2, determining a tangent plane of the surface of the obstacle at the intersection point, and calculating a normal vector of the tangent plane and a tangent vector passing through the intersection point in the tangent plane; the unmanned aerial vehicle mentioned in the embodiment of the invention can be a fixed wing unmanned aerial vehicle.
And S3, designing a current guidance law for the unmanned aerial vehicle based on the normal vector and the tangent vector so that the unmanned aerial vehicle avoids the obstacle on the premise of flying towards a target.
The embodiment of the invention provides the autonomous obstacle avoidance guidance law in a unified form and an analytic form aiming at various obstacles which an unmanned aerial vehicle cluster possibly encounters in a three-dimensional space, and can conveniently avoid the obstacles in various shapes.
On the basis of the above-described embodiment, various modified embodiments are further proposed, and it is to be noted herein that, in order to make the description brief, only the differences from the above-described embodiment are described in the various modified embodiments.
According to some embodiments of the invention, the shape of the obstacle comprises a sphere, a cylinder, a cone, a parallelepiped, and a truncated cone.
According to some embodiments of the present invention, the obtaining a connection line between the current position of the drone and the center of the obstacle and a closest intersection point to the surface of the obstacle includes:
the obstacle is represented by equation 1:
wherein, (x, y, z) T Representing any position in space, q 0 ＝(x 0 ,y 0 ,z 0 ) T Represents the center of the obstacle, a i 、p i (i =1,2,3) are all real numbers, and when p is 1 ＝p 2 ＝p 3 ＝1、a 1 ＝a 2 ＝a 3 When the obstacle is a sphere; when p is 1 ＝p 2 ＝1、p 3 ＞1、a 1 ＝a 2 When the obstacle is a cylinder; when p is 1 ＝p 2 ＝1、0＜p 3 ＜1、a 1 ＝a 2 When the obstacle is a cone; when p is 1 ,p 2 ,p 3 When the height is more than 1, the barrier is a parallelepiped; when p is 1 ＝p 2 ＝1、p 3 ＞1、
R 1 ＞R 2 When the height is more than 0, the barrier is a circular truncated cone.
According to some embodiments of the invention, said calculating a normal vector to said tangent plane and a tangent vector in said tangent plane passing through said intersection point comprises:
calculating the normal vector of the tangent plane according to formula 2:
wherein, F x (x i ,y i ,z i )、F y (x i ,y i ,z i )、F z (x i ,y i ,z i ) Respectively representing the partial derivatives of the function F (x, y, z) with respect to x, y, z at said intersection point q i ＝(x i ,y i ,z i ) T Taking the value of (A);
calculating a tangent vector in the tangent plane passing through the intersection point according to formula 3:
where x represents the outer product of the vectors, q 0 ＝(x 0 ,y 0 ,z 0 ) T Represents the center of said obstacle, p u ＝(x u ,y u ,z u ) T Representing the current location of the drone, (x) t ,y t ,z t ) T Indicating the target location.
According to some embodiments of the invention, designing a current guidance law for the drone based on the normal vector and the tangent vector comprises:
the combined vector field is determined according to equation 4:
where κ > 0 represents an adjustable parameter, F = F (x, y, z), s = ± 1 represents a parameter controlling the vector field rotation direction, V nom > 0 represents a designable speed of the current position of the drone;
calculating the desired course angle χ of the unmanned aerial vehicle according to formula 5 and formula 6 based on the combined vector field d And desired climbing angle gamma d ：
χ d ＝atan2(N dy ,N dx ) In the case of the formula 5,
wherein atan2 represents the quadrant tangent function.
The autonomous obstacle avoidance method for the unmanned aerial vehicle cluster based on the combined vector field method according to the embodiment of the invention is described in detail in a specific embodiment. It is to be understood that the following description is illustrative only and is not intended to be in any way limiting. All similar structures and similar variations thereof adopted by the invention are intended to fall within the scope of the invention.
In the embodiment of the present invention, the obstacles can be collectively expressed by the following shape function:
wherein, (x, y, z) T Is any position in space; q. q.s 0 ＝(x 0 ,y 0 ,z 0 ) T Is the "center" of a solid (e.g., spherical center, conical base center, etc.); a is i 、p i (i =1,2,3) are real numbers, and when they take different values, the above equation represents obstacles of different shapes, as follows:
a) When p is 1 ＝p 2 ＝p 3 ＝1、a 1 ＝a 2 ＝a 3 When, it represents a sphere;
b) When p is 1 ＝p 2 ＝1、p 3 ＞1、a 1 ＝a 2 When, representing a cylinder;
c) When p is 1 ＝p 2 ＝1、0＜p 3 ＜1、a 1 ＝a 2 When, a cone is represented;
d) When p is 1 ,p 2 ,p 3 When the value is more than 1, the value is a parallelepiped;
e) When p is 1 ＝p 2 ＝1、p 3 ＞1、
R 1 ＞R 2 When > 0, the circular truncated cone is indicated.
It will be readily seen that when F (x, y, z) < 0, points (x, y, z) are defined as above T Inside the obstacle; when F (x, y, z) =0, dot (x, y, z) T On the surface of the obstacle; when F (x, y, z) > 0, dot (x, y, z) T Outside the obstacle.
Let the position coordinate of the unmanned aerial vehicle be p u ＝(x u ,y u ,z u ) T With target point coordinates p t ＝(x t ,y t ,z t ) T . The autonomous obstacle avoidance problem of the cluster unmanned aerial vehicle in the three-dimensional space is to design a guidance law to enable F (x) u ,y u ,z u ) Always > 0, and drone towards (x) t ,y t ,z t ) T And (5) moving.
The unmanned aerial vehicle cluster autonomous obstacle avoidance method based on the combined vector field method comprises the following steps:
calculating a projection point;
the projected point is defined as a point on the surface of the obstacle corresponding to the current position of the drone. Defining the projection point as the current position p of the unmanned aerial vehicle for the obstacles such as a sphere, a cone, a parallelepiped, a circular truncated cone and the like u ＝(x u ,y u ,z u ) T With center q of the obstacle model 0 ＝(x 0 ,y 0 ,z 0 ) T A closest intersection point of the connecting line of (a) and the surface of the obstacle; defining the projection points as unmanned for an obstacle modeled as a cylinderCurrent position p of the machine u ＝(x u ,y u ,z u ) T From the center (x) of the cylinder at the current height 0 ,y 0 ,z u ) T A closest intersection point of the connecting line of (a) with the surface of the cylinder. The projection point can be obtained by solving the simultaneous equations of the three-dimensional straight line and the surface of the obstacle, and is recorded as q i ＝(x i ,y i ,z i ) T 。
Step two, calculating tangent vectors and normal vectors at the projection points;
defining the normal vector at the projection point as the projection point q of the barrier surface i Tangential plane P of i Is calculated by the following formula:
wherein, F x 、F y 、F z Representing the partial derivatives of the function with respect to x, y, z, respectively, F x (x i ,y i ,z i )、F y (x i ,y i ,z i )、F z (x i ,y i ,z i ) Then the partial derivatives are respectively indicated at the point q i ＝(x i ,y i ,z i ) T The value of (c) is as follows.
Defining the tangent vector at the projection point as tangent plane P i And a passing point q 0 、q u 、q t Plane P of 1 The direction vector of the intersection line of (a) is calculated by the following formula:
where x represents the outer product of the vectors.
Step three, calculating a combined vector field;
and combining the normal vector and the tangent vector at the projection point to obtain a combined vector field as follows:
where κ > 0 is an adjustable parameter, F = F (x, y, z) is (1) mean function at spatial point (x, y, z) T Where, s = ± 1 is a parameter controlling the rotation direction (clockwise, counterclockwise, etc.) of the vector field, V nom > 0 is a programmable speed for the current position. Combined vector field N d The magnitude and direction of (a) gives the magnitude and direction of the desired speed of the drone at the current location.
Calculating a guidance law;
from the designed combined vector field, a guidance law (including a desired heading angle χ) is calculated d And desired climbing angle gamma d )
χ d ＝atan2(N dy ,N dx ) (5)
Wherein atan2 is the quadrant tangent function.
Examples of embodiment:
in the calculation example, the four steps are respectively carried out on 5 obstacles such as a sphere, a cone, a cylinder, a parallelepiped, a circular truncated cone and the like, and a combined vector field and an autonomous obstacle avoidance track (a track determined by a desired heading angle and a desired climbing angle) corresponding to the 5 obstacles can be obtained. The simulation results are shown in fig. 2-11.
In the embodiment of the invention, the normal vector formula determined in the step (2) and the tangent vector formula determined in the step (3) are key quantities which form a combined vector field and are uniform in the shape of obstacles with different shapes. The combined vector field formula determined in the step (4) comprises a plurality of adjustable parameters, can be used for adjusting the direction, the urgency degree and the like of the combined vector field, and can be used for conveniently determining the guidance law of autonomous obstacle avoidance.
The embodiment of the invention provides an autonomous obstacle avoidance guidance law in a unified form and an analytic form aiming at threat bodies (obstacles) in various shapes possibly encountered by an unmanned aerial vehicle cluster in a three-dimensional space, and can conveniently avoid the 5-shaped obstacles. And because the guidance law is an analytic expression, the guidance law is easy to realize and has low calculation cost.
It should be noted that the above-mentioned embodiments are only preferred embodiments of the present invention, and are not intended to limit the present invention, and those skilled in the art can make various modifications and changes. Any modification, equivalent replacement, or improvement made within the spirit and principle of the present invention should be included in the protection scope of the present invention.
An embodiment of a second aspect of the present invention provides an unmanned aerial vehicle cluster autonomous obstacle avoidance apparatus based on a combined vector field method, including:
the intersection point acquisition unit is used for acquiring a connecting line between the current position of the unmanned aerial vehicle and the center of the obstacle and an intersection point closest to the surface of the obstacle;
a vector calculation unit for determining a tangent plane of the obstacle surface at the intersection point, and calculating a normal vector of the tangent plane and a tangent vector passing through the intersection point in the tangent plane;
and the guidance law determining unit is used for designing the current guidance law for the unmanned aerial vehicle on the basis of the normal vector and the tangent vector so that the unmanned aerial vehicle avoids the obstacle on the premise of flying towards a target.
The embodiment of the invention provides the autonomous obstacle avoidance guidance law in a unified form and an analytic form aiming at various obstacles which may be encountered by an unmanned aerial vehicle cluster in a three-dimensional space, and can conveniently avoid the obstacles in various shapes.
On the basis of the above-described embodiment, modified embodiments are further proposed, and it is to be noted here that, in order to make the description brief, only the differences from the above-described embodiment are described in each modified embodiment.
According to some embodiments of the invention, the intersection acquisition unit is configured to:
the obstacle is represented by equation 1:
wherein, (x, y, z) T Representing any position in space, q 0 ＝(x 0 ,y 0 ,z 0 ) T Represents the center of the obstacle, a i 、p i (i =1,2,3) are all real numbers, and when p is 1 ＝p 2 ＝p 3 ＝1、a 1 ＝a 2 ＝a 3 When the obstacle is a sphere; when p is 1 ＝p 2 ＝1、p 3 ＞1、a 1 ＝a 2 When the obstacle is a cylinder; when p is 1 ＝p 2 ＝1、0＜p 3 ＜1、a 1 ＝a 2 When the obstacle is a cone; when p is 1 ,p 2 ,p 3 When the height is more than 1, the barrier is a parallelepiped; when p is 1 ＝p 2 ＝1、p 3 ＞1、
R 1 ＞R 2 When the height is more than 0, the barrier is a circular truncated cone.
According to some embodiments of the invention, the vector calculation unit is to:
calculating the normal vector of the tangent plane according to formula 2:
wherein, F x (x i ,y i ,z i )、F y (x i ,y i ,z i )、F z (x i ,y i ,z i ) Respectively, the partial derivatives of the function F (x, y, z) with respect to x, y, z at said intersection point q i ＝(x i ,y i ,z i ) T Taking the value of (A);
calculating a tangent vector in the tangent plane passing through the intersection point according to formula 3:
where x represents the outer product of the vectors, q 0 ＝(x 0 ,y 0 ,z 0 ) T Represents the center of said obstacle, p u ＝(x u ,y u ,z u ) T Representing the current location of the drone, (x) t ,y t ,z t ) T Indicating the target location.
According to some embodiments of the invention, the guidance law determination unit is configured to:
the combined vector field is determined according to equation 4:
where κ > 0 represents an adjustable parameter, F = F (x, y, z), s = ± 1 represents a parameter controlling the direction of rotation of the vector field, V nom > 0 represents a designable speed of the current position of the drone;
calculating the desired course angle χ of the unmanned aerial vehicle according to formula 5 and formula 6 based on the combined vector field d And desired climbing angle gamma d ：
χ d ＝atan2(N dy ,N dx ) In the case of the formula 5,
wherein atan2 represents a four quadrant tangent function.
According to the computer readable storage medium of the embodiment of the present invention, the computer readable storage medium stores the implementation program of information transfer, and the program realizes the steps of the method as described above when being executed by a processor.
An embodiment of the third aspect of the present invention provides a computer-readable storage medium, on which an implementation program for information transmission is stored, where the program, when executed by a processor, implements the steps of the method as described in the embodiment of the first aspect.
It should be noted that the computer-readable storage medium in this embodiment includes, but is not limited to: ROM, RAM, magnetic or optical disks, and the like. The program can be a mobile phone, a computer, a server, an air conditioner, or a network device.
Suffixes such as "module", "part", or "unit" used to denote elements are used only for facilitating the description of the present invention, and have no specific meaning in themselves. Thus, "module", "component" or "unit" may be used mixedly.
Reference to the description of the terms "one embodiment," "some embodiments," "illustrative embodiments," "examples," "specific examples," or "some examples," or the like, means that a particular feature, structure, material, or characteristic described in connection with the embodiment or example is included in at least one embodiment or example of the invention. In this specification, the schematic representations of the terms used above do not necessarily refer to the same embodiment or example. Although some embodiments described herein include some but not other features included in other embodiments, combinations of features of different embodiments are meant to be within the scope of the invention and form different embodiments. The particular features, structures, materials, or characteristics described may be combined in any suitable manner in any one or more embodiments or examples. For example, in the claims, any of the claimed embodiments may be used in any combination.
The terms "comprises," "comprising," or any other variation thereof, are intended to cover a non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements does not include only those elements but may include other elements not expressly listed or inherent to such process, method, article, or apparatus. Without further limitation, an element defined by the phrase "comprising one of 8230, and" comprising 8230does not exclude the presence of additional like elements in a process, method, article, or apparatus comprising the element.
Any reference signs placed between parentheses shall not be construed as limiting the claim. The word "a" or "an" preceding an element does not exclude the presence of a plurality of such elements. The usage of the words first, second and third, etcetera do not indicate any ordering. These words may be interpreted as names.
In some instances, well-known methods, structures and techniques have not been shown in detail in order not to obscure an understanding of this description.
Claims (3)
1. An unmanned aerial vehicle cluster autonomous obstacle avoidance method based on a combined vector field method is characterized by comprising the following steps:
acquiring a connecting line between the current position of the unmanned aerial vehicle and the center of the barrier and a nearest intersection point with the surface of the barrier;
determining a tangent plane of the surface of the obstacle at the intersection point, and calculating a normal vector of the tangent plane and a tangent vector passing through the intersection point in the tangent plane;
designing a current guidance law for the unmanned aerial vehicle based on the normal vector and the tangent vector so that the unmanned aerial vehicle avoids the obstacle on the premise of flying towards a target;
the shape of the barrier comprises a sphere, a cylinder, a cone, a parallelepiped and a round table;
the line of obtaining unmanned aerial vehicle current position and barrier center, an nodical with the nearest barrier surface includes:
the obstacle is represented by equation 1:
wherein, (x, y, z) T Representing any position in space, q 0 ＝(x 0 ,y 0 ,z 0 ) T Represents the center of the obstacle, a i 、p i (i =1,2,3) are all real numbers, and when p is 1 ＝p 2 ＝p 3 ＝1、a 1 ＝a 2 ＝a 3 When the obstacle is a sphere; when p is 1 ＝p 2 ＝1、p 3 ＞1、a 1 ＝a 2 When the obstacle is a cylinder; when p is 1 ＝p 2 ＝1、0＜p 3 ＜1、a 1 ＝a 2 When the barrier is a cone; when p is 1 ,p 2 ,p 3 When the height is more than 1, the barrier is a parallelepiped; when p is 1 ＝p 2 ＝1、p 3 ＞1、
R 1 ＞R 2 When the height is more than 0, the barrier is a circular truncated cone;
the calculating a normal vector of the tangent plane and a tangent vector passing through the intersection point in the tangent plane includes:
calculating the normal vector of the tangent plane according to formula 2:
wherein, F x (x i ,y i ,z i )、F y (x i ,y i ,z i )、F z (x i ,y i ,z i ) Respectively representing the partial derivatives of the function F (x, y, z) with respect to x, y, z at said intersection point q i ＝(x i ,y i ,z i ) T Taking the value of (A);
calculating a tangent vector passing through the intersection point in the tangent plane according to formula 3:
where x represents the outer product of the vectors, q 0 ＝(x 0 ,y 0 ,z 0 ) T Represents the center of said obstacle, p u ＝(x u ,y u ,z u ) T Indicating the current position of the dronePosition (x) t ,y t ,z t ) T Representing a target location;
the designing a current guidance law for the unmanned aerial vehicle based on the normal vector and the tangent vector comprises:
the combined vector field is determined according to equation 4:
where κ > 0 represents an adjustable parameter, F = F (x, y, z), s = ± 1 represents a parameter controlling the vector field rotation direction, V nom > 0 represents a designable speed of the current position of the drone;
calculating the desired course angle χ of the unmanned aerial vehicle according to formula 5 and formula 6 based on the combined vector field d And desired climbing angle gamma d ：
χ d ＝atan2(N dy ,N dx ) In the case of the formula 5,
wherein atan2 represents a four quadrant tangent function.
2. The utility model provides an unmanned aerial vehicle cluster is barrier device independently keeps away based on combination vector field method which characterized in that includes:
the intersection point acquisition unit is used for acquiring a connecting line between the current position of the unmanned aerial vehicle and the center of the obstacle and an intersection point closest to the surface of the obstacle;
a vector calculation unit for determining a tangent plane of the obstacle surface at the intersection point, and calculating a normal vector of the tangent plane and a tangent vector passing through the intersection point in the tangent plane;
a guidance law determining unit, configured to design a current guidance law for the unmanned aerial vehicle based on the normal vector and the tangent vector, so that the unmanned aerial vehicle avoids the obstacle on the premise of flying toward a target;
the intersection point obtaining unit is configured to:
the obstacle is represented by equation 1:
wherein, (x, y, z) T Representing any position in space, q 0 ＝(x 0 ,y 0 ,z 0 ) T Represents the center of the obstacle, a i 、p i (i =1,2,3) are all real numbers, and when p is 1 ＝p 2 ＝p 3 ＝1、a 1 ＝a 2 ＝a 3 When the obstacle is a sphere; when p is 1 ＝p 2 ＝1、p 3 ＞1、a 1 ＝a 2 When the obstacle is a cylinder; when p is 1 ＝p 2 ＝1、0＜p 3 ＜1、a 1 ＝a 2 When the obstacle is a cone; when p is 1 ,p 2 ,p 3 When the height is more than 1, the barrier is a parallelepiped; when p is 1 ＝p 2 ＝1、p 3 ＞1、
R 1 ＞R 2 When the height is more than 0, the barrier is a circular truncated cone;
the vector calculation unit is configured to:
calculating the normal vector of the tangent plane according to formula 2:
wherein, F x (x i ,y i ,z i )、F y (x i ,y i ,z i )、F z (x i ,y i ,z i ) Respectively, the partial derivatives of the function F (x, y, z) with respect to x, y, z at said intersection point q i ＝(x i ,y i ,z i ) T Taking the value of (1);
calculating a tangent vector in the tangent plane passing through the intersection point according to formula 3:
where x represents the outer product of the vectors, q 0 ＝(x 0 ,y 0 ,z 0 ) T Represents the center of said obstacle, p u ＝(x u ,y u ,z u ) T Representing the current location of the drone, (x) t ,y t ,z t ) T Representing a target location;
the guidance law determining unit is configured to:
the combined vector field is determined according to equation 4:
where κ > 0 represents an adjustable parameter, F = F (x, y, z), s = ± 1 represents a parameter controlling the direction of rotation of the vector field, V nom A designable speed of the current position of the unmanned aerial vehicle is represented by more than 0;
calculating the desired course angle χ of the unmanned aerial vehicle according to formula 5 and formula 6 based on the combined vector field d And desired climbing angle gamma d ：
χ d ＝atan2(N dy ,N dx ) In the case of the formula 5,
wherein atan2 represents the quadrant tangent function.
3. A computer-readable storage medium, on which an information transfer implementing program is stored, which, when being executed by a processor, implements the steps of the method according to claim 1.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110705077.9A CN113433969B (en) | 2021-06-24 | 2021-06-24 | Unmanned aerial vehicle cluster autonomous obstacle avoidance method and device based on combined vector field method |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110705077.9A CN113433969B (en) | 2021-06-24 | 2021-06-24 | Unmanned aerial vehicle cluster autonomous obstacle avoidance method and device based on combined vector field method |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113433969A CN113433969A (en) | 2021-09-24 |
| CN113433969B true CN113433969B (en) | 2023-02-17 |
Family
ID=77754231
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110705077.9A Active CN113433969B (en) | 2021-06-24 | 2021-06-24 | Unmanned aerial vehicle cluster autonomous obstacle avoidance method and device based on combined vector field method |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113433969B (en) |
Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2001046766A1 (en) * | 1999-12-21 | 2001-06-28 | Lockheed Martin Corporation | Spatial avoidance method and apparatus |
| CN107966153A (en) * | 2017-11-24 | 2018-04-27 | 中国海洋大学 | Submarine navigation device path planning algorithm |
| CN111596675A (en) * | 2020-05-15 | 2020-08-28 | 中国海洋大学 | Underwater robot optimization decision-making method facing non-wide area target search task |
-
2021
- 2021-06-24 CN CN202110705077.9A patent/CN113433969B/en active Active
Patent Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2001046766A1 (en) * | 1999-12-21 | 2001-06-28 | Lockheed Martin Corporation | Spatial avoidance method and apparatus |
| CN107966153A (en) * | 2017-11-24 | 2018-04-27 | 中国海洋大学 | Submarine navigation device path planning algorithm |
| CN111596675A (en) * | 2020-05-15 | 2020-08-28 | 中国海洋大学 | Underwater robot optimization decision-making method facing non-wide area target search task |
Non-Patent Citations (3)
| Title |
|---|
| Cooperative Bicircular Target Tracking Using Multiple Unmanned Aerial Vehicles;Liang, YQ et al.;《IEEE Conference on Decision and Control》;20141231;第982-987页 * |
| 基于扰动流体动态系统的无人机三维航路规划方法与应用;王宏伦 等;《无人系统技术》;20181231(第1期);第72-82页 * |
| 基于流体扰动算法与深度神经网络的无人机自适应路径规划;王延祥 等;《无人系统技术》;20201130;第3卷(第6期);第50-58页 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113433969A (en) | 2021-09-24 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN112880688B (en) | Unmanned aerial vehicle three-dimensional track planning method based on chaotic self-adaptive sparrow search algorithm | |
| CN108153328B (en) | Multi-missile collaborative track planning method based on segmented Bezier curve | |
| CN107219857B (en) | Unmanned aerial vehicle formation path planning algorithm based on three-dimensional global artificial potential function | |
| US10459445B2 (en) | Unmanned aerial vehicle and method for operating an unmanned aerial vehicle | |
| Kumar et al. | Radar-assisted collision avoidance/guidance strategy for planar flight | |
| CN109190852B (en) | Aircraft target strike track planning method | |
| CN103713642A (en) | Unmanned plane three dimensional route program method based on disturbed fluid dynamic system | |
| CN111024085B (en) | Unmanned aerial vehicle track planning method with end point direction and time constraints | |
| WO2021199449A1 (en) | Position calculation method and information processing system | |
| KR101622260B1 (en) | Impact time control guidance method and device | |
| Kim | Three‐dimensional discrete‐time controller to intercept a targeted UAV using a capture net towed by multiple aerial robots | |
| Mejias et al. | Omnidirectional bearing-only see-and-avoid for small aerial robots | |
| CN107807671A (en) | Unmanned plane cluster danger bypassing method | |
| Liu et al. | An autonomous quadrotor avoiding a helicopter in low-altitude flights | |
| CN113433969B (en) | Unmanned aerial vehicle cluster autonomous obstacle avoidance method and device based on combined vector field method | |
| CN114578856A (en) | Representation method for formation motion characteristic scene of fixed-wing unmanned aerial vehicles | |
| WO2022067759A1 (en) | Flight control method, aircraft, control terminal, and readable storage medium | |
| Kukreti et al. | Detection and localization using unmanned aerial systems for firefighting applications | |
| Zhang et al. | UAV swarm real-time path planning algorithm based on improved artificial potential field method | |
| CN114967735A (en) | Multi-UCAV collaborative real-time track planning method | |
| CN115562335A (en) | 3D map-based unmanned aerial vehicle flight control method, system and medium | |
| Yang et al. | Design, implementation, and verification of a low‐cost terminal guidance system for small fixed‐wing UAVs | |
| Fan et al. | Multi-UAV Cooperative Hunting in Cluttered Environments Considering Downwash Effects | |
| Astrov et al. | Flight control of a trirotor mini-uav for enhanced situational awareness | |
| Astrov | Two-rate motion control of VTAV by NARMA-L2 controller for enhanced situational awareness |
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 | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |