Three-dimensional velocity obstacle avoidance method and device for unmanned aerial vehicle

Abstract

The present disclosure provides a three-dimensional speed obstacle avoidance method and device for a UAV. The method comprises constructing an inertial coordinate system, a body coordinate system, a spherical protection region and a tangent point circular region according to position information of the UAV and position and speed information of an obstacle; generating a potential field model according to a center of a preset repulsive potential field and the spherical protection region, and generating a radial speed based on the potential field model in a case where a modulus of a position vector of the position information of the obstacle meets a preset condition; obtaining a point set according to a vertex coordinate of a preset collision cone and a first rotation matrix; determining a second rotation matrix according to position information of a target point and a rotation angle, and processing the vertex coordinate, the tangent point circular region and the point set to obtain a boundary point set; determining a speed amount of the UAV towards the target point according to the boundary point set and a discretized first parameter variable; and generating a target speed component of the UAV flying towards the target point while avoiding the obstacle according to a third rotation matrix, the speed amount and the radial speed.

Description

TECHNICAL FIELD

[0001] The present disclosure relates to the technical field of unmanned aerial vehicles, and more particularly, to a three-dimensional velocity obstacle avoidance method and device for unmanned aerial vehicles. BACKGROUND

[0002] In recent years, with the increasing application of unmanned aerial vehicles, the application field of unmanned aerial vehicles is gradually expanding, and outdoor cruising, surrounding tracking, swarm confrontation, and closed area exploration have attracted the attention of many researchers. Therefore, higher requirements are put forward for the flexibility and intelligence of unmanned aerial vehicles during operation. The obstacle avoidance function is the basis for realizing many of the above functions and is the guarantee for the safe and stable flight of unmanned aerial vehicles. In recent years, the obstacle avoidance of unmanned aerial vehicles mainly includes the collision cone method, the potential field method, the trajectory planning method, and the velocity obstacle avoidance method.

[0003] In the process of implementing the present disclosure, the inventors found that at least the following problems exist in the related art: The obstacle avoidance path of the unmanned aerial vehicle determined in the related art is easy to enter the protection area of the obstacle, thereby colliding with the obstacle, and the flight safety index of the unmanned aerial vehicle is low. SUMMARY

[0004] Therefore, the present disclosure provides a three-dimensional velocity obstacle avoidance method and device for unmanned aerial vehicles.

[0005] One aspect of the present disclosure provides a three-dimensional velocity obstacle avoidance method for unmanned aerial vehicles, comprising:

[0006] According to the position information of the unmanned aerial vehicle and the position and velocity information of the obstacle, an inertial coordinate system, a body coordinate system of the unmanned aerial vehicle, a spherical protection area of the obstacle, and a tangent point circular area are constructed, respectively, wherein the tangent point circular area represents a circular area formed by a tangent point that is emitted from the position information of the unmanned aerial vehicle and is tangent to the spherical protection area;

[0007] According to the center of the preset repulsive potential field and the spherical protection area, a potential field model is generated, and in the case that the modulus of the position vector of the position information of the obstacle in the body coordinate system satisfies a preset condition, a radial velocity is generated based on the potential field model;

[0008] According to the vertex coordinates of the preset collision cone in the body coordinate system and a first rotation matrix, a point set is obtained, wherein the first rotation matrix is used to convert a cone coordinate system into the body coordinate system, the cone coordinate system is obtained by rotating the body coordinate system based on a preset rotation rule, and the point set represents a set of points formed by a plurality of tangent rays of the spherical protection area on the side surface of the collision cone, wherein the tangent rays are emitted from the position information of the unmanned aerial vehicle;

[0009] According to the position information of the target point in the body coordinate system and the rotation angle of the obstacle avoidance plane, a second rotation matrix is determined, and based on the second rotation matrix, the vertex coordinates, the tangent point circular region and the point set are processed by using a first parameter variable to obtain a boundary point set on the obstacle avoidance plane, wherein the second rotation matrix is used to convert the obstacle avoidance plane coordinate system to the body coordinate system.

[0010] According to the boundary point set and the discretized first parameter variable, a speed amount of the unmanned aerial vehicle in the obstacle avoidance plane coordinate system towards the target point is determined.

[0011] Based on the distance between the unmanned aerial vehicle and the obstacle being located in a preset position range, a target speed component of the unmanned aerial vehicle flying towards the target point while avoiding the obstacle is generated according to at least one of a third rotation matrix, the speed amount and the radial speed, wherein the third rotation matrix is generated according to the rotation angle and is used to convert the body coordinate system to the obstacle avoidance plane coordinate system.

[0012] According to an embodiment of the present disclosure, the body coordinate system has the same coordinate system direction as the inertial coordinate system, the origin of the body coordinate system is the position information of the unmanned aerial vehicle, and the preset position range is determined according to the protection radius of the spherical protection region and the potential field radius of the preset repulsive potential field.

[0013] According to an embodiment of the present disclosure, the cone coordinate system is obtained by rotating the body coordinate system based on a preset rotation rule, comprising:

[0014] The body coordinate system is rotated around a vertical axis by a first rotation angle to obtain a transition coordinate system;

[0015] The transition coordinate system is rotated around a longitudinal axis by a second rotation angle to obtain the cone coordinate system;

[0016] The first rotation matrix is generated according to a first parameter variable, the first rotation angle and the second rotation angle.

[0017] According to an embodiment of the present disclosure, the point set is obtained according to the vertex coordinates of the body coordinate system and the first rotation matrix of the preset collision cone, comprising:

[0018] The tangent point circular region is processed by using the first parameter variable to obtain a processed tangent point circular region;

[0019] The point set is generated according to the processed tangent point circular region, the vertex coordinates and a second parameter variable.

[0020] According to an embodiment of the present disclosure, the second rotation matrix is determined according to the position information of the target point in the body coordinate system and the rotation angle of the obstacle avoidance plane, and the vertex coordinates, the tangent point circular region and the point set are processed based on the second rotation matrix and the first parameter variable to obtain the boundary point set on the obstacle avoidance plane, including:

[0021] The position information of the target point is converted into a vector in the body coordinate system to obtain a target point position vector, wherein the target point position vector includes a longitudinal coordinate component and a vertical coordinate component;

[0022] The rotation angle of the obstacle avoidance plane is generated according to the longitudinal coordinate component and the vertical coordinate component;

[0023] The second rotation matrix is generated according to the rotation angle;

[0024] The vertex coordinates, the tangent point circular region and the point set are converted in the obstacle avoidance plane coordinate system based on the second rotation matrix and the first parameter variable to obtain transition vertex coordinates, a transition tangent point circular region and a transition point set;

[0025] The transition point set is converted based on the transition vertex coordinates, the transition tangent point circular region and a second parameter variable to obtain a converted transition point set;

[0026] In a case where the second parameter variable satisfies a preset parameter variable condition, the boundary point set on the obstacle avoidance plane is generated according to the preset parameter variable condition and the converted transition point set, wherein the preset parameter variable condition is determined according to the vertical coordinate of the vertex coordinates in the obstacle avoidance plane coordinate system and the vertical coordinate of the tangent point circular region in the obstacle avoidance plane coordinate system.

[0027] According to an embodiment of the present disclosure, the speed amount of the unmanned aerial vehicle towards the target point in the obstacle avoidance plane coordinate system is determined according to the boundary point set and the discretized first parameter variable, including:

