CN115755951A - Unmanned aerial vehicle obstacle avoidance method for quickly recovering flight path - Google Patents
Unmanned aerial vehicle obstacle avoidance method for quickly recovering flight path Download PDFInfo
- Publication number
- CN115755951A CN115755951A CN202211183901.XA CN202211183901A CN115755951A CN 115755951 A CN115755951 A CN 115755951A CN 202211183901 A CN202211183901 A CN 202211183901A CN 115755951 A CN115755951 A CN 115755951A
- Authority
- CN
- China
- Prior art keywords
- point
- track
- unmanned aerial
- aerial vehicle
- obstacle
- 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.)
- Pending
Links
Images
Classifications
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Landscapes
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The invention discloses an unmanned aerial vehicle obstacle avoidance method for quickly recovering a track, which mainly solves the problems of low track recovery speed and poor recovery effect of the existing unmanned aerial vehicle obstacle avoidance method. The scheme is as follows: 1) Converting the known starting point, the target point and the obstacle from a global coordinate system to a local coordinate system; 2) Judging the turning direction of the unmanned aerial vehicle in a local coordinate system; 3) According to the minimum turning radius of the unmanned aerial vehicle and the radius of the obstacle, enlarging the radius of the obstacle; 4) Calculating key track point coordinates of the unmanned aerial vehicle obstacle avoidance under the local coordinate system according to the geometric relation between the unmanned aerial vehicle and the obstacle; 5) And transforming the coordinates of the key track points in the local coordinate system to the global coordinate system, and enabling the unmanned aerial vehicle to sequentially fly over all the key track points around the obstacle to finish obstacle avoidance and rapid track recovery. The invention greatly shortens the track length in the obstacle avoidance stage, improves the track recovery speed of the unmanned aerial vehicle after bypassing the obstacle, and can be used for unmanned aerial vehicle cooperative reconnaissance, fixed point striking and communication relay.
Description
Technical Field
The invention belongs to the field of unmanned aerial vehicle track planning, and particularly relates to an unmanned aerial vehicle obstacle avoidance method which can be used for unmanned aerial vehicle cooperative reconnaissance, fixed-point striking and communication relay.
Background
At present, an unmanned aerial vehicle has the advantages of low power consumption, light weight, small size, strong maneuverability and low cost, and is widely applied to scenes such as cooperative reconnaissance, fixed-point striking, communication relay and the like. In a plurality of task scenes, the unmanned aerial vehicle and some obstacles such as a no-fly area, a mountain and the like share the same airspace, so that the unmanned aerial vehicle is very likely to collide with the obstacles in the task area in the task execution process, and the safety problem of the unmanned aerial vehicle is threatened. Therefore, when planning the flight path from the starting point to the target point of the unmanned aerial vehicle, whether the obstacle existing in the environment affects the flight path of the unmanned aerial vehicle or not must be considered, and then planning the obstacle avoidance flight path of the unmanned aerial vehicle. The unmanned plane obstacle avoidance means that an ideal flight path for avoiding collision is planned by changing the flight state of the unmanned plane. In a reconnaissance scene, the unmanned aerial vehicle can carry out full-coverage cruise on a task area along a set off-line track when executing a reconnaissance task, and obstacle avoidance needs to be carried out when encountering an obstacle, so that the unmanned aerial vehicle obstacle avoidance needs to be considered in the aspect of obstacle avoidance planning, and the unmanned aerial vehicle needs to return to the original off-line track as soon as possible to continue cruise after the obstacle avoidance is completed. How to improve unmanned aerial vehicle's track speed of resuming, shorten the whole track length of keeping away the barrier stage and be vital to improving unmanned aerial vehicle to the coverage of task area.
A representative unmanned aerial vehicle obstacle avoidance method mainly comprises the following steps: the method comprises the steps of obstacle avoidance based on an artificial potential field, an obstacle avoidance based on an RRT algorithm, obstacle avoidance based on a course control law and a track recovery method. The obstacle avoidance method based on the artificial potential field is used for guiding the unmanned aerial vehicle to avoid the obstacle and finally reach a target point according to the direction of resultant force by establishing the virtual potential field; the obstacle avoidance method based on the RRT algorithm has randomness when searching for a path, and randomly generates track points meeting the condition of avoiding obstacles, so that the obstacle avoidance of the unmanned aerial vehicle is realized; the obstacle avoidance and track recovery method based on the course control law considers the problem of track recovery, and calculates key track points in the obstacle avoidance process of the unmanned aerial vehicle according to the course control law, so that the unmanned aerial vehicle avoids obstacles and realizes track recovery.
Guo Jiaorong in his master thesis "multi-unmanned plane real-time air route planning algorithm research and implementation" (west 'an: west' electronic technology university, 2018), it provides an unmanned plane obstacle avoidance and track recovery method based on course control law. The obstacle avoidance method comprises the steps of modeling an obstacle as a circle of a two-dimensional plane, describing the circle by the circle center and the radius, judging the obstacle avoidance turning direction of the unmanned aerial vehicle by using a vector cross-multiplication method, calculating the turning angle by using a point turning method, calculating key track points on an obstacle avoidance path, obtaining coordinates of each key track point in a track recovery stage according to a symmetrical relation, and finally obtaining the complete unmanned aerial vehicle obstacle avoidance and track recovery track. The method has the defects that the track point of the unmanned aerial vehicle, which starts to avoid the obstacle, is too far away from the obstacle, and the speed of recovering the track is slow, so that the length of the track of the whole obstacle avoidance section is too long.
Disclosure of Invention
The invention aims to provide an unmanned aerial vehicle obstacle avoidance method for quickly recovering a flight path aiming at the defects of the prior art, so as to accelerate the flight path recovery speed of the unmanned aerial vehicle after the unmanned aerial vehicle bypasses the obstacle and shorten the flight path of the unmanned aerial vehicle in the whole obstacle avoidance stage.
The technical scheme of the invention is as follows: fully consider the environmental constraint and the maneuvering performance constraint of unmanned aerial vehicle in the obstacle avoidance process, namely the initial point S, the target point G, the circle center coordinate of the obstacle P and the obstacle radius R, and the minimum turning radius R of the unmanned aerial vehicle min Flight speed v, minimum track length l min And converting the coordinates of the starting point, the target point and the obstacle into a local coordinate system from a global coordinate system, judging the turning direction of the obstacle avoidance of the unmanned aerial vehicle, calculating key track points in the obstacle avoidance process according to the geometric relationship between the unmanned aerial vehicle and the obstacle, and converting all the obtained track point coordinates into the global coordinate system again to obtain an obstacle avoidance track which is smooth and flyable and has high track recovery speed. The implementation steps comprise:
(1) The known starting point S (x) S ,y S ) Target point G (x) G ,y G ) Obstacle P (x) P ,y P ) Conversion from the global coordinate system into the local coordinate system:
(1a) Starting point S (x) S ,y S ) As the origin of the local coordinate system, translating the global coordinate system to enable the origin of the global coordinate system to coincide with the starting point S;
(1b) From the starting point S (x) S ,y S ) And target point (x) G ,y G ) Determining a vector pointing from the starting point S to the target point G
(1c) Will vector
Setting an included angle with the positive direction of the x axis of the global coordinate system as theta, and rotating the global coordinate system in the anticlockwise direction by theta to ensure that the positive direction of the x axis and the vector are
Overlapping to obtain a local coordinate system;
(1d) Calculating a coordinate point S ' (x ') of the starting point S, the target point G and the obstacle P in the local coordinate system ' S ,y' S )、G'(x' G ,y' G )、P'(x' P ,y' P )；
(2) Judging the turning direction of the unmanned aerial vehicle in a local coordinate system:
if the longitudinal coordinate value of the center of the circle of the obstacle is larger than 0, judging that the center of the circle is positioned on the left side of the straight track, and enabling the unmanned aerial vehicle to bypass the obstacle from the right side to avoid the obstacle;
if the longitudinal coordinate value of the center of the circle of the obstacle is less than or equal to 0, judging that the center of the circle is positioned on the right side of the linear track or on the linear track, and enabling the unmanned aerial vehicle to bypass the obstacle from the left side to avoid the obstacle;
(3) And (3) processing the radius of the obstacle:
let the minimum turning radius of the unmanned aerial vehicle be R min And comparing it with the sum of the obstacle radius r and half w of the drone width:
if R is min R + w is less than or equal to, and the minimum turning radius constraint of the unmanned aerial vehicle is met, and then the radius r + is adoptedw planning an unmanned aerial vehicle obstacle avoidance track;
if R is min R + w, which does not satisfy the minimum turning radius constraint of the unmanned aerial vehicle, then the radius R of the obstacle is enlarged to R min W according to radius R min Planning an obstacle avoidance track of the unmanned aerial vehicle;
(4) Calculating the key track point coordinates of the unmanned aerial vehicle obstacle avoidance under the local coordinate system:
(4a) The integral process from starting obstacle avoidance to returning to the original track of the unmanned aerial vehicle is divided into three stages: an obstacle avoidance stage, a flight winding stage and a track recovery stage;
(4b) Calculating track points of unmanned aerial vehicle obstacle avoidance in each stage:
in the flying stage, a waypoint C ' (x ') is obtained by a dichotomy when the unmanned aerial vehicle finishes flying straight and starts flying around the obstacle ' C ,y' C ) And calculating a track point D' when the flight winding end starts to fly straight:
(x' D ,y' D )＝(2x' P -x' C ,y' C )，
wherein (x' D ,y' D ) The coordinates of the track point D' are represented;
in the obstacle avoidance stage, calculating a track point A 'when the unmanned aerial vehicle starts to avoid the obstacle and turns and a track point B' when the unmanned aerial vehicle finishes turning and starts to fly straight:
(x' B ,y' B )＝(x' C -l min ·cosα,y' C +l min ·sinα)
wherein, (x' A ,y' A ) Coordinates representing track Point A ' (' x ' B ,y' B ) Coordinates, l, representing track points B min Represents a minimum track segment length constraint for the drone, (x' H ,y' H ) The coordinate of an intersection point H 'of a straight line which is perpendicular to the passing point C' and the P 'C' and the x axis is represented, and alpha represents an included angle between the H 'C' and the x axis in the positive direction;
in the track recovery stage, calculating a track point E 'when the unmanned aerial vehicle directly flies, starts turning and prepares to return to the original track, and a track point F' when the unmanned aerial vehicle finishes turning and returns to the original track:
(x' E ,y' E )＝(2x' P -x' B ,y' B )
(x' F ,y' F )＝(2x' P -x' A ,y' A )
wherein (x' E ,y' E ) Coordinates representing track Point E '(x' F ,y' F ) Representing the coordinates of the track point F';
(5) Transforming the coordinates of the key track points A ', B', C ', D', E 'and F' in the local coordinate system calculated in the step (4B) to a global coordinate system, and correspondingly obtaining the coordinates of the track points A, B, C, D, E, F;
(6) The unmanned aerial vehicle sequentially flies through the track points A, B, C, D, E, F to complete obstacle avoidance and rapid track recovery.
Compared with the prior art, the invention has the following advantages:
1. when the turning direction of the unmanned aerial vehicle is judged, the turning direction of the unmanned aerial vehicle can be determined only by comparing whether the ordinate of the circle center of the obstacle under the local coordinate system is larger than 0, and compared with the existing complex method for judging by adopting vector cross multiplication for obstacle avoidance and track recovery of the unmanned aerial vehicle based on the course control law, the method is not only free from calculation, but also simple and visual.
2. According to the method, by comprehensively considering the obstacle constraint in the environment and the self maneuvering performance constraint of the unmanned aerial vehicle, the key track point in the obstacle avoidance process is calculated by using the geometric relationship between the unmanned aerial vehicle and the obstacle, so that the starting obstacle avoidance point and the track recovery point are closer to the obstacle, the track length in the obstacle avoidance stage is greatly shortened, and the track recovery speed of the unmanned aerial vehicle after the obstacle is bypassed is improved.
Drawings
FIG. 1 is a flow chart of an implementation of the present invention;
FIG. 2 is a schematic diagram of a rotating coordinate system according to the present invention;
FIG. 3 is a schematic diagram of determining a turning direction of an unmanned aerial vehicle according to the present invention;
FIG. 4 is a schematic diagram of obstacle avoidance and track recovery of the UAV of the present invention;
FIG. 5 is a schematic diagram of solving coordinates of a key track point by using a dichotomy method in the invention;
FIG. 6 is an obstacle avoidance track graph planned by the present invention and the existing obstacle avoidance method based on course control law, respectively;
fig. 7 is a comparison graph of the distance between the initial obstacle avoidance point and the obstacle, the distance between the track recovery point and the obstacle, and the track length in the obstacle avoidance phase in the two obstacle avoidance tracks in fig. 6.
Detailed Description
Embodiments and effects of the present invention will be described in further detail below with reference to the accompanying drawings.
Referring to fig. 1, the implementation steps for the example are as follows:
and step 1, coordinate conversion.
1.1 Obtaining starting point S (x) of unmanned plane S ,y S ) Target point G (x) G ,y G ) Obstacle P (x) P ,y P ) These known coordinates;
1.2 Will start point S (x) S ,y S ) As the origin of the local coordinate system, translating the global coordinate system to enable the origin of the global coordinate system to coincide with the starting point S;
1.3 From a starting point S (x) S ,y S ) And target point G (x) G ,y G ) Determining a vector pointing from the starting point S to the target point G
1.4 Will vector vectors
Setting an included angle with the positive direction of the x axis of the global coordinate system as theta, and rotating the global coordinate system in the anticlockwise direction by theta to enable the positive direction of the x axis and the vector
Overlapping to obtain a local coordinate system;
1.5 Calculating a coordinate point S ' (x ') of the start point S, the target point G and the obstacle P in the local coordinate system ' S ,y' S )、G'(x' G ,y' G )、P'(x' P ,y' P ):
Referring to FIG. 2, in this step, a point S ' (x ') is calculated in a local coordinate system ' S ,y' S )、G'(x' G ,y' G )、P'(x' P ,y' P ) The coordinates of (a) are as follows:
wherein d is 1 Representing points S to vectors
Distance of point T above, d 2 Representing vectors
Distance of point T to point U, d 3 Represents the distance from point P to point V, d 4 Representing the distance from point U to point V.
And 2, judging the turning direction of the unmanned aerial vehicle in the local coordinate system.
The step is to judge according to the longitudinal coordinate value of the circle center of the obstacle:
if the longitudinal coordinate value of the center of the circle of the obstacle is larger than 0, judging that the center of the circle is positioned on the left side of the straight track, and enabling the unmanned aerial vehicle to bypass the obstacle from the right side to avoid the obstacle;
if the longitudinal coordinate value of the center of the circle of the obstacle is less than or equal to 0, judging that the center of the circle is positioned on the right side of the straight track or on the straight track, and enabling the unmanned aerial vehicle to bypass the obstacle from the left side to avoid the obstacle;
as shown in fig. 3, in the local coordinate system, the center P of the obstacle circle 1 If the ordinate value is greater than 0, thenJudging that the circle center is positioned on the left side of the linear track, and leading the unmanned aerial vehicle to follow the right side along the edge l 1 Obstacle avoidance is carried out; center P of obstacle 2 If the longitudinal coordinate value is less than 0, the center P of the circle is determined 2 On the right side of the straight track, the unmanned aerial vehicle follows the left side along l 2 And (6) avoiding obstacles.
And 3, processing the radius of the obstacle.
Let the minimum turning radius of the unmanned aerial vehicle be R min And comparing it with the sum of the obstacle radius r and half w of the drone width:
if R is min R + w is less than or equal to, the minimum turning radius constraint of the unmanned aerial vehicle is met, and an obstacle avoidance track of the unmanned aerial vehicle is planned according to the radius r + w;
if R is min R + w, which does not satisfy the minimum turning radius constraint of the unmanned plane, then the obstacle radius R is enlarged to R min W according to radius R min And planning an unmanned plane obstacle avoidance track.
And 4, calculating the key track point coordinates of the unmanned aerial vehicle obstacle avoidance under the local coordinate system.
4.1 The whole process from the beginning of obstacle avoidance to the returning of the unmanned aerial vehicle to the original flight path is divided into three stages: an obstacle avoidance stage, a flight winding stage and a track recovery stage;
as shown in fig. 4, a point a 'and a point F' are an obstacle avoidance starting point and a track recovery point, respectively, a process from the point a 'to the point C' via the point B 'is referred to as an obstacle avoidance phase, a process from the point C' to the point D 'is referred to as an unmanned aerial vehicle flight around phase, and a process from the point D' to the point F 'via the point E' is referred to as a track recovery phase.
Wherein, the arc line
And arc line
Is represented by R min Is a radial arc section track, B ' C ' and D ' E ' are straight line section tracks, and are tangent to a circle which takes a point P ' as the center of circle and r + w as the radius, and an arc line
To get out of the wayThe central point P' of the obstacle is the center of the circle, and r + w is the arc track of the radius.
4.2 Calculating track points of obstacle avoidance of the unmanned aerial vehicle at each stage;
4.2.1 In the flying stage, a waypoint C ' (x ') is obtained by dichotomy when the unmanned aerial vehicle finishes flying straight and starts flying around the obstacle ' C ,y' C )：
Referring to fig. 5, the specific implementation of this step is as follows:
first, the drone starts from point a' along an arc
Using point J' as the center of circle, R min Turning for a radius, wherein a point Q ' is positioned on a line segment C ' H ', and J ' Q ' is vertical to the line segment C ' H '; setting point M '(x' M ,y' M ) A point N '(x') is set as a point closer to the origin of two intersection points of a circle with the circle center being P 'and the radius being r + w' N ,y' N ) The center point of a minor arc generated by intersecting a circle with P' as the center of a circle and r + w as the radius with the x axis is calculated as follows:
(x' N ,y' N )＝(x' P ,y' P -r-w)；
then, the bisection method is adopted to form an arc line
Dot is continuously taken as point C '(x' C ,y' C ) The perpendicular line passing through the point C 'as P' C 'is crossed with the x axis at a point H', the size of & lt M 'H' C 'is recorded as alpha, and the size of & lt M' H 'C' is recorded along a vector
Taking a point B ' on the line segment C ' H ' causes B ' C ' = l min H 'B' = H 'C' -B 'C', the coordinates of point H 'and point B' are calculated:
(x' B ,y' B )＝(x' C -l min ·cosα,y' C +l min ·sinα)
wherein l min Representing a minimum track segment length constraint for the drone;
then, H 'B'/R min And H 'Q'/R min And (3) comparison:
if it is
FIG. 5 (a) shows that point B ' is located on line Q ' C ', Q ' C ' > l min The optimal solution for point C' should lie on the arc
Up, then continue in the arc line
Searching for an optimal C' point by utilizing a bisection method;
if it is
FIG. 5 (B) shows that the point B ' is located on the line segment H ' Q ', and Q ' C ' < l min The optimal solution for point C' should lie on the arc
Up, then continue in the arc line
Searching for an optimal C' point by utilizing a bisection method;
if it is
When point B ' coincides with point Q ', the division by two is stopped, and point C ' (x ') at this time ' C ,y' C ) The optimal solution is obtained;
then, after obtaining the coordinates of the point C ', a course point D' when the flight is started straight around the end of the flight is calculated:
(x' D ,y' D )＝(2x' P -x' C ,y' C )
wherein, (x' D ,y' D ) The coordinates of the track point D' are represented;
4.2.2 In the obstacle avoidance stage, coordinates of a track point A 'when the unmanned aerial vehicle starts to avoid the obstacle and turns and coordinates of a track point B' when the unmanned aerial vehicle finishes turning and starts to fly straight are calculated:
(x' B ,y' B )＝(x' C -l min ·cosα,y' C +l min ·sinα)
wherein (x' A ,y' A ) Coordinates representing track Point A ' (' x ' B ,y' B ) Denotes the coordinates of track point B ' (' x ' H ,y' H ) The coordinate of an intersection point H 'of a straight line which is perpendicular to the passing point C' and the P 'C' and the x axis is represented, and alpha represents the positive included angle of the H 'C' and the x axis;
4.2.3 In a track recovery stage, calculating a track point E 'when the unmanned aerial vehicle directly flies, starts turning and prepares to return to the original track, and a track point F' when the unmanned aerial vehicle finishes turning and returns to the original track:
(x' E ,y' E )＝(2x' P -x' B ,y' B )
(x' F ,y' F )＝(2x' P -x' A ,y' A )
wherein (x' E ,y' E ) Coordinates representing track Point E '(x' F ,y' F ) The coordinates of the track point F' are represented.
And 5, transforming the coordinates of the key track points A ', B', C ', D', E 'and F' in the local coordinate system calculated in the step 4 into a global coordinate system to correspondingly obtain the coordinates (x) of the track point A, B, C, D, E, F A ,y A )、(x B ,y B )、(x C ,y C )、(x D ,y D )、(x E ,y E )、(x F ,y F )：
(x A ,y A )＝(x' A cosθ-y' A sinθ+x S ,y' A cosθ+x' A sinθ+y S )，
(x B ,y B )＝(x' B cosθ-y' B sinθ+x S ,y' B cosθ+x' B sinθ+y S )，
(x C ,y C )＝(x' C cosθ-y' C sinθ+x S ,y' C cosθ+x' C sinθ+y S )，
(x D ,y D )＝(x' D cosθ-y' D sinθ+x S ,y' D cosθ+x' D sinθ+y S )，
(x E ,y E )＝(x' E cosθ-y' E sinθ+x S ,y' E cosθ+x' E sinθ+y S )，
(x F ,y F )＝(x' F cosθ-y' F sinθ+x S ,y' F cosθ+x' F sinθ+y S )。
And 6, sequentially flying the unmanned aerial vehicle over the track points A, B, C, D, E, F to finish obstacle avoidance and rapid track recovery.
The effect of the invention is further explained by combining simulation experiments as follows:
1. setting simulation parameters:
the flight environment and the self maneuvering performance simulation parameters of the unmanned aerial vehicle are set as shown in table 1:
TABLE 1 simulation parameter setting of flight environment and self-maneuvering performance of unmanned aerial vehicle
| Parameter(s) | Value taking |
| Task scene size | 3300m×3300m |
| Obstacle P 1 Circle center coordinate | (900，1000)m |
| Obstacle P 1 Radius r 1 | 300m |
| Obstacle P 2 Circle center coordinate | (2500，2400)m |
| Obstacle P 2 Radius r 2 | 300m |
| Unmanned aerial vehicle initial point S coordinate | (200，100)m |
| Unmanned aerial vehicle target point G coordinate | (3200，3100)m |
| Velocity v of unmanned aerial vehicle | 20m/s |
| Minimum turning radius R of unmanned aerial vehicle min | 100m |
| Minimum flight path length l of unmanned aerial vehicle min | 200m |
| Unmanned aerial vehicle width 2w | 40m |
2. Simulation content and result analysis thereof:
As can be seen in fig. 6, due to the obstacle P 1 Above the straight track, both methods therefore choose to avoid the obstacle from below it around its minor arc, while the obstacle P 2 Just above the straight track, both methods choose to bypass over the obstacle.
And 2, for the unmanned aerial vehicle obstacle avoidance and track recovery method based on the course control law, the three aspects of the distance from the obstacle avoidance point to the obstacle, the distance from the track recovery point to the obstacle and the total track length in the obstacle avoidance stage are evaluated and compared, and the result is shown in fig. 7. Wherein, aiming at two obstacles P 1 And P 2 The distance from the obstacle-avoiding point to the obstacle is respectively A 1 K 1 And A 1 ’K 1 、A 2 K 2 And A 2 ’K 2 The distances from the track recovery point to the obstacle are respectively represented by F 1 T 1 And F 1 ’T 1 、F 2 T 2 And F 2 ’T 2 The total track length of the obstacle avoidance stage is respectively represented by A 1 F 1 And A 1 ’F 1 ’、A 2 F 2 And A 2 ’F 2 ' means.
As can be seen from fig. 7, for the obstacle P 1 Compared with the comparison method, the method shortens the total track length of the obstacle avoidance stage by nearly 1000m; the method shortens the distance from the obstacle avoidance point to the obstacle and the distance from the track recovery point to the obstacle by nearly 500m. For obstacle P 2 The three distances are shortened by about 716m, 363m, respectively.
The simulation result shows that the unmanned aerial vehicle obstacle avoidance method for rapidly recovering the flight path provided by the invention can enable the starting obstacle avoidance point and the flight path recovery point to be closer to the obstacle, greatly shorten the flight path length in the obstacle avoidance stage and improve the flight path recovery speed of the unmanned aerial vehicle after the unmanned aerial vehicle bypasses the obstacle.
Claims (4)
1. An unmanned aerial vehicle obstacle avoidance method for quickly recovering a flight path is characterized by comprising the following steps:
(1) The known starting point S (x) S ,y S ) Target point G (x) G ,y G ) Obstacle P (x) P ,y P ) Conversion from the global coordinate system into the local coordinate system:
(1a) Starting point S (x) S ,y S ) As the origin of the local coordinate system, translating the global coordinate system to enable the origin of the global coordinate system to coincide with the starting point S;
(1b) From the starting point S (x) S ,y S ) And target Point (x) G ,y G ) Determining a vector pointing from the starting point S to the target point G
(1c) Will vector
Setting an included angle with the positive direction of the x axis of the global coordinate system as theta, and rotating the global coordinate system in the anticlockwise direction by theta to ensure that the positive direction of the x axis and the vector are
Overlapping to obtain a local coordinate system;
(1d) Calculating a coordinate point S ' (x ') of the starting point S, the target point G and the obstacle P in the local coordinate system ' S ,y' S )、G'(x' G ,y' G )、P'(x' P ,y' P )；
(2) Judging the turning direction of the unmanned aerial vehicle in a local coordinate system:
if the longitudinal coordinate value of the center of the circle of the obstacle is larger than 0, judging that the center of the circle is positioned on the left side of the straight track, and enabling the unmanned aerial vehicle to bypass the obstacle from the right side to avoid the obstacle;
if the longitudinal coordinate value of the center of the circle of the obstacle is less than or equal to 0, judging that the center of the circle is positioned on the right side of the linear track or on the linear track, and enabling the unmanned aerial vehicle to bypass the obstacle from the left side to avoid the obstacle;
(3) And (3) processing the radius of the obstacle:
let the minimum turning radius of the unmanned aerial vehicle be R min And comparing it with the sum of the obstacle radius r and half w of the drone width:
if R is min R + w is less than or equal to, the minimum turning radius constraint of the unmanned aerial vehicle is met, and an obstacle avoidance track of the unmanned aerial vehicle is planned according to the radius r + w;
if R is min R + w, which does not satisfy the minimum turning radius constraint of the unmanned aerial vehicle, then the radius R of the obstacle is enlarged to R min W according to radius R min Planning an obstacle avoidance track of the unmanned aerial vehicle;
(4) Calculating the key track point coordinates of the unmanned aerial vehicle obstacle avoidance under the local coordinate system:
(4a) The integral process from starting obstacle avoidance to returning to the original track of the unmanned aerial vehicle is divided into three stages: an obstacle avoidance stage, a flight winding stage and a track recovery stage;
(4b) Calculating track points of unmanned aerial vehicles in each stage for avoiding obstacles:
in the flying stage, a waypoint C ' (x ') is obtained by a dichotomy when the unmanned aerial vehicle finishes flying straight and starts flying around the obstacle ' C ,y' C ) And calculating a track point D' when the flight winding end starts to fly straight:
(x' D ,y' D )＝(2x' P -x' C ,y' C )，
wherein (x' D ,y' D ) The coordinates of the track point D' are represented;
in the obstacle avoidance stage, calculating a track point A 'when the unmanned aerial vehicle starts to avoid the obstacle and turns and a track point B' when the unmanned aerial vehicle finishes turning and starts to fly straight:
(x' B ,y' B )＝(x' C -l min ·cosα,y' C +l min ·sinα)
wherein (x' A ,y' A ) Coordinates representing track Point A ' (' x ' B ,y' B ) Coordinates, l, representing track points B min Represents a minimum track segment length constraint for the drone, (x' H ,y' H ) The coordinate of an intersection point H 'of a straight line which is perpendicular to the passing point C' and the P 'C' and the x axis is represented, and alpha represents the positive included angle of the H 'C' and the x axis;
in the track recovery stage, calculating a track point E 'when the unmanned aerial vehicle directly flies, starts turning and prepares to return to the original track, and a track point F' when the unmanned aerial vehicle finishes turning and returns to the original track:
(x' E ,y' E )＝(2x' P -x' B ,y' B )
(x' F ,y' F )＝(2x' P -x' A ,y' A )
wherein, (x' E ,y' E ) Denotes the coordinates of track point E '(x' F ,y' F ) The coordinates of the track point F' are represented;
(5) Transforming the coordinates of the key track points A ', B', C ', D', E 'and F' in the local coordinate system calculated in the step (4B) to a global coordinate system to correspondingly obtain the coordinates of the track point A, B, C, D, E, F;
(6) The unmanned aerial vehicle sequentially flies through the track points A, B, C, D, E, F to complete obstacle avoidance and rapid track recovery.
2. The method of claim 1, wherein: in step (1 d), a coordinate point S ' (x ') of the point S, G, P in the global coordinate system in the local coordinate system is calculated ' S ,y' S )、G'(x' G ,y' G )、P'(x' P ,y' P ) The formula is as follows:
3. the method of claim 1, wherein: in the step (4 b), in the flying around stage, a waypoint C ' (x ') of the unmanned aerial vehicle when the unmanned aerial vehicle finishes flying straight and starts flying around the obstacle is obtained by using a dichotomy ' C ,y' C ) The implementation is as follows:
(4b1) Suppose that the obstacle P in the global coordinate system is converted to the coordinate P ' (x ') in the local coordinate system ' P ,y' P ) The point closer to the origin of two intersection points generated by intersecting a straight track with a circle having a circle center of P ' and a radius of r + w is set as a point M ' (x ' M ,y' M ) The midpoint of the minor arc resulting from the intersection is denoted as point N '(x' N ,y' N ) The two point coordinates are respectively expressed as follows:
the M' point coordinates are:
n 'point coordinate is according to y' P Whether or not greater than 0 is determined in two cases:
if y' P If > 0, the coordinates of point N' are: (x' N ,y' N )＝(x' P ,y' P -r-w)
If y' P And if the coordinate of the point N 'is less than or equal to 0, the coordinate of the point N' is as follows: (x' N ,y' N )＝(x' P ,y' P +r+w)
(4b2) Binary method for solving track point C '(x' C ,y' C )：
By bisection in an arc
Dot is continuously extracted as C '(x' C ,y' C ) The perpendicular line passing through the point C 'as P' C 'is crossed with the x axis at a point H', the size of & lt M 'H' C 'is recorded as alpha, and the size of & lt M' H 'C' is recorded along a vector
Taking a point B ' on the line segment C ' H ' causes B ' C ' = l min H ' B ' = C ' H ' -B ' C ', and the coordinates (x ' of the point H ' are calculated, respectively ' H ,y' H ) And the coordinates (x ') of point B' B ,y' B )：
(x' B ,y' B )＝(x' C -l min ·cosα,y' C +l min ·sinα)；
Let the unmanned plane start from point A' along an arc
Using the point J' as the center, R min Making a turn for a radius, with point Q ' on line segment C ' H ', with J ' Q ' perpendicular to C ' H ', with H ' B '/R min And H 'Q'/R min And (3) comparison:
if it is
Continue on the arc line
Searching for an optimal C' point by utilizing a bisection method;
if it is
Continue on the arc line
Searching for an optimal C' point by utilizing a bisection method;
if it is
Stop bisection, point C ' (x ') at this time ' C ,y' C ) I.e. the optimal solution.
4. The method of claim 1, wherein: in the step (5), the coordinates of the key track points A ', B', C ', D', E 'and F' in the local coordinate system are transformed to the global coordinate system, and the coordinates (x) of the track point A, B, C, D, E, F are correspondingly obtained A ,y A )、(x B ,y B )、(x C ,y C )、(x D ,y D )、(x E ,y E )、(x F ,y F ) The formula is as follows:
(x A ,y A )＝(x' A cosθ-y' A sinθ+x S ,y' A cosθ+x' A sinθ+y S )
(x B ,y B )＝(x' B cosθ-y' B sinθ+x S ,y' B cosθ+x' B sinθ+y S )
(x C ,y C )＝(x' C cosθ-y' C sinθ+x S ,y' C cosθ+x' C sinθ+y S )
(x D ,y D )＝(x' D cosθ-y' D sinθ+x S ,y' D cosθ+x' D sinθ+y S )
(x E ,y E )＝(x' E cosθ-y' E sinθ+x S ,y' E cosθ+x' E sinθ+y S )
(x F ,y F )＝(x' F cosθ-y' F sinθ+x S ,y' F cosθ+x' F sinθ+y S )。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211183901.XA CN115755951A (en) | 2022-09-27 | 2022-09-27 | Unmanned aerial vehicle obstacle avoidance method for quickly recovering flight path |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211183901.XA CN115755951A (en) | 2022-09-27 | 2022-09-27 | Unmanned aerial vehicle obstacle avoidance method for quickly recovering flight path |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN115755951A true CN115755951A (en) | 2023-03-07 |
Family
ID=85350360
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202211183901.XA Pending CN115755951A (en) | 2022-09-27 | 2022-09-27 | Unmanned aerial vehicle obstacle avoidance method for quickly recovering flight path |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115755951A (en) |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116679757A (en) * | 2023-07-31 | 2023-09-01 | 深圳高度创新技术有限公司 | Obstacle avoidance method of unmanned aerial vehicle and unmanned aerial vehicle |
| CN116736879A (en) * | 2023-08-16 | 2023-09-12 | 成都飞航智云科技有限公司 | Unmanned aerial vehicle automatic obstacle avoidance method and obstacle avoidance system based on cloud computing |
-
2022
- 2022-09-27 CN CN202211183901.XA patent/CN115755951A/en active Pending
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116679757A (en) * | 2023-07-31 | 2023-09-01 | 深圳高度创新技术有限公司 | Obstacle avoidance method of unmanned aerial vehicle and unmanned aerial vehicle |
| CN116679757B (en) * | 2023-07-31 | 2023-10-27 | 深圳高度创新技术有限公司 | Obstacle avoidance method of unmanned aerial vehicle and unmanned aerial vehicle |
| CN116736879A (en) * | 2023-08-16 | 2023-09-12 | 成都飞航智云科技有限公司 | Unmanned aerial vehicle automatic obstacle avoidance method and obstacle avoidance system based on cloud computing |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN115755951A (en) | Unmanned aerial vehicle obstacle avoidance method for quickly recovering flight path | |
| CN110749333B (en) | Unmanned vehicle motion planning method based on multi-objective optimization | |
| CN107807658B (en) | Trajectory planning method for formation flight tasks of multiple unmanned aerial vehicles | |
| CN109685237B (en) | Unmanned aerial vehicle flight path real-time planning method based on Dubins path and branch limit | |
| CN111006667B (en) | Automatic driving track generation system under high-speed scene | |
| CN112947594B (en) | Unmanned aerial vehicle-oriented track planning method | |
| CN113031621B (en) | Bridge crane safety obstacle avoidance path planning method and system | |
| CN116185014A (en) | Intelligent vehicle global optimal track planning method and system based on dynamic planning | |
| Yang et al. | Mobile robot path planning based on enhanced dynamic window approach and improved A∗ algorithm | |
| CN111896004A (en) | Narrow passage vehicle track planning method and system | |
| CN113467476A (en) | Non-collision detection rapid stochastic tree global path planning method considering corner constraint | |
| CN115729266A (en) | Unmanned aerial vehicle obstacle avoidance method for effectively tracking preset air route | |
| CN115562290A (en) | Robot path planning method based on A-star penalty control optimization algorithm | |
| CN113886764A (en) | Intelligent vehicle multi-scene track planning method based on Frenet coordinate system | |
| Avanzini et al. | Using monocular visual SLAM to manually convoy a fleet of automatic urban vehicles | |
| Zhang et al. | Structured road-oriented motion planning and tracking framework for active collision avoidance of autonomous vehicles | |
| CN110262473B (en) | Unmanned ship automatic collision avoidance method based on improved Bi-RRT algorithm | |
| Wang et al. | Research on AGV task path planning based on improved A* algorithm | |
| CN113325852B (en) | Leader follower mode-based multi-agent formation change control method in advancing process | |
| CN111121804A (en) | Intelligent vehicle path planning method and system with safety constraint | |
| CN109556623A (en) | Fusion antenna algorithm and the iterative path planning algorithm for cutting line-plot method | |
| CN113138594B (en) | Automatic driving method and device | |
| CN114460933A (en) | Mobile robot local path planning algorithm for dynamic environment | |
| CN113104053A (en) | Automatic turning tracking method and unmanned vehicle | |
| CN113758485B (en) | Unmanned aerial vehicle cluster collaborative dynamic track planning method based on preset track points |
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 |