CN111781949B - Method for avoiding rod-shaped obstacle by unmanned aerial vehicle - Google Patents
Method for avoiding rod-shaped obstacle by unmanned aerial vehicle Download PDFInfo
- Publication number
- CN111781949B CN111781949B CN202010631248.3A CN202010631248A CN111781949B CN 111781949 B CN111781949 B CN 111781949B CN 202010631248 A CN202010631248 A CN 202010631248A CN 111781949 B CN111781949 B CN 111781949B
- Authority
- CN
- China
- Prior art keywords
- unmanned aerial
- aerial vehicle
- rod
- obstacle
- shaped barrier
- 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
- 238000000034 method Methods 0.000 title claims abstract description 27
- 230000004888 barrier function Effects 0.000 claims abstract description 62
- 238000001514 detection method Methods 0.000 claims abstract description 41
- 238000011156 evaluation Methods 0.000 claims abstract description 8
- 239000013598 vector Substances 0.000 claims description 23
- 239000011159 matrix material Substances 0.000 claims description 21
- 230000008447 perception Effects 0.000 claims description 4
- 238000000354 decomposition reaction Methods 0.000 claims description 3
- 230000002441 reversible effect Effects 0.000 claims description 3
- 238000004364 calculation method Methods 0.000 abstract description 9
- 238000005507 spraying Methods 0.000 description 8
- 238000010586 diagram Methods 0.000 description 6
- 230000008901 benefit Effects 0.000 description 5
- 230000008569 process Effects 0.000 description 4
- 230000003068 static effect Effects 0.000 description 4
- 230000000694 effects Effects 0.000 description 3
- RZVHIXYEVGDQDX-UHFFFAOYSA-N 9,10-anthraquinone Chemical group C1=CC=C2C(=O)C3=CC=CC=C3C(=O)C2=C1 RZVHIXYEVGDQDX-UHFFFAOYSA-N 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 2
- 238000011161 development Methods 0.000 description 2
- 230000007613 environmental effect Effects 0.000 description 2
- 238000005259 measurement Methods 0.000 description 2
- 230000007246 mechanism Effects 0.000 description 2
- 238000011160 research Methods 0.000 description 2
- 238000006467 substitution reaction Methods 0.000 description 2
- 230000009466 transformation Effects 0.000 description 2
- 238000013459 approach Methods 0.000 description 1
- 230000005540 biological transmission Effects 0.000 description 1
- 239000002775 capsule Substances 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 230000000295 complement effect Effects 0.000 description 1
- 230000008878 coupling Effects 0.000 description 1
- 238000010168 coupling process Methods 0.000 description 1
- 238000005859 coupling reaction Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000002474 experimental method Methods 0.000 description 1
- 230000007774 longterm Effects 0.000 description 1
- 238000007726 management method Methods 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 239000000203 mixture Substances 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012544 monitoring process Methods 0.000 description 1
- 230000000149 penetrating effect Effects 0.000 description 1
- 230000002093 peripheral effect Effects 0.000 description 1
- 239000007921 spray Substances 0.000 description 1
- 239000000126 substance Substances 0.000 description 1
- 238000012360 testing method Methods 0.000 description 1
- 238000012876 topography Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/10—Simultaneous control of position or course in three dimensions
- G05D1/101—Simultaneous control of position or course in three dimensions specially adapted for aircraft
Landscapes
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The invention provides a method for avoiding a rod-shaped obstacle by an unmanned aerial vehicle, which comprises the following steps: sensing and identifying the surrounding environment on a flight path through detection equipment on an unmanned aerial vehicle, wherein the detection equipment outputs detection data in real time; the unmanned aerial vehicle acquires relative position information of the rod-shaped barrier according to the detection data, wherein the relative position information comprises the relative distance between the unmanned aerial vehicle and the rod-shaped barrier and the relative angle between the unmanned aerial vehicle and the rod-shaped barrier; the unmanned aerial vehicle determines the profile of the rod-shaped barrier according to the relative distance between the unmanned aerial vehicle and the rod-shaped barrier and the relative angle between the unmanned aerial vehicle and the rod-shaped barrier; the unmanned aerial vehicle calculates the minimum distance under the current attitude according to the profile of the rod-shaped obstacle; and according to the calculated minimum distance under the current attitude, the unmanned aerial vehicle carries out collision urgency evaluation and determines whether to adjust the flying track route. On one hand, the invention greatly simplifies the calculation complexity of the algorithm and reduces the execution time on the basis of certain numerical precision.
Description
Technical Field
The invention relates to the technical field of unmanned aerial vehicles, in particular to a method for avoiding a rod-shaped obstacle by an unmanned aerial vehicle.
Background
In the actual operation process, the relative distance between the drone and the obstacle varies over time, especially for static obstacles on the ground. Despite the variety of types of static obstacles in the field, they can still be classified according to the actual size of the obstacle: micro obstacles such as overhead cables, branches of trees, and the like; small and medium-sized obstacles such as standing utility poles and dispersedly cultivated trees in the field; large obstacles: houses, high voltage electrical towers, and the like. Considering the difference between the terrain and the topography of different areas, the crop cultivation conditions and the local field planting management policy, the probability that large areas of trees, high-voltage towers and houses exist in fields of a plurality of areas is very low, and the plant protection unmanned aerial vehicle generally selects low-altitude operation, namely flight height of 3-5 m, so that high-altitude cables which possibly threaten are covered. Through real-field exploration, telegraph poles are ubiquitous in the field or around as infrastructures for production and life.
With the progress of the operation, the distance between the spraying unit and the field barrier is continuously shortened, which means that the proximity between the spraying unit and the field barrier is continuously deepened. If the distance between barrier and the unmanned aerial vehicle is too close, then the paddle air current that the high-speed rotatory paddle of unmanned aerial vehicle formed contacts the barrier wall and can form the reflection flow, can cause the interference to the distribution of paddle air current to influence unmanned aerial vehicle flight performance, noise interference each other between the electronic equipment can lead to the organism to strike barrier or other incident's emergence even. However, if the distance is too far, that is, the unmanned aerial vehicle performs avoidance operation extremely early without any factor prompt, as a result, the obstacle avoidance is smooth, but the phenomena of re-spraying of the adjacent row crops and missing spraying of the crops around the obstacle are caused, and further, the crop operation efficiency and the biological control effect are not effectively ensured. Therefore, the accurate determination of the minimum safe distance between the plant protection unmanned aerial vehicle and the field barrier is a key factor restricting the long-term development of the aviation plant protection spraying industry. In fact, the minimum distance measurement of the drone is a challenging task with a set of multidisciplinary couplings of dynamics, kinematics, hydrodynamics, etc. This work not only can guarantee the safety of equipment, unmanned aerial vehicle and crop, can also provide the guide effect to each item technical parameter that follow-up spraying operation realization high-quality effect needs.
In order to improve the maneuverability of the unmanned aerial vehicle for avoiding the obstacle, meet the working requirements under various working conditions, improve the safety and reliability of the unmanned aerial vehicle for avoiding the obstacle operation, and accurately determine the minimum distance between the unmanned aerial vehicle and the obstacle, the method is very important. Therefore, it is necessary to quantify the minimum distance between the drone and the obstacle. Some researchers judge and measure the minimum distance by means of obstacle detection and identification technology in development and test of some autonomous driving type implements, and obtain stage research results. Berker M et al and Bouadallah use multiple ultrasonic sensors for obstacle detection in Design and control of quadrats with application to autonomous flight, but the ultrasonic coverage is limited and distance control cannot be performed well. Scherer S et al flying fast and low atmospheric obstacles, methods and experiments belong to the first department of putting laser scanners into obstacle detection. However, data acquired by the laser scanner are discrete information, and for one of the characteristics of the two-dimensional laser scanner, data drift can occur along with time to greatly affect the accuracy of scanned data, and the data drift is rare in the field of agricultural unmanned aerial vehicles.
In fact, the millimeter wave radar has the advantages of strong penetrating power and long detectable distance, and effectively avoids the huge influence of environmental interference by depending on the work of a microwave band, but can only describe the parallel distance of obstacles in a field and cannot give further information such as a contour angle, and on the contrary, the detection range of the ultrasonic sensor is limited, and the detection range of the ultrasonic sensor is limited by environmental factors and monitoring blind areas due to various factors such as a working mechanism and the like many other sensors.
Disclosure of Invention
Aiming at the defects in the prior art, the invention provides a method for avoiding a rod-shaped obstacle by an unmanned aerial vehicle, which expands the capsule method in the field of obstacle avoidance by the unmanned aerial vehicle, wherein the obstacle comprises any slender rod-shaped object with a certain diameter, and the slender rod-shaped object can have certain similarity with a cylinder in a set.
The present invention achieves the above-described object by the following technical means.
An unmanned aerial vehicle avoidance method for a rod-type obstacle comprises the following steps:
sensing and identifying the surrounding environment on a flight path through detection equipment on an unmanned aerial vehicle, wherein the detection equipment outputs detection data in real time;
the unmanned aerial vehicle acquires relative position information of the rod-shaped barrier according to the detection data, wherein the relative position information comprises the relative distance between the unmanned aerial vehicle and the rod-shaped barrier and the relative angle between the unmanned aerial vehicle and the rod-shaped barrier; the unmanned aerial vehicle determines the profile of the rod-shaped barrier according to the relative distance between the unmanned aerial vehicle and the rod-shaped barrier and the relative angle between the unmanned aerial vehicle and the rod-shaped barrier;
the unmanned aerial vehicle calculates the minimum distance d under the current attitude according to the profile of the rod-shaped obstacle min The method comprises the following steps:
according to unmanned aerial vehicle parcel organism outer fringe is the ellipsoid, establishes the coordinate of arbitrary point in inertial coordinate system on the unmanned aerial vehicle of ellipsoid:
x ′ ＝R -1 (x,y,z) T ，
wherein, (x, y, z) T Is the position of any point on the ellipsoid;
R -1 is the inverse of the rotation matrix R, which is:
last point s of ellipsoid unmanned aerial vehicle 1 Can be expressed as: s 1 ＝s 0 +λ 2 (s 1 -s 0 ) Wherein λ is 2 Is constant, b < lambda 2 A, s0 is the central position vector of the ellipsoid, s 1 Is a vector of any point position on the ellipsoid; a is the major axis radius of the ellipsoid, and b is the minor axis radius of the ellipsoid;
determining the positions of two ends of the bar-shaped barrier and a gyration radius rho according to the outline of the bar-shaped barrier;
any point s on the obstacle 2 Can be expressed as: s is 2 ＝u+λ 1 (q-u) wherein λ 1 Is a constant number 0<λ 1 <1,q and u are the position vectors of two axial end points of the bar-shaped barrier respectively;
the minimum distance between any point on the ellipsoid and the surface of the obstacle is as follows: min (Δ r) = min (| s) 1 s 2 L-p), Δ r is the spatial minimum distance vector, and Δ r = s 2 -s 1 ρ is the radius of gyration of the obstacle;
through quadratic programming and orthogonal decomposition, min (delta r) = min (v) is obtained T v) wherein v = Rx + Q T y, Q is a matrix with the column vector as unit length and orthogonal with each other, and R is a triangular matrix; vector y = u-s 0 ；
And (4) optimizing and solving to obtain:
wherein v is min Is the minimum value of v;
according to the minimum distance d under the current posture min And the unmanned aerial vehicle carries out collision urgency evaluation and determines whether to adjust the flying track route.
Further, the minimum distance min (Δ r) between any point on the ellipsoid and the surface of the obstacle is defined by two times as:
min(Δr)＝min(Ax+y) T (Ax+y)
wherein the matrix
Vector y = u-s 0 Vector x = [ x ] 1 x 2 ] T And 0 is<x 1 <1，b＜＜x 2 ＜＜a。
Further, the matrix a is orthogonally decomposed to obtain min (Δ r) = min (Rx + Q) T y) T (Rx+Q T y)。
Further, the evaluation of the collision urgency is specifically as follows:
minimum distance d under current attitude min Less than or equal to a set threshold distance d th Re-planning the path by an obstacle avoidance algorithm;
minimum distance d at current attitude min Less than the collision risk distance d haz Collision is avoided by reverse flight.
Further, the detection device comprises a millimeter wave radar and an ultrasonic sensor, and is used for obstacle identification and perception.
Further, determining the positions of the two ends of the bar-shaped obstacle and the gyration radius rho according to the profile of the bar-shaped obstacle, specifically:
the outline of the rod-shaped barrier is composed of scattered point data measured by the detection equipment at each moment, and the scattered point data comprises the relative distance between the unmanned aerial vehicle and the rod-shaped barrier and the relative angle between the unmanned aerial vehicle and the rod-shaped barrier; and solving the gyration radius rho of the bar-shaped barrier according to the curve arc length and the radian of the profile of the bar-shaped barrier, and determining the spatial positions of two axial end points of the barrier according to the height of the bar-shaped barrier measured by the detection equipment.
The invention has the beneficial effects that:
according to the unmanned aerial vehicle pole-type obstacle avoidance method, on one hand, the calculation complexity of an algorithm is greatly simplified on the basis of certain numerical precision, and the execution time is reduced; on the other hand, efficient distance calculation and evaluation are beneficial to timely adjustment of the plant protection unmanned aerial vehicle on field condition changes in the operation process, spraying operation is smoothly completed, and spraying efficiency is improved so as to meet expected requirements.
Drawings
Fig. 1 is a flowchart of an avoidance method of a stick-type obstacle by an unmanned aerial vehicle according to the present invention.
Fig. 2 is a schematic diagram of obtaining the relative position of the obstacle according to the present invention.
Fig. 3 is a schematic diagram illustrating calculation of minimum distance between an unmanned aerial vehicle and an obstacle space according to the present invention.
Fig. 4 is a preset area diagram of the unmanned aerial vehicle as a central observation object according to the present invention.
FIG. 5 is a flow chart of the crash urgency evaluation logic determination according to the present invention.
FIG. 6 is a schematic diagram of the transformation of the spatial coordinate system according to the present invention.
In fig. 4:
i: an elliptical convex envelope line of the unmanned aerial vehicle can be regarded as a body of the unmanned aerial vehicle;
II, outer boundary lines of the dangerous area;
III: a secondary hazard zone outer boundary line;
IV: an outer boundary line of the free region;
d haz collision risk distance;
d th a safe threshold distance.
Detailed Description
The technical solutions of the embodiments of the present invention will be described below clearly with reference to the drawings in the embodiments of the present invention, and it is obvious that the described embodiments are only a part of the embodiments of the present invention, and not all embodiments. All other embodiments, which can be obtained by a person skilled in the art without making creative efforts based on the embodiments of the present invention, belong to the protection scope of the present invention.
It should be noted that all research theories or calculation methods in the invention have theoretical sources, the components are all in rigid connection, the unmanned aerial vehicle space is regarded as a rigid body, and the model and the principle are combined to be pasted with implementation cases and result descriptions in the appendix, thereby belonging to the protection scope of the invention.
Unless defined otherwise, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this invention belongs. The terminology used in the description of the invention herein is for the purpose of describing particular embodiments only and is not intended to be limiting of the invention. As used herein, the term "and/or" includes any and all combinations of one or more of the associated listed items.
Some embodiments of the invention are described in detail below with reference to the accompanying drawings. The embodiments and features of the embodiments described below can be combined with each other without conflict.
As shown in fig. 1, the method for avoiding a stick-type obstacle by an unmanned aerial vehicle according to the present invention includes the following steps:
step 1, in the process that the unmanned aerial vehicle flies according to a preset operation route, sensing the surrounding environment through the detection equipment, and acquiring detection data output by the detection equipment in real time.
The execution main body of the method of the embodiment is flight control equipment, and specifically can be a flight controller of an unmanned aerial vehicle. In this embodiment, unmanned aerial vehicle can realize setting for the operation route to require unmanned aerial vehicle to fly according to this route, be provided with the detection device among the unmanned aerial vehicle, this detection device can carry out perception and detection to the peripheral barrier environment of current unmanned aerial vehicle operation route, and output detection data, allow flight control ware to acquire this detection data in real time simultaneously.
The detection equipment adopts a combined mode of combining a millimeter wave radar and an ultrasonic sensor to identify and detect the obstacle. By utilizing the method, the former exerts the advantages of medium-long distance anti-interference detection, and the latter makes up the weakness of short-distance identification and positioning. The combined approach maximizes performance advantages while meeting acceptable costs and improves system detection reliability over other engineering techniques that do not have redundant sensors for obstacle identification and detection. Therefore, the two sensors depending on different working principles are complemented on respective advantages and disadvantages, and a good hardware platform is provided for the actual measurement of the distance between obstacles.
Step 2, the unmanned aerial vehicle acquires relative position information of the rod-shaped obstacle according to the detection data, wherein the relative position information comprises the relative distance between the unmanned aerial vehicle and the rod-shaped obstacle and the relative angle between the unmanned aerial vehicle and the rod-shaped obstacle; the unmanned aerial vehicle determines the profile of the rod-shaped barrier according to the relative distance between the unmanned aerial vehicle and the rod-shaped barrier and the relative angle between the unmanned aerial vehicle and the rod-shaped barrier;
the relative distance between the unmanned aerial vehicle and the rod-shaped barrier represents the spatial distance between the center point of the detection device and the surface of the barrier, and the distance is not the minimum distance between the unmanned aerial vehicle and the barrier required by the invention; the relative angle between the unmanned aerial vehicle and the rod-shaped barrier refers to the included angle of the barrier relative to the positive direction of the head of the unmanned aerial vehicle. The relative distance and the relative angle are used for subsequently positioning the space positions of two axial end points of the barrier.
As shown in fig. 2, the unmanned aerial vehicle can obtain an approximate contour of the current obstacle at each time point, the contour is formed according to scatter data measured by the detection device at each time point, the scatter data includes a relative distance and a relative angle, the scatter points are sequentially connected to form a contour curve, the radius ρ of the obstacle is solved according to the arc length and the radian of the contour curve, the axis position of the obstacle can be positioned at the time, the height of the scatter points detected by the unmanned aerial vehicle at the time can be determined according to height information provided by the ground station, and the spatial positions of two axial end points of the obstacle, namely q and u, can be calculated and determined.
Because the operation of the unmanned aerial vehicle is a dynamic process, and the characteristics of the detection equipment are different, the data between the detection equipment can be complementary and redundant. Particularly, when unmanned aerial vehicle is far away from the barrier, the data acquisition of long-range distance and angle can be realized to the advantage of highly utilizing the long-range perception of millimeter wave radar, along with unmanned aerial vehicle is close to the barrier gradually, when acquireing the distance with the angle, the static profile of barrier also can be through the all-round acquireing of ultrasonic wave. Through scattered point data from different detection devices at each moment, a point set of different position points of the obstacle can be basically constructed, and the static outline of the obstacle can be described.
Step 3, the unmanned aerial vehicle calculates the minimum distance d under the current attitude according to the profile of the rod-shaped obstacle min Firstly, obtaining the coordinates of the current space position point of the unmanned aerial vehicle through a ground station, establishing a rigid body coordinate system as shown in figure 3 by taking the coordinates as a central point, establishing a minimum volume ellipsoid external surface equation wrapping the outer edge of the body, obtaining the coordinates of any point on an ellipsoid, taking the rod-shaped barrier as a cylinder to obtain the coordinates of any point on the surface of the barrier, and minimizing the difference between the two coordinates to obtain the minimum distance d between the unmanned aerial vehicle and the barrier min The method comprises the following steps:
assuming that the unmanned aerial vehicle is a rigid body, establishing a rigid body coordinate system with the unmanned aerial vehicle as an original point and establishing a minimum volume ellipsoid outer surface equation wrapping the outer edge of the body:
wherein (x) 0 ,y 0 ,z 0 ) T Is the unmanned aerial vehicle center point coordinate; (x, y, z) T Is the position of any point on the ellipsoid;
a is the radius of the major axis of the ellipsoid, i.e. along x B (ii) a b is the minor axis radius of the ellipsoid along y B (ii) a c is the polar radius, i.e. along z B (ii) a And | x | < a, | y | < b, | z | < c, a>0,b>0,c>0，z＜＜b＜＜a。
Combining an unmanned aerial vehicle-obstacle space minimum distance calculation schematic diagram 3, acquiring current space position information of the unmanned aerial vehicle through the ground station, specifically acquiring a certain position point in space, and setting the position point coordinate as (x) 0 ,y 0 ,z 0 ) T This point can be considered as the drone center point.
The coordinates of any point on the ellipsoid in the inertial coordinate system can be expressed as formula (1)
In detail, the transformation of coordinates in formula (1) requires calculation by means of a rotation matrix, and the calculation formula is shown in formula (2)
x′＝R -1 (x,y,z) T (2)
Wherein R is -1 Is the inverse matrix of the rotation matrix.
The inertial frame can describe the space uniformly and isotropically, while in general engineering dynamics, the frame fixed to the earth can be used as an approximate inertial frame. The rotation matrix is used for describing the composition of a basic matrix after the object rotates a certain angle around three axes of a right-hand Cartesian coordinate system in sequence, and can be used for describing the attitude angle of a rigid body in space. In flight dynamics, as in FIG. 6 with respect to the right-hand Cartesian coordinate System x w 、y w 、z w The rotation of the shaft is called roll, pitch, yaw rotation, respectively.
The rotation matrix R is as follows:
wherein the content of the first and second substances,
for roll angle, theta is pitch angle and psi is yaw angle. The three angles being about x w 、y w 、z w The angle of rotation of the shaft.
Converting the points in the rigid coordinate system into the inertial coordinate system through the conversion matrix, and analyzing by combining with an unmanned aerial vehicle-obstacle space minimum distance calculation schematic diagram 3, wherein one point on the ellipsoid is positionedSpecific positions in the inertial coordinate system can be obtained and expressed as
The spheroid center of the ellipsoid is expressed as
As can be seen from FIG. 3, a point s on the ellipsoid 1 Can be expressed as formula (3)
s 1 ＝s 0 +λ 2 (s 1 -s 0 ) (3)
Wherein λ 2 Is constant, b < lambda 2 ＜＜a，s 0 Is the body center position vector, s 1 Is the position vector of any point on the ellipsoid.
Similarly, any point s on the barrier 2 Can be expressed as formula (4)
s 2 ＝u+λ 1 (q-u) (4)
Wherein λ is 1 Is a constant number 0<λ 1 <1,q and u are two end-point position vectors in the axial direction for a bar-type obstacle.
Thus, the problem is transformed into a problem of calculating the minimum distance between a point on the ellipsoid and a point on the surface of the obstacle, represented by equation (5)
min(Δr)＝min(|s 1 s 2 |-ρ) (5)
Wherein Δ r represents the spatial minimum distance vector, and Δ r = s 2 -s 1 ρ is the radius of the obstacle, and in consideration of ρ being a constant, equation (5) can be transcribed as equation (6)
min(Δr)＝min(|s 1 s 2 |) (6)
The equation (6) can be further modified into a quadratic programming form as shown in equation (7)
min(Δr)＝min(Ax+y) T (Ax+y) (7)
Wherein the matrix
Vector y = u-s 0 Vector x = [ x ] 1 x 2 ] T And 0 is<x 1 <1，b＜＜x 2 ＜＜a。
Said formula (7) can be transcribed as formula (8) by orthogonal decomposition of the matrix A
min(Δr)＝min(Rx+Q T y) T (Rx+Q T y) (8)
Wherein, Q is a 6 × 2 matrix with column vectors as unit length and orthogonal with each other, and R is a 2 × 2 upper triangular matrix.
Converting the formula (8) into the formula (9) by simple substitution
min(Δr)＝min(v T v) (9)
Wherein v = Rx + Q T y。
Then the minimum distance can be solved optimally by equation (9), as shown in equation (10):
wherein v is min Is the point where v is minimized in equation (9), i.e., the feasible solution set is closest to the origin or feasible solution set boundary.
Step 4, calculating the minimum distance d under the current posture min And the unmanned aerial vehicle carries out collision urgency evaluation and determines whether to adjust the flying track route. As shown in FIGS. 4 and 5, the minimum distance d at the current attitude min Less than or equal to a set threshold distance d th Re-planning the path by an obstacle avoidance algorithm; minimum distance d in front attitude min Greater than a set threshold distance d th And keeping the original flight track. Minimum distance d at current attitude min Less than the collision risk distance d haz Collision is avoided by reverse flight.
Steps 5 to 7 show that after the collision urgency evaluation described in step 4 is performed, the current situation of the unmanned aerial vehicle is judged, and then an adjustment signal is sent to the flight controller, so that an adjustment path related to the obstacle avoidance path is made through the signal receiving controller. The track route of the unmanned aerial vehicle flying can be reflected in real time through a remote control interface used by a user or a computer ground station interface connected through data transmission.
With plant protection unmanned aerial vehicle for example, plant protection unmanned aerial vehicle can spray the operation according to predetermined operation route. However, the actual field environment is complex, and the types of obstacles are diversified, including but not limited to telegraph poles, high voltage line towers, high voltage lines, houses, trees, and the like. Therefore, the high-pole type obstacle according to the present embodiment may be an obstacle having geometric characteristics of a long and thin cylinder with a certain diameter in terms of geometric mechanism, such as a utility pole, a high-voltage power tower, or a high-voltage line.
For such ubiquitous field obstacles as poles, the obstacles may be viewed as cylinders having a height and radius thickness, and the radius thickness is designated as ρ, the actual height and radius of which need to be estimated in the field at a previous time to be in the approximate size range.
The present invention is not limited to the above-described embodiments, and any obvious improvements, substitutions or modifications can be made by those skilled in the art without departing from the spirit of the present invention.
Claims (5)
1. An unmanned aerial vehicle avoidance method for a rod-shaped obstacle is characterized by comprising the following steps:
sensing and identifying the surrounding environment on a flight path through detection equipment on an unmanned aerial vehicle, wherein the detection equipment outputs detection data in real time;
the unmanned aerial vehicle acquires relative position information of the rod-shaped barrier according to the detection data, wherein the relative position information comprises the relative distance between the unmanned aerial vehicle and the rod-shaped barrier and the relative angle between the unmanned aerial vehicle and the rod-shaped barrier; the unmanned aerial vehicle determines the profile of the rod-shaped barrier according to the relative distance between the unmanned aerial vehicle and the rod-shaped barrier and the relative angle between the unmanned aerial vehicle and the rod-shaped barrier;
said is free ofThe man-machine calculates the minimum distance d under the current posture according to the profile of the rod-shaped barrier min The method comprises the following steps:
according to unmanned aerial vehicle parcel organism outer fringe is the ellipsoid, establishes the coordinate of arbitrary point in inertial coordinate system on the unmanned aerial vehicle of ellipsoid:
x′＝R -1 (x,y,z) T ，
wherein, (x, y, z) T Is the position of any point on the ellipsoid;
R -1 is the inverse of the rotation matrix R, which is:
last point s of ellipsoid unmanned aerial vehicle 1 Can be expressed as: s 1 ＝s 0 +λ 2 (s 1 -s 0 ) Wherein λ is 2 Is constant, b < lambda 2 ＜＜a，s 0 Is a vector of the central position of an ellipsoid, s 1 Is a vector of any point position on the ellipsoid; a is the major axis radius of the ellipsoid, and b is the minor axis radius of the ellipsoid;
determining the positions of two ends of the bar-shaped barrier and a gyration radius rho according to the outline of the bar-shaped barrier;
any point s on the obstacle 2 Can be expressed as: s 2 ＝u+λ 1 (q-u) wherein λ 1 Is a constant number 0<λ 1 <1,q and u are the position vectors of two axial end points of the bar-shaped barrier respectively;
the minimum distance between any point on the ellipsoid and the surface of the obstacle is as follows: min (Δ r) = min (| s) 1 s 2 | ρ), Δ r is the spatial minimum distance vector, and Δ r = s 2 -s 1 ρ is the radius of gyration of the obstacle;
by passingQuadratic programming and orthogonal decomposition to obtain min (Δ r) = min (v) T v) wherein v = Rx + Q T y, Q is a matrix with the column vector as unit length and orthogonal with each other, and R is a triangular matrix; vector y = u-s 0 ；
And (4) optimizing and solving to obtain:
wherein v is min Is the minimum value of v;
according to the minimum distance d under the current posture min The unmanned aerial vehicle carries out collision urgency evaluation, determines whether to adjust the flight trajectory route, specifically is:
minimum distance d at current attitude min Less than or equal to a set threshold distance d th Re-planning the path by an obstacle avoidance algorithm;
minimum distance d at current attitude min Less than the collision risk distance d haz Collision is avoided by reverse flight.
2. An evasion method for stick-type obstacles by unmanned aerial vehicle according to claim 1, wherein the minimum distance min (Δ r) between any point on the ellipsoid and the surface of the obstacle is defined by twice:
min(Δr)＝min(Ax+y) T (Ax+y)
wherein, the matrix
Vector y = u-s 0 Vector x = [ x ] 1 x 2 ] T And 0 is<x 1 <1，b＜＜x 2 ＜＜a。
3. An avoidance method of a stick type obstacle by an unmanned aerial vehicle according to claim 2, wherein the matrix A is orthogonally decomposed to obtain min (Δ r) = min (Rx + Q) T y) T (Rx+Q T y)。
4. An avoidance method of a rod-type obstacle by an unmanned aerial vehicle according to claim 1, wherein the detection device comprises a millimeter wave radar and an ultrasonic sensor for obstacle recognition and perception.
5. An avoidance method of a rod-shaped obstacle by an unmanned aerial vehicle according to claim 1, wherein the positions of the two ends of the rod-shaped obstacle and the turning radius p are determined according to the profile of the rod-shaped obstacle, specifically:
the outline of the rod-shaped barrier is composed of scattered point data measured by the detection equipment at each moment, and the scattered point data comprises the relative distance between the unmanned aerial vehicle and the rod-shaped barrier and the relative angle between the unmanned aerial vehicle and the rod-shaped barrier; and solving the gyration radius rho of the bar-shaped barrier according to the curve arc length and radian of the profile of the bar-shaped barrier, and determining the space positions of two axial end points of the barrier according to the height of the bar-shaped barrier measured by the detection equipment.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010631248.3A CN111781949B (en) | 2020-07-03 | 2020-07-03 | Method for avoiding rod-shaped obstacle by unmanned aerial vehicle |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010631248.3A CN111781949B (en) | 2020-07-03 | 2020-07-03 | Method for avoiding rod-shaped obstacle by unmanned aerial vehicle |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN111781949A CN111781949A (en) | 2020-10-16 |
| CN111781949B true CN111781949B (en) | 2022-11-18 |
Family
ID=72758873
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202010631248.3A Active CN111781949B (en) | 2020-07-03 | 2020-07-03 | Method for avoiding rod-shaped obstacle by unmanned aerial vehicle |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN111781949B (en) |
Families Citing this family (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113297739A (en) * | 2021-05-25 | 2021-08-24 | 深圳市边界智控科技有限公司 | Geometric flight corridor generation method and device and related components thereof |
| CN114047788B (en) * | 2022-01-11 | 2022-04-15 | 南京南机智农农机科技研究院有限公司 | Automatic keep away mooring unmanned aerial vehicle of barrier and follow car system |
| CN115337430B (en) * | 2022-08-11 | 2024-06-07 | 深圳市隆瑞科技有限公司 | Control method and device for spraying trolley |
| CN115616578A (en) * | 2022-12-05 | 2023-01-17 | 成都航空职业技术学院 | Radar detection method and device for unmanned aerial vehicle |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106598070A (en) * | 2016-12-14 | 2017-04-26 | 东北农业大学 | Obstacle avoiding methods for agricultural plant protection unmanned aerial vehicles under multiple obstacles and small obstacles in spraying process and unmanned aerial vehicle |
| CN107703519A (en) * | 2017-10-17 | 2018-02-16 | 杭州瓦屋科技有限公司 | Plant protection unmanned plane obstacle detection method based on laser displacement sensor |
| CN107831777A (en) * | 2017-09-26 | 2018-03-23 | 中国科学院长春光学精密机械与物理研究所 | A kind of aircraft automatic obstacle avoiding system, method and aircraft |
| CN107885224A (en) * | 2017-11-06 | 2018-04-06 | 北京韦加无人机科技股份有限公司 | Unmanned plane barrier-avoiding method based on tri-item stereo vision |
| CN110456792A (en) * | 2019-08-06 | 2019-11-15 | 清华大学 | The navigation of multiple agent group's system and barrier-avoiding method and device under dynamic environment |
-
2020
- 2020-07-03 CN CN202010631248.3A patent/CN111781949B/en active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106598070A (en) * | 2016-12-14 | 2017-04-26 | 东北农业大学 | Obstacle avoiding methods for agricultural plant protection unmanned aerial vehicles under multiple obstacles and small obstacles in spraying process and unmanned aerial vehicle |
| CN107831777A (en) * | 2017-09-26 | 2018-03-23 | 中国科学院长春光学精密机械与物理研究所 | A kind of aircraft automatic obstacle avoiding system, method and aircraft |
| CN107703519A (en) * | 2017-10-17 | 2018-02-16 | 杭州瓦屋科技有限公司 | Plant protection unmanned plane obstacle detection method based on laser displacement sensor |
| CN107885224A (en) * | 2017-11-06 | 2018-04-06 | 北京韦加无人机科技股份有限公司 | Unmanned plane barrier-avoiding method based on tri-item stereo vision |
| CN110456792A (en) * | 2019-08-06 | 2019-11-15 | 清华大学 | The navigation of multiple agent group's system and barrier-avoiding method and device under dynamic environment |
Also Published As
| Publication number | Publication date |
|---|---|
| CN111781949A (en) | 2020-10-16 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111781949B (en) | Method for avoiding rod-shaped obstacle by unmanned aerial vehicle | |
| CN107561547B (en) | Method, device and system for measuring distance from power transmission line to target object | |
| CN206057974U (en) | A kind of obstacle avoidance system applied on rotor wing unmanned aerial vehicle | |
| WO2022007859A1 (en) | Lidar parameter adjustment method, apparatus, and lidar | |
| CN109872352B (en) | Automatic registration method for power line inspection LiDAR data based on tower feature points | |
| CN110456343A (en) | A kind of instant localization method and system based on FMCW millimetre-wave radar | |
| CN107479059A (en) | A kind of overhead line based on unmanned plane and vegetation distance-measuring device and method | |
| CN107818330B (en) | System and method for spatial filtering using data having widely different error magnitudes | |
| CN107783133B (en) | Anti-collision system and anti-collision method for fixed-wing unmanned aerial vehicle of millimeter wave radar | |
| CN104298248A (en) | Accurate visual positioning and orienting method for rotor wing unmanned aerial vehicle | |
| CN112230676B (en) | Unmanned aerial vehicle flight line correction system, method and related device | |
| CN105652255B (en) | The spatial registration method of Radar Network System | |
| WO2023024669A1 (en) | Flight anti-collision method and apparatus based on electromagnetic field detection of overhead transmission line | |
| CN107783547A (en) | Post disaster relief rotor wing unmanned aerial vehicle obstacle avoidance system and method | |
| Schouten et al. | RadarSLAM: Biomimetic SLAM using ultra-wideband pulse-echo radar | |
| CN107783128B (en) | Multi-target anti-collision system of fixed-wing unmanned aerial vehicle based on millimeter wave radar | |
| Bian et al. | A point-line-based SLAM framework for UAV close proximity transmission tower inspection | |
| CN101923158B (en) | System and method for Gaussian decomposition of weather radar data for communication | |
| CN114910918A (en) | Positioning method and device, radar device, unmanned aerial vehicle system, controller and medium | |
| CN112823353A (en) | Object localization using machine learning | |
| Kan et al. | Contour based path planning with B-spline trajectory generation for unmanned aerial vehicles (UAVs) over hostile terrain | |
| CN107783090B (en) | Millimeter wave radar-based radar signal processing method for collision avoidance system of fixed-wing unmanned aerial vehicle | |
| CN103176477A (en) | Airborne SAR (synthetic aperture radar) flying route arrangement method based on dynamic wind speed and direction adjustment | |
| US11754698B2 (en) | Radar based system for processing radar data representing range and radial velocity of a detected object | |
| US20210199798A1 (en) | Continuous wave radar terrain prediction method, device, system, and unmanned aerial vehicle |
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 |