[0028] The obstacle avoidance coordinates of the target point in the obstacle avoidance plane coordinate system are obtained according to the target point position vector and the rotation angle;

[0029] The first parameter variable is discretized based on a preset discretization rule to obtain the discretized first parameter variable;

[0030] The speed amount towards the target point is generated according to the boundary point set, the discretized first parameter variable and the obstacle avoidance coordinates.

[0031] According to an embodiment of the present disclosure, the target speed component includes a first speed component;

[0032] wherein, based on the distance between the UAV and the obstacle being within the preset position range, the target speed component of the UAV flying away from the obstacle and towards the target point is generated according to at least one of the third rotation matrix, the speed amount and the radial speed.

[0033] generating the third rotation matrix according to the rotation angle;

[0034] in the case that the distance between the UAV and the obstacle is within the preset position range, the first speed component is generated according to the third rotation matrix, the speed amount and the radial speed.

[0035] According to an embodiment of the present disclosure, the target speed component further comprises a second speed component and / or a third speed component;

[0036] wherein, based on the distance between the UAV and the obstacle being within the preset position range, the target speed component of the UAV flying away from the obstacle and towards the target point is generated according to at least one of the third rotation matrix, the speed amount and the radial speed.

[0037] in the case that the distance between the UAV and the obstacle is less than the preset position range, a second target speed component is generated according to the radial speed;

[0038] in the case that the distance between the UAV and the obstacle is greater than the preset position range, a third target speed component is generated according to the third rotation matrix and the speed amount.

[0039] According to an embodiment of the present disclosure, after the target speed component is generated, the method further comprises:

[0040] transmitting the target speed component to a controller of the UAV, so that the controller controls the UAV to fly away from the obstacle and towards the target point according to the target speed component.

[0041] Another aspect of the embodiments of the present disclosure provides a three-dimensional speed obstacle avoidance device of a UAV, comprising:

[0042] a construction module configured to construct an inertial coordinate system, a body coordinate system of the UAV, a spherical protection region of the obstacle and a tangent point circular region according to position information of the UAV and position and speed information of the obstacle, wherein the tangent point circular region represents a circular region formed by a tangent point which is emitted from the position information of the UAV and tangent to the spherical protection region;

[0043] The generating module is configured to generate a potential field model according to a center of a preset repulsive potential field and the spherical protection region, and generate a radial velocity based on the potential field model in a case where a modulus of a position vector of the position information of the obstacle in the body coordinate system satisfies a preset condition.

[0044] The obtaining module is configured to obtain a point set according to a vertex coordinate of the body coordinate system and a first rotation matrix of a preset collision cone, where the first rotation matrix is used to convert a cone coordinate system into the body coordinate system, the cone coordinate system is obtained by rotating the body coordinate system based on a preset rotation rule, and the point set represents a set of points formed by a plurality of rays emitted from the position information of the unmanned aerial vehicle and tangent to the spherical protection region on a cone side of the collision cone.

[0045] The processing module is configured to determine a second rotation matrix according to position information of the target point in the body coordinate system and a rotation angle of the obstacle avoidance plane, and process the vertex coordinate, the tangent point circular region and the point set by using a first parameter variable based on the second rotation matrix to obtain a boundary point set on the obstacle avoidance plane, where the second rotation matrix is used to convert an obstacle avoidance plane coordinate system into the body coordinate system.

[0046] The determining module is configured to determine a velocity amount of the unmanned aerial vehicle in the obstacle avoidance plane coordinate system towards the target point according to the boundary point set and the discretized first parameter variable.

[0047] The obstacle avoidance module is configured to generate a target velocity component of the unmanned aerial vehicle flying towards the target point away from the obstacle according to at least one of a third rotation matrix, the velocity amount and the radial velocity in a case where a distance between the unmanned aerial vehicle and the obstacle is located in a preset position range, where the third rotation matrix is generated according to the rotation angle and is used to convert the body coordinate system into the obstacle avoidance plane coordinate system.

[0048] According to embodiments of this disclosure, by constructing an inertial coordinate system, a UAV body coordinate system, a spherical protection region of an obstacle, and a tangent circular region, a radial velocity under a potential field model is generated based on the center of a preset repulsive potential field and the spherical protection region. A set of boundary points on the obstacle avoidance plane is obtained based on a first parameter, vertex coordinates, the tangent circular region, and a set of points. Based on the boundary point set and the discretized first parameter, the velocity of the UAV toward the target point in the obstacle avoidance plane coordinate system is determined. Based on the distance between the UAV and the obstacle, a target velocity component of the UAV is generated based on at least one of the velocity and the radial velocity to avoid the obstacle and fly toward the target point. Since the radial velocity determined by the potential field model can provide a certain radial velocity for the final target velocity component, the UAV can avoid entering the obstacle's protection region. At the same time, the target velocity component of the UAV determined based on the collision cone and the repulsive potential field can effectively avoid the obstacle, improving the obstacle avoidance effect of the UAV and preventing the UAV from entering the spherical protection region of the obstacle and colliding with the obstacle during flight, thereby improving the flight safety of the UAV. In addition, the introduction of a target point to optimize the selection of the obstacle avoidance plane can reduce the occupancy of computing resources. Attached Figure Description

[0049] The above and other objects, features and advantages of this disclosure will become clearer from the following description of embodiments with reference to the accompanying drawings, in which:

[0050] Figure 1 A flowchart illustrating a three-dimensional velocity obstacle avoidance method for a drone according to an embodiment of the present disclosure is shown schematically.

[0051] Figure 2 A schematic diagram of a drone test platform according to an embodiment of the present disclosure is shown.

[0052] Figure 3 A schematic diagram illustrating a first set of experiments according to an embodiment of the present disclosure is shown in three-dimensional space.

[0053] Figure 4 A schematic diagram illustrating a second set of experiments according to an embodiment of the present disclosure is shown in three-dimensional space.

[0054] Figure 5 The diagram schematically illustrates the distance curves between obstacles and drones in a first set of experiments according to embodiments of the present disclosure.

[0055] Figure 6 The diagram schematically illustrates the distance curves between obstacles and drones in a second set of experiments according to embodiments of the present disclosure; and

[0056] Figure 7 A block diagram of a three-dimensional speed obstacle avoidance device according to an embodiment of the present disclosure is shown schematically. Detailed Implementation

[0057] Hereinafter, embodiments of the present disclosure will be described with reference to the accompanying drawings. It should be understood, however, that the description is merely exemplary and is not intended to limit the scope of the present disclosure. In the following detailed description of the embodiments of the present disclosure, numerous specific details are set forth in order to provide a thorough understanding of the embodiments of the present disclosure. However, it would be apparent to those skilled in the art that the embodiments of the present disclosure can be practiced without these specific details. In other instances, well-known structures and methods are not described in detail in order to avoid obscuring the concepts of the present disclosure.

[0058] The terms used herein are merely used to describe specific embodiments and are not intended to limit the present disclosure. The terms "include", "comprise" and the like used herein indicate the presence of the described features, steps, operations, and / or components, but do not exclude the presence or addition of one or more other features, steps, operations, or components.

[0059] All terms used herein, including technical and scientific terms, have the same meanings as those generally understood by a person skilled in the art unless otherwise defined. It should be noted that the terms used herein should be interpreted as having meanings consistent with the context of the specification, and should not be interpreted in an idealized or overly formal manner.

