CN115657725A - Primary-secondary unmanned aerial vehicle release decision and path planning integrated method and system - Google Patents
Primary-secondary unmanned aerial vehicle release decision and path planning integrated method and system Download PDFInfo
- Publication number
- CN115657725A CN115657725A CN202211403755.7A CN202211403755A CN115657725A CN 115657725 A CN115657725 A CN 115657725A CN 202211403755 A CN202211403755 A CN 202211403755A CN 115657725 A CN115657725 A CN 115657725A
- Authority
- CN
- China
- Prior art keywords
- path
- point
- unmanned aerial
- primary
- aerial vehicle
- 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
Abstract
The invention discloses a method and a system for integrating release decision and path planning of a primary-secondary unmanned aerial vehicle, and relates to the technical field of unmanned aerial vehicle path planning. The technical points of the invention comprise: constructing a three-dimensional map environment according to the task requirements of the primary-secondary unmanned aerial vehicle; determining the number of submachine of the primary-secondary unmanned aerial vehicle and the target position of each submachine; establishing a path cost function according to the target position and the terrain height of each submachine; and obtaining the flight paths of the primary platform and the secondary machine in the primary-secondary unmanned aerial vehicle by using an improved particle swarm algorithm, wherein the initial flight path point of the secondary machine is the thrown position. The method takes the specific separation point selection in the releasing process of the primary and secondary unmanned aerial vehicles as the core, and fills the research blank of the path planning method of the primary and secondary unmanned aerial vehicles; aiming at the problems that the traditional particle swarm algorithm has too high convergence speed and is easy to fall into local optimization, the improved particle swarm algorithm is provided, the local optimality and the global optimality in path optimization are considered, and the quality of a planned path is effectively improved.
Description
Technical Field
The invention relates to the technical field of unmanned aerial vehicle path planning, in particular to a method and a system for integrating delivery decision and path planning of a parent-child unmanned aerial vehicle.
Background
The primary-secondary unmanned aerial vehicle is a novel unmanned aerial vehicle which is carried or hung by an unmanned primary platform and operates in cooperation with the primary platform after the primary platform puts in one or more small unmanned submachine according to task requirements. The primary platform carries the submachine to fly to the high altitude, and the submachine is returned or continues to monitor the high altitude after being thrown in the target area; the submachine has small volume, high maneuverability and strong space accessibility, and can effectively complete tasks such as approaching reconnaissance, target tracking and the like. The primary and secondary unmanned aerial vehicle has the characteristics of long endurance time, wide coverage range, strong environment adaptability and the like, and is suitable for occasions of long-distance and large-range patrol, reconnaissance operation in a multi-obstacle environment and the like. The system mainly undertakes a large-range target reconnaissance task in the military field, can effectively increase the combat radius, and improves the coverage range and the air control capability; has higher application prospect in the aspects of terrain exploration, personnel search and rescue and the like in the civil field.
The integration of the release decision and the path planning of the primary-secondary unmanned aerial vehicle refers to a method for finding out the optimal or sub-optimal flight path between a secondary machine release position and a primary-secondary platform from a map environment according to a certain rule under the condition of knowing a target point position, and generally comprises three contents: firstly, constructing a map environment, namely sensing a working scene and the position and the size of an obstacle in the working scene in a pre-generated or real-time detection mode, and expressing the working scene and the position and the size of the obstacle in a corresponding mathematical form to generate a three-dimensional digital map environment; secondly, generating a feasible path, namely completing exploration of a map environment according to a certain rule to obtain a feasible path meeting task constraints (compared with the traditional unmanned aerial vehicle path planning problem, in the path planning task of the primary-secondary unmanned aerial vehicle, the path starting point of the secondary machine is necessarily on the flight track of the primary platform); and thirdly, optimizing the planned path, namely optimizing indexes such as the length and the safety of the feasible path by adopting a certain method to generate the optimal or sub-optimal launching position and the flight path of the parent platform and the child machine.
At present, algorithms commonly used for unmanned aerial vehicle path planning can be divided into three types, namely a search algorithm, a sampling algorithm and an intelligent algorithm. The search algorithm generally divides a map environment into grid spaces, calculates the path cost of adjacent grids by designing an evaluation function, and continuously expands the path cost to the adjacent grids with the lowest path cost, wherein representative algorithms comprise a Dijkstra algorithm, an A algorithm and the like; the sampling algorithm is that on the basis of a previous path point (father node), a plurality of new path points (child nodes) are generated in a map according to a certain rule, and the father and child nodes meeting obstacle avoidance constraints are connected to realize the expansion of the path points, wherein the representative algorithm is a Probability Route Map (PRM) algorithm, a fast random tree search (RRT) algorithm and the like; the intelligent algorithm models a path planning task into a path optimizing problem under the constraint of a map environment, firstly generates a plurality of initial paths capable of connecting a starting point to a target point, then optimizes and updates the initial paths according to a certain rule, and continuously improves the quality of the planned paths according to a path cost reduction principle until corresponding requirements are met, wherein the representative algorithms are genetic algorithms, particle swarm algorithms and the like.
However, none of the above methods solves the problems of the child-machine release decision and the path planning of the child-machine type unmanned aerial vehicle.
Disclosure of Invention
Therefore, the invention provides a method and a system for integrating delivery decision and path planning of a primary-secondary unmanned aerial vehicle, which aim to solve or at least alleviate at least one problem existing in the prior art.
According to one aspect of the invention, a method for integrating delivery decision and path planning of a primary-secondary unmanned aerial vehicle is provided, which comprises the following steps:
step one, constructing a three-dimensional map environment according to the task requirements of a primary-secondary unmanned aerial vehicle; the construction of the three-dimensional map environment comprises the steps of determining the precision of the three-dimensional map, determining the flight boundary of the primary-secondary unmanned aerial vehicle, and establishing a coordinate system and the terrain height corresponding to each coordinate point in the coordinate system;
determining the number of submarines of the primary and secondary unmanned aerial vehicles and the target position of each submarines;
step three, establishing a path cost function according to the target position of each submachine and the terrain height;
and step four, obtaining the flight paths of the primary platform and the secondary machines in the primary-secondary unmanned aerial vehicle by using the improved particle swarm algorithm, wherein the flight paths correspond to the lowest path cost of the improved particle swarm algorithm, and the initial flight path point of the secondary machines is the thrown position.
Further, the path cost function in step three is a functional combination of the path length and the penalty function, that is:
F cost ＝C l (1+C p )
in the formula, C l Denotes the path length, C l The sum of the path length from the initial point to the separation point of the mother platform and the path length from the separation point to each target point of each submachine; c p And representing a penalty function, and representing a path point path cost penalty item which does not meet the requirement.
Further, the expression of the penalty function in step three is:
in the formula, N represents the total number of path points; z is a radical of m (x i ,y i ) Represents a coordinate point (x) i ,y i ) The terrain height of the place; z is a radical of i Representing the actual height of the waypoint i compared to the X-Y plane.
Further, the specific steps of the fourth step include:
step four, initializing particles of the improved particle swarm algorithm, wherein the method comprises the following steps:
initializing particles into three groups, wherein the first group of separation points takes the X-Y plane projection position of a polygonal Fermat point consisting of a starting point and a target point as X-axis and Y-axis coordinates; the second group of separation points takes the X-Y plane projection position of a random point in a polygon formed by the starting point and the target point as X-axis and Y-axis coordinates; the third group takes random point positions in an X-Y plane of the whole three-dimensional map as X-axis and Y-axis coordinates; the Z-axis coordinates of all the particles are the terrain height of the point; after the coordinates of the separation point are determined, the coordinates of other path points to be optimized in each particle are randomly distributed on a straight line connecting the separation point and the starting point and the separation point and the target point;
step two, iterative updating is carried out on the particles, and the iterative updating comprises the following steps:
calculating the path cost value of each particle, and selecting the particle position with the lowest global path cost value as g best Selecting the particle position with the lowest path cost value in each group as gr i,best Recording the lowest position of the historical path cost value of each particle in the iterative process as p i,best Then, the particle position updating formula of the improved particle swarm algorithm is as follows:
V i (t+1)＝ωV i (t)+c 1 r 1 (p i,best -p i )+c 2 r 2 (g best -p i )+c 3 r 3 (gr i,best -p i )
p i (t+1)＝p i (t)+V i (t+1)
in the formula, t represents the number of iterations; v i (t) represents the moving speed of the ith particle; p is a radical of i ＝p i (t) represents the position of the ith particle; gr is a group of i,best Representing the optimal particle position of the ith particle corresponding group; ω represents the inertial weight; c. C 1 、c 2 、c 3 Representing acceleration factors corresponding to different portions; r is 1 、r 2 、r 3 Are all real numbers between 0 and 1;
step three, the step two is executed iteratively, and the iteration is stopped until the maximum iteration times or the maximum calculation time is met; and selecting the particles with the lowest path cost value in the population, wherein the represented planned path is the flight path of the mother platform and the submachine, and the initial flight path point of the submachine is the thrown position.
Further, the step four includes: and performing cubic spline interpolation on the path points to be optimized of the three groups of particles to obtain all the path points of the particles.
According to another aspect of the present invention, there is provided a system for integrating delivery decision and path planning of a parent-child unmanned aerial vehicle, the system comprising:
the three-dimensional map building module is configured to build a three-dimensional map environment according to the task requirements of the primary-secondary unmanned aerial vehicle; the construction of the three-dimensional map environment comprises the steps of determining the precision of the three-dimensional map, determining the flight boundary of the primary-secondary unmanned aerial vehicle, and establishing a coordinate system and the terrain height corresponding to each coordinate point in the coordinate system;
a path cost establishing module configured to determine the number of submachine of the primary-secondary unmanned aerial vehicle and a target position of each submachine; establishing a path cost function according to the target position of each submachine and the terrain height;
and the path planning module is configured to obtain a flight path of a primary platform and a secondary machine in the primary-secondary unmanned aerial vehicle by utilizing an improved particle swarm algorithm, wherein the flight path corresponds to the lowest path cost of the improved particle swarm algorithm, and the initial flight path point of the secondary machine is the thrown position.
Further, the path cost function in the path cost establishing module is a functional combination of the path length and the penalty function, that is:
F cost ＝C l (1+C p )
in the formula, C l Denotes the path length, C l The path length from the initial point to the separation point of the mother platform and the path length from the separation point to each target point of each submachine are summed; c p And representing a penalty function, and representing a path point path cost penalty item which does not meet the requirement.
Further, the expression of the penalty function in the path cost establishing module is:
in the formula, N represents the total number of path points; z is a radical of formula m (x i ,y i ) Represents a coordinate point (x) i ,y i ) The terrain height of the place; z is a radical of i Representing the actual height of the waypoint i compared to the X-Y plane.
Further, the specific steps of obtaining the flight path of the primary platform and the secondary machine in the primary-secondary unmanned aerial vehicle and the throwing position of the secondary machine by utilizing the improved particle swarm algorithm in the path planning module comprise:
step four, initializing the particles of the improved particle swarm algorithm, comprising the following steps of:
initializing particles into three groups, wherein the first group of separation points takes the X-Y plane projection position of a polygonal Fermat point consisting of a starting point and a target point as X-axis and Y-axis coordinates; the second group of separation points takes the projection position of an X-Y plane of a random point in a polygon formed by the starting point and the target point as X-axis and Y-axis coordinates; the third group takes random point positions in an X-Y plane of the whole three-dimensional map as X-axis and Y-axis coordinates; the Z-axis coordinates of all the particles are the terrain height of the point; after the coordinates of the separation point are determined, the coordinates of other path points to be optimized in each particle are randomly distributed on a straight line connecting the separation point and the starting point and the separation point and the target point.
Step two, iterative updating is carried out on the particles, and the iterative updating comprises the following steps:
calculating the path cost value of each particle, and selecting the position of the particle with the lowest global path cost value as g best Selecting the particle position with the lowest path cost value in each group as gr i,best Recording the lowest position of the historical path cost value of each particle in the iterative process as p i,best Then, the particle position updating formula of the improved particle swarm algorithm is as follows:
V i (t+1)＝ωV i (t)+c 1 r 1 (p i,best -p i )+c 2 r 2 (g best -p i )+c 3 r 3 (gr i,best -p i )
p i (t+1)＝p i (t)+V i (t+1)
in the formula, t represents the number of iterations; v i (t) represents the moving speed of the ith particle; p is a radical of i ＝p i (t) represents the position of the ith particle; gr is a gas cylinder i,best Representing the optimal particle position of the ith particle corresponding group; ω represents the inertial weight; c. C 1 、c 2 、c 3 Representing acceleration factors corresponding to different portions; r is 1 、r 2 、r 3 Are all real numbers between 0 and 1;
step four, the step four and the step two are executed in an iterative mode, and the iteration is stopped until the maximum iteration times or the maximum calculation time is met; and selecting the particles with the lowest path cost value in the population, wherein the represented planned path is the flight path of the mother platform and the submachine, and the initial flight path point of the submachine is the thrown position.
The beneficial technical effects of the invention are as follows:
the invention provides a method and a system for integrating the throwing decision and path planning of a primary-secondary unmanned aerial vehicle aiming at the path planning process of separating and flying a submachine to a plurality of target points in a primary-secondary unmanned aerial vehicle, and provides a method for integrating the throwing decision and path planning of flying the submachine carried by a primary platform, throwing the submachine at the primary platform and moving the submachine to the target points aiming at the novel unmanned aerial vehicle, namely the primary-secondary unmanned aerial vehicle, for the first time, wherein the method takes the specific separation point selection in the throwing process of the primary-secondary unmanned aerial vehicle as the core and fills the blank of the research on the path planning method of the primary-secondary unmanned aerial vehicle; in addition, aiming at the problems that the convergence speed of the traditional particle swarm algorithm is too high and the traditional particle swarm algorithm is easy to fall into local optimum, the invention provides an improved particle swarm algorithm, so that the particles are converged to the optimum particles in each group at first and gradually converged to the global optimum particles along with the change of iteration times, thereby considering both the local optimality and the global optimality in path optimization and effectively improving the quality of a planned path.
In conclusion, the invention takes the primary-secondary unmanned aerial vehicle as an application object, improves and optimizes on the basis of the traditional particle swarm algorithm, provides the improved particle swarm algorithm taking separation point selection as a core, reduces the total path length while ensuring the obstacle avoidance effect of the unmanned aerial vehicle, and has higher engineering application value.
Drawings
The present invention may be better understood by reference to the following description taken in conjunction with the accompanying drawings, which are incorporated in and form a part of this specification, and which are used to further illustrate preferred embodiments of the present invention and explain the principles and advantages of the present invention.
Fig. 1 is a flowchart of an integrated method for release decision and path planning of a parent-child unmanned aerial vehicle according to an embodiment of the present invention.
Fig. 2 is an exemplary diagram of two-dimensional grid height map data employed in an embodiment of the present invention.
FIG. 3 is a diagram illustrating the variation of the acceleration factor value with the number of iterations in an embodiment of the present invention.
Fig. 4 is a comparison diagram of the path planning effect between the method of the embodiment of the present invention and the conventional particle swarm optimization.
Detailed Description
In order that those skilled in the art will better understand the disclosure, exemplary embodiments or examples of the disclosure are described below with reference to the accompanying drawings. It is obvious that the described embodiments or examples are only some, but not all embodiments or examples of the invention. All other embodiments or examples, which can be obtained by a person skilled in the art without making any creative effort based on the embodiments or examples in the present invention, shall fall within the protection scope of the present invention.
The particle swarm algorithm is a random search algorithm based on swarm cooperation and provided by simulating foraging behavior of a bird swarm. In the path planning of the unmanned aerial vehicle, each particle represents a planning path, an algorithm is initialized to be a plurality of random particles, in each iteration, the particles tend to the optimal particle positions in a population and the optimal positions in individual history, and the path optimizing process is achieved. The particle swarm algorithm has the advantages of being easy to implement, strong in universality, high in convergence speed and the like, and is widely applied to the field of unmanned aerial vehicle path planning.
The invention provides an improved particle swarm algorithm aiming at the special problem of the release decision and path planning of the primary and secondary unmanned aerial vehicles on the basis of the traditional particle swarm algorithm from the aspects of particle initialization, particle updating strategies and the like, and related achievements can be popularized to various primary and secondary unmanned aerial vehicles carrying different numbers of submachine, so that the improved particle swarm algorithm has higher practical application value.
The traditional path planning algorithm is dedicated to the generation of a point-to-point barrier-free passing path, and for the primary-secondary unmanned aerial vehicle, on the basis of the traditional path planning algorithm, how to select a proper release position in a map environment and ensure that the comprehensive path of a primary platform and a secondary machine is optimal is a core problem to be solved. The invention provides an improved particle swarm optimization-based integrated method for decision release and path planning of a primary and secondary unmanned aerial vehicle, which has good obstacle avoidance capability of the traditional path planning algorithm, takes the selection of a separation point of the primary and secondary unmanned aerial vehicle and the global exploration capability of the algorithm into consideration, and performs innovative design on the initialization and iterative optimization process of the algorithm, thereby further shortening the length of a planned path and improving the quality of the planned path.
The embodiment of the invention provides a method for integrating release decision and path planning of a primary-secondary unmanned aerial vehicle, which comprises the following steps:
step one, constructing a three-dimensional map environment according to the task requirements of a primary-secondary unmanned aerial vehicle; the construction of the three-dimensional map environment comprises the steps of determining the precision of the three-dimensional map, determining the flight boundary of the primary-secondary unmanned aerial vehicle, and establishing a coordinate system and the relief height corresponding to each coordinate point in the coordinate system;
determining the number of submarines of the primary and secondary unmanned aerial vehicles and the target position of each submarines;
step three, establishing a path cost function according to the target position of each submachine and the terrain height;
and step four, obtaining the flight paths of the primary platform and the secondary machines in the primary-secondary unmanned aerial vehicle by using the improved particle swarm algorithm, wherein the flight paths correspond to the lowest path cost of the improved particle swarm algorithm, and the initial flight path point of the secondary machines is the thrown position.
In this embodiment, preferably, the path cost function in step three is a function combination of the path length and the penalty function, that is:
F cost ＝C l (1+C p )
in the formula, C l Denotes the path length, C l The path length from the initial point to the separation point of the mother platform and the path length from the separation point to each target point of each submachine are summed; c p And representing a penalty function, and representing a path point path cost penalty item which does not meet the requirement.
In this embodiment, preferably, the expression of the penalty function in step three is:
in the formula, N represents the total number of path points; z is a radical of formula m (x i ,y i ) Represents a coordinate point (x) i ,y i ) The terrain height of the place; z is a radical of formula i Representing the actual height of the waypoint i compared to the X-Y plane.
In this embodiment, preferably, the specific steps of step four include:
step four, initializing particles of the improved particle swarm algorithm, wherein the method comprises the following steps:
initializing particles into three groups, wherein the first group of separation points takes the X-Y plane projection position of a polygonal Fermat point consisting of a starting point and a target point as X-axis and Y-axis coordinates; the second group of separation points takes the projection position of an X-Y plane of a random point in a polygon formed by the starting point and the target point as X-axis and Y-axis coordinates; the third group takes random point positions in an X-Y plane of the whole three-dimensional map as X-axis and Y-axis coordinates; the Z-axis coordinates of all the particles are the terrain height of the point; after the coordinates of the separation point position are determined, the coordinates of other path points to be optimized in each particle are randomly distributed on a straight line connecting the separation point and the starting point and the separation point and the target point;
step two, iterative updating is carried out on the particles, and the iterative updating comprises the following steps:
calculating the path cost value of each particle, and selecting the particle position with the lowest global path cost value as g best Selecting the particle position with the lowest path cost value in each group as gr i,best Recording the lowest position of the historical path cost value of each particle in the iterative process as p i,best Then, the particle position updating formula of the improved particle swarm algorithm is as follows:
V i (t+1)＝ωV i (t)+c 1 r 1 (p i,best -p i )+c 2 r 2 (g best -p i )+c 3 r 3 (gr i,best -p i )
p i (t+1)＝p i (t)+V i (t+1)
in the formula, t represents the number of iterations; v i (t) represents the moving speed of the ith particle; p is a radical of i ＝p i (t) represents the position of the ith particle; gr is a group of i,best Representing the optimal particle position of the ith particle corresponding group; ω represents the inertial weight; c. C 1 、c 2 、c 3 Representing acceleration factors corresponding to different portions; r is a radical of hydrogen 1 、r 2 、r 3 Are all real numbers between 0 and 1;
step three, the step two is executed iteratively, and the iteration is stopped until the maximum iteration times or the maximum calculation time is met; and selecting the particles with the lowest path cost value in the population, wherein the represented planned path is the flight path of the mother platform and the submachine, and the initial flight path point of the submachine is the thrown position.
In this embodiment, preferably, the first step further includes: and carrying out cubic spline interpolation on the path points to be optimized of the three groups of particles to obtain all the path points of each group of particles.
Another embodiment of the present invention provides a method for integrating delivery decision and path planning of a primary-secondary type unmanned aerial vehicle, as shown in fig. 1, the method is implemented according to the following steps:
the method comprises the following steps: determining the task requirements of the primary-secondary unmanned aerial vehicle, and constructing a three-dimensional map environment: determining the number of submarines of the primary and secondary unmanned aerial vehicles and the target position of each submarines, and establishing a space rectangular coordinate system according to task requirements and environmental data information; determining flight boundary and map precision, defining passable space and impassable space, and generating a three-dimensional digital map.
According to the embodiment of the invention, the primary-secondary unmanned aerial vehicle is formed by mounting two secondary machines on a primary platform. In the airdrop task, after the primary platform flies to a certain point in the space, two sub-machines are airdropped simultaneously, then the two sub-machines fly to corresponding target points respectively, and the primary platform lands in situ, namely the task is completed. The map environment is given by a two-dimensional grid height map, data is shown in fig. 2, wherein numbers in the grid represent the terrain heights of corresponding coordinate points, a three-dimensional height map with the accuracy of 100 multiplied by 50 and 1m can be generated by carrying out cubic polynomial interpolation on the digital map, a passable space is defined above the terrain heights, a non-passable space is defined below the terrain heights, and a space rectangular coordinate system is established according to the passable space. The primary position of the primary-secondary unmanned aerial vehicle is a coordinate origin p 0 = 0, the target point-coordinate is p t1 = 100,0, target point two coordinates p t2 ＝(100,100,0)。
Step two: giving improved particle swarm algorithm parameters, and establishing a path cost model: parameters of the improved particle swarm optimization comprise the number of path points to be optimized, the number of clusters, the number of total path points, the maximum iteration times, inertia weight, acceleration factors and the like; for the three-dimensional path planning problem of the primary-secondary unmanned aerial vehicle, the path cost can be generally selected as the sum of the path length of the primary platform from the starting point to the separation point and the path length of each secondary machine from the separation point to the target point.
According to the embodiment of the invention, parameters of the improved particle swarm optimization are shown in table 1, 9 path points are optimized in the particle swarm optimization process (a mother platform and 2 shelving machines respectively correspond to 3 path points to be optimized, as shown in fig. 4), and the rest path points are obtained by performing cubic spline interpolation on the path points to be optimized (the mother platform and 2 shelving machines respectively correspond to 100 path points). In this embodiment, the inertia weight ω and the acceleration factor c are designed 2 Acceleration factor c 3 The isoparametric changes with the iteration number, and the change rule is shown in figure 3.
TABLE 1 improved particle swarm algorithm parameters
As can be seen from Table 1, the planned path of the parent platform is p m ＝{p 0 ,p 1 ,...,p 100 In which p is 100 Namely the separation point; the handset 1 plans a path to be p s1 ＝{p 100 ,p 101 ,...,p 200 ,p t1 }; the handset 2 plans a path p s2 ＝{p 100 ,p 201 ,...,p 300 ,p t2 }。
Path cost F cost Is the path length C l And a penalty function C p The function combination of (a), namely:
F cost ＝C l (1+C p ) (1)
wherein, C l Path length C from starting point to departure point by parent platform m The path length C from the separation point to the target point of the sub-unit 1 s1 The path length C from the separation point to the target point of the slave unit 2 s2 And (c) the sum composition, namely:
C l ＝C m +C s1 +C s2 (2)
in equations (3) - (5), i is the path point number, and | is the two-norm of the vector, i.e., representing the distance between two points.
In the flying process of the unmanned aerial vehicle in the environment, the constraint that the flying height of the unmanned aerial vehicle is greater than the terrain height of a point is obviously satisfied, so a penalty function is designed to be used as a path point path cost penalty item which does not satisfy the requirement, and the expression is as follows:
wherein z is m (x i ,y i ) Is shown at coordinate point (x) i ,y i ) Corresponding to the terrain height.
Step three: initializing particles of an improved particle swarm algorithm: compared with the traditional particle swarm algorithm for randomly generating the initial path, the method provided by the invention considers the important influence of the separation point on the path planning, and focuses on the design of the selection of the separation point. The method comprises the following steps that particles are initialized into three groups during initialization, and the first group of separation points takes the X-Y plane projection position of a polygon Fermat point (the distance from a polygon plane to a polygon vertex and the shortest point) consisting of a starting point and a target point as X-axis coordinates and Y-axis coordinates; the second group of separation points takes the projection position of an X-Y plane of a random point in a polygon formed by the starting point and the target point as X-axis and Y-axis coordinates; and the third group takes random point positions in an X-Y plane of the whole map space as X-axis and Y-axis coordinates. In order to enable all the separation points to meet obstacle avoidance constraints, the Z-axis coordinates of all the particles are the terrain height of the points; after the coordinates of the separation point are determined, the coordinates of other path points to be optimized in each particle are randomly distributed on a straight line connecting the separation point and the starting point and the separation point and the target point.
According to an embodiment of the invention, the particles are initialized into three groups, wherein the first group of particles accounts for one third of the population, the second group of particles accounts for one half of the population, and the third group of particles accounts for one sixth of the population. The first set of particles is initialized as follows: and generating an initial separation point of the group of particles by taking a triangular Fermat point consisting of the starting point, the target point I and the target point II as X and Y coordinates and taking the topographic height of the point as a Z coordinate. The separation point coordinates for this set of particles were calculated to be (78.87, 21.13, 2.533). And other path points to be optimized are randomly selected on connecting lines from the starting point to the separation point, from the separation point to the first target point and from the separation point to the second target point respectively.
And the coordinates of the separation points X and Y of the second group are randomly selected in a triangle consisting of the starting point, the target point I and the target point II, the coordinate Z is the terrain height of the corresponding path point, and the selection mode of the rest path points to be optimized is the same as that of the first group.
And the X and Y coordinates of the separation points of the third group are randomly selected in an X-Y plane, the Z coordinate is the corresponding terrain height, and the selection mode of the rest path points to be optimized is the same as that of the first group.
And performing cubic spline interpolation on the path points to be optimized of each group of particles to obtain all the path points of each group of particles, namely completing the particle initialization of the improved particle swarm algorithm.
Step four: based on an improved particle swarm algorithm, the particles are iteratively updated: in consideration of the fact that the separation points in the third step are initialized in three ways, in order to improve the local search capability of the algorithm, the invention initializes the particles into three groups on the basis of the traditional particle swarm algorithm, and the optimal particles in each group are additionally used for particle updating. Meanwhile, in order to integrate the local search and global search capabilities, the acceleration factor is designed as a function of the iteration times, so that the particles gradually change from tending to local optimum to tending to global optimum along with the increase of the iteration times. In addition, in order to facilitate the unmanned aerial vehicle to effectively track the path, a cubic spline interpolation method is adopted, and all path points are generated according to the interpolation of the path points to be optimized, so that the planned path of the primary-secondary unmanned aerial vehicle is formed.
According to the embodiment of the invention, for the initialized particles, the path cost of each particle is calculated according to the formulas (1) to (6), and the position of the particle with the lowest global path cost is selected and recorded as g best Selecting the positions of the particles with the lowest path cost in each group as gr 1,best ,gr 2,best ,gr 3,best Recording the lowest position of the historical path cost of each particle in the iterative process as p i,best Then, the particle position updating formula of the improved particle swarm optimization is as follows:
V i (t+1)＝ωV i (t)+c 1 r 1 (p i,best -p i )+c 2 r 2 (g best -p i )+c 3 r 3 (gr i,best -p i ) (7)
p i (t+1)＝p i (t)+V i (t+1) (8)
wherein, V i Is the moving velocity of the ith particle, p i Position of the ith particle, gr i,best For the ith particle corresponds to the optimal particle position of the grouping, i.e. gr i,best ∈{gr 1,best ,gr 2,best ,gr 3,best }. It should be noted that, in this embodiment, each particle is composed of 9 path points to be optimized and three-dimensional coordinates thereof, and a planning path is formed after three-time spline interpolation is performed on the path points to be optimized to obtain all the path points. Therefore, the change of the path point to be optimized represented by the particle in each iteration represents the continuous optimization of the planned path.
Step five: and repeating the fourth operation until the termination conditions such as the maximum iteration times or the maximum calculation time are met, selecting the particles with the lowest path cost in the population, wherein the represented planned path is the flight path of the primary platform and the secondary machine, the initial flight path point of the secondary machine is the release position, and the release position and the planned path are obtained to provide task reference for the primary-secondary unmanned aerial vehicle.
According to the embodiment of the invention, the four operations are repeated until the maximum iteration times reach 200 times, the particles with the lowest path cost in the population are selected, and the represented planned path is the planned path of the primary platform and the secondary machine in the primary-secondary unmanned aerial vehicle.
In this embodiment, the path point to be optimized and the planned path obtained by the improved particle swarm optimization are calculated, as shown by the circular dots and the solid lines in fig. 4, the obtained path does not collide with the undulating terrain, the obtained separation point is (65.565, 36.893, 29.445), and the path length is 230.8m. In addition, the path point to be optimized and the planned path obtained by calculation through the traditional particle swarm algorithm are shown as a triangle and a dotted line in fig. 4, the path also has an obstacle avoidance effect, the obtained separation point positions are (65.402, 28.273, 44.328), and the path length is 250.3m. As can be seen from the figure, the integrated method for the release decision and the path planning of the primary and secondary unmanned aerial vehicles can effectively complete release position determination and path planning tasks of the primary and secondary unmanned aerial vehicles, and the planned path has a good obstacle avoidance effect, is shorter in path length compared with a traditional particle swarm algorithm, and has a good engineering application prospect.
Another embodiment of the present invention further provides a system for integrating delivery decision and path planning of a primary-secondary type unmanned aerial vehicle, the system comprising:
the three-dimensional map building module is configured to build a three-dimensional map environment according to the task requirements of the primary-secondary unmanned aerial vehicle; the construction of the three-dimensional map environment comprises the steps of determining the precision of the three-dimensional map, determining the flight boundary of the primary-secondary unmanned aerial vehicle, and establishing a coordinate system and the terrain height corresponding to each coordinate point in the coordinate system;
a path cost establishing module configured to determine the number of submachine of the primary-secondary unmanned aerial vehicle and a target position of each submachine; establishing a path cost function according to the target position of each submachine and the terrain height;
and the path planning module is configured to obtain a flight path of a primary platform and a secondary machine in the primary-secondary unmanned aerial vehicle by using an improved particle swarm algorithm, the flight path corresponds to the lowest path cost of the improved particle swarm algorithm, and the initial flight path point of the secondary machine is the thrown-in position.
In this embodiment, preferably, the path cost function in the path cost establishing module is a function combination of a path length and a penalty function, that is:
F cost ＝C l (1+C p )
in the formula, C l Denotes the path length, C l The path length from the initial point to the separation point of the mother platform and the path length from the separation point to each target point of each submachine are summed; c p And representing a penalty function, and representing a path point path cost penalty item which does not meet the requirement.
In this embodiment, preferably, the expression of the penalty function in the path cost establishing module is:
in the formula, N represents the total number of path points; z is a radical of m (x i ,y i ) Represents a coordinate point (x) i ,y i ) The terrain height of the place; z is a radical of i Representing the actual height of the waypoint i compared to the X-Y plane.
In this embodiment, preferably, the specific steps of obtaining the flight paths of the primary platform and the secondary machine in the primary and secondary unmanned aerial vehicles and the positions where the secondary machine is thrown in by using the improved particle swarm algorithm in the path planning module include:
step four, initializing particles of the improved particle swarm algorithm, wherein the method comprises the following steps:
initializing particles into three groups, wherein the first group of separation points takes the X-Y plane projection position of a polygonal Fermat point consisting of a starting point and a target point as X-axis and Y-axis coordinates; the second group of separation points takes the projection position of an X-Y plane of a random point in a polygon formed by the starting point and the target point as X-axis and Y-axis coordinates; the third group takes random point positions in an X-Y plane of the whole three-dimensional map as X-axis and Y-axis coordinates; the Z-axis coordinates of all the particles are the terrain height of the point; after the coordinates of the separation point are determined, the coordinates of other path points to be optimized in each particle are randomly distributed on a straight line connecting the separation point and the starting point and the separation point and the target point;
step two, iterative updating is carried out on the particles, and the iterative updating comprises the following steps:
calculating the path cost value of each particle, and selecting the position of the particle with the lowest global path cost value as g best Selecting the particle position with the lowest path cost value in each group as gr i,best Recording the lowest position of the historical path cost value of each particle in the iterative process as p i,best Then, the particle position updating formula of the improved particle swarm optimization is:
V i (t+1)＝ωV i (t)+c 1 r 1 (p i,best -p i )+c 2 r 2 (g best -p i )+c 3 r 3 (gr i,best -p i )
p i (t+1)＝p i (t)+V i (t+1)
in the formula, t represents the number of iterations; v i (t) represents the moving speed of the ith particle; p is a radical of formula i ＝p i (t) represents the position of the ith particle; gr is a group of i,best Representing the optimal particle position of the ith particle corresponding group; ω represents the inertial weight; c. C 1 、c 2 、c 3 Representing acceleration factors corresponding to different portions; r is a radical of hydrogen 1 、r 2 、r 3 Are all real numbers between 0 and 1;
step four, the step four and the step two are executed in an iterative mode, and the iteration is stopped until the maximum iteration times or the maximum calculation time is met; and selecting the particles with the lowest path cost value in the population, wherein the represented planned path is the flight path of the mother platform and the submachine, and the initial flight path point of the submachine is the thrown position.
The function of the integrated system for releasing decision and path planning of the primary-secondary unmanned aerial vehicle in this embodiment can be described by the integrated method for releasing decision and path planning of the primary-secondary unmanned aerial vehicle, so that the detailed description of this embodiment is omitted, and reference may be made to the above method embodiments, and further description is omitted here.
While the invention has been described with respect to a limited number of embodiments, those skilled in the art, having benefit of this description, will appreciate that other embodiments can be devised which do not depart from the scope of the invention as described herein. The present invention has been disclosed in an illustrative rather than a restrictive sense, and the scope of the present invention is defined by the appended claims.
Claims (9)
1. A primary-secondary unmanned aerial vehicle putting decision and path planning integrated method is characterized by comprising the following steps:
step one, constructing a three-dimensional map environment according to the task requirements of a primary-secondary unmanned aerial vehicle; the construction of the three-dimensional map environment comprises the steps of determining the precision of the three-dimensional map, determining the flight boundary of the primary-secondary unmanned aerial vehicle, and establishing a coordinate system and the terrain height corresponding to each coordinate point in the coordinate system;
step two, determining the number of submachine of the primary and secondary unmanned aerial vehicles and the target position of each submachine;
step three, establishing a path cost function according to the target position of each submachine and the terrain height;
and step four, obtaining the flight paths of the primary platform and the secondary machine in the primary-secondary unmanned aerial vehicle by using the improved particle swarm algorithm, wherein the flight path corresponds to the lowest path cost of the improved particle swarm algorithm, and the initial flight path point of the secondary machine is the thrown position.
2. The integrated method for making a release decision and planning a path for a parent-child unmanned aerial vehicle according to claim 1, wherein the path cost function in the third step is a function combination of a path length and a penalty function, that is:
F cost ＝C l (1+C p )
in the formula, C l Denotes the path length, C l The sum of the path length from the initial point to the separation point of the mother platform and the path length from the separation point to each target point of each submachine; c p And representing a penalty function, and representing a path point path cost penalty item which does not meet the requirement.
3. The integrated method for releasing decision and path planning of parent-child unmanned aerial vehicle of claim 2, wherein the expression of the penalty function in step three is:
wherein N represents the total number of path points; z is a radical of formula m (x i ,y i ) Represents a coordinate point (x) i ,y i ) The terrain height of the place; z is a radical of i Representing the actual height of the waypoint i compared to the X-Y plane.
4. The integrated method for releasing decision and path planning of a parent-child unmanned aerial vehicle according to claim 3, wherein the fourth step comprises the following specific steps:
step four, initializing the particles of the improved particle swarm algorithm, comprising the following steps of:
initializing and dividing particles into three groups, wherein the first group of separation points takes the X-Y plane projection position of a polygonal Fermat point consisting of a starting point and a target point as X-axis and Y-axis coordinates; the second group of separation points takes the X-Y plane projection position of a random point in a polygon formed by the starting point and the target point as X-axis and Y-axis coordinates; the third group takes random point positions in an X-Y plane of the whole three-dimensional map as X-axis and Y-axis coordinates; the Z-axis coordinates of all the particles are the terrain height of the point; after the coordinates of the separation point are determined, the coordinates of other path points to be optimized in each particle are randomly distributed on a straight line connecting the separation point and the starting point and the separation point and the target point;
step two, iterative updating is carried out on the particles, and the iterative updating comprises the following steps:
calculating the path cost value of each particle, and selecting the particle position with the lowest global path cost value as g best Selecting the particle position with the lowest path cost value in each group as gr i,best Recording the lowest position of the historical path cost value of each particle in the iterative process as p i,best Then, the particle position updating formula of the improved particle swarm algorithm is as follows:
V i (t+1)＝ωV i (t)+c 1 r 1 (p i,best -p i )+c 2 r 2 (g best -p i )+c 3 r 3 (gr i,best -p i )
p i (t+1)＝p i (t)+V i (t+1)
in the formula, t represents the number of iterations; v i (t) represents a moving speed of the ith particle; p is a radical of i ＝p i (t) represents the position of the ith particle; gr is a group of i,best Representing the optimal particle position of the ith particle corresponding group; ω represents the inertial weight; c. C 1 、c 2 、c 3 Representing acceleration factors corresponding to the different portions; r is 1 、r 2 、r 3 Are all real numbers between 0 and 1;
step three, the step two is executed iteratively, and the iteration is stopped until the maximum iteration times or the maximum calculation time is met; and selecting the particles with the lowest path cost value in the population, wherein the represented planned path is the flight path of the mother platform and the submachine, and the initial flight path point of the submachine is the thrown position.
5. The integrated method for son-mother unmanned aerial vehicle release decision and path planning of claim 4, wherein the step four includes: and performing cubic spline interpolation on the path points to be optimized of the three groups of particles to obtain all the path points of the particles.
6. The utility model provides a son and mother formula unmanned aerial vehicle puts in decision-making and path planning integration system which characterized in that includes:
the three-dimensional map building module is configured to build a three-dimensional map environment according to the task requirements of the primary-secondary unmanned aerial vehicle; the construction of the three-dimensional map environment comprises the steps of determining the precision of the three-dimensional map, determining the flight boundary of the primary-secondary unmanned aerial vehicle, and establishing a coordinate system and the terrain height corresponding to each coordinate point in the coordinate system;
a path cost establishing module configured to determine the number of submachine of the primary-secondary unmanned aerial vehicle and a target position of each submachine; establishing a path cost function according to the target position of each submachine and the terrain height;
and the path planning module is configured to obtain a flight path of a primary platform and a secondary machine in the primary-secondary unmanned aerial vehicle by using an improved particle swarm algorithm, the flight path corresponds to the lowest path cost of the improved particle swarm algorithm, and the initial flight path point of the secondary machine is the thrown-in position.
7. The system of claim 6, wherein the path cost function in the path cost establishing module is a functional combination of a path length and a penalty function, that is:
F cost ＝C l (1+C p )
in the formula, C l To representPath length, C l The path length from the initial point to the separation point of the mother platform and the path length from the separation point to each target point of each submachine are summed; c p And representing a penalty function, and representing a path point path cost penalty item which does not meet the requirement.
8. The system of claim 7, wherein the penalty function in the path cost establishing module has an expression as follows:
wherein N represents the total number of path points; z is a radical of m (x i ,y i ) Represents a coordinate point (x) i ,y i ) The terrain height of the place; z is a radical of i Representing the actual height of the waypoint i compared to the X-Y plane.
9. The system of claim 8, wherein the path planning module, using an improved particle swarm algorithm, obtains the flight path of the primary platform and the secondary machine of the primary-secondary unmanned aerial vehicle and the position where the secondary machine is launched, comprises:
step four, initializing the particles of the improved particle swarm algorithm, comprising the following steps of:
dividing the particles into three groups, wherein the first group of separation points takes the X-Y plane projection position of a polygonal Fermat point consisting of a starting point and a target point as X-axis and Y-axis coordinates; the second group of separation points takes the projection position of an X-Y plane of a random point in a polygon formed by the starting point and the target point as X-axis and Y-axis coordinates; the third group takes random point positions in an X-Y plane of the whole three-dimensional map as X-axis and Y-axis coordinates; the Z-axis coordinate of all the particles is the terrain height of the point; after the coordinates of the separation point position are determined, the coordinates of other path points to be optimized in each particle are randomly distributed on a straight line connecting the separation point and the starting point and the separation point and the target point;
step two, carrying out iterative update on the particles, including:
calculating the path cost value of each particle, and selecting the particle position with the lowest global path cost value as g best Selecting the particle position with the lowest path cost value in each group as gr i,best Recording the lowest position of the historical path cost value of each particle in the iterative process as p i,best Then, the particle position updating formula of the improved particle swarm optimization is:
V i (t+1)＝ωV i (t)+c 1 r 1 (p i,best -p i )+c 2 r 2 (g best -p i )+c 3 r 3 (gr i,best -p i )
p i (t+1)＝p i (t)+V i (t+1)
in the formula, t represents the number of iterations; v i (t) represents a moving speed of the ith particle; p is a radical of formula i ＝p i (t) represents the position of the ith particle; gr is a gas cylinder i,best Representing the optimal particle position of the ith particle corresponding group; ω represents the inertial weight; c. C 1 、c 2 、c 3 Representing acceleration factors corresponding to the different portions; r is a radical of hydrogen 1 、r 2 、r 3 Are all real numbers between 0 and 1;
step four, the step four and the step two are executed in an iterative mode, and the iteration is stopped until the maximum iteration times or the maximum calculation time is met; and selecting the particles with the lowest path cost value in the population, wherein the represented planned path is the flight path of the mother platform and the submachine, and the initial flight path point of the submachine is the thrown position.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211403755.7A CN115657725A (en) | 2022-11-10 | 2022-11-10 | Primary-secondary unmanned aerial vehicle release decision and path planning integrated method and system |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211403755.7A CN115657725A (en) | 2022-11-10 | 2022-11-10 | Primary-secondary unmanned aerial vehicle release decision and path planning integrated method and system |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN115657725A true CN115657725A (en) | 2023-01-31 |
Family
ID=85021904
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202211403755.7A Pending CN115657725A (en) | 2022-11-10 | 2022-11-10 | Primary-secondary unmanned aerial vehicle release decision and path planning integrated method and system |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115657725A (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117195744A (en) * | 2023-10-16 | 2023-12-08 | 南京工业大学 | Trafficability migration evaluation method for cooperative crossing of primary and secondary mobile robots |
-
2022
- 2022-11-10 CN CN202211403755.7A patent/CN115657725A/en active Pending
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117195744A (en) * | 2023-10-16 | 2023-12-08 | 南京工业大学 | Trafficability migration evaluation method for cooperative crossing of primary and secondary mobile robots |
| CN117195744B (en) * | 2023-10-16 | 2024-04-05 | 南京工业大学 | Trafficability migration evaluation method for cooperative crossing of primary and secondary mobile robots |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| De Filippis et al. | Path planning strategies for UAVS in 3D environments | |
| CN110320930B (en) | Reliable transformation method for formation of multiple unmanned aerial vehicles based on Voronoi diagram | |
| CN108958285B (en) | Efficient multi-unmanned aerial vehicle collaborative track planning method based on decomposition idea | |
| Zheng et al. | Evolutionary route planner for unmanned air vehicles | |
| Chen et al. | Path planning for multi-UAV formation | |
| Ergezer et al. | 3D path planning for multiple UAVs for maximum information collection | |
| CN110850891B (en) | Multi-unmanned aerial vehicle dynamic route planning method based on space and time coordination | |
| Lindhé et al. | Flocking with obstacle avoidance: A new distributed coordination algorithm based on voronoi partitions | |
| Ali et al. | Cooperative path planning of multiple UAVs by using max–min ant colony optimization along with cauchy mutant operator | |
| CN109508035A (en) | Multizone stagewise unmanned plane formation paths planning method based on distributed AC servo system | |
| CN105700549A (en) | Unmanned plane multi-track planning method based on sequence ecological niche PSO (particle swarm optimization) algorithm | |
| CN106815443A (en) | Towards the hedgehopping device many batches of Multiple routes planning methods of three-dimensional of changing environment | |
| Shanmugavel et al. | Collision avoidance and path planning of multiple UAVs using flyable paths in 3D | |
| CN102880186A (en) | Flight path planning method based on sparse A* algorithm and genetic algorithm | |
| CN112230678A (en) | Three-dimensional unmanned aerial vehicle path planning method and planning system based on particle swarm optimization | |
| CN110471426A (en) | Unmanned intelligent vehicle automatic Collision Avoidance method based on quantum wolf pack algorithm | |
| CN114355981B (en) | Method and system for autonomous exploration and mapping of four-rotor unmanned aerial vehicle | |
| CN112327939B (en) | Collaborative path planning method for high-rise fire-fighting multiple unmanned aerial vehicles in city block environment | |
| CN112666981B (en) | Unmanned aerial vehicle cluster dynamic route planning method based on dynamic group learning of original pigeon group | |
| CN110362115A (en) | A kind of time-constrain reaches multiple no-manned plane path planning algorithm simultaneously | |
| CN112327927B (en) | Multi-angle strike track planning method for formation unmanned aerial vehicles based on grid planning algorithm | |
| CN115657725A (en) | Primary-secondary unmanned aerial vehicle release decision and path planning integrated method and system | |
| Ma et al. | A Fast path re-planning method for UAV based on improved A* algorithm | |
| CN112650306A (en) | Unmanned aerial vehicle motion planning method based on dynamics RRT | |
| CN113985922A (en) | Unmanned aerial vehicle hierarchical path planning method under multi-target constraint |
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 |