CN115328208A - Unmanned aerial vehicle path planning method for realizing global dynamic path planning - Google Patents
Unmanned aerial vehicle path planning method for realizing global dynamic path planning Download PDFInfo
- Publication number
- CN115328208A CN115328208A CN202211152996.9A CN202211152996A CN115328208A CN 115328208 A CN115328208 A CN 115328208A CN 202211152996 A CN202211152996 A CN 202211152996A CN 115328208 A CN115328208 A CN 115328208A
- Authority
- CN
- China
- Prior art keywords
- node
- speed
- aerial vehicle
- unmanned aerial
- 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
- 238000000034 method Methods 0.000 title claims abstract description 57
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 69
- 238000011156 evaluation Methods 0.000 claims abstract description 56
- 241001481710 Cerambycidae Species 0.000 claims abstract description 53
- 230000001133 acceleration Effects 0.000 claims abstract description 26
- 230000003068 static effect Effects 0.000 claims abstract description 21
- 238000010845 search algorithm Methods 0.000 claims abstract description 12
- 238000012217 deletion Methods 0.000 claims abstract description 8
- 230000037430 deletion Effects 0.000 claims abstract description 8
- 238000005457 optimization Methods 0.000 claims abstract description 8
- 230000004888 barrier function Effects 0.000 claims description 18
- 238000004088 simulation Methods 0.000 claims description 10
- 241000254173 Coleoptera Species 0.000 claims description 7
- 230000008859 change Effects 0.000 claims description 4
- 230000008569 process Effects 0.000 claims description 4
- 238000009499 grossing Methods 0.000 claims description 3
- 238000005070 sampling Methods 0.000 claims description 3
- 230000006870 function Effects 0.000 abstract description 34
- 230000008901 benefit Effects 0.000 abstract description 6
- 241000283690 Bos taurus Species 0.000 description 8
- 238000005516 engineering process Methods 0.000 description 8
- 230000004927 fusion Effects 0.000 description 6
- 230000007547 defect Effects 0.000 description 4
- 238000004364 calculation method Methods 0.000 description 3
- 230000002068 genetic effect Effects 0.000 description 2
- 230000006872 improvement Effects 0.000 description 2
- 238000012545 processing Methods 0.000 description 2
- 238000011160 research Methods 0.000 description 2
- 238000013459 approach Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000019637 foraging behavior Effects 0.000 description 1
- 239000002245 particle Substances 0.000 description 1
- 230000002787 reinforcement Effects 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/10—Simultaneous control of position or course in three dimensions
- G05D1/101—Simultaneous control of position or course in three dimensions specially adapted for aircraft
-
- 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
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The invention discloses an unmanned aerial vehicle path planning method for realizing global dynamic path planning, which comprises the following steps: and (3) global static planning: and obtaining a globally optimal static path through a longicorn stigma search algorithm. Local dynamic planning: firstly, correcting a static path generated by a longicorn stigma search algorithm based on an improved dynamic window algorithm, updating the position of a dynamic obstacle, and dynamically avoiding the obstacle; secondly, reducing the length of the static path through a redundant node deletion strategy; and finally, improving the evaluation function, correcting the problem of overlarge linear speed, searching the next optimal node, finishing path optimization and generating an optimal path. The invention has the advantages that: the running time is reduced, the path length is shortened, the influence of acceleration and deceleration circulation can be reduced, and the method has the functions of global guidance and real-time obstacle avoidance.
Description
Technical Field
The invention relates to the technical field of unmanned aerial vehicle path planning, in particular to an unmanned aerial vehicle path planning method integrating a longicorn whisker search algorithm and a dynamic window algorithm.
Background
Along with the development of unmanned aerial vehicle technique, unmanned aerial vehicle's application area is wider and wider. At present, the main application field of the unmanned aerial vehicle gradually turns to emerging fields such as urban logistics distribution from audio-video entertainment, and in the face of complex environments, the operation efficiency of the unmanned aerial vehicle can be effectively improved by reasonably selecting an unmanned aerial vehicle path planning algorithm, and the operation risk of the unmanned aerial vehicle is reduced.
The existing unmanned aerial vehicle path planning algorithms are divided into two categories, namely a global path planning algorithm and a local planning algorithm.
The global path planning algorithm mainly comprises the following contents: traditional algorithms (Dijkstra algorithm, a algorithm, etc.), intelligent algorithms (PSO algorithm, genetic algorithm, reinforcement learning, etc.). The global path planning algorithm can complete static global optimal path planning, but most of the algorithms have the defects of poor dynamic obstacle avoidance capability, slow searching capability, poor local planning capability and the like.
The local planning algorithm mainly comprises the following contents: artificial Potential Field (APF), dynamic Window (DWA), etc. The local path planning algorithm has better real-time performance, can realize dynamic obstacle avoidance and dynamic path planning, but most of the algorithms lack guidance of global planning and are easy to fall into the local optimal problem.
Prior art 1
In journal, "journal of Instrument and Meter" 2021, volume 42, no. 03, pages 132-140 [1] The robot random obstacle avoidance method research based on the fusion of the improved A-algorithm and the dynamic window method is recorded, global path nodes are searched through the improved A-algorithm, and local planning is carried out between adjacent nodes by adopting the dynamic window method.
The technology provides an optimized search point selection strategy and improves an algorithm heuristic function, wherein the optimized search point selection strategy discards 3 search directions in the algorithm 8 field search according to the connecting line included angle between a target point and a current point, and reduces the number of search nodes; the improved heuristic function is to add a weight value which can change along with the distance from the current point to the target point to the estimation value of the original heuristic function of the A-x algorithm. The improvement of the technology on the A-algorithm can reduce the number of searching nodes and avoid invalid traversal, and improves the searching efficiency of the A-algorithm.
The technology provides a redundant point deleting strategy in the aspect of fusion of a global planning algorithm and a local planning algorithm, the strategy specifies that intermediate points on the same straight line are redundant points, all the redundant points are deleted after all the nodes are traversed, unnecessary nodes in a global path are effectively deleted, and the path length is reduced.
Although the robot obstacle avoidance method combining the improved A-algorithm and the dynamic window method combines the global path planning algorithm and the local path planning algorithm, real-time random obstacle avoidance can be realized on the basis of the global optimal path, but the robot obstacle avoidance method has the following defects:
1. the improved A-star algorithm still has the problems of large calculation amount and long operation time;
2. after sub-target points are added into a dynamic window method, due to the constraint of a dynamic window, no person can have the phenomenon of continuous alternation of acceleration and deceleration, so that the linear velocity of the unmanned plane is changed too fast, and the method cannot solve the problem;
3. the path planned by the dynamic window method is an arc line, the two key nodes are connected by the arc line, the path is longer than the straight line connection, and the redundant node deleting method in the technology can not solve the problem that the path is lengthened after the dynamic window method is used under the condition that the influence of dynamic obstacles is not large.
Prior art 2
In journal, scientific and technical and engineering 2019,19, vol.19, pp.185 to 190 [2] The method comprises the steps of recording agricultural robot path planning method research based on an improved A algorithm and a celestial cow whisker search algorithm, statically planning a global path through the A algorithm, correcting and optimizing the A algorithm to generate the path by using the improved celestial cow whisker algorithm, and finally increasing path continuity by using a Bessel function.
The technology introduces a self-adaptive function and provides a step-length-variable longicorn algorithm. The step length and the proportion of the step length and the length in the traditional celestial cow whisker algorithm are properly adjusted, a large step length is adopted in the early stage of path search, the step length is reduced along with the increase of iteration times, the relation between the precision and the global optimum is effectively balanced, and the convergence speed of the celestial cow whisker algorithm is accelerated.
The technology uses the improved Tianniu algorithm to perform secondary search on the expansion nodes of the A-x algorithm generated path, the redundant nodes are deleted, the expansion node positions are updated through the position updating formula of the improved Tianniu algorithm, and the path planning performance is improved.
The technology introduces a Bezier curve, and brings path nodes generated after the modified Tianniu whisker algorithm is modified into a Bezier curve formula to generate a relatively smooth path.
Although the agricultural robot path planning method with the fusion of the improved A-star algorithm and the Tianniu algorithm realizes the deletion of redundant nodes in the path planning process, improves the path searching speed and improves the smoothness of the generated path, the method still has the following defects:
1. lack of real-time path planning;
2. dynamic obstacle avoidance cannot be realized.
Abbreviations and Key term definitions
The Search of celestial cow whiskers (BAS), also called Beetle whiskers, is a single intelligent optimization algorithm for simulating foraging behavior of celestial cows. The method is characterized in that the efficient optimization can be realized without knowing the specific form of the function and the gradient information in the search of the longicorn stigma. Compared with a particle swarm algorithm and a genetic algorithm, the longicorn needs to search only one individual, namely one longicorn, and the operation amount is greatly reduced. In summary, the longicorn stigma search algorithm has the advantages of simple principle, few parameters, small calculation amount and the like, and has great advantages in processing low-dimensional optimization targets. But the method has the problems of low convergence speed, easy falling into local optimization when processing multidimensional complex problems and the like.
Dynamic Window Approach (DWA) is a commonly used local Dynamic path planning algorithm. The DWA algorithm samples a plurality of groups of speed and speed combinations comprising linear speed and angular speed under the condition of fully considering the mechanical characteristic limit of the UAV and the speed constraint of the environment, simultaneously simulates the tracks of the UAV under the speed combinations within a certain time interval, and selects the track with the highest evaluation value according to an evaluation function to complete path planning. The dynamic window method has good dynamic obstacle avoidance capability and real-time path planning capability, but due to lack of guidance of global environment information, the method is easy to fall into local optimum, and even has the problem that a target point cannot be reached.
Reference (e.g. patent/thesis/standard)
[1] Helxu, plum blossom, fei Jiyou, a robot random obstacle avoidance method study based on the fusion of an improved A algorithm and a dynamic window method [ J ]. Instrument and Meter report, 2021,42 (03): 132-140;
[2] zhao Hui, hao Mengya, wang Gongjun, yueauer, agricultural robot path planning method based on the improved a algorithm and the skynet search algorithm [ J ] science and technology, 2019,19 (31): 185-190;
[3] ge Wenya, li Ping. Mobile robot Global dynamic Path planning fusion Algorithm [ J/OL ]. University of Chinese newspaper (Nature science edition): 1-10[2022-07-27].
Disclosure of Invention
Aiming at the defects of the prior art, the invention provides an unmanned aerial vehicle path planning method for realizing global dynamic path planning.
In order to realize the purpose, the technical scheme adopted by the invention is as follows:
an unmanned aerial vehicle path planning method for realizing global dynamic path planning comprises the following steps:
step 1, global static planning: and obtaining a globally optimal static path through a longicorn stigma search algorithm.
Step 2, local dynamic planning: firstly, correcting a static path generated by a longicorn stigma search algorithm based on an improved dynamic window algorithm, updating the position of a dynamic obstacle, and dynamically avoiding the obstacle; secondly, reducing the length of the static path by a redundant node deletion strategy; and finally, improving the evaluation function, correcting the problem of overlarge linear speed change, searching the next optimal node, finishing path optimization and generating an optimal path.
Further, the specific sub-steps of step 1 include:
step 11, rasterizing a two-dimensional environment map of an unmanned aerial vehicle flight area, and determining coordinates of a starting node and a target node;
step 12, assuming the longicorn from the initial node, setting the longicornPosition X in K-dimensional space 0 ＝(X 1 ，X 2 ，...，X k ) Specifying a longicorn speed V (i, j) = r, wherein i represents a speed magnitude and j represents a speed direction; direction of rotation
Is composed of
Where k represents the centroid-to-whisker distance (representing the longicorn size);
step 13, creating the coordinates of the longicorn left and right whiskers:
in the formula: x t Representing the barnyard cattle centroid coordinates at the t iteration; d denotes the distance between two whiskers;
is a direction vector;
step 14, updating the next position of the longhorn beetle according to the following longhorn beetle position updating formula:
in the formula: s t Is the step size at the t-th iteration, s t ＝S t-1 X α, where α is the step factor, 0.95; f (X) is the fitness function of X; sign () is a sign function;
step 15, compare f (X) left ) And f (X) right ) If f (X) left )＜f(X right ) If the direction of the hair is right, the hair is left;
step 16, after determining the forward direction, setting the direction alongThe next node is an expansion node, and the direction vector of the direction is
Judging whether the expansion node in the grid map is an obstacle node:
(1) if the expansion node is an obstacle node, the longicorn moves to the right to bypass the obstacle;
(2) if the expansion node is not the barrier node, follow
And continuously determining the next node of the expanded node by the direction, and making a difference between the ordinate of the node and the ordinate of the expanded node. If the difference value is 0, shifting the longicorn to the right by one grid; if the difference value is 1, moving the longicorn upwards by one grid; if the difference value is-1, the longicorn is downwards moved by one grid, and if no barrier exists between the grids, the longicorn is obliquely penetrated;
and step 17, after the steps are completed, the longicorn completes one-time position movement, the step 14 is returned, the next step movement is carried out again according to the steps 14 to 16 until the destination is reached, and the search is stopped. Until the global static path search is complete.
Further, the specific sub-steps of step 2 include:
step 21, extracting the longicorn whisker algorithm to obtain a global optimal path including a starting point p 1 End point p n Node set P = { P = { P } inclusive i ，1≤i≤n}；
Step 22, sequentially traversing all nodes in the node set P in the step 21, finding out redundant nodes, deleting the redundant nodes, and storing all nodes which are not deleted into a key point set U;
step 23, initializing parameters of a dynamic window method, taking the current key point as a starting point and the next key point as a target point;
step 24, sampling the speed of the current point, and determining a speed space, namely a speed window, based on speed limit, acceleration limit and brake limit;
step 25, selecting an optimal track reaching the next key point based on an evaluation function considering the direction angle, the distance of the obstacle and the speed;
step 26, judging whether the current key point is the final target point, if not, returning to the step 23 until the final target point is reached;
and 27, outputting the final path and ending the process.
Further, the substeps of step 22 are as follows:
(1) sequentially traversing all nodes in the node set P in the step 21, if the current node P i And the previous node p i-l And a subsequent node p i+1 If the nodes are all on the same straight line, the current node can be regarded as a redundant node, and the node is deleted;
(2) after all the intermediate nodes on the same straight line are deleted, a new node set { p is obtained i I =1,2,3,.., n }, from p 1 Node begins, connects p 3 Node, if line segment p 1 p 3 If the distance from the nearest barrier is greater than the planned safety distance, continuing to connect p 1 Node and p 4 Node, if line segment p 1 p 4 If the distance from the nearest barrier is greater than the planned safety distance, the next node is connected continuously for comparison until a certain node p k The distance between the line segment and the nearest barrier is less than the set safety distance, and the node p is deleted 1 And node p k All nodes in the middle, update the node set, and then get new p 2 The nodes start to repeat the steps until all the nodes are traversed;
(3) after the redundant nodes are deleted preliminarily, a new residual node set { p } is obtained i I =1,2,3,.., n }, and sequentially calculating a remainder b of i ÷ 3 i All the remainder b i A non-final node deletion of value 2;
(4) establishing a key point set U = { p = i I =1,2,3,.., n }, and all nodes that are not deleted are stored in the set U.
Further, the substeps of step 24 are as follows:
(1) and (3) speed limitation: when the unmanned aerial vehicle runs, the speed v and the angular speed omega are restrained by the performance of the unmanned aerial vehicle, so that the unmanned aerial vehicle is subjected to maximum speed limitation when a dynamic window method is applied:
V s ＝{(v，ω)|v∈[v min ，v max ]，ω∈[ω min ，ω max ]}
in the formula: v. of min ，v max Respectively representing the minimum and maximum linear speeds that the unmanned aerial vehicle can reach; omega min ，ω max Representing the minimum and maximum angular velocities that the drone can achieve;
(2) and (3) acceleration limitation: acceleration can receive motor torque's restriction, and in time period delta t, unmanned aerial vehicle's speed range is:
V d ＝{(v，ω)|v∈[v i -aΔt，v i +aΔt]，ω∈[ω i -αΔt，ω i +αΔt]}
in the formula: v. of i ，ω i Respectively representing the linear velocity and the angular velocity of the unmanned aerial vehicle at the current node; a, alpha respectively represents the linear acceleration and the angular acceleration of the unmanned aerial vehicle;
(3) and (3) braking limitation: when the dynamic window detects an obstacle, a distance should be reserved for deceleration braking to avoid collision. The closer the unmanned aerial vehicle is to the nearest barrier, the smaller the braking speed is, and the braking speed can meet the following requirements:
V a ＝{(v，ω)|v≤(2da) 0.5 ，ω≤(2da) 0.5 }
in the formula: d is the distance from the terminal point of the corresponding speed simulation track to the nearest obstacle; a, alpha is the current point-line acceleration and the angular acceleration respectively;
(4) speed space: after considering the three speed limits, the final available speed space is: v r ＝V s ∩V d ∩V a 。
Further, the substeps of step 25 are as follows:
(1) overall evaluation function: after obtaining the velocity space, all the feasible velocities (v) need to be processed i ，ω i ) And evaluating to obtain a group of optimal speed combinations. The overall evaluation function consists of a direction angle evaluation, an obstacle distance evaluation and a speed evaluation:
G(v i ，ω i )＝σ×(α·head(v i ，ω i )+β·dist(v i ，ω i )+γ·vel(v i ，ω i ))
in the formula: σ is a smoothing coefficient; alpha is a direction angle evaluation coefficient; beta is an obstacle distance evaluation coefficient; gamma is a velocity evaluation function;
(2) evaluation function of orientation angle: heading Angle evaluation function head (v) i ，ω i ) The difference between the angle of the unmanned aerial vehicle under the current angular velocity motion and the target point angle is evaluated. The smaller the difference, the better the evaluation:
head(v i ，ω i )＝180°-|target-θ|
in the formula: target is the included angle between the connecting line from the simulation track end point to the target point and the horizontal direction; theta is an included angle between the angular velocity direction and the horizontal direction at the end point of the simulation track;
(3) obstacle distance evaluation function: dist (v) i ，ω i ) For indicating the distance of the drone from the nearest obstacle. When the angular velocity and the linear velocity of the unmanned aerial vehicle are both constant values, the motion trail of the unmanned aerial vehicle is more than one
A circle with a radius, the longer the arc length of the trajectory from the current point to the nearest obstacle, the better the evaluation:
dist(ν i ，ω i )＝r·δ
in the formula: r is the radius of the track circle; delta is an included angle between the connecting line of the unmanned aerial vehicle position, the obstacle position and the circle center;
(4) velocity evaluation function: vel (v) i ，ω i ) The linear velocity of the unmanned aerial vehicle is evaluated, theoretically, the higher the velocity is, the better the velocity is, but considering that the speed cycle of acceleration-deceleration-acceleration can be caused after sub-goal nodes are added, the closer the current key point velocity is to the previous key point velocity, the better the evaluation is:
in the formula: v. of i The current key point line speed is obtained; v. of i-1 The linear velocity of the last critical point.
Compared with the prior art, the invention has the advantages that:
1. the invention uses the longicorn beard algorithm with less parameters and small calculation amount to carry out global static path planning, and compared with the traditional global path planning algorithm, the invention can effectively reduce the operation time and improve the efficiency. Compared with the A-star algorithm which is commonly used at present, the running time of the longicorn whisker algorithm is reduced by 70.4 percent;
2. the global path planning algorithm and the local path planning algorithm are fused for advantage complementation, so that the problem that the global path planning algorithm is difficult to solve the local shortest path and the problem that the local path planning algorithm lacks global guidance is solved;
3. the invention provides a redundant point deleting strategy aiming at turning points in a global optimal path obtained by a longicorn algorithm, when a turning angle beta formed by three nodes is less than or equal to 135 degrees, the middle node is deleted, two end nodes are reserved, and a path drawn by a dynamic window rule is a section of circular arc connecting two end points. Taking a certain three nodes with an included angle of 135 degrees in a section of path as an example, the method can shorten the path length by about 7.38 percent;
4. according to the method, a speed evaluation function of a dynamic window method is improved, the dynamic window method can restrict the braking speed of the unmanned aerial vehicle, and after a globally planned path key node is taken as a sub-target point and is fused with the dynamic window method, the speed of the unmanned aerial vehicle can be in an 'acceleration-deceleration-acceleration' cycle until a final target point is reached. The improved speed evaluation function judges the proximity degree of the current linear speed and the linear speed of the previous node, and reduces the influence of acceleration and deceleration circulation by selecting the closest linear speed.
Drawings
Fig. 1 is a flow chart of main steps of a path planning method for an unmanned aerial vehicle according to an embodiment of the present invention;
fig. 2 is a flowchart illustrating the detailed steps of the path planning method according to the embodiment of the present invention.
Detailed Description
In order to make the objects, technical solutions and advantages of the present invention more apparent, the present invention will be further described in detail below with reference to the accompanying drawings by way of examples.
As shown in fig. 1, a path planning method using the fusion of the improved longicorn whisker algorithm and the dynamic window algorithm includes the following steps:
step 1, global static planning: and obtaining a globally optimal static path through a longicorn stigma search algorithm.
Step 2, local dynamic planning: firstly, correcting a static path generated by a longicorn stigma search algorithm based on an improved dynamic window algorithm, updating the position of a dynamic obstacle, and dynamically avoiding the obstacle; secondly, reducing the length of the static path through a redundant node deletion strategy; and finally, improving the evaluation function, correcting the problem of overlarge linear speed change, searching the next optimal node, finishing path optimization and generating an optimal path.
As shown in fig. 2, the detailed sub-step 1 includes:
step 11, rasterizing a two-dimensional environment map of an unmanned aerial vehicle flight area, and determining coordinates of a starting node and a target node;
step 12, assuming the longicorn from the initial node, setting the position X of the longicorn in the K-dimensional space 0 ＝(X 1 ，X 2 ，...，X k ) Specifying a longicorn speed V (i, j) = r, wherein i represents a speed magnitude and j represents a speed direction; direction of rotation
Is composed of
Where k represents the centroid-to-whisker distance (representing the longicorn size);
step 13, creating the coordinates of the longicorn left and right whiskers:
in the formula: x t Representing the barnyard cattle centroid coordinates at the t iteration; d refers to the distance between two whiskers;
is a direction vector;
step 14, updating the next position of the longhorn beetle according to the following longhorn beetle position updating formula:
in the formula: s t Is the step size at the t-th iteration, s t ＝S t-1 X α, where α is the step factor, 0.95; f (X) is the fitness function of X; sign () is a sign function;
step 15, compare f (X) left ) And f (X) right ) If f (X) left )＜f(X right ) Otherwise, the user must move forward to the right direction;
step 16, after determining the forward direction, setting the direction along the forward direction (setting the direction vector of the direction as
) The next node is an expansion node, and whether the expansion node is an obstacle node in the grid map is judged:
(1) if the expansion node is an obstacle node, the longicorn moves to the right to bypass the obstacle;
(2) if the expansion node is not the barrier node, follow
And continuously determining the next node of the expanded node by the direction, and making a difference between the ordinate of the node and the ordinate of the expanded node. If the difference value is 0, shifting the longicorn to the right by one grid; if the difference value is 1, moving the longicorn upwards by one grid; if the difference is-1, the longicorn is moved downwards for one grid. (note: if there is no obstacle between the grids, the longicorn can be worn obliquely);
and step 17, after the steps are completed, the longicorn completes one-time position movement, the step 14 is returned, the next step movement is carried out again according to the steps 14 to 16 until the destination is reached, and the search is stopped. Until the global static path search is complete.
As shown in fig. 2, the specific sub-steps of step 2 include:
step 21, extracting the longicorn whisker algorithm to obtain a global optimal path including a starting point p 1 End point p n Node set P = { P = { P } inclusive i ，1≤i≤n}；
Step 22, deleting redundant nodes:
(1) sequentially traversing all nodes in the node set P in the step 21, if the current node P i And the previous node p i-1 And a subsequent node p i+1 If the nodes are all on the same straight line, the current node can be regarded as a redundant node, and the node is deleted;
(2) after all the intermediate nodes on the same straight line are deleted, a new node set { p is obtained i I =1,2,3,.., n }, from p 1 Node begins, connects p 3 Node, if line segment p 1 p 3 If the distance from the nearest barrier is greater than the planned safety distance, continuing to connect p 1 Node and p 4 Node, if line segment p 1 p 4 If the distance from the nearest barrier is greater than the planned safety distance, the next node is connected continuously for comparison until a certain node p k The distance between the line segment and the nearest barrier is less than the set safety distance, and the node p is deleted 1 And node p k All nodes in the middle, update node set, and then get new p 2 The nodes start to repeat the steps until all the nodes are traversed;
(3) after the redundant node is deleted initially, a new residual node set { p is obtained i I =1,2,3,.., n }, and sequentially calculating a remainder b of i ÷ 3 i All the remainder b i A non-final node deletion of value 2;
(4) establishing a key node set U = { p = { (p) } i I =1,2,3,.., n }, and all nodes which are not deleted are stored in a set U;
step 23, initializing parameters of a dynamic window method, taking the current key point as a starting point and the next key point as a target point;
step 24, sampling the speed of the current point, and determining a speed space based on the speed limit, the acceleration limit and the braking limit, namely a speed window:
(1) speed limitation: when the unmanned aerial vehicle operates, the speed and the angular speed can be restrained by the performance of the unmanned aerial vehicle, and therefore the unmanned aerial vehicle is subjected to maximum speed limitation when a dynamic window method is applied:
V s ＝{(v，ω)|v∈[v min ，v max ]，ω∈[ω min ，ω max ]}
in the formula: v. of min ，v max Respectively representing the minimum and maximum linear speeds that the unmanned aerial vehicle can reach; omega min ，ω max Representing the minimum and maximum angular velocities that the drone can achieve;
(2) and (3) acceleration limitation: acceleration can receive motor torque's restriction, and in time cycle delta t, unmanned aerial vehicle's speed range is:
V d ＝{(v，ω)|v∈[v i -aΔt，v i +aΔt]，ω∈[ω i -αΔt，ω i +αΔt]}
in the formula: v. of i ，ω i Respectively representing the linear velocity and the angular velocity of the unmanned aerial vehicle at the current node; a, alpha respectively represents the linear acceleration and the angular acceleration of the unmanned aerial vehicle;
(3) and (3) braking limitation: when the dynamic window detects an obstacle, a distance should be reserved for deceleration braking to avoid collision. The closer the unmanned aerial vehicle is to the nearest barrier, the smaller the braking speed is, and the braking speed can meet the following requirements:
V a ＝{(v，ω)|v≤(2da) 0.5 ，ω≤(2dα) 0.5 }
in the formula: d is the distance from the terminal point of the corresponding speed simulation track to the nearest obstacle; a, alpha is the current point-line acceleration and the angular acceleration respectively;
(4) velocity space (dynamic window): after considering the three speed constraints, the final available speed space is:
V r ＝V s ∩V d ∩V a
and 25, selecting an optimal track reaching the next key point based on an evaluation function considering the direction angle, the distance from the obstacle and the speed:
(1) overall evaluation function: after obtaining the velocity space, all the feasible velocities (v) need to be processed i ，ω i ) And evaluating to obtain a group of optimal speed combinations. The overall evaluation function consists of a direction angle evaluation, an obstacle distance evaluation and a speed evaluation:
G(v i ，ω i )＝σ×(α·head(v i ，ω i )+β·dist(v i ，ω i )+γ·vel(v i ，ω i ))
in the formula: σ is a smoothing coefficient; alpha is a direction angle evaluation coefficient; beta is an obstacle distance evaluation coefficient; gamma is a velocity evaluation function;
(2) evaluation function of direction angle: heading Angle evaluation function head (v) i ，ω i ) The difference between the angle of the unmanned aerial vehicle under the current angular velocity motion and the target point angle is evaluated. The smaller the difference, the better the evaluation:
head(v i ，ω i )＝180°-|target-θ|
in the formula: target is an included angle between a connecting line from the simulation track end point to the target point and the horizontal direction; theta is an included angle between the angular velocity direction and the horizontal direction at the end point of the simulation track;
(3) obstacle distance evaluation function: dist (v) i ，ω i ) For indicating the distance of the drone from the nearest obstacle. When the angular velocity and the linear velocity of the unmanned aerial vehicle are both constant values, the motion trail of the unmanned aerial vehicle is more than one
A circle with a radius, the longer the arc length of the trajectory from the current point to the nearest obstacle, the better the evaluation:
dist(v i ，ω i )＝r·δ
in the formula: r is the radius of the track circle; delta is an included angle between the connecting line of the unmanned aerial vehicle position, the obstacle position and the circle center;
(4) velocity evaluation function: vel (v) i ，ω i ) The linear velocity of the unmanned aerial vehicle is evaluated, theoretically, the higher the velocity is, the better the velocity is, but considering that a sub-target node is added and then a velocity cycle of 'acceleration-deceleration-acceleration' is involved, the closer the current key point velocity is to the previous key point velocity, the better the evaluation is:
in the formula: v. of i The current key point line speed is obtained; v. of i-1 The linear velocity of the last key point;
step 26, judging whether the current key point is the final target point, if not, returning to the step 23 until the final target point is reached;
and 27, outputting the final path and ending the process.
The purpose of improving the speed cycle achieved by the speed evaluation function improvement method in the embodiment can also be achieved to a certain extent by improving the braking constraint of the dynamic window method. Original brake constraint of dynamic window method is changed into [3] ：
In the formula: v a (v i ，ω i ) The braking speed range of the current point; v. of i ，ω i Respectively the linear velocity and the angular velocity of the current point; v. of i-1 ，ω i-1 The linear velocity and the angular velocity of the previous point are respectively; d is (v) i ，ω i ) The distance from the terminal point of the corresponding speed simulation track to the nearest barrier; a and alpha are the current dotted line acceleration and the current angular acceleration respectively.
It will be appreciated by those of ordinary skill in the art that the examples described herein are intended to assist the reader in understanding the manner in which the invention is practiced, and it is to be understood that the scope of the invention is not limited to such specifically recited statements and examples. Those skilled in the art can make various other specific changes and combinations based on the teachings of the present invention without departing from the spirit of the invention, and these changes and combinations are within the scope of the invention.
Claims (6)
1. An unmanned aerial vehicle path planning method for realizing global dynamic path planning is characterized by comprising the following steps:
step 1, global static planning: obtaining a globally optimal static path through a longicorn stigma search algorithm;
step 2, local dynamic planning: firstly, correcting a static path generated by a longicorn stigma search algorithm based on an improved dynamic window algorithm, updating the position of a dynamic obstacle, and dynamically avoiding the obstacle; secondly, reducing the length of the static path through a redundant node deletion strategy; and finally, improving the evaluation function, correcting the problem of overlarge linear speed change, searching the next optimal node, finishing path optimization and generating an optimal path.
2. The unmanned aerial vehicle path planning method of claim 1, wherein:
the concrete steps of step 1 include:
step 11, rasterizing a two-dimensional environment map of an unmanned aerial vehicle flight area, and determining coordinates of a starting node and a target node;
step 12, assuming the longicorn from the initial node, setting the position X of the longicorn in the K-dimensional space 0 ＝(X 1 ，X 2 ，...，X k ) Specifying a longicorn speed V (i, j) = r, wherein i represents a speed magnitude and j represents a speed direction; direction of rotation
Is composed of
Where k represents the centroid-to-whisker distance;
step 13, creating the coordinates of the longicorn left and right whiskers:
in the formula: x t Representing the barnyard coordinate of the longicorn at the t iteration; d denotes the distance between two whiskers;
is a direction vector;
step 14, updating the next position of the longhorn beetle according to the following longhorn beetle position updating formula:
in the formula: s t Is the step size at the t-th iteration, S t ＝S t-1 X α, where α is the step factor, 0.95; f (X) is the fitness function of X; sign () is a sign function;
step 15, compare f (X) left ) And f (X) right ) If f (X) left )＜f(X right ) Otherwise, the user must move forward to the right direction;
step 16, after the forward direction is determined, setting the next node along the direction as an expansion node, wherein the direction vector of the direction is
Judging whether the expansion node is an obstacle node in the grid map:
(1) if the expansion node is an obstacle node, the longicorn moves to the right to bypass the obstacle;
(2) if the expansion node is not the barrier node, follow
Continuously determining the next node of the expanded node in the direction, and subtracting the ordinate of the node from the ordinate of the expanded node; if the difference value is 0, shifting the longicorn to the right by one grid; if the difference value is 1, moving the longicorn upwards by one grid; if the difference value is-1, the longicorn is moved downwards by one grid, and if no obstacle exists between the grids, the longicorn is obliquely penetrated;
step 17, after the above steps are completed, the longicorn completes one-time position movement, the step 14 is returned, the next step movement is carried out again according to the steps 14 to 16 until the destination is reached, and the search is stopped; until the global static path search is complete.
3. The unmanned aerial vehicle path planning method of claim 1, wherein:
the specific sub-steps of step 2 include:
step 21, extracting the longicorn whisker algorithm to obtain a global optimal path including a starting point p 1 End point p n Node set P = { P = { P } inclusive i ，1≤i≤n}；
Step 22, sequentially traversing all nodes in the node set P in the step 21, finding out redundant nodes, deleting the redundant nodes, and storing all nodes which are not deleted into a key point set U;
step 23, initializing parameters of a dynamic window method, taking the current key point as a starting point and the next key point as a target point;
step 24, sampling the speed of the current point, and determining a speed space, namely a speed window, based on the speed limit, the acceleration limit and the braking limit;
step 25, selecting an optimal track reaching the next key point based on an evaluation function considering the direction angle, the distance from the obstacle and the speed;
step 26, judging whether the current key point is the final target point, if not, returning to the step 23 until the final target point is reached;
and 27, outputting the final path and ending the process.
4. The unmanned aerial vehicle path planning method of claim 3, wherein:
the substeps of step 22 are as follows:
(1) sequentially traversing all nodes in the node set P in the step 21, if the current node P i With the previous node p i-1 And a subsequent node p i+1 If the nodes are all on the same straight line, the current node can be regarded as a redundant node, and the node is deleted;
(2) after all the intermediate nodes on the same straight line are deleted, a new node set { p is obtained i I =1,2,3,.., n }, from p 1 Node begins, connects p 3 Node, if line segment p 1 p 3 If the distance between the nearest barrier and the nearest barrier is larger than the set safety distance, continuing to connect p 1 Node and p 4 Node, if line segment p 1 p 4 If the distance between the node p and the nearest barrier is larger than the planned safety distance, the next node is continuously connected for comparison until a certain node p k The distance between the line segment and the nearest barrier is less than the set safety distance, and the node p is deleted 1 And node p k All nodes in the middle, update node set, and then get new p 2 The nodes start to repeat the steps until all the nodes are traversed;
(3) after the redundant nodes are deleted preliminarily, a new residual node set { p } is obtained i I =1,2,3,.., n }, and sequentially calculating a remainder b of i ÷ 3 i All the remainder b i A non-final node deletion of value 2;
(4) establishing a key point set U = { p = { (p) } i I =1,2,3,.., n }, and all nodes that are not deleted are stored in the set U.
5. The unmanned aerial vehicle path planning method of claim 3, wherein:
the substeps of step 24 are as follows:
(1) speed limitation: when the unmanned aerial vehicle runs, the speed v and the angular speed omega are restricted by the performance of the unmanned aerial vehicle, so that the unmanned aerial vehicle is subjected to maximum speed limitation when a speed window method is applied:
V s ＝{(v，ω)|v∈[v min ，v max ]，ω∈[ω min ，ω max ]}
in the formula: v. of min ，v max Respectively representing the minimum and maximum linear speeds that the unmanned aerial vehicle can reach; omega min ，ω max Representing the minimum and maximum angular velocities that the drone can achieve;
(2) and (3) acceleration limitation: acceleration can receive motor torque's restriction, and in time period delta t, unmanned aerial vehicle's speed range is:
V d ＝{(v，ω)|v∈[v i -aΔt，v i +aΔt]，ω∈[ω i -αΔt，ω i +αΔt]}
in the formula: v. of i ，ω i Respectively representing the linear velocity and the angular velocity of the unmanned aerial vehicle at the current node; a, alpha respectively represents the linear acceleration and the angular acceleration of the unmanned aerial vehicle;
(3) and (3) braking limitation: when the dynamic window detects an obstacle, a distance is reserved for decelerating and braking to avoid collision; the closer the unmanned aerial vehicle is to the nearest barrier, the smaller the braking speed is, and the braking speed can meet the following requirements:
V a ＝{(v，ω)|v≤(2da) 0.5 ，ω≤(2dα) 0.5 }
in the formula: d is the distance from the corresponding speed simulation track end point to the nearest obstacle; a, alpha is the current point-line acceleration and the angular acceleration respectively;
(4) speed space: after considering the three speed constraints, the final available speed space is: v r ＝V s ∩V d ∩V a 。
6. The unmanned aerial vehicle path planning method of claim 3, wherein:
the substeps of step 25 are as follows:
(1) overall merit function: after obtaining the velocity space, all the possible velocities (v) need to be measured i ，ω i ) Evaluating to obtain a group of optimal speed combinations; the overall evaluation function consists of a direction angle evaluation, an obstacle distance evaluation and a speed evaluation:
G(v i ，ω i )＝σ×(α·head(v i ，ω i )+β·dist(v i ，ω i )+γ·vel(v i ，ω i ))
in the formula: σ is a smoothing coefficient; alpha is a direction angle evaluation coefficient; beta is an obstacle distance evaluation coefficient; gamma is a velocity evaluation function;
(2) evaluation function of orientation angle: heading Angle evaluation function head (v) i ，ω i ) Evaluating the difference between the angle of the unmanned aerial vehicle under the current angular velocity motion and the target point angle; the smaller the difference, the better the evaluation:
head(v i ，ω i )＝180°-|target-θ|
in the formula: target is the included angle between the connecting line from the simulation track end point to the target point and the horizontal direction; theta is an included angle between the angular velocity direction and the horizontal direction at the end point of the simulation track;
(3) obstacle distance evaluation function: dist (v) i ，ω i ) For indicating the distance of the drone from the nearest obstacle; when the angular velocity and the linear velocity of the unmanned aerial vehicle are both constant values, the motion trail of the unmanned aerial vehicle is more than one
A circle with a radius, the longer the arc length of the trajectory from the current point to the nearest obstacle, the better the evaluation:
dist(u i ，ω i )＝r·δ
in the formula: r is the radius of the track circle; delta is an included angle between the connecting line of the unmanned aerial vehicle position, the obstacle position and the circle center;
(4) velocity evaluation function: vel (v) i ，ω i ) The linear velocity of the unmanned aerial vehicle is evaluated, theoretically, the higher the velocity is, the better the velocity is, but considering that a sub-target node is added and then a velocity cycle of 'acceleration-deceleration-acceleration' is involved, the closer the current key point velocity is to the previous key point velocity, the better the evaluation is:
in the formula: v. of i The current key point line speed is obtained; v. of i-1 The linear velocity of the last critical point.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211152996.9A CN115328208A (en) | 2022-09-21 | 2022-09-21 | Unmanned aerial vehicle path planning method for realizing global dynamic path planning |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211152996.9A CN115328208A (en) | 2022-09-21 | 2022-09-21 | Unmanned aerial vehicle path planning method for realizing global dynamic path planning |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN115328208A true CN115328208A (en) | 2022-11-11 |
Family
ID=83914011
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202211152996.9A Pending CN115328208A (en) | 2022-09-21 | 2022-09-21 | Unmanned aerial vehicle path planning method for realizing global dynamic path planning |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115328208A (en) |
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115951683A (en) * | 2023-01-29 | 2023-04-11 | 南京理工大学紫金学院 | Artificial potential field path planning method for mixed gradient descent and longicorn whisker search |
| CN116893687A (en) * | 2023-08-21 | 2023-10-17 | 广东工业大学 | Unmanned aerial vehicle path planning method for improving lazy theta in complex environment |
| CN117451057A (en) * | 2023-12-25 | 2024-01-26 | 长春理工大学 | Unmanned aerial vehicle three-dimensional path planning method, equipment and medium based on improved A-algorithm |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111290390A (en) * | 2020-02-25 | 2020-06-16 | 智慧航海（青岛）科技有限公司 | Intelligent ship path planning method based on longicorn stigma search |
| CN112327850A (en) * | 2020-11-06 | 2021-02-05 | 大连海事大学 | Unmanned surface vehicle path planning method |
| CN113608531A (en) * | 2021-07-26 | 2021-11-05 | 福州大学 | Unmanned vehicle real-time global path planning method based on dynamic window of safety A-guiding point |
| CN115016482A (en) * | 2022-06-17 | 2022-09-06 | 南通大学 | Improved indoor mobile robot global and local path coordination optimization method |
-
2022
- 2022-09-21 CN CN202211152996.9A patent/CN115328208A/en active Pending
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111290390A (en) * | 2020-02-25 | 2020-06-16 | 智慧航海（青岛）科技有限公司 | Intelligent ship path planning method based on longicorn stigma search |
| CN112327850A (en) * | 2020-11-06 | 2021-02-05 | 大连海事大学 | Unmanned surface vehicle path planning method |
| CN113608531A (en) * | 2021-07-26 | 2021-11-05 | 福州大学 | Unmanned vehicle real-time global path planning method based on dynamic window of safety A-guiding point |
| CN115016482A (en) * | 2022-06-17 | 2022-09-06 | 南通大学 | Improved indoor mobile robot global and local path coordination optimization method |
Non-Patent Citations (2)
| Title |
|---|
| 程传奇;郝向阳;李建胜;张振杰;孙国鹏: "融合改进A~*算法和动态窗口法的全局动态路径规划", 西安交通大学学报 * |
| 程传奇;郝向阳;李建胜;张振杰;孙国鹏;: "融合改进A~*算法和动态窗口法的全局动态路径规划" * |
Cited By (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115951683A (en) * | 2023-01-29 | 2023-04-11 | 南京理工大学紫金学院 | Artificial potential field path planning method for mixed gradient descent and longicorn whisker search |
| CN115951683B (en) * | 2023-01-29 | 2023-11-28 | 南京理工大学紫金学院 | Artificial potential field path planning method for mixed gradient descent and longhorn beetle whisker search |
| CN116893687A (en) * | 2023-08-21 | 2023-10-17 | 广东工业大学 | Unmanned aerial vehicle path planning method for improving lazy theta in complex environment |
| CN116893687B (en) * | 2023-08-21 | 2024-01-23 | 广东工业大学 | Unmanned aerial vehicle path planning method for improving lazy theta in complex environment |
| CN117451057A (en) * | 2023-12-25 | 2024-01-26 | 长春理工大学 | Unmanned aerial vehicle three-dimensional path planning method, equipment and medium based on improved A-algorithm |
| CN117451057B (en) * | 2023-12-25 | 2024-03-12 | 长春理工大学 | Unmanned aerial vehicle three-dimensional path planning method, equipment and medium based on improved A-algorithm |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN115328208A (en) | Unmanned aerial vehicle path planning method for realizing global dynamic path planning | |
| CN110887484B (en) | Mobile robot path planning method based on improved genetic algorithm and storage medium | |
| CN110487279B (en) | Path planning method based on improved A-Algorithm | |
| CN112378408A (en) | Path planning method for realizing real-time obstacle avoidance of wheeled mobile robot | |
| CN111174798A (en) | Foot type robot path planning method | |
| CN112731916A (en) | Global dynamic path planning method integrating skip point search method and dynamic window method | |
| CN111930121B (en) | Mixed path planning method for indoor mobile robot | |
| CN113359718B (en) | Method and equipment for fusing global path planning and local path planning of mobile robot | |
| CN114397896A (en) | Dynamic path planning method for improving particle swarm optimization | |
| CN112486178A (en) | Dynamic path planning method based on directed D (delta) algorithm | |
| Li et al. | Mobile robot path planning based on improved genetic algorithm with A-star heuristic method | |
| CN114705196B (en) | Self-adaptive heuristic global path planning method and system for robot | |
| CN113515111A (en) | Vehicle obstacle avoidance path planning method and device | |
| CN114428499A (en) | Astar and DWA algorithm fused mobile trolley path planning method | |
| Zhang et al. | Research on obstacle avoidance algorithm of multi-UAV consistent formation based on improved dynamic window approach | |
| CN117387635A (en) | Unmanned aerial vehicle navigation method based on deep reinforcement learning and PID controller | |
| CN113124875A (en) | Path navigation method | |
| Yu et al. | RDT-RRT: Real-time double-tree rapidly-exploring random tree path planning for autonomous vehicles | |
| Guan et al. | Robot dynamic path planning based on improved A* and DWA algorithms | |
| CN115826586B (en) | Path planning method and system integrating global algorithm and local algorithm | |
| CN116009558A (en) | Mobile robot path planning method combined with kinematic constraint | |
| CN117008617A (en) | Environment rapid autonomous exploration planning method and device suitable for dynamics-limited ground robot | |
| Feng et al. | A hybrid motion planning algorithm for multi-robot formation in a dynamic environment | |
| Dang | Autonomous mobile robot path planning based on enhanced A* algorithm integrating with time elastic band | |
| CN116243724A (en) | Unmanned aerial vehicle path planning method based on A-algorithm and improved minimized snap |
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 | ||
| RJ01 | Rejection of invention patent application after publication |
Application publication date: 20221111 |
|
| RJ01 | Rejection of invention patent application after publication |