[0060] In the case of using expressions similar to "at least one of A, B, and C, etc.", it should be generally interpreted as having the same meaning as the expression "one or more of the items from the group consisting of A, B, and C" (e.g., "a system having at least one of A, B, and C" should include but not be limited to a system having A alone, a system having B alone, a system having C alone, a system having both A and B, a system having both A and C, a system having both B and C, and / or a system having A, B, and C, etc.).

[0061] There are currently three main methods for obstacle avoidance of UAVs. The more classic one is the potential field-based obstacle avoidance method. This obstacle avoidance strategy is easy to design, easy to implement, and occupies less computing resources. Some researchers have proposed a formation obstacle avoidance control strategy. UAVs can be divided into small formations when avoiding obstacles, and then recombined after the obstacle avoidance is completed. However, the most commonly used obstacle avoidance method is trajectory planning. Related researchers have proposed a trajectory planning method called "Faster", which enables autonomous flight of UAVs in unknown environments. Related research teams have also made many outstanding achievements in this field, proposing a trajectory planning method called "EGO-planner", which enables UAV formation to shuttle in unknown forest environments. In addition, there is also a geometric-based velocity obstacle avoidance method. Other researchers have designed a new three-dimensional velocity obstacle avoidance strategy, modeling obstacles as cylindrical shapes, and have verified the effectiveness of the designed obstacle avoidance strategy through UAV flight experiments.

[0062] However, the above obstacle avoidance control research on the unmanned aerial vehicle has certain limitations: some existing control designs consume a large amount of computing resources, in addition, most three-dimensional obstacle avoidance strategies based on speed do not consider singular solutions generated when the unmanned aerial vehicle enters the obstacle protection domain, and there is no real flight to verify the effectiveness, so that the unmanned aerial vehicle is prone to collide with the obstacle when flying to avoid the obstacle, thereby reducing the flight safety of the unmanned aerial vehicle.

[0063] Therefore, embodiments of the present disclosure provide a three-dimensional speed obstacle avoidance method and device for an unmanned aerial vehicle. The method comprises constructing an inertial coordinate system, a body coordinate system of the unmanned aerial vehicle, a spherical protection region of the obstacle and a tangent point circular region according to position information of the unmanned aerial vehicle and position and speed information of the obstacle; generating a potential field model according to a center of a preset repulsive potential field and the spherical protection region, and generating a radial speed based on the potential field model in a case where a modulus of a position vector of the position information of the obstacle in the body coordinate system satisfies a preset condition; obtaining a point set according to a vertex coordinate of a preset collision cone in the body coordinate system and a first rotation matrix; determining a second rotation matrix according to position information of a target point in the body coordinate system and a rotation angle of an obstacle avoidance plane, and processing the vertex coordinate, the tangent point circular region and the point set using a first parameter variable based on the second rotation matrix to obtain a boundary point set on the obstacle avoidance plane; determining a speed amount of the unmanned aerial vehicle towards the target point in an obstacle avoidance plane coordinate system according to the boundary point set and the discretized first parameter variable; and generating a target speed component of the unmanned aerial vehicle flying towards the target point away from the obstacle according to at least one of a third rotation matrix, the speed amount and the radial speed based on the distance between the unmanned aerial vehicle and the obstacle being within a preset position range.

[0064] Figure 1 A flowchart of a three-dimensional speed obstacle avoidance method for an unmanned aerial vehicle according to an embodiment of the present disclosure is schematically shown.

[0065] As shown in FIG. 1, the three-dimensional speed obstacle avoidance method for the unmanned aerial vehicle comprises operations S101-S106. Figure 1

[0066] In operation S101, an inertial coordinate system, a body coordinate system of the unmanned aerial vehicle, a spherical protection region of the obstacle and a tangent point circular region are constructed according to position information of the unmanned aerial vehicle and position and speed information of the obstacle, wherein the tangent point circular region represents a circular region formed by a tangent point from the position information of the unmanned aerial vehicle and tangent to the spherical protection region;

[0067] In operation S102, a potential field model is generated according to a center of a preset repulsive potential field and the spherical protection region, and a radial speed is generated based on the potential field model in a case where a modulus of a position vector of the position information of the obstacle in the body coordinate system satisfies a preset condition;

[0068] ​In operation S103, a point set is obtained according to a vertex coordinate of a preset collision cone in a body coordinate system and a first rotation matrix, wherein the first rotation matrix is used to convert a cone coordinate system into the body coordinate system, the cone coordinate system is obtained by rotating the body coordinate system based on a preset rotation rule, and the point set represents a set of points formed by a plurality of rays tangent to the spherical protection region and emitted from the position information of the unmanned aerial vehicle on a side surface of the collision cone.

[0069] In operation S104, a second rotation matrix is determined according to position information of the target point in the body coordinate system and a rotation angle of the obstacle avoidance plane, and the vertex coordinate, the tangent point circular region and the point set are processed by using the first parameter variable based on the second rotation matrix to obtain a boundary point set on the obstacle avoidance plane, wherein the second rotation matrix is used to convert an obstacle avoidance plane coordinate system into the body coordinate system.

[0070] In operation S105, a velocity amount of the unmanned aerial vehicle towards the target point in the obstacle avoidance plane coordinate system is determined according to the boundary point set and the discretized first parameter variable.

[0071] In operation S106, a target velocity component of the unmanned aerial vehicle flying towards the target point while avoiding the obstacle is generated based on at least one of the third rotation matrix, the velocity amount and the radial velocity, wherein the third rotation matrix is generated according to the rotation angle and is used to convert the body coordinate system into the obstacle avoidance plane coordinate system.

[0072] According to embodiments of the present disclosure, the body coordinate system and the inertial coordinate system have the same coordinate system direction, the origin of the body coordinate system is the position information of the unmanned aerial vehicle, and the preset position range is determined according to a protection radius r pz of the spherical protection region and a potential field radius r pf of the preset repulsive potential field.

[0073] According to embodiments of the present disclosure, the position of the unmanned aerial vehicle is defined as A and the position of the obstacle is defined as B. Wherein, {I}={x I ,y I ,z I} is defined as the inertial coordinate system, {b}={x b ,y b ,z b} represents the body coordinate system of the unmanned aerial vehicle, and {c} is defined as the cone coordinate system, x i ,y i ,z i (i=I, B) respectively correspond to the unit vectors of the three principal axes directions of the coordinate system. The three coordinate axis directions of the {b} coordinate system and the {I} coordinate system direction can be the same, and the position of the origin coincides with the position A of the unmanned aerial vehicle. pz is the spherical protection region of the obstacle, B is the center, and r pzTo protect the radius, the tangent point is a circular region B. vo It originates from A and is related to S pz Tangent rays and S pz The circular region formed by the points of tangency, C vo It is B vo The center of the circle, r vo For B vo radius, d vo For A and C vo The distance, θ vo It originates from A and is related to S pz The angle θ between the tangent ray and ray AB az The obstacle is relative to the drone in the {b} coordinate system relative to x. b The yaw angle of the coordinates, θ el This is the pitch angle in the {b} coordinate system. Define {Φ} as the obstacle avoidance plane coordinate system, which is formed by the coordinate system {b} around the x-axis. b The obstacle avoidance plane is obtained by rotating the axis by φ, where φ is the rotation angle of the selected obstacle avoidance plane.

[0074] According to embodiments of this disclosure, the center of the preset repulsive potential field region is defined as B, and the potential field direction is from B along r pz Direction points to S pz Outside, r pf Let be the radius of the potential field. Define the potential field model Υ. pf (·) As shown in formula (1).

[0075]

[0076] Where Θ pf (·) As shown in formula (2):

[0077]

[0078] Among them, c pf A constant greater than 0 The bulge function is shown in formula (3).

[0079]

[0080] Where cos(·) is the cosine function, h is a constant and h∈(0,1). The function Θ(·) is shown in formula (4).

[0081]

[0082] Where a, b, c, and e are constants, and satisfy 0 < a ≤ b and Then the position vector of obstacle B in coordinate system {b} The modulus satisfies The radial velocity v generated by the preset repulsive potential field pf As shown in formula (5).

[0083]

[0084] wherein k pf represents a positive gain, represents a sigma norm, as shown in formula (6).

[0085]

[0086] wherein ∈ is a positive normal number.

[0087] According to an embodiment of the present disclosure, the coordinates of the vertex of the preset collision cone in the coordinate system {b} are defined as A vo As shown in formula (7).

[0088]

[0089] wherein x A , y A , and z A are the components of A VO in the x b , y b , and z b directions in the coordinate system {b}.

[0090] According to an embodiment of the present disclosure, the point set G vo (t p , t g ) is obtained according to the vertex coordinates of the preset collision cone in the body coordinate system and the first rotation matrix The position vector of the target point in the coordinate system {b} is defined as as the components of A b in the x b , y b , and z φ directions in the coordinate system {b}.

[0091] According to an embodiment of the present disclosure, the rotation angle of the collision avoidance plane is φ, and the second rotation matrix R p is determined according to the position information of the target point in the body coordinate system and the rotation angle of the obstacle avoidance plane, as shown in formula (8). The second rotation matrix is used to convert the obstacle avoidance plane coordinate system {Φ} to the body coordinate system.

[0092]

[0093] According to an embodiment of the present disclosure, based on the second rotation matrix, the first parameter variable t pThe vertex coordinates, the tangent point circular region and the point set are processed to obtain a boundary point set VO on an obstacle avoidance plane φ (t p ), and the velocity amount of the UAV in the obstacle avoidance plane coordinate system towards the target point is determined according to the boundary point set VO φ (t p ) and the discretized first parameter variable

[0094] According to an embodiment of the present disclosure, when the distance d AB between the UAV and the obstacle is located in a preset position range, at least one of the third rotation matrix, the velocity amount and the radial velocity is used to generate a target velocity component v d of the UAV flying away from the obstacle towards the target point. It should be noted that the preset position range can be [r pz ,r pf ].

[0095] According to an embodiment of the present disclosure, the inertial coordinate system, the body coordinate system of the UAV, the spherical protection region of the obstacle and the tangent point circular region are respectively constructed, the radial velocity under the potential field model is generated according to the center of the preset repulsive potential field and the spherical protection region, the boundary point set on the obstacle avoidance plane is obtained according to the first parameter variable, the vertex coordinates, the tangent point circular region and the point set, the velocity amount of the UAV in the obstacle avoidance plane coordinate system towards the target point is determined according to the boundary point set and the discretized first parameter variable, and the target velocity component of the UAV flying away from the obstacle towards the target point is generated based on the distance d AB between the UAV and the obstacle and at least one of the velocity amount and the radial velocity. Since the radial velocity determined by the potential field model can provide a certain radial velocity for the final target velocity component, the UAV can be prevented from entering the protection region of the obstacle, and the target velocity component of the UAV determined based on the collision cone and the repulsive potential field can effectively avoid the obstacle, thereby improving the obstacle avoidance effect of the UAV, avoiding the collision between the UAV and the obstacle in the spherical protection region of the obstacle during the flight of the UAV, improving the flight safety of the UAV, and reducing the occupancy rate of the computing resources by introducing the target point to optimize the selection of the obstacle avoidance plane.

[0096] According to an embodiment of the present disclosure, the cone coordinate system is obtained by rotating the body coordinate system based on a preset rotation rule, including the following operations:

[0097] Rotating the body coordinate system around the vertical axis z b by a first rotation angle to obtain a transition coordinate system;

[0098] Rotating the transition coordinate system around the longitudinal axis y b by a second rotation angle to obtain the cone coordinate system {c}.

[0099] The first rotation matrix is determined according to the first parameter variable tp , the first rotation angle θ az , and the second rotation angle -θ el are generated.

[0100] According to an embodiment of the present disclosure, the first rotation angle can be a yaw angle θ b of the obstacle relative to the x az coordinate of the unmanned aerial vehicle in the {b} coordinate system, and the second rotation angle can be a negative value of a pitch angle θ el in the {b} coordinate system.

[0101] According to an embodiment of the present disclosure, the first rotation matrix R is as shown in formula (9).

[0102]

[0103] wherein cos(·) is a cosine function and sin(·) is a sine function.

[0104] According to an embodiment of the present disclosure, the point set is obtained according to the vertex coordinates of the preset collision cone in the body coordinate system and the first rotation matrix, including the following operations:

[0105] The tangent point circular region is processed by using the first parameter variable to obtain a processed tangent point circular region.

[0106] The point set is generated according to the processed tangent point circular region, the vertex coordinates and the second parameter variable.

[0107] According to an embodiment of the present disclosure, the tangent point circular region B p is processed by using the first parameter variable t vo to obtain a processed tangent point circular region, in other words, B vo may be represented by the first parameter variable t p , as shown in formula (10).

[0108]

[0109] wherein x B , y B and z B are components of B vo in the x b , y b and z b directions of the {b} coordinate system.

[0110] According to an embodiment of the present disclosure, the point set G g of all points on the side surface of the cone formed by all the rays emitted by A and tangent to S pz is represented by the second parameter variable t vo (tp g ), as shown in equation (11).

[0111]

[0112] wherein x g (t p ), y g (t p ), and z g (t p ) are the components of G vo in the x b , y b , and z b directions in the coordinate system {b}.

[0113] According to an embodiment of the present disclosure, a second rotation matrix is determined according to position information of a target point in a body coordinate system and a rotation angle of an obstacle avoidance plane, and based on the second rotation matrix, a vertex coordinate, a tangent point circular region, and a point set are processed using a first parameter variable to obtain a boundary point set on the obstacle avoidance plane, including the following operations:

[0114] The position information of the target point is vector-converted in the body coordinate system to obtain a target point position vector, wherein the target point position vector includes a longitudinal coordinate component and a vertical coordinate component;

[0115] A rotation angle of the obstacle avoidance plane is generated according to the longitudinal coordinate component and the vertical coordinate component;

[0116] A second rotation matrix is generated according to the rotation angle;

[0117] The vertex coordinate, the tangent point circular region, and the point set are respectively converted in an obstacle avoidance plane coordinate system based on the second rotation matrix and the first parameter variable to obtain a transition vertex coordinate, a transition tangent point circular region, and a transition point set;

[0118] The transition point set is converted based on the transition vertex coordinate, the transition tangent point circular region, and a second parameter variable to obtain a converted transition point set;

[0119] In a case where the second parameter variable satisfies a preset parameter variable condition, a boundary point set on the obstacle avoidance plane is generated according to the preset parameter variable condition and the converted transition point set, wherein the preset parameter variable condition is determined according to a vertical coordinate of the vertex coordinate in the obstacle avoidance plane coordinate system and a vertical coordinate of the tangent point circular region in the obstacle avoidance plane coordinate system.

[0120] According to an embodiment of the present disclosure, the position information of the target point is vector-converted in the body coordinate system {b} to obtain a target point position vector .​ The x-coordinate in coordinate system {b} b y-axis b Vertical coordinate z b Components in direction.

[0121] According to an embodiment of this disclosure, the rotation angle of the obstacle avoidance plane is selected as φ, based on the vertical coordinate components. and vertical coordinate components The rotation angle φ of the obstacle avoidance plane is generated as shown in formula (12).

[0122]

[0123] Where arctan(·) is the arctangent function. The second rotation matrix R φ Let be the rotation matrix from the {Φ} coordinate system to the {b} coordinate system, as shown in formula (13).

[0124]

[0125] According to embodiments of this disclosure, based on the second rotation matrix Rφ and the first parameter t p Set the vertex coordinates A vo Circular region B at the tangent point vo The set of points G vo Transform them into the obstacle avoidance plane coordinate system {Φ} to obtain the transition vertex coordinates. Circular area of ​​transition tangent point and transition point set As shown in formulas (14) to (16).

[0126]

[0127]

[0128]

[0129] in, for x in coordinate system {Φ} Φ y Φ z Φ Components in direction; for x in coordinate system {Φ} Φ y Φ z Φ Components in direction; for x in coordinate system {Φ} Φ y Φ z Φcomponents in the x

[0130] According to an embodiment of the present disclosure, the transition vertex coordinates Transition cut point circular region and the second variable t g The transition point set is converted to obtain a converted transition point set, as shown in equation (17).

[0131]

[0132] Let That is, the second variable satisfies the preset variable condition, that is When, the boundary point set VO φ (t p ) on the obstacle avoidance plane can be obtained, as shown in equation (18).

[0133]

[0134] According to an embodiment of the present disclosure, the speed amount of the unmanned aerial vehicle towards the target point in the obstacle avoidance plane coordinate system is determined according to the boundary point set and the discretized first variable, including the following operations:

[0135] According to the target point position vector and the rotation angle, the obstacle avoidance coordinates of the target point in the obstacle avoidance plane coordinate system are obtained;

[0136] The first variable is discretized based on a preset discretization rule to obtain a discretized first variable;

[0137] The speed amount towards the target point is generated according to the boundary point set, the discretized first variable, and the obstacle avoidance coordinates.

[0138] According to an embodiment of the present disclosure, the obstacle avoidance coordinates of the target point in the obstacle avoidance plane coordinate system {Φ} are obtained according to the target point position vector and the rotation angle, as shown in equation (19).

[0139]

[0140] Wherein, is The components in the x Φ , y Φ directions in the coordinate system {Φ}.

[0141] According to an embodiment of the present disclosure, the first variable is discretized based on a preset discretization rule to obtain a discretized first variable, as shown in equation (20).

[0142]

[0143] Wherein, n is a positive integer selected according to specific application conditions.

[0144] According to embodiments of the present disclosure, t p The i-th element of t p (i) is selected as the performance index with the included angle along the target point direction, and the velocity quantity as far as possible to the target point is selected, so that the velocity quantity to the target point is generated according to the boundary point set, the discretized first parameter variable and the obstacle avoidance coordinates. The selected velocity quantity can be expressed by formula (21) in the coordinate system {Φ}.

[0145]

[0146] Wherein, arccos(·) is the inverse cosine function, and argmin(·) represents the parameter value when the function value in the parentheses is the minimum.

[0147] According to embodiments of the present disclosure, the target velocity component includes a first velocity component;

[0148] Wherein, based on the distance between the unmanned aerial vehicle and the obstacle being located in the preset position range, the target velocity component of the unmanned aerial vehicle flying to the target point while avoiding the obstacle is generated according to at least one of the third rotation matrix, the velocity quantity and the radial velocity, including the following operations:

[0149] The third rotation matrix is generated according to the rotation angle;

[0150] In the case that the distance between the unmanned aerial vehicle and the obstacle is located in the preset position range, the first velocity component is generated according to the third rotation matrix, the velocity quantity and the radial velocity.

[0151] According to embodiments of the present disclosure, the third rotation matrix R (-φ) is generated according to the rotation angle φ, as shown in formula (22). R (-φ) is the rotation matrix from the {b} coordinate system to the {Φ} coordinate system.

[0152]

[0153] According to embodiments of the present disclosure, v d is defined as the final selected target velocity component, d AB represents the distance between the unmanned aerial vehicle and the obstacle. When d AB ∈ [r pz , r pf ], the first velocity component

[0154] According to embodiments of the present disclosure, the target velocity component further includes a second velocity component and / or a third velocity component;

[0155] The process of generating a target velocity component for the drone to avoid the obstacle and fly toward the target point based on the distance between the drone and the obstacle being within a preset position range, and according to at least one of the third rotation matrix, velocity quantity, and radial velocity, also includes the following operations:

[0156] When the distance between the drone and the obstacle is less than the preset position range, a second target velocity component is generated based on the radial velocity;

[0157] When the distance between the drone and the obstacle is greater than the preset position range, a third target velocity component is generated based on the third rotation matrix and the velocity quantity.

[0158] According to embodiments of this disclosure, when d AB ∈[0,r pz At that time, the second target velocity component v d 2 =v pf When d AB ∈[r pz ,r pf ]hour, When d AB ∈[r pf When [+∞], the velocity component of the third target

[0159] According to embodiments of this disclosure, after generating the target velocity component, the following operations are further included:

[0160] The target velocity component is transmitted to the drone's controller, so that the controller can control the drone to avoid obstacles and fly to the target point based on the target velocity component.

[0161] According to embodiments of this disclosure, the speed signal enables the drone to avoid obstacles while flying toward the target point, thus preventing the drone from colliding with the obstacles.

[0162] Figure 2 An unmanned aerial vehicle (UAV) test platform according to an embodiment of the present disclosure is illustrated schematically. Figure 3 A schematic diagram of a first set of experiments according to an embodiment of the present disclosure is shown. Figure 4 A schematic diagram of a second set of experiments according to an embodiment of the present disclosure is shown. Figure 5 The diagram schematically illustrates the distance curves between obstacles and drones in a first set of experiments according to embodiments of the present disclosure. Figure 6 The diagram schematically illustrates the distance curves between the obstacle and the drone in a second set of experiments according to embodiments of the present disclosure.

[0163] To verify the practical effectiveness of the three-dimensional velocity obstacle avoidance method proposed in this disclosure, flight experiments were conducted using an experimental platform.Figure 2 As shown in the figure, the platform is mainly composed of two quadrotor UAVs with a wheelbase of 0.25 m, which are used to realize obstacle avoidance and act as obstacles, respectively. Each UAV is equipped with an ARM embedded computing board and a controller. The designed control law runs in the onboard ARM embedded computing board at a frequency of 50 Hz, and the controller of the UAV is used to receive the speed instruction of the UAV. The real-time positions of the two quadrotor UAVs are measured by the OptiTrack dynamic capture system. In addition, the ground station PC is used to monitor, obtain experimental data and control the whole experimental process.

[0164] There are two groups of experiments in total. In the first group of experiments, the two UAVs take off at the same time, and the UAV acting as an obstacle flies to the designed hovering point after taking off The UAV designed to perform speed obstacle avoidance flies to the starting point after taking off After both UAVs reach the starting position, the UAV acting as an obstacle continues to hover at the current position, and the UAV designed to perform speed obstacle avoidance flies to the target point Referring to Figure 3 The three-dimensional space diagram is shown in the figure. The parameter selection of the first group of experiments is shown in Table 1. The two UAVs select the same parameters.

[0165] Table 1

[0166]

[0167] In the second group of experiments, the two UAVs still take off at the same time, and the UAV acting as an obstacle flies to the designed starting point after taking off The UAV designed to perform speed obstacle avoidance strategy flies to the starting point after taking off After both UAVs reach the starting position, the UAV acting as an obstacle flies to the target point at a constant speed in a straight line The UAV designed to perform speed obstacle avoidance strategy flies to the target point Referring to Figure 4 The three-dimensional space diagram is shown in the figure. The parameter selection of the second group of experiments is shown in Table 2. The two UAVs select the same parameters.

[0168] Table 2

[0169]

[0170]

[0171] Statistical analysis of experimental data shows that in the first group of experiments, the minimum distance between the UAV and the obstacle during the whole experiment is about d AB = 1.34 m. The radius r pf= 1.5m, that is, the preset repulsive potential field domain provides a certain radial velocity for the UAV in the process of obstacle avoidance, which provides a certain guarantee for preventing the UAV from entering the spherical protection area of the obstacle. And the minimum distance d AB = 1.34m > r pz = 1m, see the distance curve between the UAV and the obstacle shown in Figure 5

[0172] In the second group of experiments, the minimum distance between the UAV and the obstacle during the entire experiment is about d AB = 1.31m. And the set radius r pf = 1.5m of the spherical potential field domain of the obstacle. That is, the preset repulsive potential field domain also provides a certain radial velocity for the UAV in the process of dynamic obstacle avoidance, which provides a certain guarantee for preventing the UAV from entering the spherical protection area of the obstacle. And the minimum distance d AB = 1.31m > r pz = 1m, see the distance curve between the UAV and the obstacle shown in Figure 6 The experimental results prove that the three-dimensional velocity obstacle avoidance strategy based on the preset collision cone and the preset repulsive potential field can effectively avoid static and dynamic obstacles and has good obstacle avoidance effect.

[0173] Figure 7 A block diagram of a three-dimensional velocity obstacle avoidance device according to an embodiment of the present disclosure is schematically shown.

[0174] As shown in Figure 7 The three-dimensional velocity obstacle avoidance device 700 of the UAV includes a construction module 710, a generation module 720, an obtaining module 730, a processing module 740, a determination module 750, and an obstacle avoidance module 760.

[0175] The construction module 710 is configured to construct an inertial coordinate system, a body coordinate system of the UAV, a spherical protection area of the obstacle, and a tangent point circular area according to the position information of the UAV and the position and velocity information of the obstacle, wherein the tangent point circular area represents a circular area formed by a tangent point from the position information of the UAV and tangent to the spherical protection area.

[0176] The generation module 720 is configured to generate a potential field model according to the center of the preset repulsive potential field and the spherical protection area, and generate a radial velocity based on the potential field model in a case where the modulus of the position vector of the position information of the obstacle in the body coordinate system satisfies a preset condition.

[0177] ​The obtaining module 730 is configured to obtain a point set according to the vertex coordinate of the preset collision cone in the body coordinate system and a first rotation matrix, wherein the first rotation matrix is used for converting a cone coordinate system into the body coordinate system, the cone coordinate system is obtained by rotating the body coordinate system based on a preset rotation rule, and the point set represents a set of points formed by a plurality of rays tangent to the spherical protection region and the cone side of the collision cone from the position information of the unmanned aerial vehicle.

[0178] The processing module 740 is configured to determine a second rotation matrix according to the position information of the target point in the body coordinate system and the rotation angle of the obstacle avoidance plane, and process the vertex coordinate, the tangent point circular region and the point set by using the first parameter variable based on the second rotation matrix to obtain a boundary point set on the obstacle avoidance plane.

[0179] The determining module 750 is configured to determine a velocity amount of the unmanned aerial vehicle towards the target point in the obstacle avoidance plane coordinate system according to the boundary point set and the discretized first parameter variable.

[0180] The obstacle avoidance module 760 is configured to generate a target velocity component of the unmanned aerial vehicle flying towards the target point away from the obstacle based on at least one of the third rotation matrix, the velocity amount and the radial velocity, wherein the third rotation matrix is generated according to the rotation angle and is used for converting the body coordinate system into the obstacle avoidance plane coordinate system.

[0181] According to the embodiments of the present disclosure, by respectively constructing the inertial coordinate system, the body coordinate system of the unmanned aerial vehicle, the spherical protection region of the obstacle and the tangent point circular region, generating the radial velocity under the potential field model according to the center of the preset repulsive potential field and the spherical protection region, obtaining the boundary point set on the obstacle avoidance plane according to the first parameter variable, the vertex coordinate, the tangent point circular region and the point set, determining the velocity amount of the unmanned aerial vehicle towards the target point in the obstacle avoidance plane coordinate system according to the boundary point set and the discretized first parameter variable, and generating the target velocity component of the unmanned aerial vehicle flying towards the target point away from the obstacle based on at least one of the velocity amount and the radial velocity, since the radial velocity determined by the potential field model can provide a certain radial velocity for the final target velocity component, the unmanned aerial vehicle can be prevented from entering the protection region of the obstacle, the target velocity component of the unmanned aerial vehicle determined based on the collision cone and the repulsive potential field can effectively avoid the obstacle, the obstacle avoidance effect of the unmanned aerial vehicle is improved, the unmanned aerial vehicle is prevented from colliding with the obstacle when entering the spherical protection region of the obstacle during flight, and thus the flight safety of the unmanned aerial vehicle is improved. In addition, the target point is introduced to optimize the selection of the obstacle avoidance plane, and the occupancy rate of the computing resources can be reduced.

[0182] According to an embodiment of the present disclosure, the body coordinate system has the same coordinate system direction as the inertial coordinate system, the origin of the body coordinate system is the position information of the unmanned aerial vehicle, and the preset position range is determined according to the protection radius of the spherical protection region and the potential field radius of the preset repulsive potential field.

[0183] According to an embodiment of the present disclosure, the conical coordinate system is obtained through the first rotating unit and the second rotating unit.

[0184] The first rotating unit is configured to rotate the body coordinate system around the vertical axis by a first rotation angle to obtain a transition coordinate system.

[0185] The second rotating unit is configured to rotate the transition coordinate system around the longitudinal axis by a second rotation angle to obtain the conical coordinate system.

[0186] The first rotation matrix is generated according to the first variable, the first rotation angle and the second rotation angle.

[0187] According to an embodiment of the present disclosure, the obtaining module 730 includes a processing unit and a first generating unit.

[0188] The processing unit is configured to process the tangent point circular region by using the first variable to obtain a processed tangent point circular region.

[0189] The first generating unit is configured to generate a point set according to the processed tangent point circular region, the vertex coordinate and the second variable.

[0190] According to an embodiment of the present disclosure, the processing module 740 includes a converting unit, a second generating unit, a third generating unit, a first obtaining unit, a second obtaining unit and a fourth generating unit.

[0191] The converting unit is configured to perform vector conversion on the position information of the target point in the body coordinate system to obtain a target point position vector, wherein the target point position vector includes a longitudinal coordinate component and a vertical coordinate component.

[0192] The second generating unit is configured to generate a rotation angle of the obstacle avoidance plane according to the longitudinal coordinate component and the vertical coordinate component.

[0193] The third generating unit is configured to generate a second rotation matrix according to the rotation angle.

[0194] The first obtaining unit is configured to convert the vertex coordinate, the tangent point circular region and the point set in the obstacle avoidance plane coordinate system based on the second rotation matrix and the first variable to obtain a transition vertex coordinate, a transition tangent point circular region and a transition point set.

[0195] The second obtaining unit is configured to convert the transition point set based on the transition vertex coordinate, the transition tangent point circular region and the second variable to obtain a converted transition point set.

[0196] The fourth generating unit is configured to generate a boundary point set on the obstacle avoidance plane according to the preset parameter condition and the converted transition point set when the second parameter satisfies the preset parameter condition, wherein the preset parameter condition is determined according to a vertical coordinate of the vertex coordinate in the obstacle avoidance plane coordinate system and a vertical coordinate of the tangent point circular region in the obstacle avoidance plane coordinate system.

[0197] According to an embodiment of the present disclosure, the determining module 750 includes a third obtaining unit, a fourth obtaining unit, and a fifth generating unit.

[0198] The third obtaining unit is configured to obtain the obstacle avoidance coordinate of the target point in the obstacle avoidance plane coordinate system according to the target point position vector and the rotation angle.

[0199] The fourth obtaining unit is configured to discretize the first parameter based on a preset discretization rule to obtain a discretized first parameter.

[0200] The fifth generating unit is configured to generate the velocity component toward the target point according to the boundary point set, the discretized first parameter, and the obstacle avoidance coordinate.

[0201] According to an embodiment of the present disclosure, the target velocity component includes a first velocity component.

[0202] According to an embodiment of the present disclosure, the obstacle avoidance module 760 includes a sixth generating unit and a seventh generating unit.

[0203] The sixth generating unit is configured to generate a third rotation matrix according to the rotation angle.

[0204] The seventh generating unit is configured to generate the first velocity component according to the third rotation matrix, the velocity component, and the radial velocity when the distance between the unmanned aerial vehicle and the obstacle is within the preset position range.

[0205] According to an embodiment of the present disclosure, the target velocity component further includes a second velocity component and / or a third velocity component.

[0206] According to an embodiment of the present disclosure, the obstacle avoidance module 760 further includes an eighth generating unit and a ninth generating unit.

[0207] The eighth generating unit is configured to generate the second target velocity component according to the radial velocity when the distance between the unmanned aerial vehicle and the obstacle is less than the preset position range.

[0208] The ninth generating unit is configured to generate the third target velocity component according to the third rotation matrix and the velocity component when the distance between the unmanned aerial vehicle and the obstacle is greater than the preset position range.

[0209] According to an embodiment of the present disclosure, the three-dimensional velocity obstacle avoidance apparatus 700 further includes a transmission module.

[0210] The transmission module is configured to transmit the target speed component to a controller of the UAV, so that the controller controls the UAV to fly towards the target point and avoid the obstacle according to the target speed component.

[0211] Any one or more of the modules, units, or at least part of any one or more of the modules, units according to the embodiments of the present disclosure can be implemented in one module. Any one or more of the modules, units according to the embodiments of the present disclosure can be split into multiple modules. Any one or more of the modules, units according to the embodiments of the present disclosure can be implemented at least in part as a hardware circuit, for example, a Field Programmable Gate Array (FPGA), a Programmable Logic Array (PLA), a System on Chip, a System on Board, a System in Package, an Application Specific Integrated Circuit (ASIC), or any other reasonable manner of hardware or firmware by integrating or packaging the circuit, or in any one of software, hardware, and firmware or in a proper combination of any one or more of them. Alternatively, one or more of the modules, units according to the embodiments of the present disclosure can be implemented at least in part as computer program modules, which can perform corresponding functions when the computer program modules are run.

[0212] For example, any of the plurality of the constructing module 710, the generating module 720, the obtaining module 730, the processing module 740, the determining module 750, and the obstacle avoidance module 760 can be combined in one module / unit, or any of the modules / units can be split into multiple modules / units. Alternatively, at least part of the function of one or more of the modules / units can be combined with at least part of the function of other modules / units, and implemented in one module / unit. According to an embodiment of the present disclosure, at least one of the constructing module 710, the generating module 720, the obtaining module 730, the processing module 740, the determining module 750, and the obstacle avoidance module 760 can be at least partially implemented as a hardware circuit, such as a field programmable gate array (FPGA), a programmable logic array (PLA), a system on chip, a system on board, a system on package, an application specific integrated circuit (ASIC), or any other reasonable manner of integrating or packaging a circuit, etc. hardware or firmware, or in any one of software, hardware, and firmware implementations or in a proper combination of any of the above. Alternatively, at least one of the constructing module 710, the generating module 720, the obtaining module 730, the processing module 740, the determining module 750, and the obstacle avoidance module 760 can be at least partially implemented as a computer program module that can perform the corresponding function when the computer program module is run.

[0213] It should be noted that the three-dimensional velocity obstacle avoidance device part in the embodiments of the present disclosure corresponds to the three-dimensional velocity obstacle avoidance method part in the embodiments of the present disclosure, and the description of the three-dimensional velocity obstacle avoidance device part is specifically referred to the three-dimensional velocity obstacle avoidance method part, which will not be repeated here.

[0214] The above describes the embodiments of the present disclosure. However, these embodiments are only for illustrative purposes, and are not intended to limit the scope of the present disclosure. Although each embodiment is described above separately, this does not mean that the measures in each embodiment cannot be used advantageously in combination. The scope of the present disclosure is defined by the appended claims and their equivalents. Without departing from the scope of the present disclosure, those skilled in the art can make various substitutions and modifications, which should all fall within the scope of the present disclosure.

Claims

1. A three-dimensional velocity obstacle avoidance method for unmanned aerial vehicles (UAVs), comprising: Based on the position information of the UAV and the position and velocity information of the obstacle, an inertial coordinate system, a UAV body coordinate system, a spherical protection area of ​​the obstacle, and a tangent circular area are constructed respectively. The tangent circular area represents the circular area formed by the tangent point that is emanating from the position information of the UAV and is tangent to the spherical protection area. Based on the center of the preset repulsive potential field and the spherical protection area, a potential field model is generated, and a radial velocity is generated based on the potential field model when the magnitude of the position vector of the obstacle in the body coordinate system satisfies a preset condition. Based on the vertex coordinates of the preset collision cone in the body coordinate system and the first rotation matrix, a set of points is obtained. The first rotation matrix is ​​used to convert the cone coordinate system into the body coordinate system. The cone coordinate system is obtained by rotating the body coordinate system based on the preset rotation rules. The set of points represents the set of multiple points formed on the cone side of the collision cone by multiple rays emanating from the position information of the UAV and tangent to the spherical protection area. Based on the position information of the target point in the body coordinate system and the rotation angle of the obstacle avoidance plane, a second rotation matrix is ​​determined. Based on the second rotation matrix, the vertex coordinates, the tangent circular region, and the point set are processed using the first parameter to obtain the boundary point set on the obstacle avoidance plane. The second rotation matrix is ​​used to transform the obstacle avoidance plane coordinate system to the body coordinate system. Based on the boundary point set and the discretized first parameter, determine the velocity of the UAV toward the target point in the obstacle avoidance plane coordinate system; Based on the fact that the distance between the drone and the obstacle is within a preset position range, a target velocity component is generated for the drone to avoid the obstacle and fly towards the target point according to at least one of the third rotation matrix, the velocity quantity, and the radial velocity, including: The third rotation matrix is ​​generated based on the rotation angle, wherein the third rotation matrix is ​​generated based on the rotation angle and is used to convert the body coordinate system into the obstacle avoidance plane coordinate system; When the distance between the drone and the obstacle is within a preset range, a first velocity component is generated based on the third rotation matrix, the velocity quantity, and the radial velocity, and the target velocity component includes the first velocity component; When the distance between the drone and the obstacle is less than a preset position range, a second velocity component is generated based on the radial velocity; When the distance between the UAV and the obstacle is greater than a preset position range, a third velocity component is generated based on the third rotation matrix and the velocity quantity. The target velocity component also includes a second velocity component and / or a third velocity component.

2. The method according to claim 1, wherein, The coordinate system of the body coordinate system and the coordinate system of the inertial coordinate system are in the same direction. The origin of the body coordinate system is the position information of the UAV. The preset position range is determined based on the protection radius of the spherical protection area and the potential field radius of the preset repulsive potential field.

3. The method according to claim 2, wherein, The conical coordinate system is obtained by rotating the body coordinate system based on a preset rotation rule, including: By rotating the body coordinate system about the vertical axis by a first rotation angle, a transition coordinate system is obtained; The transition coordinate system is rotated about the longitudinal axis by a second rotation angle to obtain the conical coordinate system; The first rotation matrix is ​​generated based on the first parameter, the first rotation angle, and the second rotation angle.

4. The method according to claim 1, wherein, The point set obtained based on the vertex coordinates of the preset collision cone in the body coordinate system and the first rotation matrix includes: The first parameter is used to process the circular region of the tangent point to obtain the processed circular region of the tangent point. The point set is generated based on the processed circular region of the tangent point, the vertex coordinates, and the second parameter.

5. The method according to claim 1, wherein, The step involves determining a second rotation matrix based on the target point's position in the body coordinate system and the rotation angle of the obstacle avoidance plane. Then, based on this second rotation matrix, the vertex coordinates, the tangent circular region, and the point set are processed using a first parameter to obtain the boundary point set on the obstacle avoidance plane. This includes: The position information of the target point is transformed into a vector in the body coordinate system to obtain the target point position vector, wherein the target point position vector includes a vertical coordinate component and a vertical coordinate component; The rotation angle of the obstacle avoidance plane is generated based on the ordinate component and the vertical coordinate component. The second rotation matrix is ​​generated based on the rotation angle; Based on the second rotation matrix and the first parameter, the vertex coordinates, the tangent circular region, and the point set are respectively transformed into the obstacle avoidance plane coordinate system to obtain the transition vertex coordinates, the transition tangent circular region, and the transition point set. Based on the coordinates of the transition vertex, the circular region of the transition tangent point, and the second parameter, the set of transition points is transformed to obtain the transformed set of transition points. When the second parameter satisfies the preset parameter conditions, the boundary point set on the obstacle avoidance plane is generated according to the preset parameter conditions and the transformed transition point set. The preset parameter conditions are determined based on the vertical coordinates of the vertex coordinates in the obstacle avoidance plane coordinate system and the vertical coordinates of the tangent circular region in the obstacle avoidance plane coordinate system.

6. The method according to claim 5, wherein, The step of determining the velocity of the UAV toward the target point in the obstacle avoidance plane coordinate system based on the boundary point set and the discretized first parameter includes: Based on the target point position vector and the rotation angle, the obstacle avoidance coordinates of the target point in the obstacle avoidance plane coordinate system are obtained; The first parameter variable is discretized based on a preset discretization rule to obtain the discretized first parameter variable; The velocity toward the target point is generated based on the boundary point set, the discretized first parameter, and the obstacle avoidance coordinates.

7. The method according to claim 1, wherein, After generating the target velocity component, the process also includes: The target velocity component is transmitted to the controller of the UAV, so that the controller controls the UAV to avoid the obstacle and fly towards the target point based on the target velocity component.

8. A three-dimensional velocity obstacle avoidance device for an unmanned aerial vehicle (UAV), comprising: The construction module is used to construct an inertial coordinate system, a body coordinate system of the UAV, a spherical protection area of ​​the obstacle, and a tangent circular area based on the position information of the UAV and the position and velocity information of the obstacle. The tangent circular area represents the circular area formed by the tangent points that are emanating from the position information of the UAV and are tangent to the spherical protection area. The generation module is used to generate a potential field model based on the center of the preset repulsive potential field and the spherical protection area, and to generate a radial velocity based on the potential field model when the magnitude of the position vector of the obstacle in the body coordinate system satisfies a preset condition. The module is used to obtain a set of points based on the vertex coordinates of the preset collision cone in the body coordinate system and a first rotation matrix. The first rotation matrix is ​​used to convert the cone coordinate system into the body coordinate system. The cone coordinate system is obtained by rotating the body coordinate system based on a preset rotation rule. The set of points represents the set of multiple points formed on the cone side of the collision cone by multiple rays emanating from the position information of the UAV and tangent to the spherical protection area. The processing module is used to determine a second rotation matrix based on the position information of the target point in the body coordinate system and the rotation angle of the obstacle avoidance plane, and to process the vertex coordinates, the tangent circular region and the point set based on the second rotation matrix using a first parameter to obtain the boundary point set on the obstacle avoidance plane. The second rotation matrix is ​​used to transform the obstacle avoidance plane coordinate system to the body coordinate system. The determination module is used to determine the velocity of the UAV toward the target point in the obstacle avoidance plane coordinate system based on the boundary point set and the discretized first parameter. An obstacle avoidance module is used to generate a target velocity component for the drone to avoid the obstacle and fly towards the target point based on the distance between the drone and the obstacle being within a preset position range, according to at least one of a third rotation matrix, the velocity quantity, and the radial velocity. The obstacle avoidance module includes: The sixth generation unit is used to generate a third rotation matrix based on the rotation angle, wherein the third rotation matrix is ​​generated based on the rotation angle to convert the body coordinate system into the obstacle avoidance plane coordinate system; The seventh generation unit is used to generate a first velocity component based on the third rotation matrix, velocity quantity and radial velocity when the distance between the UAV and the obstacle is within a preset position range. The target velocity component includes the first velocity component. The eighth generation unit is used to generate a second velocity component based on the radial velocity when the distance between the UAV and the obstacle is less than a preset position range. The ninth generation unit is used to generate a third velocity component based on a third rotation matrix and a velocity quantity when the distance between the UAV and the obstacle is greater than a preset position range. The target velocity component also includes a second velocity component and / or a third velocity component.

Images